Merge pull request #175 in SWDEV/franka_ros from SRR-1142-review-gaze…

…bo-motion-generators to develop

* commit '68968a9fc617ae442ac2fbb210cb0385a83ba829': (22 commits)
  update CHANGELOG.md
  fix desired robot states
  refactor doSwitch for loops
  move controller verification into a separate class
  add implement prepareSwitch to avoid starting malformed controllers
  only allow revolute joints for position control
  remove joint_names_arm
  refactor claimsInterface
  reliably launch joint_{position,velocity}_example_controller on startup in gazebo.
  adapt the output of the desired robot states to match the behavior of the real robot
  rename efforts_initialized_ to robot_initialized_
  improve pid initialization
  refactor doSwitch method
  add joint_{position,velocity}_example_controller to franka_gazebo
  add possibility to switch between control methods at runtime
  add all transmission types to panda_arm.urdf.xacro
  feat: add effort and position traj controllers to simulation
  feat: add position and velocity motion generators to FrankaHWSim
  fix: fix conflict with #181
  Expose transmission argument in franka_gazebo/panda.launch
  ...

CHANGELOG.md

@@ -1,5 +1,13 @@
 # CHANGELOG
 
+## 0.x - UNRELEASED
+
+* `franka_gazebo`:
+  - Add JointPosition and JointVelocity Interface
+  - Fix: Robot now keeps position when no controller is running
+  - joint_{position,velocity}_example controller are now available in `franka_gazebo`
+
+
 ## 0.8.2 - 2022-02-22
 
 Requires `libfranka` >= 0.8.0

franka_description/robots/panda_arm.urdf.xacro

@@ -44,6 +44,21 @@
       <child  link="$(arg arm_id)_link0" />
     </joint>
 
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint1" transmission="hardware_interface/PositionJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint2" transmission="hardware_interface/PositionJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint3" transmission="hardware_interface/PositionJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint4" transmission="hardware_interface/PositionJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint5" transmission="hardware_interface/PositionJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint6" transmission="hardware_interface/PositionJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint7" transmission="hardware_interface/PositionJointInterface" />
+
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint1" transmission="hardware_interface/VelocityJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint2" transmission="hardware_interface/VelocityJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint3" transmission="hardware_interface/VelocityJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint4" transmission="hardware_interface/VelocityJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint5" transmission="hardware_interface/VelocityJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint6" transmission="hardware_interface/VelocityJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint7" transmission="hardware_interface/VelocityJointInterface" />
 
     <xacro:gazebo-joint joint="$(arg arm_id)_joint1" transmission="hardware_interface/EffortJointInterface" />
     <xacro:gazebo-joint joint="$(arg arm_id)_joint2" transmission="hardware_interface/EffortJointInterface" />

franka_gazebo/CMakeLists.txt

@@ -80,6 +80,7 @@ add_library(franka_hw_sim
   src/franka_hw_sim.cpp
   src/joint.cpp
   src/model_kdl.cpp
+  src/controller_verifier.cpp
   )
 target_link_libraries(franka_hw_sim
   ${catkin_LIBRARIES}

franka_gazebo/config/franka_hw_sim.yaml

@@ -11,3 +11,24 @@ franka_gripper:
 
   finger2:
     gains: { p: 100, i: 0, d: 1.0 }
+
+# Motion generators PID gains
+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: 30, d: 0, i: 0 }
+      panda_joint2: { p: 30, d: 0, i: 0 }
+      panda_joint3: { p: 30, d: 0, i: 0 }
+      panda_joint4: { p: 30, d: 0, i: 0 }
+      panda_joint5: { p: 10, d: 0, i: 0 }
+      panda_joint6: { p: 10, d: 0, i: 0 }
+      panda_joint7: { p:  5, d:  0, i: 0 }

franka_gazebo/config/sim_controllers.yaml

