Merge branch 'main' of https://github.com/perezjln/gym-lowcostrobot

examples/dynamixel_gym_leader.py

@@ -0,0 +1,128 @@
+import os, argparse
+import numpy as np
+
+import mujoco
+import gymnasium as gym
+import gym_lowcostrobot  # noqa
+
+
+# Function to get a single character input
+if os.name == 'nt':
+    import msvcrt
+    def getch():
+        return msvcrt.getch().decode()
+else:
+    import sys, tty, termios
+    fd = sys.stdin.fileno()
+    old_settings = termios.tcgetattr(fd)
+    def getch():
+        try:
+            tty.setraw(sys.stdin.fileno())
+            ch = sys.stdin.read(1)
+        finally:
+            termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
+        return ch
+
+from dynamixel_sdk import *  # Uses Dynamixel SDK library
+
+
+def real_to_mujoco(real_positions, inverted_joints=[], half_inverted_joints=[]):
+    """
+    Convert joint positions from real robot (in degrees) to Mujoco (in radians),
+    with support for inverted joints.
+    
+    Parameters:
+    real_positions (list or np.array): Joint positions in degrees.
+    inverted_joints (list): List of indices for joints that are inverted.
+    
+    Returns:
+    np.array: Joint positions in radians.
+    """
+    real_positions = np.array(real_positions)
+    mujoco_positions = real_positions * (np.pi / 180.0)
+
+    # Apply inversion if necessary
+    for index in inverted_joints:
+        mujoco_positions[index] += np.pi
+        mujoco_positions[index] *= -1
+
+    # Apply half inversion if necessary
+    for index in half_inverted_joints:
+        mujoco_positions[index] -= np.pi / 2.0
+        mujoco_positions[index] *= -1
+
+    return mujoco_positions
+
+
+def do_read_pos(packetHandler, portHandler, ADDR_MX_PRESENT_POSITION):
+    # Read present position
+    real_pos = np.zeros(6)
+    for current_id in range(6):
+        dxl_present_position, dxl_comm_result, dxl_error = packetHandler.read2ByteTxRx(portHandler, current_id + 1, ADDR_MX_PRESENT_POSITION)
+        real_pos[current_id] = dxl_present_position*360/4096
+    return real_to_mujoco(real_pos, inverted_joints=[1, 2, 3, 5], half_inverted_joints=[4])
+
+
+def do_env_sim(args):
+
+    # Control table address
+    ADDR_MX_PRESENT_POSITION = 132
+
+    # Protocol version
+    PROTOCOL_VERSION = args.protocol_version  # Use the protocol version from command line
+
+    # Configuration from command line arguments
+    DEVICENAME = args.device
+
+    # Initialize PortHandler instance
+    portHandler = PortHandler(DEVICENAME)
+
+    # Initialize PacketHandler instance
+    packetHandler = PacketHandler(PROTOCOL_VERSION)
+
+    # Open port
+    if not portHandler.openPort():
+        print("Failed to open the port")
+        print("Press any key to terminate...")
+        getch()
+        quit()
+
+    env = gym.make("PickPlaceCube-v0", observation_mode="state", render_mode="human", action_mode="joint")
+    env.reset()
+
+    pos_arm = do_read_pos(packetHandler, portHandler, ADDR_MX_PRESENT_POSITION)
+
+    # compute forward kinematics
+    env.data.qpos[-6:] = pos_arm
+    mujoco.mj_forward(env.model, env.data)
+    env.viewer.sync()
+
+    max_step = 1000000
+    for _ in range(max_step):
+
+        new_pos_arm = do_read_pos(packetHandler, portHandler, ADDR_MX_PRESENT_POSITION)
+        action = (new_pos_arm - pos_arm)*100
+        observation, reward, terminated, truncated, info = env.step(action)
+        pos_arm = new_pos_arm
+
+        env.render()
+        if terminated:
+            if not truncated:
+                print(f"Cube reached the target position at step: {env.current_step} with reward {reward}")
+            else:
+                print(
+                    f"Cube didn't reached the target position at step: {env.current_step} with reward {reward} but was truncated"
+                )
+            env.reset()
+
+    # Close port
+    portHandler.closePort()
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description="Choose between 5dof and 6dof lowcost robot simulation.")
+    parser.add_argument('--device', type=str, default='/dev/ttyACM0', help='Port name (e.g., COM1, /dev/ttyUSB0, /dev/tty.usbserial-*)')
+    parser.add_argument('--protocol_version', type=float, default=2.0, help='Protocol version (e.g., 1.0 or 2.0)')    
+    args = parser.parse_args()
+
+    do_env_sim(args)

