Assistance with Control system in MuJoCo #2007

kankanzheli · 2024-02-08T07:46:44Z

kankanzheli
Feb 8, 2024

Dear MuJoCo Support Team,

I am currently working on a project involving a robotic simulation in MuJoCo, where I aim to control the robot's joints based on target positions.
The version of mujoco is 3.1.2
The version of python is 3.8.17
I've tried using velocity controllers, position controllers, and even modifying qpos directly to control the robotic arm. However, all controllers have resulted in very poor control outcomes. Below is the XML file of the robot used in my research.

<?xml version="1.0"?>
<mujoco>
  <size nstack="300000000" /> 
  <option gravity="0 0 0" solver="CG"/>
  <asset>
    <mesh name=......
  </asset>

  <worldbody>
    <body name="base_body">
      <body name="frame">
        <inertial diaginertia="222.35 143.37 209.67" mass="1e-2" pos="0 0 0.66"/>
        <geom name="frame" type="mesh" mesh="frame" pos="0 0 0" contype="0" conaffinity="0"/>
        <body name="Y_linear" pos="0 0 1.137">
          <inertial diaginertia="1e-4 1e-4 1e-4" mass="1e-2" pos="0.01 -0.02 -0.1"/>
          <geom name="Y_linear" type="mesh" mesh="Y_linear" pos="0 0 0" contype="1" conaffinity="0"/>
          <joint name="Y_linear_joint" type="slide" axis="0 -1 0" pos="0 0 0" damping="0.1" frictionloss="0.1" limited="true" range="-0.3455 0.3455"/>
          <body name="X_linear" pos="0 0 -0.002">
            <inertial diaginertia="1e-4 1e-4 1e-4" mass="1e-2" pos="-0.04 -0.01 -0.2"/>
            <geom name="X_linear" type="mesh" mesh="X_linear" pos="0 0 0" contype="1" conaffinity="0"/>
            <joint name="X_linear_joint" type="slide" axis="1 0 0" pos="0 0 0" damping="0.1" frictionloss="0.1" limited="true" range="-0.225 0.281"/>
            <!-- Arm -->
            <body name="Z1_linear" pos="0 0 0">
              <inertial diaginertia="1e-4 1e-4 1e-4" mass="1e-2" pos="-0.04 0 -0.24"/>
              <geom name="Z1_linear" type="mesh" mesh="Z1_linear" pos="0 0 0" contype="1" conaffinity="0"/>
              <joint name="Z1_linear_joint" type="slide" axis="0 0 -1" pos="0 0 0" damping="0.1" frictionloss="0.1" limited="true" range="0 0.276"/>
              <body name="Z2_linear" pos="0 0 0">
                <inertial diaginertia="1e-4 1e-4 1e-4" mass="1e-2" pos="-0.01 0 -0.25"/>
                <geom name="Z2_linear" type="mesh" mesh="Z2_linear" pos="0 0 0" contype="1" conaffinity="0"/>
                <joint name="Z2_linear_joint" type="slide" axis="0 0 -1" pos="0 0 0" damping="0.1" frictionloss="0.1" limited="true" range="0 0.325"/>
                <body name="Z3_linear" pos="0 0 0.207">
                  <inertial diaginertia="1e-4 1e-4 1e-4" mass="1e-2" pos="0 0 0"/>
                  <geom name="Z3_linear" type="mesh" mesh="Z3_linear" pos="0 0 0.207" contype="1" conaffinity="0"/>
                  <joint name="Z3_linear_joint" type="slide" axis="0 0 -1" pos="0 0 0" damping="0.1" frictionloss="0.1" limited="true" range="0 0.415"/>
                  <!-- Joint -->
                  <body name="yaw"  pos="0 0 -0.657">
                    <inertial diaginertia="1e-4 1e-4 1e-4" mass="1e-2" pos="-0.01 -0.01 0.05"/>
                    <geom name="yaw" type="mesh" mesh="yaw" pos="0 0 0" contype="1" conaffinity="0"/>
                    <joint name="yaw_joint" type="hinge" axis="0 0 -1" pos="0 0 0" damping="0.1" frictionloss="0.1" limited="true" range="-180 180"/>
                    <body name="pitch" pos="0 0 0">
                      <inertial diaginertia="1e-4 1e-4 1e-4" mass="0.01" pos="0.05 0.01 0"/>
                      <geom name="pitch" type="mesh" mesh="pitch" pos="0 0 0" contype="1" conaffinity="0"/>
                      <joint name="pitch_joint" type="hinge" pos="0 0 0" axis="0 -1 0" damping="0.1" armature="0.01" frictionloss="0.1" limited="true" range="-180 30" />
                      <!-- harvest_tool -->
                      <body name="roll" pos="0.114 0 0">
                        <inertial diaginertia="1e-4 1e-4 1e-4" mass="0.01" pos="0.08 -0.02 0.01"/>
                        <geom name="roll" type="mesh" mesh="roll" pos="0 0 0" contype="1" conaffinity="0"/>
                        <joint name="roll_joint" type="hinge" pos="0 0 0" axis="1 0 0" damping="0.1" armature="0.01" frictionloss="0.1" limited="true" range="-180 180" />
                      </body>
                    </body>
                    <body name="joint5" pos="0.05225 0.0 0.06807">
                      <inertial diaginertia="1e-4 1e-4 1e-4" mass="0.01" pos="0.0 -2.628e-02 5.7e-04"/>
                      <geom name="joint5" type="mesh" mesh="joint1" pos="0 0 0" contype="1" conaffinity="0"/>
                      <joint name="joint5_joint" type="hinge" pos="0 0 0" axis="0 1 0" damping="0.1" armature="0.01" frictionloss="0.1" limited="true" range="-25 90"/>
                      <body name="joint6" pos="0.069 0.0 0.0">
                        <inertial diaginertia="1e-4 1e-4 1e-4" mass="0.01" pos="8.26e-03 -1.4481e-01 3.7e-04"/>
                        <geom name="joint6" type="mesh" mesh="joint2" pos="0 0 0" contype="1" conaffinity="0"/>
                        <joint name="joint6_joint" type="hinge" pos="0 0 0" axis="0 0 1" damping="0.1" armature="0.01" frictionloss="0.1" limited="true" range="-115 115"/>
                        <body name="joint3" pos="0.149 0.0 0.0">
                          <inertial diaginertia="1e-4 1e-4 1e-4" mass="0.01" pos="1.507e-01 0.0 3.8e-04"/>
                          <geom name="joint3" type="mesh" mesh="joint3" pos="0 0 0" contype="1" conaffinity="0"/>
                          <joint name="joint3_joint" type="hinge" pos="0 0 0" axis="0 0 1" damping="0.1" armature="0.01" frictionloss="0.1" limited="true" range="-115 115"/>
                          <body name="joint4" pos="0.149 0.0 0.0">
                            <inertial diaginertia="1e-4 1e-4 1e-4" mass="0.01" pos="4.4206755e-02 3.6839985e-07 8.9142216e-03"/>
                            <geom name="joint4" type="mesh" mesh="joint4" pos="0 0 0" contype="1" conaffinity="0"/>
                            <joint name="joint4_joint" type="hinge" pos="0 0 0" axis="0 0 1" damping="0.1" armature="0.01" frictionloss="0.1" limited="true" range="-115 115" />
                          </body>
                        </body>
                      </body>
                    </body>
                  </body>
                </body>
              </body>
            </body>
          </body>
        </body>
      </body>
    </body>
  </worldbody>
  <actuator>
    <position name="Y_linear_position" joint="Y_linear_joint" ctrllimited="true" ctrlrange="-0.3455 0.3455" kp="10" kv="1"/>
    <position name="X_linear_position" joint="X_linear_joint" ctrllimited="true" ctrlrange="-0.225 0.281" kp="10" kv="1"/>
    <position name="Z1_linear_position" joint="Z1_linear_joint" ctrllimited="true" ctrlrange="0 0.276" kp="10" kv="1"/>
    <position name="Z2_linear_position" joint="Z2_linear_joint" ctrllimited="true" ctrlrange="0 0.325" kp="10" kv="1"/>
    <position name="Z3_linear_position" joint="Z3_linear_joint" ctrllimited="true" ctrlrange="0 0.207" kp="10" kv="1"/>
    <position name="yaw_position" joint="yaw_joint" ctrllimited="true" ctrlrange="-3.14159 3.14159" kp="10" kv="1"/>
    <position name="pitch_position" joint="pitch_joint" ctrllimited="true" ctrlrange="-3.14159 0.523599" kp="10" kv="1"/>
    <position name="roll_position" joint="roll_joint" ctrllimited="true" ctrlrange="-3.14159 3.14159" kp="10" kv="1"/>
    <position name="joint3_position" joint="joint3_joint" ctrllimited="true" ctrlrange="-2.01 2.01" kp="10" kv="1"/>
    <position name="joint4_position" joint="joint4_joint" ctrllimited="true" ctrlrange="-2.01 2.01" kp="10" kv="1"/>
    <position name="joint5_position" joint="joint5_joint" ctrllimited="true" ctrlrange="-0.44 1.57" kp="10" kv="1"/>
    <position name="joint6_position" joint="joint6_joint" ctrllimited="true" ctrlrange="-2.01 2.01" kp="10" kv="1"/>
    <velocity name="Y_linear_velocity" joint="Y_linear_joint" kv="1"/>
    <velocity name="X_linear_velocity" joint="X_linear_joint" kv="1"/>
    <velocity name="Z1_linear_velocity" joint="Z1_linear_joint" kv="1"/>
    <velocity name="Z2_linear_velocity" joint="Z2_linear_joint" kv="1"/>
    <velocity name="Z3_linear_velocity" joint="Z3_linear_joint" kv="1"/>
    <velocity name="yaw_velocity" joint="yaw_joint" kv="1"/>
    <velocity name="pitch_velocity" joint="pitch_joint" kv="1"/>
    <velocity name="roll_velocity" joint="roll_joint" kv="1"/>
    <velocity name="joint3_velocity" joint="joint3_joint" kv="1"/>
    <velocity name="joint4_velocity" joint="joint4_joint" kv="1"/>
    <velocity name="joint5_velocity" joint="joint5_joint" kv="1"/>
    <velocity name="joint6_velocity" joint="joint6_joint" kv="1"/>
  </actuator>
