diff --git a/README.md b/README.md index 6680f76..8da643e 100644 --- a/README.md +++ b/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) diff --git a/cartesian_impedance_msgs/CMakeLists.txt b/cartesian_impedance_msgs/CMakeLists.txt new file mode 100644 index 0000000..68545a8 --- /dev/null +++ b/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) diff --git a/cartesian_impedance_msgs/msg/CartesianDamping.msg b/cartesian_impedance_msgs/msg/CartesianDamping.msg new file mode 100644 index 0000000..a97758f --- /dev/null +++ b/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 diff --git a/cartesian_impedance_msgs/msg/CartesianForceCtrlErrorCodes.msg b/cartesian_impedance_msgs/msg/CartesianForceCtrlErrorCodes.msg new file mode 100644 index 0000000..9c5c362 --- /dev/null +++ b/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 diff --git a/cartesian_impedance_msgs/msg/CartesianImpedanceErrorCodes.msg b/cartesian_impedance_msgs/msg/CartesianImpedanceErrorCodes.msg new file mode 100644 index 0000000..4277f5b --- /dev/null +++ b/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 diff --git a/cartesian_impedance_msgs/msg/CartesianStiffness.msg b/cartesian_impedance_msgs/msg/CartesianStiffness.msg new file mode 100644 index 0000000..f92cced --- /dev/null +++ b/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 diff --git a/cartesian_impedance_msgs/msg/MaxCartesianVelocity.msg b/cartesian_impedance_msgs/msg/MaxCartesianVelocity.msg new file mode 100644 index 0000000..d4691d6 --- /dev/null +++ b/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 diff --git a/cartesian_impedance_msgs/msg/MaxControlForce.msg b/cartesian_impedance_msgs/msg/MaxControlForce.msg new file mode 100644 index 0000000..9714420 --- /dev/null +++ b/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 diff --git a/cartesian_impedance_msgs/msg/MaxCtrlForceExceeded.msg b/cartesian_impedance_msgs/msg/MaxCtrlForceExceeded.msg new file mode 100644 index 0000000..bbc37e3 --- /dev/null +++ b/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 diff --git a/cartesian_impedance_msgs/msg/MaxPathDeviation.msg b/cartesian_impedance_msgs/msg/MaxPathDeviation.msg new file mode 100644 index 0000000..41b6f24 --- /dev/null +++ b/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 diff --git a/cartesian_impedance_msgs/msg/NullSpace.msg b/cartesian_impedance_msgs/msg/NullSpace.msg new file mode 100644 index 0000000..08d9a7c --- /dev/null +++ b/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 diff --git a/cartesian_impedance_msgs/msg/SetCartesianForceCtrl.msg b/cartesian_impedance_msgs/msg/SetCartesianForceCtrl.msg new file mode 100644 index 0000000..2cb2693 --- /dev/null +++ b/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 diff --git a/cartesian_impedance_msgs/msg/SetCartesianImpedance.msg b/cartesian_impedance_msgs/msg/SetCartesianImpedance.msg new file mode 100644 index 0000000..0e53c7c --- /dev/null +++ b/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 diff --git a/cartesian_impedance_msgs/package.xml b/cartesian_impedance_msgs/package.xml new file mode 100644 index 0000000..be33487 --- /dev/null +++ b/cartesian_impedance_msgs/package.xml @@ -0,0 +1,28 @@ + + cartesian_impedance_msgs + 0.1.0 + BSD + Risto Kojcev + + cartesian_impedance_msgs + + Risto Kojcev + + catkin + + std_msgs + geometry_msgs + message_generation + + std_msgs + geometry_msgs + message_runtime + + + + + + + + + diff --git a/cartesian_impedance_msgs/srv/ConfigureCartesianImpedance.srv b/cartesian_impedance_msgs/srv/ConfigureCartesianImpedance.srv new file mode 100644 index 0000000..6113fcf --- /dev/null +++ b/cartesian_impedance_msgs/srv/ConfigureCartesianImpedance.srv @@ -0,0 +1,4 @@ +cartesian_impedance_msgs/SetCartesianImpedance cart_impedance_params +--- +bool success +CartesianImpedanceErrorCodes error diff --git a/cartesian_impedance_msgs/srv/ConfigureForceControl.srv b/cartesian_impedance_msgs/srv/ConfigureForceControl.srv new file mode 100644 index 0000000..f717a6b --- /dev/null +++ b/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