[dynamixel_general_hw] Reflect joint limits

dynamixel_general_hw/CMakeLists.txt

@@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS
   dynamixel_workbench_msgs
   dynamixel_workbench_toolbox
   hardware_interface
+  joint_limits_interface
   message_generation
   roscpp
   std_msgs
@@ -31,6 +32,7 @@ catkin_package(
   dynamixel_workbench_msgs
   dynamixel_workbench_toolbox
   hardware_interface
+  joint_limits_interface
   message_runtime
   roscpp
   std_msgs

dynamixel_general_hw/include/dynamixel_general_hw/dynamixel_general_hw.h

@@ -12,10 +12,15 @@
 // ROS base
 #include <ros/ros.h>
 #include <ros/callback_queue.h>
+#include <urdf/model.h>
 
 // ros_control
 #include <hardware_interface/robot_hw.h>
 #include <transmission_interface/transmission_interface_loader.h>
+#include <joint_limits_interface/joint_limits.h>
+#include <joint_limits_interface/joint_limits_urdf.h>
+#include <joint_limits_interface/joint_limits_rosparam.h>
+#include <joint_limits_interface/joint_limits_interface.h>
 
 // ROS msg and srv
 #include <dynamixel_workbench_msgs/DynamixelCommand.h>
@@ -58,6 +63,15 @@ class DynamixelGeneralHw : public hardware_interface::RobotHW
   transmission_interface::RobotTransmissions robot_transmissions_;
   std::unique_ptr<transmission_interface::TransmissionInterfaceLoader> transmission_loader_;
 
+  // Joint limits interface
+  std::vector<std::string> jnt_names_;
+  joint_limits_interface::PositionJointSaturationInterface pos_jnt_sat_interface_;
+  joint_limits_interface::VelocityJointSaturationInterface vel_jnt_sat_interface_;
+  joint_limits_interface::EffortJointSaturationInterface eff_jnt_sat_interface_;
+  joint_limits_interface::PositionJointSoftLimitsInterface pos_jnt_soft_interface_;
+  joint_limits_interface::VelocityJointSoftLimitsInterface vel_jnt_soft_interface_;
+  joint_limits_interface::EffortJointSoftLimitsInterface eff_jnt_soft_interface_;
+
   // Actuator interface to transmission loader
   hardware_interface::ActuatorStateInterface actr_state_interface_;
   hardware_interface::PositionActuatorInterface pos_actr_interface_;
@@ -117,7 +131,7 @@ class DynamixelGeneralHw : public hardware_interface::RobotHW
 
   void readDynamixelState(void);
   void read(void);
-  void write(void);
+  void write(const ros::Time& time, const ros::Duration& period);
 
   bool isJntCmdIgnored(void);
 

dynamixel_general_hw/package.xml

@@ -14,6 +14,7 @@
   <depend>dynamixel_workbench_msgs</depend>
   <depend>dynamixel_workbench_toolbox</depend>
   <depend>hardware_interface</depend>
+  <depend>joint_limits_interface</depend>
   <depend>roscpp</depend>
   <depend>std_msgs</depend>
   <depend>transmission_interface</depend>

dynamixel_general_hw/src/dynamixel_general_control_node.cpp

@@ -91,7 +91,7 @@ int main(int argc, char** argv)
     const bool controller_ignored = dxl_hw.isJntCmdIgnored();
     const bool reset_controllers = (prev_controller_ignored && !controller_ignored);
     cm.update(now, elapsed_time, reset_controllers);
-    dxl_hw.write();
+    dxl_hw.write(now, elapsed_time);
     prev_time = now;
     prev_controller_ignored = controller_ignored;
 

dynamixel_general_hw/src/dynamixel_general_hw.cpp

@@ -382,7 +382,7 @@ bool DynamixelGeneralHw::initRosInterface(void)
   actr_cmd_pos_.resize(actr_names_.size(), 0);
   actr_cmd_vel_.resize(actr_names_.size(), 0);
   actr_cmd_eff_.resize(actr_names_.size(), 0);
-  for (int i = 0; i < dynamixel_.size(); i++)
+  for (int i = 0; i < actr_names_.size(); i++)
   {
     hardware_interface::ActuatorStateHandle state_handle(actr_names_[i], &actr_curr_pos_[i], &actr_curr_vel_[i], &actr_curr_eff_[i]);
     actr_state_interface_.registerHandle(state_handle);
@@ -448,6 +448,7 @@ bool DynamixelGeneralHw::initRosInterface(void)
   }
 
   // Load transmissions composed of target actuators
+  jnt_names_.clear();
   for (const transmission_interface::TransmissionInfo& info : infos)
   {
     if (std::find(actr_names_.begin(), actr_names_.end(), info.actuators_[0].name_) != actr_names_.end())
@@ -466,9 +467,69 @@ bool DynamixelGeneralHw::initRosInterface(void)
         ROS_ERROR_STREAM("Error loading transmission: " << info.name_);
         return false;
       }
-      else
+      ROS_INFO_STREAM("Loaded transmission: " << info.name_);
+      for (const transmission_interface::JointInfo& joint : info.joints_)
       {
-        ROS_INFO_STREAM("Loaded transmission: " << info.name_);
+        jnt_names_.push_back(joint.name_);
+      }
+    }
+  }
+
+  // Prepare joint limits interfaces
+  urdf::Model urdf_model;
+  if (!urdf_model.initString(urdf_string))
+  {
+    ROS_ERROR("Error parsing URDF");
+    return false;
+  }
+  hardware_interface::PositionJointInterface* pos_if = get<hardware_interface::PositionJointInterface>();
+  hardware_interface::VelocityJointInterface* vel_if = get<hardware_interface::VelocityJointInterface>();
+  hardware_interface::EffortJointInterface* eff_if = get<hardware_interface::EffortJointInterface>();
+  for (const std::string& jnt_name : jnt_names_)
+  {
+    joint_limits_interface::JointLimits limits;
+    joint_limits_interface::SoftJointLimits soft_limits;
+    bool has_limits_urdf = getJointLimits(urdf_model.getJoint(jnt_name), limits);
+    bool has_limits_param = getJointLimits(jnt_name, nh_, limits);
+    if (has_limits_urdf || has_limits_param)
+    {
+      if (pos_if)
+      {
+        joint_limits_interface::PositionJointSaturationHandle pos_sat_handle(pos_if->getHandle(jnt_name), limits);
+        pos_jnt_sat_interface_.registerHandle(pos_sat_handle);
+      }
+      if (vel_if)
+      {
+        joint_limits_interface::VelocityJointSaturationHandle vel_sat_handle(vel_if->getHandle(jnt_name), limits);
+        vel_jnt_sat_interface_.registerHandle(vel_sat_handle);
+      }
+      if (eff_if)
+      {
+        joint_limits_interface::EffortJointSaturationHandle eff_sat_handle(eff_if->getHandle(jnt_name), limits);
+        eff_jnt_sat_interface_.registerHandle(eff_sat_handle);
+      }
+    }
+    has_limits_urdf = getSoftJointLimits(urdf_model.getJoint(jnt_name), soft_limits);
+    has_limits_param = getSoftJointLimits(jnt_name, nh_, soft_limits);
+    if (has_limits_urdf || has_limits_param)
+    {
+      if (pos_if)
+      {
+        joint_limits_interface::PositionJointSoftLimitsHandle pos_soft_handle(pos_if->getHandle(jnt_name), limits,
+                                                                              soft_limits);
+        pos_jnt_soft_interface_.registerHandle(pos_soft_handle);
+      }
+      if (vel_if)
+      {
+        joint_limits_interface::VelocityJointSoftLimitsHandle vel_soft_handle(vel_if->getHandle(jnt_name), limits,
+                                                                              soft_limits);
+        vel_jnt_soft_interface_.registerHandle(vel_soft_handle);
+      }
+      if (eff_if)
+      {
+        joint_limits_interface::EffortJointSoftLimitsHandle eff_soft_handle(eff_if->getHandle(jnt_name), limits,
+                                                                            soft_limits);
+        eff_jnt_soft_interface_.registerHandle(eff_soft_handle);
       }
     }
   }
