Inverse dynamics with equality constraints (parallel robot)

[tturpin]

Intro

Hi!

I am a 3d printing hobbyist, and I use MuJoCo to simulate a delta 3d printer design (parallel bot). I am totally new to multibody system dynamics, so I apologize for my ignorance.

My setup

MuJoCo 3.3.3 (but python says 335) on Ubuntu 24.04

My question

I want to estimate the forces applying to various elements of the design so I modeled the delta 3d printer in MuJoCo using equality constraints to "close the loops" in the kinematic chain. Conceptually, those equality should be strictly enforced, but I understand that MuJoCo considers soft constraints, so I just set model.eq_solref to the smallest possible value.

I have meaningful results in forward dynamics, using three position actuators for the printer's three carriages: setting the actuators' positions to move the end-effector according to a sinusoïdal function of time, I could check that the forces at various joints match what I could estimate by other means, given the involved masses. I also tried a fixed position combined with an external force applied using xfrc_applied on the end effector (which is a root "floating" body only linked to the rest through constraints) with success.

However, inverse dynamics gives me meaningless values, so I wonder if I'm doing something wrong, or if inverse dynamics is the adequate tool for my problem with equality constraints. Here is what I did:

• control the actuators to position the system in a given still state, running forward kinematics until speeds and accelerations become almost zero
• Set the qacc values of my end-effector body to a non-null acceleration
• call mj_inverse
• read the force sensors

I have non-zero values in sensors and in qfrc_inverse, but those value are inconsistent.

I read the "Inverse dynamics" section of the manual's Simulation chapter, but I don't have the required background to understand it fully. So, any hint to put me on the right track will be greatly appreciated !

Minimal model and/or code that explain my question

