Hello,
I’m working with the operational_space.py controller in isaac orbit which within the code, you can enable the gravity and inertial compensation, so you can input the mass matrix and the gravity vector:
# reset desired joint torques
self._desired_torques[:] = 0
# compute for motion-control
if desired_ee_pos is not None:
# check input is provided
if ee_pose is None or ee_vel is None:
raise ValueError("End-effector pose and velocity are required for motion control.")
# -- end-effector tracking error
pose_error = compute_pose_error(
ee_pose[:, :3], ee_pose[:, 3:], desired_ee_pos, desired_ee_rot, rot_error_type="axis_angle"
)
velocity_error = -ee_vel # zero target velocity
# -- desired end-effector acceleration (spring damped system)
des_ee_acc = self._p_gains * pose_error + self._d_gains * velocity_error
# -- inertial compensation
if self.cfg.inertial_compensation:
# check input is provided
if mass_matrix is None:
raise ValueError("Mass matrix is required for inertial compensation.")
# compute task-space dynamics quantities
# wrench = (J M^(-1) J^T)^(-1) * \ddot(x_des)
mass_matrix_inv = torch.inverse(mass_matrix)
if self.cfg.uncouple_motion_wrench:
# decoupled-mass matrices
lambda_pos = torch.inverse(jacobian[:, 0:3] @ mass_matrix_inv * jacobian[:, 0:3].T)
lambda_ori = torch.inverse(jacobian[:, 3:6] @ mass_matrix_inv * jacobian[:, 3:6].T)
# desired end-effector wrench (from pseudo-dynamics)
decoupled_force = lambda_pos @ des_ee_acc[:, 0:3]
decoupled_torque = lambda_ori @ des_ee_acc[:, 3:6]
des_motion_wrench = torch.cat(decoupled_force, decoupled_torque)
else:
# coupled dynamics
lambda_full = torch.inverse(jacobian @ mass_matrix_inv * jacobian.T)
# desired end-effector wrench (from pseudo-dynamics)
des_motion_wrench = lambda_full @ des_ee_acc
else:
# task-space impedance control
# wrench = \ddot(x_des)
des_motion_wrench = des_ee_acc
# -- joint-space wrench
self._desired_torques += jacobian.T @ self._selection_matrix_motion @ des_motion_wrench
and gravity:
# add gravity compensation (bias correction)
if self.cfg.gravity_compensation:
# check input is provided
if gravity is None:
raise ValueError("Gravity vector is required for gravity compensation.")
# add gravity compensation
self._desired_torques += gravity
For example, I’m using franka model imported from:
from omni.isaac.orbit_assets import FRANKA_PANDA_HIGH_PD_CFG, RIDGEBACK_FRANKA_PANDA_CFG # isort:skip
Which uses the franka_ros repository as reference. This same repository has its own example controllers where I can find this functions to get the coriolis and mass matrix for example (Mass and coriolis):
std::array<double, 49> inertia_array = arm_data.model_handle_->getMass();
std::array<double, 7> coriolis_array = arm_data.model_handle_->getCoriolis();
How can I get the mass, coriolis and gravity matrixes in isaac orbit so I can use them in the controller? I’m really stuck here cause, I would appreciate some help.
Thanks in advance and regards