@@ -773,7 +834,7 @@ void DynamixelGeneralHw::read(void)
   is_hold_pos_ = is_hold_pos_raw_;
 }
 
-void DynamixelGeneralHw::write(void)
+void DynamixelGeneralHw::write(const ros::Time& time, const ros::Duration& period)
 {
   std::lock_guard<std::mutex> lock(mtx_);
 
@@ -792,6 +853,10 @@ void DynamixelGeneralHw::write(void)
     {
       if (robot_transmissions_.get<transmission_interface::JointToActuatorPositionInterface>())
       {
+        // Enforce joint position limits
+        pos_jnt_sat_interface_.enforceLimits(period);
+        pos_jnt_soft_interface_.enforceLimits(period);
+
         // Propagate joint position commands to actuators
         robot_transmissions_.get<transmission_interface::JointToActuatorPositionInterface>()->propagate();
 
@@ -834,6 +899,10 @@ void DynamixelGeneralHw::write(void)
       }
       if (robot_transmissions_.get<transmission_interface::JointToActuatorVelocityInterface>())
       {
+        // Enforce joint velocity limits
+        vel_jnt_sat_interface_.enforceLimits(period);
+        vel_jnt_soft_interface_.enforceLimits(period);
+
         // Propagate joint velocity commands to actuators
         robot_transmissions_.get<transmission_interface::JointToActuatorVelocityInterface>()->propagate();
 
@@ -913,6 +982,10 @@ void DynamixelGeneralHw::write(void)
       if (is_calc_effort_ && is_current_ctrl_ &&
           robot_transmissions_.get<transmission_interface::JointToActuatorEffortInterface>())
       {
+        // Enforce joint effort limits
+        eff_jnt_sat_interface_.enforceLimits(period);
+        eff_jnt_soft_interface_.enforceLimits(period);
+
         // Propagate joint effort commands to actuators
         robot_transmissions_.get<transmission_interface::JointToActuatorEffortInterface>()->propagate();
 

dynamixel_general_hw/urdf/sample4.urdf

@@ -27,8 +27,6 @@
   <!--
     Joint limits are example values for XC330-T288-T with sample_trans (it is written later).
     velocity: (No Load Speed [rad/s]) / (mechanicalReduction in sample_trans) = (65 [rev/min] * 2 * pi / 60) / 1.0
-    Note that the controller used in this sample (velocity_controllers/JointGroupVelocityController) is simple and does not load those limits.
-    If you want to limit the real actuator motion, find/write a controller loading those limits (limit in joint side) or set Min/Max Position Limit in dynamixel_info.yaml (limit in actuator side)
   -->
   <joint name="sample_joint" type="continuous">
     <origin rpy="0 0 0" xyz="0 0 0.1"/>

dynamixel_general_hw/urdf/sample5.urdf

@@ -28,8 +28,6 @@
     Joint limits are example values for XC330-T288-T with sample_trans (it is written later).
     effort: (Stall Torque [Nm]) * (mechanicalReduction in sample_trans) = 0.92 * 1.0
     velocity: (No Load Speed [rad/s]) / (mechanicalReduction in sample_trans) = (65 [rev/min] * 2 * pi / 60) / 1.0
-    Note that the controller used in this sample (effort_controllers/JointGroupEffortController) is simple and does not load those limits.
-    If you want to limit the real actuator motion, find/write a controller loading those limits (limit in joint side) or set Current Limit in dynamixel_info.yaml (limit in actuator side)
   -->
   <joint name="sample_joint" type="continuous">
     <origin rpy="0 0 0" xyz="0 0 0.1"/>

dynamixel_general_hw/urdf/sample6.urdf

@@ -28,8 +28,6 @@
     Joint limits are example values for XC330-T288-T with sample_trans (it is written later).
     effort: (Stall Torque [Nm]) * (mechanicalReduction in sample_trans) = 0.92 * 1.0
     velocity: (No Load Speed [rad/s]) / (mechanicalReduction in sample_trans) = (65 [rev/min] * 2 * pi / 60) / 1.0
-    Note that the controller used in this sample (effort_controllers/JointGroupEffortController) is simple and does not load those limits.
-    If you want to limit the real actuator motion, find/write a controller loading those limits (limit in joint side) or set Current Limit in dynamixel_info.yaml (limit in actuator side)
   -->
   <joint name="sample_joint" type="continuous">
     <origin rpy="0 0 0" xyz="0 0 0.1"/>