ry fixes for botop

rai/Control/CtrlMsgs.h

@@ -49,7 +49,7 @@ struct GripperAbstraction {
   virtual void close(double force=.0, //relative to [min,max]
                      double width=.2, //relative to [min,max]
                      double speed=.2) = 0; //relative to [min,max]
-  virtual void close(const char* objName, double force=.0, double width=.2, double speed=.2){ close(force, width, speed); }
+  virtual void closeGrasp(const char* objName, double force=.0, double width=.2, double speed=.2){ close(force, width, speed); }
   virtual double pos() = 0;
   virtual bool isDone() = 0;
 };

rai/Kin/simulation.cpp

@@ -150,6 +150,7 @@ Simulation::Simulation(Configuration& _C, Simulation::SimulatorEngine _engine, i
 }
 
 Simulation::~Simulation() {
+  LOG(0) <<"shutting down Simulation";
 }
 
 void Simulation::step(const arr& u_control, double tau, ControlMode u_mode) {
@@ -365,7 +366,7 @@ void Simulation::closeGripper(const char* gripperFrameName, double width, double
       if(p.b == finger1) fing1close.setAppend(p.a);
       if(p.a == finger2) fing2close.setAppend(p.b);
       if(p.b == finger2) fing2close.setAppend(p.a);
-      LOG(0) <<"near objects: " <<p;
+      //LOG(0) <<"near objects: " <<p;
     }
   }
 

rai/ry/ry-Config.cpp

@@ -361,90 +361,12 @@ To get really precise distances and penetrations use the FS.distance feature wit
 reloads, displays and animates the configuration whenever the file is changed"
       )
 
