Merge pull request #39 in SWDEV/franka_ros from cartesian-impedance-example to kinetic-devel

* commit '13eb503c': (48 commits)
  Apply make format
  Correct minor formatting issues
  Get intial robot pose through topic instead of TF
  Set build to Release
  Correct check-tidy
  Rename topics to panda
  Add position limits to the equilibrium point
  Add torque rate saturation
  Remove unnecessary include and add license information
  Add specific comments about the controllers
  removes twist filter and adds tau_ext to match the implementation at libfranka
  set arm and joint names to panda
  corrects paths in launch files for example controllers
  modifies force PD control to PI control
  adapts include of service call from franka_hw to franka_control
  Revert "merge origin/master"
  Revert "corrects include path fro franka_control"
  corrects include path fro franka_control
  merge origin/master
  removes the tau_ext term from the controller
  ...

franka_example_controllers/CMakeLists.txt

 cmake_minimum_required(VERSION 2.8.3)
 project(franka_example_controllers)
 
+set(CMAKE_BUILD_TYPE Release)
+
 set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
@@ -11,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
   realtime_tools
   roscpp
   message_generation
+  dynamic_reconfigure
 )
 
 add_message_files(FILES
@@ -19,6 +22,11 @@ add_message_files(FILES
 
 generate_messages()
 
+generate_dynamic_reconfigure_options(
+  cfg/compliance_param.cfg
+  cfg/desired_mass_param.cfg
+)
+
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES franka_example_controllers
@@ -34,12 +42,16 @@ add_library(franka_example_controllers
   src/joint_velocity_example_controller.cpp
   src/model_example_controller.cpp
   src/joint_impedance_example_controller.cpp
+  src/cartesian_impedance_example_controller.cpp
+  src/force_example_controller.cpp
 )
 
 add_dependencies(franka_example_controllers
   ${${PROJECT_NAME}_EXPORTED_TARGETS}
   ${catkin_EXPORTED_TARGETS}
   ${PROJECT_NAME}_generate_messages_cpp
+  ${PROJECT_NAME}_gencpp
+  ${PROJECT_NAME}_gencfg
 )
 
 target_link_libraries(franka_example_controllers ${catkin_LIBRARIES})

franka_example_controllers/cfg/compliance_param.cfg

+#!/usr/bin/env python
+PACKAGE = "franka_example_controllers"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("translational_stiffness", double_t, 0, "Cartesian translational stiffness", 200, 0, 400)
+gen.add("rotational_stiffness", double_t, 0, "Cartesian rotational stiffness", 20, 0, 100)
+gen.add("nullspace_stiffness", double_t, 0, "Stiffness of the joint space nullspace controller (the desired configuration is the one at startup)", 0, 0, 100)
+
+exit(gen.generate(PACKAGE, "dynamic_compliance", "compliance_param"))

franka_example_controllers/cfg/desired_mass_param.cfg

+#!/usr/bin/env python
+PACKAGE = "franka_example_controllers"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("desired_mass", double_t, 0, "desired mass for rendered force due to gravity applied in the z axis", 0.0,  0.0, 5.0)
+gen.add("k_p", double_t, 0, "force P gain", 0.0,  0.0, 20.0)
+gen.add("k_i", double_t, 0, "force I gain", 0.0,  0.0, 20.0)
+
+exit(gen.generate(PACKAGE, "dynamic_mass", "desired_mass_param"))

franka_example_controllers/config/franka_example_controllers.yaml

@@ -14,6 +14,18 @@ model_example_controller:
     type: franka_example_controllers/ModelExampleController
     arm_id: panda
 
+force_example_controller:
+    type: franka_example_controllers/ForceExampleController
+    arm_id: panda
+    joint_names:
+        - panda_joint1
+        - panda_joint2
+        - panda_joint3
+        - panda_joint4
+        - panda_joint5
+        - panda_joint6
+        - panda_joint7
+
 joint_impedance_example_controller:
     type: franka_example_controllers/JointImpedanceExampleController
     arm_id: panda
@@ -46,3 +58,15 @@ joint_impedance_example_controller:
     vel_max: 0.15
     publish_rate: 10.0    
     coriolis_factor: 1.0
+
+cartesian_impedance_example_controller:
+    type: franka_example_controllers/CartesianImpedanceExampleController
+    arm_id: panda
+    joint_names:
+        - panda_joint1
+        - panda_joint2
+        - panda_joint3
+        - panda_joint4
+        - panda_joint5
+        - panda_joint6
+        - panda_joint7

franka_example_controllers/franka_example_controllers_plugin.xml

@@ -29,4 +29,15 @@
       A controller that tracks a cartesian path with a joint impedance controller that compensates coriolis torques. The torque commands are compared to measured torques in Console outputs.
     </description>
   </class>
+  <class name="franka_example_controllers/CartesianImpedanceExampleController" type="franka_example_controllers::CartesianImpedanceExampleController" base_class_type="controller_interface::ControllerBase">
+    <description>
+      A controller that renders a spring damper system in cartesian space. Compliance parameters and the equilibrium pose can be modified online with dynamic reconfigure and an interactive marker respectively. 
+    </description>
+  </class>
+  <class name="franka_example_controllers/ForceExampleController" type="franka_example_controllers::ForceExampleController" base_class_type="controller_interface::ControllerBase">
+    <description>
+      A PI controller that applies a force corresponding to a user-provided desired mass in the z axis. The desired mass value can be modified online with dynamic reconfigure. 
+    </description>
+  </class>
+
 </library>

franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h

+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <controller_interface/multi_interface_controller.h>
+#include <dynamic_reconfigure/server.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+#include <boost/scoped_ptr.hpp>
+#include <eigen3/Eigen/Dense>
+
+#include <franka_example_controllers/compliance_paramConfig.h>
+#include <franka_hw/franka_model_interface.h>
+#include <franka_hw/franka_state_interface.h>
+
+namespace franka_example_controllers {
+
+class CartesianImpedanceExampleController : public controller_interface::MultiInterfaceController<
+                                                franka_hw::FrankaModelInterface,
+                                                hardware_interface::EffortJointInterface,
+                                                franka_hw::FrankaStateInterface> {
+ public:
+  CartesianImpedanceExampleController();
+  bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle);
+  void starting(const ros::Time&);
+  void update(const ros::Time&, const ros::Duration& period);
+
+ private:
+  std::string arm_id_;
+  ros::NodeHandle node_handle_;
+  std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_;
+  std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
+  std::vector<hardware_interface::JointHandle> joint_handles_;
+  std::vector<std::string> joint_names_;
+  double filter_params_{0.005};
+  double nullspace_stiffness_{20.0};
+  double nullspace_stiffness_target_{20.0};
+  const double delta_tau_max_{1.0};
+  Eigen::Matrix<double, 6, 6> cartesian_stiffness_;
+  Eigen::Matrix<double, 6, 6> cartesian_stiffness_target_;
+  Eigen::Matrix<double, 6, 6> cartesian_damping_;
+  Eigen::Matrix<double, 6, 6> cartesian_damping_target_;
+  Eigen::Matrix<double, 7, 1> q_d_nullspace_;
+  Eigen::Vector3d position_d_;
+  Eigen::Quaterniond orientation_d_;
+  Eigen::Vector3d position_d_target_;
+  Eigen::Quaterniond orientation_d_target_;
+
+  // Saturation
+  Eigen::Matrix<double, 7, 1> saturateTorqueRate(
+      const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
+      const Eigen::Matrix<double, 7, 1>& tau_J_d,  // NOLINT (readability-identifier-naming)
+      const Eigen::Matrix<double, 7, 1>& gravity);
+
+  // Dynamic reconfigure
+  boost::scoped_ptr<dynamic_reconfigure::Server<franka_example_controllers::compliance_paramConfig>>
+      dynamic_server_compliance_param_;
+  ros::NodeHandle dynamic_reconfigure_compliance_param_node_;
+  void complianceParamCallback(franka_example_controllers::compliance_paramConfig& config,
+                               uint32_t level);
+
+  // Equilibrium pose subscriber
+  ros::Subscriber sub_equilibrium_pose_;
+  void equilibriumPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg);
+};
+
+}  // namespace franka_example_controllers

