Skip to content

Commit a66f8d9

Browse files
ndunkelb-nasandunkelb-nasa
committed
playing around with on the fly
1 parent e01783e commit a66f8d9

File tree

2 files changed

+205
-1
lines changed

2 files changed

+205
-1
lines changed

‎clr_mujoco_config/launch/clr_mujoco.launch.py‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,14 @@ def generate_launch_description():
2626

2727
declared_arguments = []
2828

29+
Node(
30+
package="mujoco_ros2_control",
31+
executable="robot_description_to_mjcf.sh",
32+
output="both",
33+
emulate_tty=True,
34+
arguments=["--publish_topic", "/mujoco_robot_description"],
35+
)
36+
2937
clr_launch = include_launch_file(
3038
package_name="clr_deploy",
3139
launch_file="control.launch.py",

‎clr_mujoco_config/urdf/clr_xacro.urdf‎

Lines changed: 197 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -42,10 +42,206 @@
4242
generate_ros2_control_tag="false"/>
4343
</xacro:if>
4444

45+
46+
<mujoco_inputs>
47+
48+
<!-- Copied directly into the final MJCF -->
49+
<raw_inputs>
50+
<option integrator="implicitfast"/>
51+
<default>
52+
<material specular="0.5" shininess="0.25"/>
53+
<joint axis="0 1 0" range="-6.28319 6.28319" armature="0.1"/>
54+
<position ctrlrange="-6.2831 6.2831"/>
55+
<general biastype="affine" ctrlrange="-6.2831 6.2831" />
56+
<default class="size4">
57+
<joint damping="10"/>
58+
<general forcerange="-330 330" gainprm="100000" biasprm="0 -100000 -10000" />
59+
</default>
60+
<default class="size3">
61+
<joint damping="5"/>
62+
<general forcerange="-150 150" gainprm="40000" biasprm="0 -40000 -4000" />
63+
<default class="size3_limited">
64+
<joint range="-3.1415 3.1415"/>
65+
<general ctrlrange="-3.1415 3.1415" />
66+
</default>
67+
</default>
68+
<default class="size2">
69+
<joint damping="2"/>
70+
<general forcerange="-56 56" gainprm="10000" biasprm="0 -10000 -1000" />
71+
</default>
72+
<default class="visual">
73+
<geom group="2" type="mesh" contype="0" conaffinity="0"/>
74+
</default>
75+
<default class="collision">
76+
<geom group="3" type="mesh"/>
77+
</default>
78+
</default>
79+
80+
<actuator>
81+
<position joint="vention_rail_base_to_carriage" name="vention_rail_base_to_carriage" kp="25000" dampratio="1.0" ctrlrange="0.0 2.0" />
82+
<position joint="ewellix_lift_lower_to_higher" name="ewellix_lift_lower_to_higher" kp="75000" dampratio="1.0" ctrlrange="0.0 0.7" />
83+
<position class="size4" name="shoulder_pan_joint" joint="shoulder_pan_joint"/>
84+
<position class="size4" name="shoulder_lift_joint" joint="shoulder_lift_joint"/>
85+
<position class="size3_limited" name="elbow_joint" joint="elbow_joint"/>
86+
<position class="size2" name="wrist_1_joint" joint="wrist_1_joint"/>
87+
<position class="size2" name="wrist_2_joint" joint="wrist_2_joint"/>
88+
<position class="size2" name="wrist_3_joint" joint="wrist_3_joint"/>
89+
<position tendon="split" name="finger_1_joint" kp="10000" dampratio="3.0" ctrlrange="0.0 0.025" forcerange="-100 100"/>
90+
</actuator>
91+
92+
<!--
93+
This adds stability to the model by having a tendon that distributes the forces between both
94+
joints, such that the equality constraint doesn't have to do that much work in order to equalize
95+
both joints. Since both joints share the same sign, we split the force between both equally by
96+
setting coef=0.5
97+
-->
98+
<tendon>
99+
<fixed name="split">
100+
<joint joint="finger_1_joint" coef="0.5"/>
101+
<joint joint="finger_2_joint" coef="0.5"/>
102+
</fixed>
103+
</tendon>
104+
105+
<equality>
106+
<joint joint1="finger_1_joint" joint2="finger_2_joint" polycoef="0 1 0 0 0" solimp="0.95 0.99 0.001" solref="0.005 1"/>
107+
</equality>
108+
109+
<!-- Same logic as above... -->
110+
<tendon>
111+
<fixed name="lift_middle_mimic_higher">
112+
<joint joint="ewellix_lift_lower_to_higher" coef="0.5"/>
113+
<joint joint="middle_mimic_higher" coef="0.5"/>
114+
</fixed>
115+
</tendon>
116+
117+
<!-- The mimic joint is scaled by 50% in the URDF -->
118+
<equality>
119+
<joint name="upper_mimic_lower" joint1="ewellix_lift_lower_to_higher" joint2="middle_mimic_higher" polycoef="0 2 0 0 0"/>
120+
</equality>
121+
122+
<sensor>
123+
<force name="tcp_fts_sensor_force" site="ft_frame"/>
124+
<torque name="tcp_fts_sensor_torque" site="ft_frame"/>
125+
</sensor>
126+
127+
<!-- clr contacts -->
128+
<contact>
129+
<exclude body1="world" body2="vention_rail_carriage"/>
130+
<exclude body1="world" body2="ewellix_lift_higher_link"/>
131+
<exclude body1="world" body2="ewellix_lift_middle_link"/>
132+
<exclude body1="vention_rail_carriage" body2="ewellix_lift_middle_link"/>
133+
</contact>
134+
135+
<!-- environment contacts -->
136+
<contact>
137+
<exclude body1="world" body2="lorge/hatch_face"/>
138+
<exclude body1="world" body2="bench_lid"/>
139+
<exclude body1="world" body2="cabinet_1"/>
140+
<exclude body1="world" body2="cabinet_2"/>
141+
<exclude body1="world" body2="cabinet_3"/>
142+
<exclude body1="world" body2="cabinet_4"/>
143+
<exclude body1="world" body2="drawer_1_face"/>
144+
<exclude body1="world" body2="drawer_2_face"/>
145+
146+
<exclude body1="world" body2="cabinet_1"/>
147+
<exclude body1="world" body2="round_latch_handle"/>
148+
<exclude body1="cabinet_1" body2="round_latch_handle"/>
149+
150+
<exclude body1="world" body2="cabinet_2"/>
151+
<exclude body1="world" body2="push_latch_button"/>
152+
<exclude body1="cabinet_2" body2="push_latch_button"/>
153+
<exclude body1="world" body2="push_latch_handle"/>
154+
<exclude body1="cabinet_2" body2="push_latch_handle"/>
155+
<exclude body1="push_latch_button" body2="push_latch_handle"/>
156+
157+
<exclude body1="world" body2="cabinet_3"/>
158+
159+
<exclude body1="world" body2="cabinet_4"/>
160+
<exclude body1="world" body2="paddle_latch_handle"/>
161+
<exclude body1="cabinet_4" body2="paddle_latch_handle"/>
162+
163+
<exclude body1="world" body2="drawer_1_face"/>
164+
165+
<exclude body1="world" body2="drawer_2_face"/>
166+
<exclude body1="world" body2="recessed_latch_handle"/>
167+
<exclude body1="drawer_2_face" body2="recessed_latch_handle"/>
168+
</contact>
169+
</raw_inputs>
170+
171+
<!-- Process arguments for the URDF -->
172+
<processed_inputs>
173+
174+
<!-- Specifying decompose_mesh will set the `decompose` flag when using obj2mjcf -->
175+
<decompose_mesh mesh_name="finger_v6" threshold="0.05"/>
176+
<decompose_mesh mesh_name="clr_base" threshold="0.05"/>
177+
178+
<!-- env stuff -->
179+
<decompose_mesh mesh_name="grab_handle" threshold="0.05"/>
180+
<decompose_mesh mesh_name="simplified_hatch_4060_face" threshold="0.05"/>
181+
<decompose_mesh mesh_name="ivr_hatch_frame_no_left" threshold="0.05"/>
182+
<decompose_mesh mesh_name="hatch_4060_frame" threshold="0.05"/>
183+
<decompose_mesh mesh_name="bench_lid" threshold="0.05"/>
184+
185+
<!-- The camera with the specified values will be added at the specified site name. -->
186+
<!-- The position and quaterinion will be filled in by the converter -->
187+
<camera site="wrist_mounted_camera_color_optical_frame" name="wrist_mounted_camera" fovy="58" mode="fixed" resolution="640 480"/>
188+
<camera site="lift_camera_color_optical_frame" name="lift_camera" fovy="58" mode="fixed" resolution="640 480"/>
189+
190+
<!-- Don't let the merlin open -->
191+
<modify_element type="joint" name="door_to_frame_joint" range="-0.01 0" />
192+
193+
<!-- Add gravity compensation for moveable CLR components to improve control -->
194+
<modify_element type="body" name="vention_rail_carriage" gravcomp="1.0"/>
195+
<modify_element type="body" name="ewellix_lift_higher_link" gravcomp="1.0"/>
196+
<modify_element type="body" name="ewellix_lift_middle_link" gravcomp="1.0"/>
197+
<modify_element type="body" name="shoulder_link" gravcomp="1.0"/>
198+
<modify_element type="body" name="upper_arm_link" gravcomp="1.0"/>
199+
<modify_element type="body" name="forearm_link" gravcomp="1.0"/>
200+
<modify_element type="body" name="wrist_1_link" gravcomp="1.0"/>
201+
<modify_element type="body" name="wrist_2_link" gravcomp="1.0"/>
202+
<modify_element type="body" name="wrist_3_link" gravcomp="1.0"/>
203+
204+
<!-- Add frictionloss and armature to fingers to help with backdriving -->
205+
<modify_element type="joint" name="finger_1_joint" armature="0.5" frictionloss="5.0" />
206+
<modify_element type="joint" name="finger_2_joint" armature="0.5" frictionloss="5.0" />
207+
208+
<!-- Add joint properties to lorge hatch latch spring joint -->
209+
<modify_element type="joint" name="lorge/latch_spring" limited="true" springref="0.035" stiffness="10000" damping="2" armature="0.001"/>
210+
211+
</processed_inputs>
212+
<scene>
213+
<statistic center="-0.5 1.0 1.0" extent="3.5"/>
214+
215+
<option gravity="0 0 -9.81">
216+
<flag contact="enable" />
217+
</option>
218+
219+
<visual>
220+
<headlight diffuse="0.6 0.6 0.6" ambient="0.1 0.1 0.1" specular="0 0 0"/>
221+
<rgba haze="0.15 0.25 0.35 1"/>
222+
<global azimuth="25" elevation="-10"/>
223+
</visual>
224+
225+
<asset>
226+
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
227+
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.8 0.8 0.8" rgb2="0.8 0.8 0.8"
228+
markrgb="0.5 0.5 0.5" width="300" height="300"/>
229+
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="3.0 3.0" reflectance="0.0"/>
230+
</asset>
231+
232+
<worldbody>
233+
<light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
234+
<geom name="floor" pos="0 0 0.001" size="0 0 0.05" type="plane" material="groundplane"/>
235+
</worldbody>
236+
</scene>
237+
238+
</mujoco_inputs>
239+
240+
45241
<ros2_control name="MujocoSystem" type="system">
46242
<hardware>
47243
<plugin>mujoco_ros2_control/MujocoSystemInterface</plugin>
48-
<param name="mujoco_model">$(find clr_mujoco_config)/description/scene.xml</param>
244+
<!-- <param name="mujoco_model">$(find clr_mujoco_config)/description/scene.xml</param> -->
49245

50246
<!-- Increase this parameter to run the sim faster than realtime. -->
51247
<param name="sim_speed_factor">1.0</param>

0 commit comments

Comments
 (0)