calibPoints with joint optimization; rai renamings

CMakeLists.txt

@@ -200,12 +200,12 @@ if(USE_PYBIND)
   add_definitions(-DRAI_PYBIND)
   include_directories(${pybind11_INCLUDE_DIRS})
 
-  pybind11_add_module(ry SHARED
+  pybind11_add_module(_robotic SHARED
     rai/src/ry/unity.cxx
-    src/BotOp/pyBot.cxx
+    src/BotOp/py-BotOp.cxx
   )
 
-  target_link_libraries(ry PRIVATE
+  target_link_libraries(_robotic PRIVATE
     rai
   )
 endif()
@@ -219,6 +219,11 @@ message(STATUS "[rai] compiler flags: " "${_defs}")
 
 ################################################################################
 
+add_custom_target(docstrings #ALL
+  DEPENDS _robotic
+  COMMAND pybind11-stubgen --ignore-invalid=all _robotic
+  COMMAND mv stubs/_robotic*/__init__.pyi _robotic.pyi)
+
 add_executable(kinEdit rai/bin/src_kinEdit/main.cpp)
 target_link_libraries(kinEdit rai)
 
@@ -235,7 +240,7 @@ set(PYTHON_SITE "lib/python${PYBIND11_PYTHON_VERSION}/site-packages")
 message(STATUS "[rai] installing python packages in " ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITE}/robotic )
 
 install(
-  TARGETS ry
+  TARGETS _robotic
   DESTINATION ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITE}/robotic)
 
 file(GLOB sources rai/src/* src/*)

config.mk

@@ -27,7 +27,7 @@ CERES = 0
 NLOPT = 0
 IPOPT = 0
 
-## we use the following collision/physics libraries by default, but can be disabled
+## we can use the following collision/physics libraries, some disabled by default
 #FCL = 0
 #PHYSX = 0
 BULLET = 0

retired/perception/calibration/main.cpp

@@ -82,11 +82,11 @@ void online(){
     cout <<"CALIB:" <<calib <<endl;
 
     {
-      arr fxypxy = OP.cam_Fxypxy.get();
+      arr fxycxy = OP.cam_fxycxy.get();
       auto armCalib = OP.armPoseCalib.set();
       armCalib->setZero();
-      armCalib->operator()(0,0) = calib(1)/fxypxy(1);
-      armCalib->operator()(0,1) = calib(0)/fxypxy(0);
+      armCalib->operator()(0,0) = calib(1)/fxycxy(1);
+      armCalib->operator()(0,1) = calib(0)/fxycxy(0);
 //      armCalib->operator()(0,2) = .2* calib(2);
       armCalib->operator()(0,5) = calib(2);
     }
@@ -151,7 +151,7 @@ void online(){
      <<"  mean depth=" <<d
     <<"  error="<<cameraHeight - (tableHeight+d) <<endl;
 
-  Depth2PointCloud doPcl(doCamera.depth, doCamera.getFxypxy()); //(0), doCamera.depth_fxypxy(1), doCamera.depth_fxypxy(2), doCamera.depth_fxypxy(3));
+  Depth2PointCloud doPcl(doCamera.depth, doCamera.getFxycxy()); //(0), doCamera.depth_fxycxy(1), doCamera.depth_fxycxy(2), doCamera.depth_fxycxy(3));
   PointCloudViewer pcview(doPcl.points, doCamera.color);
   doPcl.points.waitForNextRevision(10);
 

retired/perception/objectDetection/main.cpp

@@ -16,7 +16,7 @@ void testBGSThread() {
   LOG(0) <<"..done";
 
   RS.depth.waitForNextRevision();
-  Depth2PointCloud cvt2pcl(RS.depth, RS.depth_fxypxy(0), RS.depth_fxypxy(1), RS.depth_fxypxy(2), RS.depth_fxypxy(3));
+  Depth2PointCloud cvt2pcl(RS.depth, RS.depth_fxycxy(0), RS.depth_fxycxy(1), RS.depth_fxycxy(2), RS.depth_fxycxy(3));
   PointCloudViewer pcview(cvt2pcl.points, RS.color, 1.);
 
   Var<floatA> cameraDepth(RS.depth);

retired/perception/registration/main.cpp

@@ -16,7 +16,7 @@ void online(){
   Var<floatA> cam_depth; //camera output
   Var<byteA> cam_color;  //camera output
   Var<arr> cam_pose;     //camera pose
-  Var<arr> cam_Fxypxy;   //camera parameters
+  Var<arr> cam_fxycxy;   //camera parameters
 
   Var<arr> regError;
 
@@ -25,8 +25,8 @@ void online(){
   //-- camera thread
   auto cam = make_shared<RealSenseThread>(cam_color, cam_depth);
   cam_depth.waitForNextRevision();
-  cam_Fxypxy.set() = cam->depth_fxypxy;
-  cout <<"RS calib=" <<cam_Fxypxy.get()() <<endl;
+  cam_fxycxy.set() = cam->depth_fxycxy;
+  cout <<"RS calib=" <<cam_fxycxy.get()() <<endl;
 
   //methods
   ExplainBackground exBackground;
@@ -37,7 +37,7 @@ void online(){
   byteA labels;
   byteA _cam_color;
   floatA _cam_depth;
-  arr _cam_fxypxy, _cam_pose;
+  arr _cam_fxycxy, _cam_pose;
   int cL=80,cR=110,cT=60,cB=100;
 
   for(uint k=0;;k++){
@@ -48,10 +48,10 @@ void online(){
     //-- crop
     _cam_color = cam_color.get()->sub(cT,-cB-1,cL,-cR-1,0,-1);
     _cam_depth = cam_depth.get()->sub(cT,-cB-1,cL,-cR-1);
-    _cam_fxypxy = cam_Fxypxy.get();
+    _cam_fxycxy = cam_fxycxy.get();
     _cam_pose = cam_pose.get();
-    _cam_fxypxy(2) -= cL;
-    _cam_fxypxy(3) -= cT;
+    _cam_fxycxy(2) -= cL;
+    _cam_fxycxy(3) -= cT;
 
     pixelColorNormalizeIntensity(_cam_color);
 
@@ -68,7 +68,7 @@ void online(){
 
   //extract object from cluster
   FlatPercept& flat = exNovel.flats.first();
-  ptr<Object> obj = createObjectFromPercept(flat, labels, _cam_color, _cam_depth, _cam_pose, _cam_fxypxy, OT_box);
+  ptr<Object> obj = createObjectFromPercept(flat, labels, _cam_color, _cam_depth, _cam_pose, _cam_fxycxy, OT_box);
 
   cout <<"CREATED OBJECT: " <<*obj <<endl;
 
@@ -80,10 +80,10 @@ void online(){
     //-- crop
     _cam_color = cam_color.get()->sub(cT,-cB-1,cL,-cR-1,0,-1);
     _cam_depth = cam_depth.get()->sub(cT,-cB-1,cL,-cR-1);
-    _cam_fxypxy = cam_Fxypxy.get();
+    _cam_fxycxy = cam_fxycxy.get();
     _cam_pose = cam_pose.get();
-    _cam_fxypxy(2) -= cL;
-    _cam_fxypxy(3) -= cT;
+    _cam_fxycxy(2) -= cL;
+    _cam_fxycxy(3) -= cT;
 
     pixelColorNormalizeIntensity(_cam_color);
 

retired/perception/simulateCam/main.cpp

@@ -83,7 +83,7 @@ void test_pickAndPlace(){
   K.optimizeTree();
   Var<arr> cameraPose;
   cameraPose.set() = K["camera"]->X.getArr7d();
-  Var<arr> cameraFxypxy;
+  Var<arr> cameraFxycxy;
 
 
   Var<CtrlMsg> ctrl;
@@ -108,7 +108,7 @@ void test_pickAndPlace(){
   //-- camera simulation
   rai::Sim_CameraView CV(RK, .05, "camera");
   auto sen = CV.C.currentSensor;
-  cameraFxypxy.set() = ARR(sen->cam.focalLength*sen->height, sen->cam.focalLength*sen->height, .5*sen->width, .5*sen->height);
+  cameraFxycxy.set() = ARR(sen->cam.focalLength*sen->height, sen->cam.focalLength*sen->height, .5*sen->width, .5*sen->height);
 
   //-- pipeline: depth -> point cloud
 //  Depth2PointCloud depth2pc(CV.depth, CV.C.currentSensor->cam.focalLength*300);
@@ -150,7 +150,7 @@ void test_pickAndPlace(){
 //    return 0;
 //  });
 
-  CV_BackgroundSubstraction_Thread BS(CV.color, CV.depth, robotMask.color, cameraPose, cameraFxypxy, 1);
+  CV_BackgroundSubstraction_Thread BS(CV.color, CV.depth, robotMask.color, cameraPose, cameraFxycxy, 1);
 
   cout <<"WAITING FOR BACKGROUND..." <<flush;
   CV.depth.waitForRevisionGreaterThan(20);

src/BotOp/bot.cpp

@@ -28,11 +28,17 @@ BotOp::BotOp(rai::Configuration& C, bool useRealRobot){
   bool useGripper = rai::getParameter<bool>("bot/useGripper", true);
   bool robotiq = rai::getParameter<bool>("bot/useRobotiq", false);
   rai::String useArm = rai::getParameter<rai::String>("bot/useArm", "left");
+  bool blockRealRobot = rai::getParameter<bool>("bot/blockRealRobot", false);
 
   C.ensure_indexedJoints();
   qHome = C.getJointState();
   state.set()->initZero(qHome.N);
 
+  if(blockRealRobot && useRealRobot){
+    LOG(0) <<"-- blocking useRealRobot -- ";
+    useRealRobot=false;
+  }
+
   //-- launch robots & grippers
   if(useRealRobot){
     LOG(0) <<"CONNECTING TO FRANKAS";
@@ -388,7 +394,7 @@ void BotOp::getImageAndDepth(byteA& image, floatA& depth, const char* sensor){
   cam->getImageAndDepth(image, depth);
 }
 
-arr BotOp::getCameraFxypxy(const char* sensor){
+arr BotOp::getCameraFxycxy(const char* sensor){
   auto cam = getCamera(sensor);
   return cam->getFxycxy();
 }

src/BotOp/bot.h

@@ -65,7 +65,7 @@ struct BotOp{
   //-- camera commands
   void getImageAndDepth(byteA& image, floatA& depth, const char* sensor);
   void getImageDepthPcl(byteA& image, floatA& depth, arr& points, const char* sensor, bool globalCoordinates=false);
-  arr  getCameraFxypxy(const char* sensor);
+  arr  getCameraFxycxy(const char* sensor);
 
   //-- sync the user's C with the robot, update the display, return pressed key
   int sync(rai::Configuration& C, double waitTime=.1);

src/BotOp/pyBot.cxx → src/BotOp/py-BotOp.cxx

@@ -8,7 +8,7 @@
 
 #ifdef RAI_PYBIND
 
-#include "pyBot.h"
+#include "py-BotOp.h"
 
 #include <ry/types.h>
 
@@ -24,7 +24,7 @@
 
 void init_BotOp(pybind11::module& m) {
 
-  pybind11::class_<BotOp, shared_ptr<BotOp>>(m, "BotOp", "needs some docu!")
+  pybind11::class_<BotOp, shared_ptr<BotOp>>(m, "BotOp", "Robot Operation interface -- see https://marctoussaint.github.io/robotics-course/tutorials/1b-botop.html")
 
   .def(pybind11::init<rai::Configuration&, bool>(),
        "constructor",
@@ -114,7 +114,7 @@ void init_BotOp(pybind11::module& m) {
        "returns if gripper is done",
        pybind11::arg("leftRight"))
 
-  .def("getCameraFxypxy", &BotOp::getCameraFxypxy,
+  .def("getCameraFxycxy", &BotOp::getCameraFxycxy,
        "returns camera intrinsics",
        pybind11::arg("sensorName"))
 

src/BotOp/simulation.cpp

@@ -117,8 +117,11 @@ void BotThreadedSim::step(){
     P_compliance = cmdGet->P_compliance;
   }
 
+  if(cmd_q_ref.N && cmd_qDot_ref.N){
   sim->step((cmd_q_ref, cmd_qDot_ref), tau, sim->_posVel);
-//    sim->step({}, tau, sim->_none);
+  }else{
+    sim->step({}, tau, sim->_none);
+  }
   q_real = simConfig.getJointState();
   if(cmd_qDot_ref.N==qDot_real.N) qDot_real = cmd_qDot_ref;
 

src/BotOp/tools.cpp

@@ -39,11 +39,11 @@ bool sense_HsvBlob(BotOp& bot, rai::Configuration& C, const char* camName, const
 
   arr x;
   if(Pinv.N){
-    makeHomogeneousImageCoordinate(u, img.d0);
+    makeHomogeneousImageCoordinate(u);
     x = Pinv*u;
   }else{
     x = u;
-    depthData2point(x, bot.getCameraFxypxy(camName));
+    depthData2point(x, bot.getCameraFxycxy(camName));
   }
   if(verbose>0) LOG(0)  <<"dot in cam coords: " <<x;
   C[camName]->get_X().applyOnPoint(x);

src/FlatVision/flatVision.cpp

@@ -55,8 +55,8 @@ void FlatVisionThread::step(){
   // TODO this assumes that the calibration was done with the already cropped image!
   // This of course can be fixed easily in the project method, which should get the cropping parameters
 
-//  _cam_fxypxy(2) -= cL;
-//  _cam_fxypxy(3) -= cT;
+//  _cam_fxycxy(2) -= cL;
+//  _cam_fxycxy(3) -= cT;
 
 
   //-- background
@@ -70,32 +70,32 @@ void FlatVisionThread::step(){
   exRobot.compute(labels, _cam_color, _cam_depth, _model_segments, _model_depth);
   {
     //  cout <<"calib_L=" <<exRobot.calib <<endl;
-    arr fxypxy = cam_Fxypxy.get();
+    arr fxycxy = cam_fxycxy.get();
     auto armCalib = armPoseCalib.set();
     armCalib()[0] = exRobot.calib;
-    armCalib()(0,0) /= fxypxy(0);
-    armCalib()(0,1) /= fxypxy(1);
+    armCalib()(0,0) /= fxycxy(0);
+    armCalib()(0,1) /= fxycxy(1);
   }
 
   objectManager.displayLabelPCL(exRobot.label,
                                 labels, _cam_depth,
-                                _cam_pose, _cam_fxypxy,
+                                _cam_pose, _cam_fxycxy,
                                 config.set());
 
   exRobot.label=PixelLabel(PL_robot|1);
   exRobot.compute(labels, _cam_color, _cam_depth, _model_segments, _model_depth);
   {
     //  cout <<"calib_R=" <<exRobot.calib <<endl;
-    arr fxypxy = cam_Fxypxy.get();
+    arr fxycxy = cam_fxycxy.get();
     auto armCalib = armPoseCalib.set();
     armCalib()[1] = exRobot.calib;
-    armCalib()(1,0) /= fxypxy(0);
-    armCalib()(1,1) /= fxypxy(1);
+    armCalib()(1,0) /= fxycxy(0);
+    armCalib()(1,1) /= fxycxy(1);
   }
 
   objectManager.displayLabelPCL(exRobot.label,
                                 labels, _cam_depth,
-                                _cam_pose, _cam_fxypxy,
+                                _cam_pose, _cam_fxycxy,
                                 config.set());
 #endif
 
@@ -105,7 +105,7 @@ void FlatVisionThread::step(){
   exObject.verbose=1;
   exObject.compute(labels, _cam_color, _cam_depth, _model_segments, _model_depth);
   {
-    objectManager.updateObjectPose(exObject.label, exObject.calib, _cam_fxypxy);
+    objectManager.updateObjectPose(exObject.label, exObject.calib, _cam_fxycxy);
   }
 
   objectManager.explainObjectPixels(labels, _cam_color, _cam_depth, _model_segments, _model_depth);
@@ -147,12 +147,12 @@ void FlatVisionThread::step(){
 //  }
 
 //  //make objects from novel flat percepts
-//  CHECK(_cam_fxypxy.N, "need camera calibration parameters");
+//  CHECK(_cam_fxycxy.N, "need camera calibration parameters");
 //  if(true){
 //    auto P = percepts.set();
 //    P->clear();
 //    for(FlatPercept& flat : exNovel.flats){
-//      P->append( novelPerceptToObject(flat, labels, _cam_depth, _cam_pose, _cam_fxypxy) );
+//      P->append( novelPerceptToObject(flat, labels, _cam_depth, _cam_pose, _cam_fxycxy) );
 //    }
 //  }
 }