How to get inertia, coriolis and gravity matrixes from franka robot asset [orbit]


I’m working with the 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 =, decoupled_torque)
                    # 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
                # 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