robotic 0.2.9.dev1__cp38-cp38-manylinux2014_x86_64.whl → 0.2.9.dev2__cp38-cp38-manylinux2014_x86_64.whl

robotic/__init__.py

@@ -9,5 +9,9 @@ from .version import __version__
 
 from .manipulation import KOMO_ManipulationHelper
 
+from .src.config_mujoco import MujocoLoader
+from .src.config_urdf import URDFLoader
+from .src.mesh_helper import MeshHelper
+
 import os
 setRaiPath( os.path.abspath(os.path.dirname(__file__)) + '/rai-robotModels' )

robotic/_robotic.pyi

@@ -275,7 +275,7 @@ class Config:
         """
         animate with random spline in limits bounding box [T=#spline points]
         """
-    def asDict(self) -> dict:
+    def asDict(self, parentsInKeys: bool = True) -> dict:
         """
         return the configuration description as a dict, e.g. for file export
         """
@@ -367,7 +367,7 @@ class Config:
         """
         collect all inertia at root frame of links, optionally reestimate all inertias based on standard surface density, optionally relocate the link frame to the COM with diagonalized I)
         """
-    def processStructure(self, pruneNamed: bool, pruneNonContactNonMarker: bool, pruneTransparent: bool) -> None:
+    def processStructure(self, pruneRigidJoints: bool = False, reconnectToLinks: bool = True, pruneNonContactShapes: bool = False, pruneTransparent: bool = False) -> None:
         """
         structurally simplify the Configuration (deleting frames, relinking to minimal tree)
         """
@@ -453,7 +453,7 @@ class Config:
         """
         write the full configuration in a ply mesh file
         """
-    def writeMeshes(self, pathPrefix: ..., copyTextures: bool) -> None:
+    def writeMeshes(self, pathPrefix: ..., copyTextures: bool = True, enumerateTextures: bool = False) -> None:
         """
         write all object meshes in a directory
         """
@@ -788,11 +788,11 @@ class Frame:
         """
     def setImplicitSurface(self, data: ..., size: arr, blur: int, resample: float = -1.0) -> Frame:
         ...
-    def setJoint(self, jointType: JT, limits: arr = ...) -> Frame:
+    def setJoint(self, jointType: JT, limits: arr = ..., scale: float = 1.0, mimic: Frame = None) -> Frame:
         ...
     def setJointState(self, arg0: arr) -> Frame:
         ...
-    def setMass(self, arg0: float) -> Frame:
+    def setMass(self, mass: float, inertiaMatrix: arr = ...) -> Frame:
         ...
     def setMesh(self, vertices: arr, triangles: uintA, colors: ... = ..., cvxParts: uintA = ...) -> Frame:
         """

robotic/cleanMeshes.py

@@ -0,0 +1,59 @@
+#!/usr/bin/env python3
+
+import argparse
+import glob
+import os
+from robotic.src.mesh_helper import *
+
+parser = argparse.ArgumentParser(
+    description='Utility to clean meshes in meshes/')
+
+parser.add_argument('-view', help='view mesh', action="store_true")
+parser.add_argument('-meshlab', help='apply meshlab filters', action="store_true", default=False)
+
+def main():
+    args = parser.parse_args()
+
+    files = sorted(glob.glob('meshes/*.h5'))
+
+    for file in files:
+        if file[-4]=='-':
+            continue
+
+        M = MeshHelper(file)
+        if M.mesh is None:
+            continue
+
+        #-- repair
+        print(' before repair:')
+        M.report()
+        M.repair(mergeTolerance=1e-4)
+        # M.texture2vertexColors()
+        print(' after repair:')
+        print('  watertight:', M.mesh.is_watertight)
+        print('  oriented:', M.mesh.is_winding_consistent)
+        M.report()
+
+        #-- export/view
+        M.export_h5()
+        # M.export_h5(M.filebase+'-.h5')
+        if args.view:
+            M.view()
+
+        #-- meshlab processing
+        if args.meshlab:
+            M.export_ply('z.ply')
+            print('<<< meshlab <<<')
+            ret = os.system(f'meshlabserver -i z.ply -o {M.filebase}.ply -m vc -s meshlabFilters.mlx')
+            print('>>> meshlab >>>')
+            if not ret:
+                M2 = MeshHelper(f'{M.filebase}.ply')
+                # uv = M2.mesh.visual.uv
+                # M2.mesh.visual = M.mesh.visual
+                # M2.mesh.visual.uv = uv
+                # M2.view()
+                M2.export_h5(M.filebase+'.h5')
+            # os.system(f'rm z.ply {M.filebase}.ply')
+
+if __name__ == "__main__":
+    main()

robotic/include/rai/Core/array.ipp

