Access joint limit violation force without using xml sensors.

[P-Schumacher]

Hi everyone,
several people have approached me to ask how to implement joint limit violation forces, as used in my recent preprint

My implementation uses limit force sensors embedded in the xml files. These are quite inconvenient and it's not always possible to edit the xml files for every model.

I'm wondering if there is a programmatic way of accessing this information? I think it would be a great service to the community to implement a function like

force_value = get_limitfrc(joint_name)

But I'm a bit unsure how to do it. I tried starting with efc_type and efc_force, but it's unclear how you would relate this to the joint name.

[elladyr]

One suggested way in this issue here, might help. Essentially, you measure the joint torque with qfrc_constraint and qfrc_smooth in the joint data structure, then manually check if your joint exceeds the limits.

[P-Schumacher]

This would get you the total force acting on the joint, right? It’s a proxy, but not perfect.

EDIT: Let me try, might be a solution

[P-Schumacher]

The posted solution works when there ar eno additional constraints, e.g. an arm that is suspended in the air. Still have to test in the case of contact forces etc

EDIT: Doesn't seem to work when other constraints are present. I think we would need to filter all constraints by joint limit constraints and assign the right joint to each one at each time step

[P-Schumacher]

Got it:

def get_limitfrc(joint_name, mj_model, mj_data):
    non_joint_limit_efc_idxs = np.where(mj_data.efc_type != mujoco.mjtConstraint.mjCNSTR_LIMIT_JOINT)[0]
    only_jnt_lim_efc_force = mj_data.efc_force.copy()
    only_jnt_lim_efc_force[non_joint_limit_efc_idxs] = 0.0
    joint_force = np.zeros((mj_model.nv,))
    mujoco.mj_mulJacTVec(mj_model, mj_data, joint_force, only_jnt_lim_efc_force)
    return joint_force[mj_model.joint(joint_name).dofadr]