Update control and state interfaces for non PID actuators

doc/index.rst

@@ -81,6 +81,48 @@ An overview is available through the "camera" demo.
 
 For additional information refer to the ``mujoco_ros2_control_demos/mujoco_models`` for examples.
 
+Controller Options
+------------------
+The drivers support two modes of control, either wrapping Mujoco's actuation interface or wrapped PID control of joint interfaces.
+
+Mujoco's built-in controllers essentially provide PD control of the actuator's interface.
+For more information refer to the documentation in their `actuation model <https://mujoco.readthedocs.io/en/stable/computation/index.html#geactuation>`_ docs.
+To use their controllers, actuators must be added and tuned directly in the MJCF XML, as in many of the demo examples.
+No additional configuration is needed from the drivers side, as commands will be sent directly to the actuator's `ctrl` data.
+For example,
+
+.. code-block:: XML
+
+  <actuator>
+      <position name="slider_to_cart_pos" joint="slider_to_cart" kp="5" dampratio="1" />
+      <velocity name="slider_to_cart_vel" joint="slider_to_cart" kv="5" />
+  </actuator>
+
+To use the PID wrapper, users can omit the actuators from their MJCF descriptions, and instead specify gains directly in the ros2_control xacro.
+An example is provided in the `examples repo <https://github.com/moveit/mujoco_ros2_control_examples/blob/main/panda_resources/panda_moveit_config/config/panda.ros2_control.xacro>`_.
+In this case, we note that the `command_interface` _must_ be suffixed by `_pid`,
+
+.. code-block:: XML
+
+  <joint name="panda_joint1">
+      <xacro:if value="${ros2_control_hardware_type == 'mujoco'}">
+        <param name="position_kp">3000</param>
+        <param name="position_ki">1</param>
+        <param name="position_kd">100</param>
+        <param name="position_i_max">10000</param>
+        <command_interface name="position_pid"/>
+      </xacro:if>
+      <xacro:unless value="${ros2_control_hardware_type == 'mujoco'}">
+        <command_interface name="position"/>
+      </xacro:unless>
+      <state_interface name="position">
+        <param name="initial_value">${initial_positions['panda_joint1']}</param>
+      </state_interface>
+      <state_interface name="velocity">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+  </joint>
+
 Specify the location of Mujoco models and the controller configuration file
 ----------------------------------------------------------------------------
 You need to pass parameters for paths as shown in the following example.

mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system.hpp

@@ -82,6 +82,7 @@ class MujocoSystem : public MujocoSystemInterface
     int mj_joint_type;
     int mj_pos_adr;
     int mj_vel_adr;
+    int mj_actuator_id;
   };
 
   template <typename T>

mujoco_ros2_control/src/mujoco_system.cpp

@@ -42,7 +42,7 @@ hardware_interface::return_type MujocoSystem::read(
   {
     joint_state.position = mj_data_->qpos[joint_state.mj_pos_adr];
     joint_state.velocity = mj_data_->qvel[joint_state.mj_vel_adr];
-    joint_state.effort = mj_data_->qfrc_applied[joint_state.mj_vel_adr];
+    joint_state.effort = mj_data_->actuator_force[joint_state.mj_vel_adr];
   }
 
   // IMU Sensor data
@@ -114,7 +114,7 @@ hardware_interface::return_type MujocoSystem::write(
       }
       else
       {
-        mj_data_->qpos[joint_state.mj_pos_adr] = joint_state.position_command;
+        mj_data_->ctrl[joint_state.mj_actuator_id] = joint_state.position_command;
       }
     }
 
@@ -129,7 +129,7 @@ hardware_interface::return_type MujocoSystem::write(
       }
       else
       {
-        mj_data_->qvel[joint_state.mj_vel_adr] = joint_state.velocity_command;
+        mj_data_->ctrl[joint_state.mj_actuator_id] = joint_state.velocity_command;
       }
     }
 
@@ -184,12 +184,32 @@ void MujocoSystem::register_joints(
       continue;
     }
 
+    // Try to locate the matching actuator id for the joint, if available
+    int mujoco_actuator_id = -1;
+    for (int i = 0; i < mj_model_->nu; ++i)
+    {
+      // If it is the correct type and matches the joint id, we're done
+      if (
+        mj_model_->actuator_trntype[i] == mjTRN_JOINT &&
+        mj_model_->actuator_trnid[2 * i] == mujoco_joint_id)
+      {
+        mujoco_actuator_id = i;
+        break;
+      }
+    }
+    if (mujoco_actuator_id == -1)
+    {
+      // This isn't a failure the joint just won't be controllable
+      RCLCPP_WARN_STREAM(logger_, "No actuator found for joint: " << joint.name);
+    }
+
     // save information in joint_states_ variable
     JointState joint_state;
     joint_state.name = joint.name;
     joint_state.mj_joint_type = mj_model_->jnt_type[mujoco_joint_id];
     joint_state.mj_pos_adr = mj_model_->jnt_qposadr[mujoco_joint_id];
     joint_state.mj_vel_adr = mj_model_->jnt_dofadr[mujoco_joint_id];
+    joint_state.mj_actuator_id = mujoco_actuator_id;
 
     joint_states_.at(joint_index) = joint_state;
     JointState &last_joint_state = joint_states_.at(joint_index);

mujoco_ros2_control_demos/examples/example_position.cpp

@@ -114,17 +114,17 @@ int main(int argc, char * argv[])
   point.positions[0] = 0.0;
 
   trajectory_msgs::msg::JointTrajectoryPoint point2;
