Hello,
Though I already posted question in discord, I am posting this to forum too.
I wanted to ask if it is possible to implement a soft terrain contact model without using Isaac’s default rigid body physics (i.e., by disabling the collider).
The idea is to implement this force model ([1303.7065] A Terradynamics of Legged Locomotion on Granular Media) to compute the contact forces and then directly apply the computed forces to my robot’s foot (bipedal robot).
The contact can be either a single-point contact or a patch contact.
I just need to create virtual links (empty transforms linked to the foot) to which I can apply forces using the articulation API.
In my attempts so far, the robot tends to fall immediately after the simulation starts, likely due to a moment causing it to rotate.
I would appreciate any insight you could provide!
Here is how the current code looks like.
It is not the entire code, but I think you will get the idea.
## in IsaacLab DirectEnv class ##
def _compute_external_perturbation(self)->None:
"""
Apply RFT lift and drag force
"""
self.foot_pos = self._robot_api.body_pos_w[:, -self.cfg.foot_patch_num:, :]
self.foot_velocity = self._robot_api.body_lin_vel_w[:, -self.cfg.foot_patch_num:, :]
self.foot_depth = self._robot_api.foot_depth
self.foot_angle_beta = self._robot_api.foot_beta_angle
self.foot_angle_gamma = self._robot_api.foot_gamma_angle
self.rft_force = self._rft.get_resistive_force(
self._root_pos, self._root_quat, self._robot_api.root_ang_vel_w,
self.foot_pos, self.foot_velocity, self.foot_angle_beta, self.foot_angle_gamma) # (num_envs, self.cfg.foot_patch_num, 3)
self._robot_api.set_external_force(self.rft_force, self._foot_ids, self._robot._ALL_INDICES)
class RFT:
def __init__(self, cfg: MaterialCfg, surface_area: float, boundary_depth: float):
"""
Resistive Force Theory based soft terrain model
https://www.science.org/doi/10.1126/science.1229163
"""
self.cfg = cfg
self.surface_area = surface_area
self.boundary_depth = boundary_depth
def get_resistive_force(self, root_pos:torch.Tensor, root_quat:torch.Tensor, root_ang_vel_w:torch.Tensor,
foot_pos:torch.Tensor, foot_velocity:torch.Tensor,
beta:torch.Tensor, gamma:torch.Tensor)->torch.Tensor:
"""
Find resistive force per foot
"""
num_chunks = foot_pos.shape[1]
dA = self.surface_area/(num_chunks//2)
depth = foot_pos[:, :, -1]
# calculate alpha_z
alpha_z = torch.zeros_like(depth) # A00
alpha_z += self.cfg.A00*torch.cos(2*torch.pi*(0*beta/torch.pi)) #A00
alpha_z += self.cfg.A10*torch.cos(2*torch.pi*(1*beta/torch.pi)) # A10
alpha_z += self.cfg.B01*torch.sin(2*torch.pi*(1*gamma/(2*torch.pi))) # B01
alpha_z += self.cfg.B11*torch.sin(2*torch.pi*(1*beta/torch.pi + 1*gamma/(2*torch.pi))) # B11
alpha_z += self.cfg.B_11*torch.sin(2*torch.pi*(-1*beta/torch.pi + 1*gamma/(2*torch.pi))) # B-11
# calculate alpha_x
alpha_x = torch.zeros_like(depth)
alpha_x += self.cfg.C01*torch.sin(2*torch.pi*(1*gamma/(2*torch.pi))) # C01
alpha_x += self.cfg.C11*torch.sin(2*torch.pi*(1*beta/(torch.pi) + 1*gamma/(2*torch.pi))) # C11
alpha_x += self.cfg.C_11*torch.sin(2*torch.pi*(-1*beta/(torch.pi) + 1*gamma/(2*torch.pi))) # C-11
alpha_x += self.cfg.D10*torch.cos(2*torch.pi*(1*beta/(torch.pi))) # D10
# calculate resistive force (feedforward)
depth_mask = depth < 0
fz = alpha_z * (-depth) * dA * depth_mask * (1e6) #m^3 to mm^3
fx = torch.zeros_like(fz)
fy = torch.zeros_like(fz)
force = torch.stack((fx, fy, fz), dim=-1)
return force