minor on LGP komo interface

MarcToussaint · May 10, 2018 · 57ae738 · 57ae738
1 parent c9bc307
commit 57ae738
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 8 deletions.
diff --git a/rai/KOMO/komo.cpp b/rai/KOMO/komo.cpp
@@ -820,7 +820,7 @@ void KOMO::setSkeleton(const Skeleton &S){
       continue;
     }
 
-    if(s.symbols(0)=="push")                  setPush(s.phase0, s.phase1+1., s.symbols(1), s.symbols(2), s.symbols(3), verbose); //TODO: the +1. assumes pushes always have duration 1
+    if(s.symbols(0)=="push")                       setPush(s.phase0, s.phase1+1., s.symbols(1), s.symbols(2), s.symbols(3), verbose); //TODO: the +1. assumes pushes always have duration 1
     else if(s.symbols(0)=="place" && s.symbols.N==3) setPlace(s.phase0, NULL, s.symbols(1), s.symbols(2), verbose);
     else if(s.symbols(0)=="place" && s.symbols.N==4) setPlace(s.phase0, s.symbols(1), s.symbols(2), s.symbols(3), verbose);
     else if(s.symbols(0)=="graspSlide")            setGraspSlide(s.phase0, s.symbols(1), s.symbols(2), s.symbols(3), verbose);
@@ -844,7 +844,6 @@ void KOMO::setAlign(double startTime, double endTime, const char* shape, const a
 }
 
 void KOMO::core_setTouch(double startTime, double endTime, const char* shape1, const char* shape2, ObjectiveType type, const arr& target, double prec){
-//  setTask(startTime, endTime, new TM_GJK(world, shape1, shape2, true), type, target, prec);
   setTask(startTime, endTime, new TM_PairCollision(world, shape1, shape2, true, false), type, target, prec);
 }
 
@@ -856,11 +855,10 @@ void KOMO::core_setInside(double startTime, double endTime, const char* shape1,
   setTask(startTime, endTime, new TM_InsideBox(world, shape1, NoVector, shape2), OT_ineq, NoArr, prec);
 }
 
-void KOMO::core_setImpulse(double time, const char* shape1, const char* shape2){
-//    setTask(time, time, new TM_ImpulsExchange(world, *symbols(1), *symbols(2)), OT_sumOfSqr, {}, 1e3, 2, +1); //+1 deltaStep indicates moved 1 time slot backward (to cover switch)
+void KOMO::core_setImpulse(double time, const char* shape1, const char* shape2, ObjectiveType type, double prec){
 //    setTask(time, time, new TM_ImpulsExchange(world, a, b), OT_sumOfSqr, {}, 1e3, 2, +1); //+1 deltaStep indicates moved 1 time slot backward (to cover switch)
   if(k_order>=2){
-    setTask(time, time, new TM_ImpulsExchange(world, shape1, shape2), OT_eq, {}, 1e2, 2, +1); //+1 deltaStep indicates moved 1 time slot backward (to cover switch)
+    setTask(time, time, new TM_ImpulsExchange(world, shape1, shape2), type, {}, prec, 2, +1); //+1 deltaStep indicates moved 1 time slot backward (to cover switch)
     setFlag(time, new Flag(FL_impulseExchange, world[shape1]->ID), +1);
     setFlag(time, new Flag(FL_impulseExchange, world[shape2]->ID), +1);
   }

diff --git a/rai/KOMO/komo.h b/rai/KOMO/komo.h
@@ -126,7 +126,7 @@ struct KOMO{
   void core_setTouch(double startTime, double endTime, const char* shape1, const char* shape2, ObjectiveType type=OT_eq, const arr& target=NoArr, double prec=1e2);
   void core_setAbove(double startTime, double endTime, const char* shape1, const char* shape2, double prec=1e2);
   void core_setInside(double startTime, double endTime, const char* shape1, const char* shape2, double prec=1e2);
-  void core_setImpulse(double time, const char* shape1, const char* shape2);
+  void core_setImpulse(double time, const char* shape1, const char* shape2, ObjectiveType type=OT_eq, double prec=1e2);
 
   //-- core kinematic switch symbols of skeletons
   void core_setKSstable(double time, const char *from, const char *to);

diff --git a/rai/LGP/manipulationTree.cpp b/rai/LGP/manipulationTree.cpp
@@ -190,10 +190,10 @@ void MNode::optLevel(uint level, bool collisions){
     komo.setFixSwitchedObjects(-1., -1., 1e2);
     komo.setSquaredQuaternionNorms();
 
-#if 0
+#if 1
     Skeleton S = getSkeleton({"touch", "above", "inside", "impulse",
                               "stable", "stableOn", "dynamic", "dynamicOn",
-                              "push" });
+                              "push", "graspSlide" });
     komo.setSkeleton(S);
 #else
     if(collisions) komo.setCollisions(false);