Skip to content

Commit 55a406e

Browse files
alectimison-makeralectimison-maker
committed
Merge remote-tracking branch 'upstream/master' into fix/zed-calibration-dependency
2 parents f41a4fb + 110690b commit 55a406e

11 files changed

Lines changed: 127 additions & 105 deletions

File tree

‎examples/taxim/grasp_digit_demo.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -23,11 +23,11 @@ def _progress(iterable):
2323
class PickUpDemo:
2424
def __init__(self, env: gym.Env):
2525
self.env = env
26-
self._robot = cast(SimRobot, self.env.get_wrapper_attr("robot")["robot"])
26+
self._robot = cast(SimRobot, self.env.get_wrapper_attr("robot")["right"])
2727
self.home_pose = self._robot.get_cartesian_position()
2828

2929
def _action(self, pose: Pose, gripper: list[float]) -> dict[str, Any]:
30-
return {"robot": {"xyzrpy": pose.xyzrpy(), "gripper": gripper}}
30+
return {"right": {"xyzrpy": pose.xyzrpy(), "gripper": gripper}}
3131

3232
def get_object_pose(self, geom_name: str) -> Pose:
3333
model = self.env.get_wrapper_attr("sim").model

‎extensions/rcs_fr3/src/rcs_fr3/__main__.py‎

Lines changed: 12 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -21,13 +21,19 @@
2121
@fr3_app.command()
2222
def home(
2323
ip: Annotated[str, typer.Argument(help="IP of the robot")],
24-
shut: Annotated[bool, typer.Option("-s", help="Should the robot be shut down")] = False,
25-
unlock: Annotated[bool, typer.Option("-u", help="unlocks the robot")] = False,
26-
fh: Annotated[bool, typer.Option("-h", help="franka hand open")] = False,
2724
):
2825
"""Moves the FR3 to home position"""
29-
user, pw = load_creds_franka_desk()
30-
rcs_fr3.desk.home(ip, user, pw, shut, unlock, fh)
26+
rcs_fr3.desk.home(ip)
27+
28+
29+
# griper command
30+
@fr3_app.command()
31+
def gripper(
32+
ip: Annotated[str, typer.Argument(help="IP of the robot")],
33+
close_gripper: Annotated[bool, typer.Option("-c", help="close gripper")] = False,
34+
):
35+
"""Opens or closes the gripper"""
36+
rcs_fr3.desk.gripper(ip, close_gripper)
3137

3238

3339
@fr3_app.command()
@@ -36,8 +42,7 @@ def info(
3642
include_gripper: Annotated[bool, typer.Option("-g", help="includes gripper")] = False,
3743
):
3844
"""Prints info about the robots current joint position and end effector pose, optionally also the gripper."""
39-
user, pw = load_creds_franka_desk()
40-
rcs_fr3.desk.info(ip, user, pw, include_gripper)
45+
rcs_fr3.desk.info(ip, include_gripper)
4146

4247

4348
@fr3_app.command()

‎extensions/rcs_fr3/src/rcs_fr3/configs.py‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -119,7 +119,7 @@ def config(self) -> FR3MultiHardwareEnvCreatorConfig:
119119
cfg = base.config()
120120
cfg.robot_cfg.async_control = True
121121
cfg.robot_cfg.ip = self.robot_ip
122-
cfg.robot_cfg.tcp_offset = rcs.GRIPPER_OFFSETS[common.GripperType("Robotiq2F85")]
122+
cfg.robot_cfg.tcp_offset = rcs.GRIPPER_TCP_OFFSETS[common.GripperType("Robotiq2F85")]
123123
cfg.robot_cfg.q_home = rcs.HOME_POSITIONS["FR3_DROID"]
124124

