Adjusting the model frame

[abuibaid]

Hi there,

I have just learned about this wonderful tool. I have gone through and It appears it can do multi things such as kinematics and dynamics which I'm really interested in! But I have faced one problem which has to do with frames. Actually, I have used this function "buildModelFromUrdf" to import UR5e model. It works totally fine with me but the thing is: it specifies the frames totally different from what it shall be. For example, the first frame is specified automatically the "universe", but it shall be "shoulder_pan_joint" and the last frame is specified as "wrist_3_joint" where it should be "wrist_3_link-tool0_fixed_joint". Therefore, based on these frames, The computation of Jacobian will result differently from what I have approached analytically!

Here is the code

model    = pin.buildModelFromUrdf(urdf_filename)
print('model name: ' + model.name)

# Create model and data
data = model.createData()

# Set bounds (by default they are undefinded for a the Simple Humanoid model)

model.lowerPositionLimit = -np.ones((model.nq,1))
model.upperPositionLimit = np.ones((model.nq,1))

# q = pin.randomConfiguration(model) # joint configuration
q = np.array([-2.93297704, -1.91732752,  1.66140008, -1.31517303, -1.57547641,
        0.21511427])

q_dot = np.random.rand(model.nv,1) # joint velocity
q_dotdot = np.random.rand(model.nv,1) # joint acceleration
f_ext = np.random.rand(model.nv,1) # joint acceleration

# pin.computeABADerivatives(model,data,q,q_dot,q_dotdot)
# pin.aba(model,data,q,v,a)
# Retrieve the derivatives in data
C = pin.computeCoriolisMatrix(model, data, q, q_dot)
pin.nonLinearEffects(model, data, q, q_dot)
G = pin.computeGeneralizedGravity(model, data, q)
pin.computeMinverse(model, data, q)
M = pin.crba(model, data, q)
J = pin.computeJointJacobians(model,data,q)

tau_crba =  M*q_dotdot + C*q_dot + G - J.T*f_ext
tau_crba_ = pin.crba(model, data, q)

[jcarpent]

Your code is not consistent. Could you write down what you expect to do mathematically speaking?

[jorisv]

Hello @abuibaid,

You're maybe don't looking in the good structure/function. wrist_3_link-tool0_fixed_joint exists, but it's not a Joint but a Frame. Joint depend of the q generalized position vector while Frame are a static transformation attached to a joint.

Here the UR5 Joint list:

universe
shoulder_pan_joint
shoulder_lift_joint
elbow_joint
wrist_1_joint
wrist_2_joint
wrist_3_joint

And the UR5 Frame list:

universe
root_joint
world
world_joint
base_link
base_link-base_fixed_joint
base
shoulder_pan_joint
shoulder_link
shoulder_lift_joint
upper_arm_link
elbow_joint
forearm_link
wrist_1_joint
wrist_1_link
wrist_2_joint
wrist_2_link
wrist_3_joint
wrist_3_link
ee_fixed_joint
ee_link
wrist_3_link-tool0_fixed_joint
tool0

You can compute the Frames jacobian with the pin.computeFrameJacobian function and retrieve a Frame id/index with model.getFrameId.

The following cheatsheet details the main Joint and Frame functions.

universe is the first joint. For the UR5, it's a fixed joint, but for a mobile robot it can be a planar or free flyer joint.

[jorisv]

Hello @abuibaid,

Did the last comment solve your issue ?

[abuibaid]

@jorisv Thank you so much for your feedback. I understand it right now!