tjbetter / cartesian-impedance-control

Impedance Controllers for pHRI on KUKA LWR 4+ using Stanford FRI

cartesian-impedance-control

Impedance Controllers for pHRI on KUKA LWR 4+ using Stanford FRI

In Real Mode

In real mode, the controllers are launched in separate computers.

-> On arms computer (FRI - Fast Research Interface need to be run in sudo) sudo -s su source devel/setup.bash roslaunch double_lwr_robot double_arms.launch

-> For left arm only (FRI - Fast Research Interface need to be run in sudo) sudo -s su source devel/setup.bash roslaunch double_lwr_robot double_arms.launch use_right_arm:=false

-> On pantilt computer source devel/setup.bash roslaunch pan_tilt_real pan_tilt_real.launch

You can move/control the robots like in simulation mode, so let's have a look to the simulation part.

In Simulation/Gazebo Mode

How to launch the simulation of Sigma Platform (arms + pantilt) :

source devel/setup.bash roslaunch platform_gazebo platform_gazebo.launch use_pantilt:=true

If you don't need the pantilt -> roslaunch platform_gazebo platform_gazebo.launch use_pantilt:=false

How to move the pantilt in position :

rostopic pub -1 /pantilt/pantilt_group_position_controller/command std_msgs/Float64MultiArray "data: [1.5,-0.5,0.5,0.5]"

How to see the image_raw of each camera :

rosrun image_view image_view image:=/pantilt/camera1/image_raw rosrun image_view image_view image:=/pantilt/camera2/image_raw

How to move the right or left kuka lwr arm :

The namespace 'kuka_lwr_right' is used to control the right arm. The namespace 'kuka_lwr_left' id used to control the left arm.

-> Get a list of ros services (ros controllers) available for the right arm (so need to use the 'kuka_lwr_right' namespace) : rosservice call /kuka_lwr_right/controller_manager/list_controllers

you can do the same for the left arm -> rosservice call /kuka_lwr_left/controller_manager/list_controllers

An example of available controllers (loaded but stopped by default): (of course you can write your own controller !)

controller:

name: joint_state_controller
state: running
type: joint_state_controller/JointStateController
hardware_interface: hardware_interface::JointStateInterface
resources: []

• name: kuka_group_command_controller_fri state: stopped type: kuka_lwr_controllers/GroupCommandControllerFRI hardware_interface: hardware_interface::PositionJointInterface resources: ['kuka_lwr_right_0_joint', 'kuka_lwr_right_1_joint', 'kuka_lwr_right_2_joint', 'kuka_lwr_right_3_joint', 'kuka_lwr_right_4_joint', 'kuka_lwr_right_5_joint', 'kuka_lwr_right_6_joint']
• name: kuka_one_task_inverse_kinematics state: stopped type: kuka_lwr_controllers/OneTaskInverseKinematics hardware_interface: hardware_interface::PositionJointInterface resources: ['kuka_lwr_right_0_joint', 'kuka_lwr_right_1_joint', 'kuka_lwr_right_2_joint', 'kuka_lwr_right_3_joint', 'kuka_lwr_right_4_joint', 'kuka_lwr_right_5_joint', 'kuka_lwr_right_6_joint']
• name: cartesian_velocity_control state: stopped type: kuka_lwr_controllers/CartesianVelocityControl hardware_interface: hardware_interface::PositionJointInterface resources: ['kuka_lwr_right_0_joint', 'kuka_lwr_right_1_joint', 'kuka_lwr_right_2_joint', 'kuka_lwr_right_3_joint', 'kuka_lwr_right_4_joint', 'kuka_lwr_right_5_joint', 'kuka_lwr_right_6_joint']

-> How to Start the position controller 'kuka_group_command_controller_fri' for the 'kuka_lwr_right' arm : rosservice call /kuka_lwr_right/controller_manager/switch_controller "{start_controllers: ['kuka_group_command_controller_fri'], stop_controllers: [], strictness: 2}"

The same for the left arm -> rosservice call /kuka_lwr_left/controller_manager/switch_controller "{start_controllers: ['kuka_group_command_controller_fri'], stop_controllers: [], strictness: 2}"

-> How to Send positions with the controller 'kuka_group_command_controller_fri' for the 'kuka_lwr_right' arm : rostopic pub -1 /kuka_lwr_right/kuka_group_command_controller_fri/command std_msgs/Float64MultiArray "data: [0.9,0.9,0.5,0.5,0.3,0.5,0.8]"

