Define Jacobian and GRF Frame

[Batou1406]

Define Jacobian and GRF Frame

Now, Jacobian and GRF Frame are not specified, but there are probably in the world frame

• GRF is outputed by the neural network (frame independant)
• Jacobian extracted from physX view with root_physx_view.get_jacobians() (probably world frame)

If the jacobian is in world frame, the neural network needs to learn the base to world frame transformation in order to apply meaningfull GRFs. Would be better to have evrything in the base frame

• [x] Define in which frame the jacobian is computed
• [x] Specify frame in the documentation each time used

[Batou1406]

Discussion with Giulio : Everything (GRF and Jacobian) should be in world frame !

[Batou1406]

Jacobian - A definitive Guide

How to get the Jacobian from sim

articulation.root_physx_view.get_jacobians()

• Returns the complete jacobian of every bodies, in the world frame
• The shape is (batch size, number of bodies, 6, num of joints + 6) →(..., 17, 6, 18)

Transform the jacobian from sim to usefull jacobian

We're interested only in the four jacobian that links the feet velocities to the joint velocities. Moreover, we're only interested in the translational jacobian and not the rotational one. This reduce the shape to (..., 4, 3, 18). Finally. Since they are four distinct kinematic chain, so not all joint influence every end-effector. Thus we reduce the jacobian to only joints that act on the end-effector, namely the 3 leg joints per leg. :warning: Please note that the joints indices are shifted from 6 position. This is due to how the Aliengo model is define :warning:. This gives the final jacobian shape (batch size, number of legs, 3, number of joints per leg) → (..., 4, 3, 3) The transformation can be done in with the following code :

# foot index : [13, 14, 15, 16]
foot_idx = articulation.find_bodies(".*foot")[0]    # foot index : [13, 14, 15, 16]

# Joint indices list [[0, 4, 8], [1, 5, 9], [2, 6, 10], [3, 7, 11]]
joints_idx = [articulation.find_joints("FL.*")[0], articulation.find_joints("FR.*")[0], articulation.find_joints("RL.*")[0], articulation.find_joints("RR.*")[0]]

jacobian_feet_full_w = articulation.root_physx_view.get_jacobians()[:, foot_idx, :3, :]
jacobian_w = torch.cat((jacobian_feet_full_w[:, 0, :, 6+np.asarray(joints_idx[0])].unsqueeze(1),
                        jacobian_feet_full_w[:, 1, :, 6+np.asarray(joints_idx[1])].unsqueeze(1),
                        jacobian_feet_full_w[:, 2, :, 6+np.asarray(joints_idx[2])].unsqueeze(1),
                        jacobian_feet_full_w[:, 3, :, 6+np.asarray(joints_idx[3])].unsqueeze(1)), dim=1)

This gives jacobian_w, the foot jacobian in the world frame

Small Note on Reference Frame and simulation characteristics

1. The Jacobian changes depending on which reference frame it is computed.
2. The Jacobian changes depending on the robot joint position
3. The Jacobian is computed by the simulator only after the simulation has been stepped once, and thus is not available at reset. :warning: One can see that point 3. will cause problem at initialisation :warning:

Jacobian Initialisation

From previous section, one noticed that correct initialisation of the Jacobian is not trivial. There are two solution to this problem :

• Manually compute the jacobian
• Use a known solution Manually compute the jacobian is overkill, and I will rather use a known solution. However, the problem is that the solution can't be known in advance in the world frame, because the robot pose in the world frame is unknown. Thus, the solution is to start :
  1. From the known jacobian, for a given default joint position, in the base frame jacobian_b.
  2. From the robot's pose in the world frame, compute the rotation matrix R_b_to_w
  3. Transform the jacobian from base to world frame : $Jw = R{b→w} J_b$ This will allow us to correctly initialise the jacobian

Jacobian Frame Transformation

The following code will return the jacobian in the base frame given the jacobian in the world frame

jacobian_w = ... # Given

# Retrieve the robot base orientation in the world frame as quaternions : shape(batch_size, 4)
quat_robot_base_w = articulation.data.root_quat_w 

# From the quaternions compute the rotation matrix that rotates from world frame w to base frame b : shape(batch_size, 3,3)
R_b_to_w = math_utils.matrix_from_quat(quat_robot_base_w)
R_w_to_b =  R_b_to_w.transpose(1,2) # Given the convention used, we get R_b_to_w from the quaternions, thus one need to transpose it to have R_w_to_b

# Finally, rotate the jacobian from world frame (fixed) to base frame (attached at the robot's base)
jacobian_b = torch.matmul(R_w_to_b.unsqueeze(1), jacobian_w) # (batch, 1, 3, 3) * (batch, legs, 3, 3) -> (batch, legs, 3, 3)

The following code will return the jacobian in the world frame given the jacobian in the base frame

jacobian_b = ... # Given

# Retrieve the robot base orientation in the world frame as quaternions : shape(batch_size, 4)
quat_robot_base_w = articulation.data.root_quat_w 

# From the quaternions compute the rotation matrix that rotates from base frame b to world frame w : shape(batch_size, 3,3)
R_b_to_w = math_utils.matrix_from_quat(quat_robot_base_w)

# Finally, rotate the jacobian from base to world frame : shape(batch_size, num_legs, 3, 3)
jacobian_w = torch.matmul(R_b_to_w.unsqueeze(1), jacobian_b) # (batch, 1, 3, 3) * (batch, legs, 3, 3) -> (batch, legs, 3, 3)

[Batou1406]

Done :heavy_check_mark: The last step was to update the doc and frame subscript. This was done in the refactor.