Skip to content

Commit 85446b1

Browse files
juelgjuelg
committed
temp
1 parent baee473 commit 85446b1

File tree

10 files changed

+274
-50
lines changed

10 files changed

+274
-50
lines changed

‎assets/scenes/fr3_empty_world/fr3_0.xml‎

Lines changed: 119 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,123 @@
11
<mujoco>
2+
<compiler angle="radian" meshdir="assets"/>
3+
4+
<option integrator="implicitfast" impratio="20" noslip_iterations="5" cone="elliptic"/>
5+
6+
<default>
7+
<default class="fr3">
8+
<joint armature="0.1" damping="1"/>
9+
<position inheritrange="1"/>
10+
<default class="finger">
11+
<joint axis="0 1 0" type="slide" range="0 0.04"/>
12+
</default>
13+
<default class="visual">
14+
<geom type="mesh" group="2" contype="0" conaffinity="0"/>
15+
</default>
16+
<default class="collision">
17+
<geom type="mesh" group="3" mass="0" density="0"/>
18+
<default class="fingertip_pad_collision_1">
19+
<geom type="box" size="0.0085 0.004 0.0085" pos="0 0.0055 0.0445"/>
20+
</default>
21+
<default class="fingertip_pad_collision_2">
22+
<geom type="box" size="0.003 0.002 0.003" pos="0.0055 0.002 0.05"/>
23+
</default>
24+
<default class="fingertip_pad_collision_3">
25+
<geom type="box" size="0.003 0.002 0.003" pos="-0.0055 0.002 0.05"/>
26+
</default>
27+
<default class="fingertip_pad_collision_4">
28+
<geom type="box" size="0.003 0.002 0.0035" pos="0.0055 0.002 0.0395"/>
29+
</default>
30+
<default class="fingertip_pad_collision_5">
31+
<geom type="box" size="0.003 0.002 0.0035" pos="-0.0055 0.002 0.0395"/>
32+
</default>
33+
</default>
34+
<site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4"/>
35+
</default>
36+
<default class="d435i">
37+
<material specular="0" shininess="0.25"/>
38+
<default class="visual_d435i">
39+
<geom group="2" type="mesh" contype="0" conaffinity="0" mass="0"/>
40+
</default>
41+
<default class="collision_d435i">
42+
<geom group="3" type="mesh" mass="0"/>
43+
</default>
44+
</default>
45+
</default>
46+
47+
<asset>
48+
<material name="black" rgba=".2 .2 .2 1"/>
49+
<material name="white" rgba="1 1 1 1"/>
50+
<material name="red" rgba="1 0.072272 0.039546 1"/>
51+
<material name="gray" rgba="0.863156 0.863156 0.863157 1"/>
52+
<material name="button_green" rgba="0.102241 0.571125 0.102242 1"/>
53+
<material name="button_red" rgba="0.520996 0.008023 0.013702 1"/>
54+
<material name="button_blue" rgba="0.024157 0.445201 0.737911 1"/>
55+
56+
<mesh file="fr3_link0_0.obj"/>
57+
<mesh file="fr3_link0_1.obj"/>
58+
<mesh file="fr3_link0_2.obj"/>
59+
<mesh file="fr3_link0_3.obj"/>
60+
<mesh file="fr3_link0_4.obj"/>
61+
<mesh file="fr3_link0_5.obj"/>
62+
<mesh file="fr3_link0_6.obj"/>
63+
<mesh file="fr3_link1.obj"/>
64+
<mesh file="fr3_link2.obj"/>
65+
<mesh file="fr3_link3_0.obj"/>
66+
<mesh file="fr3_link3_1.obj"/>
67+
<mesh file="fr3_link4_0.obj"/>
68+
<mesh file="fr3_link4_1.obj"/>
69+
<mesh file="fr3_link5_0.obj"/>
70+
<mesh file="fr3_link5_1.obj"/>
71+
<mesh file="fr3_link5_2.obj"/>
72+
<mesh file="fr3_link6_0.obj"/>
73+
<mesh file="fr3_link6_1.obj"/>
74+
<mesh file="fr3_link6_2.obj"/>
75+
<mesh file="fr3_link6_3.obj"/>
76+
<mesh file="fr3_link6_4.obj"/>
77+
<mesh file="fr3_link6_5.obj"/>
78+
<mesh file="fr3_link6_6.obj"/>
79+
<mesh file="fr3_link6_7.obj"/>
80+
<mesh file="fr3_link7_0.obj"/>
81+
<mesh file="fr3_link7_1.obj"/>
82+
<mesh file="fr3_link7_2.obj"/>
83+
<mesh file="fr3_link7_3.obj"/>
84+
<mesh file="franka_hand_0.obj"/>
85+
<mesh file="franka_hand_1.obj"/>
86+
<mesh file="franka_hand_2.obj"/>
87+
<mesh file="franka_hand_3.obj"/>
88+
<mesh file="franka_hand_4.obj"/>
89+
<mesh file="finger_0.obj"/>
90+
<mesh file="finger_1.obj"/>
91+
92+
<mesh name="fr3_link0_coll" file="fr3_link0.stl"/>
93+
<mesh name="fr3_link1_coll" file="fr3_link1.stl"/>
94+
<mesh name="fr3_link2_coll" file="fr3_link2.stl"/>
95+
<mesh name="fr3_link3_coll" file="fr3_link3.stl"/>
96+
<mesh name="fr3_link4_coll" file="fr3_link4.stl"/>
97+
<mesh name="fr3_link5_coll" file="fr3_link5.stl"/>
98+
<mesh name="fr3_link6_coll" file="fr3_link6.stl"/>
99+
<mesh name="fr3_link7_coll" file="fr3_link7.stl"/>
100+
<mesh name="franka_hand_coll" file="franka_hand.stl"/>
101+
102+
<!-- Camera assets -->
103+
<material class="d435i" name="Black_Acrylic" rgba="0.070360 0.070360 0.070360 1"/>
104+
<material class="d435i" name="Cameras_Gray" rgba="0.296138 0.296138 0.296138 1"/>
105+
<material class="d435i" name="IR_Emitter_Lens" rgba="0.287440 0.665387 0.327778 1"/>
106+
<material class="d435i" name="IR_Lens" rgba="0.035601 0.035601 0.035601 1"/>
107+
<material class="d435i" name="IR_Rim" rgba="0.799102 0.806952 0.799103 1"/>
108+
<material class="d435i" name="Metal_Casing" rgba="1 1 1 1"/>
109+
<material class="d435i" name="RGB_Pupil" rgba="0.087140 0.002866 0.009346 1"/>
110+
<mesh file="d435i_0.obj"/>
111+
<mesh file="d435i_1.obj"/>
112+
<mesh file="d435i_2.obj"/>
113+
<mesh file="d435i_3.obj"/>
114+
<mesh file="d435i_4.obj"/>
115+
<mesh file="d435i_5.obj"/>
116+
<mesh file="d435i_6.obj"/>
117+
<mesh file="d435i_7.obj"/>
118+
<mesh file="d435i_8.obj"/>
119+
<mesh file="Panda_RealSenseD435_Camera_Mount.obj" scale="0.001 0.001 0.001"/>
120+
</asset>
2121
<worldbody>
3122
<body name="base_0" childclass="fr3">
4123
<body name="fr3_link0_0">

