Wrench measurements different in simulation and real robot

[martinandrovich]

I have implemented a Cartesian Admittance Controller that uses the O_F_ext_hat_K estimated wrench h = [f, mu]. However, it seems that the wrench in simulation and on the real robot are inverted (or I'm just stupid). This is observed when pushing the robot's "head" along the x-axis (relative to base frame).

Simulation:

Real:

The controller is based on the paper The role of Euler parameters in robot control, which uses the vector h = [f, mu] defined as "vector of contact forces and moments exerted by the end effector on the environment". The controller works fine in simulation, but requires to negate the h vector when used with the real robot, as:

Eigen::Vector6d h_e = Eigen::Vector6d(robot_state.O_F_ext_hat_K.data());
h_e *= (in_simulation) ? 1 : -1; // invert if running on real robot

Is this expected behavior?

[peetCreative]

Maybe connects to #249

[peetCreative]

Hey Martin,

I tried to reproduce the problem using your controller. However it is starting to move when the your controller takes over. But is stopped by a franka error. Any suggestions?

I start the controller using: roslaunch teleop_grasp robot.launch robot_ip:=X.X.X.X rviz_marker:=true

And I get a cartesian_reflex as an error:

[ WARN] [1669045222.957726429]: Initialized CartesianAdmittanceController with:

in_simulation = false                                                                                                                                                              
Kp = 1000 1000 1000                                                                                                                                                                
Ko = 200 200 200                                                                                                                                                                   
Dp = 40 40 40                                                                                                                                                                      
Do = 28.28 28.28 28.28                                                                                                                                                             
Mp = 2 2 2                                                                                                                                                                         
Mo = 5 5 5                                                                                                                                                                         
kpp = 200                                                                                                                                                                          
kpo = 900                                                                                                                                                                          
kvp = 28.28                                                                                                                                                                        
kvo = 60                                                                                                                                                                           
kn = 0.5                                                                                                                                                                           
kc = 1.41421                                                                                                                                                                       
dtau_max = 1000                                                                                                                                                                    
slew_rate = 0.01                                                                                                                                                                   
state_publish_rate = 100                                                                                                                                                           
tau_ext_lowpass_filter = inf                                                                                                                                                       

[INFO] [1669045222.960372]: Controller Spawner: Loaded controllers: cartesian_admittance_controller
[INFO] [1669045222.962212]: Started controllers: franka_state_controller
[ INFO] [1669045222.965117760]: FrankaHW: Prepared switching controllers to joint_torque with parameters limit_rate=1, cutoff_frequency=100, internal_controller=joint_impedance
[ WARN] [1669045222.965818812]: updateConfig() called on a dynamic_reconfigure::Server that provides its own mutex. This can lead to deadlocks if updateConfig() is called during an update. Providing a mutex to the constructor is highly recommended in this case. Please forward this message to the node author.                                                 
[ INFO] [1669045222.972280017]: Initial configuration:
qN_d: [ -0.0962938   -0.080146    0.104098    -2.45398 -0.00695868     2.27312    0.764438]
p_d: [   0.459769 0.00315563    0.32011]
[eta_d, eps_d]: [0.0252345,   0.928816  -0.367014 -0.0443229]
RPY: [ 3.12718 -0.10103 0.751894]
T_d:

  0.726672  -0.679541  -0.100859   0.459769
 -0.684014  -0.729328 -0.0143421 0.00315563
-0.0638129  0.0794107  -0.994797    0.32011
         0          0          0          1
T_K:

1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1

[INFO] [1669045222.972692]: Started controllers: cartesian_admittance_controller
[ INFO] [1669045222.984465018]: Initial configuration:
qN_d: [ -0.0962974  -0.0801486    0.104096    -2.45398 -0.00695675     2.27312    0.764436]
p_d: [    0.45977 0.00315325   0.320112]
[eta_d, eps_d]: [0.0252326,   0.928815  -0.367016 -0.0443205]
RPY: [  3.12718 -0.101025  0.751899]
T_d:

  0.726669  -0.679544  -0.100853    0.45977
 -0.684018  -0.729325 -0.0143402 0.00315325
-0.0638096  0.0794057  -0.994798   0.320112
         0          0          0          1
T_K:

1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1

[ WARN] [1669045222.984553787]: Updated gains via dynamic reconfigure!
[ WARN] [1669045222.984603532]: The controller currently ignores desired twist and acceleration in CartesianAdmittanceController::get_desired().
[ERROR] [1669045223.587427235]: libfranka: Move command aborted: motion aborted by reflex! ["cartesian_reflex"]
control_command_success_rate: 0.93 

[marcbone]

"cartesian reflex" means that the robot detects a collision. You can increase the collision thresholds with the "set_full_collision_behavior" service. Just set all the values to 100 before you start the controller.

[martinandrovich]

@peetCreative you can read about the cartesian_reflex error here at the franka_ros docs, but as @marcbone says, it's due to the thresholds. During testing, we changed our thresholds from the franka_control_node.yaml (which we didn't push for safety reasons).

