Move to radian/quat description

JeanElsner · Nov 25, 2023 · 7bc3630 · 7bc3630
1 parent 008fa3a
commit 7bc3630
Show file tree

Hide file tree

Showing 6 changed files with 24 additions and 33 deletions.
diff --git a/doc/tutorial.rst b/doc/tutorial.rst
@@ -374,7 +374,7 @@ The latter allows us to attach cameras to the robots as well.
    panda_env.robots[robot_params.name].gripper.tool_center_point.parent.add(
        'camera',
        pos=(.1, 0, -.1),
-       euler=(180, 0, -90),
+       euler=(np.pi, 0, -np.pi / 2),
        fovy=90,
        name='wrist_camera')
 

diff --git a/examples/assets/two_arm.xml b/examples/assets/two_arm.xml
@@ -4,23 +4,17 @@
     <texture name='groundplane' type='2d' builtin='checker' rgb1='.2 .3 .4' rgb2='.1 .2 .3' width='300' height='300' mark='edge' markrgb='.8 .8 .8'/>
     <material name='groundplane' texture='groundplane' texrepeat='5 5' texuniform='true' reflectance='.2'/>
   </asset>
-
-  <!-- Always initialize the free camera to point at the origin. -->
-  <statistic center='0 0 0'/>
-  <default>
-    <geom solimp="0.9 0.95 0.001 0.5 2" solref="0.005 1" condim="6" />
-  </default>
   <worldbody>
     <geom name='ground' type='plane' size='2 2 2' material='groundplane'
         friction='0.4'/>
     <light directional='true' diffuse='.7 .7 .7' pos='1 .1 2' dir='0 -.1 -2' specular='.3 .3 .3' castshadow='true'/>
-    <body pos="0 0 1" euler="0 0 30">
+    <body pos="0 0 1" quat="0.9659 0 0 0.2588">
       <joint type="hinge" axis="0 0 1" damping="2000" />
       <geom type="box" size=".15 .2 .15" />
       <geom type="cylinder" size=".1 .3" />
       <geom type="sphere" size=".2" pos="0 0 .4" />
-      <site name="left" pos="0 .2 0" euler="-90 0 0" />
-      <site name="right" pos="0 -.2 0" euler="90 0 0" />
+      <site name="left" pos="0 .2 0" quat="1 -1 0 0" />
+      <site name="right" pos="0 -.2 0" quat="1 1 0 0" />
       <site name="control" />
     </body>
   </worldbody>

diff --git a/examples/rl_environment.py b/examples/rl_environment.py
@@ -74,7 +74,7 @@ def goal_reward(observation: spec_utils.ObservationValue):
   panda_env.robots[robot_params.name].gripper.tool_center_point.parent.add(
       'camera',
       pos=(.1, 0, -.1),
-      euler=(180, 0, -90),
+      euler=(np.pi, 0, -np.pi / 2),
       fovy=90,
       name='wrist_camera')
 

diff --git a/src/dm_robotics/panda/assets/panda/panda.xml b/src/dm_robotics/panda/assets/panda/panda.xml
@@ -1,5 +1,5 @@
 <mujoco model="panda">
-  <compiler angle="degree" />
+  <compiler angle="radian" />
   <asset>
     <mesh name="link0" file="link0.obj" />
     <mesh name="link1" file="link1.obj" />
@@ -32,37 +32,37 @@
       <body name="panda_link1" pos="0 0 0.333" gravcomp="1">
         <site name="panda_joint_site1" />
         <inertial pos="3.875e-03 2.081e-03 -4.762e-02" mass="4.970684" fullinertia="7.0337e-01 7.0661e-01 9.1170e-03 -1.3900e-04 6.7720e-03 1.9169e-02" />
-        <joint name="panda_joint1" pos="0 0 0" axis="0 0 1" limited="true" range="-166 166" damping="0.0665" frictionloss="0.2450" />
+        <joint name="panda_joint1" pos="0 0 0" axis="0 0 1" limited="true" range="-2.897 2.897" damping="0.0665" frictionloss="0.2450" />
         <geom type="mesh" material="panda_white" mesh="link1" />
-        <body name="panda_link2" pos="0 0 0" euler="-90 0 -45" gravcomp="1">
+        <body name="panda_link2" pos="0 0 0" quat="1 -1 0 0" gravcomp="1">
           <site name="panda_joint_site2" />
           <inertial pos="-3.141e-03 -2.872e-02 3.495e-03" mass="0.646926" fullinertia="7.9620e-03 2.8110e-02 2.5995e-02 -3.9250e-03 1.0254e-02 7.0400e-04" />
