robotic 0.2.8.dev4__cp311-cp311-manylinux2014_x86_64.whl → 0.3.4.dev5__cp311-cp311-manylinux2014_x86_64.whl

robotic/include/rai/Optim/utils.h

@@ -9,76 +9,23 @@
 #pragma once
 
 #include "NLP.h"
+#include "../Core/util.h"
 
-//===========================================================================
-//
-// lambda expression interfaces
-//
-
-// see function types defined in array.h
-//ScalarFunction
-//VectorFunction
-//fct
-
-struct Conv_ScalarProblem_NLP : NLP {
-  ScalarFunction f;
-  uint xDim;
-  arr bounds_lo, bounds_up;
-  Conv_ScalarProblem_NLP(const ScalarFunction& f, uint xDim): f(f), xDim(xDim) {}
-  void evaluate(arr& phi, arr& J, const arr& x) {
-    double y = f(J, NoArr, x);
-    phi = {y};
-    if(!!J) J.reshape(1, x.N);
-  }
-  void getFHessian(arr& H, const arr& x) {
-    f(NoArr, H, x);
-  }
-  void setBounds(double lo, double up) { bounds_lo.resize(xDim) = lo;  bounds_up.resize(xDim) = up; }
-};
-
-struct Conv_NLP_ScalarProblem : ScalarFunction {
+struct NLP_FiniteDifference : NLP {
   std::shared_ptr<NLP> P;
-
-  Conv_NLP_ScalarProblem(std::shared_ptr<NLP> _P) : P(_P) {
-    ScalarFunction::operator=([this](arr& g, arr& H, const arr& x) -> double {
-      return this->scalar(g, H, x);
-    });
-  }
-
-  double scalar(arr& g, arr& H, const arr& x);
+  double eps;
+  NLP_FiniteDifference(std::shared_ptr<NLP> _P, double eps=1e-6) : P(_P), eps(eps) { copySignature(*P); }
+  virtual void evaluate(arr& phi, arr& J, const arr& x0);
+  virtual void report(ostream& os, int verbose, const char *msg=0){ os <<"FiniteDifference version of: "; P->report(os, verbose, msg); }
 };
 
-struct Conv_NLP_SlackLeastSquares : NLP {
+struct NLP_SlackLeastSquares : NLP {
   std::shared_ptr<NLP> P;
+  NLP_SlackLeastSquares(std::shared_ptr<NLP> _P);
+  virtual void evaluate(arr& phi, arr& J, const arr& x);
+  virtual void report(ostream& os, int verbose, const char *msg=0){ os <<"SlackLeastSquares of: "; P->report(os, verbose, msg); }
+private:
   uintA pick;
-
-  Conv_NLP_SlackLeastSquares(std::shared_ptr<NLP> _P) : P(_P) {
-    dimension = P->dimension;
-    bounds = P->bounds;
-
-    //pick constraints
-    for(uint i=0; i<P->featureTypes.N; i++) {
-      ObjectiveType f = P->featureTypes(i);
-      if(f==OT_eq || f==OT_ineq) pick.append(i);
-    }
-    featureTypes.resize(pick.N) = OT_sos;
-  }
-
-  virtual void evaluate(arr& phi, arr& J, const arr& x) {
-    arr Pphi, PJ;
-    P->evaluate(Pphi, PJ, x);
-    phi = Pphi.sub(pick);
-    J = PJ.sub(pick);
-    for(uint i=0; i<pick.N; i++) {
-      if(P->featureTypes(pick(i))==OT_ineq) {
-        if(phi(i)<0.) { phi(i)=0.; J[i]=0.; } //ReLu for g
-      } else if(P->featureTypes(pick(i))==OT_eq) {
-        if(phi(i)<0.) { phi(i)*=-1.; J[i]*=-1.; } //make positive
-      } else {
-        NIY;
-      }
-    }
-  }
 };
 
 struct NLP_LinTransformed : NLP {
@@ -89,7 +36,7 @@ struct NLP_LinTransformed : NLP {
 
   virtual arr getInitializationSample();
   virtual void evaluate(arr& phi, arr& J, const arr& x);
-  virtual void report(ostream& os, int verbose, const char* msg=0){ P->report(os, verbose, msg); }
+  virtual void report(ostream& os, int verbose, const char *msg=0){ os <<"LinTransformed of: "; P->report(os, verbose, msg); }
 };
 
 //===========================================================================
@@ -97,30 +44,14 @@ struct NLP_LinTransformed : NLP {
 // checks, evaluation
 //
 
-bool checkDirectionalGradient(const ScalarFunction& f, const arr& x, const arr& delta, double tolerance);
-bool checkDirectionalJacobian(const VectorFunction& f, const arr& x, const arr& delta, double tolerance);
-
-//===========================================================================
-//
-// accumulative constraints
-//
-
-inline void accumulateInequalities(arr& y, arr& J, const arr& yAll, const arr& JAll) {
-  y.resize(1).setZero();
-  if(!!J) J.resize(1, JAll.d1).setZero();
-
-  for(uint i=0; i<yAll.N; i++) {
-    if(yAll.elem(i)>0.) {
-      y.scalar() += yAll.elem(i);
-      if(!!J && !!JAll) J[0] += JAll[i];
-    }
-  }
-}
+bool checkDirectionalGradient(ScalarFunction f, const arr& x, const arr& delta, double tolerance);
+bool checkDirectionalJacobian(VectorFunction f, const arr& x, const arr& delta, double tolerance);
 
 //===========================================================================
 //
 // helpers
 //
 
-void displayFunction(const ScalarFunction& f, bool wait=false, double lo=-1.2, double hi=1.2);
+void accumulateInequalities(arr& y, arr& J, const arr& yAll, const arr& JAll);
+void displayFunction(ScalarFunction f, bool wait=false, double lo=-1.2, double hi=1.2);
 

robotic/include/rai/PathAlgos/ConfigurationProblem.h

@@ -31,7 +31,7 @@ struct QueryResult {
 stdOutPipe(QueryResult)
 
 struct ConfigurationProblem {
-  rai::Configuration C;
+  shared_ptr<rai::Configuration> C;
   arr limits;
   uintA sphericalCoordinates;
 
@@ -43,8 +43,9 @@ struct ConfigurationProblem {
   //user info
   int verbose=0;
   uint evals=0;
+  double queryTime=0.;
 
-  ConfigurationProblem(const rai::Configuration& _C, bool _useBroadCollisions=true, double _collisionTolerance=1e-3, int _verbose=0);
+  ConfigurationProblem(shared_ptr<rai::Configuration> _C, bool _useBroadCollisions=true, double _collisionTolerance=1e-3, int _verbose=0);
 
   void setExplicitCollisionPairs(const StringA& _collisionPairs);
 

robotic/include/rai/PathAlgos/RRT_PathFinder.h

@@ -78,12 +78,12 @@ struct RRT_PathFinder : NonCopyable {
   arr path;
 
   //setup
-  void setProblem(const Configuration& C);
+  void setProblem(shared_ptr<Configuration> C);
   void setStartGoal(const arr& _starts, const arr& _goals);
   void setExplicitCollisionPairs(const StringA& collisionPairs);
 
   //solve
-  shared_ptr<SolverReturn> solve();
+  shared_ptr<SolverReturn> solve(int verbose=1);
 
   //output
   void view(bool pause, const char* txt=0, bool play=false);

robotic/include/rai/Perception/pcl.h

@@ -24,19 +24,19 @@
 typedef pcl::PointCloud<pcl::PointXYZ> Pcl;
 typedef pcl::PointCloud<pcl::PointXYZRGB> PclC;
 
-Pcl::Ptr conv_ArrCloud_PclCloud(const arr& pts);
-void conv_ArrCloud_PclCloud(Pcl& cloud, const arr& pts);
-void conv_PclCloud_ArrCloud(arr& pts, const Pcl& cloud);
+Pcl::Ptr conv_ArrCloud2PclCloud(const arr& pts);
+void conv_ArrCloud2PclCloud(Pcl& cloud, const arr& pts);
+void conv_PclCloud2ArrCloud(arr& pts, const Pcl& cloud);
 
-PclC::Ptr conv_ArrCloud_PclCloud(const arr& pts, const byteA& rgb);
-void conv_ArrCloud_PclCloud(PclC& cloud, const arr& pts, const byteA& rgb);
-void conv_PclCloud_ArrCloud(arr& pts, byteA& rgb, const PclC& cloud);
+PclC::Ptr conv_ArrCloud2PclCloud(const arr& pts, const byteA& rgb);
+void conv_ArrCloud2PclCloud(PclC& cloud, const arr& pts, const byteA& rgb);
+void conv_PclCloud2ArrCloud(arr& pts, byteA& rgb, const PclC& cloud);
 
-void conv_ArrCloud_PclCloud(PclC& cloud, const arr& pts, const arr& rgb);
-void conv_PclCloud_ArrCloud(arr& pts, arr& rgb, const PclC& cloud);
+void conv_ArrCloud2PclCloud(PclC& cloud, const arr& pts, const arr& rgb);
+void conv_PclCloud2ArrCloud(arr& pts, arr& rgb, const PclC& cloud);
 
-inline arr PclPoints(const PclC& cloud) { arr pts; conv_PclCloud_ArrCloud(pts, NoByteA, cloud); return pts; }
+inline arr PclPoints(const PclC& cloud) { arr pts; conv_PclCloud2ArrCloud(pts, NoByteA, cloud); return pts; }
 
-arr conv_PclNormals_Arr(const pcl::PointCloud<pcl::Normal>::Ptr& normals);
+arr conv_PclNormals2Arr(const pcl::PointCloud<pcl::Normal>::Ptr& normals);
 
 #endif

robotic/include/rai/Perception/surfels.h

@@ -42,7 +42,7 @@ struct SurfelStatistics {
 
 struct Surfels {
   OpenGL gl;
-  Mutex mx;
+  rai::Mutex mx;
   arr pos, norm, col, rad;
   rai::Array<SurfelStatistics> D;
   uint32A surfelIdx, rndPerm;

robotic/include/rai/Search/TreeSearchNode.h

@@ -42,7 +42,7 @@ struct TreeSearchNode {
 
   //I/O
   virtual void write(std::ostream& os) const { os <<name; }
-  virtual void report(std::ostream& os, int verbose) const { std::cerr <<"NOT OVERLOADED!" <<std::endl; }
+  virtual void report(std::ostream& os, int verbose) const { std::cout <<"NOT OVERLOADED!" <<std::endl; }
   virtual void data(Graph& g) const {}
 };
 inline std::ostream& operator<<(std::ostream& os, const TreeSearchNode& D) { D.write(os); return os; }

robotic/include/rai/ry/py-algo.h

@@ -0,0 +1,17 @@
+/*  ------------------------------------------------------------------
+    Copyright (c) 2011-2024 Marc Toussaint
+    email: toussaint@tu-berlin.de
+
+    This code is distributed under the MIT License.
+    Please see <root-path>/LICENSE for details.
+    --------------------------------------------------------------  */
+
+#pragma once
+
+#ifdef RAI_PYBIND
+
+#include <pybind11/pybind11.h>
+
+void init_algo(pybind11::module& m);
+
+#endif

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);
@@ -45,6 +46,7 @@ template<class T> rai::Array<T> vec2Array(const std::vector<T>& x) {
 }
 
 template<class T> pybind11::array_t<T> Array2numpy(const rai::Array<T>& x) {
+  if(!x.N) return pybind11::array_t<T>();
   return pybind11::array_t<T>(vecdim(x), x.p);
 }
 
@@ -308,7 +310,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/manipulation.py

@@ -1,6 +1,7 @@
 import robotic as ry
 import numpy as np
 import time
+import pprint
 
 class KOMO_ManipulationHelper():
 
@@ -361,18 +362,15 @@ class KOMO_ManipulationHelper():
             if not self.ret.feasible:
                 print(f'  -- infeasible:{self.info}\n     {self.ret}')
                 if verbose>1:
-                    print(self.komo.report(False, True))
+                    objs = self.komo.report()
+                    #pprint.pp(objs)
+                    for k, v in objs.items():
+                        print(f'objective {k}: {v}')
                     self.komo.view(True, f'failed: {self.info}\n{self.ret}')
-                if verbose>2:
-                    while(self.komo.view_play(True, 1.)):
-                        pass
             else:
                 print(f'  -- feasible:{self.info}\n     {self.ret}')
                 if verbose>2:
                     self.komo.view(True, f'success: {self.info}\n{self.ret}')
-                if verbose>3:
-                    while(self.komo.view_play(True, 1.)):
-                        pass
 
         return self.ret
 

robotic/mujoco-import.py

@@ -0,0 +1,8 @@
+import robotic as ry
+from robotic.src.mujoco_io import *
+
+file = '../../rai-robotModels/kitchens/models/MEDITERRANEAN_ONE_WALL_SMALL.xml'
+
+print('=====================', file)
+M = MujocoLoader(file, visualsOnly=True)
+M.C.view(True)

robotic/rai-robotModels/g1/g1.g

@@ -1,5 +1,14 @@
 ## create standard base frame before including
 
-g1_base: { X:[0, 0, .79], multibody: true, multibody_fixedBase: false }
-Include: <g1_clean.g>
+g1_base: { X:[0, 0, .79], multibody: true, multibody_fixedBase: false, multibody_gravity: true }
+Include: <g1_29dof_conv.yml>
 Edit pelvis(g1_base): {}
+
+Edit left_hip_pitch_joint { q: -.25 }
+Edit right_hip_pitch_joint { q: -.25 }
+        
+Edit left_knee_joint { q: .5 }
+Edit right_knee_joint { q: .5 }
+
+Edit left_ankle_pitch_joint { q: -.25, motorLambda: .02, motorMass: 100 }
+Edit right_ankle_pitch_joint { q: -.25, motorLambda: .02, motorMass: 100 }

robotic/rai-robotModels/g1/g1_29dof_conv.yml

@@ -0,0 +1,64 @@
+pelvis: {shape: mesh, color: [0.2, 0.2, 0.2, 1.0], mesh: <meshes/pelvis.h5>, mass: 3.814, inertia: [0.010549099999999999, 0.0, 2.1e-06, 0.009309, 0.0, 0.0079185]}
+left_hip_pitch_joint_origin (pelvis): {pose: [0.0, 0.064452, -0.1027]}
+right_hip_pitch_joint_origin (pelvis): {pose: [0.0, -0.064452, -0.1027]}
+waist_yaw_joint (pelvis): {joint: hingeZ, limits: [-2.618, 2.618], shape: mesh, color: [0.7, 0.7, 0.7, 1.0], mesh: <meshes/waist_yaw_link.h5>, mass: 0.244, inertia: [9.9587e-05, -1.833e-06, -1.2617e-05, 0.00012411, -1.18e-07, 0.00015586]}
+pelvis_contour_link_0 (pelvis): {shape: mesh, color: [0.7, 0.7, 0.7, 1.0], 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.0], mesh: <meshes/left_hip_pitch_link.h5>, mass: 1.35, inertia: [0.001811, 3.68e-05, -3.44e-05, 0.0014193, 0.000171, 0.0012812]}
+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.0], mesh: <meshes/right_hip_pitch_link.h5>, mass: 1.35, inertia: [0.001811, -3.68e-05, -3.44e-05, 0.0014193, -0.000171, 0.0012812]}
+waist_roll_joint_origin (waist_yaw_joint): {pose: [-0.0039635, 0.0, 0.035]}
+left_hip_roll_joint_origin (left_hip_pitch_joint): {pose: [0.0, 0.052, -0.030465, 0.9961786849744957, 0.0, -0.08733858026373247, 0.0]}
+right_hip_roll_joint_origin (right_hip_pitch_joint): {pose: [0.0, -0.052, -0.030465, 0.9961786849744957, 0.0, -0.08733858026373247, 0.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.0], mesh: <meshes/waist_roll_link.h5>, mass: 0.047, inertia: [7.515e-06, 0.0, 0.0, 6.398e-06, 9.9e-08, 3.988e-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.0], mesh: <meshes/left_hip_roll_link.h5>, mass: 1.52, inertia: [0.0023773, -3.8e-06, -0.0003908, 0.0024123, 1.84e-05, 0.0016595]}
+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.0], mesh: <meshes/right_hip_roll_link.h5>, mass: 1.52, inertia: [0.0023773, 3.8e-06, -0.0003908, 0.0024123, -1.84e-05, 0.0016595]}
+waist_pitch_joint_origin (waist_roll_joint): {pose: [0.0, 0.0, 0.019]}
+left_hip_yaw_joint_origin (left_hip_roll_joint): {pose: [0.025001, 0.0, -0.12412]}
+right_hip_yaw_joint_origin (right_hip_roll_joint): {pose: [0.025001, 0.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.0], mesh: <meshes/torso_link.h5>, mass: 9.599999999999998, com: [0.00042855343750000007, 0.0, -0.005838750000000002], inertia: [0.072459751385, -8.851300000000001e-05, -0.0016085661752787497, 0.060434677554607075, 8.3173e-05, 0.032630779169607084]}
+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.0], mesh: <meshes/left_hip_yaw_link.h5>, mass: 1.702, inertia: [0.0057774, -0.0005411, -0.0023948, 0.0076124, -0.0007072, 0.003149]}
+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.0], mesh: <meshes/right_hip_yaw_link.h5>, mass: 1.702, inertia: [0.0057774, 0.0005411, -0.0023948, 0.0076124, 0.0007072, 0.003149]}
+left_shoulder_pitch_joint_origin (waist_pitch_joint): {pose: [0.0039563, 0.10022, 0.23778, 0.9902640751049538, 0.13920147853022313, 4.054181625451359e-05, -9.103785623530919e-05]}
+right_shoulder_pitch_joint_origin (waist_pitch_joint): {pose: [0.0039563, -0.10021, 0.23778, 0.9902640751049538, -0.13920147853022313, 4.054181625451359e-05, 9.103785623530919e-05]}
+logo_link_0 (waist_pitch_joint): {pose: [0.0039635, 0.0, -0.054], shape: mesh, color: [0.2, 0.2, 0.2, 1.0], mesh: <meshes/logo_link.h5>}
+head_link_0 (waist_pitch_joint): {pose: [0.0039635, 0.0, -0.054], shape: mesh, color: [0.2, 0.2, 0.2, 1.0], mesh: <meshes/head_link.h5>}
+waist_support_link_0 (waist_pitch_joint): {pose: [0.0039635, 0.0, -0.054], shape: mesh, color: [0.7, 0.7, 0.7, 1.0], mesh: <meshes/waist_support_link.h5>}
+left_knee_joint_origin (left_hip_yaw_joint): {pose: [-0.078273, 0.0021489, -0.17734, 0.9961786849744957, 0.0, 0.08733858026373247, 0.0]}
+right_knee_joint_origin (right_hip_yaw_joint): {pose: [-0.078273, -0.0021489, -0.17734, 0.9961786849744957, 0.0, 0.08733858026373247, 0.0]}
+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.0], mesh: <meshes/left_shoulder_pitch_link.h5>, mass: 0.718, inertia: [0.0004291, -9.2e-06, 6.4e-06, 0.000453, 2.26e-05, 0.000423]}
+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.0], mesh: <meshes/right_shoulder_pitch_link.h5>, mass: 0.718, inertia: [0.0004291, 9.2e-06, 6.4e-06, 0.000453, -2.26e-05, 0.000423]}
+left_knee_joint (left_knee_joint_origin): {joint: hingeY, limits: [-0.087267, 2.8798], shape: mesh, color: [0.7, 0.7, 0.7, 1.0], mesh: <meshes/left_knee_link.h5>, mass: 1.932, inertia: [0.011329, 4.82e-05, -4.49e-05, 0.011277, -0.0007146, 0.0015168]}
+right_knee_joint (right_knee_joint_origin): {joint: hingeY, limits: [-0.087267, 2.8798], shape: mesh, color: [0.7, 0.7, 0.7, 1.0], mesh: <meshes/right_knee_link.h5>, mass: 1.932, inertia: [0.011329, -4.82e-05, 4.49e-05, 0.011277, 0.0007146, 0.0015168]}
+left_shoulder_roll_joint_origin (left_shoulder_pitch_joint): {pose: [0.0, 0.038, -0.013831, 0.9902682552548409, -0.1391717738427347, 0.0, 0.0]}
+right_shoulder_roll_joint_origin (right_shoulder_pitch_joint): {pose: [0.0, -0.038, -0.013831, 0.9902682552548409, 0.1391717738427347, 0.0, 0.0]}
+left_ankle_pitch_joint_origin (left_knee_joint): {pose: [0.0, -9.4445e-05, -0.30001]}
+right_ankle_pitch_joint_origin (right_knee_joint): {pose: [0.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.0], mesh: <meshes/left_shoulder_roll_link.h5>, mass: 0.643, inertia: [0.0006177, -1.0e-06, 8.7e-06, 0.0006912, -5.3e-06, 0.0003894]}
+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.0], mesh: <meshes/right_shoulder_roll_link.h5>, mass: 0.643, inertia: [0.0006177, 1.0e-06, 8.7e-06, 0.0006912, 5.3e-06, 0.0003894]}
+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.0], mesh: <meshes/left_ankle_pitch_link.h5>, mass: 0.074, inertia: [8.4e-06, 0.0, -2.9e-06, 1.89e-05, 0.0, 1.26e-05]}
+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.0], mesh: <meshes/right_ankle_pitch_link.h5>, mass: 0.074, inertia: [8.4e-06, 0.0, -2.9e-06, 1.89e-05, 0.0, 1.26e-05]}
+left_shoulder_yaw_joint_origin (left_shoulder_roll_joint): {pose: [0.0, 0.00624, -0.1032]}
+right_shoulder_yaw_joint_origin (right_shoulder_roll_joint): {pose: [0.0, -0.00624, -0.1032]}
+left_ankle_roll_joint_origin (left_ankle_pitch_joint): {pose: [0.0, 0.0, -0.017558]}
+right_ankle_roll_joint_origin (right_ankle_pitch_joint): {pose: [0.0, 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.0], mesh: <meshes/left_shoulder_yaw_link.h5>, mass: 0.734, inertia: [0.0009988, 7.9e-06, 0.0001412, 0.0010605, -2.86e-05, 0.0004354]}
+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.0], mesh: <meshes/right_shoulder_yaw_link.h5>, mass: 0.734, inertia: [0.0009988, -7.9e-06, 0.0001412, 0.0010605, 2.86e-05, 0.0004354]}
+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.0], mesh: <meshes/left_ankle_roll_link.h5>, mass: 0.608, inertia: [0.0002231, 2.0e-07, 8.91e-05, 0.0016161, -1.0e-07, 0.0016667]}
+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.0], mesh: <meshes/right_ankle_roll_link.h5>, mass: 0.608, inertia: [0.0002231, -2.0e-07, 8.91e-05, 0.0016161, 1.0e-07, 0.0016667]}
+left_elbow_joint_origin (left_shoulder_yaw_joint): {pose: [0.015783, 0.0, -0.080518]}
+right_elbow_joint_origin (right_shoulder_yaw_joint): {pose: [0.015783, 0.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.0], mesh: <meshes/left_elbow_link.h5>, mass: 0.6, inertia: [0.0002891, 6.53e-05, 1.72e-05, 0.0004152, -5.6e-06, 0.0004197]}
+right_elbow_joint (right_elbow_joint_origin): {joint: hingeY, limits: [-1.0472, 2.0944], shape: mesh, color: [0.7, 0.7, 0.7, 1.0], mesh: <meshes/right_elbow_link.h5>, mass: 0.6, inertia: [0.0002891, -6.53e-05, 1.72e-05, 0.0004152, 5.6e-06, 0.0004197]}
+left_wrist_roll_joint_origin (left_elbow_joint): {pose: [0.1, 0.00188791, -0.01]}
+right_wrist_roll_joint_origin (right_elbow_joint): {pose: [0.1, -0.00188791, -0.01]}
+left_wrist_roll_joint (left_wrist_roll_joint_origin): {joint: hingeX, limits: [-1.972222054, 1.972222054], shape: mesh, color: [0.7, 0.7, 0.7, 1.0], mesh: <meshes/left_wrist_roll_link.h5>, mass: 0.08544498, inertia: [4.821544023e-05, -4.24511021e-06, 5.10599e-09, 3.722899093e-05, -1.23525e-09, 5.482106541e-05]}
+right_wrist_roll_joint (right_wrist_roll_joint_origin): {joint: hingeX, limits: [-1.972222054, 1.972222054], shape: mesh, color: [0.7, 0.7, 0.7, 1.0], mesh: <meshes/right_wrist_roll_link.h5>, mass: 0.08544498, inertia: [4.821544023e-05, 4.24511021e-06, 5.10599e-09, 3.722899093e-05, 1.23525e-09, 5.482106541e-05]}
+left_wrist_pitch_joint_origin (left_wrist_roll_joint): {pose: [0.038, 0.0, 0.0]}
+right_wrist_pitch_joint_origin (right_wrist_roll_joint): {pose: [0.038, 0.0, 0.0]}
+left_wrist_pitch_joint (left_wrist_pitch_joint_origin): {joint: hingeY, limits: [-1.614429558, 1.614429558], shape: mesh, color: [0.7, 0.7, 0.7, 1.0], mesh: <meshes/left_wrist_pitch_link.h5>, mass: 0.48404956, inertia: [0.00016579646273, -1.231206746e-05, 1.231699194e-05, 0.0004295405741, 8.1417712e-07, 0.00042953697654]}
+right_wrist_pitch_joint (right_wrist_pitch_joint_origin): {joint: hingeY, limits: [-1.614429558, 1.614429558], shape: mesh, color: [0.7, 0.7, 0.7, 1.0], mesh: <meshes/right_wrist_pitch_link.h5>, mass: 0.48404956, inertia: [0.00016579646273, 1.231206746e-05, 1.231699194e-05, 0.0004295405741, -8.1417712e-07, 0.00042953697654]}
+left_wrist_yaw_joint_origin (left_wrist_pitch_joint): {pose: [0.046, 0.0, 0.0]}
+right_wrist_yaw_joint_origin (right_wrist_pitch_joint): {pose: [0.046, 0.0, 0.0]}
+left_wrist_yaw_joint (left_wrist_yaw_joint_origin): {joint: hingeZ, limits: [-1.614429558, 1.614429558], shape: mesh, color: [0.7, 0.7, 0.7, 1.0], mesh: <meshes/left_wrist_yaw_link.h5>, mass: 0.25457647, com: [0.027712694735691795, 0.0020033273302909765, -1.0899653184406096e-18], inertia: [0.00015079444368903163, 2.869702728036281e-05, 3.715660723580004e-06, 0.00043836153569590437, 3.73406941286e-06, 0.000356006284096386]}
+right_wrist_yaw_joint (right_wrist_yaw_joint_origin): {joint: hingeZ, limits: [-1.614429558, 1.614429558], shape: mesh, color: [0.7, 0.7, 0.7, 1.0], mesh: <meshes/right_wrist_yaw_link.h5>, mass: 0.25457647, com: [0.027712694735691795, -0.0020033273302909765, -1.0899653184406096e-18], inertia: [0.00015079444368903163, -2.869702728036281e-05, 3.715660723580004e-06, 0.00043836153569590437, -3.73406941286e-06, 0.000356006284096386]}
+left_rubber_hand_0 (left_wrist_yaw_joint): {pose: [0.0415, 0.003, 0.0], shape: mesh, color: [0.7, 0.7, 0.7, 1.0], mesh: <meshes/left_rubber_hand.h5>}
+right_rubber_hand_0 (right_wrist_yaw_joint): {pose: [0.0415, -0.003, 0.0], shape: mesh, color: [0.7, 0.7, 0.7, 1.0], mesh: <meshes/right_rubber_hand.h5>}

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> }