how to reduce the freedom degrees ? #2065

starimpact · 2024-09-19T01:34:18Z

starimpact
Sep 19, 2024

The feature, motivation and pitch

freejoint
free joint has 6 degrees(x,y,z,rx,ry,rz), but in real robot, i can only get rx,ry,rz three information, how can i remove xyz?
constraint joints
in my quad-bar linkage system, there is only one joint is actuated, the motion of rest joints are constrained by the linkage system. so practically only one freedom is significant, but mujoco gives 4(or 3 after using connect) freedoms. how to reduce 4 to 1?

Alternatives

No response

Additional context

No response

Answered by yuvaltassa

Sep 25, 2024

Nobody said writing good estimators is easy! These guys evidently know what they're doing https://arxiv.org/abs/2409.10469
Did you try using joint equality constraints? This is a good alternative to linkages if dof count is what you care about.

View full answer

yuvaltassa · 2024-09-24T22:20:59Z

yuvaltassa
Sep 24, 2024
Maintainer

2: A single dof for 4-bar linkages is a valuable feature that is not impossible (though not easy) and we could consider that. Right now I'm not sure the benefit would justify the effort as linkages with constraints are not that hard to use and generally get the job done.
1: This makes no sense?

7 replies

starimpact Sep 25, 2024
Author

thanks very much for your response.
the start point of my questions is from that I think fewer DoF will make more stable system solver.

hmmm, I know that I should use Kalman Filter to estimate states which the real scenario needs it. But it is hard to use Kalman Filter in mujoco_mpc，it always makes simulation more crazy than without using it . such as quadruped and my robot, but not for cartpole. I am still working on it.
the following leg of robot is a linkage example which just wants one dof (on the thigh or hip, denote with a green arrow, the another point which red arrow pointed is the equality/connect object.):

video demo

robot xml file:

