Solution to Exercise 2.7.1 issue

Course Support ROS2 Control Framework
1

After copying the solution, deleting the robot, rebuilding, sourcing, and running ros2 topic pub command, joint 2 is still limp and joint 1 also doesn’t move.

Everything up till this part seemed to work correctly. In the my_robot_bringup package I updated the controller_configuration.yaml and the rrbot.xacro in the rrbot_unit2 package to look like the solution.

Running

  1. ros2 service call /delete_entity 'gazebo_msgs/DeleteEntity' '{name: "robot"}'
  2. cd ~/ros2_ws/
  3. colcon build --symlink-install
  4. source install/setup.bash
  5. ros2 launch my_robot_bringup my_robot.launch.py

Results in a limp joint 2.

Animation

This is the output I see

user:~/ros2_ws$ ros2 launch my_robot_bringup my_robot.launch.py
[INFO] [launch]: All log files can be found below /home/user/.ros/log/2022-09-22-04-20-55-206940-2_xterm-12854
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [spawn_entity.py-1]: process started with pid [12856]
[INFO] [robot_state_publisher-2]: process started with pid [12858]
[robot_state_publisher-2] Link link1 had 1 children
[robot_state_publisher-2] Link link2 had 1 children
[robot_state_publisher-2] Link link3 had 1 children
[robot_state_publisher-2] Link camera_link had 1 children
[robot_state_publisher-2] Link camera_link_optical had 0 children
[robot_state_publisher-2] [INFO] [1663820455.558896823] [robot_state_publisher]: got segment camera_link
[robot_state_publisher-2] [INFO] [1663820455.558966647] [robot_state_publisher]: got segment camera_link_optical
[robot_state_publisher-2] [INFO] [1663820455.558983247] [robot_state_publisher]: got segment link1
[robot_state_publisher-2] [INFO] [1663820455.558994292] [robot_state_publisher]: got segment link2
[robot_state_publisher-2] [INFO] [1663820455.559004066] [robot_state_publisher]: got segment link3
[robot_state_publisher-2] [INFO] [1663820455.559013773] [robot_state_publisher]: got segment world
[spawn_entity.py-1] [INFO] [1663820456.012716680] [spawn_entity]: Spawn Entity started
[spawn_entity.py-1] [INFO] [1663820456.013284729] [spawn_entity]: Loading entity published on topic robot_description
[spawn_entity.py-1] [INFO] [1663820456.015276482] [spawn_entity]: Waiting for entity xml on robot_description
[spawn_entity.py-1] [INFO] [1663820456.017497448] [spawn_entity]: Waiting for service /spawn_entity, timeout = 5
[spawn_entity.py-1] [INFO] [1663820456.018135883] [spawn_entity]: Waiting for service /spawn_entity
[spawn_entity.py-1] [INFO] [1663820456.021912376] [spawn_entity]: Calling service /spawn_entity
[spawn_entity.py-1] [INFO] [1663820456.286330846] [spawn_entity]: Spawn status: SpawnEntity: Successfully spawned entity [robot]
[spawn_entity.py-1] /opt/ros/galactic/lib/python3.8/site-packages/rclpy/qos.py:307: UserWarning: DurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL is deprecated. Use DurabilityPolicy.TRANSIENT_LOCAL instead.
[spawn_entity.py-1]   warnings.warn(
[INFO] [spawn_entity.py-1]: process has finished cleanly [pid 12856]
[INFO] [spawner-3]: process started with pid [12888]
[spawner-3] [INFO] [1663820456.834249780] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[spawner-3] [INFO] [1663820457.023442423] [spawner_joint_state_broadcaster]: Configured and started joint_state_broadcaster
[INFO] [spawner-3]: process has finished cleanly [pid 12888]
[INFO] [spawner-4]: process started with pid [12898]
[spawner-4] [INFO] [1663820457.536470001] [spawner_forward_position_controller]: Loaded forward_position_controller
[spawner-4] [INFO] [1663820457.725787117] [spawner_forward_position_controller]: Configured and started forward_position_controller
[INFO] [spawner-4]: process has finished cleanly [pid 12898]

And running

user:~/ros2_ws$ ros2 topic pub /forward_position_controller/commands std_msgs/msg/Float64MultiArray "data:
- 1.57
- -1.57" -1
publisher: beginning loop
publishing #1: std_msgs.msg.Float64MultiArray(layout=std_msgs.msg.MultiArrayLayout(dim=[], data_offset=0), data=[1.57, -1.57])

doesn’t seem to have an effect.

2

Hello @pwspania,

sometimes small differences in the files can cause this kind of effect, for instance a yaml files that has an extra leading space or is not properly indented. Same with xml files (.xacro files have xml format) , elements and attributes must be formatted following a specific structure. I am not saying that this is the problem in this case, but I could be one possible cause. Other possible cause is that the launch file is referring to a wrong .yaml filename.
But let’s start by verifying your .yaml and .xacro files are correct. Please post them here so that I can have a look at them. Also please mention what the filename of each of them is.

Regards,

Roberto

3

Without having changed any code, I reloaded the course today and re-ran the commands listed above but this time it worked as expected. Although the robot doesn’t move smoothly in gazebo, it just jumps instantly to the requested position. Is there something I had to run to restart gazebo besides just the /delete_entity service?

For reference here are the two files you asked for:

src/my_robot_bringup/config/controller_configuraiton.yaml 
# Controller manager configuration
controller_manager:
  ros__parameters:
    update_rate: 10  # Hz

    ### Controllers to be initialized at startup ###
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    forward_position_controller:
      type: forward_command_controller/ForwardCommandController

    position_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController


### Properties of the controllers that we will use and definition of joints to use ###
forward_position_controller:
  ros__parameters:
    joints:
      - joint1
      - joint2
    interface_name: position


position_trajectory_controller:
  ros__parameters:
    joints:
      - joint1
      - joint2

    command_interfaces:
      - position

    state_interfaces:
      - position

    state_publish_rate: 200.0 # Defaults to 50
    action_monitor_rate: 20.0 # Defaults to 20

    allow_partial_joints_goal: false # Defaults to false
    open_loop_control: true
    allow_integration_in_goal_trajectories: true
    constraints:
      stopped_velocity_tolerance: 0.01 # Defaults to 0.01
      goal_time: 0.0 # Defaults to 0.0 (start immediately)
src/ros2_control_course/unit2/rrbot_unit2/urdf/.xacro 
<?xml version="1.0"?>
<!-- Revolute-Revolute Manipulator -->
<robot name="rrbot" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- Constants for robot dimensions -->
  <xacro:property name="PI" value="3.1415926535897931"/>
  <!-- arbitrary value for mass -->
  <xacro:property name="mass" value="1"/>
  <!-- Square dimensions (widthxwidth) of beams -->
  <xacro:property name="width" value="0.1"/>
  <!-- Link 1 -->
  <xacro:property name="height1" value="2"/>
  <!-- Link 2 -->
  <xacro:property name="height2" value="1"/>
  <!-- Link 3 -->
  <xacro:property name="height3" value="1"/>
  <!-- Size of square 'camera' box -->
  <xacro:property name="camera_link" value="0.05"/>
  <!-- Space btw top of beam and the each joint -->
  <xacro:property name="axel_offset" value="0.05"/>

  <!-- Import all Gazebo-customization elements, including Gazebo colors -->
  <xacro:include filename="$(find rrbot_unit2)/urdf/rrbot.gazebo"/>
  <!-- Import Rviz colors -->
  <xacro:include filename="$(find rrbot_unit2)/urdf/materials.xacro"/>

  <!-- Used for fixing robot to Gazebo 'base_link' -->
  <link name="world"/>

  <joint name="fixed" type="fixed">
    <parent link="world"/>
    <child link="link1"/>
  </joint>

  <!-- Base Link -->
  <link name="link1">
    <collision>
      <origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
      <geometry>
        <box size="${width} ${width} ${height1}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
      <geometry>
        <box size="${width} ${width} ${height1}"/>
      </geometry>
      <material name="orange"/>
    </visual>

    <inertial>
      <origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
      <mass value="${mass}"/>
      <inertia ixx="${mass / 12.0 * (width*width + height1*height1)}" ixy="0.0" ixz="0.0" iyy="${mass / 12.0 * (height1*height1 + width*width)}" iyz="0.0" izz="${mass / 12.0 * (width*width + width*width)}"/>
    </inertial>
  </link>

  <joint name="joint1" type="continuous">
    <parent link="link1"/>
    <child link="link2"/>
    <origin xyz="0 ${width} ${height1 - axel_offset}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="0.7"/>
  </joint>

  <!-- Middle Link -->
  <link name="link2">
    <collision>
      <origin xyz="0 0 ${height2/2 - axel_offset}" rpy="0 0 0"/>
      <geometry>
        <box size="${width} ${width} ${height2}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 ${height2/2 - axel_offset}" rpy="0 0 0"/>
      <geometry>
        <box size="${width} ${width} ${height2}"/>
      </geometry>
      <material name="black"/>
    </visual>

    <inertial>
      <origin xyz="0 0 ${height2/2 - axel_offset}" rpy="0 0 0"/>
      <mass value="${mass}"/>
      <inertia ixx="${mass / 12.0 * (width*width + height2*height2)}" ixy="0.0" ixz="0.0" iyy="${mass / 12.0 * (height2*height2 + width*width)}" iyz="0.0" izz="${mass / 12.0 * (width*width + width*width)}"/>
    </inertial>
  </link>

  <joint name="joint2" type="continuous">
    <parent link="link2"/>
    <child link="link3"/>
    <origin xyz="0 ${width} ${height2 - axel_offset*2}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="0.7"/>
  </joint>

  <!-- Top Link -->
  <link name="link3">
    <collision>
      <origin xyz="0 0 ${height3/2 - axel_offset}" rpy="0 0 0"/>
      <geometry>
        <box size="${width} ${width} ${height3}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 ${height3/2 - axel_offset}" rpy="0 0 0"/>
      <geometry>
        <box size="${width} ${width} ${height3}"/>
      </geometry>
      <material name="orange"/>
    </visual>

    <inertial>
      <origin xyz="0 0 ${height3/2 - axel_offset}" rpy="0 0 0"/>
      <mass value="${mass}"/>
      <inertia ixx="${mass / 12.0 * (width*width + height3*height3)}" ixy="0.0" ixz="0.0" iyy="${mass / 12.0 * (height3*height3 + width*width)}" iyz="0.0" izz="${mass / 12.0 * (width*width + width*width)}"/>
    </inertial>
  </link>

  <!-- Hokuyo Laser 

  <joint name="hokuyo_joint" type="fixed">
    <axis xyz="0 1 0"/>
    <origin xyz="0 0 ${height3 - axel_offset/2}" rpy="0 0 0"/>
    <parent link="link3"/>
    <child link="hokuyo_link"/>
  </joint>

  <link name="hokuyo_link">
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.1 0.1 0.1"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="$(find rrbot_description)/meshes/hokuyo.dae"/>
      </geometry>
    </visual>

    <inertial>
      <mass value="1e-5"/>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
    </inertial>
  </link>

  -->

  <joint name="camera_joint" type="fixed">
    <axis xyz="0 1 0"/>
    <origin xyz="${camera_link} 0 ${height3 - axel_offset*2}" rpy="0 0 0"/>
    <parent link="link3"/>
    <child link="camera_link"/>
  </joint>

  <!-- Camera -->
  <link name="camera_link">
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="${camera_link} ${camera_link} ${camera_link}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="${camera_link} ${camera_link} ${camera_link}"/>
      </geometry>
      <material name="red"/>
    </visual>

    <inertial>
      <mass value="1e-5"/>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
    </inertial>
  </link>

  <!-- generate an optical frame http://www.ros.org/reps/rep-0103.html#suffix-frames
   so that ros and opencv can operate on the camera frame correctly -->
  <joint name="camera_optical_joint" type="fixed">
    <!-- these values have to be these values otherwise the gazebo camera image won't
     be aligned properly with the frame it is supposedly originating from -->
    <origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
    <parent link="camera_link"/>
    <child link="camera_link_optical"/>
  </joint>

  <link name="camera_link_optical"></link>

  <ros2_control name="GazeboSystem" type="system">
    
    <hardware>
      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
    </hardware>
    
    <joint name="joint1">
      <command_interface name="position">
        <param name="min">-6.28</param>
        <param name="max">6.28</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    
    <joint name="joint2">
      <command_interface name="position">
        <param name="min">-6.28</param>
        <param name="max">6.28</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    
  </ros2_control>

  <!-- Gazebo's ros2_control plugin -->
  <gazebo>
    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
      <robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
      <parameters>$(find my_robot_bringup)/config/controller_configuration.yaml</parameters>
    </plugin>
  </gazebo>

</robot>
4

I had the same problem until today, where only joint 1 would move, and adding a joint 2 to the controller_configuration.yaml file would cause the forward_position_controller to stay inactive. I didn’t change my xacro or yaml files, but now both joints respond to the command:

ros2 topic pub /forward_position_controller/commands std_msgs/msg/Float64MultiArray "data:

  • 1.57
  • 0" -1

Did something change in the backend?

5

Hello @pwspania,

I am glad to hear it is working now!

One possible reason that could explain what happened is building and sourcing the workspace. In ROS2 you have to always build and source your workspace after every change you make, even to configuration files. If you don’t, the changes will not take effect.

Regards,

Roberto

6

Hello @deenj,

welcome to the community!

In order to be able to debug the issue I need to be able to verify that your .yaml and .xacro files are correct. Please post them here so that I can have a look at them. Also please mention what the filename of each of them is.
Also, as explained to @pwspania it is very important that you build and source your workspace after every change otherwise the old files are the ones that are considered and not the new ones that include your latest changes.

Regards,

Roberto

7

This topic was automatically closed 7 days after the last reply. New replies are no longer allowed.