├── .gitignore ├── CMakeLists.txt ├── README.md ├── config ├── controllers.yaml ├── ft_calib_data.yaml ├── ft_calib_meas.yaml ├── ft_calib_poses.yaml ├── ft_calib_q.yaml └── hw_interface.yaml ├── include ├── lwr_force_position_controllers │ ├── cartesian_inverse_dynamics_controller.h │ ├── cartesian_position_controller.h │ ├── ft_sensor_calib_controller.h │ └── hybrid_impedance_controller.h └── utils │ ├── euler_kinematical_rpy.h │ ├── euler_kinematical_zyz.h │ └── skew_pinv.h ├── launch └── single_lwr.launch ├── lwr_force_position_controllers_plugins.xml ├── model ├── end_effector.gazebo ├── end_effector.urdf.xacro └── table.urdf.xacro ├── msg ├── CartesianPositionCommandGainsMsg.msg ├── CartesianPositionCommandTraj.msg ├── CartesianPositionCommandTrajMsg.msg ├── CartesianPositionJointsMsg.msg ├── HybridImpedanceCommandGainsMsg.msg ├── HybridImpedanceCommandTrajForceMsg.msg ├── HybridImpedanceCommandTrajPosMsg.msg └── HybridImpedanceSwitchForcePosMsg.msg ├── package.xml ├── robot ├── vito.urdf.xacro └── workbench.urdf.xacro ├── src ├── cartesian_inverse_dynamics_controller.cpp ├── cartesian_position_controller.cpp ├── ft_sensor_calib_controller.cpp └── hybrid_impedance_controller.cpp ├── srv ├── CartesianPositionCommandGains.srv ├── CartesianPositionCommandTraj.srv ├── HybridImpedanceCommandGains.srv ├── HybridImpedanceCommandTrajForce.srv ├── HybridImpedanceCommandTrajPos.srv └── HybridImpedanceSwitchForcePos.srv └── worlds └── simple_environment.world /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | *.smod 19 | 20 | # Compiled Static libraries 21 | *.lai 22 | *.la 23 | *.a 24 | *.lib 25 | 26 | # Executables 27 | *.exe 28 | *.out 29 | *.app 30 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(lwr_force_position_controllers) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | eigen_conversions 9 | kdl_conversions 10 | controller_interface 11 | lwr_controllers 12 | roscpp 13 | kdljacdot 14 | ft_calib 15 | ) 16 | 17 | add_definitions (-fpermissive -std=c++11) 18 | 19 | include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}) 20 | 21 | add_message_files( 22 | FILES 23 | CartesianPositionCommandTrajMsg.msg 24 | CartesianPositionCommandGainsMsg.msg 25 | HybridImpedanceCommandTrajPosMsg.msg 26 | HybridImpedanceCommandTrajForceMsg.msg 27 | HybridImpedanceCommandGainsMsg.msg 28 | HybridImpedanceSwitchForcePosMsg.msg 29 | CartesianPositionJointsMsg.msg 30 | ) 31 | 32 | add_service_files( 33 | FILES 34 | CartesianPositionCommandTraj.srv 35 | CartesianPositionCommandGains.srv 36 | HybridImpedanceCommandTrajPos.srv 37 | HybridImpedanceCommandTrajForce.srv 38 | HybridImpedanceCommandGains.srv 39 | HybridImpedanceSwitchForcePos.srv 40 | ) 41 | 42 | generate_messages( 43 | DEPENDENCIES 44 | std_msgs 45 | geometry_msgs 46 | sensor_msgs 47 | lwr_controllers 48 | ) 49 | 50 | catkin_package( 51 | CATKIN_DEPENDS 52 | kdljacdot 53 | # ft_calib 54 | controller_interface 55 | lwr_controllers 56 | kdl_conversions 57 | eigen_conversions 58 | INCLUDE_DIRS include 59 | LIBRARIES ${PROJECT_NAME} 60 | ) 61 | 62 | add_library(${PROJECT_NAME} 63 | src/ft_sensor_calib_controller.cpp 64 | src/cartesian_position_controller.cpp 65 | src/cartesian_inverse_dynamics_controller.cpp 66 | src/hybrid_impedance_controller.cpp 67 | ) 68 | 69 | add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencpp) 70 | 71 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} yaml-cpp) 72 | 73 | install(DIRECTORY include/${PROJECT_NAME}/ 74 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 75 | 76 | install(TARGETS ${PROJECT_NAME} 77 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 78 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 79 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 80 | ) 81 | 82 | install(FILES lwr_force_position_controllers_plugins.xml 83 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 84 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Hybrid Force Position Controllers for Kuka LWR 2 | 3 | [![Hybrid Impedance Control (Kuka LWR 4+)](https://img.youtube.com/vi/0tVq7SOc8s8/0.jpg)](https://www.youtube.com/watch?v=0tVq7SOc8s8) 4 | 5 | Authors: [Nicola Piga](https://github.com/xenvre) and [Giulio Romualdi](https://github.com/giulioromualdi) 6 | 7 | ## Overview 8 | This package contains three controllers: 9 | - [__cartesian_position_controller__](src/cartesian_position_controller.cpp): an operational space controller required to 10 | move the end effector in a desired position using inverse kinematics; 11 | - [__cartesian_inverse_dynamics_controller__](src/cartesian_inverse_dynamics_controller.cpp): an operational space controller 12 | performing dynamics inversion (also wrenches at the end effector are compensated); 13 | - [__hybrid_impedance_controller__](src/hybrid_impedance_controller.cpp): an operational space controller implementing the 14 | hybrid impedance controller proposed by [Spong](http://ieeexplore.ieee.org/document/20440/) (it uses the 15 | cartesian_inverse_dynamics_controller to perform dynamics inversion); 16 | - [__ft_sensor_controller__](src/ft_sensor_controller.cpp): a JointStateInterface controller performing transformations 17 | on the force/torque measurements provided by the sensor. 18 | 19 | ## External packages required 20 | In order to run some external packages are required: 21 | - [__controller_switcher__](https://github.com/xEnVrE/controller_switcher): GUI required to switch controllers and set commands; 22 | - [__kdljacdot__](https://github.com/xEnVrE/kdljacdot): Jacobian derivative solver from KDL 1.4 (not available in ros up to now); 23 | - [__kuka-lwr__](https://github.com/CentroEPiaggio/kuka-lwr): description of the Kuka LWR manipulator; 24 | - [__vito-robot__](https://github.com/CentroEPiaggio/vito-robot): (vito_description only) required for vito robot's torso description and materials; 25 | - [__ft-calib__](https://github.com/xEnVrE/ft_calib): FTCalib class from https://github.com/kth-ros-pkg/force_torque_tools exported as ROS library; 26 | - [__gravity-compensation__](https://github.com/xEnVrE/force_torque_tools/tree/indigo/gravity_compensation): gravity-compensation node; 27 | - [__qb-interface-node__](https://github.com/xEnVrE/qb_interface_node/tree/imu): qb_interface_node with support for IMU and compatible with the gravity-compensation node. 28 | 29 | In order to compile the GUI you also need ros-*-qt-build. 30 | 31 | ## How to run a simulation 32 | 33 | WARNING: in order to run the simulation you MUST set every \ and \ tag in [kuka_lwr.urdf.xacro](https://github.com/CentroEPiaggio/kuka-lwr/blob/master/lwr_description/model/kuka_lwr.urdf.xacro) 34 | to 0.0 and disable the \ section of the 7-th link. 35 | 36 | 1. roslaunch lwr_force_position_controllers single_lwr.launch 37 | 38 | ## How to run using the real robot 39 | 40 | 1. roslaunch lwr_force_position_controllers single_lwr.launch use_lwr_sim:=false lwr_powered:=true 41 | -------------------------------------------------------------------------------- /config/controllers.yaml: -------------------------------------------------------------------------------- 1 | lwr: 2 | # CONTROLLERS USED IN THE EXAMPLE 3 | joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 100 6 | 7 | # THIS CONFIGURATION IS ACTUALLY SENDING TAU = K*Q 8 | joint_trajectory_controller: 9 | type: position_controllers/JointTrajectoryController 10 | joints: 11 | - lwr_a1_joint 12 | - lwr_a2_joint 13 | - lwr_e1_joint 14 | - lwr_a3_joint 15 | - lwr_a4_joint 16 | - lwr_a5_joint 17 | - lwr_a6_joint 18 | 19 | stiffness_trajectory_controller: 20 | type: position_controllers/JointTrajectoryController 21 | joints: 22 | - lwr_a1_joint_stiffness 23 | - lwr_a2_joint_stiffness 24 | - lwr_e1_joint_stiffness 25 | - lwr_a3_joint_stiffness 26 | - lwr_a4_joint_stiffness 27 | - lwr_a5_joint_stiffness 28 | - lwr_a6_joint_stiffness 29 | 30 | ## OTHER CUSTOM CONTROLLERS LEFT HERE AS EXAMPLES 31 | # FtSensorCalib Controller 32 | ft_sensor_calib_controller: 33 | type: lwr_force_position_controllers/FtSensorCalibController 34 | root_name: vito_anchor 35 | tip_name: lwr_7_link 36 | topic_name: /lwr/ft_sensor 37 | calibration_loop_rate: 500.0 38 | p2p_traj_duration: 5.0 39 | recover_existing_data: false 40 | publish_rate: 1000 #Hz 41 | 42 | # Cartesian Position Controller 43 | cartesian_position_controller: 44 | type: lwr_force_position_controllers/CartesianPositionController 45 | root_name: vito_anchor 46 | tip_name: lwr_7_link 47 | publish_rate: 1000 #Hz 48 | 49 | p_wrist_ee: [0, 0, 0.025] 50 | 51 | # Hybrid Impedance Controller 52 | # (Spong, Journal of Robotics And Automation, Vol. 4, Issue 5, 1988) 53 | hybrid_impedance_controller: 54 | type: lwr_force_position_controllers/HybridImpedanceController 55 | root_name: vito_anchor 56 | tip_name: lwr_7_link 57 | ft_sensor_topic_name: /lwr/ft_sensor 58 | 59 | p_wrist_ee: [0, 0, 0.025] 60 | p_base_ws: [-0.7, 0, 0.05] 61 | p_sensor_cp: [0, 0, 0.025] 62 | ws_base_angles: [0, 0, 0] 63 | 64 | publish_rate: 1000 #Hz 65 | 66 | arm_state_controller: 67 | type: arm_state_controller/ArmStateController 68 | root_name: lwr_base_link 69 | tip_name: lwr_7_link 70 | publish_rate: 30 71 | 72 | gravity_compensation_controller: 73 | type: lwr_controllers/GravityCompensation 74 | root_name: lwr_base_link 75 | tip_name: lwr_7_link 76 | 77 | # Joint Impedance Controllers 78 | joint_impedance_controller: 79 | type: lwr_controllers/JointImpedanceController 80 | root_name: lwr_base_link 81 | tip_name: lwr_7_link 82 | stiffness_gains: 300 83 | damping_gains: .7 84 | 85 | # Inverse Dynamics Controllers 86 | inverse_dynamics_controller: 87 | type: lwr_controllers/InverseDynamicsController 88 | root_name: lwr_base_link 89 | tip_name: lwr_7_link 90 | 91 | # Computed Torque Controllers 92 | computed_torque_controller: 93 | type: lwr_controllers/ComputedTorqueController 94 | root_name: lwr_base_link 95 | tip_name: lwr_7_link 96 | 97 | # One Task Inverse Kinematics 98 | one_task_inverse_kinematics: 99 | type: lwr_controllers/OneTaskInverseKinematics 100 | root_name: lwr_base_link 101 | tip_name: lwr_7_link 102 | 103 | # Cartesian Impedance 104 | cartesian_impedance_controller: 105 | type: lwr_controllers/CartesianImpedanceController 106 | robot_name: lwr 107 | root_name: lwr_base_link 108 | tip_name: lwr_7_link 109 | 110 | # Multi Task Priority Inverse Kinematics 111 | multi_task_priority_inverse_kinematics: 112 | type: lwr_controllers/MultiTaskPriorityInverseKinematics 113 | root_name: lwr_base_link 114 | tip_name: lwr_7_link 115 | pid_lwr_a1_joint: {p: 250, i: 10, d: 30, i_clamp_min: -0.3, i_clamp_max: 0.3} 116 | pid_lwr_a2_joint: {p: 220, i: 10, d: 30, i_clamp_min: -0.3, i_clamp_max: 0.3} 117 | pid_lwr_e1_joint: {p: 150, i: 6, d: 20, i_clamp_min: -0.3, i_clamp_max: 0.3} 118 | pid_lwr_a3_joint: {p: 150, i: 5, d: 12, i_clamp_min: -0.3, i_clamp_max: 0.3} 119 | pid_lwr_a4_joint: {p: 90, i: 5, d: 10, i_clamp_min: -0.3, i_clamp_max: 0.3} 120 | pid_lwr_a5_joint: {p: 40, i: 5, d: 7, i_clamp_min: -0.3, i_clamp_max: 0.3} 121 | pid_lwr_a6_joint: {p: 15, i: 2, d: 5, i_clamp_min: -0.3, i_clamp_max: 0.3} 122 | 123 | # One Task Inverse Dynamics JL 124 | one_task_inverse_dynamics_JL: 125 | type: lwr_controllers/OneTaskInverseDynamicsJL 126 | root_name: lwr_base_link 127 | tip_name: lwr_7_link 128 | pid_lwr_a1_joint: {p: 250, i: 10, d: 30, i_clamp_min: -0.3, i_clamp_max: 0.3} 129 | pid_lwr_a2_joint: {p: 220, i: 10, d: 30, i_clamp_min: -0.3, i_clamp_max: 0.3} 130 | pid_lwr_e1_joint: {p: 150, i: 6, d: 20, i_clamp_min: -0.3, i_clamp_max: 0.3} 131 | pid_lwr_a3_joint: {p: 150, i: 5, d: 12, i_clamp_min: -0.3, i_clamp_max: 0.3} 132 | pid_lwr_a4_joint: {p: 90, i: 5, d: 10, i_clamp_min: -0.3, i_clamp_max: 0.3} 133 | pid_lwr_a5_joint: {p: 40, i: 5, d: 7, i_clamp_min: -0.3, i_clamp_max: 0.3} 134 | pid_lwr_a6_joint: {p: 15, i: 2, d: 5, i_clamp_min: -0.3, i_clamp_max: 0.3} 135 | 136 | # Multi Task Priority Inverse Dynamics 137 | multi_task_priority_inverse_dynamics: 138 | type: lwr_controllers/MultiTaskPriorityInverseDynamics 139 | root_name: lwr_base_link 140 | tip_name: lwr_7_link 141 | pid_lwr_a1_joint: {p: 250, i: 10, d: 30, i_clamp_min: -0.3, i_clamp_max: 0.3} 142 | pid_lwr_a2_joint: {p: 220, i: 10, d: 30, i_clamp_min: -0.3, i_clamp_max: 0.3} 143 | pid_lwr_e1_joint: {p: 150, i: 6, d: 20, i_clamp_min: -0.3, i_clamp_max: 0.3} 144 | pid_lwr_a3_joint: {p: 150, i: 5, d: 12, i_clamp_min: -0.3, i_clamp_max: 0.3} 145 | pid_lwr_a4_joint: {p: 90, i: 5, d: 10, i_clamp_min: -0.3, i_clamp_max: 0.3} 146 | pid_lwr_a5_joint: {p: 40, i: 5, d: 7, i_clamp_min: -0.3, i_clamp_max: 0.3} 147 | pid_lwr_a6_joint: {p: 15, i: 2, d: 5, i_clamp_min: -0.3, i_clamp_max: 0.3} 148 | 149 | # Minimum Effort Inverse Dynamics 150 | minimum_effort_inverse_dynamics: 151 | type: lwr_controllers/MinimumEffortInverseDynamics 152 | root_name: lwr_base_link 153 | tip_name: lwr_7_link 154 | pid_lwr_a1_joint: {p: 250, i: 10, d: 30, i_clamp_min: -0.3, i_clamp_max: 0.3} 155 | pid_lwr_a2_joint: {p: 220, i: 10, d: 30, i_clamp_min: -0.3, i_clamp_max: 0.3} 156 | pid_lwr_e1_joint: {p: 150, i: 6, d: 20, i_clamp_min: -0.3, i_clamp_max: 0.3} 157 | pid_lwr_a3_joint: {p: 150, i: 5, d: 12, i_clamp_min: -0.3, i_clamp_max: 0.3} 158 | pid_lwr_a4_joint: {p: 90, i: 5, d: 10, i_clamp_min: -0.3, i_clamp_max: 0.3} 159 | pid_lwr_a5_joint: {p: 40, i: 5, d: 7, i_clamp_min: -0.3, i_clamp_max: 0.3} 160 | pid_lwr_a6_joint: {p: 15, i: 2, d: 5, i_clamp_min: -0.3, i_clamp_max: 0.3} 161 | 162 | # Backstepping Controller 163 | back_stepping_controller: 164 | type: lwr_controllers/BacksteppingController 165 | root_name: lwr_base_link 166 | tip_name: lwr_7_link 167 | pid_lwr_a1_joint: {p: 250, i: 10, d: 30, i_clamp_min: -0.3, i_clamp_max: 0.3} 168 | pid_lwr_a2_joint: {p: 220, i: 10, d: 30, i_clamp_min: -0.3, i_clamp_max: 0.3} 169 | pid_lwr_e1_joint: {p: 150, i: 6, d: 20, i_clamp_min: -0.3, i_clamp_max: 0.3} 170 | pid_lwr_a3_joint: {p: 150, i: 5, d: 12, i_clamp_min: -0.3, i_clamp_max: 0.3} 171 | pid_lwr_a4_joint: {p: 90, i: 5, d: 10, i_clamp_min: -0.3, i_clamp_max: 0.3} 172 | pid_lwr_a5_joint: {p: 40, i: 5, d: 7, i_clamp_min: -0.3, i_clamp_max: 0.3} 173 | pid_lwr_a6_joint: {p: 15, i: 2, d: 5, i_clamp_min: -0.3, i_clamp_max: 0.3} 174 | 175 | # Dynamic Sliding Mode Controller 176 | dynamics_sliding_mode_controller: 177 | type: lwr_controllers/DynamicSlidingModeController 178 | root_name: lwr_base_link 179 | tip_name: lwr_7_link 180 | pid_lwr_a1_joint: {p: 250, i: 10, d: 30, i_clamp_min: -0.3, i_clamp_max: 0.3} 181 | pid_lwr_a2_joint: {p: 220, i: 10, d: 30, i_clamp_min: -0.3, i_clamp_max: 0.3} 182 | pid_lwr_e1_joint: {p: 150, i: 6, d: 20, i_clamp_min: -0.3, i_clamp_max: 0.3} 183 | pid_lwr_a3_joint: {p: 150, i: 5, d: 12, i_clamp_min: -0.3, i_clamp_max: 0.3} 184 | pid_lwr_a4_joint: {p: 90, i: 5, d: 10, i_clamp_min: -0.3, i_clamp_max: 0.3} 185 | pid_lwr_a5_joint: {p: 40, i: 5, d: 7, i_clamp_min: -0.3, i_clamp_max: 0.3} 186 | pid_lwr_a6_joint: {p: 15, i: 2, d: 5, i_clamp_min: -0.3, i_clamp_max: 0.3} 187 | 188 | # Dynamic Sliding Mode Controller Task Space 189 | dynamics_sliding_mode_controller_task_space_controller: 190 | type: lwr_controllers/DynamicSlidingModeControllerTaskSpace 191 | root_name: lwr_base_link 192 | tip_name: lwr_7_link 193 | pid_lwr_a1_joint: {p: 250, i: 10, d: 30, i_clamp_min: -0.3, i_clamp_max: 0.3} 194 | pid_lwr_a2_joint: {p: 220, i: 10, d: 30, i_clamp_min: -0.3, i_clamp_max: 0.3} 195 | pid_lwr_e1_joint: {p: 150, i: 6, d: 20, i_clamp_min: -0.3, i_clamp_max: 0.3} 196 | pid_lwr_a3_joint: {p: 150, i: 5, d: 12, i_clamp_min: -0.3, i_clamp_max: 0.3} 197 | pid_lwr_a4_joint: {p: 90, i: 5, d: 10, i_clamp_min: -0.3, i_clamp_max: 0.3} 198 | pid_lwr_a5_joint: {p: 40, i: 5, d: 7, i_clamp_min: -0.3, i_clamp_max: 0.3} 199 | pid_lwr_a6_joint: {p: 15, i: 2, d: 5, i_clamp_min: -0.3, i_clamp_max: 0.3} 200 | -------------------------------------------------------------------------------- /config/ft_calib_data.yaml: -------------------------------------------------------------------------------- 1 | bias: 2 | - -0.4008578768949646 3 | - 7.880396725026467 4 | - -1.021805315647824 5 | - -0.7490123840961805 6 | - -0.08200792164801718 7 | - 0.01266374499624832 8 | gripper_mass: 0.8180173125445871 9 | gripper_com_pose: 10 | - 0.002033092466916677 11 | - -0.002542349869443385 12 | - 0.08879802276111172 13 | - 0 14 | - 0 15 | - 0 16 | gripper_com_frame_id: lwr_7_link -------------------------------------------------------------------------------- /config/ft_calib_meas.yaml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xEnVrE/lwr_force_position_controller/cb79fdd2d272307fb7142d17c18cd7b79d9bb2f9/config/ft_calib_meas.yaml -------------------------------------------------------------------------------- /config/ft_calib_poses.yaml: -------------------------------------------------------------------------------- 1 | lwr: 2 | ft_sensor_calib_controller: 3 | calib_number_poses: 34 4 | calib_poses: 5 | pose0: [-0.9, 0.3, 0.3, 1.571, 1.571, 3.130] 6 | pose1: [-0.9, 0.3, 0.3, 1.571, 0.8, 3.140] 7 | pose2: [-0.9, 0.3, 0.3, 1.571, 0.4, 3.140] 8 | pose3: [-0.9, 0.3, 0.3, 1.571, 0.0, 3.140] 9 | pose4: [-0.9, 0.3, 0.3, 1.571, -0.4, 3.140] 10 | pose5: [-0.9, 0.3, 0.3, 1.571, -0.8, 3.140] 11 | pose6: [-0.9, 0.3, 0.3, 1.571, -1.571, 3.140] 12 | pose7: [-0.9, 0.3, 0.3, 2.8, 0.0, 3.140] 13 | pose8: [-0.9, 0.3, 0.3, 2.3, 0.0, 3.140] 14 | pose9: [-0.9, 0.3, 0.3, 2.0, 0.0, 3.140] 15 | pose10: [-0.9, 0.3, 0.3, 1.1, 0.0, 3.140] 16 | pose11: [-0.9, 0.3, 0.3, 0.8, 0.0, 3.140] 17 | pose12: [-0.9, 0.3, 0.3, 0.4, 0.0, 3.140] 18 | pose13: [-0.9, 0.3, 0.3, 0.01, 0.0, 3.140] 19 | pose14: [-0.9, 0.3, 0.3, 3.093, -0.012, -2.467] 20 | pose15: [-0.9, 0.3, 0.3, 2.843, -0.012, -2.467] 21 | pose16: [-0.9, 0.3, 0.3, 2.372, -0.012, -2.467] 22 | pose17: [-0.9, 0.3, 0.3, 2.0, -0.012, -2.467] 23 | pose18: [-0.9, 0.3, 0.3, 1.6, -0.012, -2.467] 24 | pose19: [-0.9, 0.3, 0.3, 1.2, -0.012, -2.467] 25 | pose20: [-0.9, 0.3, 0.3, 0.8, -0.012, -2.467] 26 | pose21: [-0.9, 0.3, 0.3, 0.4, -0.012, -2.467] 27 | pose22: [-0.9, 0.3, 0.3, 0.0, -0.012, -2.467] 28 | pose23: [-0.9, 0.3, 0.3, 2.399, -0.568, 1.058] 29 | pose24: [-0.9, 0.3, 0.3, 2.083, -0.858, 1.531] 30 | pose25: [-0.9, 0.3, 0.3, 1.480, -0.962, 2.280] 31 | pose26: [-0.9, 0.3, 0.3, 0.890, -0.948, 3.043] 32 | pose27: [-0.9, 0.3, 0.3, 0.677, -0.429, -2.865] 33 | pose28: [-0.9, 0.3, 0.3, 0.607, -0.049, -2.576] 34 | pose29: [-0.9, 0.3, 0.3, 0.621, 0.207, -2.396] 35 | pose30: [-0.9, 0.3, 0.3, 2.252, 0.384, -2.121] 36 | pose31: [-0.9, 0.3, 0.3, 2.258, -0.402, -2.726] 37 | pose32: [-0.9, 0.3, 0.3, 2.425, -0.685, -3.043] 38 | pose33: [-0.9, 0.3, 0.3, 2.785, -0.898, 2.742] 39 | pose34: [-0.9, 0.3, 0.3, -2.869, -0.919, 1.958] -------------------------------------------------------------------------------- /config/ft_calib_q.yaml: -------------------------------------------------------------------------------- 1 | lwr: 2 | ft_sensor_calib_controller: 3 | calib_number_q: 60 4 | home_q: 5 | - 0.12783551216125488 6 | - 0.7029324173927307 7 | - 0.28488314151763916 8 | - -1.5327800512313843 9 | - 1.5623629093170166 10 | - -0.05475286394357681 11 | - 1.2095211744308472 12 | calib_q: 13 | q12: 14 | - 1.207030547574698 15 | - 0.7120867888415852 16 | - -1.128631830317076 17 | - -1.473685245508345 18 | - -1.028084287236641 19 | - 1.099692883033232 20 | - 2.921810614489727 21 | q0: 22 | - 0.12783551216125488 23 | - 0.7029324173927307 24 | - 0.28488314151763916 25 | - -1.5327800512313843 26 | - 1.5623629093170166 27 | - -0.05475286394357681 28 | - 1.2095211744308472 29 | q9: 30 | - 1.117063772347047 31 | - 0.6195448709862337 32 | - -0.9356569418360889 33 | - -1.473584070052457 34 | - -1.171736632549334 35 | - 1.200034882640741 36 | - 1.652275521743421 37 | q20: 38 | - 1.291924455964918 39 | - 1.344446616602421 40 | - -1.835025699132549 41 | - -1.622942684889253 42 | - -1.25354156747679 43 | - 0.3270112604920214 44 | - -2.838316749378947 45 | q21: 46 | - 1.275097875943948 47 | - 1.319340558331526 48 | - -1.7750104055809 49 | - -1.575161760185878 50 | - -1.136385706447335 51 | - 0.6313500387861328 52 | - -2.822149328682925 53 | q22: 54 | - 1.240292814674214 55 | - 1.332986795683989 56 | - -1.769527461511012 57 | - -1.509834991508731 58 | - -0.9351590705036479 59 | - 0.8754400749421558 60 | - -2.662755327003574 61 | q23: 62 | - 1.204567025148164 63 | - 1.334094102864507 64 | - -1.771595186007473 65 | - -1.437219079553305 66 | - -0.7382404179177637 67 | - 1.101509155686552 68 | - -2.482541370263808 69 | q1: 70 | - 0.4578696699247446 71 | - 0.3350588038054285 72 | - -0.1470711626720957 73 | - -1.464278450379614 74 | - 0.190213116364415 75 | - 0.8488940655112831 76 | - 1.288512142045775 77 | q24: 78 | - 0.8625319329400014 79 | - 0.8995364679315676 80 | - -1.106264084384027 81 | - -1.153339311790409 82 | - -0.500632484726979 83 | - 2.763807239504015 84 | - 2.507202118353474 85 | q25: 86 | - 0.9368251807128409 87 | - 0.9541468525731567 88 | - -1.219621946770582 89 | - -1.140003151667997 90 | - -0.6014114663741914 91 | - 2.381610071455043 92 | - 2.51634310861138 93 | q26: 94 | - 1.025362084850056 95 | - 0.983584370769564 96 | - -1.301569751738103 97 | - -1.185399880855755 98 | - -0.6406390091949126 99 | - 1.982643899560681 100 | - 2.651816533536802 101 | q27: 102 | - 1.099194291994068 103 | - 1.040057926092813 104 | - -1.395731627719395 105 | - -1.258544486811683 106 | - -0.7676396396705361 107 | - 1.860698626935026 108 | - 2.852996016792307 109 | q28: 110 | - 1.217358788975613 111 | - 1.00633301910443 112 | - -1.414437706366243 113 | - -1.408932562862931 114 | - -0.9414944850433242 115 | - 1.329453488413471 116 | - 2.953510973210498 117 | q29: 118 | - 1.282742776889377 119 | - 1.117291722720606 120 | - -1.565857874881446 121 | - -1.52786539291569 122 | - -1.173818332315127 123 | - 0.9301893514019772 124 | - -2.769278793072891 125 | q30: 126 | - 1.286662697199208 127 | - 1.249557348011507 128 | - -1.700252727049615 129 | - -1.593521341230181 130 | - -1.420001382158449 131 | - 0.7064184489345084 132 | - -2.350816916181495 133 | q31: 134 | - 1.301855847365768 135 | - 0.5923514549930564 136 | - -1.275908212999854 137 | - -1.633862286438649 138 | - -2.238251472741386 139 | - -0.1242175915400665 140 | - 2.880501683832376 141 | q17: 142 | - 1.322116988649505 143 | - 1.219241096104044 144 | - -1.855757108642262 145 | - -1.609333445810464 146 | - -1.953080443973142 147 | - -0.4160575814451279 148 | - 2.922979092692723 149 | q18: 150 | - 1.325415016075421 151 | - 1.255417379071753 152 | - -1.849408409387563 153 | - -1.639759741884319 154 | - -1.769461183208042 155 | - -0.1754336300673547 156 | - 2.892053895536542 157 | q19: 158 | - 1.320570406072801 159 | - 1.279489429301853 160 | - -1.821783693031135 161 | - -1.645812822703157 162 | - -1.494794791319075 163 | - 0.10003160681857 164 | - 2.955771259857935 165 | q5: 166 | - 1.130377155978582 167 | - 0.7346945392363962 168 | - -1.029608178459545 169 | - -1.454798034787319 170 | - -1.336981584689511 171 | - 1.421561086430787 172 | - 2.467074309188728 173 | q14: 174 | - 1.25975602657245 175 | - 0.8062827708587124 176 | - -1.280948833223602 177 | - -1.474028904778393 178 | - -0.8831648498383933 179 | - 1.01383341038863 180 | - -2.465123587874244 181 | q15: 182 | - 1.282645015557399 183 | - 1.169111907395432 184 | - -1.842651007837672 185 | - -1.499883399711949 186 | - -2.304857788339835 187 | - -0.8737730122703837 188 | - 2.638065955268373 189 | q16: 190 | - 1.300349018901355 191 | - 1.183630212641989 192 | - -1.852338738622665 193 | - -1.542923159457075 194 | - -2.185562062433917 195 | - -0.7198978796853304 196 | - 2.742853845035848 197 | q4: 198 | - 1.096335704052854 199 | - 0.6039015404530543 200 | - -0.8966419688265255 201 | - -1.47343224450676 202 | - -1.194775338581724 203 | - 1.218229158460186 204 | - 2.388360580445923 205 | q11: 206 | - 1.189433181412083 207 | - 0.6895583854021075 208 | - -1.086689590421098 209 | - -1.473559532960577 210 | - -1.062342010457375 211 | - 1.122630126208006 212 | - 2.810705899579565 213 | q13: 214 | - 1.239877335859367 215 | - 0.7644097234046434 216 | - -1.217063821289868 217 | - -1.473841830154118 218 | - -0.9480429774811601 219 | - 1.050371963683291 220 | - -2.82325902637738 221 | q7: 222 | - 1.072928047704162 223 | - 1.119312715078287 224 | - -1.425403446234752 225 | - -1.389052930250644 226 | - -1.641791285988968 227 | - 1.97503791624467 228 | - 2.457963326888811 229 | q6: 230 | - 1.131039862016884 231 | - 0.8703325386966885 232 | - -1.15886151664494 233 | - -1.431996735545073 234 | - -1.446227798763565 235 | - 1.620746920134254 236 | - 2.498676102828551 237 | q8: 238 | - 1.089340211165994 239 | - 0.5990580141178095 240 | - -0.8836339607432224 241 | - -1.473843888846107 242 | - -1.205252104878211 243 | - 1.224369121988236 244 | - 1.163629938672253 245 | q10: 246 | - 1.133881986064716 247 | - 0.6333583638870044 248 | - -0.9684705300375924 249 | - -1.473528894722143 250 | - -1.149708684336053 251 | - 1.183949017212997 252 | - 1.944553274746223 253 | q2: 254 | - 0.8770416471497571 255 | - 0.3996730978934462 256 | - -0.57394180183993 257 | - -1.487632958986267 258 | - -0.6745307951622337 259 | - 0.8619363100585549 260 | - 1.995800373815415 261 | q3: 262 | - 1.017009201484492 263 | - 0.4889124425667104 264 | - -0.7504973178569525 265 | - -1.485060244196266 266 | - -0.9874718757448608 267 | - 1.021968962825922 268 | - 2.242546977691962 269 | q48: 270 | - -0.1734398637517121 271 | - 0.4094406098585406 272 | - 0.8105732094587035 273 | - -1.692269512908432 274 | - 2.105716848185674 275 | - 0.5909685172188475 276 | - -0.9601942879207686 277 | q49: 278 | - -0.1190184847518516 279 | - 0.3706199392045102 280 | - 0.7449375039757538 281 | - -1.608783705631589 282 | - 0.9615656218731026 283 | - 0.4715764074457578 284 | - 0.4681643734122449 285 | q50: 286 | - 0.104769937980115 287 | - 0.3746334259181001 288 | - 0.4790165058662144 289 | - -1.47264294277899 290 | - 0.4996588548605221 291 | - 0.9634065520729793 292 | - 1.218767807751024 293 | q51: 294 | - 0.1772862548395011 295 | - 0.4625027451227597 296 | - 0.4263649329571386 297 | - -1.329060679380941 298 | - 0.3927648507155288 299 | - 1.502365473177133 300 | - 1.613666552454555 301 | q52: 302 | - 0.2104914457877864 303 | - 0.5619780469919924 304 | - 0.4199971217912442 305 | - -1.227377779033627 306 | - 0.4773617512949802 307 | - 1.990718998794609 308 | - 1.980654814369943 309 | q53: 310 | - 0.1203991780009295 311 | - 0.5370613912184456 312 | - 0.4608335566014645 313 | - -1.329537977522738 314 | - 0.907119032621579 315 | - 1.754201483409311 316 | - 2.046844406774236 317 | q54: 318 | - 0.08830909295814315 319 | - 0.4829804476245076 320 | - 0.4809582996735715 321 | - -1.386273570190623 322 | - 0.8605334011544858 323 | - 1.479191147104205 324 | - 1.509379742620466 325 | q55: 326 | - 0.02056688272764884 327 | - 0.4413523798005787 328 | - 0.546562805539323 329 | - -1.467652878435137 330 | - 0.9589373172480382 331 | - 1.189950747904459 332 | - 0.9334534539486086 333 | q56: 334 | - -0.08806247502492859 335 | - 0.4401771746662364 336 | - 0.6763947365578327 337 | - -1.545211331954206 338 | - 1.202604680960631 339 | - 0.9812985690475475 340 | - 0.2435445751627077 341 | q57: 342 | - -0.1989545942473505 343 | - 0.4961782692624306 344 | - 0.8354463774187169 345 | - -1.592726137240563 346 | - 1.49887903042109 347 | - 0.9515013419831133 348 | - -0.5320827912429058 349 | q58: 350 | - -0.2506407938358999 351 | - 0.5870051397036384 352 | - 0.9451027321252736 353 | - -1.593416738497102 354 | - 1.648246718630188 355 | - 1.103384252509457 356 | - -1.217055370156547 357 | q59: 358 | - -0.2488174480959842 359 | - 0.6771733600644065 360 | - 1.000190309382811 361 | - -1.547271107820571 362 | - 1.601769687105903 363 | - 1.330749521826033 364 | - -1.765182835911329 365 | q35: 366 | - -0.1068558900634891 367 | - 0.5488033775705183 368 | - 0.7320982954778525 369 | - -1.569748437187781 370 | - 1.761001634203915 371 | - 1.370097320744251 372 | - 0.02007135236739632 373 | q47: 374 | - -0.1105373106874898 375 | - 0.4812134785835784 376 | - 0.7518499189660277 377 | - -1.693567115249087 378 | - 2.341714023780035 379 | - 1.112453327110186 380 | - -1.517970411768211 381 | q41: 382 | - -0.2249419352353765 383 | - 0.8978148469932696 384 | - 1.206811215866612 385 | - -1.541731306872057 386 | - 2.007376553382814 387 | - 1.747315739206 388 | - -0.6379674325599369 389 | q45: 390 | - -0.1723624621955215 391 | - 0.393982187021491 392 | - 0.8899180166156739 393 | - -1.574322303030868 394 | - 2.865402070403352 395 | - -0.4796891761627053 396 | - -0.02907942346343262 397 | q34: 398 | - -0.08981222375859677 399 | - 0.4760963505021367 400 | - 0.676831425779743 401 | - -1.599043890103935 402 | - 1.712371881787893 403 | - 1.136519163671824 404 | - 0.6085936544097716 405 | q38: 406 | - -0.02702141520288581 407 | - 0.6544873511131248 408 | - 0.7238131683112892 409 | - -1.348273302103772 410 | - 1.194804498185417 411 | - 1.893034409041363 412 | - -1.559226447045186 413 | q37: 414 | - -0.05786585572879499 415 | - 0.65050290935711 416 | - 0.7443078043610845 417 | - -1.418316678639936 418 | - 1.452494429083957 419 | - 1.804416081938877 420 | - -1.007848744284819 421 | q46: 422 | - -0.0702896513588982 423 | - 0.5908862697573607 424 | - 0.7496401838136171 425 | - -1.612241418146921 426 | - 2.229224795302724 427 | - 1.591157957101807 428 | - -1.870262560739454 429 | q32: 430 | - 0.07032170619961153 431 | - 0.4065553885649233 432 | - 0.4581937877835962 433 | - -1.517977424620962 434 | - 1.164944326295794 435 | - 1.094274028085569 436 | - 2.066261613511443 437 | q43: 438 | - -0.3855934762435993 439 | - 0.672744477183338 440 | - 1.26496537352734 441 | - -1.738138808779503 442 | - 2.756229235503973 443 | - 0.7873873740179329 444 | - -0.4346784113011957 445 | q42: 446 | - -0.3188312296850961 447 | - 0.8165238277682789 448 | - 1.255224494760956 449 | - -1.678579874756114 450 | - 2.373345562111606 451 | - 1.296857465175673 452 | - -0.5158286912037662 453 | q39: 454 | - 0.01048263602200539 455 | - 0.8134648125881441 456 | - 0.9151116918323021 457 | - -1.230361768767938 458 | - 1.157779556731945 459 | - 2.345491630780787 460 | - -1.429421152246897 461 | q40: 462 | - -0.1148861544383397 463 | - 0.8942895883441038 464 | - 1.105680739272441 465 | - -1.373857859784042 466 | - 1.664316280864661 467 | - 2.003965626242966 468 | - -0.8640621742412762 469 | q36: 470 | - -0.09096758071363986 471 | - 0.6134341737081357 472 | - 0.7521945422509893 473 | - -1.501795716825248 474 | - 1.655062729781198 475 | - 1.612519792807202 476 | - -0.4939499392862423 477 | q33: 478 | - -0.02665744465973496 479 | - 0.4217652446732085 480 | - 0.5797238939309128 481 | - -1.579567821938016 482 | - 1.474127294163686 483 | - 1.013706500416527 484 | - 1.32118824104266 485 | q44: 486 | - -0.3351265082325128 487 | - 0.4707896235608526 488 | - 1.107350756610106 489 | - -1.698458277189354 490 | - 2.923282106221667 491 | - 0.192573495433769 492 | - -0.34100100680108 -------------------------------------------------------------------------------- /config/hw_interface.yaml: -------------------------------------------------------------------------------- 1 | # make the namespace match the name you use to instantiate the arm in the xacro 2 | lwr: 3 | root: vito_anchor 4 | tip: lwr_7_link 5 | # joints is not used, since with the robot name and urdf name conventions, the parsing is hardcoded 6 | # but as a general solution, joint names could be loaded as well, like controllers do 7 | joints: 8 | - lwr_a1_joint 9 | - lwr_a2_joint 10 | - lwr_e1_joint 11 | - lwr_a3_joint 12 | - lwr_a4_joint 13 | - lwr_a5_joint 14 | - lwr_a6_joint 15 | -------------------------------------------------------------------------------- /include/lwr_force_position_controllers/cartesian_inverse_dynamics_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef LWR_FORCE_POSITION_CONTROLLERS_CARTESIAN_INVERSE_DYNAMICS_CONTROLLER_H 2 | #define LWR_FORCE_POSITION_CONTROLLERS_CARTESIAN_INVERSE_DYNAMICS_CONTROLLER_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | 10 | //KDL include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace lwr_controllers 16 | { 17 | class CartesianInverseDynamicsController: public controller_interface::KinematicChainControllerBase 18 | { 19 | friend class HybridImpedanceController; 20 | 21 | public: 22 | CartesianInverseDynamicsController(); 23 | ~CartesianInverseDynamicsController(); 24 | 25 | bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n); 26 | void starting(const ros::Time& time); 27 | void update(const ros::Time& time, const ros::Duration& period); 28 | 29 | void get_gains_im(double& kp_z, double& kp_gamma, double& kd_pos, double& kd_att); 30 | void set_gains_im(double kp_z, double kp_gamma, double kd_pos, double kd_att); 31 | void set_p_wrist_ee(double x, double y, double z); 32 | void set_ft_sensor_topic_name(std::string topic); 33 | void set_p_base_ws(double x, double y, double z); 34 | void set_ws_base_angles(double alpha, double beta, double gamma); 35 | void set_command(Eigen::VectorXd& commanded_acceleration); 36 | void load_calib_data(double& total_mass, KDL::Vector& p_sensor_tool_com); 37 | 38 | private: 39 | void force_torque_callback(const geometry_msgs::WrenchStamped::ConstPtr& msg); 40 | void update_fri_inertia_matrix(Eigen::MatrixXd& fri_B); 41 | void extend_chain(ros::NodeHandle &n); 42 | 43 | // syntax: 44 | // 45 | // for jacobians x_J_y := Jacobian w.r.t reference point y expressed in basis x 46 | // for analytical jacobians x_JA_y := analytical Jacobian w.r.t reference point y expressed in basis x 47 | // for rotation matrices R_x_y := Rotation from basis y to basis x 48 | // for wrenches x_wrench_y := Wrench w.r.t reference point y expressed in basis x 49 | // for vectors in general x_vector := vector expressed in basis x 50 | // p_x_y := arm from x to y 51 | // ee := reference point of interest (typically the tool tip) 52 | // wrist := tip of the 7th link of the Kuka LWR 53 | // T := euler kinematical matrix 54 | 55 | // possibly required by inheriting classes 56 | KDL::Rotation R_ws_base_; 57 | KDL::Rotation R_ws_ee_; 58 | KDL::Wrench base_wrench_wrist_; 59 | Eigen::VectorXd ws_x_, ws_xdot_; 60 | 61 | // setup by inheriting class 62 | KDL::Vector p_wrist_ee_; 63 | KDL::Vector p_base_ws_; 64 | 65 | // pointers to solvers 66 | boost::scoped_ptr dyn_param_solver_; 67 | boost::scoped_ptr ee_jacobian_solver_, wrist_jacobian_solver_; 68 | boost::scoped_ptr ee_fk_solver_; 69 | boost::scoped_ptr ee_jacobian_dot_solver_; 70 | boost::scoped_ptr im_link4_jacobian_solver_; 71 | boost::scoped_ptr im_link4_fk_solver_; 72 | boost::scoped_ptr im_link5_jacobian_solver_; 73 | boost::scoped_ptr im_link5_fk_solver_; 74 | 75 | // chain required to move the reference point of jacobians 76 | KDL::Chain extended_chain_; 77 | // chain required to control internal motion 78 | KDL::Chain im_link4_chain_; 79 | KDL::Chain im_link5_chain_; 80 | 81 | // these matrices are sparse and initialized in init() 82 | Eigen::MatrixXd ws_TA_, ws_TA_dot_; 83 | Eigen::MatrixXd base_TA_im_link5_; 84 | 85 | // null space controller gains 86 | Eigen::Matrix Kp_im_; 87 | Eigen::Matrix Kd_im_; 88 | double gamma_im_link5_initial_; 89 | 90 | // commandss 91 | Eigen::VectorXd tau_fri_; 92 | Eigen::MatrixXd command_filter_; 93 | 94 | // ft sensor subscriber and related wrench 95 | std::string ft_sensor_topic_name_; 96 | 97 | ros::Subscriber sub_force_; 98 | KDL::Wrench wrench_wrist_; 99 | 100 | // use simulation flag 101 | bool use_simulation_; 102 | 103 | }; 104 | 105 | } // namespace 106 | 107 | #endif 108 | -------------------------------------------------------------------------------- /include/lwr_force_position_controllers/cartesian_position_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef LWR_FORCE_POSITION_CONTROLLERS_CARTESIAN_POSITION_CONTROLLER_H 2 | #define LWR_FORCE_POSITION_CONTROLLERS_CARTESIAN_POSITION_CONTROLLER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | /* 15 | Cartesian Position Controller law is: 16 | 17 | tau_cmd_ = B(q) * y + C * dq + J' * F 18 | y = - Kp * q - Kd * dq + r 19 | r = Kp * q_d 20 | q_d = inverse_kinematics(x_d) 21 | */ 22 | 23 | namespace lwr_controllers 24 | { 25 | 26 | class CartesianPositionController: public controller_interface::KinematicChainControllerBase 27 | { 28 | public: 29 | 30 | CartesianPositionController(); 31 | ~CartesianPositionController(); 32 | 33 | bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n); 34 | void starting(const ros::Time& time); 35 | void update(const ros::Time& time, const ros::Duration& period); 36 | 37 | private: 38 | void extend_chain(ros::NodeHandle &n); 39 | void ft_sensor_callback(const geometry_msgs::WrenchStamped::ConstPtr& msg); 40 | void evaluate_traj_constants(KDL::Vector&, KDL::Rotation&); 41 | void evaluate_traj_des(const ros::Duration& period); 42 | bool set_cmd_traj(lwr_force_position_controllers::CartesianPositionCommandTraj::Request&, \ 43 | lwr_force_position_controllers::CartesianPositionCommandTraj::Response&); 44 | bool set_cmd_gains(lwr_force_position_controllers::CartesianPositionCommandGains::Request&, \ 45 | lwr_force_position_controllers::CartesianPositionCommandGains::Response&); 46 | bool get_cmd_traj(lwr_force_position_controllers::CartesianPositionCommandTraj::Request&,\ 47 | lwr_force_position_controllers::CartesianPositionCommandTraj::Response&); 48 | bool get_cmd_gains(lwr_force_position_controllers::CartesianPositionCommandGains::Request&,\ 49 | lwr_force_position_controllers::CartesianPositionCommandGains::Response&); 50 | void publish_data(ros::Publisher& pub, KDL::JntArray& array); 51 | void print_joint_array(KDL::JntArray& array); 52 | void update_fri_inertia_matrix(Eigen::MatrixXd& fri_B); 53 | void load_calib_data(double& tool_mass, KDL::Vector& p_sensor_tool_com); 54 | 55 | // joint position controller 56 | double kp_, kp_a4_, kp_a5_, kp_a6_; 57 | double kd_, kd_a4_, kd_a5_, kd_a6_; 58 | 59 | // inertia matrix and coriolis 60 | boost::scoped_ptr dyn_param_solver_; 61 | 62 | // inverse kinematics q_des = ik(x_des) 63 | boost::scoped_ptr ik_solver_; 64 | 65 | // forward kinematics 66 | boost::scoped_ptr fk_solver_; 67 | boost::scoped_ptr ee_fk_solver_; 68 | 69 | // jacobian solver 70 | boost::scoped_ptr jacobian_solver_; 71 | 72 | // desired trajectory 73 | // s(t) = a5 * t^5 + a4 * t^4 + a3 * t^3 + a0 74 | KDL::JntArrayAcc traj_des_; 75 | KDL::JntArray traj_a0_, traj_a3_, traj_a4_, traj_a5_; 76 | KDL::JntArray prev_q_setpoint_; 77 | double time_; 78 | double p2p_traj_duration_; 79 | 80 | // force and torques 81 | ros::Subscriber sub_force_; 82 | KDL::Wrench wrench_wrist_; 83 | 84 | // CartesianPositionCommand services 85 | ros::ServiceServer set_cmd_gains_service_; 86 | ros::ServiceServer set_cmd_traj_service_; 87 | ros::ServiceServer get_cmd_gains_service_; 88 | ros::ServiceServer get_cmd_traj_service_; 89 | 90 | // use simulation flag 91 | bool use_simulation_; 92 | 93 | // p_wrist_ee 94 | KDL::Vector p_wrist_ee_; 95 | 96 | // extended kdl chain 97 | KDL::Chain extended_chain_; 98 | 99 | // publisher to monitor data 100 | ros::Time last_publish_time_; 101 | double publish_rate_; 102 | ros::Publisher pub_error_, pub_q_des_; 103 | 104 | }; 105 | 106 | } // namespace 107 | 108 | #endif 109 | -------------------------------------------------------------------------------- /include/lwr_force_position_controllers/ft_sensor_calib_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef LWR_CONTROLLERS_FT_SENSOR_CALIB_CONTROLLER_H 2 | #define LWR_CONTROLLERS_FT_SENSOR_CALIB_CONTROLLER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace lwr_controllers 14 | { 15 | class FtSensorCalibController: public controller_interface::KinematicChainControllerBase 16 | { 17 | 18 | public: 19 | FtSensorCalibController(); 20 | ~FtSensorCalibController(); 21 | bool init(hardware_interface::JointStateInterface* robot, ros::NodeHandle& nh); 22 | void starting(const ros::Time& time); 23 | void update(const ros::Time& time, const ros::Duration& period); 24 | 25 | private: 26 | // services 27 | bool move_home_pose(std_srvs::Empty::Request&,\ 28 | std_srvs::Empty::Response&); 29 | bool save_calib_data(std_srvs::Empty::Request&,\ 30 | std_srvs::Empty::Response&); 31 | bool move_next_calib_pose(std_srvs::Empty::Request&,\ 32 | std_srvs::Empty::Response&); 33 | bool do_estimation_step(std_srvs::Empty::Request&,\ 34 | std_srvs::Empty::Response&); 35 | bool start_autonomus_estimation(std_srvs::Empty::Request&, \ 36 | std_srvs::Empty::Response&); 37 | bool start_compensation(std_srvs::Empty::Request&,\ 38 | std_srvs::Empty::Response&); 39 | 40 | void ft_raw_topic_callback(const geometry_msgs::WrenchStamped::ConstPtr& msg); 41 | void send_joint_trajectory_msg(std::vector q_des); 42 | void get_calibration_poses(ros::NodeHandle); 43 | void get_calibration_q(ros::NodeHandle nh); 44 | bool load_calib_data(); 45 | void recover_existing_data(); 46 | void save_calib_meas(KDL::Vector gravity, KDL::Wrench ft_wrench_avg, int index,\ 47 | KDL::JntArray q); 48 | void add_measurement(KDL::Vector gravity_ft, KDL::Wrench ft_raw_avg); 49 | void publish_data(KDL::Wrench wrench, ros::Publisher& pub); 50 | void estimation_step(); 51 | 52 | KDL::Wrench ft_wrench_raw_; 53 | KDL::Wrench offset_kdl_; 54 | KDL::Wrench base_tool_weight_com_; 55 | KDL::Vector p_sensor_tool_com_kdl_; 56 | 57 | boost::scoped_ptr ik_solver_; 58 | boost::scoped_ptr fk_solver_; 59 | boost::scoped_ptr ft_calib_; 60 | 61 | std::vector ft_calib_poses_; 62 | std::vector> ft_calib_q_; 63 | std::vector ft_calib_q_home_; 64 | 65 | ros::ServiceServer move_home_pose_service_; 66 | ros::ServiceServer move_next_calib_pose_service_; 67 | ros::ServiceServer do_estimation_step_service_; 68 | ros::ServiceServer save_calib_data_service_; 69 | ros::ServiceServer start_compensation_service_; 70 | ros::ServiceServer start_autonomus_estimation_service_; 71 | ros::Subscriber sub_ft_sensor_; 72 | ros::Publisher pub_joint_traj_ctl_; 73 | ros::Publisher pub_ft_sensor_no_offset_; 74 | ros::Publisher pub_ft_sensor_no_gravity_; 75 | 76 | Eigen::Vector3d p_sensor_tool_com_; 77 | Eigen::Vector3d ft_offset_force_; 78 | Eigen::Vector3d ft_offset_torque_; 79 | 80 | double calibration_loop_rate_; 81 | double publish_rate_; 82 | double tool_mass_; 83 | 84 | // duration for joint trajectory 85 | double p2p_traj_duration_; 86 | 87 | int pose_counter_; 88 | int number_of_poses_; 89 | 90 | bool do_compensation_; 91 | 92 | ros::Time last_publish_time_; 93 | }; 94 | 95 | } // namespace 96 | 97 | #endif 98 | -------------------------------------------------------------------------------- /include/lwr_force_position_controllers/hybrid_impedance_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef LWR_FORCE_POSITION_CONTROLLERS_HYBRID_IMPEDANCE_CONTROLLER_H 2 | #define LWR_FORCE_POSITION_CONTROLLERS_HYBRID_IMPEDANCE_CONTROLLER_H 3 | 4 | #include "cartesian_inverse_dynamics_controller.h" 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace lwr_controllers 13 | { 14 | class HybridImpedanceController: public CartesianInverseDynamicsController 15 | { 16 | public: 17 | 18 | HybridImpedanceController(); 19 | ~HybridImpedanceController(); 20 | 21 | bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n); 22 | void starting(const ros::Time& time); 23 | void update(const ros::Time& time, const ros::Duration& period); 24 | 25 | private: 26 | void set_default_pos_traj(); 27 | void set_default_force_traj(); 28 | bool set_cmd_traj_pos(lwr_force_position_controllers::HybridImpedanceCommandTrajPos::Request &req, \ 29 | lwr_force_position_controllers::HybridImpedanceCommandTrajPos::Response &res); 30 | bool set_cmd_traj_force(lwr_force_position_controllers::HybridImpedanceCommandTrajForce::Request &req, \ 31 | lwr_force_position_controllers::HybridImpedanceCommandTrajForce::Response &res); 32 | bool set_cmd_gains(lwr_force_position_controllers::HybridImpedanceCommandGains::Request &req, \ 33 | lwr_force_position_controllers::HybridImpedanceCommandGains::Response &res); 34 | bool get_cmd_traj_pos(lwr_force_position_controllers::HybridImpedanceCommandTrajPos::Request &req, \ 35 | lwr_force_position_controllers::HybridImpedanceCommandTrajPos::Response &res); 36 | bool get_cmd_traj_force(lwr_force_position_controllers::HybridImpedanceCommandTrajForce::Request &req, \ 37 | lwr_force_position_controllers::HybridImpedanceCommandTrajForce::Response &res); 38 | bool get_cmd_gains(lwr_force_position_controllers::HybridImpedanceCommandGains::Request &req, \ 39 | lwr_force_position_controllers::HybridImpedanceCommandGains::Response &res); 40 | bool switch_force_position_z(lwr_force_position_controllers::HybridImpedanceSwitchForcePos::Request &req, \ 41 | lwr_force_position_controllers::HybridImpedanceSwitchForcePos::Response &res); 42 | void get_parameters(ros::NodeHandle &n); 43 | void set_p_sensor_cp(double x, double y, double z); 44 | void eval_current_point_to_point_traj(const ros::Duration& period,\ 45 | Eigen::VectorXd& x_des,\ 46 | Eigen::VectorXd& xdot_des,\ 47 | Eigen::VectorXd& xdotdot_des); 48 | void eval_point_to_point_traj_constants(Eigen::Vector3d& desired_position,\ 49 | Eigen::Vector3d& desired_attitude,\ 50 | double duration); 51 | void evaluate_force_reference_constants(double force_des, double duration); 52 | double eval_force_reference(const ros::Duration& period); 53 | void publish_data(ros::Publisher& pub, KDL::Wrench wrench); 54 | void publish_data(ros::Publisher& pub, Eigen::VectorXd& vector); 55 | 56 | // HybridImpiedanceCommand service 57 | ros::ServiceServer set_cmd_traj_pos_service_; 58 | ros::ServiceServer set_cmd_traj_force_service_; 59 | ros::ServiceServer set_cmd_gains_service_; 60 | ros::ServiceServer switch_force_position_z_service_; 61 | ros::ServiceServer get_cmd_traj_pos_service_; 62 | ros::ServiceServer get_cmd_traj_force_service_; 63 | ros::ServiceServer get_cmd_gains_service_; 64 | 65 | // hybrid impedance controller (pose) 66 | Eigen::MatrixXd Kp_, Kd_; 67 | double p2p_traj_duration_; 68 | Eigen::MatrixXf p2p_trj_const_; 69 | Eigen::Vector3d prev_pos_setpoint_; 70 | Eigen::Vector3d prev_att_setpoint_; 71 | double time_; 72 | 73 | // hybrid impedance controller (force) 74 | double prev_fz_setpoint_; 75 | double km_f_, kd_f_; 76 | double force_ref_duration_; 77 | Eigen::VectorXf force_ref_const_; 78 | double time_force_; 79 | KDL::Vector p_sensor_cp_; 80 | 81 | // publisher to monitor data 82 | ros::Time last_publish_time_; 83 | double publish_rate_; 84 | ros::Publisher pub_force_, pub_force_des_, pub_state_, pub_dstate_; 85 | ros::Publisher pub_x_des_, pub_xdot_des_, pub_xdotdot_des_; 86 | ros::Publisher pub_error_; 87 | 88 | // 89 | boost::mutex p2p_traj_mutex_; 90 | boost::mutex force_traj_mutex_; 91 | 92 | // switch between z force control and z position control 93 | bool enable_force_; 94 | }; 95 | 96 | } // namespace 97 | 98 | #endif 99 | -------------------------------------------------------------------------------- /include/utils/euler_kinematical_rpy.h: -------------------------------------------------------------------------------- 1 | // Author: Nicola Piga, Giulio Romualdi 2 | // 3 | // eul_kin_RPY() computes the matrix T(PHI) such that omega = T PHI_dot 4 | // where omega is the angular velocity vector of the body whose attitude is represented by the angles PHI (RPY) 5 | // omega is projected with the rotation matrix R(PHI) 6 | // returns the matrix T 7 | // 8 | // eul_kin_RPY_dot() computes the matrix d/dt{T(PHI)} 9 | 10 | 11 | #ifndef EULER_KIN_RPY_H 12 | #define EULER_KIN_RPY_H 13 | 14 | #include 15 | #include 16 | using namespace Eigen; 17 | 18 | inline void eul_kin_RPY(const double pitch, const double yaw, Eigen::Matrix3d &T) 19 | { 20 | T << 21 | 0, -sin(yaw), cos(pitch) * cos(yaw), 22 | 0, cos(yaw), cos(pitch) * sin(yaw), 23 | 1, 0, -sin(pitch); 24 | } 25 | 26 | inline void eul_kin_RPY_dot(const double pitch, const double yaw, 27 | const double pitch_dot, const double yaw_dot, 28 | Eigen::Matrix3d &T_dot) 29 | { 30 | T_dot << 31 | 0, -cos(yaw) * yaw_dot, -cos(yaw) * sin(pitch) * pitch_dot - cos(pitch) * sin(yaw) * yaw_dot, 32 | 0, -sin(yaw) * yaw_dot, -sin(pitch) * sin(yaw) * pitch_dot + cos(pitch) * cos(yaw) * yaw_dot, 33 | 0, 0, -cos(pitch) * pitch_dot; 34 | } 35 | 36 | 37 | #endif 38 | -------------------------------------------------------------------------------- /include/utils/euler_kinematical_zyz.h: -------------------------------------------------------------------------------- 1 | // Author: Nicola Piga, Giulio Romualdi 2 | // 3 | // eul_kin_ZYZ() computes the matrix T(PHI) such that omega = T PHI_dot 4 | // where omega is the angular velocity vector of the body whose attitude is represented by the angles PHI (ZYZ) 5 | // omega is projected with the rotation matrix R(PHI) 6 | // returns the matrix T 7 | // 8 | // eul_kin_ZYZ_dot() computes the matrix d/dt{T(PHI)} 9 | 10 | 11 | #ifndef EULER_KIN_RPY_H 12 | #define EULER_KIN_RPY_H 13 | 14 | #include 15 | #include 16 | using namespace Eigen; 17 | 18 | inline void eul_kin_ZYZ(const double beta, const double alpha, Eigen::Matrix3d &T) 19 | { 20 | T << 21 | 0, -sin(alpha), cos(alpha) * sin(beta), 22 | 0, cos(alpha), sin(alpha) * sin(beta), 23 | 1, 0, cos(beta); 24 | } 25 | 26 | inline void eul_kin_ZYZ_dot(const double beta, const double alpha, 27 | const double beta_dot, const double alpha_dot, 28 | Eigen::Matrix3d &T_dot) 29 | { 30 | T_dot << 31 | 0, -cos(alpha) * alpha_dot, -sin(alpha) * sin(beta) * alpha_dot + cos(alpha) * cos(beta) * beta_dot, 32 | 0, -sin(alpha) * alpha_dot, cos(alpha) * sin(beta) * alpha_dot + cos(beta) * sin(alpha) * beta_dot, 33 | 0, 0, -sin(beta) * beta_dot; 34 | } 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /include/utils/skew_pinv.h: -------------------------------------------------------------------------------- 1 | // Author: Nicola Piga, Giulio Romualdi 2 | // 3 | // skew_pinv evaluate() the pseudo inverse of a skew symmetric matrix 4 | // vector is the vector whose skew symmetric is of interest 5 | // returns pinv(skew(vector)) in the matrix pinv 6 | 7 | #ifndef SKEW_PINV 8 | #define SKEW_PINV 9 | 10 | #include 11 | using namespace Eigen; 12 | 13 | inline void skew_pinv(Eigen::Vector3d &vector, Eigen::Matrix3d &pinv) 14 | { 15 | double denominator = vector.squaredNorm(); 16 | double a = vector(0); 17 | double b = vector(1); 18 | double c = vector(2); 19 | 20 | pinv << 21 | 0, c, -b, 22 | -c, 0, a, 23 | b, -a, 0; 24 | 25 | pinv /= denominator; 26 | } 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /launch/single_lwr.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | [/lwr/joint_states] 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | -------------------------------------------------------------------------------- /lwr_force_position_controllers_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Force Torque sensor calibration controller 6 | 7 | 8 | 9 | 10 | 11 | Implement a cartesian position controller using inverse joint dynamics and inverse kinematics 12 | 13 | 14 | 15 | 16 | 17 | Implement an inverse dynamics controller in operational space (the update() method is not implemented and should be implemented by inheriting 18 | controllers that need dynamics inversion in operational space). 19 | 20 | 21 | 22 | 23 | 24 | Implement a hybrid impedance controller using the abstract controller CartesianInverseDynamicsController 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /model/end_effector.gazebo: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 100.0 9 | ${name}/ft_sensor 10 | sensor_joint 11 | 12 | 13 | 14 | 15 | true 16 | true 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /model/end_effector.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /model/table.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | Gazebo/Yellow 83 | ${foam_kp} 84 | ${foam_kd} 85 | 0 86 | 0 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | -------------------------------------------------------------------------------- /msg/CartesianPositionCommandGainsMsg.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | # cartesian position controller gains 3 | float64 kp 4 | float64 kp_a4 5 | float64 kp_a5 6 | float64 kp_a6 7 | float64 kd 8 | float64 kd_a4 9 | float64 kd_a5 10 | float64 kd_a6 -------------------------------------------------------------------------------- /msg/CartesianPositionCommandTraj.msg: -------------------------------------------------------------------------------- 1 | # desired final position 2 | float64 x 3 | float64 y 4 | float64 z 5 | # desired final ZYX attitude 6 | float64 yaw 7 | float64 pitch 8 | float64 roll 9 | # duration of trajectory 10 | float64 p2p_traj_duration 11 | # info 12 | bool accepted 13 | float64 remaining_time -------------------------------------------------------------------------------- /msg/CartesianPositionCommandTrajMsg.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | # desired final position 3 | float64 x 4 | float64 y 5 | float64 z 6 | # desired final ZYX attitude 7 | float64 yaw 8 | float64 pitch 9 | float64 roll 10 | # duration of trajectory 11 | float64 p2p_traj_duration 12 | # info 13 | bool accepted 14 | float64 elapsed_time -------------------------------------------------------------------------------- /msg/CartesianPositionJointsMsg.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | # joint state 3 | float64 a1 4 | float64 a2 5 | float64 e1 6 | float64 a3 7 | float64 a4 8 | float64 a5 9 | float64 a6 -------------------------------------------------------------------------------- /msg/HybridImpedanceCommandGainsMsg.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | # controller gains (position) 3 | float64 kp_pos 4 | float64 kp_att 5 | float64 kp_gamma 6 | float64 kd_pos 7 | float64 kd_att 8 | float64 kd_gamma 9 | # controller gains (force) 10 | float64 km_f 11 | float64 kd_f 12 | # internal motion gains 13 | float64 kp_z_im 14 | float64 kp_gamma_im 15 | float64 kd_pos_im 16 | float64 kd_att_im 17 | -------------------------------------------------------------------------------- /msg/HybridImpedanceCommandTrajForceMsg.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | # desired force 3 | float64 forcez 4 | float64 force_ref_duration 5 | # info 6 | bool accepted 7 | float64 elapsed_time 8 | -------------------------------------------------------------------------------- /msg/HybridImpedanceCommandTrajPosMsg.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | # desired position 3 | float64 x 4 | float64 y 5 | float64 z 6 | float64 p2p_traj_duration 7 | # desired ZYZ 8 | float64 alpha 9 | float64 beta 10 | float64 gamma 11 | # info 12 | bool accepted 13 | float64 elapsed_time 14 | 15 | -------------------------------------------------------------------------------- /msg/HybridImpedanceSwitchForcePosMsg.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | bool enable_force_z -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lwr_force_position_controllers 4 | 0.0.0 5 | The lwr_force_position_controllers package 6 | 7 | manuelb 8 | 9 | TODO 10 | 11 | catkin 12 | controller_interface 13 | lwr_controllers 14 | roscpp 15 | kdljacdot 16 | message_runtime 17 | message_generation 18 | eigen_conversions 19 | kdl_conversions 20 | 21 | controller_interface 22 | lwr_controllers 23 | roscpp 24 | kdljacdot 25 | eigen_conversions 26 | kdl_conversions 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /robot/vito.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /robot/workbench.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /src/cartesian_inverse_dynamics_controller.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #define DEFAULT_KP_IM_LINK4 0.001 12 | #define DEFAULT_KP_IM_LINK5 3 13 | #define DEFAULT_KD_IM_LINK4 10 14 | #define DEFAULT_KD_IM_LINK5 10 15 | // syntax: 16 | // 17 | // for jacobians x_J_y := Jacobian w.r.t reference point y expressed in basis x 18 | // for analytical jacobians x_JA_y := analytical Jacobian w.r.t reference point y expressed in basis x 19 | // for rotation matrices R_x_y := Rotation from basis y to basis x 20 | // for wrenches x_wrench_y := Wrench w.r.t reference point y expressed in basis x 21 | // for vectors in general x_vector := vector expressed in basis x 22 | // p_x_y := arm from x to y 23 | // ee := reference point of interest (typically the tool tip) 24 | // wrist := tip of the 7th link of the Kuka LWR 25 | // T := euler kinematical matrix 26 | 27 | namespace lwr_controllers { 28 | 29 | CartesianInverseDynamicsController::CartesianInverseDynamicsController() {} 30 | CartesianInverseDynamicsController::~CartesianInverseDynamicsController() {} 31 | 32 | bool CartesianInverseDynamicsController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n) 33 | { 34 | KinematicChainControllerBase::init(robot, n); 35 | 36 | // get use_simulation parameter from rosparam server 37 | ros::NodeHandle nh; 38 | nh.getParam("/use_simulation", use_simulation_); 39 | 40 | // extend kdl chain with end-effector 41 | extend_chain(n); 42 | 43 | // create two chain from vito_anchor to the link 44 | // specified by the parameter internal_motion_controlled_link 45 | std::string root_name, im_c_link_name; 46 | nh_.getParam("root_name", root_name); 47 | // nh_.getParam("internal_motion_controlled_link", im_c_link_name); 48 | kdl_tree_.getChain(root_name, "lwr_4_link", im_link4_chain_); 49 | kdl_tree_.getChain(root_name, "lwr_5_link", im_link5_chain_); 50 | 51 | // instantiate solvers 52 | // gravity_ is a member of KinematicChainControllerBase 53 | dyn_param_solver_.reset(new KDL::ChainDynParam(kdl_chain_, gravity_)); 54 | ee_jacobian_solver_.reset(new KDL::ChainJntToJacSolver(extended_chain_)); 55 | wrist_jacobian_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_)); 56 | ee_fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(extended_chain_)); 57 | ee_jacobian_dot_solver_.reset(new KDL::ChainJntToJacDotSolver(extended_chain_)); 58 | im_link4_jacobian_solver_.reset(new KDL::ChainJntToJacSolver(im_link4_chain_)); 59 | im_link4_fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(im_link4_chain_)); 60 | im_link5_jacobian_solver_.reset(new KDL::ChainJntToJacSolver(im_link5_chain_)); 61 | im_link5_fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(im_link5_chain_)); 62 | 63 | // instantiate wrenches 64 | wrench_wrist_ = KDL::Wrench(); 65 | base_wrench_wrist_ = KDL::Wrench(); 66 | 67 | // instantiate state and its derivatives 68 | ws_x_ = Eigen::VectorXd(6); 69 | ws_xdot_ = Eigen::VectorXd(6); 70 | 71 | // instantiate analytical to geometric transformation matrices 72 | ws_TA_ = Eigen::MatrixXd::Zero(6,6); 73 | ws_TA_.block<3,3>(0,0) = Eigen::Matrix::Identity(); 74 | ws_TA_dot_ = Eigen::MatrixXd::Zero(6,6); 75 | 76 | // instantiate analytical to geometric transformation matrices 77 | base_TA_im_link5_ = Eigen::MatrixXd::Zero(6,6); 78 | base_TA_im_link5_.block<3,3>(0,0) = Eigen::Matrix::Identity(); 79 | 80 | // set default controller gains 81 | Kp_im_ = Eigen::Matrix::Zero(); 82 | Kd_im_ = Eigen::Matrix::Zero(); 83 | for(int i = 0; i < 3; i++) 84 | Kd_im_(i,i) = DEFAULT_KD_IM_LINK4; 85 | for(int i = 3; i < 6; i++) 86 | Kd_im_(i,i) = DEFAULT_KD_IM_LINK5; 87 | 88 | // set proportional action in the z direction only (for link4) (position) 89 | Kp_im_(2,2) = DEFAULT_KP_IM_LINK4; 90 | // set proportional action along z axis only (for link5) (attitude) 91 | Kp_im_(5,5) = DEFAULT_KP_IM_LINK5; 92 | 93 | // subscribe to force/torque sensor topic 94 | sub_force_ = n.subscribe(ft_sensor_topic_name_, 1, \ 95 | &CartesianInverseDynamicsController::force_torque_callback, this); 96 | 97 | return true; 98 | } 99 | 100 | void CartesianInverseDynamicsController::starting(const ros::Time& time) 101 | { 102 | 103 | // get current robot configuration (q) for the 5th link 104 | KDL::JntArray q_im_link5; 105 | q_im_link5.resize(im_link5_chain_.getNrOfJoints()); 106 | for(size_t i=0; iJntToCart(q_im_link5, im_link5_fk_frame); 113 | im_link5_fk_frame.M.GetEulerZYZ(alpha_im_link5, beta_im_link5, gamma_im_link5); 114 | 115 | gamma_im_link5_initial_ = gamma_im_link5; 116 | } 117 | 118 | void CartesianInverseDynamicsController::update_fri_inertia_matrix(Eigen::MatrixXd& fri_B) 119 | { 120 | int n_joints = kdl_chain_.getNrOfJoints(); 121 | for(int i = 0; i < n_joints; i++) 122 | for(int j = 0; j < n_joints; j++) 123 | fri_B(i,j) = inertia_matrix_handles_[i * n_joints + j].getPosition(); 124 | } 125 | 126 | void CartesianInverseDynamicsController::update(const ros::Time& time, const ros::Duration& period) 127 | { 128 | ////////////////////////////////////////////////////////////////////////////////// 129 | // 130 | // Robot configuration 131 | // 132 | ////////////////////////////////////////////////////////////////////////////////// 133 | // 134 | 135 | // get current robot configuration (q and q dot) 136 | for(size_t i=0; iJntToMass(joint_msr_states_.q, B); 171 | 172 | // evaluate the current C(q) * q dot 173 | KDL::JntArray C; 174 | C.resize(kdl_chain_.getNrOfJoints()); 175 | dyn_param_solver_->JntToCoriolis(joint_msr_states_.q, joint_msr_states_.qdot, C); 176 | 177 | // 178 | ////////////////////////////////////////////////////////////////////////////////// 179 | 180 | ////////////////////////////////////////////////////////////////////////////////// 181 | // 182 | // Geometric Jacobians 183 | // 184 | ////////////////////////////////////////////////////////////////////////////////// 185 | // 186 | 187 | // evaluate the current geometric jacobian base_J_ee 188 | KDL::Jacobian base_J_ee; 189 | base_J_ee.resize(kdl_chain_.getNrOfJoints()); 190 | ee_jacobian_solver_->JntToJac(joint_msr_states_.q, base_J_ee); 191 | 192 | // evaluate the current geometric jacobian base_J_wrist 193 | KDL::Jacobian base_J_wrist; 194 | base_J_wrist.resize(kdl_chain_.getNrOfJoints()); 195 | wrist_jacobian_solver_->JntToJac(joint_msr_states_.q, base_J_wrist); 196 | 197 | // evaluate the current geometric jacobian related to 198 | // the internal motion controlled links (linear velocity of the third and 199 | // angular velocity of the fourth) 200 | KDL::Jacobian base_J_im_link4; 201 | KDL::Jacobian base_J_im_link5; 202 | Eigen::MatrixXd base_J_im = Eigen::MatrixXd::Zero(6,7); 203 | base_J_im_link4.resize(im_link4_chain_.getNrOfJoints()); 204 | base_J_im_link5.resize(im_link5_chain_.getNrOfJoints()); 205 | im_link4_jacobian_solver_->JntToJac(q_im_link4, base_J_im_link4); 206 | im_link5_jacobian_solver_->JntToJac(q_im_link5, base_J_im_link5); 207 | base_J_im.block(0, 0, 3, im_link4_chain_.getNrOfJoints()) = \ 208 | base_J_im_link4.data.block(0, 0, 3, im_link4_chain_.getNrOfJoints()); 209 | 210 | // 211 | ////////////////////////////////////////////////////////////////////////////////// 212 | 213 | ////////////////////////////////////////////////////////////////////////////////// 214 | // 215 | // Forward Kinematics 216 | // 217 | ////////////////////////////////////////////////////////////////////////////////// 218 | // 219 | 220 | // end effector 221 | KDL::Frame ee_fk_frame; 222 | ee_fk_solver_->JntToCart(joint_msr_states_.q, ee_fk_frame); 223 | 224 | // internal motion controlled link 225 | KDL::Frame im_link4_fk_frame; 226 | KDL::Frame im_link5_fk_frame; 227 | im_link4_fk_solver_->JntToCart(q_im_link4, im_link4_fk_frame); 228 | im_link5_fk_solver_->JntToCart(q_im_link5, im_link5_fk_frame); 229 | 230 | // 231 | ////////////////////////////////////////////////////////////////////////////////// 232 | 233 | ////////////////////////////////////////////////////////////////////////////////// 234 | // 235 | // Analytical Jacobian written w.r.t. the workspace frame ws_JA_ee 236 | // ws_JA_ee = ws_TA * ws_J_ee 237 | // 238 | ////////////////////////////////////////////////////////////////////////////////// 239 | // 240 | 241 | // get the current ZYZ attitude representation PHI from R_ws_base * ee_fk_frame.M 242 | double alpha, beta, gamma; 243 | R_ws_ee_ = R_ws_base_ * ee_fk_frame.M; 244 | R_ws_ee_.GetEulerZYZ(alpha, beta, gamma); 245 | 246 | // evaluate the transformation matrix between 247 | // the geometric and analytical jacobian TA 248 | // 249 | // ws_TA = [eye(3), zeros(3); 250 | // zeros(3), inv(T(PHI))] 251 | // where T is the Euler Kinematical Matrix 252 | // 253 | Eigen::Matrix3d ws_T; 254 | eul_kin_ZYZ(beta, alpha, ws_T); 255 | ws_TA_.block<3,3>(3,3) = ws_T.inverse(); 256 | 257 | // evaluate ws_J_ee 258 | KDL::Jacobian ws_J_ee; 259 | ws_J_ee.resize(kdl_chain_.getNrOfJoints()); 260 | KDL::changeBase(base_J_ee, R_ws_base_, ws_J_ee); 261 | 262 | Eigen::MatrixXd ws_JA_ee; 263 | ws_JA_ee = ws_TA_ * ws_J_ee.data; 264 | // 265 | ////////////////////////////////////////////////////////////////////////////////// 266 | 267 | 268 | ////////////////////////////////////////////////////////////////////////////////// 269 | // 270 | // Analytical Jacobian written w.r.t. the vito_anchor frame base_JA_im_link5 271 | // base_JA_im_link5 = base_TA_im_link5 * base_J_im_link5 272 | // 273 | ////////////////////////////////////////////////////////////////////////////////// 274 | // 275 | 276 | double alpha_im_link5, beta_im_link5, gamma_im_link5; 277 | im_link5_fk_frame.M.GetEulerZYZ(alpha_im_link5, beta_im_link5, gamma_im_link5); 278 | 279 | // evaluate the transformation matrix between 280 | // the geometric and analytical jacobian TA 281 | 282 | Eigen::Matrix3d base_T_im_link5; 283 | eul_kin_ZYZ(beta_im_link5, alpha_im_link5, base_T_im_link5); 284 | base_TA_im_link5_.block<3,3>(3,3) = base_T_im_link5.inverse(); 285 | 286 | Eigen::MatrixXd base_JA_im_link5; 287 | base_JA_im_link5 = base_TA_im_link5_ * base_J_im_link5.data; 288 | 289 | // add the angular part of base_JA_im_link5 to base_J_im 290 | base_J_im.block(3, 0, 3, im_link5_chain_.getNrOfJoints()) = \ 291 | base_JA_im_link5.block(3, 0, 3, im_link5_chain_.getNrOfJoints()); 292 | 293 | // 294 | ////////////////////////////////////////////////////////////////////////////////// 295 | 296 | 297 | ////////////////////////////////////////////////////////////////////////////////// 298 | // 299 | // Kinetic pseudo-energy BA (Siciliano p. 297) 300 | // (i.e. Operational Space Inertia Matrix) 301 | // 302 | ////////////////////////////////////////////////////////////////////////////////// 303 | // 304 | 305 | // BA = inv(ws_JA_ee * B_inv * base_J_wrist') 306 | Eigen::MatrixXd BA; 307 | if(use_simulation_) 308 | // use kdl matrix for simulation 309 | BA = ws_JA_ee * B.data.inverse() * base_J_wrist.data.transpose(); 310 | else 311 | // use kdl matrix for real scenario 312 | BA = ws_JA_ee * B.data.inverse() * base_J_wrist.data.transpose(); 313 | 314 | BA = BA.inverse(); 315 | 316 | // 317 | ////////////////////////////////////////////////////////////////////////////////// 318 | 319 | ////////////////////////////////////////////////////////////////////////////////// 320 | // 321 | // Coriolis compensation in *Operational Space* 322 | // BA * dot(ws_JA_ee) * qdot 323 | // 324 | ////////////////////////////////////////////////////////////////////////////////// 325 | // 326 | 327 | // evaluation of dot(ws_JA_ee) = d/dt{ws_TA} * ws_J_ee + ws_TA * d/dt{ws_J_ee} 328 | // 329 | // where d/dt{ws_TA} = [d/dt{eye(3)}, d/dt{zeros(3)}; 330 | // d/dt{zeros(3)}, d/dt{inv(T(PHI))}] 331 | // = [zeros(3), zeros(3); 332 | // zeros(3), -inv(T) * d/dt{T} * int(T)] 333 | // 334 | // and d/dt{ws_J_ee} = [R_ws_base_, zeros(3); 335 | // zeros(3), R_ws_base_] * d/dt{base_J_ee} 336 | // 337 | 338 | // evaluate the derivative of the state using the analytical jacobian 339 | Eigen::Matrix3d ws_T_dot; 340 | ws_xdot_ = ws_JA_ee * joint_msr_states_.qdot.data; 341 | eul_kin_ZYZ_dot(beta, alpha, ws_xdot_(4), ws_xdot_(3), ws_T_dot); 342 | ws_TA_dot_.block<3,3>(3,3) = - ws_T.inverse() * ws_T_dot * ws_T.inverse(); 343 | 344 | // evaluate the derivative of the jacobian base_J_ee 345 | KDL::JntArrayVel jnt_q_qdot; 346 | KDL::Jacobian ws_J_ee_dot; 347 | ws_J_ee_dot.resize(kdl_chain_.getNrOfJoints()); 348 | jnt_q_qdot.q = joint_msr_states_.q; 349 | jnt_q_qdot.qdot = joint_msr_states_.qdot; 350 | ee_jacobian_dot_solver_->JntToJacDot(jnt_q_qdot, ws_J_ee_dot); 351 | 352 | // and project it in the workspace frame 353 | ws_J_ee_dot.changeBase(R_ws_base_); 354 | 355 | Eigen::MatrixXd ws_JA_ee_dot; 356 | ws_JA_ee_dot = ws_TA_dot_ * ws_J_ee.data + ws_TA_ * ws_J_ee_dot.data; 357 | // 358 | ////////////////////////////////////////////////////////////////////////////////// 359 | 360 | ////////////////////////////////////////////////////////////////////////////////// 361 | // 362 | // project ft_sensor wrench in world frame 363 | // 364 | ////////////////////////////////////////////////////////////////////////////////// 365 | // 366 | 367 | Eigen::Matrix base_F_wrist; 368 | base_wrench_wrist_ = ee_fk_frame.M * wrench_wrist_; 369 | tf::wrenchKDLToEigen(base_wrench_wrist_, base_F_wrist); 370 | 371 | // 372 | ////////////////////////////////////////////////////////////////////////////////// 373 | 374 | ////////////////////////////////////////////////////////////////////////////////// 375 | // 376 | // evaluate dynamics inversion command TAU_FRI 377 | // 378 | // inheriting controllers augment TAU_FRI by calling 379 | // set_command(desired_acceleration) 380 | // so that TAU_FRI += command_filter * desired_accelration 381 | // 382 | ////////////////////////////////////////////////////////////////////////////////// 383 | // 384 | 385 | if(use_simulation_) 386 | tau_fri_ = C.data + base_J_wrist.data.transpose() * \ 387 | (base_F_wrist - BA * ws_JA_ee_dot * joint_msr_states_.qdot.data); 388 | else 389 | tau_fri_ = C.data + base_J_wrist.data.transpose() * \ 390 | (base_F_wrist - BA * ws_JA_ee_dot * joint_msr_states_.qdot.data); 391 | 392 | command_filter_ = base_J_wrist.data.transpose() * BA; 393 | 394 | // 395 | ////////////////////////////////////////////////////////////////////////////////// 396 | 397 | ////////////////////////////////////////////////////////////////////////////////// 398 | // 399 | // internal motion handling 400 | // (see A Unified Approach for Motion and Force Control 401 | // of Robot Manipulators: The Operational Space Formulation, Oussama Khatib 402 | // for details on the definition of a dynamically consistent generalized inverse) 403 | // 404 | ////////////////////////////////////////////////////////////////////////////////// 405 | // 406 | 407 | // evaluate a dynamically consistent generalized inverse 408 | Eigen::MatrixXd gen_inv; 409 | if(use_simulation_) 410 | gen_inv = B.data.inverse() * base_J_wrist.data.transpose() * \ 411 | (base_J_wrist.data * B.data.inverse() * base_J_wrist.data.transpose()).inverse(); 412 | else 413 | gen_inv = B.data.inverse() * base_J_wrist.data.transpose() * \ 414 | (base_J_wrist.data * B.data.inverse() * base_J_wrist.data.transpose()).inverse(); 415 | 416 | // evaluate the null space filter 417 | Eigen::MatrixXd ns_filter = Eigen::Matrix::Identity() - \ 418 | base_J_wrist.data.transpose() * gen_inv.transpose(); 419 | 420 | // state and derivative of the state 421 | Eigen::VectorXd im_state(6); 422 | Eigen::VectorXd im_state_dot; 423 | 424 | im_state << im_link4_fk_frame.p.x(), im_link4_fk_frame.p.y(), im_link4_fk_frame.p.z(),\ 425 | alpha_im_link5, beta_im_link5, gamma_im_link5; 426 | 427 | im_state_dot = base_J_im * joint_msr_states_.qdot.data; 428 | 429 | // control law 430 | // tau = null_space_filter * J_im' * (Kp * (x_des - x) - Kd * x_dot) 431 | Eigen::VectorXd im_des_state = Eigen::VectorXd(6); 432 | 433 | // control strategy is to command an height offset between ee and link4 434 | // and command the attitude (gamma only) of the link5 435 | // in order to avoid joints limits 436 | double z_offset = 0.8; 437 | im_des_state << ee_fk_frame.p.x(), ee_fk_frame.p.y() + 0.6 , ee_fk_frame.p.z() + z_offset, \ 438 | -M_PI / 2, M_PI / 2, 0; 439 | 440 | if(!(-M_PI / 2 < gamma_im_link5_initial_ && gamma_im_link5_initial_ < M_PI / 2)) 441 | im_des_state(5) = M_PI; 442 | 443 | Eigen::VectorXd im_error = im_des_state - im_state; 444 | // normalize angular error between - M_PI and M_PI 445 | im_error(3) = angles::normalize_angle(im_error(3)); 446 | im_error(4) = angles::normalize_angle(im_error(4)); 447 | im_error(5) = angles::normalize_angle(im_error(5)); 448 | 449 | // filter and add the command to TAU_FRI 450 | tau_fri_ += ns_filter * base_J_im.transpose() *\ 451 | (Kp_im_ * im_error - Kd_im_ * im_state_dot); 452 | 453 | // 454 | ////////////////////////////////////////////////////////////////////////////////// 455 | 456 | ////////////////////////////////////////////////////////////////////////////////// 457 | // 458 | // the following are possibly required by inheriting controllers 459 | // 460 | ////////////////////////////////////////////////////////////////////////////////// 461 | // 462 | 463 | // evaluate position vector between the origin of the workspace frame and the end-effector 464 | KDL::Vector p_ws_ee; 465 | p_ws_ee = R_ws_base_ * (ee_fk_frame.p - p_base_ws_); 466 | 467 | // evaluate the state 468 | ws_x_ << p_ws_ee(0), p_ws_ee(1), p_ws_ee(2), alpha, beta, gamma; 469 | 470 | // the derivative of the state 471 | // was already evaluated in the previous sections 472 | 473 | // 474 | ////////////////////////////////////////////////////////////////////////////////// 475 | 476 | } 477 | 478 | void CartesianInverseDynamicsController::set_command(Eigen::VectorXd& commanded_acceleration) 479 | { 480 | // augment tau_fri with the desired command specified by the inheriting controller 481 | tau_fri_ += command_filter_ * commanded_acceleration; 482 | 483 | // set joint efforts 484 | for(int i=0; i p_sensor_tool_com_vec(6); 502 | // get data from the yaml file 503 | double tool_mass = ft_data_yaml["gripper_mass"].as(); 504 | p_sensor_tool_com_vec = ft_data_yaml["gripper_com_pose"].as>(); 505 | 506 | // transform to KDL 507 | for (int i=0; i<3; i++) 508 | p_sensor_tool_com.data[i] = p_sensor_tool_com_vec[i]; 509 | 510 | // compensate for additional items attached 511 | double mass_sensor_support = 0.194; 512 | double mass_sensor = 0.099; 513 | total_mass = tool_mass + mass_sensor_support + mass_sensor; 514 | 515 | p_sensor_tool_com.x(tool_mass * p_sensor_tool_com.x() / total_mass); 516 | p_sensor_tool_com.y(tool_mass * p_sensor_tool_com.y() / total_mass); 517 | p_sensor_tool_com.z((mass_sensor_support * 0.013 + mass_sensor * 0.0335 + \ 518 | tool_mass * (0.041 + p_sensor_tool_com.z())) / total_mass); 519 | 520 | } 521 | 522 | void CartesianInverseDynamicsController::extend_chain(ros::NodeHandle &n) 523 | { 524 | // extend the default chain with a fake segment in order to evaluate 525 | // dynamics (B and C), Jacobians, derivatives of jacobians and 526 | // forward kinematics with respect to a given reference point 527 | // (typically the tool tip) 528 | // the reference point is initialized by the inheriting class with a call to set_p_wrist_ee 529 | KDL::Joint fake_joint = KDL::Joint(); 530 | KDL::Frame frame(KDL::Rotation::Identity(), p_wrist_ee_); 531 | 532 | double total_mass; 533 | double fake_cylinder_radius = 0.0475; 534 | double fake_cylinder_height = 0.31; 535 | KDL::Vector p_sensor_tool_com; 536 | load_calib_data(total_mass, p_sensor_tool_com); 537 | 538 | double fake_cyl_i_xx = 1.0 / 12.0 * total_mass * (3 * pow(fake_cylinder_radius, 2) + \ 539 | pow(fake_cylinder_height, 2)); 540 | double fake_cyl_i_zz = 1.0 / 2.0 * total_mass * pow(fake_cylinder_radius, 2); 541 | 542 | 543 | KDL::RotationalInertia rot_inertia(fake_cyl_i_xx, fake_cyl_i_xx, fake_cyl_i_zz); 544 | KDL::RigidBodyInertia inertia(total_mass, p_sensor_tool_com, rot_inertia); 545 | 546 | KDL::Segment fake_segment(fake_joint, frame, inertia); 547 | extended_chain_ = kdl_chain_; 548 | extended_chain_.addSegment(fake_segment); 549 | 550 | } 551 | 552 | void CartesianInverseDynamicsController::force_torque_callback(const geometry_msgs::WrenchStamped::ConstPtr& msg) 553 | { 554 | KDL::Wrench wrench_wrist_topic; 555 | tf::wrenchMsgToKDL(msg->wrench, wrench_wrist_topic); 556 | 557 | // reverse the measured force so that wrench_wrist represents 558 | // the force applied on the environment by the end-effector 559 | wrench_wrist_ = - wrench_wrist_topic; 560 | } 561 | 562 | void CartesianInverseDynamicsController::get_gains_im(double& kp_z, double& kp_gamma, double& kd_pos, double& kd_att) 563 | { 564 | kp_z = Kp_im_(2, 2); 565 | kp_gamma = Kp_im_(5, 5); 566 | kd_pos = Kd_im_(0, 0); 567 | kd_att = Kd_im_(3, 3); 568 | } 569 | 570 | void CartesianInverseDynamicsController::set_gains_im(double kp_z, double kp_gamma, double kd_pos, double kd_att) 571 | { 572 | Kp_im_(2,2) = kp_z; 573 | Kp_im_(5,5) = kp_gamma; 574 | for(int i = 0; i < 3; i++) 575 | Kd_im_(i,i) = kd_pos; 576 | for(int i = 3; i < 6; i++) 577 | Kd_im_(i,i) = kd_att; 578 | } 579 | 580 | void CartesianInverseDynamicsController::set_ft_sensor_topic_name(std::string topic) 581 | { 582 | ft_sensor_topic_name_ = topic; 583 | } 584 | 585 | void CartesianInverseDynamicsController::set_p_wrist_ee(double x, double y, double z) 586 | { 587 | p_wrist_ee_ = KDL::Vector(x, y, z); 588 | } 589 | 590 | void CartesianInverseDynamicsController::set_p_base_ws(double x, double y, double z) 591 | { 592 | p_base_ws_ = KDL::Vector(x, y, z); 593 | } 594 | 595 | void CartesianInverseDynamicsController::set_ws_base_angles(double alpha, double beta, double gamma) 596 | { 597 | R_ws_base_ = KDL::Rotation::EulerZYZ(alpha, beta, gamma); 598 | } 599 | 600 | } // namespace 601 | 602 | PLUGINLIB_EXPORT_CLASS(lwr_controllers::CartesianInverseDynamicsController , controller_interface::ControllerBase) 603 | -------------------------------------------------------------------------------- /src/cartesian_position_controller.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // KDL 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | #include 19 | 20 | //------------------------------------- 21 | // GAIN CONSTANTS 22 | //------------------------------------- 23 | #define DEFAULT_KP 800 24 | #define DEFAULT_KP_A4 1000 25 | #define DEFAULT_KP_A5 1000 26 | #define DEFAULT_KP_A6 18000 27 | #define DEFAULT_KD 30 28 | #define DEFAULT_KD_A4 30 29 | #define DEFAULT_KD_A5 30 30 | #define DEFAULT_KD_A6 30 31 | 32 | //------------------------------------------------------------------------------ 33 | //TRAJECTORY GENERATION CONSTANTS 34 | //------------------------------------------------------------------------------ 35 | #define FINAL_TIME 5.0 36 | #define TRAJ_5 6.0 // 5-th coeff. of trajectory polynomial 37 | #define TRAJ_4 -15.0 // 4-th coeff. of trajectory polynomial 38 | #define TRAJ_3 10.0 // 3-th coeff. of trajectory polynomial 39 | 40 | namespace lwr_controllers { 41 | 42 | CartesianPositionController::CartesianPositionController() {} 43 | 44 | CartesianPositionController::~CartesianPositionController() {} 45 | 46 | bool CartesianPositionController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n) 47 | { 48 | KinematicChainControllerBase::init(robot, n); 49 | 50 | // get use_simulation parameter from rosparam server 51 | ros::NodeHandle nh; 52 | nh.getParam("/use_simulation", use_simulation_); 53 | 54 | // get publish rate from rosparam 55 | n.getParam("publish_rate", publish_rate_); 56 | 57 | // add end-effector in kdl chain 58 | extend_chain(n); 59 | 60 | // instantiate solvers 61 | dyn_param_solver_.reset(new KDL::ChainDynParam(extended_chain_, gravity_)); 62 | ik_solver_.reset(new KDL::ChainIkSolverPos_LMA(extended_chain_)); 63 | ee_fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(extended_chain_)); 64 | if (use_simulation_) 65 | { 66 | jacobian_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_)); 67 | fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_)); 68 | } 69 | 70 | // set the default gains 71 | kp_ = DEFAULT_KP; 72 | kp_a4_ = DEFAULT_KP_A4; 73 | kp_a5_ = DEFAULT_KP_A5; 74 | kp_a6_ = DEFAULT_KP_A6; 75 | kd_ = DEFAULT_KD; 76 | kd_a4_ = DEFAULT_KD_A4; 77 | kd_a5_ = DEFAULT_KD_A5; 78 | kd_a6_ = DEFAULT_KD_A6; 79 | 80 | // set default trajectory duration 81 | p2p_traj_duration_ = FINAL_TIME; 82 | 83 | // resize desired trajectory 84 | traj_des_.resize(kdl_chain_.getNrOfJoints()); 85 | traj_a0_.resize(kdl_chain_.getNrOfJoints()); 86 | traj_a3_.resize(kdl_chain_.getNrOfJoints()); 87 | traj_a4_.resize(kdl_chain_.getNrOfJoints()); 88 | traj_a5_.resize(kdl_chain_.getNrOfJoints()); 89 | prev_q_setpoint_.resize(kdl_chain_.getNrOfJoints()); 90 | 91 | // advertise CartesianPositionCommand services 92 | set_cmd_gains_service_ = n.advertiseService("set_cartpos_gains_cmd", \ 93 | &CartesianPositionController::set_cmd_gains, this); 94 | set_cmd_traj_service_ = n.advertiseService("set_cartpos_traj_cmd", \ 95 | &CartesianPositionController::set_cmd_traj, this); 96 | get_cmd_gains_service_ = n.advertiseService("get_cartpos_gains_cmd", \ 97 | &CartesianPositionController::get_cmd_gains, this); 98 | get_cmd_traj_service_ = n.advertiseService("get_cartpos_traj_cmd", \ 99 | &CartesianPositionController::get_cmd_traj, this); 100 | 101 | // advertise topics 102 | pub_error_ = n.advertise("error", 1000); 103 | pub_q_des_ = n.advertise("q_des", 1000); 104 | 105 | if(use_simulation_) 106 | { 107 | // subscribe to force/torque sensor topic 108 | // (simulation only since it is required to compensate for the mass tool, 109 | // the real kuka compensate for mass tool internally) 110 | sub_force_ = n.subscribe("/lwr/ft_sensor", 1,\ 111 | &CartesianPositionController::ft_sensor_callback, this); 112 | } 113 | 114 | return true; 115 | } 116 | 117 | void CartesianPositionController::load_calib_data(double& total_mass, KDL::Vector& p_sensor_tool_com) 118 | { 119 | std::string file_name = ros::package::getPath("lwr_force_position_controllers") +\ 120 | "/config/ft_calib_data.yaml"; 121 | YAML::Node ft_data_yaml = YAML::LoadFile(file_name); 122 | 123 | std::vector p_sensor_tool_com_vec(6); 124 | // get data from the yaml file 125 | double tool_mass = ft_data_yaml["gripper_mass"].as(); 126 | p_sensor_tool_com_vec = ft_data_yaml["gripper_com_pose"].as>(); 127 | 128 | // transform to KDL 129 | for (int i=0; i<3; i++) 130 | p_sensor_tool_com.data[i] = p_sensor_tool_com_vec[i]; 131 | 132 | // compensate for additional items attached 133 | double mass_sensor_support = 0.194; 134 | double mass_sensor = 0.099; 135 | total_mass = tool_mass + mass_sensor_support + mass_sensor; 136 | 137 | p_sensor_tool_com.x(tool_mass * p_sensor_tool_com.x() / total_mass); 138 | p_sensor_tool_com.y(tool_mass * p_sensor_tool_com.y() / total_mass); 139 | p_sensor_tool_com.z((mass_sensor_support * 0.013 + mass_sensor * 0.0335 + \ 140 | tool_mass * (0.041 + p_sensor_tool_com.z())) / total_mass); 141 | 142 | } 143 | 144 | void CartesianPositionController::starting(const ros::Time& time) 145 | { 146 | 147 | // set desired quantities 148 | time_ = p2p_traj_duration_; 149 | for(size_t i=0; iJntToMass(joint_msr_states_.q, B); 210 | dyn_param_solver_->JntToCoriolis(joint_msr_states_.q, joint_msr_states_.qdot, C); 211 | 212 | KDL::Jacobian J_; 213 | Eigen::Matrix base_F_wrist_; 214 | if (use_simulation_) 215 | { 216 | //////////////////////////////////////////////////////////////////////////////// 217 | // evaluate the wrench applied to the wrist from the force/torque sensor 218 | // and project it in the world frame (base) 219 | // this is required to compensate for additional masses attached past the wrist 220 | // (simulation only, the real kuka compensate for mass tool internally) 221 | //////////////////////////////////////////////////////////////////////////////// 222 | // 223 | 224 | // evaluate the current geometric jacobian J(q) 225 | J_.resize(kdl_chain_.getNrOfJoints()); 226 | jacobian_solver_->JntToJac(joint_msr_states_.q, J_); 227 | 228 | // evaluate the forward kinematics required to project the wrench 229 | KDL::Frame fk_frame; 230 | KDL::Wrench base_wrench_wrist; 231 | fk_solver_->JntToCart(joint_msr_states_.q, fk_frame); 232 | base_wrench_wrist = fk_frame.M * wrench_wrist_; 233 | 234 | // write down onto an eigen vector 235 | tf::wrenchKDLToEigen(base_wrench_wrist, base_F_wrist_); 236 | // 237 | /////////////////////////////////////////////////////////////////////////////// 238 | } 239 | 240 | // evaluate B * tau_cmd 241 | KDL::JntArray B_tau_cmd; 242 | B_tau_cmd.resize(kdl_chain_.getNrOfJoints()); 243 | if (use_simulation_) 244 | // use J * base_F_wrist as a way to compensate for the mass of the tool (simulation only) 245 | B_tau_cmd.data = B.data * tau_cmd.data + C.data; 246 | else 247 | // use kdl model in real scenario 248 | B_tau_cmd.data = B.data * tau_cmd.data + C.data; 249 | 250 | // set joint efforts 251 | for(size_t i=0; i last_publish_time_ + ros::Duration(1.0 / publish_rate_)) 263 | { 264 | //update next tick 265 | last_publish_time_ += ros::Duration(1.0 / publish_rate_); 266 | 267 | // publish data 268 | publish_data(pub_error_, q_error); 269 | publish_data(pub_q_des_, traj_des_.q); 270 | } 271 | } 272 | 273 | void CartesianPositionController::publish_data(ros::Publisher& pub, KDL::JntArray& array) 274 | { 275 | lwr_force_position_controllers::CartesianPositionJointsMsg msg; 276 | msg.header.stamp = ros::Time::now(); 277 | msg.a1 = array(0); 278 | msg.a2 = array(1); 279 | msg.e1 = array(2); 280 | msg.a3 = array(3); 281 | msg.a4 = array(4); 282 | msg.a5 = array(5); 283 | msg.a6 = array(6); 284 | pub.publish(msg); 285 | } 286 | 287 | void CartesianPositionController::ft_sensor_callback(const geometry_msgs::WrenchStamped::ConstPtr& msg) 288 | { 289 | KDL::Wrench wrench_wrist_topic; 290 | tf::wrenchMsgToKDL(msg->wrench, wrench_wrist_topic); 291 | 292 | // reverse the measured force so that wrench_wrist represents 293 | // the force applied on the environment by the end-effector 294 | wrench_wrist_ = - wrench_wrist_topic; 295 | } 296 | 297 | bool CartesianPositionController::set_cmd_traj(lwr_force_position_controllers::CartesianPositionCommandTraj::Request &req,\ 298 | lwr_force_position_controllers::CartesianPositionCommandTraj::Response &res) 299 | { 300 | if (time_ < p2p_traj_duration_) 301 | { 302 | res.command.elapsed_time = time_; 303 | res.command.accepted = false; 304 | res.command.p2p_traj_duration = p2p_traj_duration_; 305 | 306 | return true; 307 | } 308 | res.command.accepted = true; 309 | 310 | KDL::Vector des_pose = KDL::Vector::Zero(); 311 | KDL::Rotation des_attitude = KDL::Rotation::Identity(); 312 | 313 | // set the desired position requested by the user 314 | des_pose.x(req.command.x); 315 | des_pose.y(req.command.y); 316 | des_pose.z(req.command.z); 317 | 318 | // set the desired attitude requested by the user 319 | des_attitude = KDL::Rotation::RPY(req.command.roll,\ 320 | req.command.pitch,\ 321 | req.command.yaw); 322 | 323 | // set p2p_traj_duration 324 | p2p_traj_duration_ = req.command.p2p_traj_duration; 325 | 326 | // evaluate new trajectory 327 | evaluate_traj_constants(des_pose, des_attitude); 328 | 329 | return true; 330 | 331 | } 332 | 333 | bool CartesianPositionController::set_cmd_gains(lwr_force_position_controllers::CartesianPositionCommandGains::Request &req, \ 334 | lwr_force_position_controllers::CartesianPositionCommandGains::Response &res) 335 | { 336 | kp_ = req.command.kp; 337 | kp_a4_ = req.command.kp_a4; 338 | kp_a5_ = req.command.kp_a5; 339 | kp_a6_ = req.command.kp_a6; 340 | kd_ = req.command.kd; 341 | kd_a4_ = req.command.kd_a4; 342 | kd_a5_ = req.command.kd_a5; 343 | kd_a6_ = req.command.kd_a6; 344 | return true; 345 | } 346 | 347 | bool CartesianPositionController::get_cmd_traj(lwr_force_position_controllers::CartesianPositionCommandTraj::Request &req,\ 348 | lwr_force_position_controllers::CartesianPositionCommandTraj::Response &res) 349 | { 350 | KDL::Frame ee_fk_frame; 351 | double yaw, pitch, roll; 352 | KDL::JntArray q; 353 | 354 | q.resize(kdl_chain_.getNrOfJoints()); 355 | for(size_t i=0; iJntToCart(q, ee_fk_frame); 359 | ee_fk_frame.M.GetRPY(roll, pitch, yaw); 360 | 361 | // get desired position 362 | res.command.x = ee_fk_frame.p.x(); 363 | res.command.y = ee_fk_frame.p.y(); 364 | res.command.z = ee_fk_frame.p.z(); 365 | 366 | // get desired attitude 367 | res.command.yaw = yaw; 368 | res.command.pitch = pitch; 369 | res.command.roll = roll; 370 | 371 | // get p2p_traj_duration 372 | res.command.p2p_traj_duration = p2p_traj_duration_; 373 | 374 | // get remaining time 375 | res.command.elapsed_time = time_; 376 | 377 | return true; 378 | } 379 | 380 | bool CartesianPositionController::get_cmd_gains(lwr_force_position_controllers::CartesianPositionCommandGains::Request &req,\ 381 | lwr_force_position_controllers::CartesianPositionCommandGains::Response &res) 382 | { 383 | // get desired gain 384 | res.command.kp = kp_; 385 | res.command.kp_a4 = kp_a4_; 386 | res.command.kp_a5 = kp_a5_; 387 | res.command.kp_a6 = kp_a6_; 388 | 389 | res.command.kd = kd_; 390 | res.command.kd_a4 = kd_a4_; 391 | res.command.kd_a5 = kd_a5_; 392 | res.command.kd_a6 = kd_a6_; 393 | 394 | return true; 395 | } 396 | 397 | void CartesianPositionController::evaluate_traj_constants(KDL::Vector& des_pose,\ 398 | KDL::Rotation& des_attitude) 399 | { 400 | // evaluate the new desired configuration q_des 401 | 402 | KDL::Frame x_des; 403 | KDL::JntArray q_des; 404 | q_des.resize(kdl_chain_.getNrOfJoints()); 405 | 406 | // get the current robot configuration 407 | for(size_t i=0; iCartToJnt(joint_msr_states_.q, x_des, q_des); 415 | 416 | // normalize q_des between - M_PI and M_PI 417 | for(int i=0; i= joint_limits_.max(i))) 424 | { 425 | std::cout<< "[cart_pos]Sorry the configuration found exceeds joint limits! Try again." <= p2p_traj_duration_) 450 | time_ = p2p_traj_duration_; 451 | 452 | for(int i=0; i p_wrist_ee; 473 | n.getParam("p_wrist_ee", p_wrist_ee); 474 | p_wrist_ee_ = KDL::Vector(p_wrist_ee.at(0), p_wrist_ee.at(1), p_wrist_ee.at(2)); 475 | 476 | // Extend the default chain with a fake segment in order to evaluate 477 | // Jacobians and forward kinematics with respect to a given reference point 478 | // (typicallly the tool tip) 479 | // also takes into account end effector mass and inertia 480 | KDL::Joint fake_joint = KDL::Joint(); 481 | KDL::Frame frame(KDL::Rotation::Identity(), p_wrist_ee_); 482 | 483 | double total_mass; 484 | double fake_cylinder_radius = 0.0475; 485 | double fake_cylinder_height = 0.31; 486 | KDL::Vector p_sensor_tool_com; 487 | load_calib_data(total_mass, p_sensor_tool_com); 488 | 489 | double fake_cyl_i_xx = 1.0 / 12.0 * total_mass * (3 * pow(fake_cylinder_radius, 2) + \ 490 | pow(fake_cylinder_height, 2)); 491 | double fake_cyl_i_zz = 1.0 / 2.0 * total_mass * pow(fake_cylinder_radius, 2); 492 | 493 | KDL::RotationalInertia rot_inertia(fake_cyl_i_xx, fake_cyl_i_xx, fake_cyl_i_zz); 494 | KDL::RigidBodyInertia inertia(total_mass, p_sensor_tool_com, rot_inertia); 495 | 496 | KDL::Segment fake_segment(fake_joint, frame, inertia); 497 | extended_chain_ = kdl_chain_; 498 | extended_chain_.addSegment(fake_segment); 499 | } 500 | 501 | void CartesianPositionController::print_joint_array(KDL::JntArray& array) 502 | { 503 | std::cout << std::fixed; 504 | std::cout << std::setprecision(3); 505 | 506 | std::cout<<"joint \t q_des \t q_min\t q_max \t"< 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #define G_FORCE 9.80665 15 | 16 | namespace lwr_controllers { 17 | 18 | FtSensorCalibController::FtSensorCalibController() {} 19 | FtSensorCalibController::~FtSensorCalibController() {} 20 | 21 | bool FtSensorCalibController::init(hardware_interface::JointStateInterface *robot, ros::NodeHandle& nh) 22 | { 23 | KinematicChainControllerBase::init(robot, nh); 24 | 25 | // subscribe to raw ft sensor topic 26 | std::string ft_sensor_topic_name; 27 | nh.getParam("topic_name", ft_sensor_topic_name); 28 | sub_ft_sensor_ = nh.subscribe(ft_sensor_topic_name, 29 | 1, 30 | &FtSensorCalibController::ft_raw_topic_callback, 31 | this); 32 | 33 | // get the calibration loop rate 34 | nh_.getParam("calibration_loop_rate", calibration_loop_rate_); 35 | 36 | // get publish rate from rosparam 37 | nh.getParam("publish_rate", publish_rate_); 38 | 39 | // get the trajectory duration from rosparam 40 | nh.getParam("p2p_traj_duration", p2p_traj_duration_); 41 | 42 | // reset ik solver 43 | ik_solver_.reset(new KDL::ChainIkSolverPos_LMA(kdl_chain_)); 44 | 45 | // reset fk solver 46 | fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_)); 47 | 48 | // reset ft_calib 49 | ft_calib_.reset(new Calibration::FTCalib()); 50 | 51 | // publisher to joint trajectory controller 52 | pub_joint_traj_ctl_ = nh.advertise("/lwr/joint_trajectory_controller/command", 1); 53 | 54 | // advertise services 55 | move_next_calib_pose_service_ = nh.advertiseService("move_next_calib_pose",\ 56 | &FtSensorCalibController::move_next_calib_pose, this); 57 | move_home_pose_service_ = nh.advertiseService("move_home_pose",\ 58 | &FtSensorCalibController::move_home_pose, this); 59 | save_calib_data_service_ = nh.advertiseService("save_calib_data",\ 60 | &FtSensorCalibController::save_calib_data, this); 61 | start_compensation_service_ = nh.advertiseService("start_compensation",\ 62 | &FtSensorCalibController::start_compensation, this); 63 | do_estimation_step_service_ = nh.advertiseService("do_estimation_step",\ 64 | &FtSensorCalibController::do_estimation_step, this); 65 | start_autonomus_estimation_service_ = nh.advertiseService("start_autonomus_estimation",\ 66 | &FtSensorCalibController::start_autonomus_estimation, this); 67 | 68 | // advertise topic 69 | pub_ft_sensor_no_offset_ = nh.advertise("ft_sensor_no_offset", 1); 70 | pub_ft_sensor_no_gravity_ = nh.advertise("ft_sensor_no_gravity", 1); 71 | 72 | // load calibration poses 73 | //get_calibration_poses(nh); 74 | get_calibration_q(nh); 75 | 76 | // reset pose counter 77 | pose_counter_ = 0; 78 | 79 | // check if existing calibration data should be used 80 | bool recover; 81 | nh_.getParam("recover_existing_data", recover); 82 | if (recover) 83 | recover_existing_data(); 84 | 85 | return true; 86 | } 87 | 88 | void FtSensorCalibController::starting(const ros::Time& time) 89 | { 90 | // initialize time 91 | last_publish_time_ = time; 92 | } 93 | 94 | void FtSensorCalibController::update(const ros::Time& time, const ros::Duration& period) 95 | { 96 | if(!do_compensation_) 97 | return; 98 | 99 | // do offset compensation 100 | KDL::Wrench ft_wrench_no_offset; 101 | ft_wrench_no_offset = ft_wrench_raw_ - offset_kdl_; 102 | 103 | //get the current robot configuration 104 | for(size_t i=0; iJntToCart(joint_msr_states_.q, fk_frame); 111 | R_ft_base = fk_frame.M.Inverse(); 112 | 113 | // compesante for tool weight 114 | // move the reference point of the weight from the com of the tool to the wrist 115 | KDL::Frame gravity_transformation(fk_frame.M.Inverse(), \ 116 | p_sensor_tool_com_kdl_); 117 | // compensate for the weight of the tool 118 | KDL::Wrench ft_wrench_no_gravity; 119 | ft_wrench_no_gravity = ft_wrench_no_offset - gravity_transformation * base_tool_weight_com_; 120 | 121 | if(time > last_publish_time_ + ros::Duration(1.0 / publish_rate_)) 122 | { 123 | //update next tick 124 | last_publish_time_ += ros::Duration(1.0 / publish_rate_); 125 | 126 | publish_data(ft_wrench_no_offset, pub_ft_sensor_no_offset_); 127 | publish_data(ft_wrench_no_gravity, pub_ft_sensor_no_gravity_); 128 | } 129 | } 130 | 131 | void FtSensorCalibController::ft_raw_topic_callback(const geometry_msgs::WrenchStamped::ConstPtr& msg) 132 | { 133 | tf::wrenchMsgToKDL(msg->wrench, ft_wrench_raw_); 134 | } 135 | 136 | // void FtSensorCalibController::get_calibration_poses(ros::NodeHandle nh) 137 | // { 138 | // nh.getParam("calib_number_q", number_of_poses_); 139 | // for (int i=0; i pose; 142 | // nh.getParam("calib_poses/pose" + std::to_string(i), pose); 143 | // ft_calib_poses_.push_back(KDL::Frame(KDL::Rotation::EulerZYX(pose.at(3), pose.at(4), pose.at(5)), 144 | // KDL::Vector(pose.at(0), pose.at(1), pose.at(2)))); 145 | // } 146 | // } 147 | 148 | void FtSensorCalibController::get_calibration_q(ros::NodeHandle nh) 149 | { 150 | nh.getParam("calib_number_q", number_of_poses_); 151 | for (int i=0; i q; 154 | nh.getParam("calib_q/q" + std::to_string(i), q); 155 | ft_calib_q_.push_back(q); 156 | } 157 | nh.getParam("home_q", ft_calib_q_home_); 158 | } 159 | 160 | void FtSensorCalibController::send_joint_trajectory_msg(std::vector q_des) 161 | { 162 | trajectory_msgs::JointTrajectory traj_msg; 163 | trajectory_msgs::JointTrajectoryPoint point; 164 | traj_msg.joint_names.push_back("lwr_a1_joint"); 165 | traj_msg.joint_names.push_back("lwr_a2_joint"); 166 | traj_msg.joint_names.push_back("lwr_e1_joint"); 167 | traj_msg.joint_names.push_back("lwr_a3_joint"); 168 | traj_msg.joint_names.push_back("lwr_a4_joint"); 169 | traj_msg.joint_names.push_back("lwr_a5_joint"); 170 | traj_msg.joint_names.push_back("lwr_a6_joint"); 171 | for (int i = 0; i < kdl_chain_.getNrOfJoints(); ++i) 172 | point.positions.push_back(q_des.at(i)); 173 | point.time_from_start = ros::Duration(p2p_traj_duration_); 174 | traj_msg.points.push_back(point); 175 | pub_joint_traj_ctl_.publish(traj_msg); 176 | } 177 | 178 | bool FtSensorCalibController::move_home_pose(std_srvs::Empty::Request& req,\ 179 | std_srvs::Empty::Response& res) 180 | { 181 | send_joint_trajectory_msg(ft_calib_q_home_); 182 | return true; 183 | } 184 | 185 | bool FtSensorCalibController::move_next_calib_pose(std_srvs::Empty::Request& req,\ 186 | std_srvs::Empty::Response& res) 187 | { 188 | if (number_of_poses_ <= pose_counter_) 189 | { 190 | std::cout << "Manual pose " << pose_counter_ << std::endl; 191 | return true; 192 | } 193 | 194 | std::cout << "Sending pose " << pose_counter_ << " to joint_trajectory_controller" << std::endl; 195 | 196 | send_joint_trajectory_msg(ft_calib_q_.at(pose_counter_)); 197 | 198 | return true; 199 | } 200 | 201 | void FtSensorCalibController::add_measurement(KDL::Vector gravity_ft, KDL::Wrench ft_raw_avg) 202 | { 203 | // transform data so that it can be used in the FTCalib class 204 | std::string frame_id = "ft_frame"; 205 | 206 | geometry_msgs::Vector3Stamped gravity_msg; 207 | geometry_msgs::WrenchStamped ft_avg_msg; 208 | 209 | gravity_msg.header.frame_id = frame_id; 210 | gravity_msg.vector.x = gravity_ft.x(); 211 | gravity_msg.vector.y = gravity_ft.y(); 212 | gravity_msg.vector.z = gravity_ft.z(); 213 | 214 | ft_avg_msg.header.frame_id = frame_id; 215 | ft_avg_msg.wrench.force.x = ft_raw_avg.force.x(); 216 | ft_avg_msg.wrench.force.y = ft_raw_avg.force.y(); 217 | ft_avg_msg.wrench.force.z = ft_raw_avg.force.z(); 218 | ft_avg_msg.wrench.torque.x = ft_raw_avg.torque.x(); 219 | ft_avg_msg.wrench.torque.y = ft_raw_avg.torque.y(); 220 | ft_avg_msg.wrench.torque.z = ft_raw_avg.torque.z(); 221 | 222 | // add measurement 223 | ft_calib_->addMeasurement(gravity_msg, ft_avg_msg); 224 | } 225 | 226 | bool FtSensorCalibController::start_autonomus_estimation(std_srvs::Empty::Request& req,\ 227 | std_srvs::Empty::Response& res) 228 | { 229 | ros::Duration wait = ros::Duration(p2p_traj_duration_ + 2); 230 | for(int i = 0; igetCalib(); 250 | 251 | tool_mass_ = ft_calib(0); 252 | 253 | p_sensor_tool_com_(0) = ft_calib(1) / tool_mass_; 254 | p_sensor_tool_com_(1) = ft_calib(2) / tool_mass_; 255 | p_sensor_tool_com_(2) = ft_calib(3) / tool_mass_; 256 | 257 | ft_offset_force_(0) = -ft_calib(4); 258 | ft_offset_force_(1) = -ft_calib(5); 259 | ft_offset_force_(2) = -ft_calib(6); 260 | 261 | ft_offset_torque_(0) = -ft_calib(7); 262 | ft_offset_torque_(1) = -ft_calib(8); 263 | ft_offset_torque_(2) = -ft_calib(9); 264 | 265 | // print the current estimation 266 | std::cout << "-------------------------------------------------------------" << std::endl; 267 | std::cout << "Current calibration estimate:" << std::endl; 268 | std::cout << std::endl << std::endl; 269 | 270 | std::cout << "Mass: " << tool_mass_ << std::endl << std::endl; 271 | 272 | std::cout << "Tool CoM (in ft sensor frame):" << std::endl; 273 | std::cout << "[" << p_sensor_tool_com_(0) << ", " << p_sensor_tool_com_(1) << ", " << p_sensor_tool_com_(2) << "]"; 274 | std::cout << std::endl << std::endl; 275 | 276 | std::cout << "FT offset: " << std::endl; 277 | std::cout << "[" << ft_offset_force_(0) << ", " << ft_offset_force_(1) << ", " << ft_offset_force_(2) << ", "; 278 | std::cout << ft_offset_torque_(0) << ", " << ft_offset_torque_(1) << ", " << ft_offset_torque_(2) << "]"; 279 | std::cout << std::endl << std::endl; 280 | std::cout << "-------------------------------------------------------------" << std::endl << std::endl << std::endl; 281 | 282 | return true; 283 | } 284 | 285 | void FtSensorCalibController::estimation_step() 286 | { 287 | ros::Rate loop_rate(calibration_loop_rate_); 288 | int number_measurements = 100; 289 | KDL::Wrench ft_raw_avg; 290 | 291 | // average 101 measurements from the ft sensor 292 | ft_raw_avg = ft_wrench_raw_; 293 | for (int i=0; iJntToCart(joint_msr_states_.q, fk_frame); 310 | R_ft_base = fk_frame.M.Inverse(); 311 | 312 | // evaluate the gravity as if it was measured by an IMU 313 | // whose reference frame is aligned with vito_anchor 314 | KDL::Vector gravity(0, 0, -G_FORCE); 315 | 316 | // rotate the gravity in ft frame 317 | KDL::Vector gravity_ft; 318 | gravity_ft = R_ft_base * gravity; 319 | 320 | geometry_msgs::Vector3Stamped gravity_msg; 321 | geometry_msgs::WrenchStamped ft_avg_msg; 322 | 323 | // add measurement to the filter 324 | add_measurement(gravity_ft, ft_raw_avg); 325 | 326 | // save data to allow data recovery if needed 327 | save_calib_meas(gravity_ft, ft_raw_avg, pose_counter_, joint_msr_states_.q); 328 | 329 | } 330 | 331 | bool FtSensorCalibController::do_estimation_step(std_srvs::Empty::Request& req,\ 332 | std_srvs::Empty::Response& res) 333 | { 334 | // do a step of the estimation proces 335 | estimation_step(); 336 | 337 | // update pose_counter 338 | pose_counter_++; 339 | 340 | // get current estimation 341 | Eigen::VectorXd ft_calib = ft_calib_->getCalib(); 342 | 343 | tool_mass_ = ft_calib(0); 344 | 345 | p_sensor_tool_com_(0) = ft_calib(1) / tool_mass_; 346 | p_sensor_tool_com_(1) = ft_calib(2) / tool_mass_; 347 | p_sensor_tool_com_(2) = ft_calib(3) / tool_mass_; 348 | 349 | ft_offset_force_(0) = -ft_calib(4); 350 | ft_offset_force_(1) = -ft_calib(5); 351 | ft_offset_force_(2) = -ft_calib(6); 352 | 353 | ft_offset_torque_(0) = -ft_calib(7); 354 | ft_offset_torque_(1) = -ft_calib(8); 355 | ft_offset_torque_(2) = -ft_calib(9); 356 | 357 | // print the current estimation 358 | std::cout << "-------------------------------------------------------------" << std::endl; 359 | std::cout << "Current calibration estimate:" << std::endl; 360 | std::cout << std::endl << std::endl; 361 | 362 | std::cout << "Mass: " << tool_mass_ << std::endl << std::endl; 363 | 364 | std::cout << "Tool CoM (in ft sensor frame):" << std::endl; 365 | std::cout << "[" << p_sensor_tool_com_(0) << ", " << p_sensor_tool_com_(1) << ", " << p_sensor_tool_com_(2) << "]"; 366 | std::cout << std::endl << std::endl; 367 | 368 | std::cout << "FT offset: " << std::endl; 369 | std::cout << "[" << ft_offset_force_(0) << ", " << ft_offset_force_(1) << ", " << ft_offset_force_(2) << ", "; 370 | std::cout << ft_offset_torque_(0) << ", " << ft_offset_torque_(1) << ", " << ft_offset_torque_(2) << "]"; 371 | std::cout << std::endl << std::endl; 372 | std::cout << "-------------------------------------------------------------" << std::endl << std::endl << std::endl; 373 | 374 | return true; 375 | } 376 | 377 | void FtSensorCalibController::recover_existing_data() 378 | { 379 | std::string file_name = ros::package::getPath("lwr_force_position_controllers") +\ 380 | "/config/ft_calib_meas.yaml"; 381 | YAML::Node ft_data_yaml = YAML::LoadFile(file_name); 382 | 383 | int number = ft_data_yaml["calib_meas_number"].as(); 384 | 385 | KDL::Vector gravity_ft; 386 | KDL::Wrench ft_wrench_avg; 387 | std::vector gravity_vec(3); 388 | std::vector ft_force_avg(3); 389 | std::vector ft_torque_avg(3); 390 | 391 | for (int i=0; i < number; i++) 392 | { 393 | gravity_vec = ft_data_yaml["calib_meas"]["pose" + std::to_string(i)]["gravity"].as>(); 394 | ft_force_avg = ft_data_yaml["calib_meas"]["pose" + std::to_string(i)]["force_avg"].as>(); 395 | ft_torque_avg = ft_data_yaml["calib_meas"]["pose" + std::to_string(i)]["torque_avg"].as>(); 396 | 397 | for (int i=0; i<3; i++) 398 | { 399 | gravity_ft.data[i] = gravity_vec[i]; 400 | ft_wrench_avg.force.data[i] = ft_force_avg[i]; 401 | ft_wrench_avg.torque.data[i] = ft_torque_avg[i]; 402 | } 403 | 404 | add_measurement(gravity_ft, ft_wrench_avg); 405 | pose_counter_++; 406 | } 407 | 408 | std::cout << "Recovered calibration data for " << number << " poses" << std::endl; 409 | } 410 | 411 | void FtSensorCalibController::save_calib_meas(KDL::Vector gravity, KDL::Wrench ft_wrench_avg, int index,\ 412 | KDL::JntArray q_kdl) 413 | { 414 | std::string file_name = ros::package::getPath("lwr_force_position_controllers") + \ 415 | "/config/ft_calib_meas.yaml"; 416 | YAML::Node ft_data_yaml = YAML::LoadFile(file_name); 417 | 418 | std::vector gravity_vec(3); 419 | std::vector ft_force_avg(3); 420 | std::vector ft_torque_avg(3); 421 | std::vector q(7); 422 | for (int i=0; i<3; i++) 423 | { 424 | gravity_vec[i] = gravity.data[i]; 425 | ft_force_avg[i] = ft_wrench_avg.force.data[i]; 426 | ft_torque_avg[i] = ft_wrench_avg.torque.data[i]; 427 | } 428 | for (int i=0; i<7; i++) 429 | q[i] = q_kdl(i); 430 | 431 | ft_data_yaml["calib_meas_number"] = index + 1; 432 | ft_data_yaml["calib_meas"]["pose" + std::to_string(index)]["gravity"] = gravity_vec; 433 | ft_data_yaml["calib_meas"]["pose" + std::to_string(index)]["force_avg"] = ft_force_avg; 434 | ft_data_yaml["calib_meas"]["pose" + std::to_string(index)]["torque_avg"] = ft_torque_avg; 435 | ft_data_yaml["calib_meas"]["pose" + std::to_string(index)]["q"] = q; 436 | 437 | std::ofstream yaml_out(file_name); 438 | yaml_out << ft_data_yaml; 439 | } 440 | 441 | bool FtSensorCalibController::save_calib_data(std_srvs::Empty::Request& req,\ 442 | std_srvs::Empty::Response& res) 443 | { 444 | std::string file_name = ros::package::getPath("lwr_force_position_controllers") +\ 445 | "/config/ft_calib_data.yaml"; 446 | YAML::Node ft_data_yaml = YAML::LoadFile(file_name); 447 | 448 | // convert data to standard types 449 | std::string frame_id = "lwr_7_link"; 450 | std::vector ft_offset(6); 451 | std::vector p_sensor_tool_com(6); 452 | 453 | for (int i=0; i<3; i++) 454 | { 455 | ft_offset[i] = ft_offset_force_(i); 456 | ft_offset[i + 3] = ft_offset_torque_(i); 457 | p_sensor_tool_com[i] = p_sensor_tool_com_(i); 458 | p_sensor_tool_com[i + 3] = 0; 459 | } 460 | 461 | // populate yaml using field names 462 | // required by the package ros-indigo-gravity-compensation 463 | ft_data_yaml["bias"] = ft_offset; 464 | ft_data_yaml["gripper_com_frame_id"] = frame_id; 465 | ft_data_yaml["gripper_com_pose"] = p_sensor_tool_com; 466 | ft_data_yaml["gripper_mass"] = tool_mass_; 467 | 468 | // save yaml file 469 | std::ofstream yaml_out(file_name); 470 | yaml_out << ft_data_yaml; 471 | 472 | return true; 473 | } 474 | 475 | bool FtSensorCalibController::load_calib_data() 476 | { 477 | std::string file_name = ros::package::getPath("lwr_force_position_controllers") +\ 478 | "/config/ft_calib_data.yaml"; 479 | YAML::Node ft_data_yaml = YAML::LoadFile(file_name); 480 | 481 | std::vector p_sensor_tool_com(6); 482 | std::vector offset(6); 483 | 484 | // get data from the yaml file 485 | tool_mass_ = ft_data_yaml["gripper_mass"].as(); 486 | p_sensor_tool_com = ft_data_yaml["gripper_com_pose"].as>(); 487 | offset = ft_data_yaml["bias"].as>(); 488 | 489 | // evaluate end-effector weight 490 | base_tool_weight_com_ = KDL::Wrench(KDL::Vector(0, 0, -tool_mass_ * G_FORCE),\ 491 | KDL::Vector::Zero()); 492 | 493 | // transform to KDL 494 | for (int i=0; i<3; i++) 495 | { 496 | p_sensor_tool_com_kdl_.data[i] = p_sensor_tool_com[i]; 497 | offset_kdl_.force.data[i] = offset[i]; 498 | offset_kdl_.torque.data[i] = offset[i + 3]; 499 | } 500 | 501 | return true; 502 | } 503 | 504 | bool FtSensorCalibController::start_compensation(std_srvs::Empty::Request& req,\ 505 | std_srvs::Empty::Response& res) 506 | { 507 | // load calibration data from yaml file 508 | load_calib_data(); 509 | 510 | // enable offset and gravity compensation 511 | do_compensation_ = true; 512 | 513 | return true; 514 | } 515 | 516 | void FtSensorCalibController::publish_data(KDL::Wrench wrench, ros::Publisher& pub) 517 | { 518 | // create the message 519 | geometry_msgs::WrenchStamped wrench_msg; 520 | wrench_msg.header.stamp = ros::Time::now(); 521 | wrench_msg.wrench.force.x = wrench.force.x(); 522 | wrench_msg.wrench.force.y = wrench.force.y(); 523 | wrench_msg.wrench.force.z = wrench.force.z(); 524 | wrench_msg.wrench.torque.x = wrench.torque.x(); 525 | wrench_msg.wrench.torque.y = wrench.torque.y(); 526 | wrench_msg.wrench.torque.z = wrench.torque.z(); 527 | 528 | // publish the message 529 | pub.publish(wrench_msg); 530 | } 531 | 532 | } // namespace 533 | 534 | PLUGINLIB_EXPORT_CLASS(lwr_controllers::FtSensorCalibController, controller_interface::ControllerBase) 535 | 536 | // bool FtSensorCalibController::move_next_calib_pose(std_srvs::Empty::Request& req,\ 537 | // std_srvs::Empty::Response& res) 538 | // { 539 | // if (number_of_poses_ == pose_counter_) 540 | // return true; 541 | 542 | // KDL::JntArray q_des; 543 | // KDL::Frame x_des; 544 | // KDL::Vector des_pose; 545 | // KDL::Rotation des_attitude; 546 | 547 | // q_des.resize(kdl_chain_.getNrOfJoints()); 548 | 549 | // //get the current robot configuration 550 | // for(size_t i=0; iCartToJnt(joint_msr_states_.q, x_des, q_des); 560 | 561 | // // normalize q_des between - M_PI and M_PI 562 | // for(int i=0; i= joint_limits_.max(i))) 568 | // { 569 | // std::cout<< "[cart_pos]Solution found unfeasible. Solution will bed modified for joint " \ 570 | // << i << std::endl; 571 | // double correction = 10 * M_PI / 180; 572 | // if(q_des(i) <= joint_limits_.min(i)) 573 | // q_des(i) += correction; 574 | // else 575 | // q_des(i) -= correction; 576 | // } 577 | 578 | // ft_calib_q_.push_back(q_des); 579 | 580 | // trajectory_msgs::JointTrajectory traj_msg; 581 | // trajectory_msgs::JointTrajectoryPoint point; 582 | // traj_msg.joint_names.push_back("lwr_a1_joint"); 583 | // traj_msg.joint_names.push_back("lwr_a2_joint"); 584 | // traj_msg.joint_names.push_back("lwr_e1_joint"); 585 | // traj_msg.joint_names.push_back("lwr_a3_joint"); 586 | // traj_msg.joint_names.push_back("lwr_a4_joint"); 587 | // traj_msg.joint_names.push_back("lwr_a5_joint"); 588 | // traj_msg.joint_names.push_back("lwr_a6_joint"); 589 | // for (int i = 0; i < kdl_chain_.getNrOfJoints(); ++i) 590 | // point.positions.push_back(q_des(i)); 591 | // point.time_from_start = ros::Duration(1.0); 592 | // traj_msg.points.push_back(point); 593 | // pub_joint_traj_ctl_.publish(traj_msg); 594 | 595 | // pose_counter_++; 596 | 597 | // return true; 598 | // } 599 | 600 | // bool FtSensorCalibController::save_ft_data(std_srvs::Empty::Request& req,\ 601 | // std_srvs::Empty::Response& res) 602 | // { 603 | // std::string file_name = ros::package::getPath("lwr_force_position_controllers") +\ 604 | // "/config/ft_calib_data.yaml"; 605 | 606 | // YAML::Node ft_data_yaml = YAML::LoadFile(file_name); 607 | 608 | // Eigen::VectorXd q; 609 | // std::vector q_std; 610 | // for(int i=0; i 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #define DEFAULT_KP_POS 1200 8 | #define DEFAULT_KP_ATT 1200 9 | #define DEFAULT_KP_GAMMA 25000 10 | #define DEFAULT_KD_POS 50 11 | #define DEFAULT_KD_ATT 50 12 | #define DEFAULT_KD_GAMMA 50 13 | #define DEFAULT_KM_F 1 14 | #define DEFAULT_KD_F 25 15 | #define DEFAULT_P2P_TRAJ_DURATION 5.0 16 | #define DEFAULT_FORCE_TRAJ_DURATION 5.0 17 | #define P2P_COEFF_3 10.0 18 | #define P2P_COEFF_4 -15.0 19 | #define P2P_COEFF_5 6.0 20 | #define FORCE_REF_COEFF_3 -2 21 | #define FORCE_REF_COEFF_2 3 22 | 23 | namespace lwr_controllers { 24 | 25 | HybridImpedanceController::HybridImpedanceController() {} 26 | 27 | HybridImpedanceController::~HybridImpedanceController() {} 28 | 29 | bool HybridImpedanceController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n) 30 | { 31 | // get parameters required by 32 | // CartesianInverseDynamicsController::init(robot, n) from rosparam server 33 | get_parameters(n); 34 | 35 | // this should be called *ONLY* after get_parameters 36 | CartesianInverseDynamicsController::init(robot, n); 37 | 38 | // get publish rate from rosparam 39 | n.getParam("publish_rate", publish_rate_); 40 | 41 | // advertise HybridImpedanceCommand service 42 | set_cmd_traj_pos_service_ = n.advertiseService("set_hybrid_traj_pos_cmd", \ 43 | &HybridImpedanceController::set_cmd_traj_pos, this); 44 | set_cmd_traj_force_service_ = n.advertiseService("set_hybrid_traj_force_cmd", \ 45 | &HybridImpedanceController::set_cmd_traj_force, this); 46 | set_cmd_gains_service_ = n.advertiseService("set_hybrid_gains_cmd", \ 47 | &HybridImpedanceController::set_cmd_gains, this); 48 | switch_force_position_z_service_ = n.advertiseService("switch_force_position_z", \ 49 | &HybridImpedanceController::switch_force_position_z, this); 50 | get_cmd_traj_pos_service_ = n.advertiseService("get_hybrid_traj_pos_cmd", \ 51 | &HybridImpedanceController::get_cmd_traj_pos, this); 52 | get_cmd_traj_force_service_ = n.advertiseService("get_hybrid_traj_force_cmd", \ 53 | &HybridImpedanceController::get_cmd_traj_force, this); 54 | get_cmd_gains_service_ = n.advertiseService("get_hybrid_gains_cmd", \ 55 | &HybridImpedanceController::get_cmd_gains, this); 56 | 57 | // instantiate variables 58 | // set default proportional gains 59 | Kp_ = Eigen::Matrix::Identity(); 60 | Kp_.block<3,3>(0,0) = Eigen::Matrix::Identity() * DEFAULT_KP_POS; 61 | Kp_.block<2,2>(3,3) = Eigen::Matrix::Identity() * DEFAULT_KP_ATT; 62 | Kp_(5,5) = DEFAULT_KP_GAMMA; 63 | 64 | // set default derivative gains 65 | Kd_ = Eigen::Matrix::Identity(); 66 | Kd_.block<3,3>(0,0) = Eigen::Matrix::Identity() * DEFAULT_KD_POS; 67 | Kd_.block<2,2>(3,3) = Eigen::Matrix::Identity() * DEFAULT_KD_ATT; 68 | Kd_(5,5) = DEFAULT_KD_GAMMA; 69 | 70 | // set default force gains 71 | km_f_ = DEFAULT_KM_F; 72 | kd_f_ = DEFAULT_KD_F; 73 | 74 | // set trajectory duration 75 | p2p_traj_duration_ = DEFAULT_P2P_TRAJ_DURATION; 76 | force_ref_duration_ = DEFAULT_FORCE_TRAJ_DURATION; 77 | 78 | p2p_trj_const_ = Eigen::MatrixXf(4, 6); 79 | force_ref_const_ = Eigen::VectorXf(3); 80 | 81 | km_f_ = DEFAULT_KM_F; 82 | kd_f_ = DEFAULT_KD_F; 83 | p2p_traj_duration_ = DEFAULT_P2P_TRAJ_DURATION; 84 | force_ref_duration_ = DEFAULT_FORCE_TRAJ_DURATION; 85 | 86 | // advertise several topics 87 | pub_force_ = n.advertise("force_measure", 1000); 88 | pub_force_des_ = n.advertise("force_setpoint", 1000); 89 | pub_state_ = n.advertise("state", 1000); 90 | pub_dstate_ = n.advertise("dstate", 1000); 91 | pub_x_des_ = n.advertise("x_des", 1000); 92 | pub_xdot_des_ = n.advertise("xdot_des", 1000); 93 | pub_xdotdot_des_ = n.advertise("xdotdot_des", 1000); 94 | pub_error_ = n.advertise("error", 1000); 95 | 96 | return true; 97 | } 98 | 99 | void HybridImpedanceController::starting(const ros::Time& time) 100 | { 101 | CartesianInverseDynamicsController::starting(time); 102 | 103 | // initialize publish time 104 | last_publish_time_ = time; 105 | 106 | /////////////////////////////////////////////////////////////// 107 | // 108 | // every time the controller is started the set point is set 109 | // to the current configuration 110 | // 111 | /////////////////////////////////////////////////////////////// 112 | // 113 | 114 | // set default trajectory (force and position) 115 | set_default_pos_traj(); 116 | set_default_force_traj(); 117 | 118 | // set z position control 119 | enable_force_ = false; 120 | 121 | // 122 | /////////////////////////////////////////////////////////////// 123 | } 124 | 125 | void HybridImpedanceController::update(const ros::Time& time, const ros::Duration& period) 126 | { 127 | // evaluate inverse dynamics 128 | CartesianInverseDynamicsController::update(time, period); 129 | 130 | // the super class evaluates the following quantities every update 131 | // 132 | // ws_x_: distance and attitude between the workspace and the tool tip (workspace basis) 133 | // ws_xdot_: derivative of ws_x_ (workspace basis) 134 | // wrench_wrist_: forces and torques with the reference point on the wrist (ee frame basis) 135 | // R_ws_base_: rotation matrix from workspace to vito_anchor 136 | // R_ws_ee: attitude of the end effector w.r.t to the workspace frame 137 | // 138 | 139 | // transform the ft_sensor wrench 140 | // move the reference point from the wrist to the tool tip and project in workspace basis 141 | 142 | KDL::Frame force_transformation(R_ws_ee_, R_ws_ee_ * (-p_sensor_cp_)); 143 | KDL::Wrench ws_F_ee; 144 | ws_F_ee = force_transformation * wrench_wrist_; 145 | 146 | Eigen::VectorXd x_des(6); 147 | Eigen::VectorXd xdot_des(6); 148 | Eigen::VectorXd xdotdot_des(6); 149 | double fz_des; 150 | eval_current_point_to_point_traj(period, x_des, xdot_des, xdotdot_des); 151 | fz_des = eval_force_reference(period); 152 | 153 | // evaluate state error 154 | Eigen::VectorXd err_x = x_des - ws_x_; 155 | 156 | // normalize angular error between - M_PI and M_PI 157 | err_x(3) = angles::normalize_angle(err_x(3)); 158 | err_x(4) = angles::normalize_angle(err_x(4)); 159 | err_x(5) = angles::normalize_angle(err_x(5)); 160 | 161 | ///////////////////////////////////////////////////////////////////// 162 | // 163 | // desired acceleration 164 | // 165 | ///////////////////////////////////////////////////////////////////// 166 | // 167 | 168 | Eigen::VectorXd acc_cmd = Eigen::VectorXd(6); 169 | 170 | // position controlled DoF 171 | // ws_x ws_y R_ee_ws_(alpha, beta, gamma) 172 | acc_cmd = Kp_ * err_x + Kd_ * (xdot_des - ws_xdot_) + xdotdot_des; 173 | // acc_cmd(0) = acc_cmd(0) - ws_F_ee.force.x(); 174 | // acc_cmd(1) = acc_cmd(1) - ws_F_ee.force.y(); 175 | // acc_cmd(0) = acc_cmd(0); 176 | // acc_cmd(1) = acc_cmd(1); 177 | acc_cmd(3) = acc_cmd(3);// - ws_F_ee.torque.x(); 178 | acc_cmd(4) = acc_cmd(4);// - ws_F_ee.torque.y(); 179 | acc_cmd(5) = acc_cmd(5);// - ws_F_ee.torque.z(); 180 | 181 | // force controlled DoF 182 | // ws_Fz 183 | double err_force; 184 | if (enable_force_) 185 | { 186 | err_force = fz_des - ws_F_ee.force.z(); 187 | acc_cmd(2) = - kd_f_ * ws_xdot_(2) + km_f_ * err_force; 188 | } 189 | 190 | // 191 | ///////////////////////////////////////////////////////////////////// 192 | 193 | // call super class method set_command 194 | set_command(acc_cmd); 195 | 196 | if(time > last_publish_time_ + ros::Duration(1.0 / publish_rate_)) 197 | { 198 | //update next tick 199 | last_publish_time_ += ros::Duration(1.0 / publish_rate_); 200 | 201 | // publish data 202 | publish_data(pub_state_, ws_x_); 203 | publish_data(pub_dstate_, ws_xdot_); 204 | publish_data(pub_x_des_, x_des); 205 | publish_data(pub_xdot_des_, xdot_des); 206 | publish_data(pub_xdotdot_des_, xdotdot_des); 207 | publish_data(pub_force_, ws_F_ee); 208 | publish_data(pub_force_des_, KDL::Wrench(KDL::Vector(0, 0, fz_des),\ 209 | KDL::Vector(0, 0, 0))); 210 | 211 | if(enable_force_) 212 | { 213 | Eigen::VectorXd errors; 214 | errors = err_x; 215 | errors(2) = err_force; 216 | publish_data(pub_error_, errors); 217 | } 218 | else 219 | { 220 | Eigen::VectorXd errors; 221 | errors = err_x; 222 | publish_data(pub_error_, errors); 223 | } 224 | } 225 | } 226 | 227 | void HybridImpedanceController::set_default_pos_traj() 228 | { 229 | // get current robot joints configuration q 230 | KDL::JntArray q; 231 | q.resize(kdl_chain_.getNrOfJoints()); 232 | for(size_t i=0; iJntToCart(q, ee_fk_frame); 238 | 239 | // evaluate current cartesian configuration 240 | KDL::Rotation R_ws_ee; 241 | KDL::Vector p_ws_ee; 242 | double alpha, beta, gamma; 243 | R_ws_ee = R_ws_base_ * ee_fk_frame.M; 244 | R_ws_ee.GetEulerZYZ(alpha, beta, gamma); 245 | p_ws_ee = R_ws_base_ * (ee_fk_frame.p - p_base_ws_); 246 | 247 | // set position and attitude tajectory constants 248 | for(int i=0; i<6; i++) 249 | { 250 | p2p_trj_const_(1, i) = 0; 251 | p2p_trj_const_(2, i) = 0; 252 | p2p_trj_const_(3, i) = 0; 253 | } 254 | 255 | for(int i=0; i<3; i++) 256 | p2p_trj_const_(0, i) = p_ws_ee.data[i]; 257 | p2p_trj_const_(0, 3) = alpha; 258 | p2p_trj_const_(0, 4) = beta; 259 | p2p_trj_const_(0, 5) = gamma; 260 | prev_pos_setpoint_ << p_ws_ee.x(), p_ws_ee.y(), p_ws_ee.z(); 261 | prev_att_setpoint_ << alpha, beta, gamma; 262 | 263 | // reset the time 264 | time_ = p2p_traj_duration_; 265 | 266 | p2p_traj_mutex_.unlock(); 267 | 268 | } 269 | 270 | void HybridImpedanceController::set_default_force_traj() 271 | { 272 | 273 | // set force trajectory constants 274 | for(int i = 0; i<3; i++) 275 | force_ref_const_(i) = 0; 276 | force_ref_const_(0) = -0.35; 277 | prev_fz_setpoint_ = -0.35; 278 | 279 | // reset the time 280 | time_force_ = force_ref_duration_; 281 | } 282 | 283 | void HybridImpedanceController::get_parameters(ros::NodeHandle &n) 284 | { 285 | // vector from the wrist to the tool tip (projected in lwr_link_7 frame) 286 | std::vector p_wrist_ee; 287 | 288 | // vector from the sensor to the contact point 289 | std::vector p_sensor_cp; 290 | 291 | // vector from vito_anchor to the workspace (projected in world frame) 292 | std::vector p_base_ws; 293 | 294 | // attitude of the workspace frame w.r.t. vito_anchor frame 295 | std::vector ws_base_angles; 296 | 297 | // name of the ft_sensor topic 298 | std::string ft_sensor_topic_name; 299 | 300 | // get parameters form rosparameter server 301 | n.getParam("p_wrist_ee", p_wrist_ee); 302 | n.getParam("p_sensor_cp", p_sensor_cp); 303 | n.getParam("p_base_ws", p_base_ws); 304 | n.getParam("ws_base_angles", ws_base_angles); 305 | n.getParam("ft_sensor_topic_name", ft_sensor_topic_name); 306 | 307 | // set internal members 308 | set_p_wrist_ee(p_wrist_ee.at(0), p_wrist_ee.at(1), p_wrist_ee.at(2)); 309 | set_p_sensor_cp(p_sensor_cp.at(0), p_sensor_cp.at(1), p_sensor_cp.at(2)); 310 | set_p_base_ws(p_base_ws.at(0), p_base_ws.at(1), p_base_ws.at(2)); 311 | set_ws_base_angles(ws_base_angles.at(0), ws_base_angles.at(1), ws_base_angles.at(2)); 312 | set_ft_sensor_topic_name(ft_sensor_topic_name); 313 | } 314 | 315 | void HybridImpedanceController::set_p_sensor_cp(double x, double y, double z) 316 | { 317 | p_sensor_cp_ = KDL::Vector(x, y, z); 318 | } 319 | 320 | bool HybridImpedanceController::set_cmd_traj_pos(lwr_force_position_controllers::HybridImpedanceCommandTrajPos::Request &req, \ 321 | lwr_force_position_controllers::HybridImpedanceCommandTrajPos::Response &res) 322 | { 323 | if (time_ < p2p_traj_duration_) 324 | { 325 | res.command.elapsed_time = time_; 326 | res.command.accepted = false; 327 | res.command.p2p_traj_duration = p2p_traj_duration_; 328 | 329 | return true; 330 | } 331 | res.command.accepted = true; 332 | 333 | Eigen::Vector3d desired_position; 334 | Eigen::Vector3d desired_attitude; 335 | 336 | // set requested position and attitude 337 | desired_position(0) = req.command.x; 338 | desired_position(1) = req.command.y; 339 | desired_position(2) = req.command.z; 340 | desired_attitude(0) = req.command.alpha; 341 | desired_attitude(1) = req.command.beta; 342 | desired_attitude(2) = req.command.gamma; 343 | 344 | p2p_traj_mutex_.lock(); 345 | 346 | p2p_traj_duration_ = req.command.p2p_traj_duration; 347 | eval_point_to_point_traj_constants(desired_position, desired_attitude,\ 348 | p2p_traj_duration_); 349 | time_ = 0; 350 | 351 | p2p_traj_mutex_.unlock(); 352 | 353 | return true; 354 | } 355 | 356 | bool HybridImpedanceController::set_cmd_traj_force(lwr_force_position_controllers::HybridImpedanceCommandTrajForce::Request &req, \ 357 | lwr_force_position_controllers::HybridImpedanceCommandTrajForce::Response &res) 358 | { 359 | if (time_force_ < force_ref_duration_) 360 | { 361 | res.command.elapsed_time = time_force_; 362 | res.command.accepted = false; 363 | res.command.force_ref_duration = force_ref_duration_; 364 | 365 | return true; 366 | } 367 | res.command.accepted = true; 368 | 369 | force_traj_mutex_.lock(); 370 | 371 | force_ref_duration_ = req.command.force_ref_duration; 372 | evaluate_force_reference_constants(req.command.forcez, force_ref_duration_); 373 | time_force_ = 0; 374 | 375 | force_traj_mutex_.unlock(); 376 | 377 | return true; 378 | } 379 | 380 | bool HybridImpedanceController::switch_force_position_z(lwr_force_position_controllers::HybridImpedanceSwitchForcePos::Request &req, \ 381 | lwr_force_position_controllers::HybridImpedanceSwitchForcePos::Response &res) 382 | { 383 | 384 | p2p_traj_mutex_.lock(); 385 | 386 | set_default_pos_traj(); 387 | 388 | p2p_traj_mutex_.unlock(); 389 | 390 | force_traj_mutex_.lock(); 391 | 392 | set_default_force_traj(); 393 | enable_force_ = req.command.enable_force_z; 394 | 395 | force_traj_mutex_.unlock(); 396 | 397 | return true; 398 | } 399 | 400 | bool HybridImpedanceController::set_cmd_gains(lwr_force_position_controllers::HybridImpedanceCommandGains::Request &req, \ 401 | lwr_force_position_controllers::HybridImpedanceCommandGains::Response &res) 402 | { 403 | // set the desired gains requested by the user 404 | Kp_.block<3,3>(0,0) = Eigen::Matrix::Identity() * req.command.kp_pos; 405 | Kp_.block<2,2>(3,3) = Eigen::Matrix::Identity() * req.command.kp_att; 406 | Kp_(5,5) = req.command.kp_gamma; 407 | 408 | Kd_.block<3,3>(0,0) = Eigen::Matrix::Identity() * req.command.kd_pos; 409 | Kd_.block<2,2>(3,3) = Eigen::Matrix::Identity() * req.command.kd_att; 410 | Kd_(5,5) = req.command.kd_gamma; 411 | 412 | km_f_ = req.command.km_f; 413 | kd_f_ = req.command.kd_f; 414 | 415 | set_gains_im(req.command.kp_z_im, req.command.kp_gamma_im, req.command.kd_pos_im, req.command.kd_att_im); 416 | 417 | return true; 418 | } 419 | 420 | bool HybridImpedanceController::get_cmd_traj_pos(lwr_force_position_controllers::HybridImpedanceCommandTrajPos::Request &req, \ 421 | lwr_force_position_controllers::HybridImpedanceCommandTrajPos::Response &res) 422 | { 423 | // get position 424 | res.command.x = prev_pos_setpoint_(0); 425 | res.command.y = prev_pos_setpoint_(1); 426 | res.command.z = prev_pos_setpoint_(2); 427 | res.command.p2p_traj_duration = p2p_traj_duration_; 428 | 429 | // get attitude 430 | res.command.alpha = prev_att_setpoint_(0); 431 | res.command.beta = prev_att_setpoint_(1); 432 | res.command.gamma = prev_att_setpoint_(2); 433 | 434 | // get elapsed time 435 | res.command.elapsed_time = time_; 436 | 437 | return true; 438 | } 439 | 440 | bool HybridImpedanceController::get_cmd_traj_force(lwr_force_position_controllers::HybridImpedanceCommandTrajForce::Request &req, \ 441 | lwr_force_position_controllers::HybridImpedanceCommandTrajForce::Response &res) 442 | { 443 | // get force 444 | res.command.forcez = prev_fz_setpoint_; 445 | res.command.force_ref_duration = force_ref_duration_; 446 | 447 | // get elapsed time 448 | res.command.elapsed_time = time_force_; 449 | 450 | return true; 451 | } 452 | 453 | bool HybridImpedanceController::get_cmd_gains(lwr_force_position_controllers::HybridImpedanceCommandGains::Request &req, \ 454 | lwr_force_position_controllers::HybridImpedanceCommandGains::Response &res) 455 | { 456 | double kp_z_im, kp_gamma_im, kd_pos_im, kd_att_im; 457 | 458 | // get gains 459 | res.command.kp_pos = Kp_(0, 0); 460 | res.command.kp_att = Kp_(3, 3); 461 | res.command.kp_gamma = Kp_(5, 5); 462 | 463 | res.command.kd_pos = Kd_(0, 0); 464 | res.command.kd_att = Kd_(3, 3); 465 | res.command.kd_gamma = Kd_(5, 5); 466 | 467 | res.command.km_f = km_f_; 468 | res.command.kd_f = kd_f_; 469 | 470 | // get internal motion gains 471 | get_gains_im(kp_z_im, kp_gamma_im, kd_pos_im, kd_att_im); 472 | res.command.kp_z_im = kp_z_im; 473 | res.command.kp_gamma_im = kp_gamma_im; 474 | res.command.kd_pos_im = kd_pos_im; 475 | res.command.kd_att_im = kd_att_im; 476 | 477 | return true; 478 | } 479 | 480 | void HybridImpedanceController::evaluate_force_reference_constants(double force_des, double duration) 481 | { 482 | force_ref_const_(0) = prev_fz_setpoint_; 483 | force_ref_const_(1) = FORCE_REF_COEFF_2 * (force_des - prev_fz_setpoint_) / pow(duration, 2); 484 | force_ref_const_(2) = FORCE_REF_COEFF_3 * (force_des - prev_fz_setpoint_) / pow(duration, 3); 485 | prev_fz_setpoint_ = force_des; 486 | } 487 | 488 | double HybridImpedanceController::eval_force_reference(const ros::Duration& period) 489 | { 490 | force_traj_mutex_.lock(); 491 | 492 | time_force_ += period.toSec(); 493 | if(time_force_ >= force_ref_duration_) 494 | time_force_ = force_ref_duration_; 495 | 496 | double value; 497 | value = force_ref_const_(2) * pow(time_force_, 3) + \ 498 | force_ref_const_(1) * pow(time_force_, 2) + \ 499 | force_ref_const_(0); 500 | 501 | force_traj_mutex_.unlock(); 502 | 503 | return value; 504 | } 505 | 506 | void HybridImpedanceController::eval_current_point_to_point_traj(const ros::Duration& period,\ 507 | Eigen::VectorXd& x_des,\ 508 | Eigen::VectorXd& xdot_des,\ 509 | Eigen::VectorXd& xdotdot_des) 510 | { 511 | p2p_traj_mutex_.lock(); 512 | 513 | time_ += period.toSec(); 514 | 515 | if (time_ > p2p_traj_duration_) 516 | time_ = p2p_traj_duration_; 517 | 518 | for (int i=0; i<6; i++) 519 | { 520 | x_des(i) = p2p_trj_const_(0, i) + p2p_trj_const_(1, i) * pow(time_, 3) + \ 521 | p2p_trj_const_(2, i) * pow(time_, 4) + p2p_trj_const_(3, i) * pow(time_, 5); 522 | 523 | xdot_des(i) = 3 * p2p_trj_const_(1, i) * pow(time_, 2) + \ 524 | 4 * p2p_trj_const_(2, i) * pow(time_, 3) + 5 * p2p_trj_const_(3, i) * pow(time_, 4); 525 | 526 | xdotdot_des(i) = 3 * 2 * p2p_trj_const_(1, i) * time_ + \ 527 | 4 * 3 * p2p_trj_const_(2, i) * pow(time_, 2) + 5 * 4 * p2p_trj_const_(3, i) * pow(time_, 3); 528 | } 529 | 530 | p2p_traj_mutex_.unlock(); 531 | 532 | } 533 | 534 | void HybridImpedanceController::eval_point_to_point_traj_constants(Eigen::Vector3d& desired_position, \ 535 | Eigen::Vector3d& desired_attitude, 536 | double duration) 537 | { 538 | // evaluate common part of constants 539 | double constant_0, constant_1, constant_2; 540 | constant_0 = P2P_COEFF_3 / pow(duration, 3); 541 | constant_1 = P2P_COEFF_4 / pow(duration, 4); 542 | constant_2 = P2P_COEFF_5 / pow(duration, 5); 543 | 544 | // evaluate constants for x and y trajectories 545 | for (int i=0; i<3; i++) 546 | { 547 | double error = desired_position(i) - prev_pos_setpoint_(i); 548 | p2p_trj_const_(0, i) = prev_pos_setpoint_(i); 549 | p2p_trj_const_(1, i) = error * constant_0; 550 | p2p_trj_const_(2, i) = error * constant_1; 551 | p2p_trj_const_(3, i) = error * constant_2; 552 | } 553 | prev_pos_setpoint_ = desired_position; 554 | 555 | // evaluate constants alpha, beta and gamma trajectories 556 | double alpha_cmd, beta_cmd, gamma_cmd; 557 | KDL::Rotation::EulerZYZ(desired_attitude(0), 558 | desired_attitude(1), 559 | desired_attitude(2)).GetEulerZYZ(alpha_cmd,\ 560 | beta_cmd, 561 | gamma_cmd); 562 | Eigen::Vector3d des_attitude_fixed; 563 | des_attitude_fixed << alpha_cmd, beta_cmd, gamma_cmd; 564 | for (int i=0; i<3; i++) 565 | { 566 | 567 | double error = angles::normalize_angle(des_attitude_fixed(i) - prev_att_setpoint_(i)); 568 | p2p_trj_const_(0, i + 3) = prev_att_setpoint_(i); 569 | p2p_trj_const_(1, i + 3) = error * constant_0; 570 | p2p_trj_const_(2, i + 3) = error * constant_1; 571 | p2p_trj_const_(3, i + 3) = error * constant_2; 572 | } 573 | prev_att_setpoint_ = des_attitude_fixed; 574 | 575 | } 576 | 577 | void HybridImpedanceController::publish_data(ros::Publisher& pub, KDL::Wrench wrench) 578 | { 579 | geometry_msgs::WrenchStamped wrench_msg; 580 | wrench_msg.header.stamp = ros::Time::now(); 581 | wrench_msg.wrench.force.x = wrench.force.x(); 582 | wrench_msg.wrench.force.y = wrench.force.y(); 583 | wrench_msg.wrench.force.z = wrench.force.z(); 584 | wrench_msg.wrench.torque.x = wrench.torque.x(); 585 | wrench_msg.wrench.torque.y = wrench.torque.y(); 586 | wrench_msg.wrench.torque.z = wrench.torque.z(); 587 | pub.publish(wrench_msg); 588 | } 589 | 590 | void HybridImpedanceController::publish_data(ros::Publisher& pub, Eigen::VectorXd& vector) 591 | { 592 | geometry_msgs::WrenchStamped msg; 593 | msg.header.stamp = ros::Time::now(); 594 | msg.wrench.force.x = vector(0); 595 | msg.wrench.force.y = vector(1); 596 | msg.wrench.force.z = vector(2); 597 | msg.wrench.torque.x = vector(3); 598 | msg.wrench.torque.y = vector(4); 599 | msg.wrench.torque.z = vector(5); 600 | pub.publish(msg); 601 | } 602 | 603 | } // namespace 604 | 605 | PLUGINLIB_EXPORT_CLASS(lwr_controllers::HybridImpedanceController, controller_interface::ControllerBase) 606 | -------------------------------------------------------------------------------- /srv/CartesianPositionCommandGains.srv: -------------------------------------------------------------------------------- 1 | # requested command 2 | CartesianPositionCommandGainsMsg command 3 | --- 4 | # current command 5 | CartesianPositionCommandGainsMsg command -------------------------------------------------------------------------------- /srv/CartesianPositionCommandTraj.srv: -------------------------------------------------------------------------------- 1 | # requested command 2 | CartesianPositionCommandTrajMsg command 3 | --- 4 | # current command 5 | CartesianPositionCommandTrajMsg command -------------------------------------------------------------------------------- /srv/HybridImpedanceCommandGains.srv: -------------------------------------------------------------------------------- 1 | # requested command 2 | HybridImpedanceCommandGainsMsg command 3 | --- 4 | # current command 5 | HybridImpedanceCommandGainsMsg command -------------------------------------------------------------------------------- /srv/HybridImpedanceCommandTrajForce.srv: -------------------------------------------------------------------------------- 1 | # requested command 2 | HybridImpedanceCommandTrajForceMsg command 3 | --- 4 | # current command 5 | HybridImpedanceCommandTrajForceMsg command -------------------------------------------------------------------------------- /srv/HybridImpedanceCommandTrajPos.srv: -------------------------------------------------------------------------------- 1 | # requested command 2 | HybridImpedanceCommandTrajPosMsg command 3 | --- 4 | # current command 5 | HybridImpedanceCommandTrajPosMsg command -------------------------------------------------------------------------------- /srv/HybridImpedanceSwitchForcePos.srv: -------------------------------------------------------------------------------- 1 | # requested command 2 | HybridImpedanceSwitchForcePosMsg command 3 | --- 4 | # current command 5 | HybridImpedanceSwitchForcePosMsg command 6 | -------------------------------------------------------------------------------- /worlds/simple_environment.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | model://sun 9 | 10 | 11 | 12 | -1.217505 -0.767856 1.134366 0 0.311642 0.876195 13 | 14 | 15 | 16 | 17 | 18 | --------------------------------------------------------------------------------