Adds position and velocity motion generators to FrankaHWSim

Features:
- This commit adds the 'hardware_interface/PositionJointInterface' and
'hardware_interface/VelocityJointInterface' interfaces to the
FrankaHWSim module. When a user chooses these interfaces, motion
generators (i.e. PID controllers) are used for controlling the robot.
- This commit adds the 'transmission' arg to the 'panda_arm.urdf.xacro'
file. By doing this, users now can set the hardware_interface that
is used in the panda arm joints when loading the 'panda_arm.urdf.xacro'
file.
- This commit adds the `effort_joint_trajectory_controller` and
`position_joint_trajectory_controller` controllers to the franka_gazebo
`sim_controllers.yaml` config.

franka_description/robots/panda_arm.urdf.xacro

@@ -4,6 +4,7 @@
   <xacro:arg name="arm_id" default="panda" /> <!-- Name of this panda -->
   <xacro:arg name="hand"   default="false" /> <!-- Should a franka_gripper be mounted at the flange?" -->
   <xacro:arg name="gazebo" default="false" /> <!-- Is the robot being simulated in gazebo?" -->
+  <xacro:arg name="transmission" default="hardware_interface/EffortJointInterface" /> <!-- The transmission that is used in the gazebo joints -->
 
   <xacro:property name="arm_id" value="$(arg arm_id)" />
 
@@ -44,13 +45,13 @@
     </joint>
 
 
-    <xacro:gazebo-joint joint="${arm_id}_joint1" transmission="hardware_interface/EffortJointInterface" />
-    <xacro:gazebo-joint joint="${arm_id}_joint2" transmission="hardware_interface/EffortJointInterface" />
-    <xacro:gazebo-joint joint="${arm_id}_joint3" transmission="hardware_interface/EffortJointInterface" />
-    <xacro:gazebo-joint joint="${arm_id}_joint4" transmission="hardware_interface/EffortJointInterface" />
-    <xacro:gazebo-joint joint="${arm_id}_joint5" transmission="hardware_interface/EffortJointInterface" />
-    <xacro:gazebo-joint joint="${arm_id}_joint6" transmission="hardware_interface/EffortJointInterface" />
-    <xacro:gazebo-joint joint="${arm_id}_joint7" transmission="hardware_interface/EffortJointInterface" />
+    <xacro:gazebo-joint joint="${arm_id}_joint1" transmission="$(arg transmission)" />
+    <xacro:gazebo-joint joint="${arm_id}_joint2" transmission="$(arg transmission)" />
+    <xacro:gazebo-joint joint="${arm_id}_joint3" transmission="$(arg transmission)" />
+    <xacro:gazebo-joint joint="${arm_id}_joint4" transmission="$(arg transmission)" />
+    <xacro:gazebo-joint joint="${arm_id}_joint5" transmission="$(arg transmission)" />
+    <xacro:gazebo-joint joint="${arm_id}_joint6" transmission="$(arg transmission)" />
+    <xacro:gazebo-joint joint="${arm_id}_joint7" transmission="$(arg transmission)" />
 
     <xacro:transmission-franka-state arm_id="${arm_id}" />
     <xacro:transmission-franka-model arm_id="${arm_id}"

franka_gazebo/config/franka_hw_sim.yaml

@@ -10,3 +10,26 @@ franka_gripper:
 
   finger2:
     gains: { p: 100, i: 25, d: 20 }
+
+# Motion generators PID gains
+# NOTE: Needed since the gazebo_ros_control currently only directly support effort hardware
+# interfaces (see #161).
+motion_generators:
+  position:
+    gains:
+      panda_joint1: { p: 600, d: 30, i: 0 }
+      panda_joint2: { p: 600, d: 30, i: 0 }
+      panda_joint3: { p: 600, d: 30, i: 0 }
+      panda_joint4: { p: 600, d: 30, i: 0 }
+      panda_joint5: { p: 250, d: 10, i: 0 }
+      panda_joint6: { p: 150, d: 10, i: 0 }
+      panda_joint7: { p:  50, d:  5, i: 0 }
+  velocity:
+    gains:
+      panda_joint1: { p: 600, d: 30, i: 0 }
+      panda_joint2: { p: 600, d: 30, i: 0 }
+      panda_joint3: { p: 600, d: 30, i: 0 }
+      panda_joint4: { p: 600, d: 30, i: 0 }
+      panda_joint5: { p: 250, d: 10, i: 0 }
+      panda_joint6: { p: 150, d: 10, i: 0 }
+      panda_joint7: { p:  50, d:  5, i: 0 }

