tweaks

MarcToussaint · Sep 28, 2023 · 9352193 · 9352193
1 parent 517c9b1
commit 9352193
Show file tree

Hide file tree

Showing 4 changed files with 45 additions and 39 deletions.
diff --git a/src/BotOp/bot.cpp b/src/BotOp/bot.cpp
@@ -26,51 +26,57 @@
 BotOp::BotOp(rai::Configuration& C, bool useRealRobot){
   //-- launch arm(s) & gripper(s)
   bool useGripper = rai::getParameter<bool>("bot/useGripper", true);
-  bool robotiq = rai::getParameter<bool>("bot/useRobotiq", true);
-  rai::String useArm = rai::getParameter<rai::String>("bot/useArm", "both");
+  bool robotiq = rai::getParameter<bool>("bot/useRobotiq", false);
+  rai::String useArm = rai::getParameter<rai::String>("bot/useArm", "left");
 
   C.ensure_indexedJoints();
   qHome = C.getJointState();
   state.set()->initZero(qHome.N);
 
   //-- launch robots & grippers
   if(useRealRobot){
-    LOG(0) <<"OPENING FRANKAS";
-    if(useArm=="left"){
-      robotL = make_shared<FrankaThread>(0, franka_getJointIndices(C,'l'), cmd, state);
-      if(useGripper) gripperL = make_shared<FrankaGripper>(0);
-    }else if(useArm=="right"){
-      robotR = make_shared<FrankaThread>(1, franka_getJointIndices(C,'r'), cmd, state);
-      if(useGripper) gripperR = make_shared<FrankaGripper>(1);
-    }else if(useArm=="both"){
-      robotL = make_shared<FrankaThread>(0, franka_getJointIndices(C,'l'), cmd, state);
-      robotR = make_shared<FrankaThread>(1, franka_getJointIndices(C,'r'), cmd, state);
-      if(useGripper){
-        LOG(0) <<"OPENING GRIPPERS";
-        if(robotiq){
-          gripperL = make_shared<RobotiqGripper>(0);
-          gripperR = make_shared<RobotiqGripper>(1);
-        }else{
-          gripperL = make_shared<FrankaGripper>(0);
-          gripperR = make_shared<FrankaGripper>(1);
+    LOG(0) <<"CONNECTING TO FRANKAS";
+    try{
+      if(useArm=="left"){
+        robotL = make_shared<FrankaThread>(0, franka_getJointIndices(C,'l'), cmd, state);
+        if(useGripper) gripperL = make_shared<FrankaGripper>(0);
+      }else if(useArm=="right"){
+        robotR = make_shared<FrankaThread>(1, franka_getJointIndices(C,'r'), cmd, state);
+        if(useGripper) gripperR = make_shared<FrankaGripper>(1);
+      }else if(useArm=="both"){
+        robotL = make_shared<FrankaThread>(0, franka_getJointIndices(C,'l'), cmd, state);
+        robotR = make_shared<FrankaThread>(1, franka_getJointIndices(C,'r'), cmd, state);
+        if(useGripper){
+          LOG(0) <<"CONNECTING TO GRIPPERS";
+          if(robotiq){
+            gripperL = make_shared<RobotiqGripper>(0);
+            gripperR = make_shared<RobotiqGripper>(1);
+          }else{
+            gripperL = make_shared<FrankaGripper>(0);
+            gripperR = make_shared<FrankaGripper>(1);
+          }
         }
+      }else if(useArm=="none"){
+        LOG(0) <<"starting botop without ANY robot module";
+      }else{
+        HALT("you need a botUseArm configuration (right, left, both)");
       }
-    }else if(useArm=="none"){
-      LOG(0) <<"starting botop without ANY robot module";
-    }else{
-      HALT("you need a botUseArm configuration (right, left, both)");
-    }
-    {// if using franka gripper, do a homing?
-      //FrankaGripper *fg = dynamic_cast<FrankaGripper*>(gripperL.get());
-      //if(fg) fg->homing();
+      {// if using franka gripper, do a homing?
+        //FrankaGripper *fg = dynamic_cast<FrankaGripper*>(gripperL.get());
+        //if(fg) fg->homing();
+      }
+      C.setJointState(get_q());
+    } catch(const std::exception& ex) {
+      LOG(-1) <<"Starting the real robot failed! Error msg: " <<ex.what();
+    } catch(...) {
+      LOG(-1) <<"Starting the real robot failed! Error msg: " <<rai::errString();
     }
   }else{
     double hyperSpeed = rai::getParameter<double>("botsim/hyperSpeed", 1.);
     simthread = make_shared<BotThreadedSim>(C, cmd, state, StringA(), .001, hyperSpeed); //, StringA(), .001, 10.);
     robotL = simthread;
     if(useGripper) gripperL = make_shared<GripperSim>(simthread, "l_gripper");
   }
-  C.setJointState(get_q());
 
   //-- initialize the control reference
   hold(false, true);

diff --git a/src/BotOp/pyBot.cxx b/src/BotOp/pyBot.cxx
@@ -145,7 +145,7 @@ void init_BotOp(pybind11::module& m) {
        pybind11::arg("waitTime") = .1)
 
    .def("wait", &BotOp::wait,
-        "wait until keypressed or end of spline buffer; keeps C sync'ed",
+       "repeatedly sync your workspace C until a key is pressed or motion ends (optionally)",
         pybind11::arg("C"),
         pybind11::arg("forKeyPressed") = true,
         pybind11::arg("forTimeToEnd") = true)

diff --git a/src/BotOp/simulation.cpp b/src/BotOp/simulation.cpp
@@ -120,7 +120,7 @@ void BotThreadedSim::step(){
   sim->step((cmd_q_ref, cmd_qDot_ref), tau, sim->_posVel);
 //    sim->step({}, tau, sim->_none);
   q_real = simConfig.getJointState();
-  qDot_real = cmd_qDot_ref;
+  if(cmd_qDot_ref.N==qDot_real.N) qDot_real = cmd_qDot_ref;
 
   //-- add other crazy perturbations?
 //  if((step_count%1000)<100) q_real(0) = .1;

diff --git a/test/05-moveTo/main.cpp b/test/05-moveTo/main.cpp
@@ -28,25 +28,25 @@ void test_bot() {
   arr q1 = q0;
   q1(1) += .4;
 
-//  bot.move((q1, q0, q1).reshape(-1,q0.N), {.5, .6, 2.});
-
-  rai::wait(.5);
-  bot.sync(C);
-
   for(;;){
-    bot.moveTo(q1, 1., false);
+    LOG(0) <<"adding snake";
+    bot.move((q1, q0, q1).reshape(-1,q0.N), {.1, 1., 2.});
+//    bot.moveTo(q1, 1., false);
     bot.wait(C);
     if(bot.keypressed=='q') break;
 
+    LOG(0) <<"adding home";
     bot.moveTo(q0, 1., false); //using timing cost=1
     bot.wait(C);
     if(bot.keypressed=='q') break;
   }
 
-  bot.stop(C); rai::wait();
+//  LOG(0) <<"immediate stop";
+//  bot.stop(C); rai::wait();
 
+  LOG(0) <<"immediate home";
   bot.home(C);
-  bot.wait(C, false, true);
+//  bot.wait(C, false, true);
 //  bot.home(C);
 //  bot.wait(C, true, true);
 //  if(bot.keypressed=='q') return;