The same for the left arm -> rostopic pub -1 /kuka_lwr_left/kuka_group_command_controller_fri/command std_msgs/Float64MultiArray "data: [0.9,0.9,0.5,0.5,0.3,0.5,0.8]"

-> How to Stop the position controller 'kuka_group_command_controller_fri' for the 'kuka_lwr_right' arm : rosservice call /kuka_lwr_right/controller_manager/switch_controller "{start_controllers: [], stop_controllers: ['kuka_group_command_controller_fri'], strictness: 1}"

-> How to Start 'cartesian inverse kinematic' controller : rosservice call /kuka_lwr_right/controller_manager/switch_controller "{start_controllers: ['kuka_one_task_inverse_kinematics'], stop_controllers: [], strictness: 2}"

-> How to Send cartesian position : rostopic pub -1 /kuka_lwr_right/kuka_one_task_inverse_kinematics/command kuka_lwr_controllers/PoseRPY '{id: 1, position: {x: -0.4, y: 0.3, z: 0.9}}'

-> How to Stop 'cartesian inverse kinematic' controller : rosservice call /kuka_lwr_right/controller_manager/switch_controller "{start_controllers: [], stop_controllers: ['kuka_one_task_inverse_kinematics'], strictness: 1}"

-> How to start 'kuka_gravity_compensation_controller' controller : rosservice call /kuka_lwr_right/controller_manager/switch_controller "{start_controllers: ['kuka_gravity_compensation_controller'], stop_controllers: [], strictness: 2}"

-> How to stop 'kuka_gravity_compensation_controller' controller : rosservice call /kuka_lwr_right/controller_manager/switch_controller "{start_controllers: [], stop_controllers: ['kuka_gravity_compensation_controller'], strictness: 2}"

-> How to start 'torque_based_position_controller' controller : rosservice call /kuka_lwr_left/controller_manager/switch_controller "{start_controllers: ['torque_based_position_controller'], stop_controllers: [], strictness: 2}"

-> How to stop 'torque_based_position_controller' controller : rosservice call /kuka_lwr_left/controller_manager/switch_controller "{start_controllers: [], stop_controllers: ['torque_based_position_controller'], strictness: 2}"

RVIZ only

roslaunch platform_rviz platform_rviz.launch

Cartesian computed torque controller

In simulation Mode ->

-> Launch ros controllers and gazebo roslaunch platform_gazebo platform_gazebo.launch

In real Mode -> left arm only -> roslaunch double_lwr_robot double_arms.launch use_right_arm:=false

right arm only -> roslaunch double_lwr_robot double_arms.launch use_left_arm:=false

-> Go to a specific position to avoid singularity : rosservice call /kuka_lwr_right/controller_manager/switch_controller "{start_controllers: ['kuka_group_command_controller_fri'], stop_controllers: [], strictness: 2}" rosservice call /kuka_lwr_left/controller_manager/switch_controller "{start_controllers: ['kuka_group_command_controller_fri'], stop_controllers: [], strictness: 2}"

rostopic pub -1 /kuka_lwr_right/kuka_group_command_controller_fri/command std_msgs/Float64MultiArray "data: [0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0]" rostopic pub -1 /kuka_lwr_left/kuka_group_command_controller_fri/command std_msgs/Float64MultiArray "data: [0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0]"

rosservice call /kuka_lwr_right/controller_manager/switch_controller "{start_controllers: [], stop_controllers: ['kuka_group_command_controller_fri'], strictness: 2}" rosservice call /kuka_lwr_left/controller_manager/switch_controller "{start_controllers: [], stop_controllers: ['kuka_group_command_controller_fri'], strictness: 2}"

-> You can do the same actions with a specific bash script (launch from the workspace root) : ./src/platform_gazebo/start_specific_position.sh right "[0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0]" ./src/platform_gazebo/start_specific_position.sh left "[0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0]"

-> Start/Stop 'cartesian_computed_torque_controller' : rosservice call /kuka_lwr_right/controller_manager/switch_controller "{start_controllers: ['cartesian_computed_torque_controller'], stop_controllers: [], strictness: 2}" rosservice call /kuka_lwr_left/controller_manager/switch_controller "{start_controllers: ['cartesian_computed_torque_controller'], stop_controllers: [], strictness: 2}"