‎assets/scenes/fr3_empty_world/scene.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
<mujoco model="fr3 scene">
2-
<include file="fr3_common.xml"/>
2+
<!-- <include file="fr3_common.xml"/> -->
33
<include file="fr3_0.xml"/>
44

55
<statistic center="0.3 0 0.4" extent="1"/>

‎assets/scenes/so101_empty_world/scene.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
<mujoco model="scene">
22
<include file="so101.xml" />
3+
<!-- <option gravity="0 0 0"/> -->
34

45
<visual>
56
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />

‎assets/so101/mjcf/so101.xml‎

Lines changed: 31 additions & 35 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,6 @@
1-
<?xml version="1.0" ?>
2-
<!-- Generated using onshape-to-robot -->
3-
<!-- Onshape https://cad.onshape.com/documents/7715cc284bb430fe6dab4ffd/w/4fd0791b683777b02f8d975a/e/826c553ede3b7592eb9ca800 -->
41
<mujoco model="so101_new_calib">
52
<compiler angle="radian" meshdir="assets" autolimits="true"/>
3+
<!-- <option integrator="implicitfast"/> -->
64
<default>
75
<default class="so101_new_calib">
86
<joint frictionloss="0.1" armature="0.005"/>
@@ -14,9 +12,6 @@
1412
<geom group="3"/>
1513
</default>
1614
</default>
17-
</default>
18-
<!-- Additional joints_properties.xml -->
19-
<default>
2015
<default class="sts3215">
2116
<geom contype="0" conaffinity="0"/>
2217
<joint damping="0.60" frictionloss="0.052" armature="0.028"/>
@@ -27,6 +22,34 @@
2722
<joint damping="0.01" frictionloss="0" armature="0.01" limited="true" range="-0.008726646259971648 0.008726646259971648"/>
2823
</default>
2924
</default>
25+
<asset>
26+
<mesh file="rotation_pitch_so101_v1.stl"/>
27+
<mesh file="moving_jaw_so101_v1.stl"/>
28+
<mesh file="sts3215_03a_v1.stl"/>
29+
<mesh file="motor_holder_so101_wrist_v1.stl"/>
30+
<mesh file="wrist_roll_follower_so101_v1.stl"/>
31+
<mesh file="base_so101_v2.stl"/>
32+
<mesh file="under_arm_so101_v1.stl"/>
33+
<mesh file="wrist_roll_pitch_so101_v2.stl"/>
34+
<mesh file="waveshare_mounting_plate_so101_v2.stl"/>
35+
<mesh file="sts3215_03a_no_horn_v1.stl"/>
36+
<mesh file="motor_holder_so101_base_v1.stl"/>
37+
<mesh file="base_motor_holder_so101_v1.stl"/>
38+
<mesh file="upper_arm_so101_v1.stl"/>
39+
<material name="base_motor_holder_so101_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
40+
<material name="base_so101_v2_material" rgba="0.964706 0.964706 0.952941 1"/>
41+
<material name="sts3215_03a_v1_material" rgba="0.627451 0.627451 0.627451 1"/>
42+
<material name="waveshare_mounting_plate_so101_v2_material" rgba="0.964706 0.964706 0.952941 1"/>
43+
<material name="motor_holder_so101_base_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
44+
<material name="rotation_pitch_so101_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
45+
<material name="upper_arm_so101_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
46+
<material name="under_arm_so101_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
47+
<material name="motor_holder_so101_wrist_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
48+
<material name="sts3215_03a_no_horn_v1_material" rgba="0.627451 0.627451 0.627451 1"/>
49+
<material name="wrist_roll_pitch_so101_v2_material" rgba="0.964706 0.964706 0.952941 1"/>
50+
<material name="wrist_roll_follower_so101_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
51+
<material name="moving_jaw_so101_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
52+
</asset>
3053
<worldbody>
3154
<!-- Link base -->
3255
<body name="base" pos="0 0 -0.03" quat="1 0 0 0" childclass="so101_new_calib">
@@ -92,7 +115,7 @@
92115
<geom type="mesh" class="visual" pos="0 -0.028 0.0181" quat="0.5 -0.5 -0.5 -0.5" mesh="wrist_roll_pitch_so101_v2" material="wrist_roll_pitch_so101_v2_material"/>
93116
<geom type="mesh" class="collision" pos="0 -0.028 0.0181" quat="0.5 -0.5 -0.5 -0.5" mesh="wrist_roll_pitch_so101_v2" material="wrist_roll_pitch_so101_v2_material"/>
94117
<!-- Link gripper -->
95-
<body name="gripper" pos="0 -0.0611 0.0181" quat="3.31663e-08 -3.31663e-08 -0.707107 -0.707107">
118+
<body name="gripper_body" pos="0 -0.0611 0.0181" quat="3.31663e-08 -3.31663e-08 -0.707107 -0.707107">
96119
<!-- Joint from wrist to gripper -->
97120
<joint axis="0 0 1" name="5" type="hinge" range="-2.7925268969992407 2.7925267093826136" class="sts3215"/>
98121
<inertial pos="0.000213627 0.000245138 -0.025187" mass="0.087" fullinertia="2.75087e-05 4.33657e-05 3.45059e-05 -3.35241e-07 -5.7352e-06 -5.17847e-08"/>
@@ -120,34 +143,7 @@
120143
</body>
121144
</body>
122145
</worldbody>
123-
<asset>
124-
<mesh file="rotation_pitch_so101_v1.stl"/>
125-
<mesh file="moving_jaw_so101_v1.stl"/>
126-
<mesh file="sts3215_03a_v1.stl"/>
127-
<mesh file="motor_holder_so101_wrist_v1.stl"/>
128-
<mesh file="wrist_roll_follower_so101_v1.stl"/>
129-
<mesh file="base_so101_v2.stl"/>
130-
<mesh file="under_arm_so101_v1.stl"/>
131-
<mesh file="wrist_roll_pitch_so101_v2.stl"/>
132-
<mesh file="waveshare_mounting_plate_so101_v2.stl"/>
133-
<mesh file="sts3215_03a_no_horn_v1.stl"/>
134-
<mesh file="motor_holder_so101_base_v1.stl"/>
135-
<mesh file="base_motor_holder_so101_v1.stl"/>
136-
<mesh file="upper_arm_so101_v1.stl"/>
137-
<material name="base_motor_holder_so101_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
138-
<material name="base_so101_v2_material" rgba="0.964706 0.964706 0.952941 1"/>
139-
<material name="sts3215_03a_v1_material" rgba="0.627451 0.627451 0.627451 1"/>
140-
<material name="waveshare_mounting_plate_so101_v2_material" rgba="0.964706 0.964706 0.952941 1"/>
141-
<material name="motor_holder_so101_base_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
142-
<material name="rotation_pitch_so101_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
143-
<material name="upper_arm_so101_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
144-
<material name="under_arm_so101_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
145-
<material name="motor_holder_so101_wrist_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
146-
<material name="sts3215_03a_no_horn_v1_material" rgba="0.627451 0.627451 0.627451 1"/>
147-
<material name="wrist_roll_pitch_so101_v2_material" rgba="0.964706 0.964706 0.952941 1"/>
148-
<material name="wrist_roll_follower_so101_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
149-
<material name="moving_jaw_so101_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
150-
</asset>
146+
151147
<actuator>
152148
<position class="sts3215" name="1" joint="1" inheritrange="1"/>
153149
<position class="sts3215" name="2" joint="2" inheritrange="1"/>

