0
$\begingroup$

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.) The red line is the position, and the left side is his ruler. 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.) The red line is the position, and the left side is his ruler. The black line is speed, and the right 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. enter image description here

$\endgroup$
3
  • $\begingroup$ which ROS distro are you using, and gazebo_ros2_control? can you add also your ros2_control tag from the URDF? $\endgroup$ Commented Jul 29 at 9:17
  • $\begingroup$ the part of some_joints in the tag is of interest. $\endgroup$ Commented Jul 29 at 10:27
  • $\begingroup$ and can you show both variables in one plot, e.g, with plotjuggler? In your plot it is not visible which entries of the joint_states your are comparing to $\endgroup$ Commented Jul 29 at 10:28

1 Answer 1

0
$\begingroup$

Be aware that the pos_kp and vel_kp only should have an effect if you use the following command_interfaces

  <command_interface name="position_pid"/>
  <command_interface name="velocity_pid"/>

First I thought that your observation comes from the PIDs, but as you are using the "normal" position interface (according to your controller config), this should not be the cause.

  • Can you please remove the unnecessary tags from your config and test again, just to be sure?

  • Can you please use one of the gazebo_ros2_control demos and try to reproduce your issue with as little changes as necessary, so we can reproduce your issue if it still persists?

  • From your graphs it is still not visible which entry of the joint_states msg are you plotting. can you post the output of ros2 node echo /joint_states --once when being in idle?

$\endgroup$
2
  • $\begingroup$ Thanks.And to better understand this phenomenon and facilitate debugging, I created a minimal case, link here: github.com/OTT123/gazebo_debug_demo.git $\endgroup$
    – OTT
    Commented Jul 30 at 3:21
  • $\begingroup$ I now had time to test your issue, and I can even reproduce a similar behavior with the released demos. I created an issue, but gazebo classic soon will be EOL and there is not a lot of effort put in there any more. If you'd like to fix it, I'm happy to review your PR. $\endgroup$ Commented Aug 6 at 10:56

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.