update with pick place cube environment

examples/dynamixel_mujoco_leader.py

@@ -25,80 +25,6 @@ def getch():
 from dynamixel_sdk import *  # Uses Dynamixel SDK library
 
 
-
-
-
-
-import gymnasium as gym
-import gym_lowcostrobot  # noqa
-
-def do_env_sim():
-
-    # 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("PushCube-v0", observation_mode="state", render_mode="human", action_mode="joint")
-    env.reset()
-
-    init_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)
-        init_pos[current_id] = dxl_present_position*360/4096
-    print(f"init_pos: {init_pos}")
-
-    env.unwrapped.data.qpos[0:6] = init_pos
-    mujoco.mj_forward(env.unwrapped.model, env.unwrapped.data)
-    env.unwrapped.viewer.sync()
-
-    max_step = 1000000
-    for _ in range(max_step):
-
-        # Read present position
-        action = 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)
-            action[current_id] = dxl_present_position*360/4096
-
-        observation, reward, terminated, truncated, info = env.step(action - init_pos)
-        init_pos = action
-
-        # print("Observation:", observation)
-        # print("Reward:", reward)
-
-        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()
-
-
 def real_to_mujoco(real_positions, inverted_joints=[], half_inverted_joints=[]):
     """
     Convert joint positions from real robot (in degrees) to Mujoco (in radians),
@@ -152,7 +78,7 @@ def do_sim(robot_id="6dof"):
         quit()
 
     if robot_id == "6dof":
-        path_scene = "gym_lowcostrobot/assets/low_cost_robot_6dof/reach_cube.xml"
+        path_scene = "gym_lowcostrobot/assets/low_cost_robot_6dof/pick_place_cube.xml"
     else:
         return
 
@@ -162,14 +88,16 @@ def do_sim(robot_id="6dof"):
 
     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()
 
-            #if getch() == chr(0x1b):
-            #    break
-
             # Read present position
             real_pos = np.zeros(6)
             for current_id in range(6):
@@ -201,4 +129,3 @@ def do_sim(robot_id="6dof"):
     args = parser.parse_args()
 
     do_sim(args.robot)
-    #do_env_sim()