Skip to content

Commit 5032dc0

Browse files
juelgjuelg
committed
iris scene
1 parent b4ef47e commit 5032dc0

7 files changed

Lines changed: 76 additions & 19 deletions

File tree

‎assets/scenes/fr3_simple_pick_up/scene.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,7 @@
2828
<camera name="side_view" pos="1.760 -1.205 0.621" xyaxes="0.681 0.733 0.000 -0.105 0.097 0.990"/>
2929

3030
<body name="box_geom" pos="0.44 0.1 0.03" quat="0 0 0 1">
31-
<geom type="box" size="0.032 0.016 0.0288" rgba="0 0.984 0.373 1" name="box_geom" friction="1 0.3 0.1" density="50" />
31+
<geom type="box" size="0.032 0.016 0.0288" rgba="0 0.984 0.373 1" name="box_geom" friction="1 0.3 0.1" density="50" group="1" />
3232
<joint type="free" name="box_joint" />
3333
</body>
3434
</worldbody>

‎examples/teleop/quest_iris_dual_arm.py‎

Lines changed: 44 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -3,8 +3,10 @@
33
from time import sleep
44

55
import numpy as np
6+
import rcs
7+
from rcs._core import common
68
from rcs._core.common import RPY, Pose, RobotPlatform
7-
from rcs._core.sim import SimConfig
9+
from rcs._core.sim import CameraType, SimCameraConfig, SimConfig
810
from rcs.camera.hw import HardwareCameraSet
911
from rcs.envs.base import (
1012
ControlMode,
@@ -19,7 +21,7 @@
1921
from rcs.utils import SimpleFrameRate
2022
from rcs_fr3.creators import RCSFR3MultiEnvCreator
2123
from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg
22-
from rcs_realsense.utils import default_realsense
24+
# from rcs_realsense.utils import default_realsense
2325
from simpub.core.simpub_server import SimPublisher
2426
from simpub.parser.simdata import SimObject, SimScene
2527
from simpub.sim.mj_publisher import MujocoPublisher
@@ -39,10 +41,14 @@
3941
# "left": "192.168.102.1",
4042
"right": "192.168.101.1",
4143
}
44+
ROBOT2ID = {
45+
"left": "0",
46+
"right": "1",
47+
}
4248

4349

44-
# ROBOT_INSTANCE = RobotPlatform.SIMULATION
45-
ROBOT_INSTANCE = RobotPlatform.HARDWARE
50+
ROBOT_INSTANCE = RobotPlatform.SIMULATION
51+
# ROBOT_INSTANCE = RobotPlatform.HARDWARE
4652
RECORD_FPS = 30
4753
# set camera dict to none disable cameras
4854
# CAMERA_DICT = {
@@ -87,7 +93,7 @@ def __init__(self, env: RelativeActionSpace):
8793
self._reset_lock = threading.Lock()
8894
self._env = env
8995

90-
self.controller_names = ROBOT2IP.keys() if ROBOT_INSTANCE == RobotPlatform.HARDWARE else ["right"]
96+
self.controller_names = ROBOT2IP.keys() if ROBOT_INSTANCE == RobotPlatform.HARDWARE else ROBOT2ID.keys()
9197
self._trg_btn = {"left": "index_trigger", "right": "index_trigger"}
9298
self._grp_btn = {"left": "hand_trigger", "right": "hand_trigger"}
9399
self._start_btn = "A"
@@ -100,7 +106,7 @@ def __init__(self, env: RelativeActionSpace):
100106
self._last_controller_pose = {key: Pose() for key in self.controller_names}
101107
self._offset_pose = {key: Pose() for key in self.controller_names}
102108

103-
for robot in ROBOT2IP:
109+
for robot in ROBOT2IP if ROBOT_INSTANCE == RobotPlatform.HARDWARE else ROBOT2ID:
104110
self._env.envs[robot].set_origin_to_current()
105111

106112
self._step_env = False
@@ -326,22 +332,50 @@ def main():
326332