I'm hoping that the above "high-level" description of what I'm doing can lead to some useful advice (I don't think my issue indicates a bug in MuJoCo), but if more details are required, I will try to minimize my model for illustration.

Confirmations

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

[v-r-a]

Hello,

I worked a bit on five-bar closed-loop mechanism earlier. I might be able to help or learn somthing new from this interesting application!

• Sharing the XML (along with the mesh files, if any) could allow anyone to resolve the exact problem.
• You may share the code to reproduce the problematic results.

1. I would recap the inputs-outputs of an inverse dynamics analysis. Inputs: Joint positions, velocity, and acceleration. Output: actuator force/torques. So, if you wanted the 3D printer head to stay at a place, you would input the corresponding qpos, qvel, and probably set qcc to zero. Inverse dynamics would give you the force/torque required by the actuators to stay in that place. You would get all the internal constraint forces as well.
2. Inverse dynamics (ID) with floating base becomes a bit tricky since the floating base is assumed to have six fictious actuators. The ID analysis is good (realistic) if those three forces and torques are close to zero for the prescribed qpos, qvel, and qacc. Otherwise the said state and qacc are not feasible.
3. Since you are modelling a parallel mechanism, you do not need to use a freejoint in your chain. I think a simpler approach is possible. You can attach the platform to one of your 3D printer arms and put only two equality constraints with respect to other two arms. This approach would also reduce the unconstrained DOFs, and number of constraints in the play.
4. You said you were using small value of solref. Remember:

The timeconst parameter should be at least two times larger than the simulation time step,

[tturpin]

Thank you for your help.

Your reformulation of inverse dynamics helped me understand that my use of of mj_inverse is completely wrong. In qacc, I am giving as input only the expected end-effector acceleration, with the intent that all other acceleration would be derived from this (since the system has only 3 DOFs in principle), but it rather means to require that all other accelerations be zero, which is possible because equality constraints are "soft". Also, I was expecting that only the 3 actuators would appear in the forces computed by inverse dynamics, while in fact, all joints, even the "free" end-effector's joint have corresponding entries in qfrec_inverse if understand correctly your "fictious actuators" explanation. Those do not have "close to zero" values in my results, which are therefore meaningless.

So clearly, It's not how mj_inverse should be used. And that think that in my case it would require some significant work: I would have to compute the positions / speeds / accelerations for all joint from the target end-effector trajectory, before invoking mj_step.

Regarding point 3, attaching the end-effector to one of the arms would be annoying because:

• I would have two distinct cases to write
• It would no longer be symmetric, as one arm would be strictly constrained and the other two would be soft
• I would need to compute coordinate changes to get or set the end effector's absolute position, speed and acceleration

And thanks for the solref/timestep advice, I did just that.

Here is my model (all in a python file): radial_delta.py
I removed unrelated things but did not make the effort to "minimize" it the strictest sense. But there is no real need to investigate it further, if my mistake is what I think it is. My file is probably terrible in many respects (for example it has an additional DOF: the end effector can spin freely). But it works enough to give me some useful data. It's just that I need to run forward dynamics computations for several hours (could probably be improved with better simulation parameters) to only approximate what inverse dynamics "should" be able to do much more precisely in seconds..., if I had a correct setup. But it's probably not worth the additional days of effort since I already have results.

A the end of this file are the commands to run three "simulations", to illustrate what I intended :

• static, with an applied force to emulate the end-effector's accelerating mass
• dynamic, following a sinusoidal trajectory (results corroborate the static simulation)
• inverse, with nonsense results

Here is the output for the three simulations:

time= 355 µs
position distance to target= 0.008601821376829378
max system velocity= 0.0007342399772526704
max system acceleration= 0.004358999294826057
carriage axial forces:
-666.4309847429395
333.21849378570914
333.21849378570914

time= 10001 µs
position distance to target= 0.006399523879688983
end-effector velocity= 0.28090485416330574
end-effector acceleration= 5.099149543249649
carriage axial forces:
-738.1835761666298
369.0891745164991
369.0891744270921

time= 1 µs
position distance to target= 0.0
velocity= 1.45497270456113e-12
acceleration= 1.45497270456113e-09
max system velocity= 1.8052943341317608e-11
max system acceleration= 1.8052943341317612e-08
carriage axial forces:
-2.6469779601696886e-23
-2.3413734979225583e-22
9.887716376849687e-23
after mj_sinverse:
carriage axial forces:
-166.58444038181912
83.29222117178843
83.29222117178843
hot_end force= 1515.059445275563

The "hot_end" force is not "close to zero" in inverse dynamics and the actuators' forces do not match the previous (correct) values.

[v-r-a]

Wow, you seem to be a mjSpec power user!

There seem to be a lot contacts in the model (adding unintended internal constraint forces): the three small and long cylinders hitting the central cylinder. You may want to disable contacts altogether with contype=0 and conaffinity=0 in all geometries.
I found that your simulation becomes much more stable (bringing down kp and kv by two orders) when I disabled contacts altogether.
model.opt.disableflags |= mujoco.mjtDisableBit.mjDSBL_CONTACT

Tweaked your code a bit:

# As it is

kp=900
kv=50

# As it is

# Static simulation with applied force: forces equilibrium
data=mujoco.MjData(model)

print(spec.to_xml())
model.opt.disableflags |= mujoco.mjtDisableBit.mjDSBL_CONTACT

# Start an instance of mujoco viewer
with mujoco.viewer.launch_passive(model, data) as viewer:

        # Simulation loop
        while viewer.is_running():
            simstart = data.time

            # Step for 1/60 sec
            while data.time - simstart < 1 / 60:
                mujoco.mj_step(model, data)
            
            viewer.sync()
            sleep(0.014)

        viewer.close()

Overall there seem to be a lot of contacts and constraints which could be cut down to make the simulation more stable.

Thoughts on cutting down the DOFs (and hence lesser constraints) :

• using three orthogonal passive prismatic joints to connect the hotend to the world
• using hinges instead of spherical joints after the prismatic actuators

[tturpin]

Thanks for having a look at my model !

There seem to be a lot contacts in the model (adding unintended internal constraint forces)

In fact those contacts do not occur in the intended reachable space, and the initialization takes care of that:

initialize_pos(data,pos)
set_pos(data,pos)

with pos=[0,0,100] is within acceptable range. Sorry, I should have stressed this. But I will make sure to disable contacts as you suggest, this is simpler than shortening the rods to avoid contacts as I did.

About kv/kp, dividing them by 100 has unwanted effects: It means that the actuators are 100 times weaker so in presence of a 1000mN applied force (or a 5m/s² acceleration), the position is quite different from the intended one, and the results are less accurate. If I tune parameters, I guess I have to consider the relationship between kv/kp, solref, timestep, the model's mass and acceleration amplitude (or the applied force in static simulation), and the number of timesteps that I can afford to compute (since I run the simulation many times at different positions and acceleration directions).

Thoughts on cutting down the DOFs (and hence lesser constraints) :

I will think more about DOFs. For the prismatic actuators, the spherical joint does not add a DOF in the system as a whole, because of equality constraints and because the joint between a rod and its "small rod" is a hinge. But it is an additional DOF in the kinematic tree. I can try to replace it by some kind of universal joint. I hope that mujoco doesn't mind redundant constraints.

For the hot-end, I will try to improve, but if I disable all rotations w.r.t. the world, I'm worried about getting correct values for the horizontal forces at the top ball joint and bottom ball joint (hot-end's low gravity center is an important aspect).

Wow, you seem to be a mjSpec power user!

Writing the model in XML is just too painful, impossible to parameterize. It's not that I love Python ;-)