franka_example_controllers/include/franka_example_controllers/force_example_controller.h

+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <controller_interface/multi_interface_controller.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+
+#include <franka_hw/franka_model_interface.h>
+#include <franka_hw/franka_state_interface.h>
+
+#include <eigen3/Eigen/Core>
+
+#include <dynamic_reconfigure/server.h>
+#include <franka_example_controllers/desired_mass_paramConfig.h>
+#include <boost/scoped_ptr.hpp>
+
+namespace franka_example_controllers {
+
+class ForceExampleController : public controller_interface::MultiInterfaceController<
+                                   franka_hw::FrankaModelInterface,
+                                   hardware_interface::EffortJointInterface,
+                                   franka_hw::FrankaStateInterface> {
+ public:
+  ForceExampleController();
+  bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle);
+  void starting(const ros::Time&);
+  void update(const ros::Time&, const ros::Duration& period);
+
+ private:
+  std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
+  std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_;
+  std::vector<hardware_interface::JointHandle> joint_handles_;
+  double desired_mass_{0.0};
+  double target_mass_{0.0};
+  double k_p_{0.0};
+  double k_i_{0.0};
+  double target_k_p_{0.0};
+  double target_k_i_{0.0};
+  double filter_gain_{0.001};
+  Eigen::Matrix<double, 7, 1> tau_ext_initial_;
+  Eigen::Matrix<double, 7, 1> tau_error_;
+
+  // Dynamic reconfigure
+  boost::scoped_ptr<
+      dynamic_reconfigure::Server<franka_example_controllers::desired_mass_paramConfig>>
+      dynamic_server_desired_mass_param_;
+  ros::NodeHandle dynamic_reconfigure_desired_mass_param_node_;
+  void desiredMassParamCallback(franka_example_controllers::desired_mass_paramConfig& config,
+                                uint32_t level);
+};
+
+}  // namespace franka_example_controllers