-  .def("komo_IK", [](shared_ptr<rai::Configuration>& self, bool useSwift) {
-//  ry::RyKOMO komo;
-    auto komo = make_shared<KOMO>();
-    komo->setModel(*self, useSwift);
-    komo->setIKOpt();
-    return komo;
-  },
-  "create KOMO solver configured to IK, useSwift determine whether for each \
-query the broadphase collision computations are done. (Necessary only when generic \
-FS.accumulatedCollision feature is needed. The explicit distance feature is independent \
-from broadphase collision computation)",
-  pybind11::arg("useSwift")
-      )
-
-  .def("komo_CGO", [](shared_ptr<rai::Configuration>& self, uint numConfigs, bool useSwift) {
-    CHECK_GE(numConfigs, 1, "");
-    auto komo = make_shared<KOMO>();
-    komo->setModel(*self, useSwift);
-    komo->setTiming(numConfigs, 1, 1., 1);
-    komo->addQuaternionNorms();
-    return komo;
-  },
-  "create KOMO solver configured for dense graph optimization, \
-numConfig gives the number of configurations optimized over, \
-useSwift determine whether for each \
-query the broadphase collision computations are done. (Necessary only when generic \
-FS.accumulatedCollision feature is needed. The explicit distance feature is independent \
-from broadphase collision computation)",
-  pybind11::arg("numConfigs"),
-  pybind11::arg("useSwift")
-      )
-
-  .def("komo_path",  [](shared_ptr<rai::Configuration>& self, double phases, uint stepsPerPhase, double timePerPhase, bool useSwift) {
-    auto komo = make_shared<KOMO>();
-    komo->setModel(*self, useSwift);
-    komo->setTiming(phases, stepsPerPhase, timePerPhase);
-    komo->add_qControlObjective({}, 2, 1.);
-    return komo;
-  },
-  "create KOMO solver configured for sparse path optimization",
-  pybind11::arg("phases"),
-  pybind11::arg("stepsPerPhase")=20,
-  pybind11::arg("timePerPhase")=5.,
-  pybind11::arg("useSwift")
-      )
-
-  .def("komo",  [](shared_ptr<rai::Configuration>& self, double phases, uint stepsPerPhase, double timePerPhase, uint k_order, bool useSwift) {
-    auto komo = make_shared<KOMO>();
-    komo->setModel(*self, useSwift);
-    komo->setTiming(phases, stepsPerPhase, timePerPhase, k_order);
-    return komo;
-  },
-  "create KOMO solver configured for sparse path optimization without control objective",
-  pybind11::arg("phases"),
-  pybind11::arg("stepsPerPhase")=20,
-  pybind11::arg("timePerPhase")=5.,
-  pybind11::arg("k_order")=2,
-  pybind11::arg("useSwift")
-      )
-
-  /*
-  .def("lgp", [](shared_ptr<rai::Configuration>& self, const std::string& folFileName) {
-    ry::RyLGP_Tree lgp;
-    lgp.lgp = make_shared<LGP_Tree_Thread>(*self, folFileName.c_str());
-    return lgp;
-  },
-  "create an LGP solver"
-      )
-
-  .def("physx", [](shared_ptr<rai::Configuration>& self) {
-    return make_shared<PhysXInterface>(*self);
-  },
-  "create a PhysX engine for physical simulation from the configuration: The configuration \
-  is being exported into a bullet instance, which can be stepped forward, and the result syced back to this configuration"
-      )
-  */
-
-  .def("bullet", [](shared_ptr<rai::Configuration>& self) {
-    return make_shared<BulletInterface>(*self);
-  },
-  "create a Bullet engine for physical simulation from the configuration: The configuration \
-  is being exported into a bullet instance, which can be stepped forward, and the result syced back to this configuration"
-      )
-
+  .def("report", [](shared_ptr<rai::Configuration>& self) {
+    rai::String str;
+    self->report(str);
+    return str;
+  }
+  )
 
   .def("simulation", [](shared_ptr<rai::Configuration>& self, rai::Simulation::SimulatorEngine engine, int verbose) {
     return make_shared<rai::Simulation>(*self, engine, verbose);
@@ -456,15 +378,6 @@ allows you to control robot motors by position, velocity, or accelerations, \
   pybind11::arg("verbose")
       )
 
-//.def("operate", [](shared_ptr<rai::Configuration>& self, const char* rosNodeName) {
-//  ry::RyOperate op;
-//  op.R = make_shared<RobotOperation>(*self, .01, rosNodeName);
-//  return op;
-//},
-//"create a module (including ROS node) to sync this configuration both ways (reading state, and controlling) to a real robot",
-//pybind11::arg("rosNodeName")
-//    )
-
   .def("sortFrames", [](shared_ptr<rai::Configuration>& self) {
     self->sortFrames();
   }, "resort the internal order of frames according to the tree topology. This is important before saving the configuration.")

rai/ry/ry-Frame.cpp

@@ -28,10 +28,15 @@ void init_Frame(pybind11::module& m) {
 
     .def("setColor", &rai::Frame::setColor )
 
-    .def("setPose", &rai::Frame::setPose )
+    .def("setPose",  [](shared_ptr<rai::Frame>& self, const char* pose){
+       self->setRelativePose(rai::Transformation(pose));
+     })
+
     .def("setPosition", &rai::Frame::setPosition )
     .def("setQuaternion", &rai::Frame::setQuaternion )
-    .def("setRelativePose", &rai::Frame::setRelativePose )
+    .def("setRelativePose", [](shared_ptr<rai::Frame>& self, const char* pose){
+       self->setRelativePose(rai::Transformation(pose));
+     })
     .def("setRelativePosition", &rai::Frame::setRelativePosition )
     .def("setRelativeQuaternion", &rai::Frame::setRelativeQuaternion )
     .def("setJoint", &rai::Frame::setJoint )

rai/ry/ry-KOMO.cpp

@@ -79,8 +79,8 @@ void init_KOMO(pybind11::module& m) {
     .def("getT", [](std::shared_ptr<KOMO>& self) { return self->T; } )
 
     .def("getFrameState", &KOMO::getConfiguration_X)
-    .def("getPath_qOrg", &KOMO::getPath_qOrg)
-    .def("getPath",  &KOMO::getPath_qAll)
+    .def("getPath", &KOMO::getPath_qOrg)
+    .def("getPath_qAll",  &KOMO::getPath_qAll)
     .def("getPathFrames", &KOMO::getPath_X)
     .def("getPathTau", &KOMO::getPath_tau)
 

rai/ry/ry.cpp

@@ -76,13 +76,13 @@ PYBIND11_MODULE(libry, m) {
 void init_enums(pybind11::module& m){
 
 #undef ENUMVAL
-#define ENUMVAL(x) .value(#x, rai::_##x)
+#define ENUMVAL(x) .value(#x, rai::x)
 
  pybind11::enum_<rai::ArgWord>(m, "ArgWord")
-    ENUMVAL(left)
-    ENUMVAL(right)
-    ENUMVAL(sequence)
-    ENUMVAL(path)
+    ENUMVAL(_left)
+    ENUMVAL(_right)
+    ENUMVAL(_sequence)
+    ENUMVAL(_path)
      .export_values();
 }