gazebo_joint_state_publisher sends wrong velocities in the joint states when controlling a robot with ros2 effort controllers/joint_group effort_controllers
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
Asked by iopoi97 on 2023-03-02 11:06:35 UTC
Comments