Merge remote-tracking branch 'origin/kinetic-devel' into...

Merge remote-tracking branch 'origin/kinetic-devel' into SWDEV-510-rbk-libfranka-ros-add-m_ee-f_x_cee

CHANGELOG.md

@@ -2,10 +2,11 @@
 
 ## 0.2.0 - UNRELEASED
 
-  * Specified run-time dependencies on `franka_description`
+  * Added missing run-time dependencies to `franka_description` and `franka_control`
   * Added epsilon to grasp action
   * Added `m_ee`, `F_x_Cee`, `I_ee`, `m_total`, `F_x_Ctotal` and `I_total`
     to the robot state
+  * Use O_T_EE_d in examples
 
 ## 0.1.2 - 2017-10-10
 

franka_control/package.xml

@@ -31,7 +31,9 @@
   <depend>tf</depend>
 
   <exec_depend>franka_description</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
   <exec_depend>message_runtime</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
 
   <export>
     <controller_interface plugin="${prefix}/franka_controller_plugins.xml"/>

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/cartesian_pose_example_controller.cpp

@@ -47,7 +47,7 @@ bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_har
 }
 
 void CartesianPoseExampleController::starting(const ros::Time& /*time*/) {
-  initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE;
+  initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE_d;
 }
 
 void CartesianPoseExampleController::update(const ros::Time& /*time*/,

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/joint_impedance_example_controller.cpp

@@ -140,7 +140,7 @@ bool JointImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw
 }
 
 void JointImpedanceExampleController::starting(const ros::Time& /*time*/) {
-  initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE;
+  initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE_d;
 }
 
 void JointImpedanceExampleController::update(const ros::Time& /*time*/,

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