franka_example_controllers/launch/cartesian_impedance_example_controller.launch

+<?xml version="1.0" ?>
+<launch>
+  <arg name="robot_ip" default="robot.franka.de" />
+  <arg name="arm_id" value="panda" />
+  <arg name="load_gripper" default="true" />
+  <include file="$(find franka_control)/launch/franka_control.launch" >
+    <arg name="robot_ip" value="$(arg robot_ip)" />
+    <arg name="arm_id" value="$(arg arm_id)" />
+    <arg name="load_gripper" value="$(arg load_gripper)" />
+  </include>
+  <group ns="$(arg arm_id)">
+    <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
+    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="cartesian_impedance_example_controller --shutdown-timeout 2"/>
+    <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/rviz/franka_description_with_marker.rviz"/>
+    <node name="interactive_marker" pkg="franka_example_controllers" type="interactive_marker.py" required="true" output="screen" />
+    <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" />
+  </group>
+</launch>

franka_example_controllers/launch/force_example_controller.launch

+<?xml version="1.0" ?>
+<launch>
+  <arg name="robot_ip" default="robot.franka.de" />
+  <arg name="arm_id" value="panda" />
+  <arg name="load_gripper" default="true" />
+  <include file="$(find franka_control)/launch/franka_control.launch" >
+    <arg name="robot_ip" value="$(arg robot_ip)" />
+    <arg name="arm_id" value="$(arg arm_id)" />
+    <arg name="load_gripper" value="$(arg load_gripper)" />
+  </include>
+  <group ns="$(arg arm_id)">
+    <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
+    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="force_example_controller --shutdown-timeout 2"/>
+    <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_description)/rviz/franka_description.rviz"/>
+    <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" />
+  </group>
+</launch>

franka_example_controllers/launch/rviz/franka_description_with_marker.rviz

+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 573
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        panda_link0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link7:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link8:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: true
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /panda/equilibrium_pose_marker/update
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: panda_link0
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 4.644041061401367
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.785398006439209
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.785398006439209
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c5fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000027000002c5000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000027000002c50000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b000000042fc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000022b000002c500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1200
+  X: 295
+  Y: 39

franka_example_controllers/launch/scripts/interactive_marker.py