‎examples/so101_env_cartesian_control.py‎

Lines changed: 81 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,81 @@
1+
import logging
2+
from time import sleep
3+
4+
from rcs.envs.base import ControlMode, RelativeTo
5+
from rcs.envs.creators import SimEnvCreator
6+
7+
import rcs
8+
from rcs import sim
9+
10+
logger = logging.getLogger(__name__)
11+
logger.setLevel(logging.INFO)
12+
13+
14+
def main():
15+
16+
robot_cfg = sim.SimRobotConfig()
17+
robot_cfg.actuators = ["1", "2", "3", "4", "5"]
18+
robot_cfg.joints = [
19+
"1",
20+
"2",
21+
"3",
22+
"4",
23+
"5",
24+
]
25+
robot_cfg.base = "base"
26+
robot_cfg.robot_type = rcs.common.RobotType.SO101
27+
robot_cfg.attachment_site = "gripper"
28+
robot_cfg.arm_collision_geoms = []
29+
gripper_cfg = sim.SimGripperConfig()
30+
gripper_cfg.min_actuator_width = -0.17453292519943295
31+
gripper_cfg.max_actuator_width = 1.7453292519943295
32+
gripper_cfg.min_joint_width = -0.17453292519943295
33+
gripper_cfg.max_joint_width = 1.7453292519943295
34+
gripper_cfg.actuator = "6"
35+
gripper_cfg.joint = "6"
36+
gripper_cfg.collision_geoms = []
37+
gripper_cfg.collision_geoms_fingers = []
38+
39+
env_rel = SimEnvCreator()(
40+
control_mode=ControlMode.CARTESIAN_TQuat,
41+
collision_guard=False,
42+
robot_cfg=robot_cfg,
43+
gripper_cfg=gripper_cfg,
44+
# cameras=default_mujoco_cameraset_cfg(),
45+
max_relative_movement=0.5,
46+
relative_to=RelativeTo.LAST_STEP,
47+
mjcf=rcs.scenes["so101_empty_world"]["mjb"],
48+
# mjcf="/home/tobi/coding/rcs_clones/prs/assets/scenes/so101_empty_world/scene.xml", #rcs.scenes["so101_empty_world"]["mjb"],
49+
urdf_path=rcs.scenes["so101_empty_world"]["urdf"],
50+
)
51+
env_rel.get_wrapper_attr("sim").open_gui()
52+
# obs, info = env_rel.reset()
53+
# env_rel.get_wrapper_attr("robot").move_home()
54+
# env_rel.get_wrapper_attr("sim").step_until_convergence()
55+
# print(obs, info)
56+
sleep(5)
57+
# obs, reward, terminated, truncated, info = env_rel.step(obs)
58+
# print(obs, info)
59+
60+
for _ in range(10):
61+
for _ in range(2):
62+
# move 1cm in x direction (forward) and close gripper
63+
act = {"tquat": [0.0, 0, 0.01, 0, 0, 0, 1], "gripper": 0}
64+
obs, reward, terminated, truncated, info = env_rel.step(act)
65+
print(info, obs)
66+
if truncated or terminated:
67+
logger.info("Truncated or terminated!")
68+
return
69+
sleep(1)
70+
for _ in range(2):
71+
# move 1cm in negative x direction (backward) and open gripper
72+
act = {"tquat": [0.0, 0, -0.01, 0, 0, 0, 1], "gripper": 1}
73+
obs, reward, terminated, truncated, info = env_rel.step(act)
74+
if truncated or terminated:
75+
logger.info("Truncated or terminated!")
76+
return
77+
sleep(1)
78+
79+
80+
if __name__ == "__main__":
81+
main()

