use pos ref for force-based grasping in sim

src/Kin/frame.cpp

@@ -1324,13 +1324,13 @@ void rai::Joint::setDofs(const arr& q_full, uint _qIndex) {
   }
 
   //copy into C state
-  // if(frame->C._state_q_isGood && &q_full!=&frame->C.q){
-  //   if(active){
-  //     frame->C.q({qIndex, qIndex+dim-1}) = q_full;
-  //   }else{
-  //     frame->C.qInactive({qIndex, qIndex+dim-1}) = q_full;
-  //   }
-  // }
+  if(frame->C._state_q_isGood && &q_full!=&frame->C.q && &q_full!=&frame->C.qInactive){
+    if(active){
+      frame->C.q({qIndex, qIndex+dim-1}) = q_full;
+    }else{
+      frame->C.qInactive({qIndex, qIndex+dim-1}) = q_full;
+    }
+  }
 }
 
 arr rai::Joint::calcDofsFromConfig() const {
@@ -1590,6 +1590,7 @@ arr rai::Joint::get_h() const {
 }
 
 double& rai::Joint::get_q() {
+  CHECK(frame->C._state_q_isGood, "");
   if(!active) return frame->C.qInactive.elem(qIndex);
   return frame->C.q.elem(qIndex);
 }

src/Kin/kin_physx.cpp

@@ -514,9 +514,11 @@ void PhysXInterface_self::addMultiBody(rai::Frame* base) {
     addShapesAndInertia(actor, shapes, type, f);
 
     //link/motor options
-    bool noMotor = f->ats->get<bool>("noMotor", false);
-    double motorKp = f->ats->get<double>("motorKp", opt.motorKp);
-    double motorKd = f->ats->get<double>("motorKd", opt.motorKd);
+    rai::Graph *ats = f->ats.get();
+    if(f->joint->mimic) ats = f->joint->mimic->frame->ats.get();
+    bool noMotor = ats->get<bool>("noMotor", false);
+    double motorKp = ats->get<double>("motorKp", opt.motorKp);
+    double motorKd = ats->get<double>("motorKd", opt.motorKd);
     double motorLambda =  f->ats->get<double>("motorLambda", -1.);
     if(motorLambda>0.){
       double motorMass =  f->ats->get<double>("motorMass", f->inertia->mass);
@@ -1068,7 +1070,7 @@ void PhysXInterface::postAddObject(rai::Frame* f) {
   }
 }
 
-void PhysXInterface::pushMotorTargets(const rai::Configuration& C, const arr& q_ref, const arr& qDot_ref, bool setStatesInstantly) {
+void PhysXInterface::pushMotorTargets(const rai::Configuration& C, const arr& qDot_ref, bool setStatesInstantly) {
   for(rai::Frame* f:C.frames) if(f->joint && self->actors(f->ID)) {
       PxArticulationLink* actor = self->actors(f->ID)->is<PxArticulationLink>();
       if(!actor) continue;
@@ -1077,9 +1079,8 @@ void PhysXInterface::pushMotorTargets(const rai::Configuration& C, const arr& q_
 
       auto axis = self->jointAxis(f->ID);
       if(axis!=PxArticulationAxis::eCOUNT){ //only joints with drive
-        double q = (f->joint->active ? q_ref(f->joint->qIndex) : f->joint->get_q());
-        if(setStatesInstantly) joint->setJointPosition(axis, f->joint->scale*q);
-        joint->setDriveTarget(axis, f->joint->scale*q);
+        if(setStatesInstantly) joint->setJointPosition(axis, f->joint->scale*f->joint->get_q());
+        joint->setDriveTarget(axis, f->joint->scale*f->joint->get_q());
 
 	if(!!qDot_ref && qDot_ref.N) { //also setting vel reference!
 	  if(setStatesInstantly) joint->setJointVelocity(axis, f->joint->scale*qDot_ref(f->joint->qIndex));

src/Kin/kin_physx.h

@@ -35,7 +35,7 @@ struct PhysXInterface {
   void pushFrameStates(const rai::Configuration& C, const arr& frameVelocities=NoArr, bool onlyKinematic=false);
   void pullDynamicStates(rai::Configuration& C, arr& frameVelocities=NoArr);
 
-  void pushMotorTargets(const rai::Configuration& C, const arr& q_ref, const arr& qDot_ref=NoArr, bool setStatesInstantly=false);
+  void pushMotorTargets(const rai::Configuration& C, const arr& qDot_ref=NoArr, bool setStatesInstantly=false);
   void pullMotorStates(rai::Configuration& C, arr& qDot);
 
   void changeObjectType(rai::Frame* f, int type);

src/Kin/simulation.cpp

@@ -42,6 +42,14 @@ struct Simulation_self {
 
   BSplineCtrlReference ref;
 
+  struct PositionRef{ rai::Dof* dof; arr q_ref; double cap=-1.; double err=0.; uint stall_counter=0; };
+  rai::Array<PositionRef> positionRefs;
+  PositionRef* getPositionRef(rai::Dof* dof, bool create){
+    for(PositionRef& r:positionRefs) if(r.dof==dof) return &r;
+    if(create){ positionRefs.append().dof=dof; return &positionRefs(-1); }
+    return nullptr;
+  }
+
   void updateDisplayData(double _time, const Configuration& _C);
   void updateDisplayData(const byteA& _image, const floatA& _depth);
 };
@@ -164,7 +172,7 @@ void Simulation::step(const arr& u_control, double tau, ControlMode u_mode) {
       imp->modControl(*this, ucontrol, tau, u_mode);
     }
 
-  //-- define a reference depending on control mode (size zero means no reference)
+  //-- bring C in reference state depending on control mode (size zero means no reference)
   arr q_ref, qDot_ref;
   time += tau;
   if(u_mode==_none) {
@@ -203,6 +211,24 @@ void Simulation::step(const arr& u_control, double tau, ControlMode u_mode) {
     }
 
   } else NIY;
+  if(q_ref.N) C.setJointState(q_ref);
+  //more refs:
+  for(Simulation_self::PositionRef& ref:self->positionRefs){
+    CHECK_EQ(&ref.dof->frame->C, &C, "this is not a dof of this configuration!");
+    if(ref.cap<0.){
+      ref.dof->setDofs(ref.q_ref);
+    }else{
+      arr q = ref.dof->getDofState();
+      double err = euclideanDistance(q, ref.q_ref);
+      if(err<ref.cap) ref.dof->setDofs(ref.q_ref);
+      else ref.dof->setDofs(q + (ref.cap/err)*(ref.q_ref-q));
+      {
+        if(err>ref.err-1e-8) ref.stall_counter++; //else ref.stall_counter=0.;
+        ref.err = err;
+      }
+    }
+  }
+  q_ref = C.getJointState();
 
   //-- imps before physics
   for(shared_ptr<SimulationImp>& imp : imps) if(imp->when==SimulationImp::_beforePhysics) {
@@ -212,11 +238,7 @@ void Simulation::step(const arr& u_control, double tau, ControlMode u_mode) {
   //-- call the physics engine
   if(engine==_physx) {
     self->physx->pushFrameStates(C, NoArr, true); //kinematicOnly (usually none anyway)
-    if(q_ref.N) {
-      // C.setJointState(q_ref);
-      C.ensure_q();
-      self->physx->pushMotorTargets(C, q_ref); //qDot_ref, motor control
-    }
+    self->physx->pushMotorTargets(C); //qDot_ref, motor control
     self->physx->step(tau);
     self->physx->pullDynamicStates(C, self->frameVelocities);
     self->physx->pullMotorStates(C, self->qDot);
@@ -233,17 +255,15 @@ void Simulation::step(const arr& u_control, double tau, ControlMode u_mode) {
     self->bridgeC.view(false, "bullet bridge");
 #endif
   } else if(engine==_kinematic) {
-    if(q_ref.N) {
-      C.setJointState(q_ref);
-    }
+    //already done
   } else NIY;
+  C.ensure_q();
 
   //-- imps after physics
   for(shared_ptr<SimulationImp>& imp : imps) if(imp->when==SimulationImp::_afterPhysics) {
       imp->modConfiguration(*this, tau);
     }
 
-  C.ensure_q();
 
   //-- data log?
   if(writeData>0){ // && !(steps%10)){
@@ -276,6 +296,13 @@ void Simulation::setSplineRef(const arr& _x, const arr& _times, bool append) {
   else self->ref.overwriteSmooth(path, times, time);
 }
 
+void Simulation::setPositionRef(Dof* dof, const arr& q_ref, double cap){
+  auto* ref = self->getPositionRef(dof, true);
+  ref->q_ref = q_ref;
+  ref->cap = cap;
+  ref->stall_counter = 0;
+}
+
 void Simulation::resetSplineRef() {
   self->ref.initialize(C.getJointState(), NoArr, time);
 }
@@ -336,8 +363,12 @@ void Simulation::moveGripper(const char* gripperFrameName, double width, double
     if(verbose>1) LOG(1) <<"initiating opening gripper " <<gripper->name <<" (without releasing obj)" <<" width:" <<width <<" speed:" <<speed;
   }
 
+#if 0
   C.ensure_q();
   imps.append(make_shared<Imp_GripperMove>(gripper, joint, fing1, fing2, speed, width));
+#else
+  setPositionRef(joint, arr{.5*width}, .01*speed);
+#endif
 }
 
 void Simulation::closeGripper(const char* gripperFrameName, double width, double speed, double force) {
@@ -416,6 +447,7 @@ void Simulation::closeGripperGrasp(const char* gripperFrameName, const char* obj
 }
 
 bool Simulation::gripperIsDone(const char* gripperFrameName) {
+#if 0
   rai::Frame* gripper = C.getFrame(gripperFrameName);
   if(!gripper) {
     LOG(-1) <<"you passed me a non-existing gripper name!";
@@ -427,6 +459,21 @@ bool Simulation::gripperIsDone(const char* gripperFrameName) {
     if(std::dynamic_pointer_cast<Imp_CloseGripper>(imp) && std::dynamic_pointer_cast<Imp_CloseGripper>(imp)->gripper==gripper) return false;
   }
   return true;
+#else
+  rai::Frame* gripper, *fing1, *fing2;
+  rai::Joint* joint;
+  getFingersForGripper(gripper, joint, fing1, fing2, C, gripperFrameName);
+  auto* ref = self->getPositionRef(joint, false);
+  if(ref){
+    if(ref->stall_counter>3) return true;
+    arr q = ref->dof->getDofState();
+    double diff = euclideanDistance(q, ref->q_ref);
+    if(ref->cap<0.) return diff<1e-3;
+    else return diff<ref->cap;
+  }
+  LOG(0) <<"you didn't set a reference for gripper " <<gripperFrameName <<"!!";
+  return false;
+#endif
 }
 
 void Simulation::getState(arr& frameState, arr& q, arr& frameVelocities, arr& qDot) {
@@ -454,7 +501,7 @@ void Simulation::setState(const arr& frameState, const arr& q, const arr& frameV
 void Simulation::pushConfigurationToSimulator(const arr& frameVelocities, const arr& qDot) {
   if(engine==_physx) {
     self->physx->pushFrameStates(C, frameVelocities);
-    self->physx->pushMotorTargets(C, C.getJointState(), qDot, true);
+    self->physx->pushMotorTargets(C, qDot, true);
   } else if(engine==_bullet) {
     self->bullet->pushFullState(C, frameVelocities);
   } else NIY;

src/Kin/simulation.h

@@ -45,6 +45,7 @@ struct Simulation {
 
   //-- adapt the spline reference to genreate motion (default way)
   void setSplineRef(const arr& _q, const arr& _times, bool append=true);
+  void setPositionRef(rai::Dof* dof, const arr&q_ref, double cap=-1.);
   void resetSplineRef();
 
   //-- send a gripper command

src/LGP/LGP_SkeletonTool.cpp

@@ -69,7 +69,7 @@ void LGP_SkeletonTool::viewConfig() {
 
 //-- extract skeleton
 void LGP_SkeletonTool::getSkeleton(Skeleton& skeleton, String& skeletonString) {
-  skeleton.collisions=lgproot->genericCollisions;
+  skeleton.useBroadCollisions=lgproot->useBroadCollisions;
   skeleton.addExplicitCollisions(lgproot->explicitCollisions);
   skeleton.addLiftPriors(lgproot->explicitLift);
 
@@ -264,7 +264,7 @@ void LGP_SkeletonTool::report(std::ostream& os) const {
   os <<"=== LGP info ===" <<endl;
   lgproot->L.report(os);
   lgproot->C.report(os);
-  os <<"genericCollisions: " <<lgproot->genericCollisions <<endl;
+  os <<"useBroadCollisions: " <<lgproot->useBroadCollisions <<endl;
   os <<"#explicitCollisions: " <<lgproot->explicitCollisions.d0 <<endl;
   os <<"#explicitLift: " <<lgproot->explicitLift.d0 <<endl;
   os <<"================" <<endl;

src/LGP/LGP_node.cpp

@@ -122,10 +122,10 @@ void LGP_Node::expand(int verbose) {
   isExpanded=true;
 }
 
-void LGP_Node::optBound(BoundType bound, bool collisions, int verbose) {
+void LGP_Node::optBound(BoundType bound, bool useBroadCollisions, int verbose) {
   if(tree.filComputes)(*tree.filComputes) <<id <<'-' <<step <<'-' <<bound <<endl;
   ensure_skeleton();
-  skeleton->collisions = collisions;
+  skeleton->useBroadCollisions = useBroadCollisions;
   skeleton->verbose = verbose;
 
   arrA waypoints;

src/LGP/LGP_node.h

@@ -77,7 +77,7 @@ struct LGP_Node {
 
   //- computations on the node
   void expand(int verbose=0);           ///< expand this node (symbolically: compute possible decisions and add their effect nodes)
-  void optBound(BoundType bound, bool collisions=false, int verbose=-1);
+  void optBound(BoundType bound, bool useBroadCollisions=false, int verbose=-1);
   void resetData();
 
   //-- helpers to get other nodes