</mujoco>

As I said, I have tried three to control the robot. The codes use for control is below:

def step(self, action):
        joint_names = [
            "Y_linear_joint", "X_linear_joint", "Z1_linear_joint", "Z2_linear_joint", "Z3_linear_joint", "yaw_joint",  "pitch_joint",
            "roll_joint", "joint5_joint", "joint6_joint", "joint3_joint", "joint4_joint"
        ]
        velocity_actuator_name = [
            "Y_linear_velocity", "X_linear_velocity",  "Z1_linear_velocity", "Z2_linear_velocity", "Z3_linear_velocity", "yaw_velocity", "pitch_velocity", "roll_velocity", "joint5_velocity", "joint6_velocity", "joint3_velocity", "joint4_velocity"
        ]

        current_joint_positions = np.array([self.data.qpos[mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, jn)] for jn in joint_names])
        target_positions = current_joint_positions + action
        print("the target position is",target_positions)
        print("the action is",action)
        ####control by velocity
        simulation_steps = 10 
        step_size = action / simulation_steps 
        for step in range(simulation_steps):
            step_positions = current_joint_positions + step_size*(step+1)
            for idx, joint_name in enumerate(joint_names):
                velocity_actuator_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "velocity_actuator_name")
                self.data.ctrl[velocity_actuator_id] = action[idx]
            mujoco.mj_step(self.model, self.data)

        ####control by position
        #simulation_steps = 10
        #step_size = action / simulation_steps 
        #for step in range(simulation_steps):
        #    step_positions = current_joint_positions + step_size*(step+1)
        #    print(f"Step position {step + 1}:")
        #    for idx, joint_name in enumerate(joint_names):
        #        joint_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, joint_name)
        #        self.data.ctrl[joint_id] = step_positions[idx]
        #        print(target_positions[idx])
        #    mujoco.mj_step(self.model, self.data)
        
        ####control by change qpos
        #simulation_steps = 10 
        #step_size = action / simulation_steps  
        #for step in range(simulation_steps):
        #    step_positions = current_joint_positions + step_size*(step+1)
        #    for idx, joint_name in enumerate(joint_names):
        #        joint_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, joint_name)
        #        self.data.qpos[joint_id] = step_positions[idx]
        #    mujoco.mj_step(self.model, self.data)

        print(f"Final position is:")
        joint_positions = []
        for joint_name in joint_names:
            joint_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, joint_name)
            joint_pos = self.data.qpos[joint_id]
            joint_positions.append(f"{joint_pos}")
        print(' '.join(joint_positions))

        # get step and observation
        self.step_count += 1
        observation = self.get_observation()
        reward = self.get_reward(observation)
        done = self.check_done(observation)
        
        # check done 
        truncated = self.step_count >= 150 #max_steps
        if done and not truncated:
            truncated = False
        elif not done and truncated:
            done = False