+#!/usr/bin/python
+
+import rospy
+import tf.transformations
+import numpy as np
+
+from interactive_markers.interactive_marker_server import \
+    InteractiveMarkerServer, InteractiveMarkerFeedback
+from visualization_msgs.msg import InteractiveMarker, \
+    InteractiveMarkerControl
+from geometry_msgs.msg import PoseStamped
+from franka_msgs.msg import FrankaState
+
+marker_pose = PoseStamped()
+initial_pose_found = False
+pose_pub = None
+# [[min_x, max_x], [min_y, max_y], [min_z, max_z]]
+position_limits = [[-0.6, 0.6], [-0.6, 0.6], [0.05, 0.9]]
+
+
+def publisherCallback(msg, arm_id):
+    marker_pose.header.frame_id = arm_id + "_link0"
+    marker_pose.header.stamp = rospy.Time(0)
+    pose_pub.publish(marker_pose)
+
+
+def franka_state_callback(msg):
+    initial_quaternion = \
+        tf.transformations.quaternion_from_matrix(
+            np.transpose(np.reshape(msg.O_T_EE,
+                                    (4, 4))))
+    marker_pose.pose.orientation.x = initial_quaternion[0]
+    marker_pose.pose.orientation.y = initial_quaternion[1]
+    marker_pose.pose.orientation.z = initial_quaternion[2]
+    marker_pose.pose.orientation.w = initial_quaternion[3]
+    marker_pose.pose.position.x = msg.O_T_EE[12]
+    marker_pose.pose.position.y = msg.O_T_EE[13]
+    marker_pose.pose.position.z = msg.O_T_EE[14]
+    global initial_pose_found
+    initial_pose_found = True
+
+
+def processFeedback(feedback):
+    if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
+        marker_pose.pose.position.x = max([min([feedback.pose.position.x,
+                                          position_limits[0][1]]),
+                                          position_limits[0][0]])
+        marker_pose.pose.position.y = max([min([feedback.pose.position.y,
+                                          position_limits[1][1]]),
+                                          position_limits[1][0]])
+        marker_pose.pose.position.z = max([min([feedback.pose.position.z,
+                                          position_limits[2][1]]),
+                                          position_limits[2][0]])
+        marker_pose.pose.orientation = feedback.pose.orientation
+    server.applyChanges()
+
+
+if __name__ == "__main__":
+    rospy.init_node("equilibrium_pose_node")
+    state_sub = rospy.Subscriber("franka_state_controller/franka_states",
+                                 FrankaState, franka_state_callback)
+    listener = tf.TransformListener()
+    arm_id = rospy.get_param("arm_id")
+
+    # Get initial pose for the interactive marker
+    while not initial_pose_found:
+        rospy.sleep(1)
+    state_sub.unregister()
+
+    pose_pub = rospy.Publisher(
+        "/equilibrium_pose", PoseStamped, queue_size=10)
+    server = InteractiveMarkerServer("equilibrium_pose_marker")
+    int_marker = InteractiveMarker()
+    int_marker.header.frame_id = arm_id + "_link0"
+    int_marker.scale = 0.3
+    int_marker.name = "equilibrium_pose"
+    int_marker.description = ("Equilibrium Pose\nBE CAREFUL! "
+                              "If you move the \nequilibrium "
+                              "pose the robot will follow it\n"
+                              "so be aware of potential collisions")
+    int_marker.pose = marker_pose.pose
+    # run pose publisher
+    rospy.Timer(rospy.Duration(0.005),
+                lambda msg: publisherCallback(msg, arm_id))
+
+    # insert a box
+    control = InteractiveMarkerControl()
+    control.orientation.w = 1
+    control.orientation.x = 1
+    control.orientation.y = 0
+    control.orientation.z = 0
+    control.name = "rotate_x"
+    control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+    int_marker.controls.append(control)
+
+    control = InteractiveMarkerControl()
+    control.orientation.w = 1
+    control.orientation.x = 1
+    control.orientation.y = 0
+    control.orientation.z = 0
+    control.name = "move_x"
+    control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+    int_marker.controls.append(control)
+    control = InteractiveMarkerControl()
+    control.orientation.w = 1
+    control.orientation.x = 0
+    control.orientation.y = 1
+    control.orientation.z = 0
+    control.name = "rotate_y"
+    control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+    int_marker.controls.append(control)
+    control = InteractiveMarkerControl()
+    control.orientation.w = 1
+    control.orientation.x = 0
+    control.orientation.y = 1
+    control.orientation.z = 0
+    control.name = "move_y"
+    control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+    int_marker.controls.append(control)
+    control = InteractiveMarkerControl()
+    control.orientation.w = 1
+    control.orientation.x = 0
+    control.orientation.y = 0
+    control.orientation.z = 1
+    control.name = "rotate_z"
+    control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+    int_marker.controls.append(control)
+    control = InteractiveMarkerControl()
+    control.orientation.w = 1
+    control.orientation.x = 0
+    control.orientation.y = 0
+    control.orientation.z = 1
+    control.name = "move_z"
+    control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+    int_marker.controls.append(control)
+    server.insert(int_marker, processFeedback)
+
+    server.applyChanges()
+
+    rospy.spin()

franka_example_controllers/package.xml

@@ -15,6 +15,7 @@
 
   <build_depend>roscpp</build_depend>
   <build_depend>message_generation</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
 
   <depend>controller_interface</depend>
   <depend>franka_hw</depend>

franka_example_controllers/src/cartesian_impedance_example_controller.cpp

