robotic 0.3.3__cp310-cp310-manylinux2014_x86_64.whl → 0.3.3.dev0__cp310-cp310-manylinux2014_x86_64.whl

robotic/include/rai/Optim/m_LeastSquaresZeroOrder.h

@@ -0,0 +1,38 @@
+#pragma once
+
+#include "NLP.h"
+#include "../Core/util.h"
+
+struct LeastSquaredZeroOrder{
+  shared_ptr<NLP> P;
+
+  //-- parameters of the inner problem (Lagrangian, unconstrained problem)
+  double sigma=1e-3;
+  double lambda=1e-3;
+  uint steps=0;
+
+  //-- buffers to avoid re-evaluating points
+  arr x;               ///< point where P was last evaluated
+  double phi2_x=-1.;
+  arr phi_x, J_x, H_x; ///< features at x
+  arr J;
+  arr data_X, data_Phi, data_Phi2;
+  str method="rank1";
+
+  LeastSquaredZeroOrder(shared_ptr<NLP> P, const arr& x_init={});
+
+  shared_ptr<SolverReturn> solve(){
+    while(!step()){}
+    shared_ptr<SolverReturn> ret = make_shared<SolverReturn>();
+    ret->x = x;
+    ret->sos = phi2_x;
+    ret->feasible=true;
+    return ret;
+  }
+
+  bool step();
+
+  void updateJ_rank1(arr& J, const arr& x, const arr& x_last, const arr& phi, const arr& phi_last);
+
+  void updateJ_linReg(arr& J, const arr& Xraw, const arr& Y, double lambdaReg=1e-3);
+};

robotic/include/rai/Optim/newton.h

@@ -11,16 +11,16 @@
 #include "options.h"
 #include "../Core/array.h"
 
