diff --git a/src/BotOp/bot.cpp b/src/BotOp/bot.cpp index e483387..91019a3 100644 --- a/src/BotOp/bot.cpp +++ b/src/BotOp/bot.cpp @@ -13,8 +13,10 @@ #include #include "simulation.h" #include +#include #include #include +#include #include #ifdef RAI_VIVE # include @@ -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(0); gripperR = make_shared(1); - }else if(C.getFrame("l_panda_hand", false)){ + }else if(C.getFrame("l_panda_finger_joint1", false)){ gripperL = make_shared(0); - }else if(C.getFrame("r_panda_hand", false)){ + }else if(C.getFrame("r_panda_finger_joint1", false)){ gripperR = make_shared(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: " <(robotID++, franka_getJointIndices(C,'l'), cmd, state); robotR = make_shared(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(robotID++, franka_getJointIndices(C,'l'), cmd, state); } else if(C.getFrame("r_panda_base", false)){ robotR = make_shared(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: " <(robotID++, uintA{0,1,2}, cmd, state); + } + } catch(const std::exception& ex) { + LOG(-1) <<"Starting the ranger failed! Error msg: " <(C, cmd, state); robotL = simthread; @@ -112,6 +128,13 @@ BotOp::BotOp(rai::Configuration& C, bool useRealRobot){ optitrack->pull(C); } + if(rai::getParameter("bot/useLivox", false)){ + LOG(0) <<"OPENING LIVOX"; + if(!useRealRobot) LOG(-1) <<"useLivox with real:false -- that's usually wrong!"; + livox = make_shared(); + livox->pull(C); + } + #ifdef RAI_VIVE //-- launch ViveController if(rai::getParameter("bot/useViveController", false)){ @@ -127,8 +150,8 @@ BotOp::BotOp(rai::Configuration& C, bool useRealRobot){ audio = make_shared(); } - 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: " <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){ diff --git a/src/BotOp/bot.h b/src/BotOp/bot.h index be778a5..588d41e 100644 --- a/src/BotOp/bot.h +++ b/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 gripperR; std::shared_ptr ref; std::shared_ptr optitrack; + std::shared_ptr livox; std::shared_ptr vivecontroller; std::shared_ptr audio; std::shared_ptr 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& getCamera(const char* sensor); template BotOp& setReference(); diff --git a/src/Gamepad/Makefile b/src/Gamepad/Makefile new file mode 100644 index 0000000..fb37c87 --- /dev/null +++ b/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 diff --git a/src/Gamepad/gamepad.cpp b/src/Gamepad/gamepad.cpp new file mode 100644 index 0000000..aa97314 --- /dev/null +++ b/src/Gamepad/gamepad.cpp @@ -0,0 +1,71 @@ +#include "gamepad.h" + +#ifdef RAI_PLIB + +#include +#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 = " <getNumButtons() + << "\nerror? = " <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; i20 && 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 + + diff --git a/src/Gamepad/gamepad.h b/src/Gamepad/gamepad.h new file mode 100644 index 0000000..bb3ef0a --- /dev/null +++ b/src/Gamepad/gamepad.h @@ -0,0 +1,48 @@ +#ifndef RAI_gamepad_h +#define RAI_gamepad_h + +#include +#include + +struct GamepadInterface : Thread { + struct jsJoystick *joystick; + Var gamepadState; + Var 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 diff --git a/src/Livox/livox.cpp b/src/Livox/livox.cpp new file mode 100644 index 0000000..e8a56c1 --- /dev/null +++ b/src/Livox/livox.cpp @@ -0,0 +1,126 @@ +#include "livox.h" + +#define RAI_LIVOX +#ifdef RAI_LIVOX + +#include + +#include "livox_lidar_def.h" +#include "livox_lidar_api.h" + +#include +#include + +#include +#include +#include +#include +#include +#include + + +void PointCloudCallback(uint32_t handle, const uint8_t dev_type, LivoxLidarEthernetPacket* data, void* client_data) { + if (data == nullptr) { + return; + } + // printf("point cloud handle: %u, data_num: %d, data_type: %d, length: %d, frame_counter: %d\n", + // handle, data->dot_num, data->data_type, data->length, data->frame_cnt); + + PointCloudData* pcData = static_cast(client_data); + + if (data->data_type == kLivoxLidarCartesianCoordinateHighData) { + LivoxLidarCartesianHighRawPoint *p_point_data = (LivoxLidarCartesianHighRawPoint *)data->data; + for (uint32_t i = 0; i < data->dot_num; i++) + { + Point point; + point.x = p_point_data[i].x * 0.001; + point.y = p_point_data[i].y * 0.001; + point.z = p_point_data[i].z * 0.001; + + float l1_norm = abs(point.x) + abs(point.y) + abs(point.z); + if (l1_norm != 0) + { + // If the point cloud has more than max_points, remove the first point + if (pcData->points.size() >= pcData->max_points) { + pcData->points.erase(pcData->points.begin()); + } + + // Add the point to the vector in the struct + pcData->points.push_back(point); + } + } + } + else if (data->data_type == kLivoxLidarCartesianCoordinateLowData) { + LivoxLidarCartesianLowRawPoint *p_point_data = (LivoxLidarCartesianLowRawPoint *)data->data; + } else if (data->data_type == kLivoxLidarSphericalCoordinateData) { + LivoxLidarSpherPoint* p_point_data = (LivoxLidarSpherPoint *)data->data; + } +} + +namespace rai{ + + Livox::Livox() : Thread("LivoxThread", 0.){ + max_points = rai::getParameter("livox/max_points", 30000); + points = zeros(max_points*3); + points.reshape(max_points, 3); + + points_message.max_points = max_points; + + const std::string path = "./mid360_config.json"; + + if (!LivoxLidarSdkInit(path.c_str())) + { + printf("Livox Init Failed\n"); + LivoxLidarSdkUninit(); + } + else + { + SetLivoxLidarPointCloudCallBack(PointCloudCallback, &points_message); + } + + threadLoop(); + } + + Livox::~Livox(){ + LivoxLidarSdkUninit(); + threadClose(); + } + + void Livox::pull(rai::Configuration& C){ + //-- Update configuration + const char* name = "livox_lidar"; + rai::Frame *base = C.getFrame(name, false); + if(!base){ + LOG(0) << "Creating new frame 'livox_lidar'"; + base = C.addFrame(name); + base->setColor({0., 1., 0.}); + } + + std::lock_guard lock(mux); + + base->setPointCloud(points); + } + + void Livox::step(){ + + std::lock_guard lock(mux); + + int counter = 0; + for (const auto& point : points_message.points) { + points.elem(counter) = point.x; + points.elem(counter+1) = point.y; + points.elem(counter+2) = point.z; + counter += 3; + } + points.reshape(max_points, 3); + } +} //namespace + +#else + +rai::Livox::Livox() : Thread("LivoxThread") { NICO } +rai::Livox::~Livox(){ NICO } +void rai::Livox::pull(rai::Configuration& C){ NICO } +void rai::Livox::step(){ NICO } + +#endif diff --git a/src/Livox/livox.h b/src/Livox/livox.h new file mode 100644 index 0000000..57e4985 --- /dev/null +++ b/src/Livox/livox.h @@ -0,0 +1,38 @@ +#pragma once + +#include +#include + + +struct Point { + float x; + float y; + float z; +}; + +struct PointCloudData { + std::vector points; + int max_points; +}; + +namespace rai +{ + struct Livox : Thread + { + RAI_PARAM("livox/", double, filter, .9) + + Livox(); + ~Livox(); + + void pull(rai::Configuration& C); + + void step(); + + PointCloudData points_message; + private: + std::mutex mux; + int max_points; + arr points; + }; + +} //namespace diff --git a/src/Omnibase/SimplexMotion-com.cpp b/src/Omnibase/SimplexMotion-com.cpp index c392140..1e3d9b6 100644 --- a/src/Omnibase/SimplexMotion-com.cpp +++ b/src/Omnibase/SimplexMotion-com.cpp @@ -116,6 +116,7 @@ const char* SimplexMotion_Communication::readString(int regNumber, int n){ return (char*)buf; } + #ifdef USE_HIDAPI //=========================================================================== @@ -357,7 +358,7 @@ bool SimplexMotion_Communication::readBuf(int len){ // fake (NICO) implementation // -#define NICO throw std::runtime_error("not implemented with this compiler options: usually this means that the implementation needs an external library and a corresponding compiler option - see the source code"); +#define NICO throw std::runtime_error("SimplexMotion-com.cpp -- not implemented with this compiler options: usually this means that the implementation needs an external library and a corresponding compiler option - see the source code"); SimplexMotion_Communication::SimplexMotion_Communication(const char* devPath, unsigned short vendor_id, unsigned short product_id){ NICO } diff --git a/src/Omnibase/omnibase.cpp b/src/Omnibase/omnibase.cpp index 783c530..b8c3568 100644 --- a/src/Omnibase/omnibase.cpp +++ b/src/Omnibase/omnibase.cpp @@ -1,59 +1,67 @@ #include "omnibase.h" #include "SimplexMotion.h" -#ifdef RAI_OMNIBASE -#define USE_FAKE +#ifdef RAI_OMNIBASE struct OmnibaseController{ -#ifdef USE_FAKE - arr fake_q, fake_qDot; -#else rai::Array> motors; //three motors arr qLast, sLast; //q: joint state; s: motor state -#endif + int Kp, Ki, Kd, KiLimit, KdDelay, Friction; - OmnibaseController(rai::String address){ -#ifdef USE_FAKE - fake_q.resize(3).setZero(); - fake_qDot.resize(3).setZero(); -#else + OmnibaseController(const StringA& addresses, int Kp, int Ki, int Kd, int KiLimit, int KdDelay, int Friction) + : Kp(Kp), Ki(Ki), Kd(Kd), KiLimit(KiLimit), KdDelay(KdDelay), Friction(Friction) { //-- launch 3 motors motors.resize(3); + for(uint i=0;i(address, 0x04d8, 0xf79a); - motors(i)->runReset(); //resets position counter - motors(i)->runTorque(0.); //starts torque mode - address(address.N-1)++; + motors(i) = make_shared(addresses(i), 0x04d8, 0xf79a); + cout <<"model name: '" <getModelName() <<"'" <getSerialNumber() <<"'" <getAddress() <<"'" <getMotorTemperature() <getVoltage() <runReset(); //resets position counter + motors(i)->setPID(Kp, Ki, Kd, KiLimit, KdDelay, Friction); + motors(i)->runSpeed(0.); + + LOG(0) << "Motor " << i << " battery reading: " << motors(i)->getVoltage() << "V"; } qLast = zeros(3); sLast = zeros(motors.N); -#endif } ~OmnibaseController(){ -#ifdef USE_FAKE -#else for(uint i=0;irunStop(); rai::wait(.1); for(uint i=0;irunOff(); rai::wait(.1); - for(uint i=0;ireset(); } arr getJacobian(){ //return Jacobian based on qLast - arr J(3,3); - NIY; + + arr J = { + .5, .5, -1., + .5*sqrt(3.), -.5*sqrt(3.), 0., + 1/(3.*center2wheel), 1/(3.*center2wheel), 1./(3.*center2wheel) + }; + J *= wheel_radius/gear_ratio; + J.reshape(3,3); + + double phi = qLast(2); + arr rot = eye(3); + rot(0,0) = rot(1,1) = cos(phi); + rot(0,1) = -sin(phi); + rot(1,0) = sin(phi); + J = rot * J; + return J; } void getState(arr& q, arr& qDot){ -#ifdef USE_FAKE //fake implementation: directly return fake state - q = fake_q; - qDot = fake_qDot; -#else //-- get motor state arr s(motors.N); arr sDot(motors.N); @@ -70,37 +78,31 @@ struct OmnibaseController{ q = qLast + qDelta; qDot = Jacobian * sDot; - qLast = q; sLast = s; -#endif } - void setTorques(const arr& u){ -#ifdef USE_FAKE //fake implementation: replace real physics dynamics by trivial double integrator - double invMass = 1.; - double tau=.01; - fake_qDot += .5*tau*invMass*u; - fake_q += tau*fake_qDot; - fake_qDot += .5*tau*invMass*u; - LOG(0) <<"stepping..." <setTarget(u_s(i)/motors(i)->maxTorque*32767); + // motors(i)->setPID(Kp, Ki, Kd, KiLimit, KdDelay, Friction); + motors(i)->runSpeed(v_motors(i)); } -#endif } }; +OmnibaseThread::OmnibaseThread(uint robotID, const uintA &_qIndices, const Var &_cmd, const Var &_state) + : RobotAbstraction(_cmd, _state), Thread("OmnibaseThread", .015){ + init(robotID, _qIndices); + writeData = 10; +} + OmnibaseThread::~OmnibaseThread(){ - LOG(0) <<"shutting down Omnibase " <("Omnibase/Kp_freq", arr{10., 10., 10.}); - Kd_ratio = rai::getParameter("Omnibase/Kd_ratio", arr{.5, .5, .5}); - LOG(0) << "Omnibase: Kp_freq:" << Kp_freq << " Kd_ratio:" << Kd_ratio; + outer_Kp = rai::getParameter("Omnibase/outer_Kp", 4.); + Kp = rai::getParameter("Omnibase/Kp", 2000); + Ki = rai::getParameter("Omnibase/Ki", 0); + Kd = rai::getParameter("Omnibase/Kd", 1000); + KiLimit = rai::getParameter("Omnibase/KiLimit", 100); + KdDelay = rai::getParameter("Omnibase/KdDelay", 2); + Friction = rai::getParameter("Omnibase/Friction", 0); + + // Jacobian params + gear_ratio = rai::getParameter("Omnibase/gear_ratio", 4.2); + center2wheel = rai::getParameter("Omnibase/center2wheel", .35); + wheel_radius = rai::getParameter("Omnibase/wheel_radius", .06); + + LOG(0) << "Omnibase/PID: Kp:" << Kp << " Ki:" << Ki << " Kd:" << Kd << "KdDelay:" << KdDelay << "KiLimit:" << KiLimit << " Friction:" << Friction; + LOG(0) << "Omnibase/Jacobian: gear_ratio:" << gear_ratio << " center2wheel:" << center2wheel << " wheel_radius:" << wheel_radius; //-- get robot address - address = rai::getParameter("Omnibase/address", "172.16.0.2"); + addresses = rai::getParameter("Omnibase/addresses", {"/dev/hidraw0", "/dev/hidraw1", "/dev/hidraw2"}); //-- start thread and wait for first state signal - LOG(0) <<"launching Omnibase " <(address); + robot = make_shared(addresses, Kp, Ki, Kd, KiLimit, KdDelay, Friction); - //-- initialize state and ctrl with first state - arr q_real, qDot_real; - robot->getState(q_real, qDot_real); + //-- initialize state and ctrl with first state + arr q_real, qDot_real; + robot->getState(q_real, qDot_real); - auto stateSet = state.set(); + auto stateSet = state.set(); - //ensure state variables have sufficient size - while(stateSet->q.N<=qIndices_max) stateSet->q.append(0.); - while(stateSet->qDot.N<=qIndices_max) stateSet->qDot.append(0.); - while(stateSet->tauExternalIntegral.N<=qIndices_max) stateSet->tauExternalIntegral.append(0.); + //ensure state variables have sufficient size + while(stateSet->q.N<=qIndices_max) stateSet->q.append(0.); + while(stateSet->qDot.N<=qIndices_max) stateSet->qDot.append(0.); + while(stateSet->tauExternalIntegral.N<=qIndices_max) stateSet->tauExternalIntegral.append(0.); - for(uint i=0; i<3; i++){ - stateSet->q.elem(qIndices(i)) = q_real(i); - stateSet->qDot.elem(qIndices(i)) = qDot_real(i); - stateSet->tauExternalIntegral.elem(qIndices(i)) = 0.; - stateSet->tauExternalCount=0; - } + for(uint i=0; i<3; i++){ + stateSet->q.elem(qIndices(i)) = q_real(i); + stateSet->qDot.elem(qIndices(i)) = qDot_real(i); + stateSet->tauExternalIntegral.elem(qIndices(i)) = 0.; + stateSet->tauExternalCount=0; + } } void OmnibaseThread::step(){ @@ -154,8 +168,6 @@ void OmnibaseThread::step(){ //-- get current state from robot arr q_real, qDot_real; robot->getState(q_real, qDot_real); -// arr torquesExternal_real(robot_state.tau_ext_hat_filtered.begin(), robot_state.tau_ext_hat_filtered.size(), false); -// arr torques_real(robot_state.tau_J.begin(), robot_state.tau_J.size(), false); //-- publish state & INCREMENT CTRL TIME arr state_q_real, state_qDot_real; @@ -223,62 +235,44 @@ void OmnibaseThread::step(){ arr del = q_ref - q_real; err = ::sqrt(scalarProduct(del, P_compliance*del)); } - if(err>.05){ //stall! + if(err>1.05){ //stall! state.set()->stall = 2; //no progress in reference time! for at least 2 iterations (to ensure continuous stall with multiple threads) cout <<"STALLING - step:" <getJacobian(); + arr Jinv = pseudoInverse(J, NoArr, 1e-6); - //-- add feedback term if(q_ref.N==3){ - u += Kp * (q_ref - q_real); + arr delta_motor = Jinv * (q_ref - q_real); + v += outer_Kp * delta_motor; } + if(qDot_ref.N==3){ - u += Kd % (qDot_ref - qDot_real); + v += Jinv * qDot_ref; } - //-- project with compliance - if(P_compliance.N) u = P_compliance * u; - //-- data log? if(writeData>0 && !(steps%1)){ - if(!dataFile.is_open()) dataFile.open(STRING("z.omnibase"<1){ + if(!dataFile.is_open()) dataFile.open(STRING("z.omnibase"<1){ qDot_real.modRaw().write(dataFile); //3 qDot_ref.modRaw().write(dataFile); //3 - u.modRaw().write(dataFile); //3 + v.modRaw().write(dataFile); //3 qDDot_ref.modRaw().write(dataFile); - } - dataFile <setTorques(u); + robot->setVelocities(v); } void OmnibaseThread::close(){ diff --git a/src/Omnibase/omnibase.h b/src/Omnibase/omnibase.h index 69b52c3..0093735 100644 --- a/src/Omnibase/omnibase.h +++ b/src/Omnibase/omnibase.h @@ -8,13 +8,15 @@ struct OmnibaseController; struct OmnibaseThread : rai::RobotAbstraction, Thread { - OmnibaseThread(uint robotID, const uintA& _qIndices, const Var& _cmd, const Var& _state) : RobotAbstraction(_cmd, _state), Thread("OmnibaseThread", .01){ init(robotID, _qIndices); } + OmnibaseThread(uint robotID, const uintA& _qIndices, const Var& _cmd, const Var& _state); ~OmnibaseThread(); private: int robotID=0; - arr Kp_freq, Kd_ratio; //read from rai.cfg - rai::String address; + double outer_Kp; + int Kp, Ki, Kd, KdDelay, KiLimit, Friction; //read from rai.cfg + double gear_ratio, center2wheel, wheel_radius //read from rai.cfg + StringA addresses; uintA qIndices; uint qIndices_max=0; diff --git a/src/Omnibase/omnibase_old.cpp b/src/Omnibase/omnibase_old.cpp new file mode 100644 index 0000000..055bf25 --- /dev/null +++ b/src/Omnibase/omnibase_old.cpp @@ -0,0 +1,309 @@ +#if 0 +#include "omnibase.h" +#include "SimplexMotion.h" + + +#ifdef RAI_OMNIBASE + + +struct OmnibaseController{ + rai::Array> motors; //three motors + arr qLast, sLast; //q: joint state; s: motor state + + OmnibaseController(const StringA& addresses, double Kp, double Kd){ + //-- launch 3 motors + motors.resize(3); + for(uint i=0;i(addresses(i), 0x04d8, 0xf79a); + cout <<"model name: '" <getModelName() <<"'" <getSerialNumber() <<"'" <getAddress() <<"'" <getMotorTemperature() <getVoltage() <runReset(); //resets position counter + motors(i)->runTorque(0.); //starts torque mode + + LOG(0) << "Motor " << i << " battery reading: " << motors(i)->getVoltage() << "V"; + } + qLast = zeros(3); + sLast = zeros(motors.N); + } + + ~OmnibaseController(){ + for(uint i=0;irunStop(); + rai::wait(.1); + for(uint i=0;irunOff(); + rai::wait(.1); + for(uint i=0;igetMotorPosition(); + sDot(i) = motors(i)->getMotorSpeed(); + } + + //-- convert to joint state + arr sDelta = s - sLast; + // DO MAGIC HERE: convert the sDelta (change in motor positions) to a qDelta (change in joint state) + arr Jacobian = getJacobian(); + arr qDelta = Jacobian * sDelta; + + q = qLast + qDelta; + qDot = Jacobian * sDot; + qLast = q; + sLast = s; + } + + void setTorques(const arr& u_motors){ + //-- send torques to motors + CHECK_EQ(u_motors.N, motors.N, "control signal has wrong dimensionality"); + + clip(u_motors, -1., 1.); + for(uint i=0;isetTarget(u_motors(i)/motors(i)->maxTorque*32767); + } + } +}; + +OmnibaseThread::OmnibaseThread(uint robotID, const uintA &_qIndices, const Var &_cmd, const Var &_state) + : RobotAbstraction(_cmd, _state), Thread("OmnibaseThread", .015){ + init(robotID, _qIndices); + writeData = 10; +} + +OmnibaseThread::~OmnibaseThread(){ + LOG(0) <<"shutting down Omnibase " <("Omnibase/Kp", .7); + Kd = rai::getParameter("Omnibase/Kd", .01); + M_org = rai::getParameter("Omnibase/M_org", arr{0., 0., 0., 0., 0., 0., 0., 0., 0.}); + friction = rai::getParameter("Omnibase/friction", arr{0., 0., 0.}); + M_org.reshape(3,3); + LOG(0) << "Omnibase: Kp:" << Kp << " Kd:" << Kd << "M_org:" << M_org << "friction:" << friction; + + //-- get robot address + addresses = rai::getParameter("Omnibase/addresses", {"/dev/hidraw0", "/dev/hidraw1", "/dev/hidraw2"}); + + //-- start thread and wait for first state signal + LOG(0) <<"launching Omnibase " <(addresses, Kp, Kd); + + //-- initialize state and ctrl with first state + arr q_real, qDot_real; + robot->getState(q_real, qDot_real); + + auto stateSet = state.set(); + + //ensure state variables have sufficient size + while(stateSet->q.N<=qIndices_max) stateSet->q.append(0.); + while(stateSet->qDot.N<=qIndices_max) stateSet->qDot.append(0.); + while(stateSet->tauExternalIntegral.N<=qIndices_max) stateSet->tauExternalIntegral.append(0.); + + for(uint i=0; i<3; i++){ + stateSet->q.elem(qIndices(i)) = q_real(i); + stateSet->qDot.elem(qIndices(i)) = qDot_real(i); + stateSet->tauExternalIntegral.elem(qIndices(i)) = 0.; + stateSet->tauExternalCount=0; + } +} + +void OmnibaseThread::step(){ + steps++; + auto loop_start = std::chrono::steady_clock::now(); + + //-- get current state from robot + arr q_real, qDot_real; + robot->getState(q_real, qDot_real); + + //-- publish state & INCREMENT CTRL TIME + arr state_q_real, state_qDot_real; + { + auto stateSet = state.set(); + if(robotID==0){ // if this is the lead robot, increment ctrlTime if no stall + if(!stateSet->stall) stateSet->ctrlTime += metronome.ticInterval; + else stateSet->stall--; + } + ctrlTime = stateSet->ctrlTime; + for(uint i=0;i<3;i++){ + stateSet->q.elem(qIndices(i)) = q_real.elem(i); + stateSet->qDot.elem(qIndices(i)) = qDot_real.elem(i); + } + state_q_real = stateSet->q; + state_qDot_real = stateSet->qDot; + } + + //-- get current ctrl command + arr q_ref, qDot_ref, qDDot_ref, Kp_ref, Kd_ref, P_compliance; + { + auto cmdGet = cmd.get(); + + //get commanded reference from the reference callback (e.g., sampling a spline reference) + arr cmd_q_ref, cmd_qDot_ref, cmd_qDDot_ref; + if(cmdGet->ref){ + cmdGet->ref->getReference(cmd_q_ref, cmd_qDot_ref, cmd_qDDot_ref, state_q_real, state_qDot_real, ctrlTime); + CHECK(!cmd_q_ref.N || cmd_q_ref.N > qIndices_max, ""); + CHECK(!cmd_qDot_ref.N || cmd_qDot_ref.N > qIndices_max, ""); + CHECK(!cmd_qDDot_ref.N || cmd_qDDot_ref.N > qIndices_max, ""); + } + + //pick qIndices for this particular robot + if(cmd_q_ref.N){ + q_ref.resize(3); + for(uint i=0; i<3; i++) q_ref.elem(i) = cmd_q_ref.elem(qIndices(i)); + } + if(cmd_qDot_ref.N){ + qDot_ref.resize(3); + for(uint i=0; i<3; i++) qDot_ref.elem(i) = cmd_qDot_ref.elem(qIndices(i)); + } + if(cmd_qDDot_ref.N){ + qDDot_ref.resize(3); + for(uint i=0; i<3; i++) qDDot_ref.elem(i) = cmd_qDDot_ref.elem(qIndices(i)); + } + if(cmdGet->Kp.d0 >= 3 && cmdGet->Kp.d1 >=3 && cmdGet->Kp.d0 == cmdGet->Kp.d1){ + Kp_ref.resize(3, 3); + for(uint i=0; i<3; i++) for(uint j=0; j<3; j++) Kp_ref(i, j) = cmdGet->Kp(qIndices(i), qIndices(j)); + } + if(cmdGet->Kd.d0 >= 3 && cmdGet->Kd.d1 >=3 && cmdGet->Kd.d0 == cmdGet->Kd.d1){ + Kd_ref.resize(3, 3); + for(uint i=0; i<3; i++) for(uint j=0; j<3; j++) Kd_ref(i, j) = cmdGet->Kd(qIndices(i), qIndices(j)); + } + if(cmdGet->P_compliance.N) { + P_compliance.resize(3,3); + for(uint i=0; i<3; i++) for(uint j=0; j<3; j++) P_compliance(i,j) = cmdGet->P_compliance(qIndices(i), qIndices(j)); + } + + } + + //-- cap the reference difference + if(q_ref.N==3){ + double err = length(q_ref - q_real); + if(P_compliance.N){ + arr del = q_ref - q_real; + err = ::sqrt(scalarProduct(del, P_compliance*del)); + } + if(err>1.05){ //stall! + state.set()->stall = 2; //no progress in reference time! for at least 2 iterations (to ensure continuous stall with multiple threads) + cout <<"STALLING - step:" <getJacobian(); + arr Jinv = pseudoInverse(J, NoArr, 1e-6); + + //-- add feedback term + if(q_ref.N==3){ + arr delta_motor = Jinv * (q_ref - q_real); + u += Kp * delta_motor; + } + if(qDot_ref.N==3){ + arr delta_motor = Jinv * (qDot_ref - qDot_real); + u += Kd * delta_motor; + } + + //-- add feedforward term + if(qDDot_ref.N==3 && absMax(qDDot_ref)>0.){ + arr M = M_org; // + diag(arr{0.3, 0.3, 0.3)); + u += M*qDDot_ref; + } + + //-- add friction term + if(friction.N==3 && qDot_ref.N==3){ + double velThresh=1e-3; + for(uint i=0;i<3;i++){ + double coeff = qDot_ref.elem(i)/velThresh; + if(coeff>1.) coeff=1.; + if(coeff<-1.) coeff=-1.; + u.elem(i) += coeff*friction.elem(i); + } + + //-- project with compliance + if(P_compliance.N) u = P_compliance * u; + + //-- data log? + if(writeData>0 && !(steps%1)){ + if(!dataFile.is_open()) dataFile.open(STRING("z.omnibase"<1){ + qDot_real.modRaw().write(dataFile); //3 + qDot_ref.modRaw().write(dataFile); //3 + u.modRaw().write(dataFile); //3 + qDDot_ref.modRaw().write(dataFile); + } + dataFile <setTorques(u); + std::chrono::duration elapsed_time = loop_end - loop_start; + std::cout << "Elapsed time: " << elapsed_time.count() * .001 << " seconds" << std::endl; + } +} + +void OmnibaseThread::close(){ + robot.reset(); +} + +#else //RAI_Omnibase + +OmnibaseThread::~OmnibaseThread(){ NICO } +void OmnibaseThread::init(uint _robotID, const uintA& _qIndices) { NICO } +void OmnibaseThread::step(){ NICO } +void OmnibaseThread::open(){ NICO } +void OmnibaseThread::close(){ NICO } + +#endif +#endif diff --git a/src/Omnibase/omnibase_old.h b/src/Omnibase/omnibase_old.h new file mode 100644 index 0000000..555bbff --- /dev/null +++ b/src/Omnibase/omnibase_old.h @@ -0,0 +1,37 @@ +#if 0 +#pragma once + +#include +#include +#include +#include + +struct OmnibaseController; + +struct OmnibaseThread : rai::RobotAbstraction, Thread { + OmnibaseThread(uint robotID, const uintA& _qIndices, const Var& _cmd, const Var& _state); + ~OmnibaseThread(); + +private: + int robotID=0; + double Kp, Kd; //read from rai.cfg + arr M_org, friction; + StringA addresses; + + uintA qIndices; + uint qIndices_max=0; + + uint steps=0; + ofstream dataFile; + double ctrlTime=0.; + + std::shared_ptr robot; + + + void init(uint _robotID, const uintA& _qIndices); + void open(); + void step(); + void close(); +}; + +#endif diff --git a/src/Ranger/ranger.cpp b/src/Ranger/ranger.cpp new file mode 100644 index 0000000..55a7900 --- /dev/null +++ b/src/Ranger/ranger.cpp @@ -0,0 +1,635 @@ + +#include "ranger.h" + +#define RAI_RANGER +#ifdef RAI_RANGER + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include // Include for ioctl and SIOCGIFINDEX +#define PI 3.1415926 +#define DEBUG 0 + +// TODO: setLights func in botop (also like flashing lights or so) + +struct RangerController +{ + int socket_fd; + int mode; // 0: Oblique, 1: Spin, 2: Ackermann + arr qLast, sLast; //q: joint state; s: motor state + double real_steering_rad; + double real_steering_rad_s; + double target_steering_rad_s; + + RangerController(const char* address) + { + mode = 0; + qLast = zeros(3); + real_steering_rad = 0.0; // CAREFULL!! Only updates when you call "get_qDot_oblique". + real_steering_rad_s = 0.0; // CAREFULL!! Only updates when you call "get_qDot_oblique". + target_steering_rad_s = 0.0; // CAREFULL!! Only updates when you call "setVelocities". + // sLast = zeros(8); // (FL, FR, BL, BR) Angle, (FL, FR, BL, BR) Wheel + sLast = zeros(4); // FL, FR, BL, BR + + //--------- SETTING UP CAN INTERFACE ---------// + struct ifreq ifr; + struct sockaddr_can addr; + socket_fd = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (socket_fd < 0) { + LOG(-1) << "Error creating socket: " << strerror(errno) << std::endl; + } + + std::strncpy(ifr.ifr_name, address, IFNAMSIZ - 1); + if (ioctl(socket_fd, SIOCGIFINDEX, &ifr) < 0) { + LOG(-1) << "Error getting interface index: " << strerror(errno) << std::endl; + close(socket_fd); + } + + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + if (bind(socket_fd, (struct sockaddr*)&addr, sizeof(addr)) < 0) { + LOG(-1) << "Error binding socket: " << strerror(errno) << std::endl; + close(socket_fd); + } + + // CAN stuff + struct can_frame frame_control_mode; + uint32_t id_control_mode = 0x421; + frame_control_mode.can_id = id_control_mode; + frame_control_mode.can_dlc = 8; // Data length code (number of bytes) + frame_control_mode.data[0] = 0x01; // Byte 0 for control mode + std::memset(frame_control_mode.data + 1, 0, 7); // Remaining bytes = 0 + + send_package(frame_control_mode); + + // Control mode stuff + set_control_mode(0x01); + // 0x00 front and rear Ackerman mode + // 0x01 oblique motion mode + // 0x02 spin mode + // 0x03 Parking mode + + std::cout << "Control mode message sent: ID = " << std::hex << id_control_mode << std::endl; + + sLast = get_s(); + } + + ~RangerController(){ + close(socket_fd); + } + + void set_control_mode(canid_t type) { + struct can_frame frame_control_mode; + uint32_t id_control_mode = 0x141; + frame_control_mode.can_id = id_control_mode; + frame_control_mode.can_dlc = 1; // Data length code (number of bytes) + frame_control_mode.data[0] = type; + std::memset(frame_control_mode.data + 1, 0, 7); // Remaining bytes = 0 + + send_package(frame_control_mode); + } + + arr get_s() + { + struct can_frame recv_frame; + + // Front Wheels + recv_frame = recv_package(0x311); + + int32_t front_left_wheel_odom_mm = (recv_frame.data[0] << 24) | (recv_frame.data[1] << 16) | (recv_frame.data[2] << 8) | recv_frame.data[3]; + int32_t front_right_wheel_odom_mm = (recv_frame.data[4] << 24) | (recv_frame.data[5] << 16) | (recv_frame.data[6] << 8) | recv_frame.data[7]; + + double front_left_wheel_odom = static_cast(front_left_wheel_odom_mm) / 1000; + double front_right_wheel_odom = static_cast(front_right_wheel_odom_mm) / 1000; + + // Rear Wheels + recv_frame = recv_package(0x312); + + int32_t rear_left_wheel_odom_mm = (recv_frame.data[0] << 24) | (recv_frame.data[1] << 16) | (recv_frame.data[2] << 8) | recv_frame.data[3]; + int32_t rear_right_wheel_odom_mm = (recv_frame.data[4] << 24) | (recv_frame.data[5] << 16) | (recv_frame.data[6] << 8) | recv_frame.data[7]; + + double rear_left_wheel_odom = static_cast(rear_left_wheel_odom_mm) / 1000; + double rear_right_wheel_odom = static_cast(rear_right_wheel_odom_mm) / 1000; + + #if DEBUG + std::cout << "------------- Odometry -------------" << std::endl; + std::cout << "Front Left Wheel Odometer: " << front_left_wheel_odom << " m" << std::endl; + std::cout << "Front Right Wheel Odometer: " << front_right_wheel_odom << " m" << std::endl; + std::cout << "Rear Left Wheel Odometer: " << rear_left_wheel_odom << " m" << std::endl; + std::cout << "Rear Right Wheel Odometer: " << rear_right_wheel_odom << " m" << std::endl; + #endif + + return arr{front_left_wheel_odom, front_right_wheel_odom, rear_left_wheel_odom, rear_right_wheel_odom}; + } + + arr get_qDot_oblique() + { + struct can_frame recv_frame = recv_package(0x221); + + // Extract actual speed and steering angle + int16_t actual_speed = (recv_frame.data[0] << 8) | recv_frame.data[1]; + int16_t actual_steering_angle = (recv_frame.data[6] << 8) | recv_frame.data[7]; + + double actual_speed_mps = actual_speed / 1000.0; // Convert mm/s to m/s + real_steering_rad = actual_steering_angle / 1000.0; // Convert mrad to rad + real_steering_rad_s = real_steering_rad; + #if DEBUG + std::cout << "Actual Steering Angle before: " << real_steering_rad << " rad" << std::endl; + #endif + + real_steering_rad *= -1; + if (actual_speed_mps < 0) { // Lower side + real_steering_rad += PI; + } + + // Compute X-Y velocity components + double angle = real_steering_rad - qLast.elem(2); + double velocity_x = actual_speed_mps * cos(angle); + double velocity_y = actual_speed_mps * -sin(angle); + + #if DEBUG + std::cout << "Actual Linear Speed: " << actual_speed_mps << " m/s" << std::endl; + std::cout << "Actual Steering Angle: " << real_steering_rad << " rad" << std::endl; + std::cout << "qLast.z: " << qLast.elem(2) << " rad" << std::endl; + std::cout << "Velocity X: " << velocity_x << " m/s, Velocity Y: " << velocity_y << " m/s" << std::endl; + std::cout << "---------------------------------" << std::endl; + #endif + + return arr{velocity_x, velocity_y, 0.}; + } + + arr get_qDot_spin() + { + struct can_frame recv_frame = recv_package(0x221); + + // TODO: Translate wheel speed to actual q speed + int16_t actual_spin_speed = (recv_frame.data[2] << 8) | recv_frame.data[3]; + double actual_spin_speed_mps = actual_spin_speed / 1000.0; // Convert mm/s to m/s + + #if DEBUG + std::cout << "Actual Angular Speed: " << actual_spin_speed_mps << " m/s" << std::endl; + #endif + return arr{0., 0., actual_spin_speed_mps}; + } + + struct can_frame recv_package(canid_t type) + { + struct can_frame recv_frame; + while (true) + { + int nbytes = read(socket_fd, &recv_frame, sizeof(recv_frame)); + if (nbytes < 0) { + LOG(-1) << "Error reading CAN frame: " << strerror(errno) << std::endl; + } + if (recv_frame.can_id == type) { + break; + } + } + return recv_frame; + } + + void getState(arr& q, arr& qDot) + { + arr s = get_s(); + arr sDelta = s - sLast; + + arr qDelta = zeros(3); + qDot = zeros(3); + switch (mode) + { + case 1: { + // Spin mode + double mean_delta_s = (-1.*sDelta.elem(0) + + sDelta.elem(1) + + -1.*sDelta.elem(2) + + sDelta.elem(3)) * .25; // m + #if DEBUG + std::cout << "Mean delta s: " << mean_delta_s << " mDelta" << std::endl; + #endif + qDot = get_qDot_spin(); + double base_radious = .31; + double angle_traveled = atan(mean_delta_s/base_radious); + qDelta = arr{0., 0., angle_traveled}; + break; + } + + case 0: { + // Oblique mode + qDot = get_qDot_oblique(); + double mean_delta_s = (sDelta.elem(0) + + sDelta.elem(1) + + sDelta.elem(2) + + sDelta.elem(3)) * .25; // m + mean_delta_s = abs(mean_delta_s); + double steering_angle = real_steering_rad - qLast.elem(2); + double xDelta = cos(steering_angle) * mean_delta_s; + double yDelta = -sin(steering_angle) * mean_delta_s; + qDelta = arr{xDelta, yDelta, 0.}; + break; + } + + case 2: { + // Ackerman mode + break; + } + + default: { + break; + } + } + + q = qLast + qDelta; + qLast = q; + sLast = s; + } + + void setVelocitiesOblique(double dir_angle, double dir_magnitude) + { + struct can_frame frame_motion; + frame_motion.can_dlc = 8; // Data length code (number of bytes) + frame_motion.can_id = 0x111; + + if (dir_angle > PI*.5 && dir_angle < 1.5*PI) { // Lower half + if (dir_angle > PI) { // Lower left + dir_angle -= PI; + dir_angle *=-1; + // std::cout << "Lower left" << std::endl; + } else { // Lower right + dir_angle = PI - dir_angle; + // std::cout << "Lower right" << std::endl; + } + dir_magnitude *= -1; + } else { // Upper half + if (dir_angle >= 1.5*PI) { // Upper left + dir_angle = 2*PI - dir_angle; + // std::cout << "Upper left" << std::endl; + } else { // Upper right + dir_angle *=-1; + // std::cout << "Upper right" << std::endl; + } + } + + target_steering_rad_s = dir_angle; + + #if DEBUG + std::cout << "---------- Oblique mode velocities ----------" << std::endl; + std::cout << "New Mag: " << dir_magnitude << " m/s" << std::endl; + std::cout << "New Angle: " << dir_angle << " rad" << std::endl; + #endif + + int16_t linear_speed = dir_magnitude * 1000; // mm/s + int16_t spin_speed = 0; // No rotation + int16_t steering_angle = dir_angle * 1000; // 0.001 rad + + frame_motion.data[0] = (linear_speed >> 8) & 0xFF; + frame_motion.data[1] = linear_speed & 0xFF; + frame_motion.data[2] = (spin_speed >> 8) & 0xFF; + frame_motion.data[3] = spin_speed & 0xFF; + frame_motion.data[4] = 0x00; // Reserved + frame_motion.data[5] = 0x00; // Reserved + frame_motion.data[6] = (steering_angle >> 8) & 0xFF; + frame_motion.data[7] = steering_angle & 0xFF; + + send_package(frame_motion); + } + + void setSteeringAngle(double dir_angle) + { + struct can_frame frame_motion; + frame_motion.can_dlc = 8; // Data length code (number of bytes) + frame_motion.can_id = 0x111; + + int16_t linear_speed = 0; + int16_t spin_speed = 0; + int16_t steering_angle = dir_angle * 1000; // 0.001 rad + + frame_motion.data[0] = (linear_speed >> 8) & 0xFF; + frame_motion.data[1] = linear_speed & 0xFF; + frame_motion.data[2] = (spin_speed >> 8) & 0xFF; + frame_motion.data[3] = spin_speed & 0xFF; + frame_motion.data[4] = 0x00; // Reserved + frame_motion.data[5] = 0x00; // Reserved + frame_motion.data[6] = (steering_angle >> 8) & 0xFF; + frame_motion.data[7] = steering_angle & 0xFF; + + send_package(frame_motion); + } + + void setVelocitiesSpin(double spin_speed) + { + struct can_frame frame_motion; + frame_motion.can_dlc = 8; // Data length code (number of bytes) + frame_motion.can_id = 0x111; + + #if DEBUG + std::cout << "---------- Spin mode velocities ----------" << std::endl; + std::cout << "New Speed: " << spin_speed << " rad/s" << std::endl; + #endif + + int16_t linear_speed = 0; + int16_t spin_speed_ = spin_speed * 1000; + int16_t steering_angle = 0; + + frame_motion.data[0] = (linear_speed >> 8) & 0xFF; + frame_motion.data[1] = linear_speed & 0xFF; + frame_motion.data[2] = (spin_speed_ >> 8) & 0xFF; + frame_motion.data[3] = spin_speed_ & 0xFF; + frame_motion.data[4] = 0x00; // Reserved + frame_motion.data[5] = 0x00; // Reserved + frame_motion.data[6] = (steering_angle >> 8) & 0xFF; + frame_motion.data[7] = steering_angle & 0xFF; + + send_package(frame_motion); + } + + void send_package(struct can_frame frame_motion) + { + if (write(socket_fd, &frame_motion, sizeof(frame_motion)) != sizeof(frame_motion)) { + LOG(-1) << "Error sending motion command message: " << strerror(errno) << std::endl; + close(socket_fd); + } + } + + void setVelocities(const arr& qDotTarget) + { + int prev_mode = mode; + // Check which mode should be active + double thresh = 2e-2; // 2cm error + bool is_spin_mode = abs(qDotTarget.elem(0)) <= thresh && + abs(qDotTarget.elem(1)) <= thresh && + abs(qDotTarget.elem(2)) >= thresh; + + bool is_oblique_mode = (abs(qDotTarget.elem(0)) >= thresh || + abs(qDotTarget.elem(1)) >= thresh) && + abs(qDotTarget.elem(2)) <= thresh; + + bool is_still = abs(qDotTarget.elem(0)) <= thresh && + abs(qDotTarget.elem(1)) <= thresh && + abs(qDotTarget.elem(2)) <= thresh; + + bool is_ackerman_mode = false; + + if (is_spin_mode) { + mode = 1; + } else if (is_oblique_mode) { + mode = 0; + } else if (is_ackerman_mode) { + mode = 2; + } else if (is_still) { + return; + } else { + LOG(-1) << "Impossible qDotTarget! " << qDotTarget << " >:(" << std::endl; + return; + } + + // Safety + // TODO: Check if velocities are too high + switch (mode) + { + case 1: { + // Spin mode + if (mode != prev_mode) { + set_control_mode(0x02); + } + double spin_speed = qDotTarget.elem(2); + setVelocitiesSpin(spin_speed); + break; + } + + case 0: { + // Oblique mode + if (mode != prev_mode) { + set_control_mode(0x01); + } + + double dir_magnitude = ::sqrt(::pow(qDotTarget.elem(0), 2) + ::pow(qDotTarget.elem(1), 2)); + double target_steering_rad = ::acos(qDotTarget.elem(0) / dir_magnitude); + if (qDotTarget.elem(1) > 0) { + target_steering_rad = PI*2-target_steering_rad; + } + + // Adjust for current rotation + target_steering_rad += qLast.elem(2); + + if (target_steering_rad < 0) { + target_steering_rad += PI*2; + } else if (target_steering_rad >= PI*2) { + target_steering_rad -= PI*2; + } + + #if DEBUG + std::cout << "qLast.z: " << qLast.elem(2) << " rad" << std::endl; + std::cout << "Mag: " << dir_magnitude << " m/s" << std::endl; + std::cout << "Angle: " << target_steering_rad << " rad" << std::endl; + #endif + + setVelocitiesOblique(target_steering_rad, dir_magnitude); + break; + } + + case 2: { + break; + } + + default: { + break; + } + } + } +}; + +RangerThread::RangerThread(uint robotID, const uintA &_qIndices, const Var &_cmd, const Var &_state) + : RobotAbstraction(_cmd, _state), Thread("RangerThread", .015){ + init(robotID, _qIndices); + writeData = 10; +} + +RangerThread::~RangerThread(){ + LOG(0) <<"shutting down Ranger " <("Ranger/Kp", arr{1., 1., 1.}); + Ki = rai::getParameter("Ranger/Ki", arr{0., 0., 0.}); + Kd = rai::getParameter("Ranger/Kd", arr{0., 0., 0.}); + + LOG(0) << "Ranger/PID: Kp:" << Kp << " Ki:" << Ki << " Kd:" << Kd; + + //-- get robot address + address = rai::getParameter("Ranger/address", "can0"); + + //-- start thread and wait for first state signal + LOG(0) <<"launching Ranger " <(address); + + //-- initialize state and ctrl with first state + arr q_real, qDot_real; + robot->getState(q_real, qDot_real); + + auto stateSet = state.set(); + + //ensure state variables have sufficient size + while(stateSet->q.N<=qIndices_max) stateSet->q.append(0.); + while(stateSet->qDot.N<=qIndices_max) stateSet->qDot.append(0.); + while(stateSet->tauExternalIntegral.N<=qIndices_max) stateSet->tauExternalIntegral.append(0.); + + for(uint i=0; i<3; i++){ + stateSet->q.elem(qIndices(i)) = q_real(i); + stateSet->qDot.elem(qIndices(i)) = qDot_real(i); + stateSet->tauExternalIntegral.elem(qIndices(i)) = 0.; + stateSet->tauExternalCount=0; + } +} + +void RangerThread::step(){ + steps++; + + //-- get current state from robot + arr q_real, qDot_real; + robot->getState(q_real, qDot_real); + + //-- publish state & INCREMENT CTRL TIME + arr state_q_real, state_qDot_real; + { + auto stateSet = state.set(); + if(robotID==0){ // if this is the lead robot, increment ctrlTime if no stall + if(!stateSet->stall) stateSet->ctrlTime += metronome.ticInterval; + else stateSet->stall--; + } + ctrlTime = stateSet->ctrlTime; + for(uint i=0;i<3;i++){ + stateSet->q.elem(qIndices(i)) = q_real.elem(i); + stateSet->qDot.elem(qIndices(i)) = qDot_real.elem(i); + } + state_q_real = stateSet->q; + state_qDot_real = stateSet->qDot; + } + + //-- get current ctrl command + arr q_ref, qDot_ref, qDDot_ref, P_compliance; + { + auto cmdGet = cmd.get(); + + //get commanded reference from the reference callback (e.g., sampling a spline reference) + arr cmd_q_ref, cmd_qDot_ref, cmd_qDDot_ref; + if(cmdGet->ref){ + cmdGet->ref->getReference(cmd_q_ref, cmd_qDot_ref, cmd_qDDot_ref, state_q_real, state_qDot_real, ctrlTime); + CHECK(!cmd_q_ref.N || cmd_q_ref.N > qIndices_max, ""); + CHECK(!cmd_qDot_ref.N || cmd_qDot_ref.N > qIndices_max, ""); + CHECK(!cmd_qDDot_ref.N || cmd_qDDot_ref.N > qIndices_max, ""); + } + + //pick qIndices for this particular robot + if(cmd_q_ref.N){ + q_ref.resize(3); + for(uint i=0; i<3; i++) q_ref.elem(i) = cmd_q_ref.elem(qIndices(i)); + } + if(cmd_qDot_ref.N){ + qDot_ref.resize(3); + for(uint i=0; i<3; i++) qDot_ref.elem(i) = cmd_qDot_ref.elem(qIndices(i)); + } + if(cmd_qDDot_ref.N){ + qDDot_ref.resize(3); + for(uint i=0; i<3; i++) qDDot_ref.elem(i) = cmd_qDDot_ref.elem(qIndices(i)); + } + if(cmdGet->P_compliance.N) { + P_compliance.resize(3,3); + for(uint i=0; i<3; i++) for(uint j=0; j<3; j++) P_compliance(i,j) = cmdGet->P_compliance(qIndices(i), qIndices(j)); + } + } + #if DEBUG + LOG(0) << "q_real" << q_real; + LOG(0) << "qDot_real" << qDot_real; + LOG(0) << "q_ref" << q_ref; + LOG(0) << "qDot_ref" << qDot_ref; + #endif + + //-- cap the reference difference + if(q_ref.N==3){ + double err = length(q_ref - q_real); + if(P_compliance.N){ + arr del = q_ref - q_real; + err = ::sqrt(scalarProduct(del, P_compliance*del)); + } + double steering_error = abs(robot->real_steering_rad_s-robot->target_steering_rad_s); + if(err>.3 || steering_error > .1){ //stall! + state.set()->stall = 2; //no progress in reference time! for at least 2 iterations (to ensure continuous stall with multiple threads) + cout <<"STALLING - step:" < .1) { + robot->setSteeringAngle(robot->target_steering_rad_s); + #if DEBUG + std::cout << "Stalling due to steering error:" << std::endl; + std::cout << "- Real steering rad: " << robot->real_steering_rad_s << std::endl; + std::cout << "- Target steering rad: " << robot->target_steering_rad_s << std::endl; + #endif + } + } + } + + //-- compute torques from control message depending on the control type + arr qDotTarget; + qDotTarget.resize(3).setZero(); + + if(q_ref.N==3 && qDot_ref.N==3) + { + arr p_term = q_ref - q_real; + p_term *= Kp; + arr d_term(qDot_ref); + d_term *= Kd; + qDotTarget = p_term + d_term; + } + + //-- data log + if(writeData>0 && !(steps%1)){ + if(!dataFile.is_open()) dataFile.open(STRING("z.ranger"<1){ + qDot_real.modRaw().write(dataFile); + qDot_ref.modRaw().write(dataFile); + qDDot_ref.modRaw().write(dataFile); + } + dataFile <setVelocities(qDotTarget); +} + +void RangerThread::close(){ + robot.reset(); +} + +#else // RAI_RANGER + +RangerThread::~RangerThread(){ NICO } +void RangerThread::init(uint _robotID, const uintA& _qIndices) { NICO } +void RangerThread::step(){ NICO } +void RangerThread::open(){ NICO } +void RangerThread::close(){ NICO } + +#endif diff --git a/src/Ranger/ranger.h b/src/Ranger/ranger.h new file mode 100644 index 0000000..c60f163 --- /dev/null +++ b/src/Ranger/ranger.h @@ -0,0 +1,34 @@ +#pragma once + +#include +#include +#include +#include + +struct RangerController; + +struct RangerThread : rai::RobotAbstraction, Thread { + RangerThread(uint robotID, const uintA& _qIndices, const Var& _cmd, const Var& _state); + ~RangerThread(); + +private: + int robotID=0; + + // Read from rai.cfg + arr Kp, Ki, Kd; + rai::String address; + + uintA qIndices; + uint qIndices_max=0; + + uint steps=0; + ofstream dataFile; + double ctrlTime=0.; + + std::shared_ptr robot; + + void init(uint _robotID, const uintA& _qIndices); + void open(); + void step(); + void close(); +}; diff --git a/src/SLAM/slam.cpp b/src/SLAM/slam.cpp new file mode 100644 index 0000000..243d394 --- /dev/null +++ b/src/SLAM/slam.cpp @@ -0,0 +1,112 @@ +#include "slam.h" + +#define RAI_SLAM +#ifdef RAI_SLAM + +#include +#include +#include + + +const char* slam_base_frame_name = "slam_graph_base"; +const char* mobile_base_frame = "ranger_coll"; +const char* point_cloud_frame_name = "livox_lidar"; + + +void set_new_node_frame(rai::Configuration& C, int node_id) +{ + const std::string intials = "pgo_node"; + const std::string new_frame_name = intials + std::to_string(node_id); + + rai::Frame *mobile_base = C.getFrame(mobile_base_frame); + arr new_pose = mobile_base->getPose(); + + rai::Frame *new_node_frame = C.addFrame(new_frame_name.c_str(), slam_base_frame_name, nullptr, false); + new_node_frame->setShape(rai::ST_marker, {.2}); + new_node_frame->setColor({.0, .0, .7}); + new_pose.elem(2) += .75; + new_node_frame->setPose(new_pose); +} + +void store_point_cloud(rai::Configuration& C, int node_id) +{ + rai::Frame *point_cloud_frame = C.getFrame(point_cloud_frame_name); + arr pc = point_cloud_frame->getMeshPoints(); + + cout << pc << endl; + PointCloud::Ptr cloud(new PointCloud()); + + int point_len = pc.N / 3; + for (int i = 0; i < point_len; i++) { + cloud->points.push_back(PointXYZ(point1[0], point1[1], point1[2])); + } + + // Step 3: Save the PointCloud to a PCD file + const std::string intials = "pgo_node"; + const std::string file_type = ".pcd"; + const std::string filename = intials + std::to_string(node_id) + file_type; + if (io::savePCDFileASCII(filename, *cloud) == -1) { + PCL_ERROR("Couldn't write the PCD file\n"); + return -1; + } +} + +namespace rai{ + + SLAM::SLAM(rai::Configuration& C) + { + if (C.getJointState().N > 3) { + LOG(-1) <<"BotOp SLAM only works for SE(2) for now!"; + } + + new_node_thresh = rai::getParameter("SLAM/newNodeThresh", arr{.3, .3, .75}); + + rai::Frame *base = C.getFrame(slam_base_frame_name, false); + if(!base){ + LOG(0) <<"creating new frame 'slam_graph_base'"; + base = C.addFrame(slam_base_frame_name); + base->setShape(rai::ST_marker, {.3}); + base->setColor({.8, .8, .2}); + } + + q_last = C.getJointState(); + graph_nodes.push_back(q_last); + + node_counter = 1; + set_new_node_frame(C, node_counter); + } + + SLAM::~SLAM() { } + + void SLAM::update(rai::Configuration& C) + { + arr q = C.getJointState(); + + // Create new graph node if q state changed enough + bool over_thresh = false; + for (int i = 0; i < 3; i++) { + if (abs(q.elem(i) - q_last.elem(i)) > new_node_thresh.elem(i)) { + over_thresh = true; + break; + } + } + + if (over_thresh) + { + graph_nodes.push_back(q); + + node_counter++; + set_new_node_frame(C, node_counter); + + q_last = q; + } + } +} //namespace + +#else + +rai::SLAM::SLAM(rai::Configuration& C) { NICO } +rai::SLAM::~SLAM(){ NICO } +void rai::SLAM::update(rai::Configuration& C){ NICO } + +#endif diff --git a/src/SLAM/slam.h b/src/SLAM/slam.h new file mode 100644 index 0000000..daef814 --- /dev/null +++ b/src/SLAM/slam.h @@ -0,0 +1,23 @@ +#pragma once + +#include +#include + + +namespace rai{ + +struct SLAM { + + SLAM(rai::Configuration& C); + ~SLAM(); + + void update(rai::Configuration& C); + +private: + std::vector graph_nodes; + arr new_node_thresh; + arr q_last; + int node_counter; +}; + +} //namespace diff --git a/test/23-omniBase/Makefile b/test/23-omniBase/Makefile new file mode 100755 index 0000000..e553e71 --- /dev/null +++ b/test/23-omniBase/Makefile @@ -0,0 +1,8 @@ +BASE = ../../../botop/rai +BASE2 = ../../../botop + +DEPEND = KOMO Core Geo Kin Gui Optim + +LIBS += -lpthread + +include $(BASE)/_make/generic.mk diff --git a/test/23-omniBase/main.cpp b/test/23-omniBase/main.cpp new file mode 100644 index 0000000..f1ae284 --- /dev/null +++ b/test/23-omniBase/main.cpp @@ -0,0 +1,69 @@ +#include +#include + +//=========================================================================== + +arr getPath(const rai::Configuration& C, int verbose=0){ + KOMO komo(C, 3., 20, 2, false); + komo.addControlObjective({}, 2, 1.); + + komo.addObjective({1.}, FS_positionDiff, {"omnibase", "way1"}, OT_eq, {1e2}); + komo.addObjective({2.}, FS_positionDiff, {"omnibase", "way2"}, OT_eq, {1e2}); + komo.addObjective({1.,2.}, FS_vectorX, {"omnibase"}, OT_eq, {1e2}, {0,1,0}); + komo.addObjective({3.}, FS_positionDiff, {"omnibase", "way0"}, OT_eq, {1e2}); + komo.addObjective({3.}, FS_vectorX, {"omnibase"}, OT_eq, {1e2}, {1,0,0}); + komo.addObjective({3.}, FS_qItself, {}, OT_eq, {1e2}, {}, 1); + + komo.opt.verbose=verbose; + komo.optimize(); + + if(verbose>0){ + // cout <<"TIME OPTIM: total=" <setPosition({0., 0., .1}); + C.addFrame("way1") ->setPosition({.25, 0., .1}); + C.addFrame("way2") ->setPosition({0., .25, .1}); + C.view(false); + + arr q = getPath(C); + + BotOp bot(C, true); + +// bot.hold(true, false); +// bot.wait(C, true, false); return; + + rai::wait(.1); + + bot.move(q, {9.}); + bot.wait(C); + + rai::wait(); +} + +//=========================================================================== + +int MAIN(int argc,char** argv){ + rai::initCmdLine(argc,argv); + +// rnd.clockSeed(); + + moveOmnibase(); + + return 0; +} + diff --git a/test/23-omniBase/rai.cfg b/test/23-omniBase/rai.cfg new file mode 100644 index 0000000..ea16325 --- /dev/null +++ b/test/23-omniBase/rai.cfg @@ -0,0 +1,22 @@ +#Hrate = 1e-0 + +#opt/constrainedMethod = 4 +#opt/verbose = 4 +#KOMO/verbose: 6 +#KOMO/solver: dense +#opt/muInit = 1e2 +#opt/boundedNewton: false +#opt/muInc = 5. +#opt/damping = 1e1 + +#opt/stopTolerance = 1e-2 +#opt/stopGTolerance = 1e-2 + +#opt/muLBInit: 1. +#opt/muLBDec: .1 + +KOMO/verbose: 4 + +#KOMO/useFCL: false + +Omnibase/addresses: [/dev/hidraw3, /dev/hidraw4, /dev/hidraw5] diff --git a/test/24-gamepad/Makefile b/test/24-gamepad/Makefile new file mode 100644 index 0000000..6885fe3 --- /dev/null +++ b/test/24-gamepad/Makefile @@ -0,0 +1,7 @@ +BASE = ../../../botop/rai +BASE2 = ../../../botop + +PLIB = 1 +DEPEND = Core Gamepad + +include $(BASE)/_make/generic.mk diff --git a/test/24-gamepad/main.cpp b/test/24-gamepad/main.cpp new file mode 100644 index 0000000..8baa457 --- /dev/null +++ b/test/24-gamepad/main.cpp @@ -0,0 +1,30 @@ +#include + +void threadedRun() { + + GamepadInterface G; + + std::cout << "Gamepad initialized\n"; + for(;;){ + for (int i = 0; i < G.count; i++) { + G.gamepadState[i].waitForNextRevision(); + std::cout << "______ Gamepad " << i << " ______" << std::endl; + std::cout << G.getButtonPressed(i) < + +#define ON_REAL true + + +void justSpin(){ + rai::Configuration C; + // C.addFile(rai::raiPath("../rai-robotModels/ranger/ranger_simplified.g")); + C.addFile(rai::raiPath("../rai-robotModels/scenarios/panda_ranger.g")); + // C.addFile(rai::raiPath("../rai-robotModels/scenarios/pandaSingle.g")); + C.view(true); + // return; + + BotOp bot(C, ON_REAL); + + double time = 1.; + + arr q = arr{0., 0., 0., 0, -0.5, 0, -2, 0, 2, -0.5}; + bot.moveTo(q, {time}); + bot.wait(C); + + q = arr{.2, 0., 0., 0, -0.5, 0, -2, 0, 2.5, -0.5}; + bot.moveTo(q, {time}); + bot.wait(C); + + q = arr{.0, 0., 0., 0, -0.5, 0, -2, 0, 2, -0.5}; + bot.moveTo(q, {time}); + bot.wait(C); + + // q = arr{.1, .2, -1.57, 0, -0.5, 0, -2, 0, 2, -0.5}; + // bot.moveTo(q, {time}); + // bot.wait(C); + + // q = arr{.1, .2, -1.57, 0, -0.5, 0, -2, 0, 2.5, -0.5}; + // bot.moveTo(q, {time}); + // bot.wait(C); + + // q = arr{.1, .2, -1.57, 0, -0.5, 0, -2, 0, 2, -0.5}; + // bot.moveTo(q, {time}); + // bot.wait(C); + + // q = arr{.1, .2, 0, 0, -0.5, 0, -2, 0, 2, -0.5}; + // bot.moveTo(q, {time}); + // bot.wait(C); + + // q = arr{0., 0., 0., 0, -0.5, 0, -2, 0, 2, -0.5}; + // bot.moveTo(q, {time}); + // bot.wait(C); + + // arr q = arr{0, 0, 0, 0, -0.5, 0, -2, 0, 2.5, -0.5}; + // // arr q = arr{0, -0.5, 0, -2, 0, 2.5, -0.5}; + // bot.moveTo(q, {time}); + // bot.wait(C); + + // q = arr{0, 0, 0, 0, -0.5, 0, -2, 0, 2, -0.5}; + // // q = arr{0, -0.5, 0, -2, 0, 2, -0.5}; + // bot.moveTo(q, {time}); + // bot.wait(C); + C.view(true); +} + +int main(int argc, char** argv){ + rai::initCmdLine(argc, argv); + + // moveRanger(); + // moveRangerLoop(); + justSpin(); + + return 0; +} diff --git a/test/25-ranger/rai.cfg b/test/25-ranger/rai.cfg new file mode 100644 index 0000000..7765380 --- /dev/null +++ b/test/25-ranger/rai.cfg @@ -0,0 +1,23 @@ +#Hrate = 1e-0 + +#opt/constrainedMethod = 4 +#opt/verbose = 4 +#KOMO/verbose: 6 +#KOMO/solver: dense +#opt/muInit = 1e2 +#opt/boundedNewton: false +#opt/muInc = 5. +#opt/damping = 1e1 + +#opt/stopTolerance = 1e-2 +#opt/stopGTolerance = 1e-2 + +#opt/muLBInit: 1. +#opt/muLBDec: .1 + +KOMO/verbose: 4 + +#KOMO/useFCL: false + +Ranger/Kp: [1, 1, 1] +Ranger/Kd: [0, 0, 0] diff --git a/test/26-livox/Makefile b/test/26-livox/Makefile new file mode 100644 index 0000000..a6d94e4 --- /dev/null +++ b/test/26-livox/Makefile @@ -0,0 +1,8 @@ +BASE = ../../../rai +BASE2 = ../../ + +EIGEN = 1 + +DEPEND = Core Gui OptiTrack + +include $(BASE)/_make/generic.mk diff --git a/test/26-livox/main.cpp b/test/26-livox/main.cpp new file mode 100644 index 0000000..ea02cef --- /dev/null +++ b/test/26-livox/main.cpp @@ -0,0 +1,24 @@ +#include +#include + +const char *USAGE = + "\nTest of low-level (without bot interface) Livox interface" + "\n"; + +int main(int argc,char **argv){ + rai::initCmdLine(argc, argv); + + cout < + +#define ON_REAL true + + +void justSpin(){ + rai::Configuration C; + // C.addFile(rai::raiPath("../rai-robotModels/ranger/ranger_simplified.g")); + C.addFile(rai::raiPath("../rai-robotModels/scenarios/panda_ranger.g")); + // C.addFile(rai::raiPath("../rai-robotModels/scenarios/pandaSingle.g")); + + BotOp bot(C, ON_REAL); + bot.sync(C); + + C.view(true); + double time = 1.; + + // arr q = arr{0., 0., -1.57, 0, -0.5, 0, -2, 0, 2, -0.5}; + // bot.moveTo(q, {time}); + // bot.wait(C); + + // q = arr{.1, .2, -1.57, 0, -0.5, 0, -2, 0, 2, -0.5}; + // bot.moveTo(q, {time}); + // bot.wait(C); + + // q = arr{.1, .2, -1.57, 0, -0.5, 0, -2, 0, 2.5, -0.5}; + // bot.moveTo(q, {time}); + // bot.wait(C); + + // q = arr{.1, .2, -1.57, 0, -0.5, 0, -2, 0, 2, -0.5}; + // bot.moveTo(q, {time}); + // bot.wait(C); + + // q = arr{.1, .2, 0, 0, -0.5, 0, -2, 0, 2, -0.5}; + // bot.moveTo(q, {time}); + // bot.wait(C); + + // q = arr{0., 0., 0., 0, -0.5, 0, -2, 0, 2, -0.5}; + // bot.moveTo(q, {time}); + // bot.wait(C); + + arr q = arr{0, 0, 0, 0, -0.5, 0, -2, 0, 2.5, -0.5}; + // arr q = arr{0, -0.5, 0, -2, 0, 2.5, -0.5}; + bot.moveTo(q, {time}); + bot.wait(C); + + q = arr{0, 0, 0, 0, -0.5, 0, -2, 0, 2, -0.5}; + // q = arr{0, -0.5, 0, -2, 0, 2, -0.5}; + bot.moveTo(q, {time}); + bot.wait(C); + C.view(true); +} + +int main(int argc, char** argv){ + rai::initCmdLine(argc, argv); + + // moveRanger(); + // moveRangerLoop(); + justSpin(); + + return 0; +} diff --git a/test/27-livox_ranger/mid360_config.json b/test/27-livox_ranger/mid360_config.json new file mode 100644 index 0000000..8a32181 --- /dev/null +++ b/test/27-livox_ranger/mid360_config.json @@ -0,0 +1,22 @@ +{ + "MID360": { + "lidar_net_info" : { + "cmd_data_port" : 56100, + "push_msg_port" : 56200, + "point_data_port": 56300, + "imu_data_port" : 56400, + "log_data_port" : 56500 + }, + "host_net_info" : [ + { + "host_ip" : "192.168.1.100", + "multicast_ip" : "224.1.1.5", + "cmd_data_port" : 56101, + "push_msg_port" : 56201, + "point_data_port": 56301, + "imu_data_port" : 56401, + "log_data_port" : 56501 + } + ] + } +} diff --git a/test/27-livox_ranger/rai.cfg b/test/27-livox_ranger/rai.cfg new file mode 100644 index 0000000..f6925f7 --- /dev/null +++ b/test/27-livox_ranger/rai.cfg @@ -0,0 +1,2 @@ +livox/max_points: 30000 +bot/useLivox: true diff --git a/test/31-gamepad/Makefile b/test/31-gamepad/Makefile new file mode 100644 index 0000000..5561e23 --- /dev/null +++ b/test/31-gamepad/Makefile @@ -0,0 +1,7 @@ +BASE = ../../../rai +BASE2 = ../.. + +PLIB = 1 +DEPEND = Core Gamepad + +include $(BASE)/_make/generic.mk diff --git a/test/31-gamepad/main.cpp b/test/31-gamepad/main.cpp new file mode 100644 index 0000000..a981197 --- /dev/null +++ b/test/31-gamepad/main.cpp @@ -0,0 +1,26 @@ +#include + +void threadedRun() { + + GamepadInterface G; + + for(;;){ + G.gamepadState.waitForNextRevision(); + cout <<"\r" < +#include +#include +#include + +#include +#include +#include +#include +#include +#include // for memcpy + +#define ON_REAL true + +void control_loop() +{ + rai::Configuration C; + C.addFile(rai::raiPath("../rai-robotModels/ranger/ranger_simplified.g")); + BotOp bot(C, ON_REAL); + GamepadInterface G; + rai::Livox lidar; + double timeCost = 1; + double max_disp = .3; + double max_disp_ang = .3; + + // For saving data + std::time_t timestamp = std::time(nullptr); + std::string data_dir = "log_data" + std::to_string(timestamp); + std::filesystem::create_directory(data_dir); + arr last_q = bot.get_q(); + + while(1) + { + G.gamepadState.waitForNextRevision(); + if(G.quitSignal.get()) break; + + double x_rel = G.gamepadState.get()().elem(4) * -1; + double y_rel = G.gamepadState.get()().elem(3) * -1; + double phi_rel = G.gamepadState.get()().elem(5) * -1; + if (x_rel < 0.2 && x_rel > -0.2) x_rel = 0.0; + if (y_rel < 0.2 && y_rel > -0.2) y_rel = 0.0; + if (phi_rel < 0.2 && phi_rel > -0.2) phi_rel = 0.0; + + arr current_q = bot.get_q(); + + float theta = current_q.elem(2); + double x_rel_rot = x_rel * cos(theta) - y_rel * sin(theta); + double y_rel_rot = x_rel * sin(theta) + y_rel * cos(theta); + + arr rel_target = {x_rel_rot * max_disp, y_rel_rot * max_disp, phi_rel * max_disp_ang}; + arr target_q = current_q + rel_target; + bot.moveTo(target_q, {timeCost}, true); + bot.sync(C, 0.); + lidar.pull(C); + + float d_x = current_q.elem(0) - last_q.elem(0); + float d_y = current_q.elem(1) - last_q.elem(1); + float eucl_dist = sqrt(d_x*d_x + d_y*d_y); + float ang_diff = fmodf(fabsf(fmodf(current_q.elem(2) - last_q.elem(2) + M_PI, 2 * M_PI) - M_PI), 2 * M_PI); + + if (eucl_dist > .5 || ang_diff > M_PI*0.333333) { + std::time_t timestamp = std::time(nullptr); + std::string filename = data_dir + "/" + std::to_string(timestamp) + "pc.txt"; + + std::ofstream file(filename); + if (file.is_open()) { + file << current_q.elem(0) << " " << current_q.elem(1) << " " << current_q.elem(2) << "\n"; + for (const auto& point : lidar.points_message.points) { + file << point.x << " " << point.y << " " << point.z << "\n"; + } + file.close(); + } + last_q = bot.get_q(); + } + } +} + +int main(int argc, char** argv){ + rai::initCmdLine(argc, argv); + + control_loop(); + + return 0; +} diff --git a/test/32-ranger_control/main.py b/test/32-ranger_control/main.py new file mode 100644 index 0000000..4743bd1 --- /dev/null +++ b/test/32-ranger_control/main.py @@ -0,0 +1,57 @@ +import os +import rowan +import numpy as np +import robotic as ry + +qs = [] +pcs = [] + +for file_name in sorted(os.listdir("log_data")): + file_path = os.path.join("log_data", file_name) + with open(file_path, 'r') as f: + lines = f.readlines() + + # Parse the first line + first = list(map(float, lines[0].strip().split())) + if len(first) != 3: + print(f"Warning: First line in {file_name} is not 3 floats. Skipping file.") + continue + qs.append(first) + + # Parse remaining lines + rest = [] + for line in lines[1:]: + # if np.random.random() < .01: + if np.random.random() < 1: + nums = list(map(float, line.strip().split())) + if len(nums) == 3: + rest.append(nums) + else: + print(f"Warning: Skipping malformed line in {file_name}: {line.strip()}") + pcs.append(rest) + +timestamp_count = len(qs) +point_cloud_size = len(pcs[0]) +print("Total Timestamps: ", timestamp_count) +print("Point Cloud Size: ", point_cloud_size) + +C = ry.Config() + +for i, q in enumerate(qs): + + pos = [q[0], q[1], .1] + quat = rowan.from_euler(0, 0, q[2], convention="xyz") + + C.addFrame(f"pose{i}") \ + .setShape(ry.ST.marker, [.3]) \ + .setQuaternion(quat) \ + .setPosition(pos) + + C.addFrame(f"point_cloud{i}", f"pose{i}") \ + .setPointCloud(pcs[i]) \ + .setColor([0., 0., 1., .05]) + +for i in range(timestamp_count): + C.getFrame(f"point_cloud{i}").setColor([1., 0., 0., 1.]) + C.view(True) + C.getFrame(f"point_cloud{i}").setColor([0., 0., 1., .05]) diff --git a/test/32-ranger_control/mid360_config.json b/test/32-ranger_control/mid360_config.json new file mode 100644 index 0000000..8a32181 --- /dev/null +++ b/test/32-ranger_control/mid360_config.json @@ -0,0 +1,22 @@ +{ + "MID360": { + "lidar_net_info" : { + "cmd_data_port" : 56100, + "push_msg_port" : 56200, + "point_data_port": 56300, + "imu_data_port" : 56400, + "log_data_port" : 56500 + }, + "host_net_info" : [ + { + "host_ip" : "192.168.1.100", + "multicast_ip" : "224.1.1.5", + "cmd_data_port" : 56101, + "push_msg_port" : 56201, + "point_data_port": 56301, + "imu_data_port" : 56401, + "log_data_port" : 56501 + } + ] + } +} diff --git a/test/32-ranger_control/rai.cfg b/test/32-ranger_control/rai.cfg new file mode 100644 index 0000000..a87b1f7 --- /dev/null +++ b/test/32-ranger_control/rai.cfg @@ -0,0 +1,27 @@ +#Hrate = 1e-0 + +#opt/constrainedMethod = 4 +#opt/verbose = 4 +#KOMO/verbose: 6 +#KOMO/solver: dense +#opt/muInit = 1e2 +#opt/boundedNewton: false +#opt/muInc = 5. +#opt/damping = 1e1 + +#opt/stopTolerance = 1e-2 +#opt/stopGTolerance = 1e-2 + +#opt/muLBInit: 1. +#opt/muLBDec: .1 + +KOMO/verbose: 4 + +#KOMO/useFCL: false + +Ranger/Kp: [5, 5, 5] +Ranger/Kd: [0, 0, 0] +livox/max_points: 15000 + +#bot/useGripper: false +#bot/blockRealRobot: false