Get Reset robot state from Forward Kinematics

[Batou1406]

After an episode, an environment is reseted. The different environments are reseted asynchronously. Thus, we need to reset also the stored variables of the model based controller. For this, we need :

• The default feet position

Through the robot asset (ie. articulation type), the default root position and default joint position are available. Thus one could get the default feet position with inverse kinematics.

Now : The default feet position are hardcoded. This means it's specific to Aliengo in a certain position

How it should be Get the default joint position and robot position and compute feet position with the forward kinematics.

Problem

• The Forward kinematics formula is robot specific (Should not do it)
• The robot body position (ie. the feet position) are set only after a first simulation step (it mean we can use this to get the reset position)
• Hard to get the forward kinematics through Isaac Sim (but that is the way to go)

[Batou1406]

Hard-coded but Initialized

The default foot position and jacobian are hardocoded for a given robot configuration ie:

• robot Unitree - Aliengo
• Joint pos = hip=0, thigh=0.9, calf=-1.8

This isn't ideal since it depends on both the robot and the joint default position, but this will do the trick for now, thus I'm closing the issue.

Default feet position in base frame

$$ \begin{bmatrix} 0.243 & 0.138 & -0.325 \ 0.243 & -0.138 & -0.325 \ -0.236 & 0.137 & -0.326 \ -0.236 & -0.137 & -0.326 \ \end{bmatrix} $$

Default Jacobian in body frame (same for 4 feet) in base frame

$$ \begin{bmatrix} 0 & -0.311 & -0.155 \ 0.311 & 0 & 0 \ 0.083 & 0 & -0.196 \ \end{bmatrix} $$

[Batou1406]

Transform default Values to correct frame

:warning: The default values can be known only in the base frame, because the robot orientation and position (in local world frame) is set randomly after reset "pose_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5), "yaw": (-3.14, 3.14)}, :warning: Thus, from the known value in the base frame, one need to correctly transform them in the desired frame, in our case, the local world frame.

Jacobian transformation

The Jacobian transformation has already been discussed in another issue. Thus, we cited this here only for completude and indicate that the default reset jacobian is no longer in base frame but in local world frame.

Foot position transformation

After a reset event, only the articulation.data.root_xxx properties are initialised. Thus, those are the only properties one can use to make the transformation. :warning: The transformation is here a rotation and a translation, since the robot base is intialised at both a roation and translation from the local world frame origin :warning:.

1. Retrieve default foot position from default joint position in base frame : p_b
2. Retrieve Rotation of the base frame with respect to the world frame and compute rotation matrix
3. Rotate p_b from base frame to world frame
4. compute translation from base frame to local world frame
5. Apply translation and find final p_lw

   
   # Retrieve the robot base orientation in the world frame as quaternions : shape(batch_size, 4)
   quat_robot_base_w = self._asset.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)

Rotate p from base to world frame (no translation yet)

p_rotated = torch.matmul(R_b_to_w, p_b.permute(0,2,1)).permute(0,2,1)

Compute translation from base frame to local world frame

translation_b_to_lw = (self._asset.data.root_pos_w.unsqueeze(1) - self._env.scene.env_origins.unsqueeze(1))

Transform p_b into p_lw b appling final translation

p_lw = translation_b_to_lw + p_rotated

This transformation has been verified on numerical example with the `get_robot_state()` function