<mujoco model="legwheel2_expand">
    <include file="./common/skybox.xml"/>
    <include file="./common/visual.xml"/>
    <include file="./common/materials.xml"/>
    <visual>
        <global offwidth="1920" offheight="1080" />
    </visual>
    <option gravity="0 0 -9.8" timestep="0.002" wind="0 0 0" integrator="Euler" />
    <default>
        <default class="leg">
            <!-- <joint armature="0.001" damping="1" stiffness="0" /> -->
            <!-- <joint damping="0.1" stiffness="0" armature="0.001" limited="true" solimplimit="0 .99 .01" range="-90 90" /> -->
            <!-- <joint damping=".01" stiffness="1" armature="0.1" /> -->
            <joint damping="1" stiffness="1" armature="0.1" />
        </default>
        <default class="wheel">
            <joint armature="0.01" damping="0.01" stiffness="0." />
        </default>
    </default>
    <worldbody>
        <geom name="floor" pos="0 0 -.001" size="10 10 .001" type="plane" material="grid"/>
        <body name="root" pos="0 0 0.15927" euler="0 0 45">
            <freejoint name="root_joint" />
            <site name="root" pos="0 0 0.03" size="0.001" rgba="0 0 0 1"/>
            <geom size="0.05 0.05 0.03" type="box" rgba="0 0 0.8 1" priority="1"  material="self"/>
            <body name="nose"  pos="0.047 0 0" >
                <geom name="nose" size="0.03 0.03 0.0177" type="box" rgba="0 0.8 0.8 1" priority="1"  material="self" />
                <site name="nose" pos="0.03 0 0" size="0.001" rgba="1 0 0 1"/>
            </body>
            <body name="thigh_left" pos="-0.031 0.06 -0.04" euler="0 40 0" >
                <joint name="thigh_left_joint" type="hinge" pos="0 0 0.05" axis="0 1 0" class="leg" />
                <geom size="0.01 0.01 0.055" type="box" rgba="0 0.8 0. 0.5" priority="1"  material="self"/>
                <body name="calf_left" pos="0.03 0. -0.05" euler="0 -90 0">
                    <joint name="calf_left_joint" type="hinge" pos="0 0 0.03" axis="0 1 0" class="leg" />
                    <geom size="0.01 0.01 0.06" type="box" rgba="0.8 0.8 0. 0.5"  priority="1"  material="self"/>
                    <body name="rod_left" pos="0.045 0 0.066" euler="0 75 0" >
                        <joint name="rod_left_joint" type="hinge" pos="0 0 -0.045" axis="0 1 0" class="leg" />
                        <geom size="0.003 0.009 0.05" type="box" rgba="0 0.0 0.0 0.5" priority="1"  material="self"/>
                    </body>
                    <body name="wheel_left" pos="0. 0.02 -0.05" euler="90 0 0">
                        <joint name="wheel_left_joint" type="hinge" pos="0 0 0" axis="0 0 -1" class="wheel" />
                        <geom size="0.03 0.01" type="cylinder" rgba="0.8 0 0 1" priority="1" material="self"/>
                        <geom size="0.001 0.02 0.001" type="box" rgba="0.8 0.8 0.8 1"  priority="1"  material="self" pos="0 0 -0.01" />
                    </body>
                </body>
            </body>
            
            <body name="thigh_right" pos="-0.031 -0.06 -0.04" euler="0 40 0" >
                <joint name="thigh_right_joint" type="hinge" pos="0 0 0.05" axis="0 1 0" class="leg" />
                <geom size="0.01 0.01 0.055" type="box" rgba="0 0.8 0. 0.5"  priority="1"  material="self"/>
                <body name="calf_right" pos="0.03 0. -0.05" euler="0 -90 0">
                    <joint name="calf_right_joint" type="hinge" pos="0 0 0.03" axis="0 1 0" class="leg" />
                    <geom size="0.01 0.01 0.06" type="box" rgba="0.8 0.8 0. 0.5"  priority="1"  material="self"/>
                    <body name="rod_right" pos="0.045 0 0.066" euler="0 75 0" >
                        <joint name="rod_right_joint" type="hinge" pos="0 0 -0.045" axis="0 1 0" class="leg" />
                        <geom size="0.003 0.009 0.05" type="box" rgba="0 0.0 0.0 0.5"  priority="1"  material="self"/>
                    </body>
                    <body name="wheel_right" pos="0. -0.02 -0.05" euler="90 0 0">
                        <joint name="wheel_right_joint" type="hinge" pos="0 0 0" axis="0 0 -1" class="wheel" />
                        <geom size="0.03 0.01" type="cylinder" rgba="0.8 0 0 1" priority="1"  material="self"/>
                        <geom size="0.001 0.02 0.001" type="box" rgba="0.8 0.8 0.8 1"  priority="1"  material="self" pos="0 0 0.01" />
                    </body>
                </body>
            </body>
        </body>
    </worldbody>
    <equality>
        <connect body1="root" body2="rod_left" anchor="-0.043 0.06 0.02"/>
        <connect body1="root" body2="rod_right" anchor="-0.043 -0.06 0.02"/>

        <!-- <weld body1="thigh_left" body2="calf_left" />
        <weld body1="calf_left" body2="rod_left" />
        <weld body1="thigh_right" body2="calf_right" />
        <weld body1="calf_right" body2="rod_right" /> -->
    </equality>
    <actuator>
        <!-- <motor name="thigh_left_motor" joint="thigh_left_joint" gear="1" ctrllimited="true" ctrlrange="-1.0 1.0" />
        <motor name="thigh_right_motor" joint="thigh_right_joint" gear="1" ctrllimited="true" ctrlrange="-1.0 1.0" /> -->
        <position name="thigh_left_pos" joint="thigh_left_joint" kp="10" kv="0" ctrllimited="true" ctrlrange="-1.0 1.0" />
        <position name="thigh_right_pos" joint="thigh_right_joint" kp="10" kv="0" ctrllimited="true" ctrlrange="-1.0 1.0" />
        <!-- <position name="thigh_left_pos" joint="thigh_left_joint" kp="10" gear="10"  />
        <position name="thigh_right_pos" joint="thigh_right_joint" kp="10" gear="10" /> -->
        
        <motor name="wheel_left_motor" joint="wheel_left_joint" gear="1" ctrllimited="true" ctrlrange="-1.0 1.0" />
        <motor name="wheel_right_motor" joint="wheel_right_joint" gear="1" ctrllimited="true" ctrlrange="-1.0 1.0" />
    </actuator>
    <!-- <sensor>
        <gyro name="gyro" site="root"/>
        <accelerometer name="accel" site="root"/>

        <actuatorpos name="thigh_left_pos" actuator="thigh_left_pos" />
        <actuatorpos name="thigh_right_pos" actuator="thigh_right_pos" />
        <actuatorvel name="thigh_left_vel" actuator="thigh_left_pos" />
        <actuatorvel name="thigh_right_vel" actuator="thigh_right_pos" />

        <actuatorpos name="wheel_left_pos" actuator="wheel_left_motor" />
        <actuatorpos name="wheel_right_pos" actuator="wheel_right_motor" />
        <actuatorvel name="wheel_left_vel" actuator="wheel_left_motor" />
        <actuatorvel name="wheel_right_vel" actuator="wheel_right_motor" />
    </sensor> -->
    <keyframe>
        <key name="key0" qpos="0.01466  0.       0.08147  0.99933 -0.      -0.03672 -0.       0.24566
 -0.40566  0.42759  0.66486  0.24566 -0.40566  0.42759  0.66486" ctrl="-0.2 -0.2 0.     0.    " />
    </keyframe>