[peetCreative]

Hey Martin, I will try this suggestion next week. I don't have a robot by my hand. However, I don't understand why the Joints are moving at all. As for the start the end effektor position is read and is controlled to stay like it is.

[AnlageM]

"cartesian reflex" means that the robot detects a collision. You can increase the collision thresholds with the "set_full_collision_behavior" service. Just set all the values to 100 before you start the controller.

@peetCreative you can read about the cartesian_reflex error here at the franka_ros docs, but as @marcbone says, it's due to the thresholds. During testing, we changed our thresholds from the franka_control_node.yaml (which we didn't push for safety reasons).

Hi, I join this conversation because I am going to change the franka_control_node.yaml force thresholds as well, to allow interaction with the environment. Which are the threshold for lower and upper threshold you would not advice to overcome, to avoid mechanical damages to the robot? Is there any documentation referencing the maximum values one could set?

[marcbone]

Hello @AnlageM, you cannot damage the robot by setting a wrong value or sending wrong commands. The robot will always reject commands in order to protect itself from damage. IIRC the maximum values for the collision thresholds is 100. Anything above will be capped to 100.

The only way to damage the robot with a motion is to make it crash into something it doesnt know (it knows itself). However, the robot detects when it collides with something and stops. By increasing the collision threshold the robot will stop later if it crashes into something, causing more damage to itself or the object. Therefore, I suggest to always use a lower collision threshold while developing a controller. Once you have tested it and are sure that it is robust you can increase the collision thresholds.

[martinandrovich]

Hey Martin, I will try this suggestion next week. I don't have a robot by my hand. However, I don't understand why the Joints are moving at all. As for the start the end effektor position is read and is controlled to stay like it is.

@peetCreative The controller might still command a value too high, even if it's just trying to keep its current position. I would recommend their (very well written docs): https://frankaemika.github.io/docs/libfranka.html?highlight=threshold#behavioral-errors

[Maverobot]

After quick glance of the code, I think the values of tau_ext_hat_filtered/O_F_ext_hat_K/K_F_ext_hat_K in franka_gazebo are wrong . The tau_ext should have been computed by

      double tau_ext = joint->command - joint->effort - joint->gravity;

instead of

      double tau_ext = joint->effort - joint->command + joint->gravity;

Further investigation/verification is needed.

[leonmkim]

To add my two cents, I looked into this a couple months ago and found that the simulated Tau_J field in the robot state message is flipped in sign from the real robot. Regarding the expression for tau_ext, it's likely on the real robot to be computed simply as Tau_J - gravity. So I ended up swapping the expression for tau_ext with (-joint->effort) - joint->gravity Perhaps someone from franka emika can provide clarity or a correction :)

EDIT: another late update, I found on the real robot coriolis and inertial torques do in fact seem to be accounted for!