I used joint_state_broadcaster to send the status of the robot in gazebo to topic /joint_state.
When I used the information in /joint_state, I found that the position was accurate, but the velocity was completely singular.
You can observe this phenomenon in the graph below. When the position is held still, the speed also remains at a constant value, and the trend of speed and position is exactly similar.
(I send a position command to joint_1 and observe the position and speed feedback. The red line is the position, and the left side is his scale. The black line is speed, and the right side is his scale.)
(Do the same thing with joint_2 and observe the following phenomenon. The red line is the position, and the right side is his scale. The black line is speed, and the left side is his scale.)
My controller parameters file are(This robot has more degrees of freedom, so only the most critical parts can be shown here.)
controller_manager:
ros__parameters:
update_rate: 1000
use_sim_time: true
arm_controller:
type: joint_trajectory_controller/JointTrajectoryController
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
arm_controller:
ros__parameters:
joints:
- knee_pitch_joint
- hip_pitch_joint
- some joints
command_interfaces:
- position
# - velocity
state_interfaces:
- position
- velocity
# - effort
URDF(This robot has more degrees of freedom, so only the most critical parts can be shown here.)
<gazebo>
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
<robot_param>robot_description</robot_param>
<robot_param_node>robot_state_publisher</robot_param_node>
<parameters>./config/gazebo_robot_control.yaml</parameters>
</plugin>
</gazebo>
<ros2_control name="RobotSystem" type="system">
<hardware>
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</hardware>
<joint name="joint_1">
<param name="pos_kp">10</param>
<param name="pos_ki">1</param>
<param name="pos_kd">2</param>
<param name="pos_max_integral_error">10000</param>
<param name="vel_kp">10</param>
<param name="vel_ki">5</param>
<param name="vel_kd">2</param>
<param name="vel_max_integral_error">10000</param>
<command_interface name="position">
<param name="min">-1.1</param>
<param name="max">0.0</param>
</command_interface>
<command_interface name="velocity">
<param name="min">-0.44</param>
<param name="max">0.44</param>
</command_interface>
<state_interface name="position">
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="joint_2">
<param name="pos_kp">10</param>
<param name="pos_ki">1</param>
<param name="pos_kd">2</param>
<param name="pos_max_integral_error">10000</param>
<param name="vel_kp">10</param>
<param name="vel_ki">5</param>
<param name="vel_kd">2</param>
<param name="vel_max_integral_error">10000</param>
<command_interface name="position">
<param name="min">0.0</param>
<param name="max">2</param>
</command_interface>
<command_interface name="velocity">
<param name="min">-0.6</param>
<param name="max">0.6</param>
</command_interface>
<state_interface name="position">
</state_interface>
<state_interface name="velocity"/>
</joint>
......some more joints
</ros2_control>
<joint name="joint_1" type="revolute">
<parent link="leg_base_link"/>
<axis xyz="0 1 0"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
<child link="knee_pitch_link"/>
<limit effort="1000" lower="-1.1" upper="0.0" velocity="0.44"/>
</joint>
<joint name="joint_2" type="revolute">
<parent link="knee_pitch_link"/>
<axis xyz="0 1 0"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.41"/>
<child link="hip_pitch_link"/>
<limit effort="720" lower="0.0" upper="1.9" velocity="0.6"/>
</joint>
...some more joints
<link name="knee_pitch_link">
<inertial>
<origin rpy="0 0 0" xyz="0.01 0.001 0.2"/>
<mass value="15"/>
<inertia ixx="0.2" ixy="0.001" ixz="0.01" iyy="0.22" iyz="0.002" izz="0.1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="some.stl"/>
</geometry>
<material name="lightgrey">
<color rgba=".7 .7 .7 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="some stl"/>
</geometry>
</collision>
</link>
<link name="hip_pitch_link">
<inertial>
<origin rpy="0 0 0" xyz="-0.04546042 0.0 0.0038846896"/>
<mass value="0.8"/>
<inertia ixx="0.004" ixy="0.0" ixz="0.0001" iyy="0.002" iyz="0.0" izz="0.004"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="some stl"/>
</geometry>
<material name="lightgrey">
<color rgba=".7 .7 .7 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="some stl"/>
</geometry>
</collision>
</link>
...some more links
......
my launch file
import subprocess
from launch import LaunchDescription
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
import os
def generate_launch_description():
subprocess.run(["pkill", "-9", "gzserver"])
subprocess.run(["pkill", "-9", "gzclient"])
declared_arguments = []
# General arguments
##########
# gazebo #
##########
xml = open('some urdf dir', 'r').read()
robot_state_publisher_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
name='robot_state_publisher',
output="both",
parameters=[{
"use_sim_time":True,
"robot_description": xml,
"publish_frequency": 50.0,
"ignore_timestamp": False}],
)
# Launch arm controller
start_arm_controller_cmd = Node(
package="controller_manager",
executable="spawner",
arguments=[
"arm_controller",
"--controller-manager",
"/controller_manager"
]
)
pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros')
# Start Gazebo server
start_gazebo_server_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
launch_arguments={'params_file': './config/gazebo_param.yaml'}.items()
)
# Start Gazebo client
start_gazebo_client_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')),
)
# Launch joint state broadcaster
start_joint_state_broadcaster_cmd = Node(
package="controller_manager",
executable="spawner",
arguments=[
"joint_state_broadcaster",
"--controller-manager",
"/controller_manager"
]
)
# Spawn the robot
start_gazebo_ros_spawner_cmd = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=[
'-entity', 'test_robot',
'-topic', "robot_description",
],
output='screen')
nodes_to_start = [
start_arm_controller_cmd,
start_gazebo_server_cmd,
start_gazebo_client_cmd,
start_joint_state_broadcaster_cmd,
robot_state_publisher_node,
start_gazebo_ros_spawner_cmd
]
return LaunchDescription(nodes_to_start)
Environment:
ubuntu22.04
ros-humble
gazebo11
To better understand this phenomenon and facilitate debugging, I created a minimal case, link here https://github.com/OTT123/gazebo_ros2_control_a_mini_demo.git
In above case project, you shall find the below phenomenon: When the position is stable at -1, the velocity is stable at -0.0287. And when controlling the joint position, the change trend of velocity and position is the same.