-  point2.time_from_start = rclcpp::Duration::from_seconds(1.0);
+  point2.time_from_start = rclcpp::Duration::from_seconds(5.0);
   point2.positions.resize(joint_names.size());
-  point2.positions[0] = -1.0;
+  point2.positions[0] = -5.0;
 
   trajectory_msgs::msg::JointTrajectoryPoint point3;
-  point3.time_from_start = rclcpp::Duration::from_seconds(2.0);
+  point3.time_from_start = rclcpp::Duration::from_seconds(10.0);
   point3.positions.resize(joint_names.size());
-  point3.positions[0] = 1.0;
+  point3.positions[0] = 5.0;
 
   trajectory_msgs::msg::JointTrajectoryPoint point4;
-  point4.time_from_start = rclcpp::Duration::from_seconds(3.0);
+  point4.time_from_start = rclcpp::Duration::from_seconds(15.0);
   point4.positions.resize(joint_names.size());
   point4.positions[0] = 0.0;
 

mujoco_ros2_control_demos/examples/example_velocity.cpp

@@ -41,11 +41,11 @@ int main(int argc, char * argv[])
   publisher->publish(commands);
   std::this_thread::sleep_for(1s);
 
-  commands.data[0] = 1;
+  commands.data[0] = 0.5;
   publisher->publish(commands);
   std::this_thread::sleep_for(1s);
 
-  commands.data[0] = -1;
+  commands.data[0] = -0.5;
   publisher->publish(commands);
   std::this_thread::sleep_for(1s);
 

mujoco_ros2_control_demos/launch/camera_example.launch.py

@@ -53,13 +53,13 @@ def generate_launch_description():
     )
 
     load_joint_state_controller = ExecuteProcess(
-        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', '--use-sim-time',
              'joint_state_broadcaster'],
         output='screen'
     )
 
     load_position_controller = ExecuteProcess(
-        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', '--use-sim-time',
              'position_controller'],
         output='screen'
     )
@@ -74,9 +74,11 @@ def generate_launch_description():
     )
 
     return LaunchDescription([
+        node_mujoco_ros2_control,
+        node_robot_state_publisher,
         RegisterEventHandler(
             event_handler=OnProcessStart(
-                target_action=node_mujoco_ros2_control,
+                target_action=node_robot_state_publisher,
                 on_start=[load_joint_state_controller],
             )
         ),
@@ -86,7 +88,10 @@ def generate_launch_description():
                 on_exit=[load_position_controller],
             )
         ),
-        node_mujoco_ros2_control,
-        node_robot_state_publisher,
-        rviz_node,
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=load_position_controller,
+                on_exit=[rviz_node],
+            )
+        ),
     ])

mujoco_ros2_control_demos/mujoco_models/test_camera.xml

@@ -24,7 +24,7 @@
     </worldbody>
 
     <actuator>
-        <motor joint="camera_joint" ctrlrange="-1 1" gear="100"/>
+        <position name="camera_joint" joint="camera_joint" kp="5000" dampratio="1.0"/>
     </actuator>
 
 </mujoco>

mujoco_ros2_control_demos/mujoco_models/test_cart.xml

@@ -25,4 +25,8 @@
     <contact>
         <exclude body1="slideBar" body2="cart" />
     </contact>
+    <actuator>
+        <position name="slider_to_cart_pos" joint="slider_to_cart" kp="5" dampratio="1" />
+        <velocity name="slider_to_cart_vel" joint="slider_to_cart" kv="5" />
+    </actuator>
 </mujoco>

mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml

@@ -31,4 +31,8 @@
         <exclude body1="chassis" body2="left_wheel" />
         <exclude body1="chassis" body2="right_wheel" />
     </contact>
+    <actuator>
+        <velocity ctrllimited="true" ctrlrange="-5 5" name="left_wheel_joint" joint="left_wheel_joint"/>
+        <velocity ctrllimited="true" ctrlrange="-5 5" name="right_wheel_joint" joint="right_wheel_joint"/>
+    </actuator>
 </mujoco>

mujoco_ros2_control_demos/mujoco_models/test_gripper_mimic_joint.xml

@@ -25,4 +25,16 @@
     <contact>
         <exclude body1="finger_right" body2="finger_left" />
     </contact>
+    <actuator>
+        <position name="right_finger_joint" joint="right_finger_joint" kp="5" dampratio="1" />
+    </actuator>
+    <equality>
+        <joint joint1="right_finger_joint" joint2="left_finger_joint" polycoef="0 1 0 0 0" />
+    </equality>
+    <tendon>
+        <fixed name="mimic">
+            <joint joint="right_finger_joint" coef="0.5" />
+            <joint joint="left_finger_joint" coef="0.5" />
+        </fixed>
+    </tendon>
 </mujoco>

mujoco_ros2_control_demos/mujoco_models/test_tricycle_drive.xml

@@ -40,4 +40,8 @@
         <exclude body1="chassis" body2="wheel_front_link" />
         <exclude body1="steering_link" body2="wheel_front_link" />
     </contact>
+    <actuator>
+        <position name="steering_joint" joint="steering_joint" kp="5" dampratio="1" />
+        <velocity ctrllimited="true" ctrlrange="-5 5" name="traction_joint" joint="traction_joint"/>
+    </actuator>
 </mujoco>

mujoco_ros2_control_demos/package.xml

@@ -25,6 +25,9 @@
  <depend>imu_sensor_broadcaster</depend>
 
  <exec_depend>position_controllers</exec_depend>
+ <exec_depend>diff_drive_controller</exec_depend>
+ <exec_depend>tricycle_controller</exec_depend>
+ <exec_depend>tricycle_steering_controller</exec_depend>
 
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_cmake_clang_format</test_depend>