+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <franka_example_controllers/cartesian_impedance_example_controller.h>
+
+#include <cmath>
+
+#include <controller_interface/controller_base.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+#include <franka/robot_state.h>
+#include "pseudo_inversion.h"
+
+namespace franka_example_controllers {
+
+CartesianImpedanceExampleController::CartesianImpedanceExampleController() = default;
+
+bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw,
+                                               ros::NodeHandle& node_handle) {
+  std::vector<double> cartesian_stiffness_vector;
+  std::vector<double> cartesian_damping_vector;
+  node_handle_ = node_handle;
+
+  sub_equilibrium_pose_ = node_handle_.subscribe(
+      "/equilibrium_pose", 20, &CartesianImpedanceExampleController::equilibriumPoseCallback, this,
+      ros::TransportHints().reliable().tcpNoDelay());
+
+  if (!node_handle_.getParam("arm_id", arm_id_)) {
+    ROS_ERROR_STREAM("CartesianImpedanceExampleController: Could not read parameter arm_id");
+    return false;
+  }
+  if (!node_handle_.getParam("joint_names", joint_names_) || joint_names_.size() != 7) {
+    ROS_ERROR(
+        "CartesianImpedanceExampleController: Invalid or no joint_names parameters provided, "
+        "aborting "
+        "controller init!");
+    return false;
+  }
+
+  franka_hw::FrankaModelInterface* model_interface =
+      robot_hw->get<franka_hw::FrankaModelInterface>();
+  if (model_interface == nullptr) {
+    ROS_ERROR_STREAM(
+        "CartesianImpedanceExampleController: Error getting model interface from hardware");
+    return false;
+  }
+  try {
+    model_handle_.reset(
+        new franka_hw::FrankaModelHandle(model_interface->getHandle(arm_id_ + "_model")));
+  } catch (hardware_interface::HardwareInterfaceException& ex) {
+    ROS_ERROR_STREAM(
+        "CartesianImpedanceExampleController: Exception getting model handle from interface: "
+        << ex.what());
+    return false;
+  }
+
+  franka_hw::FrankaStateInterface* state_interface =
+      robot_hw->get<franka_hw::FrankaStateInterface>();
+  if (state_interface == nullptr) {
+    ROS_ERROR_STREAM(
+        "CartesianImpedanceExampleController: Error getting state interface from hardware");
+    return false;
+  }
+  try {
+    state_handle_.reset(
+        new franka_hw::FrankaStateHandle(state_interface->getHandle(arm_id_ + "_robot")));
+  } catch (hardware_interface::HardwareInterfaceException& ex) {
+    ROS_ERROR_STREAM(
+        "CartesianImpedanceExampleController: Exception getting state handle from interface: "
+        << ex.what());
+    return false;
+  }
+
+  hardware_interface::EffortJointInterface* effort_joint_interface =
+      robot_hw->get<hardware_interface::EffortJointInterface>();
+  if (effort_joint_interface == nullptr) {
+    ROS_ERROR_STREAM(
+        "CartesianImpedanceExampleController: Error getting effort joint interface from hardware");
+    return false;
+  }
+  for (size_t i = 0; i < 7; ++i) {
+    try {
+      joint_handles_.push_back(effort_joint_interface->getHandle(joint_names_[i]));
+    } catch (const hardware_interface::HardwareInterfaceException& ex) {
+      ROS_ERROR_STREAM(
+          "CartesianImpedanceExampleController: Exception getting joint handles: " << ex.what());
+      return false;
+    }
+  }
+
+  dynamic_reconfigure_compliance_param_node_ =
+      ros::NodeHandle("dynamic_reconfigure_compliance_param_node");
+
+  dynamic_server_compliance_param_.reset(
+      new dynamic_reconfigure::Server<franka_example_controllers::compliance_paramConfig>(
+          dynamic_reconfigure_compliance_param_node_));
+  dynamic_server_compliance_param_->setCallback(
+      boost::bind(&CartesianImpedanceExampleController::complianceParamCallback, this, _1, _2));
+
+  position_d_.setZero();
+  orientation_d_.coeffs() << 0.0, 0.0, 0.0, 1.0;
+  position_d_target_.setZero();
+  orientation_d_target_.coeffs() << 0.0, 0.0, 0.0, 1.0;
+
+  cartesian_stiffness_.setZero();
+  cartesian_damping_.setZero();
+
+  return true;
+}
+
+void CartesianImpedanceExampleController::starting(const ros::Time& /*time*/) {
+  // compute initial velocity with jacobian and set x_attractor and q_d_nullspace
+  // to initial configuration
+  franka::RobotState initial_state = state_handle_->getRobotState();
+  // get jacobian
+  std::array<double, 42> jacobian_array =
+      model_handle_->getZeroJacobian(franka::Frame::kEndEffector, initial_state);
+  // convert to eigen
+  Eigen::Map<Eigen::Matrix<double, 6, 7> > jacobian(jacobian_array.data());
+  Eigen::Map<Eigen::Matrix<double, 7, 1> > dq_initial(initial_state.dq.data());
+  Eigen::Map<Eigen::Matrix<double, 7, 1> > q_initial(initial_state.q.data());
+  Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
+
+  // set equilibrium point to current state
+  position_d_ = initial_transform.translation();
+  orientation_d_ = Eigen::Quaterniond(initial_transform.linear());
+  position_d_target_ = initial_transform.translation();
+  orientation_d_target_ = Eigen::Quaterniond(initial_transform.linear());
+
+  // set nullspace equilibrium configuration to initial q
+  q_d_nullspace_ = q_initial;
+}
+
+void CartesianImpedanceExampleController::update(const ros::Time& /*time*/,
+                                                 const ros::Duration& /*period*/) {
+  // get state variables
+  franka::RobotState robot_state = state_handle_->getRobotState();
+  std::array<double, 7> gravity_array =
+      model_handle_->getGravity(robot_state.m_load, robot_state.F_x_Cload);
+  std::array<double, 7> coriolis_array =
+      model_handle_->getCoriolis(robot_state.I_load, robot_state.m_load, robot_state.F_x_Cload);
+  std::array<double, 42> jacobian_array =
+      model_handle_->getZeroJacobian(franka::Frame::kEndEffector, robot_state);
+
+  // convert to Eigen
+  Eigen::Map<Eigen::Matrix<double, 7, 1> > coriolis(coriolis_array.data());
+  Eigen::Map<Eigen::Matrix<double, 7, 1> > gravity(gravity_array.data());
+  Eigen::Map<Eigen::Matrix<double, 6, 7> > jacobian(jacobian_array.data());
+  Eigen::Map<Eigen::Matrix<double, 7, 1> > q(robot_state.q.data());
+  Eigen::Map<Eigen::Matrix<double, 7, 1> > dq(robot_state.dq.data());
+  Eigen::Map<Eigen::Matrix<double, 7, 1> > tau_ext(robot_state.tau_ext_hat_filtered.data());
+  Eigen::Map<Eigen::Matrix<double, 7, 1> > tau_J_d(  // NOLINT (readability-identifier-naming)
+      robot_state.tau_J_d.data());
+  Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
+  Eigen::Vector3d position(transform.translation());
+  Eigen::Quaterniond orientation(transform.linear());
+
+  // compute error to desired pose
+  // position error
+  Eigen::Matrix<double, 6, 1> error;
+  error.head(3) << position - position_d_;
+
+  // orientation error
+  // "difference" quaternion
+  Eigen::Quaterniond error_quaternion(orientation * orientation_d_.inverse());
+  // convert to axis angle
+  Eigen::AngleAxisd error_quaternion_angle_axis(error_quaternion);
+  // compute "orientation error"
+  error.tail(3) << error_quaternion_angle_axis.axis() * error_quaternion_angle_axis.angle();
+
+  // compute control
+  // allocate variables
+  Eigen::VectorXd tau_task(7), tau_nullspace(7), tau_d(7);
+
+  // pseudoinverse for nullspace handling
+  // kinematic pseuoinverse
+  Eigen::MatrixXd jacobian_transpose_pinv;
+  pseudo_inverse(jacobian.transpose(), jacobian_transpose_pinv);
+
+  // Cartesian PD control with damping ratio = 1
+  tau_task << jacobian.transpose() *
+                  (-cartesian_stiffness_ * error - cartesian_damping_ * (jacobian * dq));
+  // nullspace PD control with damping ratio = 1
+  tau_nullspace << (Eigen::MatrixXd::Identity(7, 7) -
+                    jacobian.transpose() * jacobian_transpose_pinv) *
+                       (nullspace_stiffness_ * (q_d_nullspace_ - q) -
+                        (2.0 * sqrt(nullspace_stiffness_)) * dq);
+  // Desired torque
+  tau_d << tau_task + tau_nullspace + coriolis - tau_ext;
+  // Saturate torque rate to avoid discontinuities
+  tau_d << saturateTorqueRate(tau_d, tau_J_d, gravity);
+  for (size_t i = 0; i < 7; ++i) {
+    joint_handles_[i].setCommand(tau_d(i));
+  }
+
+  // update parameters changed online either through dynamic reconfigure or through the interactive
+  // target by filtering
+  cartesian_stiffness_ =
+      filter_params_ * cartesian_stiffness_target_ + (1.0 - filter_params_) * cartesian_stiffness_;
+  cartesian_damping_ =
+      filter_params_ * cartesian_damping_target_ + (1.0 - filter_params_) * cartesian_damping_;
+  nullspace_stiffness_ =
+      filter_params_ * nullspace_stiffness_target_ + (1.0 - filter_params_) * nullspace_stiffness_;
+  position_d_ = filter_params_ * position_d_target_ + (1.0 - filter_params_) * position_d_;
+  Eigen::AngleAxisd aa_orientation_d(orientation_d_);
+  Eigen::AngleAxisd aa_orientation_d_target(orientation_d_target_);
+  aa_orientation_d.axis() = filter_params_ * aa_orientation_d_target.axis() +
+                            (1.0 - filter_params_) * aa_orientation_d.axis();
+  aa_orientation_d.angle() = filter_params_ * aa_orientation_d_target.angle() +
+                             (1.0 - filter_params_) * aa_orientation_d.angle();
+  orientation_d_ = Eigen::Quaterniond(aa_orientation_d);
+}
+
+Eigen::Matrix<double, 7, 1> CartesianImpedanceExampleController::saturateTorqueRate(
+    const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
+    const Eigen::Matrix<double, 7, 1>& tau_J_d,  // NOLINT (readability-identifier-naming)
+    const Eigen::Matrix<double, 7, 1>& gravity) {
+  Eigen::Matrix<double, 7, 1> tau_d_saturated{};
+  for (size_t i = 0; i < 7; i++) {
+    // TODO(sga): After gravity is removed from tau_J_d, do not subtract it any more.
+    double difference = tau_d_calculated[i] - (tau_J_d[i] - gravity[i]);
+    tau_d_saturated[i] =
+        (tau_J_d[i] - gravity[i]) + std::max(std::min(difference, delta_tau_max_), -delta_tau_max_);
+  }
+  return tau_d_saturated;
+}
+
+void CartesianImpedanceExampleController::complianceParamCallback(
+    franka_example_controllers::compliance_paramConfig& config,
+    uint32_t /*level*/) {
+  cartesian_stiffness_target_.setIdentity();
+  cartesian_stiffness_target_.topLeftCorner(3, 3)
+      << config.translational_stiffness * Eigen::Matrix3d::Identity();
+  cartesian_stiffness_target_.bottomRightCorner(3, 3)
+      << config.rotational_stiffness * Eigen::Matrix3d::Identity();
+  cartesian_damping_target_.setIdentity();
+  // Damping ratio = 1
+  cartesian_damping_target_.topLeftCorner(3, 3)
+      << 2.0 * sqrt(config.translational_stiffness) * Eigen::Matrix3d::Identity();
+  cartesian_damping_target_.bottomRightCorner(3, 3)
+      << 2.0 * sqrt(config.rotational_stiffness) * Eigen::Matrix3d::Identity();
+  nullspace_stiffness_target_ = config.nullspace_stiffness;
+}
+
+void CartesianImpedanceExampleController::equilibriumPoseCallback(
+    const geometry_msgs::PoseStampedConstPtr& msg) {
+  position_d_target_ << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z;
+  Eigen::Quaterniond last_orientation_d_target(orientation_d_target_);
+  orientation_d_target_.coeffs() << msg->pose.orientation.x, msg->pose.orientation.y,
+      msg->pose.orientation.z, msg->pose.orientation.w;
+  if (last_orientation_d_target.coeffs().dot(orientation_d_target_.coeffs()) < 0.0) {
+    orientation_d_target_.coeffs() << -orientation_d_target_.coeffs();
+  }
+}
+
+}  // namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianImpedanceExampleController,
+                       controller_interface::ControllerBase)