examples/dynamixel_mujoco_leader.py

@@ -0,0 +1,131 @@
+import os, argparse, time
+
+import mujoco
+import mujoco.viewer
+import numpy as np
+
+
+# Function to get a single character input
+if os.name == 'nt':
+    import msvcrt
+    def getch():
+        return msvcrt.getch().decode()
+else:
+    import sys, tty, termios
+    fd = sys.stdin.fileno()
+    old_settings = termios.tcgetattr(fd)
+    def getch():
+        try:
+            tty.setraw(sys.stdin.fileno())
+            ch = sys.stdin.read(1)
+        finally:
+            termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
+        return ch
+
+from dynamixel_sdk import *  # Uses Dynamixel SDK library
+
+
+def real_to_mujoco(real_positions, inverted_joints=[], half_inverted_joints=[]):
+    """
+    Convert joint positions from real robot (in degrees) to Mujoco (in radians),
+    with support for inverted joints.
+    
+    Parameters:
+    real_positions (list or np.array): Joint positions in degrees.
+    inverted_joints (list): List of indices for joints that are inverted.
+    
+    Returns:
+    np.array: Joint positions in radians.
+    """
+    real_positions = np.array(real_positions)
+    mujoco_positions = real_positions * (np.pi / 180.0)
+
+    # Apply inversion if necessary
+    for index in inverted_joints:
+        mujoco_positions[index] += np.pi
+        mujoco_positions[index] *= -1
+
+    # Apply half inversion if necessary
+    for index in half_inverted_joints:
+        mujoco_positions[index] -= np.pi / 2.0
+        mujoco_positions[index] *= -1
+
+    return mujoco_positions
+
+
+def do_sim(robot_id="6dof"):
+
+    # Control table address
+    ADDR_MX_PRESENT_POSITION = 132
+
+    # Protocol version
+    PROTOCOL_VERSION = args.protocol_version  # Use the protocol version from command line
+
+    # Configuration from command line arguments
+    DEVICENAME = args.device
+
+    # Initialize PortHandler instance
+    portHandler = PortHandler(DEVICENAME)
+
+    # Initialize PacketHandler instance
+    packetHandler = PacketHandler(PROTOCOL_VERSION)
+
+    # Open port
+    if not portHandler.openPort():
+        print("Failed to open the port")
+        print("Press any key to terminate...")
+        getch()
+        quit()
+
+    if robot_id == "6dof":
+        path_scene = "gym_lowcostrobot/assets/low_cost_robot_6dof/pick_place_cube.xml"
+    else:
+        return
+
+    m = mujoco.MjModel.from_xml_path(path_scene)
+    #m.opt.timestep = 1 / 10000
+    data = mujoco.MjData(m)
+
+    with mujoco.viewer.launch_passive(m, data) as viewer:
+
+        ## position object in front of the robot
+        data.joint("cube").qpos[:3] = [0.0, -0.1, 0.01]
+        mujoco.mj_step(m, data)
+        viewer.sync()
+
+        # Run the simulation
+        while viewer.is_running():
+
+            step_start = time.time()
+
+            # Read present position
+            real_pos = np.zeros(6)
+            for current_id in range(6):
+                dxl_present_position, dxl_comm_result, dxl_error = packetHandler.read2ByteTxRx(portHandler, current_id + 1, ADDR_MX_PRESENT_POSITION)
+                real_pos[current_id] = dxl_present_position*360/4096
+
+            # check limits
+            # self.check_joint_limits(self.data.qpos)
+
+            # compute forward kinematics
+            data.qpos[-6:] = real_to_mujoco(real_pos, inverted_joints=[1, 2, 3, 5], half_inverted_joints=[4])
+            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)
+
+    # Close port
+    portHandler.closePort()
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description="Choose between 5dof and 6dof lowcost robot simulation.")
+    parser.add_argument("--robot", choices=["6dof"], default="6dof", help="Choose the lowcost robot type")
+    parser.add_argument('--device', type=str, default='/dev/ttyACM0', help='Port name (e.g., COM1, /dev/ttyUSB0, /dev/tty.usbserial-*)')
+    parser.add_argument('--protocol_version', type=float, default=2.0, help='Protocol version (e.g., 1.0 or 2.0)')    
+    args = parser.parse_args()
+
+    do_sim(args.robot)