327333
else:
328334
# FR3
329-
robot_cfg = default_sim_robot_cfg("fr3_empty_world")
335+
rcs.scenes["rcs_icra_scene"] = rcs.Scene(
336+
mjcf_scene="/home/tobi/coding/rcs_clones/prs/models/scenes/rcs_icra_scene/scene.xml",
337+
mjcf_robot=rcs.scenes["fr3_simple_pick_up"].mjcf_robot,
338+
robot_type=common.RobotType.FR3,
339+
)
340+
rcs.scenes["pick"] = rcs.Scene(
341+
mjcf_scene="/home/tobi/coding/rcs_clones/prs/assets/scenes/fr3_simple_pick_up/scene.xml",
342+
mjcf_robot=rcs.scenes["fr3_simple_pick_up"].mjcf_robot,
343+
robot_type=common.RobotType.FR3,
344+
)
345+
346+
# robot_cfg = default_sim_robot_cfg("fr3_empty_world")
347+
# robot_cfg = default_sim_robot_cfg("fr3_simple_pick_up")
348+
robot_cfg = default_sim_robot_cfg("rcs_icra_scene")
349+
# robot_cfg = default_sim_robot_cfg("pick")
350+
351+
resolution = (256, 256)
352+
cameras = {
353+
cam: SimCameraConfig(
354+
identifier=cam,
355+
type=CameraType.fixed,
356+
resolution_height=resolution[1],
357+
resolution_width=resolution[0],
358+
frame_rate=0,
359+
)
360+
for cam in ["side", "wrist"]
361+
}
330362

331363
sim_cfg = SimConfig()
332364
sim_cfg.async_control = True
333365
env_rel = SimMultiEnvCreator()(
334-
name2id=ROBOT2IP,
366+
name2id=ROBOT2ID,
335367
robot_cfg=robot_cfg,
336368
control_mode=ControlMode.CARTESIAN_TQuat,
337369
gripper_cfg=default_sim_gripper_cfg(),
338-
# cameras=default_mujoco_cameraset_cfg(),
370+
# cameras=cameras,
339371
max_relative_movement=0.5,
340372
relative_to=RelativeTo.CONFIGURED_ORIGIN,
341373
sim_cfg=sim_cfg,
342374
)
343-
sim = env_rel.unwrapped.envs[ROBOT2IP.keys().__iter__().__next__()].sim # type: ignore
375+
# sim = env_rel.unwrapped.envs[ROBOT2ID.keys().__iter__().__next__()].sim # type: ignore
376+
sim = env_rel.get_wrapper_attr("sim")
344377
sim.open_gui()
378+
# MySimPublisher(MySimScene(), MQ3_ADDR)
345379
MujocoPublisher(sim.model, sim.data, MQ3_ADDR, visible_geoms_groups=list(range(1, 3)))
346380

347381
env_rel.reset()

‎python/rcs/__init__.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@
1818
class Scene:
1919
"""Scene configuration."""
2020

21-
mjb: str
21+
mjb: str | None = None
2222
"""Path to the Mujoco binary scene file."""
2323
mjcf_scene: str
2424
"""Path to the Mujoco scene XML file."""

‎python/rcs/_core/sim.pyi‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -136,6 +136,8 @@ class SimGripperConfig(rcs._core.common.GripperConfig):
136136
min_actuator_width: float
137137
min_joint_width: float
138138
seconds_between_callbacks: float
139+
def __copy__(self) -> SimGripperConfig: ...
140+
def __deepcopy__(self, arg0: dict) -> SimGripperConfig: ...
139141
def __init__(self) -> None: ...
140142
def add_id(self, id: str) -> None: ...
141143

@@ -169,6 +171,8 @@ class SimRobotConfig(rcs._core.common.RobotConfig):
169171
mjcf_scene_path: str
170172
seconds_between_callbacks: float
171173
trajectory_trace: bool
174+
def __copy__(self) -> SimRobotConfig: ...
175+
def __deepcopy__(self, arg0: dict) -> SimRobotConfig: ...
172176
def __init__(self) -> None: ...
173177
def add_id(self, id: str) -> None: ...
174178

‎python/rcs/envs/creators.py‎

