Adjusting the range of the 6dof in the mjcf file.

First test with mujoco simulator to validate ranges.

To confirm with the builder of the robot.

examples/mujoco_interactive_simulation.py

@@ -0,0 +1,45 @@
+import time
+
+import mujoco
+import mujoco.viewer
+
+
+def do_interactive_sim_5dof():
+    m = mujoco.MjModel.from_xml_path("gym_lowcostrobot/assets/scene_one_cube.xml")
+    data = mujoco.MjData(m)
+
+    with mujoco.viewer.launch_passive(m, data) as viewer:
+        # Run the simulation
+        while viewer.is_running():
+            step_start = time.time()
+
+            # Step the simulation forward
+            mujoco.mj_step(m, data)
+            viewer.sync()
+
+            # Rudimentary time keeping, will drift relative to wall clock.
+            time_until_next_step = m.opt.timestep - (time.time() - step_start)
+            if time_until_next_step > 0:
+                time.sleep(time_until_next_step)
+
+def do_interactive_sim_6dof():
+    m = mujoco.MjModel.from_xml_path("gym_lowcostrobot/assets/scene_so_arm_6dof_one_cube.xml")
+    data = mujoco.MjData(m)
+
+    with mujoco.viewer.launch_passive(m, data) as viewer:
+        # Run the simulation
+        while viewer.is_running():
+            step_start = time.time()
+
+            # Step the simulation forward
+            mujoco.mj_step(m, data)
+            viewer.sync()
+
+            # Rudimentary time keeping, will drift relative to wall clock.
+            time_until_next_step = m.opt.timestep - (time.time() - step_start)
+            if time_until_next_step > 0:
+                time.sleep(time_until_next_step)
+
+
+if __name__ == "__main__":
+    do_interactive_sim_6dof()

examples/mujoco_simple_invk.py

@@ -7,7 +7,7 @@
 from gym_lowcostrobot.simulated_robot import SimulatedRobot
 
 
-def do_simple_invk():
+def do_simple_invk_5dof():
     m = mujoco.MjModel.from_xml_path("gym_lowcostrobot/assets/scene_one_cube.xml")
     data = mujoco.MjData(m)
     robot = SimulatedRobot(m, data)
@@ -50,5 +50,50 @@ def do_simple_invk():
                     time.sleep(time_until_next_step)
 
 
+def do_simple_invk_6dof():
+
+    m = mujoco.MjModel.from_xml_path("gym_lowcostrobot/assets/scene_so_arm_6dof_one_cube.xml")
+    data = mujoco.MjData(m)
+    robot = SimulatedRobot(m, data)
+
+    with mujoco.viewer.launch_passive(m, data) as viewer:
+        # Get the final position of the cube
+        cube_pos = data.joint("red_box_joint").qpos[:3]
+
+        # Run the simulation
+        while viewer.is_running():
+            step_start = time.time()
+            q_target_pos = robot.inverse_kinematics(ee_target_pos=cube_pos, step=1.0, joint_name="Fixed_Gripper", nb_dof=6)
+            robot.set_target_pos(q_target_pos)
+
+            # Step the simulation forward
+            mujoco.mj_step(m, data)
+            viewer.sync()
+
+            # Rudimentary time keeping, will drift relative to wall clock.
+            time_until_next_step = m.opt.timestep - (time.time() - step_start)
+            if time_until_next_step > 0:
+                time.sleep(time_until_next_step)
+
+            # Get the final position of the cube
+            cube_pos = data.joint("red_box_joint").qpos[:3]
+
+            ee_id = m.body("Moving_Jaw").id
+            ee_pos = data.geom_xpos[ee_id]
+
+            print("Cube dist:", np.linalg.norm(cube_pos - ee_pos))
+            if np.linalg.norm(cube_pos - ee_pos) < 0.1 or np.linalg.norm(cube_pos - ee_pos) > 0.13:
+                print("Cube reached the target position")
+
+                data.qpos[:6] = 0.0
+                data.joint("red_box_joint").qpos[:3] = [np.random.rand() * -0.2, np.random.rand() * -0.2, 0.01]
+                mujoco.mj_forward(m, data)
+                viewer.sync()
+
+                # Rudimentary time keeping, will drift relative to wall clock.
+                time_until_next_step = m.opt.timestep - (time.time() - step_start)
+                if time_until_next_step > 0:
+                    time.sleep(time_until_next_step)
+
 if __name__ == "__main__":
-    do_simple_invk()
+    do_simple_invk_6dof()

examples/trace_hdf5_mujoco.py

@@ -11,7 +11,7 @@
 def do_replay_hdf5(args):
     # Specify the path to your HDF5 file
     with h5py.File(args.file_path, "r") as file:
