-
Notifications
You must be signed in to change notification settings - Fork 14
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #34 from michel-aractingi/pushcubeloop-26-09-2024
Added new teleoperation script + PushCubeLoop env + small refactoring of the environments.
- Loading branch information
Showing
18 changed files
with
648 additions
and
105 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,62 @@ | ||
| import argparse | ||
|
|
||
| import mujoco | ||
| import numpy as np | ||
| import matplotlib.pyplot as plt | ||
| import gymnasium as gym | ||
| import gym_lowcostrobot # Import the low-cost robot environments | ||
|
|
||
| from lerobot.common.robot_devices.motors.dynamixel import DynamixelMotorsBus | ||
|
|
||
| def do_sim(args): | ||
|
|
||
| env = gym.make(args.env_name, render_mode="human") | ||
|
|
||
| offsets = [0, -np.pi/2, -np.pi/2, 0, -np.pi/2, 0] | ||
| counts_to_radians = np.pi * 2. / 4096. | ||
| # get the start pos from .cache/calibration directory in your local lerobot | ||
| start_pos = [2072, 2020, 1063, 3966, 3053, 1938] | ||
| axis_direction = [-1, -1, 1, -1, -1, -1] | ||
| joint_commands = [0,0,0,0,0,0] | ||
| leader_arm = DynamixelMotorsBus( | ||
| port=args.device, | ||
| motors={ | ||
| # name: (index, model) | ||
| "shoulder_pan": (1, "xl330-m077"), | ||
| "shoulder_lift": (2, "xl330-m077"), | ||
| "elbow_flex": (3, "xl330-m077"), | ||
| "wrist_flex": (4, "xl330-m077"), | ||
| "wrist_roll": (5, "xl330-m077"), | ||
| "gripper": (6, "xl330-m077"), | ||
| }, | ||
| ) | ||
|
|
||
| if not leader_arm.is_connected: | ||
| leader_arm.connect() | ||
|
|
||
| env.reset() | ||
| rewards = [] | ||
| timesteps = [] | ||
|
|
||
| while env.viewer.is_running(): | ||
| positions = leader_arm.read("Present_Position") | ||
| assert len(joint_commands) == len(positions) | ||
| for i in range(len(joint_commands)): | ||
| joint_commands[i] = axis_direction[i] * \ | ||
| (positions[i] - start_pos[i]) * counts_to_radians + offsets[i] | ||
|
|
||
| ret = env.step(joint_commands) | ||
|
|
||
| rewards.append(ret[1]) | ||
| timesteps.append(env.data.time) | ||
|
|
||
| plt.plot(timesteps, rewards) | ||
| plt.show() | ||
|
|
||
| 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('--env-name', type=str, default='PushCubeLoop-v0', help='Specify the gym-lowcost robot env to test.') | ||
| args = parser.parse_args() | ||
|
|
||
| do_sim(args) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
51 changes: 51 additions & 0 deletions
51
gym_lowcostrobot/assets/low_cost_robot_6dof/push_cube_loop.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,51 @@ | ||
| <mujoco model="bd1 scene"> | ||
| <option timestep="0.005"/> | ||
| <option gravity="0 0 -9.81" /> | ||
|
|
||
| <compiler angle="radian" autolimits="true"/> | ||
|
|
||
| <include file="follower.xml"/> | ||
|
|
||
| <asset> | ||
| <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072" /> | ||
| <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300" /> | ||
| <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2" /> | ||
| </asset> | ||
|
|
||
| <visual> | ||
| <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" /> | ||
| <rgba haze="0.15 0.25 0.35 1" /> | ||
| <global azimuth="150" elevation="-20" offheight="640" /> | ||
| </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" solref="0.0 0.0"/> | ||
| </body> | ||
|
|
||
| <body name="cube" pos="0.06 0.135 0.017"> | ||
| <freejoint name="cube"/> | ||
| <inertial pos="0 0 0" mass=".05" diaginertia="0.00001125 0.00001125 0.00001125"/> | ||
| <geom friction="1.5 1.5 1.5" condim="4" 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.5 0.225" xyaxes="-0.998 0.056 -0.000 -0.019 -0.335 0.942"/> | ||
| <camera name="camera_top" pos="0 0.1 0.6" 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"/> | ||
|
|
||
| <!-- Goal Region 1 --> | ||
| <geom name="goal_region_1" type="box" pos=".06 .135 0.01" size="0.035 0.045 0.007" rgba="1 1 0 0.3" contype="0" conaffinity="0" /> | ||
|
|
||
| <!-- Goal Region 2 --> | ||
| <geom name="goal_region_2" type="box" pos="-.06 .135 0.01" size="0.035 0.045 0.007" rgba="1 0 1 0.3" contype="0" conaffinity="0"/> | ||
|
|
||
| <!-- Define the rails that contain the cube --> | ||
| <geom name="left_wall" type="box" pos="-0.125 0.135 0.005" size="0.01 0.055 0.007" rgba="1 1 1 1" /> | ||
| <geom name="right_wall" type="box" pos="0.125 0.135 0.005" size="0.01 0.055 0.007" rgba="1 1 1 1" /> | ||
| <geom name="top_wall" type="box" pos="0 0.09 0.005" size="0.125 0.01 0.007" rgba="1 1 1 1" /> | ||
| <geom name="bottom_wall" type="box" pos="0 0.18 0.005" size="0.125 0.01 0.007" rgba="1 1 1 1" /> | ||
|
|
||
|
|
||
| </worldbody> | ||
| </mujoco> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.