Velocity controller for omnibase and gamepad module

src/BotOp/bot.cpp

@@ -13,8 +13,10 @@
 #include <Franka/FrankaGripper.h>
 #include "simulation.h"
 #include <Omnibase/omnibase.h>
+#include <Ranger/ranger.h>
 #include <Robotiq/RobotiqGripper.h>
 #include <OptiTrack/optitrack.h>
+#include <Livox/livox.h>
 #include <RealSense/RealSenseThread.h>
 #ifdef RAI_VIVE
 #  include <ViveController/vivecontroller.h>
@@ -42,12 +44,12 @@ BotOp::BotOp(rai::Configuration& C, bool useRealRobot){
   if(useRealRobot && useGripper){
     LOG(0) <<"CONNECTING TO GRIPPERS";
     try{
-      if(C.getFrame("l_panda_hand", false) && C.getFrame("r_panda_hand", false)){
+      if(C.getFrame("l_panda_finger_joint1", false) && C.getFrame("r_panda_finger_joint1", false)){
         gripperL = make_shared<FrankaGripper>(0);
         gripperR = make_shared<FrankaGripper>(1);
-      }else if(C.getFrame("l_panda_hand", false)){
+      }else if(C.getFrame("l_panda_finger_joint1", false)){
         gripperL = make_shared<FrankaGripper>(0);
-      }else if(C.getFrame("r_panda_hand", false)){
+      }else if(C.getFrame("r_panda_finger_joint1", false)){
         gripperR = make_shared<FrankaGripper>(1);
 
       }else if(C.getFrame("l_robotiq_base", false) && C.getFrame("r_robotiq_base", false)){
@@ -61,6 +63,10 @@ BotOp::BotOp(rai::Configuration& C, bool useRealRobot){
     } catch(const std::exception& ex) {
       LOG(-1) <<"Starting the gripper(s) failed! Error msg: " <<ex.what();
     }
+
+    if(!gripperL && !gripperR){
+      LOG(0) <<"WARNING: no grippers created (no frames l_panda_finger_joint1 or l_robotiq_base found in config)";
+    }
   }
 
   if(useRealRobot){
@@ -71,6 +77,7 @@ BotOp::BotOp(rai::Configuration& C, bool useRealRobot){
         robotL = make_shared<FrankaThread>(robotID++, franka_getJointIndices(C,'l'), cmd, state);
         robotR = make_shared<FrankaThread>(robotID++, franka_getJointIndices(C,'r'), cmd, state);
       } else if(C.getFrame("l_panda_base", false)){
+        std::cout << franka_getJointIndices(C,'l') << std::endl;
         robotL = make_shared<FrankaThread>(robotID++, franka_getJointIndices(C,'l'), cmd, state);
       } else if(C.getFrame("r_panda_base", false)){
         robotR = make_shared<FrankaThread>(robotID++, franka_getJointIndices(C,'r'), cmd, state);
@@ -93,6 +100,15 @@ BotOp::BotOp(rai::Configuration& C, bool useRealRobot){
       LOG(-1) <<"Starting the omnibase failed! Error msg: " <<ex.what();
     }
 
+    try{
+      if(C.getFrame("ranger_base", false)){
+        LOG(0) <<"CONNECTING TO RANGER";
+        robotR = make_shared<RangerThread>(robotID++, uintA{0,1,2}, cmd, state);
+      }
+    } catch(const std::exception& ex) {
+      LOG(-1) <<"Starting the ranger failed! Error msg: " <<ex.what();
+    }
+
   }else{
     simthread = make_shared<BotThreadedSim>(C, cmd, state);
     robotL = simthread;
@@ -112,6 +128,13 @@ BotOp::BotOp(rai::Configuration& C, bool useRealRobot){
     optitrack->pull(C);
   }
 
+  if(rai::getParameter<bool>("bot/useLivox", false)){
+    LOG(0) <<"OPENING LIVOX";
+    if(!useRealRobot) LOG(-1) <<"useLivox with real:false -- that's usually wrong!";
+    livox = make_shared<rai::Livox>();
+    livox->pull(C);
+  }
+
 #ifdef RAI_VIVE
   //-- launch ViveController
   if(rai::getParameter<bool>("bot/useViveController", false)){
@@ -127,8 +150,8 @@ BotOp::BotOp(rai::Configuration& C, bool useRealRobot){
     audio = make_shared<rai::Sound>();
   }
 
-  C.gl().setTitle("BotOp associated Configuration");
   C.view(false, STRING("time: 0"));
+  C.gl().setTitle("BotOp associated Configuration");
 }
 
 BotOp::~BotOp(){
@@ -199,6 +222,7 @@ int BotOp::sync(rai::Configuration& C, double waitTime, rai::String viewMsg){
 
   //update optitrack state
   if(optitrack) optitrack->pull(C);
+  if(livox) livox->pull(C);
 
 #ifdef RAI_VIVE
   //update vivecontroller state
@@ -221,16 +245,17 @@ int BotOp::sync(rai::Configuration& C, double waitTime, rai::String viewMsg){
   return keypressed;
 }
 
-int BotOp::wait(rai::Configuration& C, bool forKeyPressed, bool forTimeToEnd, bool forGripper){
+int BotOp::wait(rai::Configuration& C, bool forKeyPressed, bool forTimeToEnd, bool forGripper, double syncFrequency){
   C.get_viewer()->raiseWindow();
   C.get_viewer()->_resetPressedKey();
+  sync(C);
   for(;;){
-    sync(C, .1);
     //if(keypressed=='q') return keypressed;
     if(forKeyPressed && keypressed) return keypressed;
     if(forTimeToEnd && getTimeToEnd()<=0.) return keypressed;
     if(forGripper && gripperDone(rai::_left)) return 'g';
     if(!rai::getInteractivity() && !forTimeToEnd && forKeyPressed) return ' ';
+    sync(C, syncFrequency);
   }
 }
 
@@ -298,13 +323,13 @@ void BotOp::move_oldCubic(const arr& path, const arr& times, bool overwrite, dou
     arr tauInitial = {};
     if(!optTau) tauInitial = differencing(_times);
     TimingProblem timingProblem(path, {}, q, qDot, 1., 1., optTau, false, {}, tauInitial);
-    NLP_Solver solver;
+    rai::NLP_Solver solver;
     solver
         .setProblem(timingProblem.ptr())
-        .setSolver(NLPS_newton);
+        .setSolver(rai::M_Newton);
     solver.opt
-        .set_stopTolerance(1e-4)
-        .set_maxStep(1e0)
+        ->set_stopTolerance(1e-4)
+        .set_stepMax(1e0)
         .set_damping(1e-2);
     auto ret = solver.solve();
     //LOG(1) <<"timing f: " <<ret->f;
@@ -468,6 +493,22 @@ void BotOp::sound(int noteRelToC, float a, float decay){
   }
 }
 
+void BotOp::attach(str from, str to) {
+  if(!simthread){
+    LOG(-1) <<"attach only works in sim";
+  }else{
+    simthread->attach(from, to);
+  }
+}
+
+void BotOp::detach(str from, str to){
+  if(!simthread){
+    LOG(-1) <<"attach only works in sim";
+  }else{
+    simthread->detach(from, to);
+  }
+}
+
 //===========================================================================
 
 void ZeroReference::getReference(arr& q_ref, arr& qDot_ref, arr& qDDot_ref, const arr& q_real, const arr& qDot_real, double ctrlTime){

src/BotOp/bot.h

@@ -7,6 +7,7 @@
 namespace rai{
   struct GripperAbstraction;
   struct OptiTrack;
+  struct Livox;
   struct ViveController;
   struct Sound;
 }
@@ -24,6 +25,7 @@ struct BotOp{
   std::shared_ptr<rai::GripperAbstraction> gripperR;
   std::shared_ptr<rai::ReferenceFeed> ref;
   std::shared_ptr<rai::OptiTrack> optitrack;
+  std::shared_ptr<rai::Livox> livox;
   std::shared_ptr<rai::ViveController> vivecontroller;
   std::shared_ptr<rai::Sound> audio;
   std::shared_ptr<BotThreadedSim> simthread;
@@ -68,8 +70,8 @@ struct BotOp{
   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, rai::String viewMsg={});
-  int wait(rai::Configuration& C, bool forKeyPressed=true, bool forTimeToEnd=true, bool forGripper=false);
+  int sync(rai::Configuration& C, double waitTime=.05, rai::String viewMsg={});
+  int wait(rai::Configuration& C, bool forKeyPressed=true, bool forTimeToEnd=true, bool forGripper=false, double syncFrequency=.05);
 
   //-- motion macros
   void home(rai::Configuration& C);
@@ -79,6 +81,10 @@ struct BotOp{
   //-- audio
   void sound(int noteRelToC=0, float a=.5, float decay=0.0007);
 
+  //-- cheating in sim
+  void attach(str from, str to);
+  void detach(str from, str to);
+
 private:
   std::shared_ptr<rai::CameraAbstraction>& getCamera(const char* sensor);
   template<class T> BotOp& setReference();

src/Gamepad/Makefile

@@ -0,0 +1,13 @@
+BASE = ../../rai
+NAME   = $(shell basename `pwd`)
+OUTPUT = lib$(NAME).so
+
+PLIB=1
+
+OPTIM = debug
+DEPEND = Core
+
+SRCS = $(shell find . -maxdepth 1 -name '*.cpp' )
+OBJS = $(SRCS:%.cpp=%.o)
+
+include $(BASE)/makeutils/generic.mk

src/Gamepad/gamepad.cpp

@@ -0,0 +1,71 @@
+#include "gamepad.h"
+
+#ifdef RAI_PLIB
+
+#include <plib/js.h>
+#undef min
+#undef max
+
+GamepadInterface::GamepadInterface()
+    : Thread("GamepadInterface", .01) {
+    threadLoop(true);
+}
+
+GamepadInterface::~GamepadInterface(){
+    threadClose();
+}
+
+void GamepadInterface::open(){
+  jsInit();
+  for(uint i=0;;i++){
+    joystick = new jsJoystick(i);
+    if(joystick->notWorking()) break;
+    uint n=joystick->getNumAxes();
+
+    std::cout << "name     = "   << joystick->getName()
+              << "\n#axis    = " << joystick->getNumAxes()
+              // << " \n#buttons = " <<gamepad->getNumButtons()
+              << "\nerror?   = " <<joystick->notWorking()
+              << std::endl;
+
+    if(n>=4 && n<=8) break;
+    //iterate and try a new one
+    delete joystick;
+  }
+  step(); //clear the buffers...
+}
+
+void GamepadInterface::close(){
+  delete joystick;
+  gamepadState.set()->clear();
+}
+
+void GamepadInterface::step(){
+  if(joystick->notWorking()) return;
+  uint n=joystick->getNumAxes();
+  floatA A(n);
+  int B;
+  joystick->rawRead(&B, A.p);
+  gamepadState.writeAccess();
+  gamepadState().resize(n+1);
+  gamepadState()(0)=B;
+  for(uint i=0; i<n; i++) gamepadState()(i+1)=(double)A(i)/(1<<15);
+  if(step_count>20 && stopButtons(gamepadState())){
+    LOG(1) <<"*** STOP button pressed";
+//    moduleShutdown()->incrementStatus();
+    quitSignal.set()=true;
+  }
+  gamepadState.deAccess();
+
+}
+
+#else //dummy implementations
+GamepadInterface::GamepadInterface()
+    : Thread("GamepadInterfaceINACTIVE") {}
+GamepadInterface::~GamepadInterface(){ }
+void GamepadInterface::open(){ gamepadState.set()->resize(10); gamepadState.set()->setZero(); RAI_MSG("WARNING: dummy gamepad implementation"); }
+void GamepadInterface::step(){ }
+void GamepadInterface::close(){ }
+#endif
+
+

src/Gamepad/gamepad.h

@@ -0,0 +1,48 @@
+#ifndef RAI_gamepad_h
+#define RAI_gamepad_h
+
+#include <Core/array.h>
+#include <Core/thread.h>
+
+struct GamepadInterface : Thread {
+  struct jsJoystick *joystick;
+  Var<arr> gamepadState;
+  Var<bool> quitSignal;
+  GamepadInterface();
+  ~GamepadInterface();
+  void open();
+  void step();
+  void close();
+};
+
+/// The buttons on gamepad have the following values.
+/// Note that you can use the binary operators & and | to check for multiple
+/// button presses.
+enum BUTTON {
+  BTN_NONE = 0,
+  BTN_A = 1,
+  BTN_B = 2,
+  BTN_X = 4,
+  BTN_Y = 8,
+  BTN_LB = 16,
+  BTN_RB = 32,
+  BTN_LT = 64,
+  BTN_RT = 128,
+  BTN_BACK = 256,
+  BTN_START = 512,
+  BTN_LSTICK = 1024,
+  BTN_RSTICK = 2048,
+};
+
+inline bool stopButtons(const arr& gamepadState){
+  if(!gamepadState.N) return false;
+  uint mode = uint(gamepadState(0));
+  if(mode&BTN_LB || mode&BTN_RB || mode&BTN_LT || mode&BTN_RT) return true;
+  return false;
+}
+
+#ifdef  RAI_IMPLEMENTATION
+#  include "gamepad.cpp"
+#endif
+
+#endif