</mujoco>

yuvaltassa Sep 25, 2024
Maintainer

Nobody said writing good estimators is easy! These guys evidently know what they're doing https://arxiv.org/abs/2409.10469
Did you try using joint equality constraints? This is a good alternative to linkages if dof count is what you care about.

Answer selected by yuvaltassa

starimpact Sep 30, 2024
Author

I tried the equality/joint , but mujoco still gives out the original number of DoF(14), but I want 10.

        <joint joint1="calf_left_joint" joint2="thigh_left_joint" polycoef="0 -1.6 0 0 0" />
        <joint joint1="rod_left_joint" joint2="thigh_left_joint" polycoef="0 1.6 0 0 0"  />
        <joint joint1="calf_right_joint" joint2="thigh_right_joint" polycoef="0 -1.6 0 0 0"  />
        <joint joint1="rod_right_joint" joint2="thigh_right_joint" polycoef="0 1.6 0 0 0"  />

yuvaltassa Sep 30, 2024
Maintainer

Equalities don't change the number of dofs.

The point is that by using joint equalities you can remove one branch of you linkage, since the relationship between the dofs is maintained by the joint equality rather than the linkage.

starimpact Sep 30, 2024
Author

I do not understand "remove one branch of", will it reduce the DoF? How to do it?

starimpact Sep 30, 2024
Author

I hacked into kalman.cc of mujoco-mpc, print correction_ matrix value (which is 14 DoF, 28 dimension, ndstate_=28):

  // -- correction: (P * C') * (C * P * C' + R)^-1 * sensor_error -- //

  // tmp2 = (C * P * C' + R) \ sensor_error
  mju_cholSolve(tmp2_.data(), tmp1_.data(), sensor_error_.data(), nsensordata_);

  // correction = (P * C') * (C * P * C' + R) \ sensor_error = tmp0 * tmp2
  mju_mulMatVec(correction_.data(), tmp0_.data(), tmp2_.data(), ndstate_,
                nsensordata_);
  // print ele of correction_ into a file
  {
    std::ofstream outfile("correction_.txt", std::ios::app);
    for (int i = 0; i < ndstate_; i++) {
      outfile << i << ":" << correction_[i] << std::endl;
    }
    outfile << std::endl;
    outfile.close();
  }

its value will become abnormal after some iterations , such as the following:

0:-15503.6
1:-12354.1
2:28072.5
3:1407.15
4:-1054.74
5:-5383.04
6:-41.1953
7:-147.336
8:-22.0703
9:-8.98682
10:-12.5
11:1
12:-159
13:-0.744141
14:-1.55037e+06
15:-1.23542e+06
16:2.80724e+06
17:2.52128e+06
18:307968
19:1264
20:-3943.75
21:-14086.5
22:-2587.5
23:-892.438
24:-384
25:2432
26:-17216
27:50.5

I speculate the redundant DoFs is not good for solver.

how to reduce the freedom degrees ? #2065

Uh oh!

starimpact Sep 19, 2024

The feature, motivation and pitch

Alternatives

Additional context

Replies: 1 comment · 7 replies

Uh oh!

yuvaltassa Sep 24, 2024 Maintainer

Uh oh!

Uh oh!

starimpact Sep 25, 2024 Author

Uh oh!

yuvaltassa Sep 25, 2024 Maintainer

Uh oh!

starimpact Sep 30, 2024 Author

Uh oh!

Uh oh!

yuvaltassa Sep 30, 2024 Maintainer

Uh oh!

Uh oh!

starimpact Sep 30, 2024 Author

Uh oh!

Uh oh!

starimpact Sep 30, 2024 Author

starimpact
Sep 19, 2024

Replies: 1 comment 7 replies

yuvaltassa
Sep 24, 2024
Maintainer

starimpact Sep 25, 2024
Author

yuvaltassa Sep 25, 2024
Maintainer

starimpact Sep 30, 2024
Author

yuvaltassa Sep 30, 2024
Maintainer

starimpact Sep 30, 2024
Author

starimpact Sep 30, 2024
Author