cleanup and fixes for rai-python with BotOp

bin/src_kinEdit/main.cpp

@@ -126,7 +126,7 @@ int main(int argc,char **argv){
     }
 
   }else{
-    rai::editConfiguration(file, C);
+    C.watchFile(file);
   }
 
   return 0;

rai/Algo/SplineCtrlFeed.cpp

@@ -33,7 +33,7 @@ void SplineCtrlReference::append(const arr& x, const arr& t, double ctrlTime, bo
   }
 }
 
-void SplineCtrlReference::overrideSmooth(const arr& x, const arr& t, double ctrlTime){
+void SplineCtrlReference::overwriteSmooth(const arr& x, const arr& t, double ctrlTime){
   CHECK(t.first()>.001, "that's too harsh!");
   waitForInitialized();
   arr x_now, xDot_now;
@@ -45,7 +45,7 @@ void SplineCtrlReference::overrideSmooth(const arr& x, const arr& t, double ctrl
   splineSet->set(2, _x, _t+ctrlTime, xDot_now);
 }
 
-void SplineCtrlReference::overrideHard(const arr& x, const arr& t, double ctrlTime){
+void SplineCtrlReference::overwriteHard(const arr& x, const arr& t, double ctrlTime){
   waitForInitialized();
 
   CHECK_LE(t.first(), .0, "");

rai/Algo/SplineCtrlFeed.h

@@ -24,14 +24,14 @@ struct SplineCtrlReference : ReferenceFeed {
   /// append new knots to the spline; if prependLast, the currently last spline remains a zero-vel double know (holds) before starting the appended
   void append(const arr& x, const arr& t, double ctrlTime, bool prependLast);
   /// override the spline, but use the current spline's current pos/vel as start knot of the new spline; the first time knot needs to be >.1 sec
-  void overrideSmooth(const arr& x, const arr& t, double ctrlTime);
+  void overwriteSmooth(const arr& x, const arr& t, double ctrlTime);
   /// fully override the spline with new knots x and t, as well as initial vel xDot0; for safety, the first x needs to be close to the current spline's current pos
-  void overrideHard(const arr& x, const arr& t, double ctrlTime);
+  void overwriteHard(const arr& x, const arr& t, double ctrlTime);
 
   //simple helper for single goal spline
   void moveTo(const arr& x, double t, double ctrlTime, bool append){
     if(append) this->append(~x, {t}, ctrlTime, true);
-    else  overrideSmooth(~x, {t}, ctrlTime);
+    else  overwriteSmooth(~x, {t}, ctrlTime);
   }
 
   //info:

rai/Core/graph.cpp

@@ -611,7 +611,12 @@ void Graph::read(std::istream& is, bool parseInfo) {
       uint Nbefore = N;
       read(n->as<FileToken>().getIs(true), parseInfo);
       if(namePrefix.N) { //prepend a naming prefix to all nodes just read
-        for(uint i=Nbefore; i<N; i++) elem(i)->key.prepend(namePrefix);
+        for(uint i=Nbefore; i<N; i++){
+          elem(i)->key.prepend(namePrefix);
+          rai::String *tmp=0;
+          if(elem(i)->is<Graph>()) tmp=elem(i)->graph().find<rai::String>("mimic");
+          if(tmp) tmp->prepend(namePrefix);
+        }
         namePrefix.clear();
       }
       n->as<FileToken>().cd_start();

rai/Core/util.cpp

@@ -163,7 +163,7 @@ char skip(std::istream& is, const char* skipSymbols, const char* stopSymbols, bo
   char c;
   for(;;) {
     c=is.get();
-    if(is.eof()) return 255;
+    if(is.eof()) return '\255';
     if(skipCommentLines && c=='#') { skipRestOfLine(is); continue; }
     if(skipSymbols && !contains(skipSymbols, c)) break;
     if(stopSymbols && contains(stopSymbols, c)) break;

rai/Gui/opengl.cpp

@@ -680,6 +680,7 @@ void glColor(float r, float g, float b, float alpha) {
 void glColor(float* rgb) { glColor(rgb[0], rgb[1], rgb[2], 1.); }
 
 void glColor(const arr& col) {
+  if(col.N==1) glColor(col.p[0], col.p[0], col.p[0], 1.);
   if(col.N==3) glColor(col.p[0], col.p[1], col.p[2], 1.);
   if(col.N==4) glColor(col.p[0], col.p[1], col.p[2], col.p[3]);
 }

rai/Kin/featureSymbols.cpp

@@ -236,14 +236,14 @@ shared_ptr<Feature> symbols2feature(FeatureSymbol feat, const StringA& frames, c
     else f->frameIDs = C.getFrameIDs(frames);
   }
 
-  if(!!scale) {
+  if(!!scale && scale.N) {
     if(!f->scale.N) f->scale = scale;
     else if(scale.N==1) f->scale *= scale.scalar();
     else if(scale.N==f->scale.N) f->scale *= scale.scalar();
     else NIY;
   }
 
-  if(!!target) f->target = target;
+  if(!!target && target.N) f->target = target;
 
   if(order>=0) f->order = order;
 

rai/Kin/frame.cpp

@@ -166,12 +166,12 @@ void rai::Frame::_state_updateAfterTouchingQ() {
 
 void rai::Frame::getRigidSubFrames(FrameL& F, bool includeRigidJoints) const {
   for(Frame* child:children)
-    if(!child->joint || (includeRigidJoints && child->joint->type==JT_rigid)) { F.append(child); child->getRigidSubFrames(F, includeRigidJoints); }
+    if(!child->joint || /*!child->joint->active ||*/ (includeRigidJoints && child->joint->type==JT_rigid)) { F.append(child); child->getRigidSubFrames(F, includeRigidJoints); }
 }
 
 void rai::Frame::getPartSubFrames(FrameL& F) const {
   for(Frame* child:children)
-    if(!child->joint || !child->joint->isPartBreak()) { F.append(child); child->getPartSubFrames(F); }
+    if(!child->joint || /*!child->joint->active ||*/ !child->joint->isPartBreak()) { F.append(child); child->getPartSubFrames(F); }
 }
 
 void rai::Frame::getSubtree(FrameL& F) const {
@@ -933,6 +933,10 @@ void rai::Joint::setDofs(const arr& q_full, uint _qIndex) {
   for(Joint* j:mimicers){
     if(type!=JT_tau){
       j->frame->Q = Q;
+      if(j->scale==-1.){
+        j->frame->Q.pos = -j->frame->Q.pos;
+        j->frame->Q.rot.invert();
+      }
       j->frame->_state_setXBadinBranch();
     }else{
       j->frame->tau = frame->tau;
@@ -1202,7 +1206,8 @@ bool rai::Joint::isPartBreak() {
   //    return (dim!=1 && !mimic) || type==JT_tau;
 }
 
-double& rai::Joint::getQ() {
+double& rai::Joint::get_q() {
+  if(!active) return frame->C.qInactive.elem(qIndex);
   return frame->C.q.elem(qIndex);
 }
 
@@ -1556,6 +1561,15 @@ void rai::Shape::glDraw(OpenGL& gl) {
 
     if(_type==rai::ST_marker) {
       if(!gl.drawOptions.drawVisualsOnly) {
+#if 1
+        rai::Vector p=0;
+        if(frame.parent) p=frame.parent->ensure_X().pos;
+        p = p / frame.ensure_X();
+        glBegin(GL_LINES);
+        glVertex3f(0, 0, 0);
+        glVertex3f(p.x, p.y, p.z);
+        glEnd();
+#endif
         double s=1.;
         if(size.N) s = size.last();
         if(s>0.){

rai/Kin/frame.h

@@ -261,8 +261,8 @@ struct Joint : Dof, NonCopyable {
 
   bool isPartBreak();
 
-  //access the K's q vector
-  double& getQ();
+  //access the C's q vector
+  double& get_q();
 
   //structural operations
   void makeRigid();

rai/Kin/kin.cpp

@@ -2833,7 +2833,7 @@ void Configuration::readFromGraph(const Graph& G, bool addInsteadOfClear) {
     j->read(*f->ats);
   }
 
-  //if the joint is coupled to another:
+  // mimic joints: if the joint is coupled to another:
   {
     Joint* j;
     for(Frame* f: frames) if((j=f->joint) && j->mimic==(Joint*)1) {
@@ -2853,6 +2853,7 @@ void Configuration::readFromGraph(const Graph& G, bool addInsteadOfClear) {
         if(!j->mimic) HALT("The joint '" <<*j <<"' is declared mimicking '" <<jointName <<"' -- but that doesn't exist!");
         j->type = j->mimic->type;
         j->q0 = j->mimic->q0;
+        j->active = j->mimic->active;
         j->setDofs(j->q0, 0);
 
         delete mim;
@@ -2981,18 +2982,12 @@ void Configuration::setJacModeAs(const arr& J){
 }
 
 /// return a feature for a given frame(s)
-std::shared_ptr<Feature> Configuration::feature(FeatureSymbol fs, const StringA& frames) const {
-  return symbols2feature(fs, frames, *this);
+std::shared_ptr<Feature> Configuration::feature(FeatureSymbol fs, const StringA& frames, const arr& scale, const arr& target, int order) const {
+  return symbols2feature(fs, frames, *this, scale, target, order);
 }
 
-/// evaluate a feature for a given frame(s)
-arr Configuration::evalFeature(FeatureSymbol fs, const StringA& frames) const {
-  std::shared_ptr<Feature> feat = feature(fs, frames);
-  return feat->eval(feat->getFrames(*this));
-}
-
-arr Configuration::eval(FeatureSymbol fs, const StringA& frames){
-  return feature(fs,frames)->eval(getFrames(frames));
+arr Configuration::eval(FeatureSymbol fs, const StringA& frames, const arr& scale, const arr& target, int order){
+  return feature(fs, frames, scale, target, order)->eval(getFrames(frames));
 }
 
 /// Compute the new configuration q such that body is located at ytarget (with deplacement rel).
@@ -3427,17 +3422,15 @@ void _glDrawOdeWorld(dWorldID world)
 }
 */
 
-int animateConfiguration(Configuration& C, Inotify* ino) {
+int Configuration::animate(Inotify* ino) {
   arr x, x0;
-  x0 = C.getJointState();
-  arr lim = C.getLimits();
+  x0 = getJointState();
+  arr lim = getLimits();
   const int steps = 50;
-  C.checkConsistency();
-  StringA jointNames = C.getJointNames();
-
-  //  uint saveCount=0;
+  checkConsistency();
+  StringA jointNames = getJointNames();
 
-  C.viewer()->resetPressedKey();
+  viewer()->resetPressedKey();
   for(uint i=x0.N; i--;) {
     x=x0;
     double upper_lim = lim(i, 1);
@@ -3454,18 +3447,18 @@ int animateConfiguration(Configuration& C, Inotify* ino) {
       x(i) = center + (delta*(0.5*cos(RAI_2PI*t/steps + offset)));
       // Joint limits
       checkNan(x);
-      C.setJointState(x);
-      int key = C.view(false, STRING("DOF = " <<i <<" : " <<jointNames(i) <<lim[i]));
+      setJointState(x);
+      int key = view(false, STRING("DOF = " <<i <<" : " <<jointNames(i) <<lim[i]));
 
       if(key==13 || key==27 || key=='q') {
-        C.setJointState(x0);
+        setJointState(x0);
         return key;
       }
       wait(0.01);
     }
   }
-  C.setJointState(x0);
-  return C.view(true);
+  setJointState(x0);
+  return view(true);
 }
 
 Frame* movingBody=nullptr;
@@ -3626,28 +3619,56 @@ struct EditConfigurationKeyCall:OpenGL::GLKeyCall {
   }
 };
 
-void editConfiguration(const char* filename, Configuration& C) {
-  C.checkConsistency();
+void Configuration::watchFile(const char* filename) {
+  checkConsistency();
 
   //  gl.exitkeys="1234567890qhjklias, "; //TODO: move the key handling to the keyCall!
   bool exit=false;
   //  gl.addHoverCall(new EditConfigurationHoverCall(K));
-  C.gl().addKeyCall(new EditConfigurationKeyCall(C,exit));
-  C.gl().addClickCall(new EditConfigurationClickCall(C));
-  //  C.gl()->ensure_gl().reportEvents=true;
+  gl().addKeyCall(new EditConfigurationKeyCall(*this,exit));
+  gl().addClickCall(new EditConfigurationClickCall(*this));
+  //  gl()->ensure_gl().reportEvents=true;
   Inotify ino(filename);
   for(; !exit;) {
-    cout <<"watching..." <<endl;
+    //-- LOADING
+    LOG(0) <<"reloading `" <<filename <<"' ... ";
+    {
+      FileToken file(filename, true);
+      Graph G;
+      try {
+        lineCount=1;
+        G.read(file);
+        G.checkConsistency();
+      } catch(std::runtime_error& err) {
+        LOG(0) <<"g-File Synax Error line " <<lineCount <<": " <<err.what() <<" -- please check the file and re-save";
+      }
+
+      Configuration C_tmp;
+      try {
+        C_tmp.readFromGraph(G);
+        {
+          gl().dataLock(RAI_HERE);
+          copy(C_tmp, false);
+        }
+        report();
+      } catch(std::runtime_error& err) {
+        LOG(0) <<"Configuration initialization failed: " <<err.what() <<" -- please check the file and re-save";
+      }
+      file.cd_start(); //important: also on crash - cd back to original
+    }
+
+    //-- WATCHING
+    LOG(0) <<"watching...";
     int key = -1;
-    C.viewer()->recopyMeshes(C);
-    C.viewer()->resetPressedKey();
-    C.viewer()->drawText = "waiting for file change ('h' for help)";
+    viewer()->recopyMeshes(*this);
+    viewer()->resetPressedKey();
+    viewer()->drawText = "waiting for file change ('h' for help)";
     for(;;) {
-      key = C.view(false);
+      key = view(false);
       //if(key) cout <<"*** KEY:" <<key <<endl;
       if(key==13 || key==27 || key=='q') break;
       if(key=='h'){
-        C.viewer()->drawText = "HELP:\n"
+        viewer()->drawText = "HELP:\n"
                       "RIGHT CLICK - set focus point (move view and set center of rotation)\n"
                       "LEFT CLICK - rotate (ball; or around z at view rim)\n"
                       "q - quit\n"
@@ -3664,41 +3685,19 @@ void editConfiguration(const char* filename, Configuration& C) {
       wait(.2);
     }
     if(exit) break;
+
+    //-- ANIMATING
     if(key==13) {
-      cout <<"animating.." <<endl;
+      LOG(0) <<"animating..";
       //while(ino.pollForModification());
-      key = animateConfiguration(C, &ino);
+      key = animate(&ino);
     }
     //if(key) cout <<"*** KEYout:" <<key <<endl;
     if(key==27 || key=='q') break;
     if(key==-1) continue;
-
     if(!getInteractivity()) {
       exit=true;
     }
-
-    cout <<"reloading `" <<filename <<"' ... " <<endl;
-    Configuration C_tmp;
-    {
-      FileToken file(filename, true);
-      Graph G;
-      try {
-        lineCount=1;
-        G.read(file);
-        G.checkConsistency();
-      } catch(std::runtime_error& err) {
-        cout <<"g-File Synax Error line " <<lineCount <<": " <<err.what() <<" -- please check the file and re-save" <<endl;
-      }
-      try {
-        C_tmp.readFromGraph(G);
-        C = C_tmp;
-        C.report();
-      } catch(std::runtime_error& err) {
-        cout <<"Configuration initialization failed: " <<err.what() <<" -- please check the file and re-save" <<endl;
-      }
-      file.cd_start(); //important: also on crash - cd back to original
-    }
-
   }
 }
 

rai/Kin/kin.h

@@ -220,10 +220,9 @@ struct Configuration : GLDrawer {
   void setJacModeAs(const arr& J);
 
   /// @name features
-  shared_ptr<Feature> feature(FeatureSymbol fs, const StringA& frames= {}) const;
-  arr evalFeature(FeatureSymbol fs, const StringA& frames= {}) const;
+  shared_ptr<Feature> feature(FeatureSymbol fs, const StringA& frames={}, const arr& scale=NoArr, const arr& target=NoArr, int order=-1) const;
   template<class T> arr eval(const StringA& frames= {}){ return T().eval(getFrames(frames)); }
-  arr eval(FeatureSymbol fs, const StringA& frames= {});
+  arr eval(FeatureSymbol fs, const StringA& frames= {}, const arr& scale=NoArr, const arr& target=NoArr, int order=-1);
 
   /// @name high level inverse kinematics
   void inverseKinematicsPos(Frame& frame, const arr& ytarget, const Vector& rel_offset=NoVector, int max_iter=3);
@@ -270,6 +269,9 @@ struct Configuration : GLDrawer {
   Graph getGraph() const;
   void displayDot();
 
+  void watchFile(const char* filename);
+  int animate(struct Inotify* ino=nullptr);
+
   //some info
   void report(std::ostream& os=cout) const;
   void reportProxies(std::ostream& os=cout, double belowMargin=1., bool brief=true) const;
@@ -278,7 +280,6 @@ struct Configuration : GLDrawer {
 private:
   void readFromGraph(const Graph& G, bool addInsteadOfClear=false);
   friend struct KinematicSwitch;
-  friend void editConfiguration(const char* orsfile, Configuration& G);
 };
 
 stdPipes(Configuration)
@@ -302,7 +303,4 @@ void computeOptimalSSBoxes(FrameL& frames);
 void computeMeshNormals(FrameL& frames, bool force=false);
 void computeMeshGraphs(FrameL& frames, bool force=false);
 
-void editConfiguration(const char* orsfile, Configuration& G);
-int animateConfiguration(Configuration& G, struct Inotify* ino=nullptr);
-
 } //namespace rai