robotic 0.2.9__cp312-cp312-manylinux2014_x86_64.whl → 0.2.9.dev2__cp312-cp312-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
         """
@@ -363,6 +363,14 @@ class Config:
         """
     def get_viewer(self, window_title: str = None, offscreen: bool = False) -> ...:
         ...
+    def processInertias(self, recomputeInertias: bool = True, transformToDiagInertia: bool = False) -> None:
+        """
+        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, pruneRigidJoints: bool = False, reconnectToLinks: bool = True, pruneNonContactShapes: bool = False, pruneTransparent: bool = False) -> None:
+        """
+        structurally simplify the Configuration (deleting frames, relinking to minimal tree)
+        """
     def report(self) -> str:
         """
         return a string with basic info (#frames, etc)
@@ -391,10 +399,6 @@ class Config:
         ...
     def set_viewer(self, arg0: ...) -> None:
         ...
-    def simplify(self, pruneNamed: bool, pruneNonContactNonMarker: bool, pruneTransparent: bool) -> None:
-        """
-        structurally simplify the Configuration (deleting frames, relinking to minimal tree)
-        """
     def view(self, pause: bool = False, message: str = None) -> int:
         """
         open a view window for the configuration
@@ -449,7 +453,7 @@ class Config:
         """
         write the full configuration in a ply mesh file
         """
-    def writeMeshes(self, pathPrefix: ...) -> None:
+    def writeMeshes(self, pathPrefix: ..., copyTextures: bool = True, enumerateTextures: bool = False) -> None:
         """
         write all object meshes in a directory
         """
@@ -724,7 +728,7 @@ class Frame:
         """
     def asDict(self) -> dict:
         ...
-    def computeCompoundInertia(self, clearChildInertias: bool = True) -> Frame:
+    def computeCompoundInertia(self) -> Frame:
         ...
     def convertDecomposedShapeToChildFrames(self) -> Frame:
         ...
@@ -784,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/graph.h

@@ -133,8 +133,8 @@ struct Graph : NodeL {
   //-- basic node retrieval -- users should use the higher-level wrappers below
   Node* findNode(const char* key, bool recurseUp=false, bool recurseDown=false) const;   ///< returns nullptr if not found
   NodeL findNodes(const char* key, bool recurseUp=false, bool recurseDown=false) const;
-  Node* findNodeOfType(const std::type_info& type, const char* key, bool recurseUp=false, bool recurseDown=false) const;
-  NodeL findNodesOfType(const std::type_info& type, const char* key, bool recurseUp=false, bool recurseDown=false) const;
+  Node* findNodeOfType(const std::type_info& type, const char* key=0, bool recurseUp=false, bool recurseDown=false) const;
+  NodeL findNodesOfType(const std::type_info& type, const char* key=0, bool recurseUp=false, bool recurseDown=false) const;
   NodeL findGraphNodesWithTag(const char* tag) const;
 
   //

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&);
@@ -91,7 +92,7 @@ struct Matrix {
 
   double diffZero() const;
 
-  bool isDiagonal() const { return !m01 && !m02 && !m10 && !m12 && !m20 && !m21; }
+  bool isDiagonal() const;
   void deleteOffDiagonal() { m01=m02=m10=m12=m20=m21=0.; }
 
   void write(std::ostream&) const;

robotic/include/rai/Geo/mesh.h

@@ -17,7 +17,7 @@ namespace rai { struct Mesh; }
 typedef rai::Array<rai::Mesh> MeshA;
 typedef rai::Array<rai::Mesh*> MeshL;
 struct ANN;
-struct SharedTextureImage{ byteA img; uint glBufferID=UINT32_MAX; };
+struct SharedTextureImage{ byteA img; charA file; uint glBufferID=UINT32_MAX; };
 
 namespace rai {
 
@@ -45,8 +45,6 @@ struct Mesh {
   rai::Transformation glX; ///< transform (only used for drawing! Otherwise use applyOnPoints)  (optional)
 
   int version = 0;
-  long parsing_pos_start;
-  long parsing_pos_end;
 
   uint _support_vertex=0;
   bool isArrayFormatted=false;

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

@@ -146,8 +146,9 @@ struct Frame : NonCopyable {
 
   //composed object manipulation
   Transformation transformToDiagInertia(bool transformOwnMesh=false);
-  Frame& computeCompoundInertia(bool clearChildInertias=true);
+  Frame& computeCompoundInertia();
   Frame& convertDecomposedShapeToChildFrames();
+  bool standardizeInertias(bool recomputeInertias=true, bool _transformToDiagInertia=false);
 
   //I/O
   void read(const Graph& ats);
@@ -175,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
 
@@ -306,7 +307,6 @@ struct Inertia : NonCopyable {
   Frame& frame;
   double mass=0.;
   Matrix matrix=0;
-  Enum<BodyType> type;
   Vector com=0;             ///< its center of mass
 
   Inertia(Frame& f, rai::Inertia* copyInertia=nullptr);

robotic/include/rai/Kin/kin.h

@@ -181,8 +181,9 @@ 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 simplify(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.);
   void addTauJoint();
@@ -288,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/") 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/Kin/kin_physx.h

@@ -25,7 +25,7 @@ struct PhysX_Options {
 struct PhysXInterface {
   struct PhysXInterface_self* self=0;
 
-  PhysXInterface(const rai::Configuration& C, int verbose=1, const rai::PhysX_Options* _opt=0);
+  PhysXInterface(rai::Configuration& C, int verbose=1, const rai::PhysX_Options* _opt=0);
   ~PhysXInterface();
 
   void step(double tau=.01);
@@ -40,7 +40,6 @@ struct PhysXInterface {
   void addJoint(rai::Joint* j);
   void removeJoint(rai::Joint* j);
   void postAddObject(rai::Frame* f);
-  void setArticulatedBodiesKinematic(const rai::Configuration& C);
 
   void view(bool pause=false, const char* txt=nullptr);
 

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