google-deepmind / mujoco

Multi-Joint dynamics with Contact. A general purpose physics simulator.
https://mujoco.org
Apache License 2.0
7.84k stars 783 forks source link

Incorrect acceleration values for pulley system #1869

Closed wykang2 closed 3 weeks ago

wykang2 commented 1 month ago

Hi,

I'm a student that is new to MuJoCo and I'm trying to use MuJoCo to simulate a pulley system.

More specifically, I am looking to model problem 34c. from image below:

Screen Shot 2024-08-01 at 1 32 29 AM

The expected values for the acceleration are also given in the textbook as shown:

Screen Shot 2024-08-01 at 1 33 11 AM

I'm looking for some help with my acceleration measurements for my pulley system, which are inconsistent with the values in the solution.

Here is my model for problem 34c:

minimal XML ```XML ```

Here is a video, illustrating what the model looks like when ran:

https://github.com/user-attachments/assets/8ebb7e58-0298-442c-82d3-3e948472657d

In the model, I used equality constraints on the tendons to keep their lengths constant.

To model the acceleration of each mass, I used two accelerometers and plotted out the accelerations over time.

Here is the acceleration over time graph for the right mass (initially moving downwards):

accel_right

Here is the acceleration over time graph for the left mass (initially moving upwards):

accel_left

I mainly care about the accelerations of the pulley from the start of the simulation to when the right mass reaches its low point and the left mass reaches its high point (where a real pulley system would stop moving). This corresponds to around the 0 second to 1 second sections of the graphs above.

My issue is that the not only do the magnitudes of the values seem incorrect according to the answer to the problem above, the accelerations keep changing as well.

Any help with solving or identifying possible issues would be greatly appreciated.

Balint-H commented 1 month ago

EDIT:

Apologies for missing something obvious... The actuator you added is always active, and acts as a spring in your system, which is one of the causes for oscillations. The others are the angled tendons. In terms of the offset in your readings, keep in mind that accelerometers include the gravity as an offset (like real accelerometers). After a short initial transient period (a couple of frames), the reworked model below gives exactly 1/3 g and 2/3 g accelerations!

Reworked xml: ``` xml ```
Original answer: I can only provide a partial answer: The angle of the tendon will affect the accelerations, so you should align them so they are vertical. Also, no hinge joint needed in the pulley object itself. And its better to model the bottom pulley/object B as a rigid object instead of two separate connected by a pulley. Lastly, the accelerometer sensors are biased by the gravity, you need to subtract 9.81 from the vertical component (depending on the accelerometer orientation of course). What I can't give an answer for is the oscillatory behaviour, which does not purely arise from "bouncing off" the limits, and changing angles of the tendons (which can also cause it). Could someone help clear this up? ![image](https://github.com/user-attachments/assets/5dc939b5-dc43-405c-b06f-7183ccb54825) Why is the above configuration in equilibrium? The tendon limit force measured in this state is 2*g, which is appropriate to support the right object, but due to advantage, the left load should not be enough to support it.
mihirp1998 commented 1 month ago

great thank you so much @Balint-H !

Do u know if there is a easy way to visualize these constraint violation forces that get generated by mujoco. So that it can help debug in the future more easily?

For instance we didn't know having an active actuator that is not being used results in a spring like force.

Balint-H commented 1 month ago

data->qfrc_actuator holds the force actuators put on degrees of freedom. Since you used a position actuator, it will always apply a non-zero proportional force on your system when it deviates from the specified position. This is not a hidden effect of the actuator, this is what its used for! You could change its gearing or gains to zero to disable it temporarily.

This effect was not caused by constraint violations, those were working in your scene as intended. But for future reference:

Constraint forces are stored in data->qfrc_constraint and data->efc_force (in joint and constraint space respectively)

mihirp1998 commented 1 month ago

great thanks :) !

wykang2 commented 1 month ago

@Balint-H Thank you so much for the help! The new system you created seemed to be working on similar systems. However, the limitation I am currently facing is due to the mass being attached to the pulley for the bottom pulley. I am currently trying to create a similar system, but with another pulley attached to the bottom pulley instead of a mass.

Here is an image of the pulley system I am talking about: vONv

Below is some minimal xml for the pulley system on the leftside of the image above based on the code you provided in your original reply:

minimal XML ```XML ```

The main issue is that in order to create this type of system, the pulley mass has to be non-zero since we removed the mass attached to the bottom pulley. However, doing so (possibly along with other factors), will cause the values to be incorrect. Are there any workarounds or solutions for this? In other words, is there any way to create a massless bottom pulley without directly attaching the weight to it so that our system can be extended to my example attached?

Thank you so much.

Balint-H commented 1 month ago

As far as the first system you show in your figure, attaching the left mass to the centre of P1 via a rope or another rigid connection makes no difference, it is only a matter of visual notation for the illustration. Therefore you can treat pulley P1 and the left mass as a single object and you don't have a zero inertia problem.

With your second system shown I agree it is trickier. I'd start with setting the P2 geom's density to a tiny amount, increasing it with an order of magnitude if it is unstable. (Alternatively you can set the density to zero and add an inertial component.

mihirp1998 commented 1 month ago

Thanks @Balint-H for all the help!

EDIT:

Apologies for missing something obvious... The actuator you added is always active, and acts as a spring in your system, which is one of the causes for oscillations. The others are the angled tendons. In terms of the offset in your readings, keep in mind that accelerometers include the gravity as an offset (like real accelerometers). After a short initial transient period (a couple of frames), the reworked model below gives exactly 1/3 g and 2/3 g accelerations!

Reworked xml: Original answer:

With your reworked xml we get the following plots, we get accurate values for acceleration and tension after some point in time, as you pointed out - 1/3 g and 2/3 g. However there is an initial bump (marked in the red box) for the first few milliseconds in Tension and accelerations.

I was wondering what causes this bump, is it becoz the tension force only gets activated when there is a change in length in the tendon which happens after few milliseconds?

Screenshot 2024-08-19 at 10 52 55 AM
Balint-H commented 1 month ago

Hello,

Yes, that transient period is because of the softness of constraints in the engine (essentially modelling the tendon's limited elasticity). You can explore the effects of manipulating the solref and solimp parameters which governs the impedance of the constraints (see: https://mujoco.readthedocs.io/en/latest/modeling.html#solver-parameters)! You can make it harder if you need a shorter transient period.

mihirp1998 commented 1 month ago

i see thanks!