How can I make a joint work with Joint State Subscriber?

hijimasa commented 3 weeks ago

[x] I am at the right place and my issue is directly related to ROS#. General technical questions I would post e.g. at ROS Answers or Stack Overflow. For library-specific questions I would look for help in the corresponding library forums.
[x] I have thoroughly read the Contributing Guideline and writing this issue is the right thing to do in my case.

[x] I searched the Wiki, open and closed issues for an answer. I tried my best to find the answer by myself without success. I believe that the discussion we will have in this issue, and the solutions we might find, will help me, and likely other community members who have a similar problem.

Here is my question: I'm attempting to simulate a mobile robot using ROS 2 Humble and Unity. I have set up a robot model with reference to the following URL and am trying to make the robot work using Joint State instead of Joy messages, but it is not working. https://github.com/siemens/ros-sharp/wiki/User_App_ROS_TransferURDFFromROS

The using URDF is as follows.

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from /home/hijikata/colcon_ws/install/diffbot_description/share/diffbot_description/robots/diffbot.urdf.xacro | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<!-- Basic differential drive mobile base -->
<robot name="diffbot">
  <material name="gray">
    <color rgba="0.5 0.5 0.5 0.2"/>
  </material>
  <material name="ball">
    <color rgba="0.5 0.5 0.5 0.2"/>
  </material>
  <material name="white">
    <color rgba="1.0 1.0 1.0 1.0"/>
  </material>
  <link name="base_link"/>
  <joint name="body_joint" type="fixed">
    <parent link="base_link"/>
    <child link="body_link"/>
    <origin rpy="0 0 0" xyz="-0.07 0 0.07"/>
  </joint>
  <link name="body_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="1.0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.24 0.18 0.06"/>
      </geometry>
      <material name="gray"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.24 0.178 0.06"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_wheel_joint" type="continuous">
    <origin rpy="-1.5707963267948966 0 0" xyz="0 0.1 0.05"/>
    <parent link="base_link"/>
    <child link="left_wheel_link"/>
    <axis xyz="0 0 1"/>
  </joint>
  <link name="left_wheel_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.1"/>
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder length="0.02" radius="0.05"/>
      </geometry>
      <material name="gray"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.02" radius="0.05"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_wheel_joint" type="continuous">
    <origin rpy="-1.5707963267948966 0 0" xyz="0 -0.1 0.05"/>
    <parent link="base_link"/>
    <child link="right_wheel_link"/>
    <axis xyz="0 0 1"/>
  </joint>
  <link name="right_wheel_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.1"/>
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder length="0.02" radius="0.05"/>
      </geometry>
      <material name="gray"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.02" radius="0.05"/>
      </geometry>
    </collision>
  </link>
  <joint name="ball_joint" type="fixed">
    <parent link="base_link"/>
    <child link="ball_link"/>
    <origin rpy="0 0 0" xyz="-0.14 0 0.04"/>
  </joint>
  <link name="ball_link">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.04"/>
      </geometry>
      <material name="ball"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.04"/>
      </geometry>
    </collision>
  </link>
  <joint name="lidar_joint" type="fixed">
    <parent link="base_link"/>
    <child link="lidar_link"/>
    <origin rpy="0 0 0" xyz="0 0 0.2"/>
  </joint>
  <link name="lidar_link">
    <!-- If visual tag is enable, lidar's ray is blocked. -->
    <!--
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <cylinder radius="0.03" length="0.08" />
      </geometry>
      <material name="white" />
    </visual>
    -->
  </link>
  <joint name="camera_joint" type="fixed">
    <parent link="base_link"/>
    <child link="camera_link"/>
    <origin rpy="0 0 0" xyz="0.05 -0.1 0.1"/>
  </joint>
  <link name="camera_link">
    <!-- If visual tag is enable, camera's view is blocked. -->
    <!--
    <visual>
      <origin xyz="0 0 0" rpy="0 -1.57 0" />
      <geometry>
        <cylinder radius="0.01" length="0.01" />
      </geometry>
      <material name="white" />
    </visual>
    -->
  </link>
  <joint name="depth_camera_joint" type="fixed">
    <parent link="base_link"/>
    <child link="depth_camera_link"/>
    <origin rpy="0 0 0" xyz="0.05 0.1 0.1"/>
  </joint>
  <link name="depth_camera_link">
    <!-- If visual tag is enable, camera's view is blocked. -->
    <!--
    <visual>
      <origin xyz="0 0 0" rpy="0 -1.57 0" />
      <geometry>
        <cylinder radius="0.01" length="0.01" />
      </geometry>
      <material name="white" />
    </visual>
    -->
  </link>
  <ros2_control name="diffbot" type="system">
    <hardware>
      <plugin>topic_based_ros2_control/TopicBasedSystem</plugin>
      <param name="joint_commands_topic">/diffbot/joint_command</param>
      <param name="joint_states_topic">/diffbot/joint_states</param>
      <param name="sum_wrapped_joint_states">true</param>
    </hardware>
    <joint name="left_wheel_joint">
      <command_interface name="velocity">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="right_wheel_joint">
      <command_interface name="velocity">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
  </ros2_control>
</robot>

The topic given as command is as follows.

ros2 topic pub /diffbot/joint_commands sensor_msgs/msg/JointState "header:
  stamp:
    sec: 0
    nanosec: 0
  frame_id: ''
name: ["left_wheel_joint", "right_wheel_joint"]
position: []
velocity: [1.0, 1.0]
effort: []"

Is there something wrong with the way the commands are given or the robot model is set up?

hijimasa commented 3 weeks ago

I found that JointStateSubscriber.cs only accepts a position command. I think JointStateSubscriber should also accept velocity and effort commands.

memrecakal commented 3 weeks ago

Hi @hijimasa,

JointStateSubscriber doesn't fetch the velocity and effort data from MessageTypes.Sensor.JointState, but those fields are still present in the base message class, they're just not used. JointStateSubscriber triggers JointStateWriter to update the robot's kinematics. JointStateWriter then triggers the virtual OnUpdateJointStatefrom. The update method is overridden by the specific Unity joint type, continuous, prismatic or revolute. These updates are applied directly to the Unity transform component, not the rigidbody. This is the intended behaviour as JointStateSubsriber is designed for visualisation and kinematics, not simulation.

Updating velocity and effort requires you to manipulate the rigidbody component, as you can only change position and rotation via the transform component. I would not recommend combining the two at runtime as this can lead to unrealistic behaviour.

Please see the message handling wiki page.

"JointState" is the current state of a joint, not it's set point. Nothing remains to simulate here.

hijimasa commented 3 weeks ago

Hi @memrecakal , If the velocity and effort data exist in the base message class, shouldn't we use them to simulate them? I just thought that using topic_based_ros2_control would create a better simulation environment, but am I wrong?

hijimasa commented 3 weeks ago

Well, if that's how it's designed, so be it. I'll look for another tool.

siemens / ros-sharp

How can I make a joint work with Joint State Subscriber? #467