@@ -66,3 +66,45 @@ 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}
+
+joint_velocity_example_controller:
+    type: franka_example_controllers/JointVelocityExampleController
+    joint_names:
+        - $(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
+
+joint_position_example_controller:
+    type: franka_example_controllers/JointPositionExampleController
+    joint_names:
+        - $(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

franka_gazebo/include/franka_gazebo/controller_verifier.h

@@ -0,0 +1,46 @@
+#pragma once
+#include <franka_gazebo/joint.h>
+#include <hardware_interface/controller_info.h>
+#include <hardware_interface/interface_resources.h>
+#include <hardware_interface/internal/hardware_resource_manager.h>
+#include <boost/optional.hpp>
+
+namespace franka_gazebo {
+
+/// Can be used to check controllers in franka_gazebo. It also can distinguish
+/// between gripper and arm controllers.
+class ControllerVerifier {
+ public:
+  /// Creates a ControllerVerifier object to check controllers for franka_gazebo
+  /// @param joints map of joint names and joints including gripper joints
+  /// @param arm_id prefix of the joints
+  ControllerVerifier(const std::map<std::string, std::shared_ptr<franka_gazebo::Joint>>& joints,
+                     const std::string& arm_id);
+
+  /// checks if a set of joint_names only contains the joints that are used for the arm
+  bool areArmJoints(const std::set<std::string>& resources) const;
+
+  /// checks if a controller can be used in the franka_hw gazebo plugin
+  bool isValidController(const hardware_interface::ControllerInfo& controller) const;
+
+  /// checks if a set of joint_names only contains the joints that are used for the gripper
+  bool areFingerJoints(const std::set<std::string>& resources) const;
+
+  /// checks if a controller wants to use the finger joints with the effort interface
+  bool isClaimingGripperController(const hardware_interface::ControllerInfo& info) const;
+
+  /// checks if a controller that uses the joints of the arm (not gripper joints) claims a position,
+  /// velocity or effort interface.
+  bool isClaimingArmController(const hardware_interface::ControllerInfo& info) const;
+
+  /// returns the control method of a hardware interface
+  static boost::optional<ControlMethod> determineControlMethod(
+      const std::string& hardware_interface);
+
+ private:
+  std::vector<std::string> joint_names_;
+  std::string arm_id_;
+
+  static bool hasControlMethodAndValidSize(const hardware_interface::InterfaceResources& resource);
+};
+}  // namespace franka_gazebo

franka_gazebo/include/franka_gazebo/franka_hw_sim.h

@@ -1,6 +1,8 @@
 #pragma once
 
+#include <control_toolbox/pid.h>
 #include <franka/robot_state.h>
+#include <franka_gazebo/controller_verifier.h>
 #include <franka_gazebo/joint.h>
 #include <franka_hw/franka_model_interface.h>
 #include <franka_hw/franka_state_interface.h>
@@ -12,6 +14,7 @@
 #include <pluginlib/class_list_macros.h>
 #include <ros/ros.h>
 #include <urdf/model.h>
+#include <boost/optional.hpp>
 #include <cmath>
 #include <gazebo/common/common.hh>
 #include <gazebo/physics/physics.hh>
@@ -31,6 +34,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
@@ -91,18 +96,42 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim {
    */
   void eStopActive(const bool active) override;
 
+  /**
+   * Switches the control mode of the robot arm
+   * @param start_list list of controllers to start
+   * @param stop_list list of controllers to stop
+   */
+  void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
+                const std::list<hardware_interface::ControllerInfo>& stop_list) override;
+
+  /**
+   * Check (in non-realtime) if given controllers could be started and stopped from the current
+   * state of the RobotHW with regard to necessary hardware interface switches and prepare the
+   * switching. Start and stop list are disjoint. This handles the check and preparation, the actual
+   * switch is commited in doSwitch().
+   */
+  bool prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
+                     const std::list<hardware_interface::ControllerInfo>& /*stop_list*/) override;
+
  private:
   /// If gazebo::Joint::GetForceTorque() yielded already a non-zero value
-  bool efforts_initialized_;
+  bool robot_initialized_;
+
+  std::unique_ptr<ControllerVerifier> verifier_;
 
   std::array<double, 3> gravity_earth_;
 
   std::string arm_id_;
   gazebo::physics::ModelPtr robot_;
   std::map<std::string, std::shared_ptr<franka_gazebo::Joint>> joints_;
 
+  std::map<std::string, control_toolbox::Pid> position_pid_controllers_;
+  std::map<std::string, control_toolbox::Pid> velocity_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 +150,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);
@@ -192,6 +223,10 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim {
     Eigen::Matrix3d Ip = I + m * P.transpose() * P;
     return Ip;
   }
