forceをmujoco-viwerで可視化できるよいうになった

openai/mujoco-py#487
tomoya-yamanokuchi · Jul 13, 2022 · dbd520f · dbd520f
1 parent eb3c740
commit dbd520f
Show file tree

Hide file tree

Showing 4 changed files with 57 additions and 5 deletions.
diff --git a/domain/environment/DClawEnvironment.py b/domain/environment/DClawEnvironment.py
@@ -54,6 +54,7 @@ def __init__(self, config):
 
         self._valve_jnt_id               = self.model.joint_name2id('valve_OBJRx')
         self._target_bid                 = self.model.body_name2id('target')
+        self._contact_bid                = self.model.body_name2id('vis_contact_body')
         self._target_sid                 = self.model.site_name2id('tmark')
         self._target_position            = None
         self.sim                         = None
@@ -462,14 +463,19 @@ def step_with_inplicit_step(self):
         for i in range(self.inplicit_step):
             self.sim.step()
 
+        self.viewer.vopt.flags[mujoco_py.const.VIS_CONTACTFORCE] = 1
         # print("self.sim.data.ncon: ", self.sim.data.ncon)
         for i in range(self.sim.data.ncon):
             con = self.sim.data.contact[i]
             # if con.geom1 == self.sim.model.geom_name2id("phy_tip") and con.geom2 == self.sim.model.geom_name2id("phy_valve_6_oclock"):
             if con.geom1:
-                print(con.geom1)
-                print(con.geom2)
+                # print(con.geom1)
+                # print(con.geom2)
+                # contact_pos = con.pos
+                self.sim.model.body_pos[self._contact_bid][:] = con.pos
+            if self.sim.data.ncon > 1:
                 contact_pos = con.pos
 
+
     def step(self):
         self.sim.step()
diff --git a/domain/environment/model/dclaw3xh_valve3_default.xml b/domain/environment/model/dclaw3xh_valve3_default.xml
@@ -25,5 +25,9 @@
       <include file="/valve/assets/valve3_default.xml"/>
     </body>
 
+    <body name="vis_contact_body" pos="0 0 0">
+      <site name="vis_contact_site" type="sphere" size=".008" pos="0 0 0" rgba="1 0 0 1" euler="0 1.57 0"/>
+    </body>
+
   </worldbody>
 </mujoco>
diff --git a/domain/environment/model/robot/assets/claw_unit.xml b/domain/environment/model/robot/assets/claw_unit.xml
@@ -0,0 +1,42 @@
+<?xml version="1.0" encoding="utf-8"?>
+<mujoco>
+    <worldbody>
+        <body name="Finger_base" pos="0 0 0" euler="0 0 0">
+            <!-- <geom mesh="xh28" pos="0 0 0.01425" euler="1.57 0 1.57"/> -->
+            <geom  type="box" pos=".012 0 0.014" size="0.024 0.0175 0.015" mass=".093"/>
+            <body name="FF10" pos="0 0 0.01425" euler="0 0 3.14">
+                <!-- <geom mesh="metal_clamping" euler="0 0 1.57"/>
+                <geom mesh="metal_clamping_small" pos="0 0 0.055" euler="3.14 0 0"/>
+                <geom mesh="xh28" pos="0 0 0.068" euler="1.57 1.57 0"/> -->
+                <geom  type="box" pos="0 0 0.056" size="0.0175 0.015 0.024" mass=".093"/>
+                <geom  type="box" pos="0.0 0 0.026" size="0.0125 0.0214 0.003" mass=".003"/>
+                <geom  type="box" pos="0 0.02 0.013" size="0.011 0.0012 0.016" mass=".002"/>
+                <geom  type="box" pos="0 -0.02 0.013" size="0.011 0.0012 0.016" mass=".002"/>
+                <geom  type="box" pos="0 0 0.033" size="0.019 0.014 0.006" mass=".005"/>
+                <joint name="FFJ10" type="hinge" axis="0 1 0" range="-.45 1.35"/>
+                <body name="FFL11" pos="0 0 0.0675">
+                    <!-- <geom mesh="metal_clamping_small" pos="0 0 0.055" euler="3.14 0 0"/>
+                    <geom mesh="xh28" pos="0 0 0.0686" euler="1.57 1.57 0"/>
+                    <geom mesh="metal_clamping"/> -->
+                    <geom  type="box" pos="0 0 0.056" size="0.0175 0.015 0.024" mass=".093"/>
+                    <geom  type="box" pos="0 0 0.026" size="0.0214 0.0125 0.003" mass=".003"/>
+                    <geom  type="box" pos="0.02 0 0.013" size="0.0012 0.011 0.016" mass=".002"/>
+                    <geom  type="box" pos="-0.02 0 0.013" size="0.0012 0.011 0.016" mass=".002"/>
+                    <geom  type="box" pos="0 0 0.033" size="0.019 0.014 0.006" mass=".005"/>
+                    <joint name="FFJ11" type="hinge" axis="1 0 0" range="-2 2"/>
+                    <body name="FFL12" pos="0 0 0.068">
+                        <!-- <geom mesh="metal_clamping"/> -->
+                        <!-- <geom material="plastic" mesh="plastic_finger" pos="0 0 0.046"/> -->
+                        <geom  type="box" pos="0.0 0 0.026" size="0.0214 0.0125 0.003" mass=".003"/>
+                        <geom  type="box" pos="0.02 0 0.013" size="0.0012 0.011 0.016" mass=".002"/>
+                        <geom  type="box" pos="-0.02 0 0.013" size="0.0012 0.011 0.016" mass=".002"/>
+                        <geom  type="capsule" pos="0 0 0.053" size="0.011 0.02" mass=".018"/>
+                        <geom  type="box" pos="0 0 0.03" size="0.0245 0.014 0.003" mass=".007"/>
+                        <joint name="FFJ12" type="hinge" axis="1 0 0" range="-2 2"/>
+                        <site name="FFtip" type="sphere" pos="0 0 .076" size="0.01" rgba="1 .3 .3 1"/>
+                    </body>
+                </body>
+            </body>
+        </body>
+    </worldbody>
+</mujoco>
diff --git a/usecase/DemoRobotMove_with_mujoco_viewer.py b/usecase/DemoRobotMove_with_mujoco_viewer.py
@@ -28,10 +28,10 @@ def run(self, config):
         ctrl[:, 1] = np.linspace(0, np.pi*0.1, step)
         ctrl[:, 2] = np.linspace(0, np.pi*0.25, step)
 
-        for s in range(10):
+        for s in range(30):
             env.reset(state)
-            # env.canonicalize_texture() # canonicalテクスチャを設定
-            env.randomize_texture()    # randomテクスチャを設定
+            env.canonicalize_texture() # canonicalテクスチャを設定
+            # env.randomize_texture()    # randomテクスチャを設定
             for i in range(step):
                 env.set_ctrl(ctrl[i])
                 env.render_with_viewer()    # Mujocoのビューワを使って描画