-          <joint name="panda_joint2" ref="-45" pos="0 0 0" axis="0 0 1" limited="true" range="-101 101" damping="0.1987" frictionloss="0.1523" />
+          <joint name="panda_joint2" pos="0 0 0" axis="0 0 1" limited="true" range="-1.7627 1.7627" damping="0.1987" frictionloss="0.1523" />
           <geom type="mesh" material="panda_white" mesh="link2" />
-          <body name="panda_link3" pos="0 -0.316 0" euler="90 0 0" gravcomp="1">
+          <body name="panda_link3" pos="0 -0.316 0" quat="1 1 0 0" gravcomp="1">
             <site name="panda_joint_site3" />
             <inertial pos="2.7518e-02 3.9252e-02 -6.6502e-02" mass="3.228604" fullinertia="3.7242e-02 3.6155e-02 1.0830e-02 -4.7610e-03 -1.1396e-02 -1.2805e-02" />
-            <joint name="panda_joint3" pos="0 0 0" axis="0 0 1" limited="true" range="-166 166" damping="0.0399" frictionloss="0.1827" />
+            <joint name="panda_joint3" pos="0 0 0" axis="0 0 1" limited="true" range="-2.897 2.897" damping="0.0399" frictionloss="0.1827" />
             <geom type="mesh" material="panda" mesh="link3" />
-            <body name="panda_link4" pos="0.0825 0 0" euler="90 0 -135" gravcomp="1">
+            <body name="panda_link4" pos="0.0825 0 0" quat="1 1 0 0" gravcomp="1">
               <site name="panda_joint_site4" />
               <inertial pos="-5.317e-02 1.04419e-01 2.7454e-02" mass="3.587895" fullinertia="2.5853e-02 1.9552e-02 2.8323e-02 7.7960e-03 -1.3320e-03 8.6410e-03" />
-              <joint name="panda_joint4" ref="-135" pos="0 0 0" axis="0 0 1" limited="true" range="-176 -4" damping="0.2257" frictionloss="0.3591" />
+              <joint name="panda_joint4" pos="0 0 0" axis="0 0 1" limited="true" range="-3.0717 -0.0698" damping="0.2257" frictionloss="0.3591" />
               <geom type="mesh" material="panda" mesh="link4" />
-              <body name="panda_link5" pos="-0.0825 0.384 0" euler="-90 0 0" gravcomp="1">
+              <body name="panda_link5" pos="-0.0825 0.384 0" quat="1 -1 0 0" gravcomp="1">
                 <site name="panda_joint_site5" />
                 <inertial pos="-1.1953e-02 4.1065e-02 -3.8437e-02" mass="1.225946" fullinertia="3.5549e-02 2.9474e-02 8.6270e-03 -2.1170e-03 -4.0370e-03 2.2900e-04" />
-                <joint name="panda_joint5" pos="0 0 0" axis="0 0 1" limited="true" range="-166 166" damping="0.1023" frictionloss="0.2669" />
+                <joint name="panda_joint5" pos="0 0 0" axis="0 0 1" limited="true" range="-2.897 2.897" damping="0.1023" frictionloss="0.2669" />
                 <geom type="mesh" material="panda" mesh="link5" />
-                <body name="panda_link6" pos="0 0 0" euler="90 0 90" gravcomp="1">
+                <body name="panda_link6" pos="0 0 0" quat="1 1 0 0" gravcomp="1">
                   <site name="panda_joint_site6" />
                   <inertial pos="6.0149e-02 -1.4117e-02 -1.0517e-02" mass="1.666555 " fullinertia="1.9640e-03 4.3540e-03 5.4330e-03 1.0900e-04 -1.1580e-03 3.4100e-04" />
-                  <joint name="panda_joint6" ref="90" pos="0 0 0" axis="0 0 1" limited="true" range="-1 215" damping="-0.0132" frictionloss="0.1658" />
+                  <joint name="panda_joint6" pos="0 0 0" axis="0 0 1" limited="true" range="-0.0174 3.7524" damping="-0.0132" frictionloss="0.1658" />
                   <geom type="mesh" material="panda" mesh="link6" />
-                  <body name="panda_link7" pos="0.088 0 0" euler="90 0 45" gravcomp="1">
+                  <body name="panda_link7" pos="0.088 0 0" quat="1 1 0 0" gravcomp="1">
                     <site name="panda_joint_site7" />
                     <inertial pos="1.0517e-02 -4.252e-03 6.1597e-02" mass="7.35522e-01" fullinertia="1.2516e-02 1.0027e-02 4.8150e-03 -4.2800e-04 -1.1960e-03 -7.4100e-04" />