125125
return FR3MultiHardwareEnvCreatorConfig(
@@ -209,8 +209,8 @@ def config(self) -> FR3MultiHardwareEnvCreatorConfig:
209209

210210
cfg = super().config()
211211
cfg.camera_cfgs = None
212-
cfg.robot_cfgs["left"].tcp_offset = rcs.GRIPPER_OFFSETS[common.GripperType("Robotiq2F85")]
213-
cfg.robot_cfgs["right"].tcp_offset = rcs.GRIPPER_OFFSETS[common.GripperType("Robotiq2F85")]
212+
cfg.robot_cfgs["left"].tcp_offset = rcs.GRIPPER_TCP_OFFSETS[common.GripperType("Robotiq2F85")]
213+
cfg.robot_cfgs["right"].tcp_offset = rcs.GRIPPER_TCP_OFFSETS[common.GripperType("Robotiq2F85")]
214214
cfg.robot_cfgs["left"].q_home = rcs.HOME_POSITIONS["FR3_DUO_LEFT"]
215215
cfg.robot_cfgs["right"].q_home = rcs.HOME_POSITIONS["FR3_DUO_RIGHT"]
216216
cfg.gripper_cfgs = {

‎extensions/rcs_fr3/src/rcs_fr3/desk.py‎

Lines changed: 40 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -45,45 +45,49 @@ def load_creds_franka_desk(postfix: str = "") -> tuple[str, str]:
4545
return os.environ[username_key], os.environ[password_key]
4646

4747

48-
def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False, fh: bool = False):
49-
with Desk.fci(ip, username, password, unlock=unlock):
48+
def home(ip: str):
49+
default_env = DefaultFR3HardwareEnv()
50+
default_env.ip = ip
51+
env_cfg = default_env.config()
52+
robot_cfg = env_cfg.robot_cfg
53+
robot_cfg.speed_factor = 0.2
54+
f = rcs_fr3.hw.Franka(robot_cfg)
55+
f.move_home()
56+
57+
58+
def gripper(ip: str, close_gripper: bool):
59+
60+
default_env = DefaultFR3HardwareEnv()
61+
default_env.ip = ip
62+
env_cfg = default_env.config()
63+
config_hand = env_cfg.gripper_cfg
64+
assert isinstance(config_hand, rcs_fr3.hw.FHConfig)
65+
g = rcs_fr3.hw.FrankaHand(config_hand)
66+
if close_gripper:
67+
g.shut()
68+
else:
69+
g.open()
70+
71+
72+
def info(ip: str, include_hand: bool = False):
73+
robot_cfg = rcs_fr3.hw.FR3Config(ip=ip)
74+
robot_cfg.speed_factor = 0.2
75+
f = rcs_fr3.hw.Franka(robot_cfg)
76+
print("Robot info:")
77+
print("Current cartesian position:")
78+
print(f.get_cartesian_position())
79+
print("Current joint position:")
80+
print(f.get_joint_position())
81+
if include_hand:
5082
default_env = DefaultFR3HardwareEnv()
5183
default_env.ip = ip
5284
env_cfg = default_env.config()
53-
robot_cfg = env_cfg.robot_cfg
54-
robot_cfg.speed_factor = 0.2
55-
f = rcs_fr3.hw.Franka(robot_cfg)
56-
if fh:
57-
config_hand = env_cfg.gripper_cfg
58-
assert isinstance(config_hand, rcs_fr3.hw.FHConfig)
59-
g = rcs_fr3.hw.FrankaHand(config_hand)
60-
if shut:
61-
g.shut()
62-
else:
63-
g.open()
64-
f.move_home()
65-
66-
67-
def info(ip: str, username: str, password: str, include_hand: bool = False):
68-
with Desk.fci(ip, username, password):
69-
robot_cfg = rcs_fr3.hw.FR3Config(ip=ip)
70-
robot_cfg.speed_factor = 0.2
71-
f = rcs_fr3.hw.Franka(robot_cfg)
72-
print("Robot info:")
73-
print("Current cartesian position:")
74-
print(f.get_cartesian_position())
75-
print("Current joint position:")
76-
print(f.get_joint_position())
77-
if include_hand:
78-
default_env = DefaultFR3HardwareEnv()
79-
default_env.ip = ip
80-
env_cfg = default_env.config()
81-
config_hand = env_cfg.gripper_cfg
82-
assert isinstance(config_hand, rcs_fr3.hw.FHConfig)
83-
g = rcs_fr3.hw.FrankaHand(config_hand)
84-
print("Gripper info:")
85-
print("Current normalized width:")
86-
print(g.get_normalized_width())
85+
config_hand = env_cfg.gripper_cfg
86+
assert isinstance(config_hand, rcs_fr3.hw.FHConfig)
87+
g = rcs_fr3.hw.FrankaHand(config_hand)
88+
print("Gripper info:")
89+
print("Current normalized width:")
90+
print(g.get_normalized_width())
8791

8892

8993
def lock(ip: str, username: str, password: str):

‎extensions/rcs_panda/src/rcs_panda/__main__.py‎

Lines changed: 11 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -21,12 +21,18 @@
2121
@panda_app.command()
2222
def home(
2323
ip: Annotated[str, typer.Argument(help="IP of the robot")],
24-
shut: Annotated[bool, typer.Option("-s", help="Should the robot be shut down")] = False,
25-
unlock: Annotated[bool, typer.Option("-u", help="unlocks the robot")] = False,
2624
):
2725
"""Moves the panda to home position"""
28-
user, pw = load_creds_franka_desk()
29-
rcs_panda.desk.home(ip, user, pw, shut, unlock)
26+
rcs_panda.desk.home(ip)
27+
28+
29+
@panda_app.command()
30+
def gripper(
31+
ip: Annotated[str, typer.Argument(help="IP of the robot")],
32+
close_gripper: Annotated[bool, typer.Option("-c", help="close gripper")] = False,
33+
):
34+
"""Opens or closes the gripper"""
35+
rcs_panda.desk.gripper(ip, close_gripper)
3036

3137

3238
@panda_app.command()
@@ -35,8 +41,7 @@ def info(
3541
include_gripper: Annotated[bool, typer.Option("-g", help="includes gripper")] = False,
3642
):
3743
"""Prints info about the robots current joint position and end effector pose, optionally also the gripper."""
38-
user, pw = load_creds_franka_desk()
39-
rcs_panda.desk.info(ip, user, pw, include_gripper)
44+
rcs_panda.desk.info(ip, include_gripper)
4045

4146

4247
@panda_app.command()

‎extensions/rcs_panda/src/rcs_panda/configs.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -60,7 +60,7 @@ def config(self) -> PandaMultiHardwareEnvCreatorConfig:
6060
cfg = base.config()
6161
cfg.robot_cfg.async_control = True
6262
cfg.robot_cfg.ip = self.robot_ip
63-
cfg.robot_cfg.tcp_offset = rcs.GRIPPER_OFFSETS[common.GripperType("Robotiq2F85")]
63+
cfg.robot_cfg.tcp_offset = rcs.GRIPPER_TCP_OFFSETS[common.GripperType("Robotiq2F85")]
6464
cfg.robot_cfg.q_home = rcs.ROBOTS[RobotType.Panda].q_home
6565

6666
return PandaMultiHardwareEnvCreatorConfig(

‎extensions/rcs_panda/src/rcs_panda/desk.py‎

Lines changed: 36 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -45,44 +45,48 @@ def load_creds_franka_desk(postfix: str = "") -> tuple[str, str]:
4545
return os.environ[username_key], os.environ[password_key]
4646

4747

48-
def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False):
49-
with Desk.fci(ip, username, password, unlock=unlock):
48+
def home(ip: str):
49+
default_env = DefaultPandaHardwareEnv()
50+
default_env.ip = ip
51+
env_cfg = default_env.config()
52+
robot_cfg = env_cfg.robot_cfg
53+
robot_cfg.speed_factor = 0.2
54+
f = rcs_panda.hw.Franka(robot_cfg)
55+
f.move_home()
56+
57+
58+
def gripper(ip: str, close_gripper: bool):
59+
default_env = DefaultPandaHardwareEnv()
60+
default_env.ip = ip
61+
env_cfg = default_env.config()
62+
config_hand = env_cfg.gripper_cfg
63+
assert isinstance(config_hand, rcs_panda.hw.FHConfig)
64+
g = rcs_panda.hw.FrankaHand(config_hand)
65+
if close_gripper:
66+
g.shut()
67+
else:
68+
g.open()
69+
70+
71+
def info(ip: str, include_hand: bool = False):
72+
robot_cfg = rcs_panda.hw.PandaConfig(ip=ip)
73+
robot_cfg.speed_factor = 0.2
74+
f = rcs_panda.hw.Franka(robot_cfg)
75+
print("Robot info:")
76+
print("Current cartesian position:")
77+
print(f.get_cartesian_position())
78+
print("Current joint position:")
79+
print(f.get_joint_position())
80+
if include_hand:
5081
default_env = DefaultPandaHardwareEnv()
5182
default_env.ip = ip
5283
env_cfg = default_env.config()
53-
robot_cfg = env_cfg.robot_cfg
54-
robot_cfg.speed_factor = 0.2
55-
f = rcs_panda.hw.Franka(robot_cfg)
5684
config_hand = env_cfg.gripper_cfg
5785
assert isinstance(config_hand, rcs_panda.hw.FHConfig)
5886
g = rcs_panda.hw.FrankaHand(config_hand)
59-
if shut:
60-
g.shut()
61-
else:
62-
g.open()
63-
f.move_home()
64-
65-
66-
def info(ip: str, username: str, password: str, include_hand: bool = False):
67-
with Desk.fci(ip, username, password):
68-
robot_cfg = rcs_panda.hw.PandaConfig(ip=ip)
69-
robot_cfg.speed_factor = 0.2
70-
f = rcs_panda.hw.Franka(robot_cfg)
71-
print("Robot info:")
72-
print("Current cartesian position:")
73-
print(f.get_cartesian_position())
74-
print("Current joint position:")
75-
print(f.get_joint_position())
76-
if include_hand:
77-
default_env = DefaultPandaHardwareEnv()
78-
default_env.ip = ip
79-
env_cfg = default_env.config()
80-
config_hand = env_cfg.gripper_cfg
81-
assert isinstance(config_hand, rcs_panda.hw.FHConfig)
82-
g = rcs_panda.hw.FrankaHand(config_hand)
83-
print("Gripper info:")
84-
print("Current normalized width:")
85-
print(g.get_normalized_width())
87+
print("Gripper info:")
88+
print("Current normalized width:")
89+
print(g.get_normalized_width())
8690

8791

8892
def lock(ip: str, username: str, password: str):

‎extensions/rcs_taxim/src/rcs_taxim/creators.py‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -84,6 +84,7 @@ def __call__(
8484

8585
scene = EmptyWorldFR3()
8686
cfg = scene.config()
87+
cfg.robot_cfgs["right"].tcp_offset = rcs.GRIPPER_TCP_OFFSETS[rcs.common.GripperType("Robotiq2F85")]
8788
cfg.control_mode = control_mode
8889
cfg.headless = render_mode != "human"
8990
cfg.sim_cfg.realtime = render_mode == "human"
@@ -97,8 +98,8 @@ def __call__(
9798
cfg.relative_to = RelativeTo.LAST_STEP if delta_actions else RelativeTo.NONE
9899
if not delta_actions:
99100
cfg.max_relative_movement = None
100-
cfg.gripper_cfgs = {"robot": _taxim_gripper_cfg()}
101-
cfg.gripper_offsets = None
101+
cfg.gripper_cfgs = {"right": _taxim_gripper_cfg()}
102+
cfg.gripper_offsets = {"right": rcs.GRIPPER_MOUNT_OFFSETS[rcs.common.GripperType("Robotiq2F85")]}
102103
cfg.root_frame_objects = {
103104
"": (
104105
rcs.OBJECT_PATHS["green_cube"],

‎python/rcs/__init__.py‎

Lines changed: 10 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -190,11 +190,20 @@ class RobotMetaConfig:
190190
common.GripperType("Robotiq2F85"): "assets/grippers/robotiq_2f85/robotiq_2f85.xml",
191191
}
192192

193-
GRIPPER_OFFSETS: dict[common.GripperType, common.Pose] = {
193+
GRIPPER_TCP_OFFSETS: dict[common.GripperType, common.Pose] = {
194194
common.GripperType.FrankaHand: common.Pose(pose_matrix=common.FrankaHandTCPOffset()),
195195
common.GripperType("Robotiq2F85"): common.Pose(translation=np.array([0, 0.0, 0.1493])),
196196
}
197197

198+
GRIPPER_MOUNT_OFFSETS: dict[common.GripperType, common.Pose] = {
199+
common.GripperType.FrankaHand: common.Pose(
200+
rotation=common.FrankaHandTCPOffset()[:3, :3], translation=np.array([0.0, 0.0, 0.0])
201+
),
202+
common.GripperType("Robotiq2F85"): common.Pose(
203+
translation=np.array([0.0, 0.0, 0.0]), quaternion=np.array([0.0, 0.0, 0.7071068, 0.7071068])
204+
),
205+
}
206+
198207
SCENE_PATHS: dict[str, str] = {"empty_world": "assets/scenes/empty_world/scene.xml"}
199208

200209
OBJECT_PATHS: dict[str, str] = {
@@ -212,9 +221,6 @@ class RobotMetaConfig:
212221
TASKS: dict[str, Any] = {}
213222

214223
DEFAULT_TRANSFORMS = {
215-
"FR3_ROBOTIQ_GRIPPER": common.Pose(
216-
translation=np.array([0.0, 0.0, 0.0]), quaternion=np.array([0.0, 0.0, 0.7071068, 0.7071068])
217-
),
218224
"FR3_ROBOTIQ_WRIST_D405_MOUNT": common.Pose(
219225
translation=np.array([0.0, 0.0, 0.0]), quaternion=np.array([0.0, 0.0, 0.7071068, 0.7071068])
220226
),

‎python/rcs/envs/configs.py‎

Lines changed: 8 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,7 @@
44

55
import gymnasium as gym
66
import numpy as np
7-
from rcs._core.common import FrankaHandTCPOffset, GripperType, RobotType
7+
from rcs._core.common import GripperType, RobotType
88
from rcs._core.sim import (
99
CameraType,
1010
SimCameraConfig,
@@ -24,7 +24,8 @@
2424
from rcs import (
2525
CAMERA_PATHS,
2626
DEFAULT_TRANSFORMS,
27-
GRIPPER_OFFSETS,
27+
GRIPPER_MOUNT_OFFSETS,
28+
GRIPPER_TCP_OFFSETS,
2829
OBJECT_PATHS,
2930
SCENE_PATHS,
3031
)
@@ -39,7 +40,7 @@ def config(self) -> SimEnvCreatorConfig:
3940
q_home[-1] = np.pi / 4
4041
robot_cfg: SimRobotConfig[Literal[7]] = SimRobotConfig(
4142
robot_type=RobotType.FR3,
42-
tcp_offset=GRIPPER_OFFSETS[rcs.common.GripperType.FrankaHand],
43+
tcp_offset=GRIPPER_TCP_OFFSETS[rcs.common.GripperType.FrankaHand],
4344
attachment_site=rcs.ROBOTS[RobotType.FR3].attachment_site,
4445
kinematic_model_path=rcs.ROBOTS[RobotType.FR3].mjcf_model_path,
4546
joint_rotational_tolerance=0.05 * (np.pi / 180.0),
@@ -153,9 +154,7 @@ def config(self) -> SimEnvCreatorConfig:
153154
),
154155
}
155156
gripper_offsets: dict[str, rcs.common.Pose] | None = {
156-
self.robot_prefix_template: rcs.common.Pose(
157-
rotation=FrankaHandTCPOffset()[:3, :3], translation=np.array([0.0, 0.0, 0.0])
158-
)
157+
self.robot_prefix_template: GRIPPER_MOUNT_OFFSETS[rcs.common.GripperType.FrankaHand]
159158
}
160159
return SimEnvCreatorConfig(
161160
robot_cfgs=robot_cfgs,
@@ -186,7 +185,7 @@ class EmptyWorldFR3Duo(SimEnvCreator):
186185

187186
def config(self) -> SimEnvCreatorConfig:
188187
robot_cfg: SimRobotConfig[Literal[7]] = SimRobotConfig(
189-
tcp_offset=GRIPPER_OFFSETS[rcs.common.GripperType("Robotiq2F85")],
188+
tcp_offset=GRIPPER_TCP_OFFSETS[rcs.common.GripperType("Robotiq2F85")],
190189
robot_type=RobotType.FR3,
191190
attachment_site=rcs.ROBOTS[RobotType.FR3].attachment_site,
192191
kinematic_model_path=rcs.ROBOTS[RobotType.FR3].mjcf_model_path,
@@ -327,9 +326,7 @@ def config(self) -> SimEnvCreatorConfig:
327326
robot_name="right",
328327
),
329328
}
330-
gripper_offset = rcs.common.Pose(
331-
quaternion=np.array(self.gripper_mesh_quaternion_offset), translation=np.array([0.0, 0.0, 0.0])
332-
)
329+
gripper_offset = GRIPPER_MOUNT_OFFSETS[rcs.common.GripperType("Robotiq2F85")]
333330
return SimEnvCreatorConfig(
334331
robot_cfgs=robot_cfgs,
335332
sim_cfg=sim_cfg,
@@ -363,7 +360,7 @@ def config(self) -> SimEnvCreatorConfig:
363360
lead_robot_name = self.lead_robot_name(cfg)
364361

365362
robot_cfg = cfg.robot_cfgs[lead_robot_name]
366-
robot_cfg.tcp_offset = GRIPPER_OFFSETS[rcs.common.GripperType("Robotiq2F85")]
363+
robot_cfg.tcp_offset = GRIPPER_TCP_OFFSETS[rcs.common.GripperType("Robotiq2F85")]
367364
robot_cfg.attachment_site = rcs.ROBOTS[rt].attachment_site
368365
robot_cfg.kinematic_model_path = rcs.ROBOTS[rt].mjcf_model_path
369366
robot_cfg.arm_collision_geoms = []

0 commit comments

Comments
 (0)