To check the method, I print the target position, and the qpos after the control part worked.

1. For velocity,

I use the mujoco.mj_step 10 times. And I have set timestep="0.1" in xml. I have print the following result

the target position is [ 0.0010097  -0.00520363 -0.00023476 -0.00949762 -0.00929238  0.02491552
  0.00990722  0.00429056 -0.01188318  0.0102585   0.03910985  0.02659197]
Final position is:
-0.04502207277757447 -0.05250235074349127 2.1556266493768515e-05 0.00010946377189902234 -0.0008435252520855553 12.761484387779488 -0.021069473098926858 -0.0263708232504594 0.0013808771228400868 1.2976720852616457 0.2508923743219841 -0.016532000603285718
Action at step: 9
the target position is [-4.38763411e-02 -5.75431975e-02 -9.46914173e-05 -1.09164083e-02
 -1.13433752e-02  1.27881192e+01 -1.04876370e-02 -2.32409685e-02
 -1.16682784e-02  1.30687659e+00  2.94531304e-01  1.40545699e-02]
Final position is:
1.573748416558842e-06 -4.7065117619169745e-07 6.630913310495579e-06 -1.721111764816618e-05 5.7294579104081e-06 0.00046033213155404813 -2.7183555486632406e-06 -2.854643062119084e-05 -2.255514189789391e-06 0.0023435970144031915 -0.0001879699114344247 -3.757096123384091e-05

