gazebo_joint_state_publisher sends wrong velocities in the joint states when controlling a robot with ros2 effort controllers/joint_group effort_controllers

asked 2023-03-02 10:06:35 -0500

iopoi97 gravatar image

hello, I have a dual arm robot to control in an Inverse dynamics control, using ros2 galactic. Up to now I configured my robot with a gazebo joint state publisher and, with ros2 controllers effort_controllers/jointgroupeffortcontrollers. My script sends the torque outputs to the topic effort_controllers/command and the robot reaches an equilibrium position. It is completely still. But plotting the joint states velocities I noticed that the velocities are by far different of 0. Anyone could help me to understand this weird behavior from ros2 control and gazebo joint states publisher? Iĺl write down my code highlights and i will give U the outputs in velocity of plotjuggler.

xacro main parts:

  <ros2_control name="GazeboSystem" type="system">
    <hardware>
      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
    </hardware>
...#same for all the joints left and right
   <joint name="robr_joint_7">
      <command_interface name="effort">
        <param name="min">-50</param>
        <param name="max">50</param>
      </command_interface>
      <state_interface name="position">
             </state_interface>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>   
</ros2_control>
 <gazebo>
      <plugin name="gazebo_ros_joint_state_publisher"
          filename="libgazebo_ros_joint_state_publisher.so">
        <update_rate>100</update_rate>
        <joint_name>chassis_joint</joint_name>
        <joint_name>front_radar_joint</joint_name>
        <joint_name>front_laser_scanner_joint</joint_name>
...#for all the links
     <joint_name>cs_joint</joint_name>
        <joint_name>cs_camera_joint</joint_name>
      </plugin>
    </gazebo>
<gazebo>
      <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
        <robot_param>robot_description</robot_param> 
        <robot_param_node>robot_state_publisher_node</robot_param_node> 
        <parameters>$(find my_scenario)/properties/irb14000_05_50/controller.yaml</parameters>
      </plugin>
    </gazebo>

yaml file:

controller_manager:
  ros__parameters:
    update_rate: 100  # Hz
    use_sim_time: true

    effort_controllers_r_1:
      type: effort_controllers/JointGroupEffortController 
    effort_controllers_r_2:
      type: effort_controllers/JointGroupEffortController
    effort_controllers_r_3:
      type: effort_controllers/JointGroupEffortController
    effort_controllers_r_4:
      type: effort_controllers/JointGroupEffortController
    effort_controllers_r_5:
      type: effort_controllers/JointGroupEffortController
    effort_controllers_r_6:
      type: effort_controllers/JointGroupEffortController
    effort_controllers_r_7:
      type: effort_controllers/JointGroupEffortController
    effort_controllers_l:
      type: effort_controllers/JointGroupEffortController 
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

effort_controllers_r_1:
  ros__parameters:
    joints:
      -robr_joint_1
    command_interfaces:
      - effort
    state_interfaces:
      - position
      - velocity
      - effort 

effort_controllers_r_2:
  ros__parameters:
    joints:
      -robr_joint_2
    command_interfaces:
      - effort
    state_interfaces:
      - position
      - velocity
      - effort

effort_controllers_r_3:
  ros__parameters:
    joints:
      -robr_joint_3
    command_interfaces:
      - effort
    state_interfaces:
      - position
      - velocity
      - effort

effort_controllers_r_4:
  ros__parameters:
    joints:
      -robr_joint_4
    command_interfaces:
      - effort
    state_interfaces:
      - position
      - velocity
      - effort

effort_controllers_r_5:
  ros__parameters:
    joints:
      -robr_joint_5
    command_interfaces:
      - effort
    state_interfaces:
      - position
      - velocity
      - effort

effort_controllers_r_6:
  ros__parameters:
    joints:
      -robr_joint_6
    command_interfaces:
      - effort
    state_interfaces:
      - position
      - velocity
      - effort

effort_controllers_r_7:
  ros__parameters:
    joints:
      -robr_joint_7
    command_interfaces:
      - effort
    state_interfaces:
      - position
      - velocity
      - effort

effort_controllers_l:
  ros__parameters:
    joints:
      -robl_joint_1
      -robl_joint_2
      -robl_joint_3
      -robl_joint_4
      -robl_joint_5
      -robl_joint_6
      -robl_joint_7
    command_interfaces:
      - effort
    state_interfaces:
      - position
      - velocity
      - effort

image from plotjuggler when I am in the equilibrium pose: C:\fakepath\vel.png

edit retag flag offensive close merge delete