franka_gazebo/config/sim_controllers.yaml

@@ -38,3 +38,30 @@ cartesian_impedance_example_controller:
     - $(arg arm_id)_joint5
     - $(arg arm_id)_joint6
     - $(arg arm_id)_joint7
+
+position_joint_trajectory_controller:
+  type: position_controllers/JointTrajectoryController
+  joints:
+    - panda_joint1
+    - panda_joint2
+    - panda_joint3
+    - panda_joint4
+    - panda_joint5
+    - panda_joint6
+    - panda_joint7
+  constraints:
+    goal_time: 0.5
+    panda_joint1:
+      goal: 0.05
+    panda_joint2:
+      goal: 0.05
+    panda_joint3:
+      goal: 0.05
+    panda_joint4:
+      goal: 0.05
+    panda_joint5:
+      goal: 0.05
+    panda_joint6:
+      goal: 0.05
+    panda_joint7:
+      goal: 0.05

franka_gazebo/include/franka_gazebo/franka_hw_sim.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <control_toolbox/pid.h>
 #include <franka/robot_state.h>
 #include <franka_gazebo/joint.h>
 #include <franka_hw/franka_model_interface.h>
@@ -29,6 +30,8 @@ namespace franka_gazebo {
  * ### transmission_interface/SimpleTransmission
  * - hardware_interface/JointStateInterface
  * - hardware_interface/EffortJointInterface
+ * - hardware_interface/PositionJointInterface
+ * - hardware_interface/VelocityJointInterface
  *
  * ### franka_hw/FrankaStateInterface
  * ### franka_hw/FrankaModelInterface
@@ -94,8 +97,14 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim {
   gazebo::physics::ModelPtr robot_;
   std::map<std::string, std::shared_ptr<franka_gazebo::Joint>> joints_;
 
+  enum ControlMethod { EFFORT, POSITION, VELOCITY };
+  std::map<std::string, ControlMethod> joint_control_methods_;
+  std::map<std::string, control_toolbox::Pid> pid_controllers_;
+
   hardware_interface::JointStateInterface jsi_;
   hardware_interface::EffortJointInterface eji_;
+  hardware_interface::PositionJointInterface pji_;
+  hardware_interface::VelocityJointInterface vji_;
   franka_hw::FrankaStateInterface fsi_;
   franka_hw::FrankaModelInterface fmi_;
 
@@ -112,6 +121,8 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim {
 
   void initJointStateHandle(const std::shared_ptr<franka_gazebo::Joint>& joint);
   void initEffortCommandHandle(const std::shared_ptr<franka_gazebo::Joint>& joint);
+  void initPositionCommandHandle(const std::shared_ptr<franka_gazebo::Joint>& joint);
+  void initVelocityCommandHandle(const std::shared_ptr<franka_gazebo::Joint>& joint);
   void initFrankaStateHandle(const std::string& robot,
                              const urdf::Model& urdf,
                              const transmission_interface::TransmissionInfo& transmission);

franka_gazebo/include/franka_gazebo/joint.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <angles/angles.h>
+#include <joint_limits_interface/joint_limits.h>
 #include <ros/ros.h>
 #include <Eigen/Dense>
 #include <gazebo/physics/Joint.hh>
@@ -35,6 +36,10 @@ struct Joint {
   /// http://docs.ros.org/en/diamondback/api/urdf/html/classurdf_1_1Joint.html
   int type;
 
+  /// Joint limits @see
+  /// https://docs.ros.org/en/diamondback/api/urdf/html/classurdf_1_1JointLimits.html
+  joint_limits_interface::JointLimits limits;
+
   /// The axis of rotation/translation of this joint in local coordinates
   Eigen::Vector3d axis;
 

franka_gazebo/launch/panda.launch

@@ -12,6 +12,7 @@
   <arg name="arm_id"      default="panda" doc="Name of the panda robot to spawn" />
   <arg name="use_gripper" default="true"  doc="Should a franka hand be mounted on the flange?" />
   <arg name="controller"  default=" "     doc="Which example controller should be started? (One of {cartesian_impedance,model,force}_example_controller)" />
+  <arg name="transmission" default="hardware_interface/EffortJointInterface" doc="Which transmission should be used for the panda joints? (One of hardware_interface/{EffortJointInterface,PositionJointInterface,VelocityJointInterface})" />
   <arg name="x"           default="0"     doc="How far forward to place the base of the robot in [m]?" />
   <arg name="y"           default="0"     doc="How far leftwards to place the base of the robot in [m]?" />
   <arg name="z"           default="0"     doc="How far upwards to place the base of the robot in [m]?" />
@@ -42,6 +43,7 @@
                     gazebo:=true
                     hand:=$(arg use_gripper)
                     arm_id:=$(arg arm_id)
+                    transmission:=$(arg transmission)
                     xyz:='$(arg x) $(arg y) $(arg z)'
                     rpy:='$(arg roll) $(arg pitch) $(arg yaw)'">
     </param>

franka_gazebo/src/franka_hw_sim.cpp

@@ -10,7 +10,9 @@
 #include <franka_msgs/SetKFrame.h>
 #include <franka_msgs/SetLoad.h>
 #include <gazebo_ros_control/robot_hw_sim.h>
+#include <joint_limits_interface/joint_limits_urdf.h>
 #include <Eigen/Dense>
+#include <boost/algorithm/clamp.hpp>
 #include <iostream>
 #include <sstream>
 #include <string>
@@ -76,6 +78,7 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace,
       return false;
     }
     joint->type = urdf_joint->type;
+    joint_limits_interface::getJointLimits(urdf_joint, joint->limits);
     joint->axis = Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z);
 
     // Get a handle to the underlying Gazebo Joint
@@ -105,9 +108,37 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace,
         ROS_INFO_STREAM_NAMED("franka_hw_sim", "Found transmission interface of joint '"
                                                    << joint->name << "': " << k_interface);
         if (k_interface == "hardware_interface/EffortJointInterface") {
+          this->joint_control_methods_.emplace(joint->name, EFFORT);
+
           initEffortCommandHandle(joint);
           continue;
         }
+        if (k_interface == "hardware_interface/PositionJointInterface") {
+          this->joint_control_methods_.emplace(joint->name, POSITION);
+
+          // Initiate position motion generator (PID controller)
+          const ros::NodeHandle posPidGainsNh(robot_namespace +
+                                              "/motion_generators/position/gains/" + joint->name);
+          control_toolbox::Pid pid;
+          this->pid_controllers_.emplace(joint->name, pid);
+          this->pid_controllers_[joint->name].init(posPidGainsNh);
+
+          initPositionCommandHandle(joint);
+          continue;
+        }
+        if (k_interface == "hardware_interface/VelocityJointInterface") {
+          this->joint_control_methods_.emplace(joint->name, VELOCITY);
+
+          // Initiate velocity motion generator (PID controller)
+          const ros::NodeHandle velPidGainsNh(robot_namespace +
+                                              "/motion_generators/velocity/gains/" + joint->name);
+          control_toolbox::Pid pid;
+          this->pid_controllers_.emplace(joint->name, pid);
+          this->pid_controllers_[joint->name].init(velPidGainsNh);
+
+          initVelocityCommandHandle(joint);
+          continue;
+        }
       }
 
       if (transmission.type_ == "franka_hw/FrankaStateInterface") {
@@ -142,6 +173,8 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace,
 
   // After all handles have been assigned to interfaces, register them
   registerInterface(&this->eji_);
+  registerInterface(&this->pji_);
+  registerInterface(&this->vji_);
   registerInterface(&this->jsi_);
   registerInterface(&this->fsi_);
   registerInterface(&this->fmi_);
@@ -162,6 +195,16 @@ void FrankaHWSim::initEffortCommandHandle(const std::shared_ptr<franka_gazebo::J
       hardware_interface::JointHandle(this->jsi_.getHandle(joint->name), &joint->command));
 }
 
+void FrankaHWSim::initPositionCommandHandle(const std::shared_ptr<franka_gazebo::Joint>& joint) {
+  this->pji_.registerHandle(
+      hardware_interface::JointHandle(this->jsi_.getHandle(joint->name), &joint->command));
+}
+
+void FrankaHWSim::initVelocityCommandHandle(const std::shared_ptr<franka_gazebo::Joint>& joint) {
+  this->vji_.registerHandle(
+      hardware_interface::JointHandle(this->jsi_.getHandle(joint->name), &joint->command));
+}
+
 void FrankaHWSim::initFrankaStateHandle(
     const std::string& robot,
     const urdf::Model& urdf,
@@ -302,26 +345,63 @@ void FrankaHWSim::readSim(ros::Time time, ros::Duration period) {
   this->updateRobotState(time);
 }
 
-void FrankaHWSim::writeSim(ros::Time /*time*/, ros::Duration /*period*/) {
+void FrankaHWSim::writeSim(ros::Time /*time*/, ros::Duration period) {
   auto g = this->model_->gravity(this->robot_state_);
 
   for (auto& pair : this->joints_) {
     auto joint = pair.second;
     auto command = joint->command;
-
-    // Check if this joint is affected by gravity compensation
-    std::string prefix = this->arm_id_ + "_joint";
-    if (pair.first.rfind(prefix, 0) != std::string::npos) {
-      int i = std::stoi(pair.first.substr(prefix.size())) - 1;
-      command += g.at(i);
+    auto& effort = command;
+
+    // Retrieve effort control command
+    switch (joint_control_methods_[joint->name]) {
+      case EFFORT: {
+        // Check if this joint is affected by gravity compensation
+        std::string prefix = this->arm_id_ + "_joint";
+        if (pair.first.rfind(prefix, 0) != std::string::npos) {
+          int i = std::stoi(pair.first.substr(prefix.size())) - 1;
+          command += g.at(i);
+        }
+        effort = command;
+      } break;
+      case POSITION: {
+        // Use position motion generator
+        double error;
+        const double jointLowerLimit_ = joint->limits.min_position;
+        const double jointUpperLimit_ = joint->limits.max_position;
+        switch (joint->type) {
+          case urdf::Joint::REVOLUTE:
+            angles::shortest_angular_distance_with_limits(
+                joint->position, command, jointLowerLimit_, jointUpperLimit_, error);
+            break;
+          case urdf::Joint::CONTINUOUS:
+            error = angles::shortest_angular_distance(joint->position, command);
+            break;
+          default:
+            error = command - joint->position;
+        }
+        const double effortLimit_ = joint->limits.max_effort;
+        effort =
+            boost::algorithm::clamp(pid_controllers_[joint->name].computeCommand(error, period),
+                                    -effortLimit_, effortLimit_);
+      } break;
+      case VELOCITY: {
+        // Use velocity motion generator
+        const double error = command - joint->velocity;
+        const double effortLimit_ = joint->limits.max_effort;
+        effort =
+            boost::algorithm::clamp(pid_controllers_[joint->name].computeCommand(error, period),
+                                    -effortLimit_, effortLimit_);
+      } break;
     }
 
-    if (std::isnan(command)) {
+    // Send control effort control command
+    if (std::isnan(effort)) {
       ROS_WARN_STREAM_NAMED("franka_hw_sim",
                             "Command for " << joint->name << "is NaN, won't send to robot");
       continue;
     }
-    joint->handle->SetForce(0, command);
+    joint->handle->SetForce(0, effort);
   }
 }