From the results above, you can see that it is basically impossible to move the robotic arm at the speed set by the action. Additionally, in the qpos results of step 8, there is an abnormal data point, 12.761484387779488, which also exceeds the range of the corresponding joint, 'yaw'.

2. For position

In order to make the robotic arm approach the target smoothly, I will take 20 steps to gradually bring the position closer to the final target_position. The print result show in blow.

Action at step: 3
the target position is [ 5.83514846e-04 -5.96156257e-03  4.80876725e-06 -2.54770388e-03
 -3.24547931e-03  1.92781789e-02  2.26140491e-02  2.53092498e-02
  8.11666397e-03  7.27273122e-03  2.56319921e-02  7.36565574e-03]
Final position is:
-1.193897456419377e-05 -3.48689374203847e-05 6.110248581417377e-06 1.6300076395434795e-05 -3.47938845487539e-05 0.051269802613661535 0.05433566982040594 0.07394360316011088 0.06649481679400251 -0.004053727741608468 -2.7357848954645843e-05 0.002966048547774208
Action at step: 4
the target position is [ 0.00072784 -0.0057585  -0.00017314 -0.00386306 -0.00444929  0.06404162
  0.06235324  0.08219339  0.06002548  0.00378048  0.02294366  0.01283086]
Final position is:
-5.225066910612181 -37.73792027720033 -0.07470273621572865 0.12968432593597812 15.72162542616627 -2884.6105537913327 -601.4417803089914 -562.8536419706986 326.5602177287861 -542.8047537253858 -218.0844221762207 124.37233689610719

As you can see, the data here are basically highly abnormal results. The output qpos essentially exceeds the settings I made for joints and actuators in the XML file. I learned that a position actuator essentially acts like a spring. Therefore, I can understand if there's a slight deviation between the final position of the robotic arm and the position I specified. However, such substantial errors are beyond my comprehension.

3. For changing qpos

I used a method similar to the position approach, allowing the robotic arm to move towards the target position in 10 steps. The results obtained are as follows:

the target position is [ 9.30925347e-04 -1.14505605e-02  6.88018363e-06 -3.71136077e-03
 -4.96481499e-03  1.31772931e-02  1.37853244e-02  1.54776486e-02
 -5.78738836e-03  1.14493601e-02  2.86135227e-02  4.20863913e-03]
Final position is:
0.00092705136717561 -0.011367633677172589 -1.713595903321393e-05 -0.0028833809615706057 -0.003803491058706864 0.00837932165651643 0.009629703952572642 0.010475474073343998 -0.004037662773363183 0.007756633447294015 0.019140194493887316 0.0027803591063957516
Action at step: 4
the target position is [ 0.00167179 -0.01709846 -0.00019149 -0.00677451 -0.00822058  0.02113828
  0.01766253  0.01875711 -0.01045388  0.01562531  0.04210293  0.01260888]
Final position is:
72.91090023831907 825.4241764066903 -0.7662159840816303 -0.9045722427069635 -7.654998082700238 -4189.3242525411515 929.9070010509398 -536.3633058295947 251.0143766084756 1414.5425458533814 -10107.967434507695 74.01719716800515

