-
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.
Implement tele-operation over gym environment with a physical leader …
…arm from Alex. Introduce the interface module to regroup physical robot interaction The code as been adapted from the orginal repo of Alex Some prefix renaming of some example scripts
- Loading branch information
Showing
9 changed files
with
587 additions
and
59 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
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,44 @@ | ||
import gymnasium as gym | ||
import gym_lowcostrobot # Import the low-cost robot environments | ||
import numpy as np | ||
|
||
def displace_object(square_size=0.15, invert_y=False, origin_pos=[0, 0.1]): | ||
### Sample a position in a square in front of the robot | ||
if not invert_y: | ||
x = np.random.uniform(origin_pos[0] - square_size / 2, origin_pos[0] + square_size / 2) | ||
y = np.random.uniform(origin_pos[1] - square_size / 2, origin_pos[1] + square_size / 2) | ||
else: | ||
x = np.random.uniform(origin_pos[0] + square_size / 2, origin_pos[0] - square_size / 2) | ||
y = np.random.uniform(origin_pos[1] + square_size / 2, origin_pos[1] - square_size / 2) | ||
env.data.qpos[:3] = np.array([x, y, origin_pos[2]]) | ||
return env.data.qpos[:3] | ||
|
||
if __name__ == "__main__": | ||
|
||
# Create the environment | ||
env = gym.make("ReachCube-v0", render_mode="human", action_mode="ee") | ||
|
||
# Reset the environment | ||
observation, info = env.reset() | ||
cube_origin_pos = env.data.qpos[:3].astype(np.float32) | ||
for i in range(10000): | ||
if i % 500 == 0: | ||
cube_pos = displace_object(square_size=0.2, invert_y=False, origin_pos=cube_origin_pos) | ||
|
||
# Sample random action | ||
action = env.action_space.sample() | ||
ee_id = env.model.body("moving_side").id | ||
ee_pos = env.data.xpos[ee_id].astype(np.float32) # default [0.03390873 0.22571199 0.14506643] | ||
action[:3] = cube_pos + [0,0,0.1] - ee_pos | ||
# action[:3] = [0.03390873, 0.22571199, 0.14506643] | ||
action[3] = -1.5 | ||
|
||
# Step the environment | ||
observation, reward, terminted, truncated, info = env.step(action) | ||
|
||
# Reset the environment if it's done | ||
if terminted or truncated: | ||
observation, info = env.reset() | ||
|
||
# Close the environment | ||
env.close() |
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 was deleted.
Oops, something went wrong.
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,54 @@ | ||
import time | ||
import numpy as np | ||
import threading | ||
import mujoco.viewer | ||
|
||
from gym_lowcostrobot.interface.dynamixel import Dynamixel | ||
from gym_lowcostrobot.interface.robot import Robot | ||
from gym_lowcostrobot.interface.simulated_robot import SimulatedRobot | ||
|
||
def read_leader_position(): | ||
global target_pos | ||
while True: | ||
target_pos = np.array(leader.read_position()) | ||
target_pos = (target_pos / 2048 - 1) * 3.14 | ||
target_pos[1] = -target_pos[1] | ||
target_pos[3] = -target_pos[3] | ||
target_pos[4] = -target_pos[4] | ||
|
||
if __name__ == '__main__': | ||
|
||
leader_dynamixel = Dynamixel.Config(baudrate=1_000_000, device_name='/dev/tty.usbmodem57380045631').instantiate() | ||
leader = Robot(leader_dynamixel, servo_ids=[1, 2, 3, 6, 7]) | ||
leader.set_trigger_torque() | ||
|
||
m = mujoco.MjModel.from_xml_path('gym_lowcostrobot/assets/low_cost_robot_6dof/reach_cube.xml') | ||
d = mujoco.MjData(m) | ||
r = SimulatedRobot(m, d) | ||
|
||
target_pos = np.zeros(5) | ||
|
||
# Start the thread for reading leader position | ||
leader_thread = threading.Thread(target=read_leader_position) | ||
leader_thread.daemon = True | ||
leader_thread.start() | ||
|
||
with mujoco.viewer.launch_passive(m, d) as viewer: | ||
start = time.time() | ||
while viewer.is_running(): | ||
# Use the latest target_pos | ||
step_start = time.time() | ||
target_pos_local = target_pos.copy() | ||
# print(f'target pos copy {time.time() - step_start}') | ||
r.set_target_pos(target_pos_local) | ||
# print(f'set targtee pos copy {time.time() - step_start}') | ||
mujoco.mj_step(m, d) | ||
# print(f'mjstep {time.time() - step_start}') | ||
viewer.sync() | ||
# print(f'viewer sync {time.time() - step_start}') | ||
|
||
# Rudimentary time keeping, will drift relative to wall clock. | ||
time_until_next_step = m.opt.timestep - (time.time() - step_start) | ||
# print(f'time until next step {time_until_next_step}') | ||
if time_until_next_step > 0: | ||
time.sleep(time_until_next_step) |
This file was deleted.
Oops, something went wrong.
Oops, something went wrong.