External wrench estimate

[shamilmamedov]

Hello there! I need external wrench estimate in my project, but at the end-effector or flange. Libfranka provides wrench estimate at the stiffness frame. I wanted to reproduce the estimate using Jacobian of the stiffness frame and filtered external torque estimate, according to equation for some trajectory using numpy. However, numpy solution turned out to be different for torques.

What can be the reason for that? How does libfranka internally compute external wrench estimate?

[yf291115925]

Libfranka provides external wrench estimated on stiffness frame, expressed relative to the base frame (O_F_ext_hat_K) and the stiffness frame (K_F_ext_hat_K). The stiffness frame is coincide with the end-effector frame in default. The wrench description can be found in https://frankaemika.github.io/libfranka/structfranka_1_1RobotState.html. The relation between O_F_ext_hat_K and K_F_ext_hat_K can be checked as below:

import numpy as np

def S(w):
    x,y,z = w[0],w[1],w[2]
    return np.array([[0,-z,y],[z,0,-x],[-y,x,0]])

# Wrench Data
O_F_ext_hat_K = np.array([-2.06065,0.45889,-0.150951,-0.482791,-1.39347,0.109695])
K_F_ext_hat_K = np.array([-2.03638,-0.529916,0.228266,-0.275938,0.434583,0.0317351])

# Force 
O_Force = O_F_ext_hat_K[0:3].reshape((3,1))
K_Force = K_F_ext_hat_K[0:3].reshape((3,1))

# Torque
O_Torque = O_F_ext_hat_K[3:6].reshape((3,1))
K_Torque = K_F_ext_hat_K[3:6].reshape((3,1))

O_T_EE = np.array([0.998578,0.0328747,-0.0417381,0,0.0335224,-0.999317,0.0149157,0,-0.04122,-0.016294,-0.999017,0,0.305468,-0.00814133,0.483198,1])
p = O_T_EE[12:15]
O_T_EE = O_T_EE.reshape((4,4),order='F')

O_R_K = O_T_EE[0:3,0:3]
# print(O_R_K)

O_Force2 = O_R_K @ K_Force
# print(O_Force)
# print(O_Force2)

O_Torque2 = S(p) @ O_R_K @ K_Force + O_R_K @ K_Torque # pay attention here !
# print(O_Torque)
# print(O_Torque2)

print(np.allclose(O_Force, O_Force2)) # True
print(np.allclose(O_Torque, O_Torque2)) # True

The data is from https://frankaemika.github.io/docs/getting_started.html.

Hope this can solve your question.

[yf291115925]

By the way, I have no idea how tau_ext_hat_filtered is actually calculated. The relation bewtten tau and wrench is clear, but I can't verify it.

F_ext = np.array([-2.06065,0.45889,-0.150951,-0.482791,-1.39347,0.109695]) # frame O
tau_ext = np.array([0.00187271,-0.700316,0.386035,0.0914781,-0.117258,-0.00667777,-0.0252562]) 
# Jacobian can be calculated from https://github.com/aaronzguan/Franka-Robot-Path-Planning
# q=[0.0167305,-0.762614,-0.0207622,-2.34352,-0.0305686,1.53975,0.753872]
jacobian = np.array([[8.14134560e-03,1.50178440e-01,4.15058567e-03,1.35190483e-01,-4.32402242e-04,2.13848047e-01,4.33680869e-19],
 [3.05467774e-01,2.51279485e-03,3.24608529e-01,-6.42337560e-03,2.13001769e-01,1.71980513e-03,-1.73472348e-17],
 [-0.00000000e+00,-3.05288821e-01,9.15369498e-03,4.64021841e-01,-3.45620635e-03,7.92351882e-02,2.71050543e-19],
 [0.00000000e+00,-1.67297195e-02,-6.90717130e-01,1.71754363e-03,9.99946077e-01,2.02976924e-03,-4.12192551e-02],
 [0.00000000e+00,9.99860048e-01,-1.15571213e-02,-9.99895676e-01,1.86416269e-03,-9.99866322e-01,-1.62939167e-02],
 [1.00000000e+00,6.12323400e-17,7.23032696e-01,-1.43417841e-02,-1.02161072e-02,1.62240170e-02,-9.99017258e-01]])
tau_ext_check = jacobian.T @ F_ext
print(tau_ext)
print(tau_ext_check) # not same with tau_ext !

The data is from https://frankaemika.github.io/docs/getting_started.html.

[shamilmamedov]

@yf291115925 thank you for your reply! I understand the relationship between wrench in stiffness frame and base frame, but it is not the point of my question. Your second comment approaches the problem that I am concerned with from a different angle: you assume that wrench measurement is given and compute external torques. You end up with the same problem.

External wrench measurements usually are not available unless you have force-torque sensor. Thus I am sure that wrench estimate is obtained from tau_ext_hat_filtered, but I don't understand why I cannot reproduce it.

[yf291115925]

Sorry for my misunderstanding >.< The curve showed that you have reproduced the forces but failed to the torques, so I naturally thought the problem is about O_F_ext_hat_K and K_F_ext_hat_K. I agree with that wrench estimate is obtained from tau_ext_hat_filtered. Maybe we need some help :) @fwalch @sgabl

[yf291115925]

I have reproduced the forces, but the torques turned out to be different just like you.

F_ext_check = np.linalg.pinv(jacobian.T) @ tau_ext
print(F_ext) # [-2.06065   0.45889  -0.150951 -0.482791 -1.39347   0.109695]
print(F_ext_check) # [-2.06109637  0.45861425 -0.15121872 -0.26223991 -0.4439924  -0.01373689]
# Forces: √   Torques: ×

[liuyangdh]

I have reproduced the forces, but the torques turned out to be different just like you.

F_ext_check = np.linalg.pinv(jacobian.T) @ tau_ext
print(F_ext) # [-2.06065   0.45889  -0.150951 -0.482791 -1.39347   0.109695]
print(F_ext_check) # [-2.06109637  0.45861425 -0.15121872 -0.26223991 -0.4439924  -0.01373689]
# Forces: √   Torques: ×

@yf291115925 Hi, thanks a lot for the investigation. Would this result mean that the external torque/wrench estimate of libfranka is problematic?

[shamilmamedov]

Hi @liuyangdh, I used joint torques and estimated the end-effector wrench myself using jacobian.

[yetian0]

@liuyangdh hi, have you known the answer now?