@@ -78,7 +78,8 @@ template<class T> Array<T>::Array(Array<T>&& a)
     M(a.M),
     special(a.special) {
   //if(a.jac) jac = std::move(a.jac);
-  CHECK_EQ(a.d, &a.d0, "");
+  // CHECK_EQ(a.d, &a.d0, "NIY for larger tensors");
+  if(a.d!=&a.d0) { d=a.d; a.d=&a.d0; }
   a.p=NULL;
   a.N=a.nd=a.d0=a.d1=a.d2=0;
   a.isReference=false;
@@ -105,6 +106,7 @@ template<class T> Array<T>::~Array() {
   clear();
 #else //faster (leaves members non-zeroed..)
   if(special) { delete special; special=NULL; }
+  if(d!=&d0) { delete[] d; }
   if(M) {
     globalMemoryTotal -= M*sizeT;
     if(memMove==1) free(p); else delete[] p;

robotic/include/rai/Core/util.h

@@ -372,17 +372,17 @@ struct FileToken {
   std::shared_ptr<std::ifstream> is;
 
   FileToken();
-  FileToken(const char* _filename, bool change_dir=false);
+  FileToken(const char* _filename);
   FileToken(const FileToken& ft);
   ~FileToken();
   FileToken& operator()() { return *this; }
 
   void decomposeFilename();
-  void cd_start();
+  void cd_base();
   void cd_file();
   bool exists();
-  std::ostream& getOs(bool change_dir=false);
-  std::istream& getIs(bool change_dir=false);
+  std::ostream& getOs();
+  std::istream& getIs();
   operator std::istream& () { return getIs(); }
   operator std::ostream& () { return getOs(); }
 
@@ -397,7 +397,7 @@ template<class T> FileToken& operator<<(T& x, FileToken& fil) { fil.getIs() >>x;
 template<class T> void operator>>(const T& x, FileToken& fil) { fil.getOs() <<x; }
 inline bool operator==(const FileToken&, const FileToken&) { return false; }
 }
-#define FILE(filename) (rai::FileToken(filename, false)()) //it needs to return a REFERENCE to a local scope object
+#define FILE(filename) (rai::FileToken(filename)()) //it needs to return a REFERENCE to a local scope object
 
 //===========================================================================
 //
@@ -606,10 +606,11 @@ inline bool operator==(Type& t1, Type& t2) { return t1.typeId() == t2.typeId();
 //
 
  template<class T> T fromFile(const char* filename) {
-   rai::FileToken file(filename, true);
+   rai::FileToken file(filename);
    T x;
+   file.cd_file();
    x.read(file.getIs());
-   file.cd_start();
+   file.cd_base();
    return x;
  }
 

robotic/include/rai/Geo/geo.h

@@ -74,7 +74,7 @@ struct Matrix {
 
   Matrix() {}
   Matrix(int zero) { CHECK_EQ(zero, 0, "this is only for initialization with zero"); setZero(); }
-  Matrix(const arr& m) { CHECK_EQ(m.N, 9, "");  set(m.p); }
+  Matrix(const arr& m) { set(m); }
   Matrix(const Matrix& m) : m00(m.m00), m01(m.m01), m02(m.m02), m10(m.m10), m11(m.m11), m12(m.m12), m20(m.m20), m21(m.m21), m22(m.m22) {}
   double* p() { return &m00; }
   arr getArr() const { return arr(&m00, 9, true).reshape(3, 3); }
@@ -84,6 +84,7 @@ struct Matrix {
   void setZero();
   void setRandom(double range=1.);
   void setId();
+  void set(const arr& X);
   void setDiag(const arr& diag);
   void setSymmetric(const arr& entries6);
   void setSkew(const Vector&);

robotic/include/rai/KOMO/manipTools.h

@@ -31,7 +31,7 @@ struct ManipulationHelper {
   void setup_sequence(rai::Configuration& C, uint K, double homing_scale=1e-2, double velocity_scale=1e-1, bool accumulated_collisions=true, bool joint_limits=true, bool quaternion_norms=true);
   void setup_motion(rai::Configuration& C, uint K, uint steps_per_phase, double homing_scale=0., double acceleration_scale=1e-1, bool accumulated_collisions=true, bool joint_limits=true, bool quaternion_norms=true);
   void setup_pick_and_place_waypoints(rai::Configuration& C, const char* gripper, const char* obj, double homing_scale=1e-2, double velocity_scale=1e-1, bool accumulated_collisions=true, bool joint_limits=true, bool quaternion_norms=true);
-  void setup_point_to_point_motion(rai::Configuration& C, const arr& q1, double homing_scale=1e-2, double acceleration_scale=1e-1, bool accumulated_collisions=true, bool joint_limits=true, bool quaternion_norms=true);
+  void setup_point_to_point_motion(rai::Configuration& C, const arr& q1, uint steps_per_phase, double homing_scale=1e-2, double acceleration_scale=1e-1, bool accumulated_collisions=true, bool joint_limits=true, bool quaternion_norms=true);
   void setup_point_to_point_rrt(rai::Configuration& C, const arr& q0, const arr& q1, const StringA& explicitCollisionPairs);
 
   void add_stable_frame(rai::JointType type, const char* parent, const char* name, const char* initName=0, rai::Frame* initFrame=0, double markerSize=-1.);
@@ -63,7 +63,7 @@ struct ManipulationHelper {
   arr sample(const char* sampleMethod=0, int verbose=1);
   void debug(bool listObjectives=true, bool plotOverTime=false);
 
-  std::shared_ptr<ManipulationHelper> sub_motion(uint phase, bool fixEnd=true, double homing_scale=1e-2, double acceleration_scale=1e-1, bool accumulated_collisions=true, bool joint_limits=true, bool quaternion_norms=false, const StringA& activeDofs={});
+  std::shared_ptr<ManipulationHelper> sub_motion(uint phase, uint steps_per_phase, bool fixEnd=true, double homing_scale=1e-2, double acceleration_scale=1e-1, bool accumulated_collisions=true, bool joint_limits=true, bool quaternion_norms=true, const StringA& activeDofs={});
   std::shared_ptr<rai::RRT_PathFinder> sub_rrt(uint phase, const StringA& explicitCollisionPairs= {}, const StringA& activeDofs={});
 
   void play(rai::Configuration& C, double duration=1.);

robotic/include/rai/Kin/frame.h

@@ -176,9 +176,9 @@ struct Frame : NonCopyable {
   Frame& setTensorShape(const floatA& data, const arr& size);
   Frame& setImplicitSurface(const floatA& data, const arr& size, uint blur, double resample);
   Frame& setColor(const arr& color);
-  Frame& setJoint(rai::JointType jointType, const arr& limits= {});
+  Frame& setJoint(rai::JointType jointType, const arr& limits={}, double scale=1., Frame* mimic=0);
   Frame& setContact(int cont);
-  Frame& setMass(double mass);
+  Frame& setMass(double mass, const arr& inertiaMatrix={});
   Frame& setAttribute(const char* key, double value);
   Frame& setJointState(const arr& q); ///< throws error if this frame is not also a joint, and if q.size() != joint->dim
 

robotic/include/rai/Kin/kin.h

@@ -181,8 +181,8 @@ struct Configuration {
   void pruneEmptyShapes();
   void reconnectShapesToParents();
   void reconnectLinksToClosestJoints();        ///< re-connect all links to closest joint
-  void pruneUselessFrames(bool pruneNamed=false, bool pruneNonContactNonMarker=false, bool pruneTransparent=false);  ///< delete frames that have no name, joint, and shape
-  void processStructure(bool pruneNamed=false, bool pruneNonContactNonMarker=false, bool pruneTransparent=false);        ///< call the three above methods in this order
+  void pruneUselessFrames(bool pruneNamed=false, bool pruneNonContactShapes=false, bool pruneTransparent=false);  ///< delete frames that have no name, joint, and shape
+  void processStructure(bool _pruneRigidJoints=false, bool reconnectToLinks=true, bool pruneNonContactShapes=false, bool pruneTransparent=false);        ///< call the three above methods in this order
   void processInertias(bool recomputeInertias=true, bool transformToDiagInertia=false);
   void sortFrames();
   void makeObjectsFree(const StringA& objects, double H_cost=0.);
@@ -289,7 +289,7 @@ public:
   void write(Graph& G) const;
   void writeURDF(std::ostream& os, const char* robotName="myrobot") const;
   void writeCollada(const char* filename, const char* format="collada") const;
-  void writeMeshes(str pathPrefix="meshes/", bool copyTextures=false) const;
+  void writeMeshes(str pathPrefix="meshes/", bool copyTextures=false, bool enumerateTextures=false) const;
   void writeMesh(const char* filename="z.ply") const;
   void read(std::istream& is);
   Graph getGraph() const;

robotic/include/rai/ry/types.h

@@ -17,8 +17,9 @@
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
 #include <pybind11/numpy.h>
+#include <pybind11/pytypes.h>
 
-pybind11::dict graph2dict(const rai::Graph& G);
+pybind11::dict graph2dict(const rai::Graph& G, const rai::NodeL& parents={}, bool parentsInKeys=true);
 rai::Graph dict2graph(const pybind11::dict& dict);
 
 pybind11::list graph2list(const rai::Graph& G);
@@ -308,7 +309,7 @@ template <> struct type_caster<rai::Graph> {
   }
 
   static handle cast(const rai::Graph& G, return_value_policy, handle) {
-    return graph2dict(G).release();
+    return graph2dict(G, rai::NodeL{}).release();
   }
 };
 

robotic/mujoco-import.py

@@ -0,0 +1,10 @@
+from config_mujoco import *
+import sysconfig
+
+pysite = sysconfig.get_paths()["purelib"]
+file = pysite+"/gymnasium_robotics/envs/assets/kitchen_franka/kitchen_assets/kitchen_env_model.xml"
+# file = '/home/mtoussai/git/MuJoCo2Rai/kitchen_dataset/RUSTIC_ONE_WALL_SMALL.xml'
+
+print('=====================', file)
+C = Config_Mujoco(file, visualsOnly=True)
+C.view(True)

robotic/rai-robotModels/g1/g1.g

@@ -1,5 +1,5 @@
 ## create standard base frame before including
 
 g1_base: { X:[0, 0, .79], multibody: true, multibody_fixedBase: false }
-Include: <g1_clean.g>
+Include: <g1_29dof_conv.g>
 Edit pelvis(g1_base): {}

robotic/rai-robotModels/g1/g1_29dof_conv.g

@@ -0,0 +1,77 @@
+pelvis: { shape: mesh, color: [0.2, 0.2, 0.2, 1], mesh: <meshes/pelvis.h5>, mass: 0.400236, inertia: [0.00159584, -5.92804e-10, -1.2185e-09, 0.0013267, 3.05393e-09, 0.00119011] },
+pelvis_contour_joint(pelvis): { joint: rigid, shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/pelvis_contour_link.h5>, mass: 0.393642, inertia: [0.00120549, -1.39118e-07, -1.37717e-06, 0.00142459, -1.2061e-07, 0.00136524] },
+left_hip_pitch_joint_origin(pelvis): { Q: [0, 0.064452, -0.1027] },
+right_hip_pitch_joint_origin(pelvis): { Q: [0, -0.064452, -0.1027] },
+imu_in_pelvis_joint_origin(pelvis): { Q: [0.04525, 0, -0.08339] },
+waist_yaw_joint(pelvis): { joint: hingeZ, limits: [-2.618, 2.618], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/waist_yaw_link.h5>, mass: 0.168083, inertia: [8.83938e-05, -2.82438e-09, -9.9721e-06, 0.000103081, -2.02902e-09, 0.000150067] },
+left_hip_pitch_joint(left_hip_pitch_joint_origin): { joint: hingeY, limits: [-2.5307, 2.8798], shape: mesh, color: [0.2, 0.2, 0.2, 1], mesh: <meshes/left_hip_pitch_link.h5>, mass: 0.264573, inertia: [0.000474721, 4.31311e-06, 5.93997e-06, 0.000392354, 4.96908e-05, 0.000375138] },
+right_hip_pitch_joint(right_hip_pitch_joint_origin): { joint: hingeY, limits: [-2.5307, 2.8798], shape: mesh, color: [0.2, 0.2, 0.2, 1], mesh: <meshes/right_hip_pitch_link.h5>, mass: 0.264569, inertia: [0.000474711, -4.31398e-06, 5.94077e-06, 0.000392343, -4.96889e-05, 0.000375138] },
+imu_in_pelvis_joint(imu_in_pelvis_joint_origin): { joint: rigid },
+waist_roll_joint_origin(waist_yaw_joint): { Q: [-0.0039635, 0, 0.035] },
+left_hip_roll_joint_origin(left_hip_pitch_joint): { Q: [0, 0.052, -0.030465, 0.996179, 0, -0.0873386, 0] },
+right_hip_roll_joint_origin(right_hip_pitch_joint): { Q: [0, -0.052, -0.030465, 0.996179, 0, -0.0873386, 0] },
+waist_roll_joint(waist_roll_joint_origin): { joint: hingeX, limits: [-0.52, 0.52], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/waist_roll_link.h5>, mass: 0.0221562, inertia: [4.0451e-06, 7.41165e-11, -2.63317e-11, 3.67093e-06, 3.77707e-11, 2.32541e-06] },
+left_hip_roll_joint(left_hip_roll_joint_origin): { joint: hingeX, limits: [-0.5236, 2.9671], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_hip_roll_link.h5>, mass: 0.388504, inertia: [0.00122176, -2.52983e-06, -0.000212212, 0.00118448, -1.72307e-06, 0.00069136] },
+right_hip_roll_joint(right_hip_roll_joint_origin): { joint: hingeX, limits: [-2.9671, 0.5236], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_hip_roll_link.h5>, mass: 0.388501, inertia: [0.00122175, 2.52204e-06, -0.000212212, 0.00118447, 1.73174e-06, 0.000691349] },
+waist_pitch_joint_origin(waist_roll_joint): { Q: [0, 0, 0.019] },
+left_hip_yaw_joint_origin(left_hip_roll_joint): { Q: [0.025001, 0, -0.12412] },
+right_hip_yaw_joint_origin(right_hip_roll_joint): { Q: [0.025001, 0, -0.12412] },
+waist_pitch_joint(waist_pitch_joint_origin): { joint: hingeY, limits: [-0.52, 0.52], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/torso_link.h5>, mass: 3.72833, inertia: [0.0404695, -1.1264e-05, -0.000266539, 0.0333617, -1.24012e-05, 0.0221975] },
+left_hip_yaw_joint(left_hip_yaw_joint_origin): { joint: hingeZ, limits: [-2.7576, 2.7576], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_hip_yaw_link.h5>, mass: 0.681619, inertia: [0.0044275, -0.000108056, -0.00166275, 0.00545319, -0.000187376, 0.001876] },
+right_hip_yaw_joint(right_hip_yaw_joint_origin): { joint: hingeZ, limits: [-2.7576, 2.7576], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_hip_yaw_link.h5>, mass: 0.681613, inertia: [0.00442747, 0.000108056, -0.00166275, 0.00545319, 0.000187351, 0.00187597] },
+logo_joint_origin(waist_pitch_joint): { Q: [0.0039635, 0, -0.054] },
+head_joint_origin(waist_pitch_joint): { Q: [0.0039635, 0, -0.054] },
+waist_support_joint_origin(waist_pitch_joint): { Q: [0.0039635, 0, -0.054] },
+imu_in_torso_joint_origin(waist_pitch_joint): { Q: [-0.03959, -0.00224, 0.13792] },
+d435_joint_origin(waist_pitch_joint): { Q: [0.0576235, 0.01753, 0.41987, 0.91496, 0, 0.403545, 0] },
+mid360_joint_origin(waist_pitch_joint): { Q: [0.0002835, 3e-05, 0.40618, 0.999799, 0, 0.0200699, 0] },
+left_shoulder_pitch_joint_origin(waist_pitch_joint): { Q: [0.0039563, 0.10022, 0.23778, 0.990264, 0.139201, 4.05418e-05, -9.10379e-05] },
+right_shoulder_pitch_joint_origin(waist_pitch_joint): { Q: [0.0039563, -0.10021, 0.23778, 0.990264, -0.139201, 4.05418e-05, 9.10379e-05] },
+left_knee_joint_origin(left_hip_yaw_joint): { Q: [-0.078273, 0.0021489, -0.17734, 0.996179, 0, 0.0873386, 0] },
+right_knee_joint_origin(right_hip_yaw_joint): { Q: [-0.078273, -0.0021489, -0.17734, 0.996179, 0, 0.0873386, 0] },
+logo_joint(logo_joint_origin): { joint: rigid, shape: mesh, color: [0.2, 0.2, 0.2, 1], mesh: <meshes/logo_link.h5>, mass: 0.0575578, inertia: [0.000100919, -1.65034e-06, 1.03204e-06, 0.000279739, 5.60472e-08, 0.000376764] },
+head_joint(head_joint_origin): { joint: rigid, shape: mesh, color: [0.2, 0.2, 0.2, 1], mesh: <meshes/head_link.h5>, mass: 0.493975, inertia: [0.00295612, 1.28062e-08, -0.000167611, 0.00293932, 2.9539e-08, 0.0012706] },
+waist_support_joint(waist_support_joint_origin): { joint: rigid, shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/waist_support_link.h5>, mass: 0.202405, inertia: [0.00046643, -1.24677e-07, -2.44136e-07, 0.000381836, -3.23169e-08, 0.000786744] },
+imu_in_torso_joint(imu_in_torso_joint_origin): { joint: rigid },
+d435_joint(d435_joint_origin): { joint: rigid },
+mid360_joint(mid360_joint_origin): { joint: rigid },
+left_shoulder_pitch_joint(left_shoulder_pitch_joint_origin): { joint: hingeY, limits: [-3.0892, 2.6704], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_shoulder_pitch_link.h5>, mass: 0.126023, inertia: [0.000160485, -1.66228e-07, 3.91865e-07, 0.000151496, 1.75614e-05, 0.000138798] },
+right_shoulder_pitch_joint(right_shoulder_pitch_joint_origin): { joint: hingeY, limits: [-3.0892, 2.6704], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_shoulder_pitch_link.h5>, mass: 0.126023, inertia: [0.000160483, 1.65124e-07, 3.90512e-07, 0.000151492, -1.75623e-05, 0.000138793] },
+left_knee_joint(left_knee_joint_origin): { joint: hingeY, limits: [-0.087267, 2.8798], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_knee_link.h5>, mass: 0.876712, inertia: [0.00648182, 3.3975e-05, -0.000127209, 0.00672925, -0.000358869, 0.00109586] },
+right_knee_joint(right_knee_joint_origin): { joint: hingeY, limits: [-0.087267, 2.8798], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_knee_link.h5>, mass: 0.876714, inertia: [0.00648176, -3.398e-05, -0.000127209, 0.00672918, 0.000358822, 0.00109587] },
+left_shoulder_roll_joint_origin(left_shoulder_pitch_joint): { Q: [0, 0.038, -0.013831, 0.990268, -0.139172, 0, 0] },
+right_shoulder_roll_joint_origin(right_shoulder_pitch_joint): { Q: [0, -0.038, -0.013831, 0.990268, 0.139172, 0, 0] },
+left_ankle_pitch_joint_origin(left_knee_joint): { Q: [0, -9.4445e-05, -0.30001] },
+right_ankle_pitch_joint_origin(right_knee_joint): { Q: [0, 9.4445e-05, -0.30001] },
+left_shoulder_roll_joint(left_shoulder_roll_joint_origin): { joint: hingeX, limits: [-1.5882, 2.2515], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_shoulder_roll_link.h5>, mass: 0.246281, inertia: [0.000394956, 2.09873e-08, -1.93073e-07, 0.000469736, 1.57454e-05, 0.000272781] },
+right_shoulder_roll_joint(right_shoulder_roll_joint_origin): { joint: hingeX, limits: [-2.2515, 1.5882], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_shoulder_roll_link.h5>, mass: 0.246283, inertia: [0.000394966, -1.6719e-08, -1.89014e-07, 0.000469741, -1.57405e-05, 0.000272773] },
+left_ankle_pitch_joint(left_ankle_pitch_joint_origin): { joint: hingeY, limits: [-0.87267, 0.5236], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_ankle_pitch_link.h5>, mass: 0.0197524, inertia: [2.78775e-06, -2.27316e-10, -5.8987e-07, 4.52084e-06, -3.84759e-10, 2.51425e-06] },
+right_ankle_pitch_joint(right_ankle_pitch_joint_origin): { joint: hingeY, limits: [-0.87267, 0.5236], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_ankle_pitch_link.h5>, mass: 0.0197524, inertia: [2.78777e-06, 2.213e-10, -5.89865e-07, 4.52084e-06, 3.98325e-10, 2.51424e-06] },
+left_shoulder_yaw_joint_origin(left_shoulder_roll_joint): { Q: [0, 0.00624, -0.1032] },
+right_shoulder_yaw_joint_origin(right_shoulder_roll_joint): { Q: [0, -0.00624, -0.1032] },
+left_ankle_roll_joint_origin(left_ankle_pitch_joint): { Q: [0, 0, -0.017558] },
+right_ankle_roll_joint_origin(right_ankle_pitch_joint): { Q: [0, 0, -0.017558] },
+left_shoulder_yaw_joint(left_shoulder_yaw_joint_origin): { joint: hingeZ, limits: [-2.618, 2.618], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_shoulder_yaw_link.h5>, mass: 0.330191, inertia: [0.00075724, 1.01914e-05, 0.000118153, 0.000781633, -6.07748e-05, 0.000303097] },
+right_shoulder_yaw_joint(right_shoulder_yaw_joint_origin): { joint: hingeZ, limits: [-2.618, 2.618], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_shoulder_yaw_link.h5>, mass: 0.330189, inertia: [0.000757238, -1.01934e-05, 0.000118161, 0.000781632, 6.07703e-05, 0.000303094] },
+left_ankle_roll_joint(left_ankle_roll_joint_origin): { joint: hingeX, limits: [-0.2618, 0.2618], shape: mesh, color: [0.2, 0.2, 0.2, 1], mesh: <meshes/left_ankle_roll_link.h5>, mass: 0.456098, inertia: [0.000287801, 6.50248e-08, 8.30648e-05, 0.00151895, -1.18792e-07, 0.0016069] },
+right_ankle_roll_joint(right_ankle_roll_joint_origin): { joint: hingeX, limits: [-0.2618, 0.2618], shape: mesh, color: [0.2, 0.2, 0.2, 1], mesh: <meshes/right_ankle_roll_link.h5>, mass: 0.45609, inertia: [0.0002878, -9.56202e-08, 8.30614e-05, 0.00151893, 1.13682e-07, 0.00160687] },
+left_elbow_joint_origin(left_shoulder_yaw_joint): { Q: [0.015783, 0, -0.080518] },
+right_elbow_joint_origin(right_shoulder_yaw_joint): { Q: [0.015783, 0, -0.080518] },
+left_elbow_joint(left_elbow_joint_origin): { joint: hingeY, limits: [-1.0472, 2.0944], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_elbow_link.h5>, mass: 0.133212, inertia: [0.000105606, 4.41256e-05, 1.51768e-05, 0.000221879, -4.66813e-06, 0.000221601] },
+right_elbow_joint(right_elbow_joint_origin): { joint: hingeY, limits: [-1.0472, 2.0944], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_elbow_link.h5>, mass: 0.133212, inertia: [0.000105601, -4.41257e-05, 1.5176e-05, 0.000221877, 4.66758e-06, 0.000221596] },
+left_wrist_roll_joint_origin(left_elbow_joint): { Q: [0.1, 0.00188791, -0.01] },
+right_wrist_roll_joint_origin(right_elbow_joint): { Q: [0.1, -0.00188791, -0.01] },
+left_wrist_roll_joint(left_wrist_roll_joint_origin): { joint: hingeX, limits: [-1.97222, 1.97222], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_wrist_roll_link.h5>, mass: 0.0819051, inertia: [5.76855e-05, -6.70778e-07, 1.09252e-09, 4.18275e-05, -1.21848e-09, 6.33789e-05] },
+right_wrist_roll_joint(right_wrist_roll_joint_origin): { joint: hingeX, limits: [-1.97222, 1.97222], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_wrist_roll_link.h5>, mass: 0.0819045, inertia: [5.76834e-05, 6.70746e-07, 8.73074e-10, 4.18274e-05, 8.19224e-10, 6.33769e-05] },
+left_wrist_pitch_joint_origin(left_wrist_roll_joint): { Q: [0.038, 0, 0] },
+right_wrist_pitch_joint_origin(right_wrist_roll_joint): { Q: [0.038, 0, 0] },
+left_wrist_pitch_joint(left_wrist_pitch_joint_origin): { joint: hingeY, limits: [-1.61443, 1.61443], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_wrist_pitch_link.h5>, mass: 0.102165, inertia: [6.54114e-05, -1.39886e-06, 1.39875e-06, 0.000133698, 1.78434e-08, 0.000133699] },
+right_wrist_pitch_joint(right_wrist_pitch_joint_origin): { joint: hingeY, limits: [-1.61443, 1.61443], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_wrist_pitch_link.h5>, mass: 0.102164, inertia: [6.54077e-05, 1.39993e-06, 1.3996e-06, 0.000133692, -1.67215e-08, 0.000133692] },
+left_wrist_yaw_joint_origin(left_wrist_pitch_joint): { Q: [0.046, 0, 0] },
+right_wrist_yaw_joint_origin(right_wrist_pitch_joint): { Q: [0.046, 0, 0] },
+left_wrist_yaw_joint(left_wrist_yaw_joint_origin): { joint: hingeZ, limits: [-1.61443, 1.61443], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_wrist_yaw_link.h5>, mass: 0.0806154, inertia: [5.53182e-05, -1.04206e-06, 7.04147e-07, 6.51217e-05, 2.05369e-08, 4.14947e-05] },
+right_wrist_yaw_joint(right_wrist_yaw_joint_origin): { joint: hingeZ, limits: [-1.61443, 1.61443], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_wrist_yaw_link.h5>, mass: 0.0806224, inertia: [5.53259e-05, 1.04209e-06, 7.04755e-07, 6.51361e-05, -2.11753e-08, 4.15045e-05] },
+left_hand_palm_joint_origin(left_wrist_yaw_joint): { Q: [0.0415, 0.003, 0] },
+right_hand_palm_joint_origin(right_wrist_yaw_joint): { Q: [0.0415, -0.003, 0] },
+left_hand_palm_joint(left_hand_palm_joint_origin): { joint: rigid, shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_rubber_hand.h5>, mass: 0.154796, inertia: [0.000129099, 4.62168e-05, 5.26706e-06, 0.000313538, 2.47853e-06, 0.000241113] },
+right_hand_palm_joint(right_hand_palm_joint_origin): { joint: rigid, shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_rubber_hand.h5>, mass: 0.154795, inertia: [0.0001291, -4.62179e-05, 5.26836e-06, 0.000313541, -2.47823e-06, 0.000241117] }

robotic/rai-robotModels/g1/g1_clean.g

@@ -1,99 +1,64 @@
-pelvis: { shape: mesh, color: [0.2, 0.2, 0.2, 1], mesh: <meshes/pelvis.ply>, mass: 3.813, inertia: [0.010549, 0, 2.1e-06, 0.0093089, 0, 0.0079184] },
+pelvis: { shape: mesh, color: [0.2, 0.2, 0.2, 1], mesh: <meshes/pelvis.h5>, mass: 0.793878, com: [0.000297452, 1.17304e-05, -0.0760703], inertia: [0.00280151, -1.42902e-07, -1.26125e-06, 0.00275155, -1.1233e-07, 0.00255542] },
 left_hip_pitch_joint_origin(pelvis): { Q: [0, 0.064452, -0.1027] },
 right_hip_pitch_joint_origin(pelvis): { Q: [0, -0.064452, -0.1027] },
-waist_yaw_joint(pelvis): { joint: hingeZ, limits: [-2.618, 2.618, 32, -1, 88], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/waist_yaw_link.ply>, ctrl_limits: [32, -1, 88] },
-pelvis_contour_link(pelvis): { mass: 0.001, inertia: [1e-07, 1e-07, 1e-07] },
-pelvis_contour_link_0(pelvis): { shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/pelvis_contour_link.ply>, visual: True },
-left_hip_pitch_joint(left_hip_pitch_joint_origin): { joint: hingeY, limits: [-2.5307, 2.8798, 32, -1, 88], shape: mesh, color: [0.2, 0.2, 0.2, 1], mesh: <meshes/left_hip_pitch_link.ply>, ctrl_limits: [32, -1, 88] },
-right_hip_pitch_joint(right_hip_pitch_joint_origin): { joint: hingeY, limits: [-2.5307, 2.8798, 32, -1, 88], shape: mesh, color: [0.2, 0.2, 0.2, 1], mesh: <meshes/right_hip_pitch_link.ply>, ctrl_limits: [32, -1, 88] },
-waist_yaw_link(waist_yaw_joint): { mass: 0.244, inertia: [9.9587e-05, -1.833e-06, -1.2617e-05, 0.00012411, -1.18e-07, 0.00015586] },
+waist_yaw_joint(pelvis): { joint: hingeZ, limits: [-2.618, 2.618], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/waist_yaw_link.h5>, mass: 0.168083, inertia: [8.83938e-05, -2.82438e-09, -9.9721e-06, 0.000103081, -2.02902e-09, 0.000150067] },
+pelvis_contour_link_0(pelvis): { shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/pelvis_contour_link.h5> },
+left_hip_pitch_joint(left_hip_pitch_joint_origin): { joint: hingeY, limits: [-2.5307, 2.8798], shape: mesh, color: [0.2, 0.2, 0.2, 1], mesh: <meshes/left_hip_pitch_link.h5>, mass: 0.264573, inertia: [0.000474721, 4.31311e-06, 5.93997e-06, 0.000392354, 4.96908e-05, 0.000375138] },
+right_hip_pitch_joint(right_hip_pitch_joint_origin): { joint: hingeY, limits: [-2.5307, 2.8798], shape: mesh, color: [0.2, 0.2, 0.2, 1], mesh: <meshes/right_hip_pitch_link.h5>, mass: 0.264569, inertia: [0.000474711, -4.31398e-06, 5.94077e-06, 0.000392343, -4.96889e-05, 0.000375138] },
 waist_roll_joint_origin(waist_yaw_joint): { Q: [-0.0039635, 0, 0.035] },
-left_hip_pitch_link(left_hip_pitch_joint): { mass: 1.35, inertia: [0.001811, 3.68e-05, -3.44e-05, 0.0014193, 0.000171, 0.0012812] },
 left_hip_roll_joint_origin(left_hip_pitch_joint): { Q: [0, 0.052, -0.030465, 0.996179, 0, -0.0873386, 0] },
-right_hip_pitch_link(right_hip_pitch_joint): { mass: 1.35, inertia: [0.001811, -3.68e-05, -3.44e-05, 0.0014193, -0.000171, 0.0012812] },
 right_hip_roll_joint_origin(right_hip_pitch_joint): { Q: [0, -0.052, -0.030465, 0.996179, 0, -0.0873386, 0] },
-waist_roll_joint(waist_roll_joint_origin): { joint: hingeX, limits: [-0.52, 0.52, 37, -1, 50], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/waist_roll_link.ply>, ctrl_limits: [37, -1, 50] },
-left_hip_roll_joint(left_hip_roll_joint_origin): { joint: hingeX, limits: [-0.5236, 2.9671, 32, -1, 88], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_hip_roll_link.ply>, ctrl_limits: [32, -1, 88] },
-right_hip_roll_joint(right_hip_roll_joint_origin): { joint: hingeX, limits: [-2.9671, 0.5236, 32, -1, 88], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_hip_roll_link.ply>, ctrl_limits: [32, -1, 88] },
-waist_roll_link(waist_roll_joint): { mass: 0.047, inertia: [7.515e-06, 0, 0, 6.398e-06, 9.9e-08, 3.988e-06] },
+waist_roll_joint(waist_roll_joint_origin): { joint: hingeX, limits: [-0.52, 0.52], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/waist_roll_link.h5>, mass: 0.0221562, inertia: [4.0451e-06, 7.41165e-11, -2.63317e-11, 3.67093e-06, 3.77707e-11, 2.32541e-06] },
+left_hip_roll_joint(left_hip_roll_joint_origin): { joint: hingeX, limits: [-0.5236, 2.9671], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_hip_roll_link.h5>, mass: 0.388504, inertia: [0.00122176, -2.52983e-06, -0.000212212, 0.00118448, -1.72307e-06, 0.00069136] },
+right_hip_roll_joint(right_hip_roll_joint_origin): { joint: hingeX, limits: [-2.9671, 0.5236], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_hip_roll_link.h5>, mass: 0.388501, inertia: [0.00122175, 2.52204e-06, -0.000212212, 0.00118447, 1.73174e-06, 0.000691349] },
 waist_pitch_joint_origin(waist_roll_joint): { Q: [0, 0, 0.019] },
-left_hip_roll_link(left_hip_roll_joint): { mass: 1.52, inertia: [0.0023773, -3.8e-06, -0.0003908, 0.0024123, 1.84e-05, 0.0016595] },
 left_hip_yaw_joint_origin(left_hip_roll_joint): { Q: [0.025001, 0, -0.12412] },
-right_hip_roll_link(right_hip_roll_joint): { mass: 1.52, inertia: [0.0023773, 3.8e-06, -0.0003908, 0.0024123, -1.84e-05, 0.0016595] },
 right_hip_yaw_joint_origin(right_hip_roll_joint): { Q: [0.025001, 0, -0.12412] },
-waist_pitch_joint(waist_pitch_joint_origin): { joint: hingeY, limits: [-0.52, 0.52, 37, -1, 50], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/torso_link.ply>, ctrl_limits: [37, -1, 50] },
-left_hip_yaw_joint(left_hip_yaw_joint_origin): { joint: hingeZ, limits: [-2.7576, 2.7576, 32, -1, 88], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_hip_yaw_link.ply>, ctrl_limits: [32, -1, 88] },
-right_hip_yaw_joint(right_hip_yaw_joint_origin): { joint: hingeZ, limits: [-2.7576, 2.7576, 32, -1, 88], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_hip_yaw_link.ply>, ctrl_limits: [32, -1, 88] },
-torso_link(waist_pitch_joint): { mass: 8.562, inertia: [0.065675, -8.597e-05, -0.00173725, 0.0535352, 8.6899e-05, 0.0308081] },
-logo_link(waist_pitch_joint): { Q: [0.0039635, 0, -0.054], mass: 0.001, inertia: [1e-07, 1e-07, 1e-07] },
-logo_link_0(waist_pitch_joint): { Q: [0.0039635, 0, -0.054], shape: mesh, color: [0.2, 0.2, 0.2, 1], mesh: <meshes/logo_link.ply>, visual: True },
-head_link(waist_pitch_joint): { Q: [0.0039635, 0, -0.054], mass: 1.036, inertia: [0.00408505, -2.543e-06, -6.9455e-05, 0.00418521, -3.726e-06, 0.00180791] },
-head_link_0(waist_pitch_joint): { Q: [0.0039635, 0, -0.054], shape: mesh, color: [0.2, 0.2, 0.2, 1], mesh: <meshes/head_link.ply>, visual: True },
-waist_support_link(waist_pitch_joint): { Q: [0.0039635, 0, -0.054], mass: 0.001, inertia: [1e-07, 1e-07, 1e-07] },
-waist_support_link_0(waist_pitch_joint): { Q: [0.0039635, 0, -0.054], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/waist_support_link.ply>, visual: True },
-left_shoulder_pitch_joint_origin(waist_pitch_joint): { Q: [0.0039563, 0.10022, 0.23778, 0.990264, 0.139201, 1.38722e-05, -9.86868e-05] },
-right_shoulder_pitch_joint_origin(waist_pitch_joint): { Q: [0.0039563, -0.10021, 0.23778, 0.990264, -0.139201, 1.38722e-05, 9.86868e-05] },
-left_hip_yaw_link(left_hip_yaw_joint): { mass: 1.702, inertia: [0.0057774, -0.0005411, -0.0023948, 0.0076124, -0.0007072, 0.003149] },
+waist_pitch_joint(waist_pitch_joint_origin): { joint: hingeY, limits: [-0.52, 0.52], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/torso_link.h5>, mass: 4.46637, com: [0.00435575, 0.00507317, 0.15113], inertia: [0.0736175, -4.25291e-06, -0.0008917, 0.0665665, 0.000409521, 0.0246276] },
+left_hip_yaw_joint(left_hip_yaw_joint_origin): { joint: hingeZ, limits: [-2.7576, 2.7576], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_hip_yaw_link.h5>, mass: 0.681619, inertia: [0.0044275, -0.000108056, -0.00166275, 0.00545319, -0.000187376, 0.001876] },
+right_hip_yaw_joint(right_hip_yaw_joint_origin): { joint: hingeZ, limits: [-2.7576, 2.7576], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_hip_yaw_link.h5>, mass: 0.681613, inertia: [0.00442747, 0.000108056, -0.00166275, 0.00545319, 0.000187351, 0.00187597] },
+left_shoulder_pitch_joint_origin(waist_pitch_joint): { Q: [0.0039563, 0.10022, 0.23778, 0.990264, 0.139201, 4.05418e-05, -9.10379e-05] },
+right_shoulder_pitch_joint_origin(waist_pitch_joint): { Q: [0.0039563, -0.10021, 0.23778, 0.990264, -0.139201, 4.05418e-05, 9.10379e-05] },
+logo_link_0(waist_pitch_joint): { Q: [0.0039635, 0, -0.054], shape: mesh, color: [0.2, 0.2, 0.2, 1], mesh: <meshes/logo_link.h5> },
+head_link_0(waist_pitch_joint): { Q: [0.0039635, 0, -0.054], shape: mesh, color: [0.2, 0.2, 0.2, 1], mesh: <meshes/head_link.h5> },
+waist_support_link_0(waist_pitch_joint): { Q: [0.0039635, 0, -0.054], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/waist_support_link.h5> },
 left_knee_joint_origin(left_hip_yaw_joint): { Q: [-0.078273, 0.0021489, -0.17734, 0.996179, 0, 0.0873386, 0] },
-right_hip_yaw_link(right_hip_yaw_joint): { mass: 1.702, inertia: [0.0057774, 0.0005411, -0.0023948, 0.0076124, 0.0007072, 0.003149] },
 right_knee_joint_origin(right_hip_yaw_joint): { Q: [-0.078273, -0.0021489, -0.17734, 0.996179, 0, 0.0873386, 0] },
-left_shoulder_pitch_joint(left_shoulder_pitch_joint_origin): { joint: hingeY, limits: [-3.0892, 2.6704, 37, -1, 25], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_shoulder_pitch_link.ply>, ctrl_limits: [37, -1, 25] },
-right_shoulder_pitch_joint(right_shoulder_pitch_joint_origin): { joint: hingeY, limits: [-3.0892, 2.6704, 37, -1, 25], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_shoulder_pitch_link.ply>, ctrl_limits: [37, -1, 25] },
-left_knee_joint(left_knee_joint_origin): { joint: hingeY, limits: [-0.087267, 2.8798, 20, -1, 139], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_knee_link.ply>, ctrl_limits: [20, -1, 139] },
-right_knee_joint(right_knee_joint_origin): { joint: hingeY, limits: [-0.087267, 2.8798, 20, -1, 139], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_knee_link.ply>, ctrl_limits: [20, -1, 139] },
-left_shoulder_pitch_link(left_shoulder_pitch_joint): { mass: 0.718, inertia: [0.0004291, -9.2e-06, 6.4e-06, 0.000453, 2.26e-05, 0.000423] },
+left_shoulder_pitch_joint(left_shoulder_pitch_joint_origin): { joint: hingeY, limits: [-3.0892, 2.6704], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_shoulder_pitch_link.h5>, mass: 0.126023, inertia: [0.000160485, -1.66228e-07, 3.91865e-07, 0.000151496, 1.75614e-05, 0.000138798] },
+right_shoulder_pitch_joint(right_shoulder_pitch_joint_origin): { joint: hingeY, limits: [-3.0892, 2.6704], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_shoulder_pitch_link.h5>, mass: 0.126023, inertia: [0.000160483, 1.65124e-07, 3.90512e-07, 0.000151492, -1.75623e-05, 0.000138793] },
+left_knee_joint(left_knee_joint_origin): { joint: hingeY, limits: [-0.087267, 2.8798], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_knee_link.h5>, mass: 0.876712, inertia: [0.00648182, 3.3975e-05, -0.000127209, 0.00672925, -0.000358869, 0.00109586] },
+right_knee_joint(right_knee_joint_origin): { joint: hingeY, limits: [-0.087267, 2.8798], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_knee_link.h5>, mass: 0.876714, inertia: [0.00648176, -3.398e-05, -0.000127209, 0.00672918, 0.000358822, 0.00109587] },
 left_shoulder_roll_joint_origin(left_shoulder_pitch_joint): { Q: [0, 0.038, -0.013831, 0.990268, -0.139172, 0, 0] },
-right_shoulder_pitch_link(right_shoulder_pitch_joint): { mass: 0.718, inertia: [0.0004291, 9.2e-06, 6.4e-06, 0.000453, -2.26e-05, 0.000423] },
 right_shoulder_roll_joint_origin(right_shoulder_pitch_joint): { Q: [0, -0.038, -0.013831, 0.990268, 0.139172, 0, 0] },
-left_knee_link(left_knee_joint): { mass: 1.932, inertia: [0.011329, 4.82e-05, -4.49e-05, 0.011277, -0.0007146, 0.0015168] },
 left_ankle_pitch_joint_origin(left_knee_joint): { Q: [0, -9.4445e-05, -0.30001] },
-right_knee_link(right_knee_joint): { mass: 1.932, inertia: [0.011329, -4.82e-05, 4.49e-05, 0.011277, 0.0007146, 0.0015168] },
 right_ankle_pitch_joint_origin(right_knee_joint): { Q: [0, 9.4445e-05, -0.30001] },
-left_shoulder_roll_joint(left_shoulder_roll_joint_origin): { joint: hingeX, limits: [-1.5882, 2.2515, 37, -1, 25], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_shoulder_roll_link.ply>, ctrl_limits: [37, -1, 25] },
-right_shoulder_roll_joint(right_shoulder_roll_joint_origin): { joint: hingeX, limits: [-2.2515, 1.5882, 37, -1, 25], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_shoulder_roll_link.ply>, ctrl_limits: [37, -1, 25] },
-left_ankle_pitch_joint(left_ankle_pitch_joint_origin): { joint: hingeY, limits: [-0.87267, 0.5236, 37, -1, 50], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_ankle_pitch_link.ply>, ctrl_limits: [37, -1, 50] },
-right_ankle_pitch_joint(right_ankle_pitch_joint_origin): { joint: hingeY, limits: [-0.87267, 0.5236, 37, -1, 50], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_ankle_pitch_link.ply>, ctrl_limits: [37, -1, 50] },
-left_shoulder_roll_link(left_shoulder_roll_joint): { mass: 0.643, inertia: [0.0006177, -1e-06, 8.7e-06, 0.0006912, -5.3e-06, 0.0003894] },
+left_shoulder_roll_joint(left_shoulder_roll_joint_origin): { joint: hingeX, limits: [-1.5882, 2.2515], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_shoulder_roll_link.h5>, mass: 0.246281, inertia: [0.000394956, 2.09873e-08, -1.93073e-07, 0.000469736, 1.57454e-05, 0.000272781] },
+right_shoulder_roll_joint(right_shoulder_roll_joint_origin): { joint: hingeX, limits: [-2.2515, 1.5882], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_shoulder_roll_link.h5>, mass: 0.246283, inertia: [0.000394966, -1.6719e-08, -1.89014e-07, 0.000469741, -1.57405e-05, 0.000272773] },
+left_ankle_pitch_joint(left_ankle_pitch_joint_origin): { joint: hingeY, limits: [-0.87267, 0.5236], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_ankle_pitch_link.h5>, mass: 0.0197524, inertia: [2.78775e-06, -2.27316e-10, -5.8987e-07, 4.52084e-06, -3.84759e-10, 2.51425e-06] },
+right_ankle_pitch_joint(right_ankle_pitch_joint_origin): { joint: hingeY, limits: [-0.87267, 0.5236], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_ankle_pitch_link.h5>, mass: 0.0197524, inertia: [2.78777e-06, 2.213e-10, -5.89865e-07, 4.52084e-06, 3.98325e-10, 2.51424e-06] },
 left_shoulder_yaw_joint_origin(left_shoulder_roll_joint): { Q: [0, 0.00624, -0.1032] },
-right_shoulder_roll_link(right_shoulder_roll_joint): { mass: 0.643, inertia: [0.0006177, 1e-06, 8.7e-06, 0.0006912, 5.3e-06, 0.0003894] },
 right_shoulder_yaw_joint_origin(right_shoulder_roll_joint): { Q: [0, -0.00624, -0.1032] },
-left_ankle_pitch_link(left_ankle_pitch_joint): { mass: 0.074, inertia: [8.4e-06, 0, -2.9e-06, 1.89e-05, 0, 1.26e-05] },
 left_ankle_roll_joint_origin(left_ankle_pitch_joint): { Q: [0, 0, -0.017558] },
-right_ankle_pitch_link(right_ankle_pitch_joint): { mass: 0.074, inertia: [8.4e-06, 0, -2.9e-06, 1.89e-05, 0, 1.26e-05] },
 right_ankle_roll_joint_origin(right_ankle_pitch_joint): { Q: [0, 0, -0.017558] },
-left_shoulder_yaw_joint(left_shoulder_yaw_joint_origin): { joint: hingeZ, limits: [-2.618, 2.618, 37, -1, 25], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_shoulder_yaw_link.ply>, ctrl_limits: [37, -1, 25] },
-right_shoulder_yaw_joint(right_shoulder_yaw_joint_origin): { joint: hingeZ, limits: [-2.618, 2.618, 37, -1, 25], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_shoulder_yaw_link.ply>, ctrl_limits: [37, -1, 25] },
-left_ankle_roll_joint(left_ankle_roll_joint_origin): { joint: hingeX, limits: [-0.2618, 0.2618, 37, -1, 50], shape: mesh, color: [0.2, 0.2, 0.2, 1], mesh: <meshes/left_ankle_roll_link.ply>, ctrl_limits: [37, -1, 50] },
-right_ankle_roll_joint(right_ankle_roll_joint_origin): { joint: hingeX, limits: [-0.2618, 0.2618, 37, -1, 50], shape: mesh, color: [0.2, 0.2, 0.2, 1], mesh: <meshes/right_ankle_roll_link.ply>, ctrl_limits: [37, -1, 50] },
-left_shoulder_yaw_link(left_shoulder_yaw_joint): { mass: 0.734, inertia: [0.0009988, 7.9e-06, 0.0001412, 0.0010605, -2.86e-05, 0.0004354] },
+left_shoulder_yaw_joint(left_shoulder_yaw_joint_origin): { joint: hingeZ, limits: [-2.618, 2.618], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_shoulder_yaw_link.h5>, mass: 0.330191, inertia: [0.00075724, 1.01914e-05, 0.000118153, 0.000781633, -6.07748e-05, 0.000303097] },
+right_shoulder_yaw_joint(right_shoulder_yaw_joint_origin): { joint: hingeZ, limits: [-2.618, 2.618], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_shoulder_yaw_link.h5>, mass: 0.330189, inertia: [0.000757238, -1.01934e-05, 0.000118161, 0.000781632, 6.07703e-05, 0.000303094] },
+left_ankle_roll_joint(left_ankle_roll_joint_origin): { joint: hingeX, limits: [-0.2618, 0.2618], shape: mesh, color: [0.2, 0.2, 0.2, 1], mesh: <meshes/left_ankle_roll_link.h5>, mass: 0.456098, inertia: [0.000287801, 6.50248e-08, 8.30648e-05, 0.00151895, -1.18792e-07, 0.0016069] },
+right_ankle_roll_joint(right_ankle_roll_joint_origin): { joint: hingeX, limits: [-0.2618, 0.2618], shape: mesh, color: [0.2, 0.2, 0.2, 1], mesh: <meshes/right_ankle_roll_link.h5>, mass: 0.45609, inertia: [0.0002878, -9.56202e-08, 8.30614e-05, 0.00151893, 1.13682e-07, 0.00160687] },
 left_elbow_joint_origin(left_shoulder_yaw_joint): { Q: [0.015783, 0, -0.080518] },
-right_shoulder_yaw_link(right_shoulder_yaw_joint): { mass: 0.734, inertia: [0.0009988, -7.9e-06, 0.0001412, 0.0010605, 2.86e-05, 0.0004354] },
 right_elbow_joint_origin(right_shoulder_yaw_joint): { Q: [0.015783, 0, -0.080518] },
-left_ankle_roll_link(left_ankle_roll_joint): { mass: 0.608, inertia: [0.0002231, 2e-07, 8.91e-05, 0.0016161, -1e-07, 0.0016667] },
-right_ankle_roll_link(right_ankle_roll_joint): { mass: 0.608, inertia: [0.0002231, -2e-07, 8.91e-05, 0.0016161, 1e-07, 0.0016667] },
-left_elbow_joint(left_elbow_joint_origin): { joint: hingeY, limits: [-1.0472, 2.0944, 37, -1, 25], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_elbow_link.ply>, ctrl_limits: [37, -1, 25] },
-right_elbow_joint(right_elbow_joint_origin): { joint: hingeY, limits: [-1.0472, 2.0944, 37, -1, 25], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_elbow_link.ply>, ctrl_limits: [37, -1, 25] },
-left_elbow_link(left_elbow_joint): { mass: 0.6, inertia: [0.0002891, 6.53e-05, 1.72e-05, 0.0004152, -5.6e-06, 0.0004197] },
+left_elbow_joint(left_elbow_joint_origin): { joint: hingeY, limits: [-1.0472, 2.0944], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_elbow_link.h5>, mass: 0.133212, inertia: [0.000105606, 4.41256e-05, 1.51768e-05, 0.000221879, -4.66813e-06, 0.000221601] },
+right_elbow_joint(right_elbow_joint_origin): { joint: hingeY, limits: [-1.0472, 2.0944], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_elbow_link.h5>, mass: 0.133212, inertia: [0.000105601, -4.41257e-05, 1.5176e-05, 0.000221877, 4.66758e-06, 0.000221596] },
 left_wrist_roll_joint_origin(left_elbow_joint): { Q: [0.1, 0.00188791, -0.01] },
-right_elbow_link(right_elbow_joint): { mass: 0.6, inertia: [0.0002891, -6.53e-05, 1.72e-05, 0.0004152, 5.6e-06, 0.0004197] },
 right_wrist_roll_joint_origin(right_elbow_joint): { Q: [0.1, -0.00188791, -0.01] },
-left_wrist_roll_joint(left_wrist_roll_joint_origin): { joint: hingeX, limits: [-1.97222, 1.97222, 37, -1, 25], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_wrist_roll_link.ply>, ctrl_limits: [37, -1, 25] },
-right_wrist_roll_joint(right_wrist_roll_joint_origin): { joint: hingeX, limits: [-1.97222, 1.97222, 37, -1, 25], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_wrist_roll_link.ply>, ctrl_limits: [37, -1, 25] },
-left_wrist_roll_link(left_wrist_roll_joint): { mass: 0.085445, inertia: [4.82154e-05, -4.24511e-06, 5.10599e-09, 3.7229e-05, -1.23525e-09, 5.48211e-05] },
+left_wrist_roll_joint(left_wrist_roll_joint_origin): { joint: hingeX, limits: [-1.97222, 1.97222], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_wrist_roll_link.h5>, mass: 0.0819051, inertia: [5.76855e-05, -6.70778e-07, 1.09252e-09, 4.18275e-05, -1.21848e-09, 6.33789e-05] },
+right_wrist_roll_joint(right_wrist_roll_joint_origin): { joint: hingeX, limits: [-1.97222, 1.97222], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_wrist_roll_link.h5>, mass: 0.0819045, inertia: [5.76834e-05, 6.70746e-07, 8.73074e-10, 4.18274e-05, 8.19224e-10, 6.33769e-05] },
 left_wrist_pitch_joint_origin(left_wrist_roll_joint): { Q: [0.038, 0, 0] },
-right_wrist_roll_link(right_wrist_roll_joint): { mass: 0.085445, inertia: [4.82154e-05, 4.24511e-06, 5.10599e-09, 3.7229e-05, 1.23525e-09, 5.48211e-05] },
 right_wrist_pitch_joint_origin(right_wrist_roll_joint): { Q: [0.038, 0, 0] },
-left_wrist_pitch_joint(left_wrist_pitch_joint_origin): { joint: hingeY, limits: [-1.61443, 1.61443, 22, -1, 5], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_wrist_pitch_link.ply>, ctrl_limits: [22, -1, 5] },
-right_wrist_pitch_joint(right_wrist_pitch_joint_origin): { joint: hingeY, limits: [-1.61443, 1.61443, 22, -1, 5], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_wrist_pitch_link.ply>, ctrl_limits: [22, -1, 5] },
-left_wrist_pitch_link(left_wrist_pitch_joint): { mass: 0.48405, inertia: [0.000165796, -1.23121e-05, 1.2317e-05, 0.000429541, 8.14177e-07, 0.000429537] },
+left_wrist_pitch_joint(left_wrist_pitch_joint_origin): { joint: hingeY, limits: [-1.61443, 1.61443], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_wrist_pitch_link.h5>, mass: 0.102165, inertia: [6.54114e-05, -1.39886e-06, 1.39875e-06, 0.000133698, 1.78434e-08, 0.000133699] },
+right_wrist_pitch_joint(right_wrist_pitch_joint_origin): { joint: hingeY, limits: [-1.61443, 1.61443], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_wrist_pitch_link.h5>, mass: 0.102164, inertia: [6.54077e-05, 1.39993e-06, 1.3996e-06, 0.000133692, -1.67215e-08, 0.000133692] },
 left_wrist_yaw_joint_origin(left_wrist_pitch_joint): { Q: [0.046, 0, 0] },
-right_wrist_pitch_link(right_wrist_pitch_joint): { mass: 0.48405, inertia: [0.000165796, 1.23121e-05, 1.2317e-05, 0.000429541, -8.14177e-07, 0.000429537] },
 right_wrist_yaw_joint_origin(right_wrist_pitch_joint): { Q: [0.046, 0, 0] },
-left_wrist_yaw_joint(left_wrist_yaw_joint_origin): { joint: hingeZ, limits: [-1.61443, 1.61443, 22, -1, 5], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_wrist_yaw_link.ply>, ctrl_limits: [22, -1, 5] },
-right_wrist_yaw_joint(right_wrist_yaw_joint_origin): { joint: hingeZ, limits: [-1.61443, 1.61443, 22, -1, 5], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_wrist_yaw_link.ply>, ctrl_limits: [22, -1, 5] },
-left_wrist_yaw_link(left_wrist_yaw_joint): { mass: 0.0845765, inertia: [4.92913e-05, -4.57355e-07, 4.45868e-06, 5.97334e-05, 4.32172e-07, 3.92808e-05] },
-left_rubber_hand(left_wrist_yaw_joint): { Q: [0.0415, 0.003, 0], mass: 0.17, inertia: [0.000100995, 3.61859e-05, -7.43015e-07, 0.000281359, 3.3019e-06, 0.000218948] },
-left_rubber_hand_0(left_wrist_yaw_joint): { Q: [0.0415, 0.003, 0], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_rubber_hand.ply>, visual: True },
-right_wrist_yaw_link(right_wrist_yaw_joint): { mass: 0.0845765, inertia: [4.92913e-05, 4.57355e-07, 4.45868e-06, 5.97334e-05, -4.32172e-07, 3.92808e-05] },
-right_rubber_hand(right_wrist_yaw_joint): { Q: [0.0415, -0.003, 0], mass: 0.17, inertia: [0.000100995, -3.61859e-05, -7.43015e-07, 0.000281359, -3.3019e-06, 0.000218948] },
-right_rubber_hand_0(right_wrist_yaw_joint): { Q: [0.0415, -0.003, 0], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_rubber_hand.ply>, visual: True }
+left_wrist_yaw_joint(left_wrist_yaw_joint_origin): { joint: hingeZ, limits: [-1.61443, 1.61443], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_wrist_yaw_link.h5>, mass: 0.235411, com: [0.0788504, -0.00205286, 0.0021586], inertia: [0.000185982, 6.57578e-05, -7.51085e-06, 0.000766002, 3.21636e-06, 0.000670575] },
+right_wrist_yaw_joint(right_wrist_yaw_joint_origin): { joint: hingeZ, limits: [-1.61443, 1.61443], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_wrist_yaw_link.h5>, mass: 0.235418, com: [0.0788478, 0.00205232, 0.00215915], inertia: [0.00018599, -6.5759e-05, -7.50542e-06, 0.000766053, -3.21646e-06, 0.000670623] },
+left_rubber_hand_0(left_wrist_yaw_joint): { Q: [0.0415, 0.003, 0], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/left_rubber_hand.h5> },
+right_rubber_hand_0(right_wrist_yaw_joint): { Q: [0.0415, -0.003, 0], shape: mesh, color: [0.7, 0.7, 0.7, 1], mesh: <meshes/right_rubber_hand.h5> }