examples/dynamixel_leader_read.py → examples/dynamixel_read_leader.py

@@ -24,6 +24,11 @@
 """
 
 
+### Real: All strait position of the motors: 181.58203125 266.572265625 91.93359375 181.318359375 90.0 167.6953125
+### Real: Left position of the motors:       9.033203125 180.0 176.8359375 92.021484375 177.1875 167.6953125
+
+### Mujoco: All strait position of the motors: -0.03142 -1.56422 1.5423 -0.0044892 -0.000443796 0.0523634' (6 last on the qpos)
+### Mujoco: Left position of the motors:       -1.571 -0.0137084 -0.00423796 1.53604 -1.18804e-06 0.052353' (6 last on the qpos)
 
 import argparse
 import os
@@ -80,7 +85,7 @@ def main(args):
             lst_pos = []
             for current_id in range(6):
                 dxl_present_position, dxl_comm_result, dxl_error = packetHandler.read2ByteTxRx(portHandler, current_id + 1, ADDR_MX_PRESENT_POSITION)
-                lst_pos.append(str(dxl_present_position))
+                lst_pos.append(str(dxl_present_position*360/4096))
 
             print(' '.join(lst_pos))
 

gym_lowcostrobot/assets/low_cost_robot_6dof/pick_place_cube.xml

@@ -1,7 +1,9 @@
 <mujoco model="bd1 scene">
+
+	<option gravity="0 0 -9.81"/>
 	<option timestep="0.005"/>
 
-	<compiler angle="radian" autolimits="true"/>
+	<compiler angle="radian" autolimits="true" convexhull="false"/>
 
 	<asset>
 		<mesh name="base" file="base.stl"/>
@@ -30,18 +32,18 @@
 	</visual>
 
 	<worldbody>
+
 		<light pos="0 0 3" dir="0 0 -1" directional="false" />
 		<body name="floor">
 			<geom pos="0 0 0" name="floor" size="0 0 .125" type="plane" material="groundplane" conaffinity="1" contype="1" />
 		</body>
 
 		<body name="cube" pos="0.1 0.1 0.01">
 			<freejoint name="cube"/>
-			<inertial pos="0 0 0" mass="0.1" diaginertia="0.00001125 0.00001125 0.00001125"/>
+			<inertial pos="0 0 0" mass="10.0" diaginertia="0.00001125 0.00001125 0.00001125"/>
 			<geom friction="0.5" condim="3" pos="0 0 0" size="0.015 0.015 0.015" type="box" name="cube" rgba="0.5 0 0 1" priority="1"/>
 		</body>
 
-
 		<camera name="camera_front" pos="0.049 0.888 0.317" xyaxes="-0.998 0.056 -0.000 -0.019 -0.335 0.942"/>
 		<camera name="camera_top" pos="0 0 1" euler="0 0 0" mode="fixed"/>
 		<camera name="camera_vizu" pos="-0.1 0.6 0.3" quat="-0.15 -0.1 0.6 1"/>