Skip to content

Commit dbd520f

Browse files
tomoya-yamanokuchitomoya-yamanokuchi
committed
forceをmujoco-viwerで可視化できるよいうになった
openai/mujoco-py#487
1 parent eb3c740 commit dbd520f

File tree

4 files changed

+57
-5
lines changed

4 files changed

+57
-5
lines changed

‎domain/environment/DClawEnvironment.py

+8-2
Original file line numberDiff line numberDiff line change
@@ -54,6 +54,7 @@ def __init__(self, config):
5454

5555
self._valve_jnt_id = self.model.joint_name2id('valve_OBJRx')
5656
self._target_bid = self.model.body_name2id('target')
57+
self._contact_bid = self.model.body_name2id('vis_contact_body')
5758
self._target_sid = self.model.site_name2id('tmark')
5859
self._target_position = None
5960
self.sim = None
@@ -462,14 +463,19 @@ def step_with_inplicit_step(self):
462463
for i in range(self.inplicit_step):
463464
self.sim.step()
464465

466+
self.viewer.vopt.flags[mujoco_py.const.VIS_CONTACTFORCE] = 1
465467
# print("self.sim.data.ncon: ", self.sim.data.ncon)
466468
for i in range(self.sim.data.ncon):
467469
con = self.sim.data.contact[i]
468470
# if con.geom1 == self.sim.model.geom_name2id("phy_tip") and con.geom2 == self.sim.model.geom_name2id("phy_valve_6_oclock"):
469471
if con.geom1:
470-
print(con.geom1)
471-
print(con.geom2)
472+
# print(con.geom1)
473+
# print(con.geom2)
474+
# contact_pos = con.pos
475+
self.sim.model.body_pos[self._contact_bid][:] = con.pos
476+
if self.sim.data.ncon > 1:
472477
contact_pos = con.pos
473478

479+
474480
def step(self):
475481
self.sim.step()

‎domain/environment/model/dclaw3xh_valve3_default.xml

+4
Original file line numberDiff line numberDiff line change
@@ -25,5 +25,9 @@
2525
<include file="/valve/assets/valve3_default.xml"/>
2626
</body>
2727

28+
<body name="vis_contact_body" pos="0 0 0">
29+
<site name="vis_contact_site" type="sphere" size=".008" pos="0 0 0" rgba="1 0 0 1" euler="0 1.57 0"/>
30+
</body>
31+
2832
</worldbody>
2933
</mujoco>

‎domain/environment/model/robot/assets/claw_unit.xml

+42
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
<?xml version="1.0" encoding="utf-8"?>
2+
<mujoco>
3+
<worldbody>
4+
<body name="Finger_base" pos="0 0 0" euler="0 0 0">
5+
<!-- <geom mesh="xh28" pos="0 0 0.01425" euler="1.57 0 1.57"/> -->
6+
<geom type="box" pos=".012 0 0.014" size="0.024 0.0175 0.015" mass=".093"/>
7+
<body name="FF10" pos="0 0 0.01425" euler="0 0 3.14">
8+
<!-- <geom mesh="metal_clamping" euler="0 0 1.57"/>
9+
<geom mesh="metal_clamping_small" pos="0 0 0.055" euler="3.14 0 0"/>
10+
<geom mesh="xh28" pos="0 0 0.068" euler="1.57 1.57 0"/> -->
11+
<geom type="box" pos="0 0 0.056" size="0.0175 0.015 0.024" mass=".093"/>
12+
<geom type="box" pos="0.0 0 0.026" size="0.0125 0.0214 0.003" mass=".003"/>
13+
<geom type="box" pos="0 0.02 0.013" size="0.011 0.0012 0.016" mass=".002"/>
14+
<geom type="box" pos="0 -0.02 0.013" size="0.011 0.0012 0.016" mass=".002"/>
15+
<geom type="box" pos="0 0 0.033" size="0.019 0.014 0.006" mass=".005"/>
16+
<joint name="FFJ10" type="hinge" axis="0 1 0" range="-.45 1.35"/>
17+
<body name="FFL11" pos="0 0 0.0675">
18+
<!-- <geom mesh="metal_clamping_small" pos="0 0 0.055" euler="3.14 0 0"/>
19+
<geom mesh="xh28" pos="0 0 0.0686" euler="1.57 1.57 0"/>
20+
<geom mesh="metal_clamping"/> -->
21+
<geom type="box" pos="0 0 0.056" size="0.0175 0.015 0.024" mass=".093"/>
22+
<geom type="box" pos="0 0 0.026" size="0.0214 0.0125 0.003" mass=".003"/>
23+
<geom type="box" pos="0.02 0 0.013" size="0.0012 0.011 0.016" mass=".002"/>
24+
<geom type="box" pos="-0.02 0 0.013" size="0.0012 0.011 0.016" mass=".002"/>
25+
<geom type="box" pos="0 0 0.033" size="0.019 0.014 0.006" mass=".005"/>
26+
<joint name="FFJ11" type="hinge" axis="1 0 0" range="-2 2"/>
27+
<body name="FFL12" pos="0 0 0.068">
28+
<!-- <geom mesh="metal_clamping"/> -->
29+
<!-- <geom material="plastic" mesh="plastic_finger" pos="0 0 0.046"/> -->
30+
<geom type="box" pos="0.0 0 0.026" size="0.0214 0.0125 0.003" mass=".003"/>
31+
<geom type="box" pos="0.02 0 0.013" size="0.0012 0.011 0.016" mass=".002"/>
32+
<geom type="box" pos="-0.02 0 0.013" size="0.0012 0.011 0.016" mass=".002"/>
33+
<geom type="capsule" pos="0 0 0.053" size="0.011 0.02" mass=".018"/>
34+
<geom type="box" pos="0 0 0.03" size="0.0245 0.014 0.003" mass=".007"/>
35+
<joint name="FFJ12" type="hinge" axis="1 0 0" range="-2 2"/>
36+
<site name="FFtip" type="sphere" pos="0 0 .076" size="0.01" rgba="1 .3 .3 1"/>
37+
</body>
38+
</body>
39+
</body>
40+
</body>
41+
</worldbody>
42+
</mujoco>

‎usecase/DemoRobotMove_with_mujoco_viewer.py

+3-3
Original file line numberDiff line numberDiff line change
@@ -28,10 +28,10 @@ def run(self, config):
2828
ctrl[:, 1] = np.linspace(0, np.pi*0.1, step)
2929
ctrl[:, 2] = np.linspace(0, np.pi*0.25, step)
3030

31-
for s in range(10):
31+
for s in range(30):
3232
env.reset(state)
33-
# env.canonicalize_texture() # canonicalテクスチャを設定
34-
env.randomize_texture() # randomテクスチャを設定
33+
env.canonicalize_texture() # canonicalテクスチャを設定
34+
# env.randomize_texture() # randomテクスチャを設定
3535
for i in range(step):
3636
env.set_ctrl(ctrl[i])
3737
env.render_with_viewer() # Mujocoのビューワを使って描画

0 commit comments

Comments
 (0)