diff --git a/examples/dynamixel_mujoco_leader.py b/examples/dynamixel_mujoco_leader.py index 839e6c7..c83b076 100644 --- a/examples/dynamixel_mujoco_leader.py +++ b/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()