franka_example_controllers/src/force_example_controller.cpp

+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <franka_example_controllers/force_example_controller.h>
+
+#include <cmath>
+
+#include <controller_interface/controller_base.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+#include <franka/robot_state.h>
+
+namespace franka_example_controllers {
+
+ForceExampleController::ForceExampleController() = default;
+
+bool ForceExampleController::init(hardware_interface::RobotHW* robot_hw,
+                                  ros::NodeHandle& node_handle) {
+  std::vector<std::string> joint_names;
+  std::string arm_id;
+  ROS_WARN(
+      "ForceExampleController: Make sure your robot's endeffector is in contact "
+      "with a horizontal surface before starting the controller!");
+  if (!node_handle.getParam("arm_id", arm_id)) {
+    ROS_ERROR("ForceExampleController: Could not read parameter arm_id");
+    return false;
+  }
+  if (!node_handle.getParam("joint_names", joint_names) || joint_names.size() != 7) {
+    ROS_ERROR(
+        "ForceExampleController: Invalid or no joint_names parameters provided, aborting "
+        "controller init!");
+    return false;
+  }
+
+  franka_hw::FrankaModelInterface* model_interface =
+      robot_hw->get<franka_hw::FrankaModelInterface>();
+  if (model_interface == nullptr) {
+    ROS_ERROR_STREAM("ForceExampleController: Error getting model interface from hardware");
+    return false;
+  }
+  try {
+    model_handle_.reset(
+        new franka_hw::FrankaModelHandle(model_interface->getHandle(arm_id + "_model")));
+  } catch (hardware_interface::HardwareInterfaceException& ex) {
+    ROS_ERROR_STREAM(
+        "ForceExampleController: Exception getting model handle from interface: " << ex.what());
+    return false;
+  }
+
+  franka_hw::FrankaStateInterface* state_interface =
+      robot_hw->get<franka_hw::FrankaStateInterface>();
+  if (state_interface == nullptr) {
+    ROS_ERROR_STREAM("ForceExampleController: Error getting state interface from hardware");
+    return false;
+  }
+  try {
+    state_handle_.reset(
+        new franka_hw::FrankaStateHandle(state_interface->getHandle(arm_id + "_robot")));
+  } catch (hardware_interface::HardwareInterfaceException& ex) {
+    ROS_ERROR_STREAM(
+        "ForceExampleController: Exception getting state handle from interface: " << ex.what());
+    return false;
+  }
+
+  hardware_interface::EffortJointInterface* effort_joint_interface =
+      robot_hw->get<hardware_interface::EffortJointInterface>();
+  if (effort_joint_interface == nullptr) {
+    ROS_ERROR_STREAM("ForceExampleController: Error getting effort joint interface from hardware");
+    return false;
+  }
+  for (size_t i = 0; i < 7; ++i) {
+    try {
+      joint_handles_.push_back(effort_joint_interface->getHandle(joint_names[i]));
+    } catch (const hardware_interface::HardwareInterfaceException& ex) {
+      ROS_ERROR_STREAM("ForceExampleController: Exception getting joint handles: " << ex.what());
+      return false;
+    }
+  }
+
+  dynamic_reconfigure_desired_mass_param_node_ =
+      ros::NodeHandle("dynamic_reconfigure_desired_mass_param_node");
+  dynamic_server_desired_mass_param_.reset(
+      new dynamic_reconfigure::Server<franka_example_controllers::desired_mass_paramConfig>(
+          dynamic_reconfigure_desired_mass_param_node_));
+  dynamic_server_desired_mass_param_->setCallback(
+      boost::bind(&ForceExampleController::desiredMassParamCallback, this, _1, _2));
+
+  return true;
+}
+
+void ForceExampleController::starting(const ros::Time& /*time*/) {
+  franka::RobotState robot_state = state_handle_->getRobotState();
+  std::array<double, 7> gravity_array =
+      model_handle_->getGravity(robot_state, 0.0, {{0.0, 0.0, 0.0}});
+  Eigen::Map<Eigen::Matrix<double, 7, 1> > tau_measured(robot_state.tau_J.data());
+  Eigen::Map<Eigen::Matrix<double, 7, 1> > gravity(gravity_array.data());
+  // Bias correction for the current external torque
+  tau_ext_initial_ = tau_measured - gravity;
+  tau_error_.setZero();
+}
+
+void ForceExampleController::update(const ros::Time& /*time*/, const ros::Duration& period) {
+  franka::RobotState robot_state = state_handle_->getRobotState();
+  std::array<double, 42> jacobian_array =
+      model_handle_->getZeroJacobian(franka::Frame::kEndEffector, robot_state);
+  std::array<double, 7> gravity_array =
+      model_handle_->getGravity(robot_state, 0.0, {{0.0, 0.0, 0.0}});
+  Eigen::Map<Eigen::Matrix<double, 6, 7> > jacobian(jacobian_array.data());
+  Eigen::Map<Eigen::Matrix<double, 7, 1> > tau_measured(robot_state.tau_J.data());
+  Eigen::Map<Eigen::Matrix<double, 7, 1> > gravity(gravity_array.data());
+
+  Eigen::VectorXd tau_d(7), desired_force_torque(6), tau_cmd(7), tau_ext(7);
+  desired_force_torque.setZero();
+  desired_force_torque(2) = desired_mass_ * -9.81;
+  tau_ext = tau_measured - gravity - tau_ext_initial_;
+  tau_d << jacobian.transpose() * desired_force_torque;
+  tau_error_ = tau_error_ + period.toSec() * (tau_d - tau_ext);
+  // FF + PI control (PI gains are intially all 0)
+  tau_cmd = tau_d + k_p_ * (tau_d - tau_ext) + k_i_ * tau_error_;
+  for (size_t i = 0; i < 7; ++i) {
+    joint_handles_[i].setCommand(tau_cmd(i));
+  }
+
+  // Update signals changed online through dynamic reconfigure
+  desired_mass_ = filter_gain_ * target_mass_ + (1 - filter_gain_) * desired_mass_;
+  k_p_ = filter_gain_ * target_k_p_ + (1 - filter_gain_) * k_p_;
+  k_i_ = filter_gain_ * target_k_i_ + (1 - filter_gain_) * k_i_;
+}
+
+void ForceExampleController::desiredMassParamCallback(
+    franka_example_controllers::desired_mass_paramConfig& config,
+    uint32_t /*level*/) {
+  target_mass_ = config.desired_mass;
+  target_k_p_ = config.k_p;
+  target_k_i_ = config.k_i;
+}
+
+}  // namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::ForceExampleController,
+                       controller_interface::ControllerBase)

