I am working on a project involving a robotic simulation in MuJoCo. I aim to control the robot's joints based on target positions. Despite setting up position actuators in my XML configuration and attempting to control the joints by specifying target positions, I am encountering issues where the joints do not accurately reach the specified target positions.
Here is a brief overview of my approach:
I have defined position-type actuators for each joint in my MuJoCo XML model. In my control code, I calculate the target positions for each joint and assign these as control signals. After running the simulation step, I observe that the actual positions (qpos) of the joints do not match the target positions I set.
Could you please provide guidance on the following questions:
- Are there any common pitfalls or recommended practices when setting up position-based control in MuJoCo that I might be overlooking?
- How can I ensure that the joints accurately reach their target positions, considering the dynamic nature of the simulation environment?
- Is there a recommended way to debug or diagnose issues related to position control in MuJoCo?
The version of mujoco I am using is 3.0.0.
The xml file of robot:
<?xml version="1.0"?>
<mujoco>
<size nstack="300000000" />
<option gravity="0 0 0" solver="CG"/>
<asset>
<mesh name="frame" file="/home/zhaowentao/Reinforcement/train/STL/frame.stl"/>
<mesh name="Y_linear" file="/home/zhaowentao/Reinforcement/train/STL/Y_linear.stl"/>
<mesh name="X_linear" file="/home/zhaowentao/Reinforcement/train/STL/X_linear.stl"/>
<mesh name="Z1_linear" file="/home/zhaowentao/Reinforcement/train/STL/Z1_linear.stl"/>
<mesh name="Z2_linear" file="/home/zhaowentao/Reinforcement/train/STL/Z2_linear.stl"/>
<mesh name="Z3_linear" file="/home/zhaowentao/Reinforcement/train/STL/Z3_linear.stl"/>
<mesh name="yaw" file="/home/zhaowentao/Reinforcement/train/STL/yaw.stl"/>
<mesh name="pitch" file="/home/zhaowentao/Reinforcement/train/STL/pitch.stl"/>
<mesh name="roll" file="/home/zhaowentao/Reinforcement/train/STL/roll.STL"/>
</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>
</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="500"/>
<position name="X_linear_position" joint="X_linear_joint" ctrllimited="true" ctrlrange="-0.225 0.281" kp="500"/>
<position name="Z1_linear_position" joint="Z1_linear_joint" ctrllimited="true" ctrlrange="0 0.276" kp="500"/>
<position name="Z2_linear_position" joint="Z2_linear_joint" ctrllimited="true" ctrlrange="0 0.325" kp="500"/>
<position name="Z3_linear_position" joint="Z3_linear_joint" ctrllimited="true" ctrlrange="0 0.207" kp="500"/>
<position name="yaw_position" joint="yaw_joint" ctrllimited="true" ctrlrange="-3.14159 3.14159" kp="500"/>
</actuator>
</mujoco>
The control python code:
def step(self, action):
joint_names = [
"Y_linear_joint",
"X_linear_joint",
"Z1_linear_joint",
"Z2_linear_joint",
"Z3_linear_joint",
"yaw_joint"
]
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
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] = target_positions[idx]
mujoco.mj_step(self.model, self.data)
self.step_count += 1
observation = self.get_observation()
reward = self.get_reward(observation)
done = self.check_done(observation)
truncated = self.step_count >= 150
if done and not truncated:
truncated = False
elif not done and truncated:
done = False
return observation, reward, done, truncated, {}```
I need to work with my robot through reinforcement learning. This makes it necessary for me to have robots under mujoco that can accurately follow my instructions. Instead of the robot's running state always having a significant error with my instructions.
I modified the XML file to <position name="Y_linear_position" joint="Y_linear_joint" ctrllimited="true" ctrlrange="-0.3455 0.3455" kp="500" kv="10"/>, but encountered an error:
ValueError: XML Error: Schema violation: unrecognized attribute: 'kv'.
At the same time, I manually adjusted the value of kp, but there was always a significant error between the position information of the output joint and the instruction (target_position) given by my code. Do I have to manually adjust the values of kp? Can't MuJoCo accurately and automatically reach the specified position?
I also tried to modify the control and rendering parts of my step function to look like the following. But there is still a significant difference between the qpos of the output joint after running and the "target_position" I provided.
simulation_steps_per_action = 100
for _ in range(simulation_steps_per_action):
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] = target_positions[idx]
mujoco.mj_step(self.model, self.data)
After each execution of the step function, I render and obtain observations. From the visualization window, it seems like the robotic arm is teleporting. Is this normal?