-                    <joint name="panda_joint7" ref="45" pos="0 0 0" axis="0 0 1" limited="true" range="-166 166" damping="0.0638" frictionloss="1.2109" />
+                    <joint name="panda_joint7" pos="0 0 0" axis="0 0 1" limited="true" range="-2.897 2.897" damping="0.0638" frictionloss="1.2109" />
                     <geom type="mesh" material="panda" mesh="link7" />
                     <body name="panda_link8" pos="0 0 0.107">
                       <site name="wrist_site" />

diff --git a/src/dm_robotics/panda/assets/panda/panda_hand.xml b/src/dm_robotics/panda/assets/panda/panda_hand.xml
@@ -1,5 +1,5 @@
 <mujoco model="panda_hand">
-  <compiler angle="degree" />
+  <compiler angle="radian" />
   <asset>
     <mesh name="link0" file="link0.obj" />
     <mesh name="link1" file="link1.obj" />
@@ -19,17 +19,17 @@
     <joint name="couple_fingers" joint1="panda_finger_joint1" joint2="panda_finger_joint2" solimp="0.95 0.99 0.001" solref="0.005 1"/>
   </equality>
   <worldbody>
-    <body name="panda_hand" euler="0 0 -0.785398163397" gravcomp="1">
+    <body name="panda_hand" quat="1 0 0 0" gravcomp="1">
       <site name="TCP" pos="0 0 .1034" />
       <inertial pos="-1e-02 0 3e-02" mass="7.3e-01" diaginertia="1e-03 2.5e-03 1.7e-03" />
       <geom type="mesh" material="panda" mesh="hand" />
-      <body name="panda_leftfinger" pos="0 0 0.0584" euler="0 0 180" gravcomp="1">
+      <body name="panda_leftfinger" pos="0 0 0.0584" quat="0 0 0 1" gravcomp="1">
         <inertial pos="0 0 0" mass="1.5e-02" diaginertia="2.3749e-06 2.3749e-06 7.5e-07"/>
         <joint name="panda_finger_joint1" pos="0 0 0" axis="0 1 0" type="slide" limited="true" range="0 0.04" armature="0.1" damping="11" solreflimit="-30000 -200" />
         <geom type="mesh" material="panda" mesh="finger" contype="0" conaffinity="0" />
         <geom type="box" name="panda_leftfinger_collision" pos="0 0.015 0.032" size="0.012 0.015 0.022" rgba="1 1 1 .3" group="5"/>
       </body>
-      <body name="panda_rightfinger" pos="0 0 0.0584" euler="0 0 0" gravcomp="1">
+      <body name="panda_rightfinger" pos="0 0 0.0584" quat="1 0 0 0" gravcomp="1">
         <inertial pos="0 0 0" mass="1.5e-02" diaginertia="2.3749e-06 2.3749e-06 7.5e-07"/>
         <joint name="panda_finger_joint2" pos="0 0 0" axis="0 1 0" type="slide" limited="true" range="0 0.04" armature="0.1" damping="11" solreflimit="-30000 -200" />
         <geom type="mesh" material="panda" mesh="finger" contype="0" conaffinity="0" />
@@ -44,7 +44,6 @@
     </fixed>
   </tendon>
   <actuator>
-    <!-- Remap original ctrlrange (0, 0.04) to (0, 1): 0.04 * 100 / 1 = 4 -->
-    <general biastype="affine" name="panda_hand_actuator" tendon="split" forcelimited="true" forcerange="-100 100" ctrllimited="true" ctrlrange="0 1" gainprm="4 0 0" biasprm="0 -100 0"/>
+    <general biastype="affine" name="panda_hand_actuator" tendon="split" forcelimited="true" forcerange="-100 100" ctrllimited="true" ctrlrange="0 1" gainprm="240 0 0" biasprm="0 -6000 -10"/>
   </actuator>
 </mujoco>
diff --git a/src/dm_robotics/panda/environment.py b/src/dm_robotics/panda/environment.py
@@ -32,8 +32,6 @@ def __init__(self,
     self._robot_params = robot_params if isinstance(
         robot_params, Sequence) else [robot_params]
     self._control_timestep = control_timestep
-
-    self._arena.mjcf_model.compiler.angle = 'degree'
     self._arena.mjcf_model.option.timestep = physics_timestep
 
     self._robots = collections.OrderedDict()