franka_example_controllers/src/pseudo_inversion.h

+// Author: Enrico Corvaglia
+// https://github.com/CentroEPiaggio/kuka-lwr/blob/master/lwr_controllers/include/utils/pseudo_inversion.h
+// File provided under public domain
+// pseudo_inverse() computes the pseudo inverse of matrix M_ using SVD decomposition (can choose
+// between damped and not)
+// returns the pseudo inverted matrix M_pinv_
+
+#ifndef PSEUDO_INVERSION_H
+#define PSEUDO_INVERSION_H
+
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/LU>
+#include <eigen3/Eigen/SVD>
+using namespace Eigen;
+
+inline void pseudo_inverse(const Eigen::MatrixXd& M_,
+                           Eigen::MatrixXd& M_pinv_,
+                           bool damped = true) {
+  double lambda_ = damped ? 0.2 : 0.0;
+
+  JacobiSVD<MatrixXd> svd(M_, ComputeFullU | ComputeFullV);
+  JacobiSVD<MatrixXd>::SingularValuesType sing_vals_ = svd.singularValues();
+  MatrixXd S_ = M_;  // copying the dimensions of M_, its content is not needed.
+  S_.setZero();
+
+  for (int i = 0; i < sing_vals_.size(); i++)
+    S_(i, i) = (sing_vals_(i)) / (sing_vals_(i) * sing_vals_(i) + lambda_ * lambda_);
+
+  M_pinv_ = MatrixXd(svd.matrixV() * S_.transpose() * svd.matrixU().transpose());
+}
+
+#endif  // PSEUDO_INVERSION_H