Lines changed: 10 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,4 @@
1+
import copy
12
import logging
23
import typing
34
from functools import partial
@@ -157,22 +158,26 @@ def __call__( # type: ignore
157158
# ik = rcs_robotics_library._core.rl.RoboticsLibraryIK(robot_cfg.kinematic_model_path)
158159

159160
robots: dict[str, rcs.sim.SimRobot] = {}
160-
for key in name2id:
161-
robots[key] = rcs.sim.SimRobot(sim=simulation, ik=ik, cfg=robot_cfg)
161+
for key, mid in name2id.items():
162+
cfg = copy.copy(robot_cfg)
163+
cfg.add_id(mid)
164+
robots[key] = rcs.sim.SimRobot(sim=simulation, ik=ik, cfg=cfg)
162165

163166
envs = {}
164-
for key in name2id:
167+
for key, mid in name2id.items():
165168
env: gym.Env = RobotEnv(robots[key], control_mode)
166-
env = RobotSimWrapper(env, simulation, sim_wrapper)
167169
if gripper_cfg is not None:
168-
gripper = rcs.sim.SimGripper(simulation, gripper_cfg)
170+
cfg = copy.copy(gripper_cfg)
171+
cfg.add_id(mid)
172+
gripper = rcs.sim.SimGripper(simulation, cfg)
169173
env = GripperWrapper(env, gripper, binary=True)
170174

171175
if max_relative_movement is not None:
172176
env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to)
173177
envs[key] = env
174178

175179
env = MultiRobotWrapper(envs)
180+
env = RobotSimWrapper(env, simulation, sim_wrapper)
176181
if cameras is not None:
177182
camera_set = typing.cast(
178183
BaseCameraSet, SimCameraSet(simulation, cameras, physical_units=True, render_on_demand=True)

‎python/rcs/envs/utils.py‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,7 @@ def default_sim_robot_cfg(scene: str = "fr3_empty_world", idx: str = "0") -> sim
1919
robot_cfg = rcs.sim.SimRobotConfig()
2020
robot_cfg.robot_type = rcs.scenes[scene].robot_type
2121
robot_cfg.tcp_offset = common.Pose(common.FrankaHandTCPOffset())
22-
robot_cfg.add_id(idx)
22+
# robot_cfg.add_id(idx)
2323
if rcs.scenes[scene].mjb is not None:
2424
robot_cfg.mjcf_scene_path = rcs.scenes[scene].mjb
2525
else:
@@ -38,7 +38,9 @@ def default_tilburg_hw_hand_cfg(file: str | PathLike | None = None) -> THConfig:
3838

3939
def default_sim_gripper_cfg(idx: str = "0") -> sim.SimGripperConfig:
4040
cfg = sim.SimGripperConfig()
41-
cfg.add_id(idx)
41+
cfg.collision_geoms = []
42+
cfg.collision_geoms_fingers = []
43+
# cfg.add_id(idx)
4244
return cfg
4345

4446

‎src/pybind/rcs.cpp‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -472,6 +472,12 @@ PYBIND11_MODULE(_core, m) {
472472
.def_readwrite("joints", &rcs::sim::SimRobotConfig::joints)
473473
.def_readwrite("actuators", &rcs::sim::SimRobotConfig::actuators)
474474
.def_readwrite("base", &rcs::sim::SimRobotConfig::base)
475+
.def("__copy__", [](const rcs::sim::SimRobotConfig &self) {
476+
return rcs::sim::SimRobotConfig(self);
477+
})
478+
.def("__deepcopy__", [](const rcs::sim::SimRobotConfig &self, py::dict) {
479+
return rcs::sim::SimRobotConfig(self);
480+
})
475481
.def("add_id", &rcs::sim::SimRobotConfig::add_id, py::arg("id"));
476482
py::class_<rcs::sim::SimRobotState, rcs::common::RobotState>(sim,
477483
"SimRobotState")
@@ -510,6 +516,12 @@ PYBIND11_MODULE(_core, m) {
510516
&rcs::sim::SimGripperConfig::max_actuator_width)
511517
.def_readwrite("min_actuator_width",
512518
&rcs::sim::SimGripperConfig::min_actuator_width)
519+
.def("__copy__", [](const rcs::sim::SimGripperConfig &self) {
520+
return rcs::sim::SimGripperConfig(self);
521+
})
522+
.def("__deepcopy__", [](const rcs::sim::SimGripperConfig &self, py::dict) {
523+
return rcs::sim::SimGripperConfig(self);
524+
})
513525
.def("add_id", &rcs::sim::SimGripperConfig::add_id, py::arg("id"));
514526
py::class_<rcs::sim::SimGripperState, rcs::common::GripperState>(
515527
sim, "SimGripperState")

0 commit comments

Comments
 (0)