At Action at step: 3, you can see that I directly modified qpos, but there is still a discrepancy between the final position of the robotic arm and my target position. And at Action at step: 4, you can see that a massive error occurred directly.

So why those error happened?

yuvaltassa · 2024-02-09T05:12:14Z

yuvaltassa
Feb 9, 2024
Maintainer

Related to #1375

0 replies

kankanzheli · 2024-02-13T13:05:47Z

kankanzheli
Feb 13, 2024
Author

@yuvaltassa
Sorry. You did not answer any questions related to qpos. After I change the timestep in xml into timestep="0.005", the result become better than before. I divide the action into ten parts to make the robot approach the target step by step. The code used for running is as follows:
` simulation_steps = 10
step_size = action / simulation_steps
for step in range(simulation_steps):
step_positions = current_joint_positions + step_size*(step+1)
for idx, joint_name in enumerate(joint_names):
joint_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, joint_name)
self.data.qpos[joint_id] = step_positions[idx]
mujoco.mj_step(self.model, self.data)

        print(f"{step} position is:",step_positions)
        joint_positions = []
        for joint_name in joint_names:
            joint_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, joint_name)
            joint_pos = self.data.qpos[joint_id]
            joint_positions.append(f"{joint_pos}")
        print(' '.join(joint_positions))`

You can see that I print my command position and the joint position after running mj_step every time I loop. The obtained results are as follows:
1 position is: [ 0.01232157 -0.04687623 -0.04029872 -0.04124235 -0.0337841 -0.09266873 -0.01634201 0.28713616 -0.22471789 0.15571785 0.20563665 -0.00371253] 0.01230942397845612 -0.04684847563269736 -0.035299262040744496 -0.03623684363563046 -0.029670520194537843 -0.09076135214604435 -0.015952698397438504 0.280552871676317 -0.21978203641416685 0.15229992544727136 0.20100245401198386 -0.0036930131143871 2 position is: [ 0.01293323 -0.04841952 -0.04171966 -0.04308539 -0.03533558 -0.09486929 -0.01633778 0.29692421 -0.23199347 0.15994348 0.21328715 -0.00276474] 0.01292035302219545 -0.04838957572853507 -0.03664373310435637 -0.03794570560268773 -0.03110320740997756 -0.09290569522611623 -0.01581054810630078 0.2901034424116881 -0.22699179522847115 0.1564339034093393 0.20847129852008067 -0.0027490721235974205 3 position is: [ 0.01354489 -0.0499628 -0.04314061 -0.04492842 -0.03688706 -0.09706986 -0.01633354 0.30671225 -0.23926904 0.16416911 0.22093764 -0.00181695] 0.013531283199997869 -0.04993059262479955 -0.037946953770453595 -0.03962359542658837 -0.03250779983059547 -0.09506004425501247 -0.01567468937699564 0.29965368729582675 -0.2341792394840092 0.16055760406791905 0.21593612316297733 -0.0018052826012564653 4 position is: [ 0.01415655 -0.05150609 -0.04456156 -0.04677146 -0.03843854 -0.09927043 -0.0163293 0.3165003 -0.24654461 0.16839474 0.22858814 -0.00086916] 0.014142151730755802 -0.051471514123460106 -0.03922617193551289 -0.041279706379038515 -0.03389494719705386 -0.09720692791813883 -0.015575896385369456 0.30920633440255524 -0.2413389901992624 0.16468308334277593 0.22340251005537207 -0.0008615551177314335
From the printed results, you can see that there is a significant difference between the qpos results of "Z1_linear_joint", "Z2_linear_joint", "Z3_linear_joint" and the instruction. Why is this happening? Is it because this joint slides vertically? Can I set the gravity to 0 in the XML file? And I modified the position of the robotic arm through QPOS here, right?

0 replies

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Assistance with Control system in MuJoCo #2007

{{title}}

{{editor}}'s edit

{{editor}}'s edit

Replies: 2 comments

{{title}}

{{title}}

{{editor}}'s edit

{{editor}}'s edit

Select a reply

Assistance with Control system in MuJoCo #2007

kankanzheli Feb 8, 2024

1. For velocity,

2. For position

3. For changing qpos

Replies: 2 comments

yuvaltassa Feb 9, 2024 Maintainer

kankanzheli Feb 13, 2024 Author

kankanzheli
Feb 8, 2024

yuvaltassa
Feb 9, 2024
Maintainer

kankanzheli
Feb 13, 2024
Author