search

google-deepmind / mujoco

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

A resistiv torque on joint #1600

Closed Alesel-9 closed 7 months ago

Alesel-9 commented 7 months ago

Hello im asking this question since i didn't understand the answer given in issue#1095

Similarly to that issue im trying to find a way to see how much torque is exerted on a hinge type joint. The reason im trying to see this is to use a general type actuator to produce a resistiv torque as to simulate a damping moment. For clarity see this model:


 <worldbody>
    <geom type="plane" size="0 0 .01" material="grid" friction="1 1 0.1"/>

    <body name="main" pos="0 0 4" euler="-0 0 0"  >
      <freejoint/>
      <geom class="body" rgba=" 0.2 0.3 0 1" />
      <inertial mass="10900" pos="0.3 0 0.8" diaginertia="0.000734064 0.000468438 0.000398719"/>
      <light name="top light" pos="0 0 2" mode="trackcom" diffuse=".4 .4 .4"/>
      <!--Pendel Arm-->
      <body name="parm1" pos="-2.8 -1.2 0.5" euler="0 45 0 ">
          <site name="fls" pos="0.4 0 0"/>
          <joint name="jparm1" type="hinge"  axis="0 1 0" limited="true" range="-45 45"  stiffness="17000"  springref="15" />
          <geom name = "parm1" type="capsule" fromto="0 0 0 0.7 0 0" size="0.07" rgba="0.5 0.2 0 1"/>
          <!--Wheel -->
          <body pos="0.7 -0.1 0" euler="90 0 0"  >
              <joint name="jw1" type="hinge" axis="0 0 1" frictionloss="10" actuatorfrcrange="-0.5 0.5"/>
              <geom name = "w1" type="cylinder"  fromto="0 0 0 0 0 0.25" size="0.35" rgba=".05 .2 .1 1" friction="1 1 0.1"/>
          </body>
      </body>
  </worldbody>

  <actuator>
      <general name="damper" joint="jparm1" ctrlrange="0 15000" />
      <velocity name= "move" joint="jw1" ctrlrange="0 50"/>
  </actuator>`

My goal is to see the torque that is exerted upon the joint and then apply a counter torque from the general acuator.

My question is to understand what the data.joint("parm1").qfrc_constraint and data.joint("parm1").qfrc_smooth actually gives me. I've looked through the documentation but it is only mention sparsly what it actually shows, so here I am. Im working in python. Please ask me to clarify if i didn't explain properly

Edit: This is what i get when I try to identify what they are:

import mujoco as mj
model = mj.MjModel.from_xml_path('model.xml')
data=mj.MjData(model)
mj.mj_resetData(model,data)
data.actuator("damper").ctrl=9573
print('qfrc_smooth=',data.joint("jparm1").qfrc_smooth[0],'  qfrc_constraint=',data.joint("jparm1").qfrc_constraint[0])
print(data.actuator("torque1").ctrl[0])
for i in range(300):
    mj.mj_step(model, data)
print('qfrc_smooth=',round(data.joint("jparm1").qfrc_smooth[0],2),'  qfrc_constraint=',round(data.joint("jparm1").qfrc_constraint[0],1))

And the resulting output:

qfrc_smooth= 0.0   qfrc_constraint= 0.0
9573.0
qfrc_smooth= 675.57   qfrc_constraint= -452.1
yuvaltassa commented 7 months ago

The reason im trying to see this is to use a general type actuator to produce a resistiv torque as to simulate a damping moment.

You know about dampers, yes?

qfrc_{smooth, constraint} are the smooth forces and constraint forces, respectively. You seem to also want the forces applied by the other actuator, so look at qfrc_actuator or actuator_force. It might help to read the file engine_forward.c, this is the high level pipeline.

Alesel-9 commented 7 months ago

@yuvaltassa Well yes, I do know about dampers but they are prportional to the velocity of the joint and that is not what I seek to do. I looked at engine_forward.c but it didn't tell me what it calculates, it actually made be more confused since at line 951 there is a summation of the the two value, which as seen in my 'Edit' section above would be equal to 0.

What do you mean when you say 'smooth forces and constraint forces'? And how do they relate to the torque of the joint?

Alesel-9 commented 7 months ago

After testing around a bit more i have made the following observations:

qfrc_smooth is the total torque that the joint imposes on itself. For example, in the model above there is a stiffness property defined in the joint, the tourque applied by that spring on the joint is what shows up in qfrc_smooth . Additionally if there are other torque inducing properties applied on the joint they would too be summed up into qfrc_smooth and displayed as an equivalent torque.

qfrc_constraint is essentially the opposite. It displays the external torque applied to the joint. For example; in the model above there is a moment arm extanding from the joint and then makes contact with the ground. The normal force that the ground exerts at the end of the moment arm is converted to torque around the joint, but in the opposite direction to the torque that the spring induces.

That is why, at equlibrium the two values qfrc_smooth and qfrc_constraint would be equal, allbeit opposite sign. Becuse (in the context of the above model) the sum of the internal torque (the spring pressing the moment arm into the ground) and the external torque (the normal force on the moment arm) would be 0 since they have reached a steady state and pushing the joint to rotate in different directions.

Im I understanding this correctly? If not, where am I wrong?
@yuvaltassa

yuvaltassa commented 7 months ago

More or less, yes.

Here is the definition of qfrc_smooth: the sum of bias (Coriolis, centripetal, gravitational) forces, passive (spring, damper, fluid) forces and actuation forces (both modeled with native actuators and directly applied).

qfrc_constraint is the projection of constraint forces onto joint space. These include contacts, equalities and joint limits. Here is where these two components are added together to get the total force $f$ and the equation $Ma=f$ is solved for the acceleration $a$.

Alesel-9 commented 7 months ago

Then if I were to use the sum of qfrc_smooth and qfrc_constraint to set a torque in the opposite sign, wouldn't there be a contridiction since I'm accounting for the actuator twice. Once from the qfrc_smooth variable and once when im changing the actuator, like a positv feedback loop? Or am i missunderstaning what qfrc_smooth containes? @yuvaltassa

yuvaltassa commented 7 months ago

Yes. That's what qfrc_applied is for: applied actuation forces that are outside of the standard actuation framework.

Alesel-9 commented 7 months ago

@yuvaltassa Can you provide an example for qfrc_applied or explain in a different way. I understand that there is a contrediction by using the sum of qfrc_smooth and qfrc_constraint to set a resistiv torque, and therefore i need to use qfrc_applied . But I don't understand what qfrc_applied gives me or what it represents.

When i tried the following'.

import mujoco as mj
model = mj.MjModel.from_xml_path('MAIN_Scene_v2.xml')
data=mj.MjData(model)
mj.mj_resetData(model,data)
print(data.joint('parm1'))
data.actuator('damper').ctrl=3000

for i in range(1000):
    mj.mj_step(model, data)
print(data.joint('parm1'))

I got this:


<_MjDataJointViews
  cdof: array([[0., 0., 0., 0., 0., 0.]])
  cdof_dot: array([[0., 0., 0., 0., 0., 0.]])
  id: 1
  name: 'jnt_pendel_arm_1'
  qLDiagInv: array([0.])
  qLDiagSqrtInv: array([0.])
  qacc: array([0.])
  qacc_smooth: array([0.])
  qacc_warmstart: array([0.])
  qfrc_actuator: array([0.])
  qfrc_applied: array([0.])
  qfrc_bias: array([0.])
  qfrc_constraint: array([0.])
  qfrc_inverse: array([0.])
  qfrc_passive: array([0.])
  qfrc_smooth: array([0.])
  qpos: array([0.])
  qvel: array([0.])
  xanchor: array([0., 0., 0.])
  xaxis: array([0., 0., 0.])
>
<_MjDataJointViews
  cdof: array([[0., 0., 0., 0., 1., 0.]])
  cdof_dot: array([[0., 0., 0., 0., 0., 0.]])
  id: 1
  name: 'jnt_pendel_arm_1'
  qLDiagInv: array([0.02269422])
  qLDiagSqrtInv: array([0.15064602])
  qacc: array([-0.95754868])
  qacc_smooth: array([232.76126695])
  qacc_warmstart: array([-0.95754868])
  qfrc_actuator: array([2999.13293061])
  qfrc_applied: array([0.])
  qfrc_bias: array([-495.68075765])
  qfrc_constraint: array([-12582.45606242])
  qfrc_inverse: array([0.])
  qfrc_passive: array([9099.06195486])
  qfrc_smooth: array([12593.87564312])
  qpos: array([-0.10043278])
  qvel: array([0.86515429])
  xanchor: array([ 1.21370952, -3.00947769,  1.0609724 ])
  xaxis: array([-9.99997867e-01, -1.87180061e-03,  8.73684102e-04])
>

The issue is that qfrc_applied is zero in both cases so I don't understand what it shows and how it's supposed to be used?

yuvaltassa commented 7 months ago

qfrc_applied if for the user (you). Just write into it whatever applied force you want.

Alesel-9 commented 7 months ago

@yuvaltassa So just for confirmention; I use qfrc_smooth and qfrc_constraint to measure the total torque in the joint and then use qfrc_applied to set the resulting resistiv torque? Like this, if the total torque in the joint is +3014: data.joint.qfrc_applied('parm1')=-3014 and this will apply a 3014 Nm torque in the opposite direction to the joint 'parm1', free of contradictions and such?

yuvaltassa commented 7 months ago

Yes

Alesel-9 commented 7 months ago

@yuvaltassa I've tried what you suggested with qfrc_applied but it seems like it has no impact on the simulation whatsoever. Sure it seems like it changes the total torque in the joint when I look at the numbers qfrc_smooth and qfrc_constraint but when I render it, it shows no impact at all regardless on how high of an input i give. What I did notice though was that if I give to large of a value in qfrc_applied for example: -2*sum_torque_joint it quickly gets unstable and starts showing that I have 1e11 Nm torque in the joint. Again without showing any respons in the render, like the input I gave didn't even register.