Custom dynamics utilization with contact simulation

[sentojh]

Intro

Hi!

I am a graduate student majoring in Mechanical Engineering. I use MuJoCo for my research on the force control of robot manipulators.

My setup

Currently, I use the Python package of MuJoCo in Window OS. (Python version: 3.10, Mujoco version: 3.3.2)

My question

So, my question is whether there is any way to set the dynamic parameters in mujco (especially mjdata.qM and mjdata.qfrc_bias) as my custom dynamic parameter during contact? (Since I want to test my own dynamics algorithm in a force control scenario, the dynamics result differs from the inherent result in mujoco supports.)

In free motion (without contact), it is okay, but if I contact the robot manipulator's end-effector with other environment geometry, the "FactorizeHessian: rank-deficient sparse Hessian" error directly occurs when the contact is initiated.

Below is my control loop. The self.robot is another class where I implemented for my own dynamics algorithm. So I override the variables in mujoco (qM and qfrc_bias) to my computation, and then update.

   # class
   class Simulator:
        def __init__(self, scene_path, robot, cmode=1, show_viewer=True):
            self.data.qpos[:self.robot.jDof] = self.robot.q
            self.data.qvel[:self.robot.jDof] = self.robot.qdot

             # Load Mujoco model
            self.scene_path = scene_path
            self.model = mujoco.MjModel.from_xml_path(scene_path)
            self.data = mujoco.MjData(self.model)
            self.robot = robot

    (~~~~~~)
    # class function
        def control_loop(self):
            while True:
                # dynamics overload to mujoco variables
                self.data.qpos[:self.robot.jDof] = self.robot.q
                self.data.qvel[:self.robot.jDof] = self.robot.qdot
                mujoco.mj_step1(self.model, self.data)
                custom_mass_matrix_flattened = self.robot.M[np.tril_indices(self.robot.jDof)]
                self.data.qM[:] = custom_mass_matrix_flattened

                self.model.dof_damping[:] = 0
                self.model.dof_frictionloss[:] = 0
                self.model.opt.gravity[:] = 0


                # custom control torque calculation (force controller)
                tau_c = (~~~~)

                # inject torque and bias force (bias force is based on custom dynamics)
                self.data.qfrc_applied[:] = tau_c.flatten()
                self.data.qfrc_bias[:] = self.robot.C @ self.robot.qdot + self.robot.g


                # simulate one step
                 mujoco.mj_step2(self.model, self.data)

                # state update (kin/dyn) of robot manipulator class
                self.robot.update_state(self.data.qpos[:6], self.data.qvel[:6])

However, ``mujoco.FatalError: FactorizeHessian: rank-deficient sparse Hessian'' error has occurred.
Is there any way to set custom dynamics, but use internal contact solver in MuJoCo in a stable manner... I cannot change the variables in a consistent way to my own dynamic parameters in the QP solver for contact resolution.

Confirmations

• I searched the latest documentation thoroughly before posting.
• I searched previous Issues and Discussions, I am certain this has not been raised before.