experimental: time joints and optimization

rai/Geo/geo.cpp

@@ -91,6 +91,12 @@ void Vector::makeColinear(const Vector& b) {
 /// L1-norm to zero
 double Vector::diffZero() const { return fabs(x)+fabs(y)+fabs(z); }
 
+/// check whether isZero is true
+void Vector::checkZero() const {
+  bool iszero = (x==0. && y==0. && z==0.);
+  CHECK_EQ(iszero, isZero, "you must have set this by hand!");
+}
+
 /// is it normalized?
 bool Vector::isNormalized() const { return fabs(lengthSqr()-1.)<1e-6; }
 
@@ -736,6 +742,12 @@ double Quaternion::diffZero() const { return (w>0.?fabs(w-1.):fabs(w+1.))+fabs(x
 
 double Quaternion::sqrDiffZero() const { return (w>0.?rai::sqr(w-1.):rai::sqr(w+1.))+rai::sqr(x)+rai::sqr(y)+rai::sqr(z); }
 
+/// check whether isZero is true
+void Quaternion::checkZero() const {
+  bool iszero = ((w==1. || w==-1.) && x==0. && y==0. && z==0.);
+  CHECK_EQ(iszero, isZero, "you must have set this by hand!");
+}
+
 /// return the squared-error between two quads, modulo flipping
 double Quaternion::sqrDiff(const Quaternion& _q2) const{
   arr q1(&w, 4, true);

rai/Geo/geo.h

@@ -44,6 +44,7 @@ struct Vector {
   void makeColinear(const Vector&);
 
   double diffZero() const;
+  void checkZero() const;
   bool isNormalized() const;
   double isColinear(const Vector&) const;
   double length() const;
@@ -135,6 +136,7 @@ struct Quaternion {
 
   double diffZero() const;
   double sqrDiffZero() const;
+  void checkZero() const;
   double sqrDiff(const Quaternion& q2) const;
   double normalization() const;
   bool isNormalized() const;

rai/KOMO/komo.cpp

@@ -119,9 +119,9 @@ void KOMO::setTiming(double _phases, uint _stepsPerPhase, double durationPerPhas
   maxPhase = _phases;
   stepsPerPhase = _stepsPerPhase;
   if(stepsPerPhase>=0){
-    T = stepsPerPhase*maxPhase;
+    T = ceil(stepsPerPhase*maxPhase)+1;
     CHECK(T, "using T=0 to indicate inverse kinematics is deprecated.");
-    tau = durationPerPhase*maxPhase/T;
+    tau = durationPerPhase/double(stepsPerPhase);
   }
 //    setTiming(stepsPerPhase*maxPhase, durationPerPhase*maxPhase);
   k_order = _k_order;
@@ -1255,6 +1255,28 @@ void KOMO::plotTrajectory(){
   gnuplot("load 'z.trajectories.plt'");
 }
 
+void KOMO::plotPhaseTrajectory(){
+  ofstream fil("z.phase");
+  //first line: legend
+  fil <<"phase" <<endl;
+
+  arr X = getPath_times();
+
+  X.reshape(T, 1);
+  X.write(fil, NULL, NULL, "  ");
+  fil.close();
+
+  ofstream fil2("z.phase.plt");
+  fil2 <<"set key autotitle columnheader" <<endl;
+  fil2 <<"set title 'phase'" <<endl;
+  fil2 <<"set term qt 2" <<endl;
+  fil2 <<"plot 'z.phase' u 0:1 w l lw 3 lc 1 lt 1" <<endl;
+  fil2 <<endl;
+  fil2.close();
+
+  gnuplot("load 'z.phase.plt'");
+}
+
 struct DrawPaths : GLDrawer{
   arr& X;
   DrawPaths(arr& X): X(X){}
@@ -1406,7 +1428,7 @@ void KOMO::set_x(const arr& x){
   uint x_count=0;
   for(uint t=0;t<T;t++){
     uint s = t+k_order;
-    uint x_dim = dim_x(t); //configurations(s)->getJointStateDimension();
+    uint x_dim = dim_x(t);
     if(x_dim){
       if(x.nd==1)  configurations(s)->setJointState(x({x_count, x_count+x_dim-1}));
       else         configurations(s)->setJointState(x[t]);
@@ -1416,6 +1438,7 @@ void KOMO::set_x(const arr& x){
       }
       x_count += x_dim;
     }
+//    configurations(s)->checkConsistency();
   }
   CHECK_EQ(x_count, x.N, "");
 }
@@ -1518,6 +1541,7 @@ rai::Array<rai::Transformation> KOMO::reportEffectiveJoints(std::ostream& os){
 
 Graph KOMO::getReport(bool gnuplt, int reportFeatures, std::ostream& featuresOs) {
   if(featureValues.N>1){ //old optimizer -> remove some time..
+    HALT("outdated");
     arr tmp;
     for(auto& p:featureValues) tmp.append(p);
     featureValues = ARRAY<arr>(tmp);
@@ -1702,8 +1726,11 @@ void KOMO::Conv_MotionProblem_KOMO_Problem::phi(arr& phi, arrA& J, arrA& H, uint
   //-- set the trajectory
   komo.set_x(x);
 
-  if(komo.animateOptimization)
-    komo.displayPath(false);
+  if(komo.animateOptimization>0){
+    komo.displayPath(komo.animateOptimization>1);
+//    komo.plotPhaseTrajectory();
+//    rai::wait();
+  }
 
   CHECK(dimPhi,"getStructure must be called first");
 //  getStructure(NoUintA, featureTimes, tt);

rai/KOMO/komo.h

@@ -57,7 +57,7 @@ struct KOMO{
   arr dualSolution;             ///< the dual solution computed during constrained optimization
   struct OpenGL *gl=NULL;       ///< internal only: used in 'displayTrajectory'
   int verbose;                  ///< verbosity level
-  bool animateOptimization=false; ///< display the current path for each evaluation during optimization
+  int animateOptimization=0;    ///< display the current path for each evaluation during optimization
   double runTime=0.;            ///< just measure run time
   ofstream *fil=NULL;
 
@@ -183,6 +183,7 @@ struct KOMO{
   rai::Array<rai::Transformation> reportEffectiveJoints(ostream& os=std::cout);
   void checkGradients();          ///< checks all gradients numerically
   void plotTrajectory();
+  void plotPhaseTrajectory();
   bool displayTrajectory(double delay=0.01, bool watch=true, const char* saveVideoPrefix=NULL); ///< display the trajectory; use "vid/z." as vid prefix
   bool displayPath(bool watch=true); ///< display the trajectory; use "vid/z." as vid prefix
   rai::Camera& displayCamera();   ///< access to the display camera to change the view
@@ -216,7 +217,6 @@ struct KOMO{
     virtual void getStructure(uintA& variableDimensions, uintA& featureTimes, ObjectiveTypeA& featureTypes);
     virtual void phi(arr& phi, arrA& J, arrA& H, uintA& featureTimes, ObjectiveTypeA& tt, const arr& x, arr& lambda);
   } komo_problem;
-
 };
 
 //===========================================================================

rai/Kin/TM_time.cpp

@@ -0,0 +1,9 @@
+#include "TM_time.h"
+
+void TM_Time::phi(arr &y, arr &J, const rai::KinematicWorld &K){
+  y = ARR( K.frames(0)->time );
+
+  if(&J){
+    K.jacobianTime(J, K.frames(0));
+  }
+}

rai/Kin/TM_time.h

@@ -0,0 +1,10 @@
+#include "taskMap.h"
+
+struct TM_Time : TaskMap {
+  TM_Time(){}
+
+  virtual void phi(arr& y, arr& J, const rai::KinematicWorld& K);
+  virtual uint dim_phi(const rai::KinematicWorld& K){ return 1; }
+
+  virtual rai::String shortTag(const rai::KinematicWorld& G){ return STRING("Time"); }
+};

rai/Kin/TM_transition.cpp

@@ -58,7 +58,7 @@ void TM_Transition::phi(arr& y, arr& J, const WorldL& Ktuple){
   if(!handleSwitches){ //simple implementation
     //-- transition costs
     double h = H_rate*sqrt(tau), tau2=tau*tau;
-//    arr h = sqrt(H_rate_diag)*sqrt(tau);
+    h = H_rate;
     y.resize(Ktuple.last()->q.N).setZero();
     if(order==1) velCoeff = 1.;
     if(order>=0 && posCoeff) y +=  posCoeff      *(Ktuple.elem(-1)->q); //penalize position
@@ -91,6 +91,10 @@ void TM_Transition::phi(arr& y, arr& J, const WorldL& Ktuple){
     }
 
     if(&J) {
+      arr Jtau1;  Ktuple(-1)->jacobianTime(Jtau1, Ktuple(-1)->frames(0));  expandJacobian(Jtau1, Ktuple, -1);
+      arr Jtau2;  Ktuple(-2)->jacobianTime(Jtau2, Ktuple(-2)->frames(0));  expandJacobian(Jtau2, Ktuple, -2);
+      arr Jtau = Jtau1 - Jtau2;
+
       uint n = Ktuple.last()->q.N;
       J.resize(y.N, Ktuple.N, n).setZero();
       for(uint i=0;i<n;i++){
@@ -109,6 +113,7 @@ void TM_Transition::phi(arr& y, arr& J, const WorldL& Ktuple){
         //      if(order>=3){ J(i,3,i) = 1.;  J(i,2,i) = -3.;  J(i,1,i) = +3.;  J(i,0,i) = -1.; }
       }
       J.reshape(y.N, Ktuple.N*n);
+
       for(rai::Joint *j: Ktuple.last()->fwdActiveJoints) for(uint i=0;i<j->qDim();i++){
         double hj = h*j->H;
         if(j->frame.flags && !(j->frame.flags & (1<<FL_normalControlCosts))) hj=0.;
@@ -119,6 +124,9 @@ void TM_Transition::phi(arr& y, arr& J, const WorldL& Ktuple){
         for(uint k=0;k<J.d1;k++) J.elem(l+k) *= hj;
 #endif
       }
+
+      J += (-2./tau)*y*Jtau;
+
     }
   }else{ //with switches
     rai::Array<rai::Joint*> matchingJoints = getMatchingJoints(Ktuple.sub(-1-order,-1), effectiveJointsOnly);

rai/Kin/frame.cpp

@@ -17,6 +17,17 @@
 #include <Gui/opengl.h>
 #endif
 
+//===========================================================================
+
+template<> const char* rai::Enum<rai::JointType>::names []={
+  "JT_hingeX", "JT_hingeY", "JT_hingeZ", "JT_transX", "JT_transY", "JT_transZ", "JT_transXY", "JT_trans3", "JT_transXYPhi", "JT_universal", "JT_rigid", "JT_quatBall", "JT_phiTransXY", "JT_XBall", "JT_free", "JT_time", NULL
+};
+
+template<> const char* rai::Enum<rai::BodyType>::names []={
+  "BT_dynamic", "BT_kinematic", "BT_static", NULL
+};
+
+
 //===========================================================================
 //
 // Frame
@@ -56,6 +67,7 @@ rai::Frame::~Frame() {
 
 void rai::Frame::calc_X_from_parent(){
   CHECK(parent, "");
+  time = parent->time;
   Transformation &from = parent->X;
   X = from;
   X.appendTransformation(Q);
@@ -322,6 +334,10 @@ void rai::Joint::calc_Q_from_q(const arr &q, uint _qIndex){
 
     case JT_rigid:
       break;
+
+    case JT_time:
+      frame.time = 1e-1 * q.elem(_qIndex);
+      break;
     default: NIY;
     }
   }
@@ -429,6 +445,10 @@ arr rai::Joint::calc_q_from_Q(const rai::Transformation &Q) const{
     q(3)=Q.rot.y;
     q(4)=Q.rot.z;
     break;
+  case JT_time:
+    q.resize(1);
+    q(0) = 1e1 * frame.time;
+    break;
   default: NIY;
   }
   return q;
@@ -518,6 +538,7 @@ uint rai::Joint::getDimFromType() const {
   if(type==JT_free) return 7;
   if(type==JT_rigid || type==JT_none) return 0;
   if(type==JT_XBall) return 5;
+  if(type==JT_time) return 1;
   HALT("shouldn't be here");
   return 0;
 }

rai/Kin/frame.h

@@ -31,7 +31,7 @@ struct Shape;
 struct Inertia;
 struct Contact;
 //enum ShapeType { ST_none=-1, ST_box=0, ST_sphere, ST_capsule, ST_mesh, ST_cylinder, ST_marker, ST_retired_SSBox, ST_pointCloud, ST_ssCvx, ST_ssBox };
-enum JointType { JT_none=-1, JT_hingeX=0, JT_hingeY=1, JT_hingeZ=2, JT_transX=3, JT_transY=4, JT_transZ=5, JT_transXY=6, JT_trans3=7, JT_transXYPhi=8, JT_universal=9, JT_rigid=10, JT_quatBall=11, JT_phiTransXY=12, JT_XBall, JT_free };
+enum JointType { JT_none=-1, JT_hingeX=0, JT_hingeY=1, JT_hingeZ=2, JT_transX=3, JT_transY=4, JT_transZ=5, JT_transXY=6, JT_trans3=7, JT_transXYPhi=8, JT_universal=9, JT_rigid=10, JT_quatBall=11, JT_phiTransXY=12, JT_XBall, JT_free, JT_time };
 enum BodyType  { BT_none=-1, BT_dynamic=0, BT_kinematic, BT_static };
 }
 

rai/Kin/kin.cpp

@@ -74,16 +74,6 @@ rai::Shape& NoShape = *((rai::Shape*)NULL);
 rai::Joint& NoJoint = *((rai::Joint*)NULL);
 rai::KinematicWorld& NoWorld = *((rai::KinematicWorld*)NULL);
 
-template<> const char* rai::Enum<rai::JointType>::names []={
-  "JT_hingeX", "JT_hingeY", "JT_hingeZ", "JT_transX", "JT_transY", "JT_transZ", "JT_transXY", "JT_trans3", "JT_transXYPhi", "JT_universal", "JT_rigid", "JT_quatBall", "JT_phiTransXY", "JT_XBall", "JT_free", NULL
-};
-
-template<> const char* rai::Enum<rai::BodyType>::names []={
-  "BT_dynamic", "BT_kinematic", "BT_static", NULL
-};
-
-
-
 uintA stringListToShapeIndices(const rai::Array<const char*>& names, const rai::KinematicWorld& K) {
   uintA I(names.N);
   for(uint i=0; i<names.N; i++) {
@@ -277,6 +267,7 @@ void rai::KinematicWorld::copy(const rai::KinematicWorld& K, bool referenceSwift
     on the edges, this calculates the absolute frames of all other nodes (propagating forward
     through trees and testing consistency of loops). */
 void rai::KinematicWorld::calc_fwdPropagateFrames() {
+  if(fwdActiveSet.N!=frames.N) calc_activeSets();
   for(Frame *f:fwdActiveSet){
 #if 1
     if(f->parent) f->calc_X_from_parent();
@@ -301,6 +292,7 @@ void rai::KinematicWorld::calc_fwdPropagateFrames() {
 }
 
 arr rai::KinematicWorld::calc_fwdPropagateVelocities(){
+  if(fwdActiveSet.N!=frames.N) calc_activeSets();
   arr vel(frames.N, 2, 3);  //for every frame we have a linVel and angVel, each 3D
   vel.setZero();
   rai::Transformation f;
@@ -682,6 +674,7 @@ void rai::KinematicWorld::jacobianPos(arr& J, Frame *a, const rai::Vector& pos_w
     a = a->parent;
   }
 }
+
 #else
 void rai::KinematicWorld::jacobianPos(arr& J, Frame *a, const rai::Vector& pos_world) const {
   J.resize(3, getJointStateDimension()).setZero();
@@ -704,6 +697,27 @@ void rai::KinematicWorld::jacobianPos(arr& J, Frame *a, const rai::Vector& pos_w
 }
 #endif
 
+void rai::KinematicWorld::jacobianTime(arr& J, rai::Frame *a) const {
+  //get Jacobian
+  uint N=getJointStateDimension();
+  J.resize(1, N).setZero();
+
+  while(a) { //loop backward down the kinematic tree
+    Joint *j=a->joint;
+    if(j && j->active) {
+      uint j_idx=j->qIndex;
+      if(j_idx>=N) CHECK(j->type==JT_rigid, "");
+      if(j_idx<N){
+        if(j->type==JT_time) {
+          J(0, j_idx) += 1e-1;
+        }
+      }
+    }
+    if(!a->parent) break; //frame has no inlink -> done
+    a = a->parent;
+  }
+}
+
 /** @brief return the jacobian \f$J = \frac{\partial\phi_i(q)}{\partial q}\f$ of the position
   of the i-th body W.R.T. the 6 axes of an arbitrary shape-frame, NOT the robot's joints (3 x 6 tensor)
   WARNING: this does not check if s is actually in the kinematic chain from root to b.
@@ -1569,6 +1583,7 @@ void rai::KinematicWorld::displayDot(){
 }
 
 void rai::KinematicWorld::report(std::ostream &os) const {
+  CHECK_EQ(fwdActiveSet.N, frames.N, "you need to calc_activeSets before");
   uint nShapes=0, nUc=0;
   for(Frame *f:fwdActiveSet) if(f->shape) nShapes++;
   for(Joint *j:fwdActiveJoints) if(j->uncertainty) nUc++;
@@ -1822,6 +1837,7 @@ void rai::KinematicWorld::gravityToForces(double g) {
 
 /** similar to invDynamics using NewtonEuler; but only computing the backward pass */
 void rai::KinematicWorld::NewtonEuler_backward(){
+  CHECK_EQ(fwdActiveSet.N, frames.N, "you need to calc_activeSets before");
   uint N=fwdActiveSet.N;
   rai::Array<arr> h(N);
   arr Q(N, 6, 6);
@@ -2292,7 +2308,7 @@ void rai::KinematicWorld::optimizeTree(bool preserveNamed){
 }
 
 void rai::KinematicWorld::fwdIndexIDs(){
-  CHECK_EQ(fwdActiveSet.N ,frames.N, "");
+  CHECK_EQ(fwdActiveSet.N ,frames.N, "you need to calc_activeSets before");
   frames = fwdActiveSet;
   uint i=0;
   for(Frame *f: frames) f->ID = i++;
@@ -2354,11 +2370,13 @@ bool rai::KinematicWorld::checkConsistency(){
 
   Joint *j;
   for(Frame *f: frames) if((j=f->joint)){
-    CHECK(j->from(), "");
-    CHECK(j->from()->outLinks.findValue(&j->frame)>=0,"");
+    if(j->type.x!=JT_time){
+      CHECK(j->from(), "");
+      CHECK(j->from()->outLinks.findValue(&j->frame)>=0,"");
+    }
     CHECK_EQ(j->frame.joint, j,"");
     CHECK_GE(j->type.x, 0, "");
-    CHECK(j->type.x<=JT_free, "");
+    CHECK(j->type.x<=JT_time, "");
 
     if(j->mimic){
       CHECK(j->dim==0, "");
@@ -2385,6 +2403,14 @@ bool rai::KinematicWorld::checkConsistency(){
       for(Frame *f: frames) if(f->joint && f->joint->active) CHECK(jointIsInActiveSet(f->ID), "");
   }
 
+  //check isZero for all transformations
+  for(Frame *a: frames){
+    a->X.pos.checkZero();
+    a->X.rot.checkZero();
+    a->Q.pos.checkZero();
+    a->Q.rot.checkZero();
+  }
+
   for(const Proxy& p : proxies){
     CHECK_EQ(this, &p.a->K, "");
     CHECK_EQ(this, &p.b->K, "");