Merge remote-tracking branch 'origin/add_motion_generators' into add_…

…simulation_motion_generators

franka_gazebo/config/franka_hw_sim.yaml

@@ -11,3 +11,26 @@ franka_gripper:
 
   finger2:
     gains: { p: 100, i: 0, d: 1.0 }
+
+# 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

@@ -66,3 +66,23 @@ effort_joint_trajectory_controller:
     $(arg arm_id)_joint5: { goal: 0.05 }
     $(arg arm_id)_joint6: { goal: 0.05 }
     $(arg arm_id)_joint7: { goal: 0.05 }
+
+position_joint_trajectory_controller:
+  type: position_controllers/JointTrajectoryController
+  joints:
+    - $(arg arm_id)_joint1
+    - $(arg arm_id)_joint2
+    - $(arg arm_id)_joint3
+    - $(arg arm_id)_joint4
+    - $(arg arm_id)_joint5
+    - $(arg arm_id)_joint6
+    - $(arg arm_id)_joint7
+  constraints:
+    goal_time: 0.5
+    $(arg arm_id)_joint1: { goal: 0.05}
+    $(arg arm_id)_joint2: { goal: 0.05}
+    $(arg arm_id)_joint3: { goal: 0.05}
+    $(arg arm_id)_joint4: { goal: 0.05}
+    $(arg arm_id)_joint5: { goal: 0.05}
+    $(arg arm_id)_joint6: { goal: 0.05}
+    $(arg arm_id)_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>
@@ -31,6 +32,8 @@ const double kDefaultTauExtLowpassFilter = 1.0;  // no filtering per default of
  * ### transmission_interface/SimpleTransmission
  * - hardware_interface/JointStateInterface
  * - hardware_interface/EffortJointInterface
+ * - hardware_interface/PositionJointInterface
+ * - hardware_interface/VelocityJointInterface
  *
  * ### franka_hw/FrankaStateInterface
  * ### franka_hw/FrankaModelInterface
@@ -101,8 +104,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_;
 
@@ -121,6 +130,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/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>
@@ -85,6 +87,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
@@ -114,9 +117,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 kPosPidGainsNh(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(kPosPidGainsNh);
+
+          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 kVelPidGainsNh(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(kVelPidGainsNh);
+
+          initVelocityCommandHandle(joint);
+          continue;
+        }
       }
 
       if (transmission.type_ == "franka_hw/FrankaStateInterface") {
@@ -153,6 +184,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_);
@@ -173,6 +206,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,
@@ -315,27 +358,62 @@ 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_, this->gravity_earth_);
 
   for (auto& pair : this->joints_) {
     auto joint = pair.second;
 
-    // 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;
-      joint->gravity = g.at(i);
+    // Retrieve effort control command
+    double effort;
+    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;
+          joint->gravity = g.at(i);
+        }
+        effort = joint->command + joint->gravity;
+      } break;
+      case POSITION: {
+        // Use position motion generator
+        double error;
+        const double kJointLowerLimit = joint->limits.min_position;
+        const double kJointUpperLimit = joint->limits.max_position;
+        switch (joint->type) {
+          case urdf::Joint::REVOLUTE:
+            angles::shortest_angular_distance_with_limits(
+                joint->position, joint->command, kJointLowerLimit, kJointUpperLimit, error);
+            break;
+          case urdf::Joint::CONTINUOUS:
+            error = angles::shortest_angular_distance(joint->position, joint->command);
+            break;
+          default:
+            error = joint->command - joint->position;
+        }
+        const double kEffortLimit = joint->limits.max_effort;
+        effort =
+            boost::algorithm::clamp(pid_controllers_[joint->name].computeCommand(error, period),
+                                    -kEffortLimit, kEffortLimit);
+      } break;
+      case VELOCITY: {
+        // Use velocity motion generator
+        const double kError = joint->command - joint->velocity;
+        const double kEffortLimit = joint->limits.max_effort;
+        effort =
+            boost::algorithm::clamp(pid_controllers_[joint->name].computeCommand(kError, period),
+                                    -kEffortLimit, kEffortLimit);
+      } break;
     }
 
-    auto command = joint->command + joint->gravity;
-
-    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);
   }
 }
 
@@ -480,11 +558,9 @@ void FrankaHWSim::updateRobotState(ros::Time time) {
     this->robot_state_.tau_J[i] = joint->effort;
     this->robot_state_.dtau_J[i] = joint->jerk;
 
-    // since we don't support position or velocity interface yet, we set the desired joint
-    // trajectory to zero indicating we are in torque control mode
-    this->robot_state_.q_d[i] = joint->position;  // special case for resetting motion generators
-    this->robot_state_.dq_d[i] = 0;
-    this->robot_state_.ddq_d[i] = 0;
+    this->robot_state_.q_d[i] = joint->position;
+    this->robot_state_.dq_d[i] = joint->velocity;
+    this->robot_state_.ddq_d[i] = joint->acceleration;
     this->robot_state_.tau_J_d[i] = joint->command;
 
     // For now we assume no flexible joints