‎examples/so101_env_joint_control.py‎

Lines changed: 14 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
11
import logging
2+
from time import sleep
23

34
import numpy as np
45
from rcs.envs.base import ControlMode, RelativeTo
@@ -23,14 +24,18 @@ def main():
2324
"5",
2425
]
2526
robot_cfg.base = "base"
26-
robot_cfg.robot_type = rcs.common.RobotType.UR5e
27+
robot_cfg.robot_type = rcs.common.RobotType.SO101
2728
robot_cfg.attachment_site = "gripper"
2829
robot_cfg.arm_collision_geoms = []
29-
gripper_cfg = sim.SimGripperConfig
30+
gripper_cfg = sim.SimGripperConfig()
3031
gripper_cfg.min_actuator_width = -0.17453292519943295
3132
gripper_cfg.max_actuator_width = 1.7453292519943295
3233
gripper_cfg.min_joint_width = -0.17453292519943295
3334
gripper_cfg.max_joint_width = 1.7453292519943295
35+
gripper_cfg.actuator = "6"
36+
gripper_cfg.joint = "6"
37+
gripper_cfg.collision_geoms = []
38+
gripper_cfg.collision_geoms_fingers = []
3439
env_rel = SimEnvCreator()(
3540
control_mode=ControlMode.JOINTS,
3641
collision_guard=False,
@@ -39,16 +44,20 @@ def main():
3944
# cameras=default_mujoco_cameraset_cfg(),
4045
max_relative_movement=np.deg2rad(5),
4146
relative_to=RelativeTo.LAST_STEP,
42-
mjcf=rcs.scenes["so101_empty_world"]["mjb"],
47+
# mjcf=rcs.scenes["so101_empty_world"]["mjb"],
48+
mjcf="/home/tobi/coding/rcs_clones/prs/assets/scenes/so101_empty_world/scene.xml",
4349
urdf_path=rcs.scenes["so101_empty_world"]["urdf"],
4450
)
4551
env_rel.get_wrapper_attr("sim").open_gui()
4652

4753
for _ in range(10):
48-
obs, info = env_rel.reset()
49-
for _ in range(10):
54+
# obs, info = env_rel.reset()
55+
# env_rel.get_wrapper_attr("robot").move_home()
56+
sleep(100)
57+
for _ in range(1000):
5058
# sample random relative action and execute it
5159
act = env_rel.action_space.sample()
60+
print(act)
5261
obs, reward, terminated, truncated, info = env_rel.step(act)
5362
if truncated or terminated:
5463
logger.info("Truncated or terminated!")

‎include/rcs/Robot.h‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -62,8 +62,10 @@ static const std::unordered_map<RobotType, RobotMetaConfig> robots_meta_config =
6262
// TODO: update this to angle values
6363
RobotMetaConfig{
6464
// q_home (5‐vector):
65-
(VectorXd(5) << -0.1805846, -1.91335968, 1.56934507, 1.16014604,
66-
-0.25400112)
65+
// (VectorXd(5) << -0.1805846, -1.91335968, 1.56934507, 1.16014604,
66+
// -0.25400112)
67+
(VectorXd(5) << 0, 0, 0, 1.5707963267948966,
68+
0)
6769
.finished(),
6870
// dof:
6971
5,

0 commit comments

Comments
 (0)