wangzixiang
/
lerobot
mirror of https://github.com/huggingface/lerobot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

working in simulation

This commit is contained in:
apirrone 2024-11-25 17:57:19 +01:00
parent 13a2091db3
commit 03a44a56f3
3 changed files with 66 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
lerobot/common/datasets/video_utils.py
View File

@ -166,7 +166,8 @@ def encode_video_frames(
imgs_dir: Path, imgs_dir: Path,
video_path: Path, video_path: Path,
fps: int, fps: int,
vcodec: str = "libsvtav1", # vcodec: str = "libsvtav1",
vcodec: str = "libx264",
pix_fmt: str = "yuv420p", pix_fmt: str = "yuv420p",
g: int | None = 2, g: int | None = 2,
crf: int | None = 30, crf: int | None = 30,

121
lerobot/common/robot_devices/robots/reachy2.py
View File

@ -29,7 +29,8 @@ from lerobot.common.robot_devices.cameras.utils import Camera
class ReachyRobotConfig: class ReachyRobotConfig:
robot_type: str | None = "Reachy2" robot_type: str | None = "Reachy2"
cameras: dict[str, Camera] = field(default_factory=lambda: {}) cameras: dict[str, Camera] = field(default_factory=lambda: {})
ip_address: str | None = "172.17.135.207" # ip_address: str | None = "172.17.135.207"
ip_address: str | None = "localhost"
class ReachyRobot: class ReachyRobot:
@ -44,10 +45,13 @@ class ReachyRobot:
self.robot_type = self.config.robot_type self.robot_type = self.config.robot_type
self.cameras = self.config.cameras self.cameras = self.config.cameras
self.has_camera = True
self.num_cameras = len(self.cameras)
self.is_connected = False self.is_connected = False
self.teleop = None self.teleop = None
self.logs = {} self.logs = {}
self.reachy: ReachySDK = ReachySDK(host=config.ip_address) self.reachy: ReachySDK = ReachySDK(host=config.ip_address)
self.reachy.turn_on()
self.is_connected = True # at init Reachy2 is in fact connected... self.is_connected = True # at init Reachy2 is in fact connected...
self.mobile_base_available = self.reachy.mobile_base is not None self.mobile_base_available = self.reachy.mobile_base is not None
@ -62,7 +66,6 @@ class ReachyRobot:
f"Cannot connect to Reachy at address {self.config.ip_address}. Maybe a connection already exists." f"Cannot connect to Reachy at address {self.config.ip_address}. Maybe a connection already exists."
) )
raise ConnectionError() raise ConnectionError()
self.reachy.turn_on() self.reachy.turn_on()
print(self.cameras) print(self.cameras)
if self.cameras is not None: if self.cameras is not None:
@ -116,6 +119,8 @@ class ReachyRobot:
action["mobile_base_y.vel"] = last_cmd_vel["y"] action["mobile_base_y.vel"] = last_cmd_vel["y"]
action["mobile_base_theta.vel"] = last_cmd_vel["theta"] action["mobile_base_theta.vel"] = last_cmd_vel["theta"]
action = torch.as_tensor(list(action.values()))
obs_dict = self.capture_observation() obs_dict = self.capture_observation()
action_dict = {} action_dict = {}
action_dict["action"] = action action_dict["action"] = action
@ -133,57 +138,25 @@ class ReachyRobot:
else: else:
odometry = {"x": 0, "y": 0, "theta": 0, "vx": 0, "vy": 0, "vtheta": 0} odometry = {"x": 0, "y": 0, "theta": 0, "vx": 0, "vy": 0, "vtheta": 0}
return { return {
"neck_yaw.pos": np.radians(self.reachy.head.neck.yaw.present_position), "neck_yaw.pos": self.reachy.head.neck.yaw.present_position,
"neck_pitch.pos": np.radians( "neck_pitch.pos": self.reachy.head.neck.pitch.present_position,
self.reachy.head.neck.pitch.present_position "neck_roll.pos": self.reachy.head.neck.roll.present_position,
), "r_shoulder_pitch.pos": self.reachy.r_arm.shoulder.pitch.present_position,
"neck_roll.pos": np.radians( "r_shoulder_roll.pos": self.reachy.r_arm.shoulder.roll.present_position,
self.reachy.head.neck.roll.present_position "r_elbow_yaw.pos": self.reachy.r_arm.elbow.yaw.present_position,
), "r_elbow_pitch.pos": self.reachy.r_arm.elbow.pitch.present_position,
"r_shoulder_pitch.pos": np.radians( "r_wrist_roll.pos": self.reachy.r_arm.wrist.roll.present_position,
self.reachy.r_arm.shoulder.pitch.present_position "r_wrist_pitch.pos": self.reachy.r_arm.wrist.pitch.present_position,
), "r_wrist_yaw.pos": self.reachy.r_arm.wrist.yaw.present_position,
"r_shoulder_roll.pos": np.radians( "r_gripper.pos": self.reachy.r_arm.gripper.present_position,
self.reachy.r_arm.shoulder.roll.present_position "l_shoulder_pitch.pos": self.reachy.l_arm.shoulder.pitch.present_position,
), "l_shoulder_roll.pos": self.reachy.l_arm.shoulder.roll.present_position,
"r_elbow_yaw.pos": np.radians( "l_elbow_yaw.pos": self.reachy.l_arm.elbow.yaw.present_position,
self.reachy.r_arm.elbow.yaw.present_position "l_elbow_pitch.pos": self.reachy.l_arm.elbow.pitch.present_position,
), "l_wrist_roll.pos": self.reachy.l_arm.wrist.roll.present_position,
"r_elbow_pitch.pos": np.radians( "l_wrist_pitch.pos": self.reachy.l_arm.wrist.pitch.present_position,
self.reachy.r_arm.elbow.pitch.present_position "l_wrist_yaw.pos": self.reachy.l_arm.wrist.yaw.present_position,
), "l_gripper.pos": self.reachy.l_arm.gripper.present_position,
"r_wrist_roll.pos": np.radians(
self.reachy.r_arm.wrist.roll.present_position
),
"r_wrist_pitch.pos": np.radians(
self.reachy.r_arm.wrist.pitch.present_position
),
"r_wrist_yaw.pos": np.radians(
self.reachy.r_arm.wrist.yaw.present_position
),
"r_gripper.pos": np.radians(self.reachy.r_arm.gripper.present_position),
"l_shoulder_pitch.pos": np.radians(
self.reachy.l_arm.shoulder.pitch.present_position
),
"l_shoulder_roll.pos": np.radians(
self.reachy.l_arm.shoulder.roll.present_position
),
"l_elbow_yaw.pos": np.radians(
self.reachy.l_arm.elbow.yaw.present_position
),
"l_elbow_pitch.pos": np.radians(
self.reachy.l_arm.elbow.pitch.present_position
),
"l_wrist_roll.pos": np.radians(
self.reachy.l_arm.wrist.roll.present_position
),
"l_wrist_pitch.pos": np.radians(
self.reachy.l_arm.wrist.pitch.present_position
),
"l_wrist_yaw.pos": np.radians(
self.reachy.l_arm.wrist.yaw.present_position
),
"l_gripper.pos": np.radians(self.reachy.l_arm.gripper.present_position),
"mobile_base.vx": odometry["vx"], "mobile_base.vx": odometry["vx"],
"mobile_base.vy": odometry["vy"], "mobile_base.vy": odometry["vy"],
"mobile_base.vtheta": odometry["vtheta"], "mobile_base.vtheta": odometry["vtheta"],
@ -232,29 +205,31 @@ class ReachyRobot:
if not self.is_connected: if not self.is_connected:
raise ConnectionError() raise ConnectionError()
self.reachy.head.neck.yaw.goal_position = action[0] self.reachy.head.neck.yaw.goal_position = float(action[0])
self.reachy.head.neck.pitch.goal_position = action[1] self.reachy.head.neck.pitch.goal_position = float(action[1])
self.reachy.head.neck.roll.goal_position = action[2] self.reachy.head.neck.roll.goal_position = float(action[2])
self.reachy.r_arm.shoulder.pitch.goal_position = action[3] self.reachy.r_arm.shoulder.pitch.goal_position = float(action[3])
self.reachy.r_arm.shoulder.roll.goal_position = action[4] self.reachy.r_arm.shoulder.roll.goal_position = float(action[4])
self.reachy.r_arm.elbow.yaw.goal_position = action[5] self.reachy.r_arm.elbow.yaw.goal_position = float(action[5])
self.reachy.r_arm.elbow.pitch.goal_position = action[6] self.reachy.r_arm.elbow.pitch.goal_position = float(action[6])
self.reachy.r_arm.wrist.roll.goal_position = action[7] self.reachy.r_arm.wrist.roll.goal_position = float(action[7])
self.reachy.r_arm.pitch.roll.goal_position = action[8] self.reachy.r_arm.wrist.roll.goal_position = float(action[8])
self.reachy.r_arm.wrist.yaw.goal_position = action[9] self.reachy.r_arm.wrist.yaw.goal_position = float(action[9])
self.reachy.r_arm.gripper.set_opening(action[10]) self.reachy.r_arm.gripper.set_opening(float(action[10]))
self.reachy.l_arm.shoulder.pitch.goal_position = action[11] self.reachy.l_arm.shoulder.pitch.goal_position = float(action[11])
self.reachy.l_arm.shoulder.roll.goal_position = action[12] self.reachy.l_arm.shoulder.roll.goal_position = float(action[12])
self.reachy.l_arm.elbow.yaw.goal_position = action[13] self.reachy.l_arm.elbow.yaw.goal_position = float(action[13])
self.reachy.l_arm.elbow.pitch.goal_position = action[14] self.reachy.l_arm.elbow.pitch.goal_position = float(action[14])
self.reachy.l_arm.wrist.roll.goal_position = action[15] self.reachy.l_arm.wrist.roll.goal_position = float(action[15])
self.reachy.l_arm.pitch.roll.goal_position = action[16] self.reachy.l_arm.wrist.roll.goal_position = float(action[16])
self.reachy.l_arm.wrist.yaw.goal_position = action[17] self.reachy.l_arm.wrist.yaw.goal_position = float(action[17])
self.reachy.l_arm.gripper.set_opening(action[18]) self.reachy.l_arm.gripper.set_opening(float(action[18]))
self.reachy.send_goal_positions() s = time.time()
self.reachy.send_goal_positions(check_positions=False)
print("send_goal_positions", time.time() - s)
if self.mobile_base_available: if self.mobile_base_available:
self.reachy.mobile_base.set_goal_speed(action[19], action[20], action[21]) self.reachy.mobile_base.set_goal_speed(action[19], action[20], action[21])

30
lerobot/configs/robot/reachy2.yaml
View File

@ -13,24 +13,26 @@ cameras:
head_left: head_left:
_target_: lerobot.common.robot_devices.cameras.reachy2.ReachyCamera _target_: lerobot.common.robot_devices.cameras.reachy2.ReachyCamera
name: teleop name: teleop
host: 172.17.135.207 # host: 172.17.135.207
host: localhost
port: 50065 port: 50065
image_type: left image_type: left
head_right: # head_right:
_target_: lerobot.common.robot_devices.cameras.reachy2.ReachyCamera # _target_: lerobot.common.robot_devices.cameras.reachy2.ReachyCamera
name: teleop # name: teleop
host: 172.17.135.207 # host: 172.17.135.207
port: 50065 # port: 50065
image_type: right # image_type: right
torso_rgb: torso_rgb:
_target_: lerobot.common.robot_devices.cameras.reachy2.ReachyCamera _target_: lerobot.common.robot_devices.cameras.reachy2.ReachyCamera
name: depth name: depth
host: 172.17.135.207 # host: 172.17.135.207
host: localhost
port: 50065 port: 50065
image_type: rgb image_type: rgb
torso_depth: # torso_depth:
_target_: lerobot.common.robot_devices.cameras.reachy2.ReachyCamera # _target_: lerobot.common.robot_devices.cameras.reachy2.ReachyCamera
name: depth # name: depth
host: 172.17.135.207 # host: 172.17.135.207
port: 50065 # port: 50065
image_type: depth # image_type: depth