rosservice call /kuka_lwr_right/controller_manager/switch_controller "{start_controllers: [], stop_controllers: ['cartesian_computed_torque_controller'], strictness: 2}" rosservice call /kuka_lwr_left/controller_manager/switch_controller "{start_controllers: [], stop_controllers: ['cartesian_computed_torque_controller'], strictness: 2}"

-> Publish Pose : rostopic pub -1 /kuka_lwr_right/cartesian_computed_torque_controller/command kuka_lwr_controllers/PoseRPY '{id: 1, position: {x: -0.5, y: 0.0, z: 0.9}}' rostopic pub -1 /kuka_lwr_left/cartesian_computed_torque_controller/command kuka_lwr_controllers/PoseRPY '{id: 1, position: {x: -0.5, y: 0.0, z: 0.9}}'

rostopic pub -1 /kuka_lwr_right/cartesian_computed_torque_controller/command kuka_lwr_controllers/PoseRPY '{id: 0, position: {x: -0.5, y: 0.0, z: 0.9}, orientation: {roll: 0.0, pitch: 0.0, yaw: 0.0}}' rostopic pub -1 /kuka_lwr_left/cartesian_computed_torque_controller/command kuka_lwr_controllers/PoseRPY '{id: 0, position: {x: -0.5, y: 0.0, z: 0.9}, orientation: {roll: 0.0, pitch: 0.0, yaw: 0.0}}'

-> Set PID gains : rostopic pub -1 /kuka_lwr_right/cartesian_computed_torque_controller/set_gains std_msgs/Float64MultiArray "data: [3000,1800,1800,1400,100,100,100,181,62,53,54,10,10,10]" rostopic pub -1 /kuka_lwr_left/cartesian_computed_torque_controller/set_gains std_msgs/Float64MultiArray "data: [3000,1800,1800,1400,100,100,100,181,62,53,54,10,10,10]"

For Moveit!

-> Launch controllers + moveit! + gazebo roslaunch platform_gazebo platform_gazebo.launch load_moveit:=true

-> Launch rviz plugin for moveit! roslaunch platform_gazebo show_moveit_rviz.launch use_left_arm:=true

-> Launch a test node using move group roslaunch test_move_group test_cartesian_position.launch group_name:=kuka_lwr_left

Launch rqt plugin to start/stop controllers graphically

rosrun rqt_gui rqt_gui

Menu Plugins / Robot Tools / Controller Manager

Simple Cartesian Controller :

-> How to start 'kuka_simple_cartesian_impedance_controller' controller : rosservice call /kuka_lwr_right/controller_manager/switch_controller "{start_controllers: ['kuka_simple_cartesian_impedance_controller'], stop_controllers: [], strictness: 2}"

-> How to stop 'kuka_simple_cartesian_impedance_controller' controller : rosservice call /kuka_lwr_right/controller_manager/switch_controller "{start_controllers: [], stop_controllers: ['kuka_simple_cartesian_impedance_controller'], strictness: 2}"

-> Set Cartesian stiffness or damping values rostopic pub -1 /kuka_lwr_right/kuka_simple_cartesian_impedance_controller/setCartesianStiffness std_msgs/Float64MultiArray "data: [800.0, 800.0, 800.0, 50.0, 50.0, 50.0]" rostopic pub -1 /kuka_lwr_right/kuka_simple_cartesian_impedance_controller/setCartesianDamping std_msgs/Float64MultiArray "data: [ 0.8, 0.8, 0.8, 0.8, 0.8, 0.8]"

-> Set Cartesian pose or wrench values -> Pose data to send -> [RXX, RXY, RXZ, TX, RYX, RYY, RYZ, TY, RZX, RZY, RZZ, TZ] rostopic pub -1 /kuka_lwr_right/kuka_simple_cartesian_impedance_controller/setCartesianPose std_msgs/Float64MultiArray "data: [RXX, RXY, RXZ, TX, RYX, RYY, RYZ, TY, RZX, RZY, RZZ, TZ]" -> Wrench data to send -> [fx, fy, fz, tx, ty, tz] rostopic pub -1 /kuka_lwr_right/kuka_simple_cartesian_impedance_controller/setCartesianWrench std_msgs/Float64MultiArray "data: [0.0, 0.0, 0.01, 0.0, 0.0, 0.0]"