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

examples/gym_test_ik.py

@@ -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()

examples/lerobotdataset_load.py

@@ -5,8 +5,6 @@
 
 from lerobot.common.datasets.lerobot_dataset import LeRobotDataset
 
-
-
 def main():
 
     # You can easily load a dataset from a Hugging Face repository

examples/mujoco_simple_invk.py

@@ -5,7 +5,7 @@
 import mujoco.viewer
 import numpy as np
 
-from gym_lowcostrobot.simulated_robot import LevenbegMarquardtIK
+from gym_lowcostrobot.interface.simulated_robot import LevenbegMarquardtIK
 
 
 def displace_object(data, m, object_id, viewer, square_size=0.2, invert_y=False, origin_pos=[0, 0.1]):

examples/teleoperate_simulated_robot.py

@@ -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)