+
+  void forControlledJoint(
+      const std::list<hardware_interface::ControllerInfo>& controllers,
+      const std::function<void(franka_gazebo::Joint& joint, const ControlMethod&)>& f);
 };
 
 }  // namespace franka_gazebo

franka_gazebo/include/franka_gazebo/joint.h

@@ -1,16 +1,22 @@
 #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>
 
 namespace franka_gazebo {
 
+/**
+ * Specifies the current control method of the joint.
+ */
+enum ControlMethod { EFFORT, POSITION, VELOCITY };
+
 /**
  * A data container holding all relevant information about a robotic joint.
  *
- * Calling @ref update on this object will compute its internal state based on the all currenlty
+ * Calling @ref update on this object will compute its internal state based on the all currently
  * supplied information such as position, efforts etc.
  */
 struct Joint {
@@ -20,7 +26,7 @@ struct Joint {
   Joint(const Joint&) = delete;
 
   /**
-   * Calculate all members such as accelerations, jerks velocities by differention
+   * Calculate all members such as accelerations, jerks velocities by differentiation
    * @param[in] dt the current time step since last time this method was called
    */
   void update(const ros::Duration& dt);
@@ -35,13 +41,32 @@ 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;
 
   /// The currently applied command from a controller acting on this joint either in \f$N\f$ or
   /// \f$Nm\f$ without gravity
   double command = 0;
 
+  /// The current desired position that is used for the PID controller when the joints control
+  /// method is "POSITION". When the control method is not "POSITION", this value will only be
+  /// updated once at the start of the controller and stay the same until a new controller is
+  /// started.
+  double desired_position = 0;
+
+  /// The current desired velocity that is used for the PID controller when the joints control
+  /// method is "VELOCITY". When the control method is not "VELOCITY", this value will be set to
+  /// zero.
+  double desired_velocity = 0;
+
+  /// Decides whether the joint is doing torque control or if the position or velocity should
+  /// be controlled.
+  ControlMethod control_method = POSITION;
+
   /// The currently acting gravity force or torque acting on this joint in \f$N\f$ or \f$Nm\f$
   double gravity = 0;
 
@@ -58,7 +83,7 @@ struct Joint {
   /// \f$\frac{rad}{s^3}\f$
   double jerk = 0;
 
-  /// The currenlty acting acceleration on this joint in either \f$\frac{m}{s^2}\f$ or
+  /// The currently acting acceleration on this joint in either \f$\frac{m}{s^2}\f$ or
   /// \f$\frac{rad}{s^2}\f$
   double acceleration = 0;
 

franka_gazebo/launch/panda.launch

@@ -11,7 +11,7 @@
   <!-- Robot Customization -->
   <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="controller"  default=" "     doc="Which example controller should be started? (One of {cartesian_impedance,model,force,joint_position,joint_velocity}_example_controller)" />
   <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]?" />
@@ -21,9 +21,9 @@
   <arg name="initial_joint_positions"
        doc="Initial joint configuration of the panda. Specify as a list of name/value pairs in form of '-J [name-of-joint] [value-in-rad]'. Default is a 90 degree bend in the elbow"
        default="-J $(arg arm_id)_joint1 0
-                -J $(arg arm_id)_joint2 0
+                -J $(arg arm_id)_joint2 -0.785398163
                 -J $(arg arm_id)_joint3 0
-                -J $(arg arm_id)_joint4 -1.57079632679
+                -J $(arg arm_id)_joint4 -2.35619449
                 -J $(arg arm_id)_joint5 0
                 -J $(arg arm_id)_joint6 1.57079632679
                 -J $(arg arm_id)_joint7 0.785398163397
@@ -70,8 +70,9 @@
         respawn="false"
   />
 
-  <node pkg="controller_manager"
-        type="spawner"
+  <!-- spawns the controller after the robot was put into its initial joint pose -->
+  <node pkg="franka_gazebo"
+        type="delayed_controller_spawner.py"
         name="$(arg arm_id)_controller_spawner"
         respawn="false" output="screen"
         args="franka_state_controller $(arg controller)"