Proposed msgs for Cartesian Impedance

README.md

@@ -1,2 +1,19 @@
 # majorana
 GSoC Project for Impedance Control
+
+See [discussion regarding Cart Impedance and Cart Control msg](http://wiki.ros.org/robot_mechanism_controllers/Reviews/Cartesian%20Trajectory%20Proposal%20API%20Review) on ros.org
+
+
+Initial msg definition
+---------------
+Start by using the [following idea](https://github.com/RCPRG-ros-pkg/cartesian_trajectory_msgs) as well as other discussions and real robot definitions. Defined all the parameters for setting
+Cartesian Impedance mode via messages.
+Each type of message represents different parameters that need to be set for executing trajectories with Cartesian Impedance.
+
+
+
+# Real robot tests are performed on the KUKA IIWA 7 R800:
+
+The ROS robot model is taken from [iiwa_stack](https://github.com/SalvoVirga/iiwa_stack)
+
+Interface with the iiwa controller: [GRL](https://github.com/ahundt/grl)

cartesian_impedance_msgs/CMakeLists.txt

@@ -0,0 +1,44 @@
+cmake_minimum_required(VERSION 2.8.6)
+
+project(cartesian_impedance_msgs)
+
+find_package(catkin REQUIRED COMPONENTS
+std_msgs
+geometry_msgs
+message_generation
+cmake_modules
+roscpp
+rospy
+)
+
+add_message_files(DIRECTORY msg
+  FILES
+  CartesianStiffness.msg
+  CartesianDamping.msg
+  MaxCartesianVelocity.msg
+  MaxControlForce.msg
+  MaxPathDeviation.msg
+  NullSpace.msg
+  CartesianImpedanceErrorCodes.msg
+  MaxCtrlForceExceeded.msg
+  SetCartesianImpedance.msg
+  SetCartesianForceCtrl.msg
+  CartesianForceCtrlErrorCodes.msg
+  )
+
+add_service_files(DIRECTORY srv
+      FILES
+      ConfigureCartesianImpedance.srv
+      ConfigureForceControl.srv
+)
+
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  geometry_msgs
+  )
+
+
+catkin_package(
+  CATKIN_DEPENDS std_msgs geometry_msgs message_runtime)

cartesian_impedance_msgs/msg/CartesianDamping.msg

@@ -0,0 +1,13 @@
+# Damping parameters for the Cartesian Impedance
+# the values are divided into translational and rotational
+# each of the translational and rotational components is defined as Vector3
+
+# Spring damping (type: double)
+# The spring damping determines the extent to which the virtual springs
+# oscillate after deflection.
+# For all degrees of freedom (without unit: Lehr’s damping ratio)
+
+# [ratio:0.1-1.0]
+geometry_msgs/Vector3 translational
+# [ratio:0.1-1.0]
+geometry_msgs/Vector3 rotational

cartesian_impedance_msgs/msg/CartesianForceCtrlErrorCodes.msg

@@ -0,0 +1,8 @@
+# overall behavior
+int32 SUCCESS=1
+int32 FAILURE=99999
+
+int32 SET_DOF_FAILED=-1
+int32 SET_CONTROL_FORCE_FAILED=-2
+int32 SET_FORCE_STIFFNESS_FAILED=-3
+int32 SET_CARTESIAN_CONTROL_FAILED=-4

cartesian_impedance_msgs/msg/CartesianImpedanceErrorCodes.msg

@@ -0,0 +1,18 @@
+# overall behavior
+int32 SUCCESS=1
+int32 FAILURE=99999
+
+int32 SET_STIFFNESS_FAILED=-1
+int32 SET_DAMPING_FAILED=-2
+int32 SET_MAX_CARTESIAN_VELOCITY_FAILED=-3
+int32 SET_MAX_CONTROL_FORCE_FAILED=-4
+int32 SET_MAX_PATH_DEVIATION=-5
+int32 SET_NULL_SPACE_PARAMETERS_FAILED = -6
+int32 SET_CARTESIAN_CONTROL_FAILED=-7
+int32 TIMED_OUT=-8
+int32 PREEMPTED=-9
+
+# these parameters are set for the load and center of gravity of the end-effector when we have tool attached to it
+# It is important to set the right parameters for tool mass and center of gravity because in that way the robot can
+# recalculate the torques necessary to be in gravity compensation or apply constant force when in force control
+int32 NO_VALID_LOAD_DATA_SET=-10

cartesian_impedance_msgs/msg/CartesianStiffness.msg

@@ -0,0 +1,8 @@
+# Stiffness parameters for the Cartesian Impedance
+# the values are divided into translational and rotational
+# each of the translational and rotational components is defined as Vector3
+
+# [N/m]
+geometry_msgs/Vector3 translational
+# [Nm/rad]
+geometry_msgs/Vector3 rotational

cartesian_impedance_msgs/msg/MaxCartesianVelocity.msg

@@ -0,0 +1,10 @@
+# parameters for Maximum allowed Cartesian Velocity when in Cartesian Impedance
+# the values are divided into translational and rotational
+# the overall message is defined as Twist.
+# The Twist message is composed of:
+#  1. geometry_msgs/Vector3 linear
+#  2. geometry_msgs/Vector3 angular
+
+# trans: [m/s]
+# rotation: [rad/s]
+geometry_msgs/Twist set

cartesian_impedance_msgs/msg/MaxControlForce.msg

@@ -0,0 +1,15 @@
+# parameters for Maximum allowed Cartesian Force when in Cartesian Impedance
+# This is not the values that the user needs to set for Force Control on the controller
+# these parameters are just a safety set and if the controller exceeds the maximum external force
+# the user can provide different alternatives, e.g. stop the motion or move away the robot from the object.
+
+# Wrench is defined with two components:
+# Vector3  force
+# Vector3  torque
+
+# xyz: Newtons  (all >=0)
+# rpy:Nm
+geometry_msgs/Wrench set
+
+# handler if max control force is exceeded
+ MaxCtrlForceExceeded exceeded_action

cartesian_impedance_msgs/msg/MaxCtrlForceExceeded.msg

@@ -0,0 +1,6 @@
+# Message for handling different actions if the end-effector
+# exceeds maximum force set in the Cartesian Impedance parameters
+
+bool stop
+bool gravity_compensation
+bool move_away

cartesian_impedance_msgs/msg/MaxPathDeviation.msg

@@ -0,0 +1,9 @@
+# parameters for Maximum allowed Path Deviation from goal when in Cartesian Impedance
+# the values are divided into translational and rotational
+# each of the translational and rotational components is defined as Vector3
+
+# maximum deviation from set goal in m
+geometry_msgs/Vector3 translation
+
+# maximum deviation from set goal in radians
+geometry_msgs/Vector3 rotation

cartesian_impedance_msgs/msg/NullSpace.msg

@@ -0,0 +1,18 @@
+# parameters for a redundant joint of the robot when in Cartesian Impedance
+# the values are divided into translational and rotational
+# each of the translational and rotational components is defined as Float64
+
+# Kinematically, the manipulator's null space describes the motions
+# the arm can make that don't move the end effector, known shorthand as "self-motion";
+# for a standard 7 DOF revolute RPRPRPR manipulator this is usually equivalent to moving the elbow.
+# The range of self-motion for a manipulator is a mathematically well defined space.
+# Khatib et al., 1987
+
+string[] link_names
+
+# [Nm/rad] must be => 0.0
+float64[] stiffness
+
+# without unit: Lehr’s damping ratio
+# must be between 0.3-1.0 suggested is 0.7
+float64[] damping

cartesian_impedance_msgs/msg/SetCartesianForceCtrl.msg

@@ -0,0 +1,6 @@
+#this is the axis we want the robot to exert constant force.
+string DOF
+#the amount of force that the end effector needs to apply in the selected DOF
+float64 force
+#set the stiffness of that DOF
+float64 stiffness

cartesian_impedance_msgs/msg/SetCartesianImpedance.msg

@@ -0,0 +1,11 @@
+# Here we combine all the parameters message definitions into a single message
+# that needs to be send to the controller in order to adjust the Cartesian Impedance Parameters
+
+string                 tool
+CartesianStiffness     stiffness
+#use this if the damping parameters are defined as ratios (Lehr's damping ratio)
+CartesianDamping       damping
+MaxCartesianVelocity   max_cart_vel
+MaxControlForce        max_ctrl_force
+MaxPathDeviation       max_path_deviation
+NullSpace              null_space_params

cartesian_impedance_msgs/package.xml

@@ -0,0 +1,28 @@
+<package>
+  <name>cartesian_impedance_msgs</name>
+  <version>0.1.0</version>
+  <license>BSD</license>
+  <maintainer email="rkojcev@gmail.com">Risto Kojcev</maintainer>
+  <description>
+    cartesian_impedance_msgs
+  </description>
+  <author>Risto Kojcev</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>std_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+
+  <run_depend>std_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>message_runtime</run_depend>
+
+  <export>
+  <!-- You can specify that this package is a metapackage here: -->
+  <!-- <metapackage/> -->
+
+  <!-- Other tools can request additional information be placed here -->
+
+</export>
+</package>

cartesian_impedance_msgs/srv/ConfigureCartesianImpedance.srv

@@ -0,0 +1,4 @@
+cartesian_impedance_msgs/SetCartesianImpedance cart_impedance_params
+---
+bool success
+CartesianImpedanceErrorCodes error

cartesian_impedance_msgs/srv/ConfigureForceControl.srv

@@ -0,0 +1,5 @@
+cartesian_impedance_msgs/SetCartesianImpedance cart_impedance_params
+cartesian_impedance_msgs/SetCartesianForceCtrl cart_force_control
+---
+bool success
+CartesianForceCtrlErrorCodes error