-int optNewton(arr& x, const ScalarFunction& f, rai::OptOptions opt=DEFAULT_OPTIONS);
+int optNewton(arr& x, ScalarFunction& f, rai::OptOptions opt=DEFAULT_OPTIONS);
 
 struct OptNewton {
-  ScalarFunction f;
+  ScalarFunction& f;
   arr& x;
   rai::OptOptions opt;
 
   enum StopCriterion { stopNone=0, stopDeltaConverge, stopTinyFSteps, stopTinyXSteps, stopCritEvals, stopStepFailed, stopLineSearchSteps };
 
-  OptNewton(arr& _x, const ScalarFunction& _f, const rai::OptOptions& _opt);
+  OptNewton(arr& _x, ScalarFunction& _f, const rai::OptOptions& _opt);
   ~OptNewton();
   OptNewton& setBounds(const arr& _bounds);
   void reinit(const arr& _x);

robotic/include/rai/Optim/opt-ceres.h

@@ -27,12 +27,12 @@ struct CeresInterface {
 
 //===========================================================================
 
-struct Conv_NLP_CeresProblem {
+struct Conv_NLP2Ceres {
   shared_ptr<NLP_Factored> P;
 
   arr x_full, phi_full; //the full linear memory for all decision variables and all features
 
   shared_ptr<ceres::Problem> ceresProblem;
 
-  Conv_NLP_CeresProblem(const shared_ptr<NLP_Factored>& _P);
+  Conv_NLP2Ceres(const shared_ptr<NLP_Factored>& _P);
 };

robotic/include/rai/Optim/options.h

@@ -14,9 +14,10 @@ namespace rai {
 
 enum OptMethod { M_none=0,
 		 M_gradientDescent, M_rprop, M_LBFGS, M_newton,
-		 M_augmentedLag, M_squaredPenalty, M_logBarrier, M_singleSquaredPenalty,
+		 M_augmentedLag, M_logBarrier, M_slackGN_logBarrier, M_squaredPenalty, M_singleSquaredPenalty,
 		 M_slackGN,
-		 M_NLopt, M_Ipopt, M_Ceres };
+		 M_NLopt, M_Ipopt, M_slackGN_Ipopt, M_Ceres,
+		 M_LSZO};
 
 
 struct OptOptions {

robotic/include/rai/Optim/primalDual.h

@@ -25,7 +25,7 @@ struct PrimalDualProblem : ScalarFunction {
 
   PrimalDualProblem(const arr& x, const shared_ptr<NLP>& P, const rai::OptOptions& opt);
 
-  double primalDual(arr& r, arr& R, const arr& x); ///< CORE METHOD: the unconstrained scalar function F
+  double f(arr& r, arr& R, const arr& x); ///< CORE METHOD: the unconstrained scalar function F
 
   void updateMu();
 };

robotic/include/rai/Optim/testProblems_Opt.h

@@ -11,17 +11,11 @@
 #include "NLP.h"
 #include "../Core/util.h"
 
-extern ScalarFunction RosenbrockFunction();
-extern ScalarFunction RastriginFunction();
-extern ScalarFunction SquareFunction();
-extern ScalarFunction SumFunction();
-extern ScalarFunction HoleFunction();
-extern ScalarFunction ChoiceFunction();
-
 std::shared_ptr<NLP> getBenchmarkFromCfg();
 
 //===========================================================================
 
+#if 0
 struct ScalarUnconstrainedProgram : NLP {
   double forsythAlpha = -1.;
   shared_ptr<ScalarFunction> S;
@@ -47,9 +41,10 @@ struct ScalarUnconstrainedProgram : NLP {
   }
   virtual double f(arr& g, arr& H, const arr& x) {
     CHECK(S, "no scalar function given in the constructor");
-    return (*S)(g, H, x);
+    return S->f(g, H, x);
   }
 };
+#endif
 
 //===========================================================================
 
@@ -132,6 +127,20 @@ struct NLP_RastriginSOS : NLP {
 
 //===========================================================================
 
+struct NLP_Rastrigin : ScalarFunction {
+  NLP_Rastrigin(uint _dim) { dim=_dim; }
+  virtual double f(arr& g, arr& H, const arr& x);
+};
+
+//===========================================================================
+
+struct NLP_Rosenbrock : ScalarFunction {
+  NLP_Rosenbrock(uint _dim) { dim=_dim; }
+  virtual double f(arr& g, arr& H, const arr& x);
+};
+
+//===========================================================================
+
 /// $f(x) = x^T C x$ where C has eigen values ranging from 1 to 'condition'
 struct NLP_Squared : NLP {
   arr C; /// $A = C^T C $
@@ -150,6 +159,7 @@ struct NLP_Wedge : NLP {
     dimension=2;
     featureTypes = { OT_f };
     featureTypes.append(rai::consts(OT_ineq, 2));
+    bounds = arr{-2.,-2.,2.,2.}.reshape(2,2);
   }
 
   virtual void evaluate(arr& phi, arr& J, const arr& x) {
@@ -168,6 +178,7 @@ struct NLP_HalfCircle : NLP {
     dimension=2;
     featureTypes = { OT_f };
     featureTypes.append(rai::consts(OT_ineq, 2));
+    bounds = arr{-2.,-2.,2.,2.}.reshape(2,2);
   }
 
   virtual void evaluate(arr& phi, arr& J, const arr& x) {
@@ -205,6 +216,7 @@ struct ChoiceConstraintFunction : NLP {
   enum WhichConstraint { none=0, wedge2D=1, halfcircle2D, randomLinear, circleLine2D, boundConstrained, boundConstrainedIneq } which;
   uint n;
   arr randomG;
+  shared_ptr<ScalarFunction> f_uc;
   ChoiceConstraintFunction();
 
   void evaluate(arr& phi, arr& J, const arr& x);

robotic/include/rai/Optim/utils.h

@@ -9,76 +9,33 @@
 #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);
+struct Conv_ScalarFunction2NLP : NLP {
+  shared_ptr<ScalarFunction> f;
+  Conv_ScalarFunction2NLP(shared_ptr<ScalarFunction> f);
+  virtual void evaluate(arr& phi, arr& J, const arr& x);
+  virtual void getFHessian(arr& H, const arr& x);
+  virtual void report(ostream& os, int verbose, const char *msg=0){
+    os <<"ScalarFunction of type '" <<rai::niceTypeidName(typeid(*this)) <<"'";
+    if(msg) os <<' ' <<msg;
+    os <<" dimension:" <<f->dim;
   }
-  void setBounds(double lo, double up) { bounds_lo.resize(xDim) = lo;  bounds_up.resize(xDim) = up; }
 };
 
-struct Conv_NLP_ScalarProblem : ScalarFunction {
+struct Conv_NLP2ScalarProblem : ScalarFunction {
   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);
+  Conv_NLP2ScalarProblem(std::shared_ptr<NLP> _P) : P(_P) { dim = P->dimension; }
+  virtual double f(arr& g, arr& H, const arr& x);
 };
 
 struct Conv_NLP_SlackLeastSquares : NLP {
   std::shared_ptr<NLP> P;
+  Conv_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.pick(pick);
-    J = PJ.pick(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 +46,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); }
 };
 
 //===========================================================================
@@ -100,27 +57,11 @@ struct NLP_LinTransformed : NLP {
 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];
-    }
-  }
-}
-
 //===========================================================================
 //
 // 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/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/rai-robotModels/panda/panda.g

@@ -32,15 +32,15 @@ panda_coll7(panda_joint7): { shape: capsule, color: [1.,1.,1.,.1], size: [.1, .0
 
 ## zero position
 
-Edit panda_joint1: { q: 0.0 }
-Edit panda_joint2: { q: -.5 }
-Edit panda_joint3: { q: 0. }
-Edit panda_joint4: { q: -2. }
-Edit panda_joint5: { q: -0. }
-Edit panda_joint6: { q: 2., limits: [.5, 3.] }
-Edit panda_joint7: { q: -.5 }
-Edit panda_finger_joint1: { q: .04 }
-Edit panda_finger_joint2: { q: .04 }
+Edit panda_joint1: { q: 0.0, mj_actuator_kp: "870.", mj_joint_damping: "100." }
+Edit panda_joint2: { q: -.5, mj_actuator_kp: "870.", mj_joint_damping: "100." }
+Edit panda_joint3: { q: 0., mj_actuator_kp: "870.", mj_joint_damping: "100." }
+Edit panda_joint4: { q: -2., mj_actuator_kp: "870.", mj_joint_damping: "100." }
+Edit panda_joint5: { q: -0., mj_actuator_kp: "120.", mj_joint_damping: "10." }
+Edit panda_joint6: { q: 2., limits: [.5, 3.], mj_actuator_kp: "120.", mj_joint_damping: "10." }
+Edit panda_joint7: { q: -.5, mj_actuator_kp: "120.", mj_joint_damping: "10." }
+Edit panda_finger_joint1: { q: .04, mj_actuator_kp: "500.", mj_joint_damping: "100." }
+Edit panda_finger_joint2: { q: .04, mj_actuator_kp: "500.", mj_joint_damping: "100." }
 
 ## kill rigid hand joints
 

robotic/rai-robotModels/panda/panda_withoutCollisionModels.g

@@ -1,13 +1,5 @@
 Include: <panda_clean.g>
 
-Delete panda_link0_0:
-Delete panda_link1_0:
-Delete panda_link2_0:
-Delete panda_link3_0:
-Delete panda_link4_0:
-Delete panda_link5_0:
-Delete panda_link6_0:
-Delete panda_link7_0:
-Delete panda_hand_0:
-Delete panda_leftfinger_0:
-Delete panda_rightfinger_0:
+Delete: panda_hand_0
+#Delete: panda_leftfinger_0
+Delete: panda_rightfinger_0

robotic/rai-robotModels/pr2/pr2.g

@@ -1,5 +1,5 @@
 world: {}
-worldTranslationRotation (world): { joint: transXYPhi, Q: "d(90 0 0 1)", gains: [1, 1], limits: [-10, -10, -6, 10, 10, 6], ctrl_limits: [1, 1, 1], ctrl_H: 10, base }
+worldTranslationRotation (world): { joint: transXYPhi, Q: "d(90 0 0 1)", gains: [1, 1], limits: [-10, -10, -6, 10, 10, 6], ctrl_H: 10, base }
 
 Include: <pr2_conv.g>
 
@@ -28,10 +28,10 @@ Edit bl_caster_r_wheel_joint: { joint: none }
 Edit br_caster_l_wheel_joint: { joint: none }
 Edit br_caster_r_wheel_joint: { joint: none }
 
-#Delete l_gripper_r_parallel_link_1: {}:
-#Delete l_gripper_l_parallel_link_1: {}:
-#Delete r_gripper_r_parallel_link_1: {}:
-#Delete r_gripper_l_parallel_link_1: {}:
+#Delete: l_gripper_r_parallel_link_1
+#Delete: l_gripper_l_parallel_link_1
+#Delete: r_gripper_r_parallel_link_1
+#Delete: r_gripper_l_parallel_link_1
 
 Edit r_gripper_l_finger_joint: { q: .1 }
 Edit l_gripper_l_finger_joint: { q: .1 }

robotic/rai-robotModels/pr2/pr2_clean.g

@@ -1,11 +1,11 @@
 base_footprint: { mass: 1, inertia: [0.01, 0.01, 0.01] }
 
-base_footprint_0(base_footprint): { shape: box, size: [0.01, 0.01, 0.01, 0], visual: True }
+base_footprint_0(base_footprint): { shape: box, size: [0.01, 0.01, 0.01], visual: True }
 
 base_link(base_footprint): { pose: [0, 0, 0.051, 1, 0, 0, 0], mass: 116, inertia: [5.65223, -0.00971993, 1.29399, 5.66947, -0.00737958, 3.6832] }
 base_link_0(base_footprint): { pose: [0, 0, 0.051, 1, 0, 0, 0], shape: mesh, mesh: <meshes/base.h5>, visual: True }
 base_laser_link(base_footprint): { pose: [0.275, 0, 0.303, 1, 0, 0, 0], mass: 0.001, inertia: [0.0001, 1e-06, 0.0001] }
-base_laser_link_0(base_footprint): { pose: [0.275, 0, 0.303, 1, 0, 0, 0], shape: box, size: [0.001, 0.001, 0.001, 0], visual: True }
+base_laser_link_0(base_footprint): { pose: [0.275, 0, 0.303, 1, 0, 0, 0], shape: box, size: [0.001, 0.001, 0.001], visual: True }
 
 fl_caster_rotation_joint_origin(base_footprint): { pose: [0.2246, 0.2246, 0.0792, 1, 0, 0, 0] }
 fr_caster_rotation_joint_origin(base_footprint): { pose: [0.2246, -0.2246, 0.0792, 1, 0, 0, 0] }
@@ -37,7 +37,7 @@ br_caster_r_wheel_joint_origin(br_caster_rotation_joint): { pose: [0, -0.049, 0,
 torso_lift_link(torso_lift_joint): { mass: 36.248, inertia: [2.77165, 0.00428452, -0.160419, 2.51002, 0.0296645, 0.526432] }
 torso_lift_link_0(torso_lift_joint): { shape: mesh, mesh: <meshes/torso_lift.h5>, visual: True }
 imu_link(torso_lift_joint): { pose: [-0.02977, -0.1497, 0.164, -1.03412e-13, 0, 1, 0], mass: 0.001, inertia: [0.0001, 1e-06, 0.0001] }
-imu_link_0(torso_lift_joint): { pose: [-0.02977, -0.1497, 0.164, -1.03412e-13, 0, 1, 0], shape: box, size: [0.001, 0.001, 0.001, 0], visual: True }
+imu_link_0(torso_lift_joint): { pose: [-0.02977, -0.1497, 0.164, -1.03412e-13, 0, 1, 0], shape: box, size: [0.001, 0.001, 0.001], visual: True }
 head_pan_joint_origin(torso_lift_joint): { pose: [-0.01707, 0, 0.38145, 1, 0, 0, 0] }
 laser_tilt_mount_joint_origin(torso_lift_joint): { pose: [0.09893, 0, 0.227, 1, 0, 0, 0] }
 r_shoulder_pan_joint_origin(torso_lift_joint): { pose: [0, -0.188, 0, 1, 0, 0, 0] }
@@ -76,7 +76,7 @@ head_tilt_joint_origin(head_pan_joint): { pose: [0.068, 0, 0, 1, 0, 0, 0] }
 laser_tilt_mount_link(laser_tilt_mount_joint): { mass: 0.05, inertia: [0.0001, 1e-05, 0.0001] }
 laser_tilt_mount_link_0(laser_tilt_mount_joint): { shape: mesh, mesh: <meshes/tilting_hokuyo.h5>, visual: True }
 laser_tilt_link(laser_tilt_mount_joint): { pose: [0, 0, 0.03, 1, 0, 0, 0], mass: 0.001, inertia: [0.0001, 1e-06, 0.0001] }
-laser_tilt_link_0(laser_tilt_mount_joint): { pose: [0, 0, 0.03, 1, 0, 0, 0], shape: box, size: [0.001, 0.001, 0.001, 0], visual: True }
+laser_tilt_link_0(laser_tilt_mount_joint): { pose: [0, 0, 0.03, 1, 0, 0, 0], shape: box, size: [0.001, 0.001, 0.001], visual: True }
 r_shoulder_pan_link(r_shoulder_pan_joint): { mass: 25.7993, inertia: [0.866179, -0.0608651, -0.121181, 0.874217, -0.0588661, 0.273538] }
 r_shoulder_pan_link_0(r_shoulder_pan_joint): { shape: mesh, mesh: <meshes/shoulder_pan.h5>, visual: True }
 r_shoulder_lift_joint_origin(r_shoulder_pan_joint): { pose: [0.1, 0, 0, 1, 0, 0, 0] }
@@ -89,27 +89,27 @@ l_shoulder_lift_joint(l_shoulder_lift_joint_origin): { joint: hingeY, limits: [-
 head_tilt_link(head_tilt_joint): { mass: 1.74973, inertia: [0.0106023, -0.000408814, 0.00198304, 0.0118744, 0.000197909, 0.00551679] }
 head_tilt_link_0(head_tilt_joint): { shape: mesh, mesh: <meshes/head_tilt.h5>, visual: True }
 head_plate_frame(head_tilt_joint): { pose: [0.0232, 0, 0.0645, 1, 0, 0, 0], mass: 0.01, inertia: [0.001, 0.001, 0.001] }
-head_plate_frame_0(head_tilt_joint): { pose: [0.0232, 0, 0.0645, 1, 0, 0, 0], shape: box, size: [0.01, 0.01, 0.01, 0], visual: True }
+head_plate_frame_0(head_tilt_joint): { pose: [0.0232, 0, 0.0645, 1, 0, 0, 0], shape: box, size: [0.01, 0.01, 0.01], visual: True }
 sensor_mount_link(head_tilt_joint): { pose: [0.0232, 0, 0.0645, 1, 0, 0, 0], mass: 0.05, inertia: [0.001, 0.001, 0.01] }
-sensor_mount_link_0(head_tilt_joint): { pose: [0.0232, 0, 0.0645, 1, 0, 0, 0], shape: box, size: [0.01, 0.01, 0.01, 0], visual: True }
+sensor_mount_link_0(head_tilt_joint): { pose: [0.0232, 0, 0.0645, 1, 0, 0, 0], shape: box, size: [0.01, 0.01, 0.01], visual: True }
 high_def_frame(head_tilt_joint): { pose: [0.0232, -0.109, 0.0995, 1, 0, 0, 0], mass: 0.01, inertia: [0.001, 0.001, 0.001] }
-high_def_frame_0(head_tilt_joint): { pose: [0.0032, -0.109, 0.0995, 1, 0, 0, 0], shape: box, size: [0.04, 0.04, 0.04, 0], visual: True }
+high_def_frame_0(head_tilt_joint): { pose: [0.0032, -0.109, 0.0995, 1, 0, 0, 0], shape: box, size: [0.04, 0.04, 0.04], visual: True }
 high_def_optical_frame(head_tilt_joint): { pose: [0.0232, -0.109, 0.0995, 0.5, -0.5, 0.5, -0.5], mass: 0.01, inertia: [0.001, 0.001, 0.001] }
-high_def_optical_frame_0(head_tilt_joint): { pose: [0.0232, -0.109, 0.0995, 0.707107, -3.46245e-12, 0.707107, -5.55112e-17], shape: cylinder, size: [0, 0, 0.05, 0.02], visual: True }
+high_def_optical_frame_0(head_tilt_joint): { pose: [0.0232, -0.109, 0.0995, 0.707107, -3.46245e-12, 0.707107, -5.55112e-17], shape: cylinder, size: [0.05, 0.02], visual: True }
 double_stereo_link(head_tilt_joint): { pose: [0.0232, 0, 0.0675, 1, 0, 0, 0], mass: 0.1, inertia: [0.001, 0.001, 0.01] }
-double_stereo_link_0(head_tilt_joint): { pose: [0.0132, 0, 0.0925, 1, 0, 0, 0], shape: box, size: [0.02, 0.12, 0.05, 0], visual: True }
+double_stereo_link_0(head_tilt_joint): { pose: [0.0132, 0, 0.0925, 1, 0, 0, 0], shape: box, size: [0.02, 0.12, 0.05], visual: True }
 wide_stereo_link(head_tilt_joint): { pose: [0.0232, 0.03, 0.098, 1, 0, 0, 0], mass: 0.1, inertia: [0.01, 0.01, 0.01] }
-wide_stereo_link_0(head_tilt_joint): { pose: [0.0232, 0.03, 0.098, 1, 0, 0, 0], shape: box, size: [0.001, 0.001, 0.001, 0], visual: True }
+wide_stereo_link_0(head_tilt_joint): { pose: [0.0232, 0.03, 0.098, 1, 0, 0, 0], shape: box, size: [0.001, 0.001, 0.001], visual: True }
 wide_stereo_gazebo_l_stereo_camera_frame(head_tilt_joint): { pose: [0.0232, 0.03, 0.098, 1, 0, 0, 0], mass: 0.01, inertia: [0.001, 0.001, 0.001] }
-wide_stereo_gazebo_l_stereo_camera_frame_0(head_tilt_joint): { pose: [0.0232, 0.03, 0.098, 1, 0, 0, 0], shape: box, size: [0.01, 0.01, 0.01, 0], visual: True }
+wide_stereo_gazebo_l_stereo_camera_frame_0(head_tilt_joint): { pose: [0.0232, 0.03, 0.098, 1, 0, 0, 0], shape: box, size: [0.01, 0.01, 0.01], visual: True }
 wide_stereo_gazebo_r_stereo_camera_frame(head_tilt_joint): { pose: [0.0232, -0.06, 0.098, 1, 0, 0, 0], mass: 0.01, inertia: [0.001, 0.001, 0.001] }
-wide_stereo_gazebo_r_stereo_camera_frame_0(head_tilt_joint): { pose: [0.0232, -0.06, 0.098, 1, 0, 0, 0], shape: box, size: [0.01, 0.01, 0.01, 0], visual: True }
+wide_stereo_gazebo_r_stereo_camera_frame_0(head_tilt_joint): { pose: [0.0232, -0.06, 0.098, 1, 0, 0, 0], shape: box, size: [0.01, 0.01, 0.01], visual: True }
 narrow_stereo_link(head_tilt_joint): { pose: [0.0232, 0.06, 0.098, 1, 0, 0, 0], mass: 0.1, inertia: [0.01, 0.01, 0.01] }
-narrow_stereo_link_0(head_tilt_joint): { pose: [0.0232, 0.06, 0.098, 1, 0, 0, 0], shape: box, size: [0.001, 0.001, 0.001, 0], visual: True }
+narrow_stereo_link_0(head_tilt_joint): { pose: [0.0232, 0.06, 0.098, 1, 0, 0, 0], shape: box, size: [0.001, 0.001, 0.001], visual: True }
 narrow_stereo_gazebo_l_stereo_camera_frame(head_tilt_joint): { pose: [0.0232, 0.06, 0.098, 1, 0, 0, 0], mass: 0.01, inertia: [0.001, 0.001, 0.001] }
-narrow_stereo_gazebo_l_stereo_camera_frame_0(head_tilt_joint): { pose: [0.0232, 0.06, 0.098, 1, 0, 0, 0], shape: box, size: [0.01, 0.01, 0.01, 0], visual: True }
+narrow_stereo_gazebo_l_stereo_camera_frame_0(head_tilt_joint): { pose: [0.0232, 0.06, 0.098, 1, 0, 0, 0], shape: box, size: [0.01, 0.01, 0.01], visual: True }
 narrow_stereo_gazebo_r_stereo_camera_frame(head_tilt_joint): { pose: [0.0232, -0.03, 0.098, 1, 0, 0, 0], mass: 0.01, inertia: [0.001, 0.001, 0.001] }
-narrow_stereo_gazebo_r_stereo_camera_frame_0(head_tilt_joint): { pose: [0.0232, -0.03, 0.098, 1, 0, 0, 0], shape: box, size: [0.01, 0.01, 0.01, 0], visual: True }
+narrow_stereo_gazebo_r_stereo_camera_frame_0(head_tilt_joint): { pose: [0.0232, -0.03, 0.098, 1, 0, 0, 0], shape: box, size: [0.01, 0.01, 0.01], visual: True }
 r_shoulder_lift_link(r_shoulder_lift_joint): { mass: 2.74988, inertia: [0.0210558, 0.00496704, -0.00194809, 0.0212722, 0.00110425, 0.0197575] }
 r_shoulder_lift_link_0(r_shoulder_lift_joint): { shape: mesh, mesh: <meshes/shoulder_lift.h5>, visual: True }
 l_shoulder_lift_link(l_shoulder_lift_joint): { mass: 2.74988, inertia: [0.0210558, 0.00496704, -0.00194809, 0.0212722, 0.00110425, 0.0197575] }
@@ -139,14 +139,14 @@ r_forearm_roll_link_0(r_forearm_roll_joint): { shape: mesh, mesh: <meshes/forear
 r_forearm_link(r_forearm_roll_joint): { mass: 2.57968, inertia: [0.00364857, 5.21588e-05, 0.000715348, 0.0150774, -1.31077e-05, 0.0165931] }
 r_forearm_link_0(r_forearm_roll_joint): { shape: mesh, mesh: <meshes/forearm.h5>, visual: True }
 r_forearm_cam_frame(r_forearm_roll_joint): { pose: [0.135, 0, 0.044, 0.679288, 0.679288, -0.196387, 0.196387], mass: 0.01, inertia: [0.001, 0.001, 0.001] }
-r_forearm_cam_frame_0(r_forearm_roll_joint): { pose: [0.135, 0, 0.044, 0.679288, 0.679288, -0.196387, 0.196387], shape: box, size: [0.01, 0.01, 0.01, 0], visual: True }
+r_forearm_cam_frame_0(r_forearm_roll_joint): { pose: [0.135, 0, 0.044, 0.679288, 0.679288, -0.196387, 0.196387], shape: box, size: [0.01, 0.01, 0.01], visual: True }
 r_wrist_flex_joint_origin(r_forearm_roll_joint): { pose: [0.321, 0, 0, 1, 0, 0, 0] }
 l_forearm_roll_link(l_forearm_roll_joint): { mass: 0.1, inertia: [0.01, 0.01, 0.01] }
 l_forearm_roll_link_0(l_forearm_roll_joint): { shape: mesh, mesh: <meshes/forearm_roll.h5>, visual: True }
 l_forearm_link(l_forearm_roll_joint): { mass: 2.57968, inertia: [0.00364857, 5.21588e-05, 0.000715348, 0.0150774, -1.31077e-05, 0.0165931] }
 l_forearm_link_0(l_forearm_roll_joint): { shape: mesh, mesh: <meshes/forearm.h5>, visual: True }
 l_forearm_cam_frame(l_forearm_roll_joint): { pose: [0.135, 0, 0.044, 0.679288, -0.679288, -0.196387, -0.196387], mass: 0.01, inertia: [0.001, 0.001, 0.001] }
-l_forearm_cam_frame_0(l_forearm_roll_joint): { pose: [0.135, 0, 0.044, 0.679288, -0.679288, -0.196387, -0.196387], shape: box, size: [0.01, 0.01, 0.01, 0], visual: True }
+l_forearm_cam_frame_0(l_forearm_roll_joint): { pose: [0.135, 0, 0.044, 0.679288, -0.679288, -0.196387, -0.196387], shape: box, size: [0.01, 0.01, 0.01], visual: True }
 l_wrist_flex_joint_origin(l_forearm_roll_joint): { pose: [0.321, 0, 0, 1, 0, 0, 0] }
 r_wrist_flex_joint(r_wrist_flex_joint_origin): { joint: hingeY, limits: [-2.094, 0, 3.078, -1, 10], ctrl_limits: [3.078, -1, 10] }
 l_wrist_flex_joint(l_wrist_flex_joint_origin): { joint: hingeY, limits: [-2.094, 0, 3.078, -1, 10], ctrl_limits: [3.078, -1, 10] }
@@ -161,7 +161,7 @@ r_wrist_roll_link_0(r_wrist_roll_joint): { shape: mesh, mesh: <meshes/wrist_roll
 r_gripper_palm_link(r_wrist_roll_joint): { mass: 0.58007, inertia: [0.000352239, -1.58048e-05, -9.175e-07, 0.000677413, -5.9554e-07, 0.000865633] }
 r_gripper_palm_link_0(r_wrist_roll_joint): { shape: mesh, mesh: <meshes/gripper_palm.h5>, visual: True }
 r_gripper_motor_accelerometer_link(r_wrist_roll_joint): { mass: 0.001, inertia: [0.001, 0.001, 0.001] }
-r_gripper_motor_accelerometer_link_0(r_wrist_roll_joint): { shape: box, size: [0.001, 0.001, 0.001, 0], visual: True }
+r_gripper_motor_accelerometer_link_0(r_wrist_roll_joint): { shape: box, size: [0.001, 0.001, 0.001], visual: True }
 r_gripper_l_finger_joint_origin(r_wrist_roll_joint): { pose: [0.07691, 0.01, 0, 1, 0, 0, 0] }
 r_gripper_r_finger_joint_origin(r_wrist_roll_joint): { pose: [0.07691, -0.01, 0, 1, 0, 0, 0] }
 r_gripper_l_parallel_root_joint_origin(r_wrist_roll_joint): { pose: [0.05891, 0.031, 0, 1, 0, 0, 0] }
@@ -171,7 +171,7 @@ l_wrist_roll_link_0(l_wrist_roll_joint): { shape: mesh, mesh: <meshes/wrist_roll
 l_gripper_palm_link(l_wrist_roll_joint): { mass: 0.58007, inertia: [0.000352239, -1.58048e-05, -9.175e-07, 0.000677413, -5.9554e-07, 0.000865633] }
 l_gripper_palm_link_0(l_wrist_roll_joint): { shape: mesh, mesh: <meshes/gripper_palm.h5>, visual: True }
 l_gripper_motor_accelerometer_link(l_wrist_roll_joint): { mass: 0.001, inertia: [0.001, 0.001, 0.001] }
-l_gripper_motor_accelerometer_link_0(l_wrist_roll_joint): { shape: box, size: [0.001, 0.001, 0.001, 0], visual: True }
+l_gripper_motor_accelerometer_link_0(l_wrist_roll_joint): { shape: box, size: [0.001, 0.001, 0.001], visual: True }
 l_gripper_l_finger_joint_origin(l_wrist_roll_joint): { pose: [0.07691, 0.01, 0, 1, 0, 0, 0] }
 l_gripper_r_finger_joint_origin(l_wrist_roll_joint): { pose: [0.07691, -0.01, 0, 1, 0, 0, 0] }
 l_gripper_l_parallel_root_joint_origin(l_wrist_roll_joint): { pose: [0.05891, 0.031, 0, 1, 0, 0, 0] }

robotic/rai-robotModels/robotiq/robotiq_clean.g

@@ -19,6 +19,6 @@ right_inner_knuckle_0(right_inner_knuckle_joint): { shape: mesh, color: [0.1, 0.
 left_inner_finger_joint(left_outer_finger>left_inner_finger_joint): { joint: hingeX, limits: [0, 0.8757, 2, -1, 1000], mimic: "finger_joint", ctrl_limits: [2, -1, 1000] }
 right_inner_finger_joint(right_outer_finger>right_inner_finger_joint): { joint: hingeX, limits: [0, 0.8757, 2, -1, 1000], mimic: "finger_joint", ctrl_limits: [2, -1, 1000] }
 left_inner_finger_0(left_inner_finger_joint): { shape: mesh, color: [0.1, 0.1, 0.1, 1], mesh: <meshes/visual/robotiq_arg2f_85_inner_finger.ply>, meshscale: 0.001, visual: True }
-left_inner_finger_pad_0(left_inner_finger_joint): { Q: [0, -0.0220203, 0.03242, 1, 0, 0, 0], shape: box, size: [0.022, 0.00635, 0.0375, 0], color: [0.9, 0.9, 0.9, 1], visual: True }
+left_inner_finger_pad_0(left_inner_finger_joint): { Q: [0, -0.0220203, 0.03242, 1, 0, 0, 0], shape: box, size: [0.022, 0.00635, 0.0375], color: [0.9, 0.9, 0.9, 1], visual: True }
 right_inner_finger_0(right_inner_finger_joint): { shape: mesh, color: [0.1, 0.1, 0.1, 1], mesh: <meshes/visual/robotiq_arg2f_85_inner_finger.ply>, meshscale: 0.001, visual: True }
-right_inner_finger_pad_0(right_inner_finger_joint): { Q: [0, -0.0220203, 0.03242, 1, 0, 0, 0], shape: box, size: [0.022, 0.00635, 0.0375, 0], color: [0.9, 0.9, 0.9, 1], visual: True }
+right_inner_finger_pad_0(right_inner_finger_joint): { Q: [0, -0.0220203, 0.03242, 1, 0, 0, 0], shape: box, size: [0.022, 0.00635, 0.0375], color: [0.9, 0.9, 0.9, 1], visual: True }

robotic/rai-robotModels/scenarios/liftRing.g

@@ -1,4 +1,4 @@
-world: { multibody: true }
+world: {}
 
 table (world): {
  shape: ssBox, Q: "t(0 0. .6)", size: [1., 2., .1, .02], color: [.3, .3, .3]
@@ -41,6 +41,6 @@ target (table): {
 #Include: <panda_fixGripper.g>
 #Include: <panda_fixRobotiq.g>
 Include: <../panda/panda.g>
-(table panda_base): { joint: rigid, Q: "t(.3 .8 .05) d(-90 0 0 1)" }
+Edit panda_base(table): { joint: rigid, Q: "t(.3 .8 .05) d(-90 0 0 1)" }
 
 Edit panda_finger_joint1: { joint_active: false }

robotic/rai-robotModels/scenarios/pandaSingle.g

@@ -3,7 +3,7 @@ world: {}
 ### table
 
 table(world): {
- shape: ssBox, Q: "t(0 0. .6)", size: [2.5, 2.5, .1, .02], color: [.3, .3, .3],
+ shape: ssBox, Q: [0 0. .6], size: [2.5, 2.5, .1, .02], color: [.3, .3, .3],
  contact: 1, logical: { },
  friction: .1
 }

robotic/rai-robotModels/tests/arm.g

@@ -1,22 +1,21 @@
 
-stem: { X: "t(0 0 1)", shape: capsule, size: [2, .1] }
+stem { X: "t(0 0 1)" shape:capsule size=[2 .1] }
 
-arm1: { shape: capsule, size: [.4, .1], contact: -1, }
-arm2: { shape: capsule, size: [.4, .1], contact: -1, }
-arm3: { shape: capsule, size: [.4, .1], contact: -1, }
-arm4: { shape: capsule, size: [.4, .1], contact: -1, }
-arm5: { shape: capsule, size: [.4, .1], contact: -1, }
-arm6: { shape: capsule, size: [.4, .1], contact: -1, }
-arm7: { shape: capsule, size: [.4, .1], contact: -1, }
+arm1(j1) { shape:capsule Q:[0 0 .2], size=[.4 .1] contact:-1, }
+arm2(j2) { shape:capsule Q:[0 0 .2], size=[.4 .1] contact:-1, }
+arm3(j3) { shape:capsule Q:[0 0 .2], size=[.4 .1] contact:-1, }
+arm4(j4) { shape:capsule Q:[0 0 .2], size=[.4 .1] contact:-1, }
+arm5(j5) { shape:capsule Q:[0 0 .2], size=[.4 .1] contact:-1, }
+arm6(j6) { shape:capsule Q:[0 0 .2], size=[.4 .1] contact:-1, }
+arm7(j7) { shape:capsule Q:[0 0 .2], size=[.4 .1] contact:-1, }
 
-(stem arm1): { joint: hingeX, limits:[-2,2], pre: "t(0 0 1) d(90 1 0 0)", post: "t(0 0 .2)", Q: "T d(1 0 0 0)" }
-
-(arm1 arm2): { joint: hingeX, limits:[-2,2], pre: "t(0 0 0.2) d(45 0 0 1)", post: "t(0 0 .2)", Q: "T d(1 0 0 0)" }
-(arm2 arm3): { joint: hingeX, limits:[-2,2], pre: "t(0 0 0.2) d(45 0 0 1)", post: "t(0 0 .2)", Q: "T d(1 0 0 0)" }
-(arm3 arm4): { joint: hingeX, limits:[-2,2], pre: "t(0 0 0.2) d(45 0 0 1)", post: "t(0 0 .2)", Q: "T d(1 0 0 0)" }
-(arm4 arm5): { joint: quatBall, limits:[-2,-2,-2,-2,2,2,2,2], pre: "t(0 0 0.2) d(45 0 0 1)", post: "t(0 0 .2)", Q: "T d(1 0 0 0)" }
-(arm5 arm6): { joint: hingeX, limits:[-2,2], pre: "t(0 0 0.2) d(45 0 0 1)", post: "t(0 0 .2)", Q: "T d(1 0 0 0)" }
-(arm6 arm7): { joint: hingeX, limits:[-2,2], pre: "t(0 0 0.2) d(45 0 0 1)", post: "t(0 0 .2)", Q: "T d(1 0 0 0)" }
+j1(stem) { joint:hingeX A: "t(0 0 1) d(90 1 0 0)" limits:[-2 2] }
+j2(arm1) { joint:hingeX A: "t(0 0 0.2) d(45 0 0 1)" limits:[-2 2] }
+j3(arm2) { joint:hingeX A: "t(0 0 0.2) d(45 0 0 1)" limits:[-2 2] }
+j4(arm3) { joint:hingeX A: "t(0 0 0.2) d(45 0 0 1)" limits:[-2 2] }
+j5(arm4) { joint:quatBall A: "t(0 0 0.2) d(45 0 0 1)" limits:[0 -1 -1 -1 1.5 1 1 1] }
+j6(arm5) { joint:hingeX A: "t(0 0 0.2) d(45 0 0 1)" limits:[-2 2] }
+j7(arm6) { joint:hingeX A: "t(0 0 0.2) d(45 0 0 1)" limits:[-2 2] }
 
 target: { X: "t(.7 -.5 1.2)", shape: sphere, size: [.05], color: [0, .5, 0] }
 

robotic/ry-h5info

@@ -2,29 +2,20 @@
 
 import argparse
 import h5py
+from robotic.src.h5_helper import *
 
 parser = argparse.ArgumentParser(description='h5-file info')
 
 parser.add_argument('FILE', type=str,
                     help='h5-file name')
 
-def print_attrs(name, obj):
-    if isinstance(obj, h5py.Dataset):
-        print('   ', name, obj.name, obj.shape, obj.dtype, f'{obj.size*obj.dtype.itemsize/1024:.2f}kB')
-        if obj.dtype=='int8':
-            print('       ', ''.join([chr(x) for x in obj[()]]))
-        elif obj.size<20:
-            print('       ', obj[()])
-    else:
-        print('---', name)
-
 def main():
     args = parser.parse_args()
 
     print('=== file', args.FILE)
     try:
-        with h5py.File(args.FILE, 'r') as fil:
-            fil.visititems(print_attrs)
+        h5 = H5Reader(args.FILE)
+        h5.print_info()
     except KeyboardInterrupt:
         sys.exit(1)