-        m = mujoco.MjModel.from_xml_path("gym_lowcostrobot/assets/scene_one_cube.xml")
+        m = mujoco.MjModel.from_xml_path("gym_lowcostrobot/assets/scene_so_arm_6dof_one_cube.xml")
         data = mujoco.MjData(m)
         robot = SimulatedRobot(m, data)
 
@@ -25,7 +25,7 @@ def do_replay_hdf5(args):
                 step_start = time.time()
 
                 # Step the simulation forward
-                robot.set_target_pos(group_action[step][0:5])
+                robot.set_target_pos(group_qpos[step][0:6])
                 mujoco.mj_step(m, data)
 
                 viewer.sync()
@@ -35,8 +35,10 @@ def do_replay_hdf5(args):
                 if time_until_next_step > 0:
                     time.sleep(time_until_next_step)
 
+
                 step += 1
-                step = step % len(group_qpos)
+                print(group_qpos[step][0:6])
+                #step = step % len(group_qpos)
 
 
 if __name__ == "__main__":

gym_lowcostrobot/assets/low_cost_robot_6dof.xml

@@ -23,24 +23,24 @@
                 <position ctrlrange="-60.14158 60.14158" kp="25"/>
             </default>
             <default class="Shoulder_Pitch">
-                <joint pos="0 0 0" axis="0 0 1" range="-3.14158 3.14158" damping="0.1"/>
-                <position ctrlrange="-3.14158 3.14158" kp="25"/>
+                <joint pos="0 0 0" axis="0 0 1" range="-2.17 1.1" damping="0.1"/>
+                <position ctrlrange="-2.17 1.1" kp="25"/>
             </default>
             <default class="Elbow">
-                <joint pos="0 0 0" axis="0 0 1" range="-3.14158 3.14158" damping="0.1"/>            
-                <position ctrlrange="-3.14158 3.14158" kp="25"/>
+                <joint pos="0 0 0" axis="0 0 1" range="-1.5 2.25" damping="0.1"/>            
+                <position ctrlrange="-1.5 2.25" kp="25"/>
             </default>
             <default class="Wrist_Pitch">
-                <joint pos="0 0 0" axis="1 0 0" range="-3.14158 3.14158" damping="0.1"/>
-                <position ctrlrange="-3.14158 3.14158" kp="25"/>
+                <joint pos="0 0 0" axis="1 0 0" range="-1.95 0.725" damping="0.1"/>
+                <position ctrlrange="-1.95 0.725" kp="25"/>
             </default>
             <default class="Wrist_Roll">
-                <joint pos="0 0 0" axis="0 0 1" range="-2.45 0.032" damping="0.1"/>            
-                <position ctrlrange="-2.45 0.032" kp="25"/>
+                <joint pos="0 0 0" axis="0 0 1" range="-1.5 1.5" damping="0.1"/>            
+                <position ctrlrange="-1.5 1.5" kp="25"/>
             </default>
             <default class="Gripper">
-                <joint pos="0 0 0" axis="0 0 1" range="0.032 2.45" damping="0.1"/>            
-                <position ctrlrange="0.032 2.45" kp="25"/>
+                <joint pos="0 0 0" axis="0 0 1" range="0.032 1.5" damping="0.1"/>            
+                <position ctrlrange="0.032 1.5" kp="25"/>
             </default>
         </default>
     </default>

gym_lowcostrobot/simulated_robot.py

@@ -39,11 +39,11 @@ def _norm2pwm(self, x: np.ndarray) -> np.ndarray:
         """
         return x * 4096
 
-    def read_position(self) -> np.ndarray:
+    def read_position(self, nb_dof=5) -> np.ndarray:
         """
         :return: numpy array of current joint positions in range [0, 4096]
         """
-        return self.d.qpos[:5]
+        return self.d.qpos[:nb_dof]
 
     def read_velocity(self):
         """
@@ -60,7 +60,7 @@ def read_ee_pos(self, joint_name="end_effector"):
         joint_id = self.m.body(joint_name).id
         return self.d.geom_xpos[joint_id]
 
-    def inverse_kinematics(self, ee_target_pos, step=0.2, joint_name="end_effector"):
+    def inverse_kinematics(self, ee_target_pos, step=0.2, joint_name="end_effector", nb_dof=5):
         """
         :param ee_target_pos: numpy array of target end effector position
         :param joint_name: name of the end effector joint
@@ -75,8 +75,8 @@ def inverse_kinematics(self, ee_target_pos, step=0.2, joint_name="end_effector")
         mujoco.mj_jacBodyCom(self.m, self.d, jac, None, joint_id)
 
         # compute target joint velocities
-        qpos = self.read_position()
-        qdot = np.dot(np.linalg.pinv(jac[:, :5]), ee_target_pos - ee_pos)
+        qpos = self.read_position(nb_dof=nb_dof)
+        qdot = np.dot(np.linalg.pinv(jac[:, :nb_dof]), ee_target_pos - ee_pos)
 
         # apply the joint velocities
         q_target_pos = qpos + qdot * step