From 6a561e091f9533ebd13048fe0aaae6a830d2e0df Mon Sep 17 00:00:00 2001 From: Tupryk Date: Mon, 4 Dec 2023 14:57:00 +0100 Subject: [PATCH 01/30] omnibase test --- src/Omnibase/omnibase.cpp | 19 ++++++----- src/Omnibase/omnibase.h | 2 +- test/23-omniBase/Makefile | 8 +++++ test/23-omniBase/main.cpp | 66 +++++++++++++++++++++++++++++++++++++++ test/23-omniBase/rai.cfg | 20 ++++++++++++ 5 files changed, 106 insertions(+), 9 deletions(-) create mode 100755 test/23-omniBase/Makefile create mode 100644 test/23-omniBase/main.cpp create mode 100644 test/23-omniBase/rai.cfg diff --git a/src/Omnibase/omnibase.cpp b/src/Omnibase/omnibase.cpp index 9322a9b..920d02a 100644 --- a/src/Omnibase/omnibase.cpp +++ b/src/Omnibase/omnibase.cpp @@ -13,7 +13,7 @@ struct OmnibaseController{ arr qLast, sLast; //q: joint state; s: motor state #endif - OmnibaseController(rai::String address){ + OmnibaseController(const StringA& addresses){ #ifdef USE_FAKE fake_q.resize(3).setZero(); fake_qDot.resize(3).setZero(); @@ -21,10 +21,9 @@ struct OmnibaseController{ //-- launch 3 motors motors.resize(3); for(uint i=0;i(address, 0x04d8, 0xf79a); + motors(i) = make_shared(addresses(i), 0x04d8, 0xf79a); motors(i)->runReset(); //resets position counter motors(i)->runTorque(0.); //starts torque mode - address(address.N-1)++; } qLast = zeros(3); sLast = zeros(motors.N); @@ -44,8 +43,12 @@ struct OmnibaseController{ arr getJacobian(){ //return Jacobian based on qLast - arr J(3,3); - NIY; + arr J = { + -1./3., 1./::sqrt(3.), 1./3., + -1./3., -1./::sqrt(3.), 1./3., + 2./3., 0., 1./3. + }; + J.resize(3,3); return J; } @@ -116,10 +119,10 @@ void OmnibaseThread::init(uint _robotID, const uintA& _qIndices) { LOG(0) << "Omnibase: Kp_freq:" << Kp_freq << " Kd_ratio:" << Kd_ratio; //-- 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); //-- initialize state and ctrl with first state arr q_real, qDot_real; diff --git a/src/Omnibase/omnibase.h b/src/Omnibase/omnibase.h index 69b52c3..d74636b 100644 --- a/src/Omnibase/omnibase.h +++ b/src/Omnibase/omnibase.h @@ -14,7 +14,7 @@ struct OmnibaseThread : rai::RobotAbstraction, Thread { private: int robotID=0; arr Kp_freq, Kd_ratio; //read from rai.cfg - rai::String address; + StringA addresses; uintA qIndices; uint qIndices_max=0; 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..7156353 --- /dev/null +++ b/test/23-omniBase/main.cpp @@ -0,0 +1,66 @@ +#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({1., 0., .1}); + C.addFrame("way2") ->setPosition({0., 1., .1}); + C.view(false); + + arr q = getPath(C); + + BotOp bot(C, true); + + rai::wait(.1); + + bot.move(q, {5.}); + 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..f120390 --- /dev/null +++ b/test/23-omniBase/rai.cfg @@ -0,0 +1,20 @@ +#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 From e7710956b2e7f9033f6c03fe2d925bd3b7c30b59 Mon Sep 17 00:00:00 2001 From: Tupryk Date: Tue, 5 Dec 2023 18:58:15 +0100 Subject: [PATCH 02/30] omnibase kinda moves --- src/Omnibase/omnibase.cpp | 78 ++++++++++++++++++++++++++++++--------- src/Omnibase/omnibase.h | 2 +- test/23-omniBase/main.cpp | 9 +++-- test/23-omniBase/rai.cfg | 4 ++ 4 files changed, 71 insertions(+), 22 deletions(-) diff --git a/src/Omnibase/omnibase.cpp b/src/Omnibase/omnibase.cpp index 920d02a..2517f62 100644 --- a/src/Omnibase/omnibase.cpp +++ b/src/Omnibase/omnibase.cpp @@ -3,7 +3,7 @@ #ifdef RAI_OMNIBASE -#define USE_FAKE +//#define USE_FAKE struct OmnibaseController{ #ifdef USE_FAKE @@ -22,6 +22,13 @@ struct OmnibaseController{ motors.resize(3); for(uint i=0;i(addresses(i), 0x04d8, 0xf79a); + cout <<"model name: '" <getModelName() <<"'" <getSerialNumber() <<"'" <getAddress() <<"'" <getMotorTemperature() <getVoltage() <setPID(300, 0, 200, 100, 2, 0); motors(i)->runReset(); //resets position counter motors(i)->runTorque(0.); //starts torque mode } @@ -43,12 +50,25 @@ struct OmnibaseController{ arr getJacobian(){ //return Jacobian based on qLast + double gears = 3.; + double r = .06; + double R = .3; arr J = { - -1./3., 1./::sqrt(3.), 1./3., - -1./3., -1./::sqrt(3.), 1./3., - 2./3., 0., 1./3. + -.25, -.25, .5, + .25*sqrt(3.), -.25*sqrt(3.), 0., + 1/(3.*R), 1/(3.*R), 1./(3.*R) }; - J.resize(3,3); + J *= r/gears; + 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; } @@ -74,12 +94,14 @@ struct OmnibaseController{ q = qLast + qDelta; qDot = Jacobian * sDot; +// cout <<"state: q: " <setTarget(u_s(i)/motors(i)->maxTorque*32767); + motors(i)->setTarget(u_motors(i)/motors(i)->maxTorque*32767); } #endif } }; +OmnibaseThread::OmnibaseThread(uint robotID, const uintA &_qIndices, const Var &_cmd, const Var &_state) + : RobotAbstraction(_cmd, _state), Thread("OmnibaseThread", .02){ + init(robotID, _qIndices); +} + OmnibaseThread::~OmnibaseThread(){ - LOG(0) <<"shutting down Omnibase " <.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 0 if(q_ref.N==3){ u += Kp * (q_ref - q_real); } if(qDot_ref.N==3){ u += Kd % (qDot_ref - qDot_real); } +#else + if(q_ref.N==3){ + arr delta_motor = Jinv * (q_ref - q_real); + u += .1 * delta_motor; + } + if(qDot_ref.N==3){ + qDot_ref.setZero(); + arr delta_motor = Jinv * (qDot_ref - qDot_real); + u += .005 * delta_motor; + } +#endif //-- project with compliance if(P_compliance.N) u = P_compliance * u; diff --git a/src/Omnibase/omnibase.h b/src/Omnibase/omnibase.h index d74636b..8176f84 100644 --- a/src/Omnibase/omnibase.h +++ b/src/Omnibase/omnibase.h @@ -8,7 +8,7 @@ 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: diff --git a/test/23-omniBase/main.cpp b/test/23-omniBase/main.cpp index 7156353..f1ae284 100644 --- a/test/23-omniBase/main.cpp +++ b/test/23-omniBase/main.cpp @@ -36,17 +36,20 @@ void moveOmnibase(){ C.addFile(rai::raiPath("../rai-robotModels/omnibase/omnibase.g")); C.addFrame("way0") ->setPosition({0., 0., .1}); - C.addFrame("way1") ->setPosition({1., 0., .1}); - C.addFrame("way2") ->setPosition({0., 1., .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, {5.}); + bot.move(q, {9.}); bot.wait(C); rai::wait(); diff --git a/test/23-omniBase/rai.cfg b/test/23-omniBase/rai.cfg index f120390..136c2b3 100644 --- a/test/23-omniBase/rai.cfg +++ b/test/23-omniBase/rai.cfg @@ -18,3 +18,7 @@ KOMO/verbose: 4 #KOMO/useFCL: false + +Omnibase/addresses: [/dev/hidraw3, /dev/hidraw4, /dev/hidraw5] +Omnibase/Kp_freq: [5., 5., 5.] +Omnibase/Kd_ratio: [.1, .1, .1] From e59e601fbba4cbd0e4832d5baf9add1681245ec0 Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Wed, 8 May 2024 14:45:08 +0200 Subject: [PATCH 03/30] Updated PD and jacobian parameters --- src/Omnibase/omnibase.cpp | 63 ++++++++++++++------------------------- src/Omnibase/omnibase.h | 2 +- 2 files changed, 24 insertions(+), 41 deletions(-) diff --git a/src/Omnibase/omnibase.cpp b/src/Omnibase/omnibase.cpp index 2517f62..6d46440 100644 --- a/src/Omnibase/omnibase.cpp +++ b/src/Omnibase/omnibase.cpp @@ -1,6 +1,9 @@ #include "omnibase.h" #include "SimplexMotion.h" + +#define RAI_OMNIBASE 1 + #ifdef RAI_OMNIBASE //#define USE_FAKE @@ -13,7 +16,7 @@ struct OmnibaseController{ arr qLast, sLast; //q: joint state; s: motor state #endif - OmnibaseController(const StringA& addresses){ + OmnibaseController(const StringA& addresses, double Kp, double Kd){ #ifdef USE_FAKE fake_q.resize(3).setZero(); fake_qDot.resize(3).setZero(); @@ -28,9 +31,10 @@ struct OmnibaseController{ cout <<"motor temperature: " <getMotorTemperature() <getVoltage() <setPID(300, 0, 200, 100, 2, 0); motors(i)->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); @@ -50,18 +54,22 @@ struct OmnibaseController{ arr getJacobian(){ //return Jacobian based on qLast - double gears = 3.; + double gears = 4.2; double r = .06; - double R = .3; + double R = .35; +// arr J = { +// -.25, -.25, .5, +// .25*sqrt(3.), -.25*sqrt(3.), 0., +// 1/(3.*R), 1/(3.*R), 1./(3.*R) +// }; arr J = { - -.25, -.25, .5, - .25*sqrt(3.), -.25*sqrt(3.), 0., - 1/(3.*R), 1/(3.*R), 1./(3.*R) + -.5, -.5, 1., + .5*sqrt(3.), -.5*sqrt(3.), 0., + 1/(3.*R), 1/(3.*R), 1./(3.*R) }; J *= r/gears; J.reshape(3,3); - double phi = qLast(2); arr rot = eye(3); rot(0,0) = rot(1,1) = cos(phi); @@ -114,7 +122,7 @@ struct OmnibaseController{ CHECK_EQ(u_motors.N, motors.N, "control signal has wrong dimensionality"); clip(u_motors, -1., 1.); - cout <<" sending motor torques: " <setTarget(u_motors(i)/motors(i)->maxTorque*32767); } @@ -140,9 +148,9 @@ void OmnibaseThread::init(uint _robotID, const uintA& _qIndices) { qIndices_max = rai::max(qIndices); //-- basic Kp Kd settings for reference control mode - Kp_freq = rai::getParameter("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; + Kp = rai::getParameter("Omnibase/Kp", .2); + Kd = rai::getParameter("Omnibase/Kd", .02); + LOG(0) << "Omnibase: Kp:" << Kp << " Kd:" << Kd; //-- get robot address addresses = rai::getParameter("Omnibase/addresses", {"/dev/hidraw0", "/dev/hidraw1", "/dev/hidraw2"}); @@ -156,7 +164,7 @@ void OmnibaseThread::init(uint _robotID, const uintA& _qIndices) { void OmnibaseThread::open(){ // connect to robot - robot = make_shared(addresses); + robot = make_shared(addresses, Kp, Kd); //-- initialize state and ctrl with first state arr q_real, qDot_real; @@ -261,22 +269,6 @@ void OmnibaseThread::step(){ //-- compute torques from control message depending on the control type arr u; - //-- compute desired torques - arr Kp(3), Kd(3); - CHECK_EQ(Kp_freq.N, 3,""); - CHECK_EQ(Kd_ratio.N, 3,""); - for(uint i=0;i<3;i++){ - double freq = Kp_freq.elem(i); - Kp.elem(i) = freq*freq; - Kd.elem(i) = 2.*Kd_ratio.elem(i)*freq; - } - - if(P_compliance.N){ - Kp = P_compliance * (Kp % P_compliance); - }else{ - Kp = diag(Kp); - } - //-- initialize zero torques u.resize(3).setZero(); qDot_ref.setZero(); //REMOVE LATER!!! @@ -285,24 +277,15 @@ void OmnibaseThread::step(){ arr Jinv = pseudoInverse(J, NoArr, 1e-6); //-- add feedback term -#if 0 - if(q_ref.N==3){ - u += Kp * (q_ref - q_real); - } - if(qDot_ref.N==3){ - u += Kd % (qDot_ref - qDot_real); - } -#else if(q_ref.N==3){ arr delta_motor = Jinv * (q_ref - q_real); - u += .1 * delta_motor; + u += Kp * delta_motor; } if(qDot_ref.N==3){ qDot_ref.setZero(); arr delta_motor = Jinv * (qDot_ref - qDot_real); - u += .005 * delta_motor; + u += Kd * delta_motor; } -#endif //-- project with compliance if(P_compliance.N) u = P_compliance * u; diff --git a/src/Omnibase/omnibase.h b/src/Omnibase/omnibase.h index 8176f84..a1af5fc 100644 --- a/src/Omnibase/omnibase.h +++ b/src/Omnibase/omnibase.h @@ -13,7 +13,7 @@ struct OmnibaseThread : rai::RobotAbstraction, Thread { private: int robotID=0; - arr Kp_freq, Kd_ratio; //read from rai.cfg + double Kp, Kd; //read from rai.cfg StringA addresses; uintA qIndices; From e5f3b14dca97914c512b719bf53fb49dc990d361 Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Wed, 8 May 2024 15:22:50 +0200 Subject: [PATCH 04/30] Update omnibase.cpp --- src/Omnibase/omnibase.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/Omnibase/omnibase.cpp b/src/Omnibase/omnibase.cpp index cad8087..f659d7a 100644 --- a/src/Omnibase/omnibase.cpp +++ b/src/Omnibase/omnibase.cpp @@ -2,8 +2,6 @@ #include "SimplexMotion.h" -#define RAI_OMNIBASE 1 - #ifdef RAI_OMNIBASE //#define USE_FAKE From 9ad59ab4b30c8401ad3e997fc229c6e159015dec Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Wed, 8 May 2024 15:23:55 +0200 Subject: [PATCH 05/30] Update rai.cfg --- test/23-omniBase/rai.cfg | 2 -- 1 file changed, 2 deletions(-) diff --git a/test/23-omniBase/rai.cfg b/test/23-omniBase/rai.cfg index 136c2b3..ea16325 100644 --- a/test/23-omniBase/rai.cfg +++ b/test/23-omniBase/rai.cfg @@ -20,5 +20,3 @@ KOMO/verbose: 4 #KOMO/useFCL: false Omnibase/addresses: [/dev/hidraw3, /dev/hidraw4, /dev/hidraw5] -Omnibase/Kp_freq: [5., 5., 5.] -Omnibase/Kd_ratio: [.1, .1, .1] From 7db32bed57f80f30aea48b6e739b38aedcfb260e Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Wed, 8 May 2024 16:01:00 +0200 Subject: [PATCH 06/30] Changed Omnibase x sign in jacobian --- src/Omnibase/omnibase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Omnibase/omnibase.cpp b/src/Omnibase/omnibase.cpp index f659d7a..e392647 100644 --- a/src/Omnibase/omnibase.cpp +++ b/src/Omnibase/omnibase.cpp @@ -61,7 +61,7 @@ struct OmnibaseController{ // 1/(3.*R), 1/(3.*R), 1./(3.*R) // }; arr J = { - -.5, -.5, 1., + .5, .5, -1., .5*sqrt(3.), -.5*sqrt(3.), 0., 1/(3.*R), 1/(3.*R), 1./(3.*R) }; From f8030a50d57b15f407aa3ca2dfa70aeffb7acf5d Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Fri, 28 Jun 2024 14:10:21 +0200 Subject: [PATCH 07/30] new pd controller and adjusted parameters --- src/Omnibase/SimplexMotion-com.cpp | 3 +- src/Omnibase/omnibase.cpp | 172 +++++++--------- src/Omnibase/omnibase.h | 3 +- src/Omnibase/omnibase_old.cpp | 308 +++++++++++++++++++++++++++++ src/Omnibase/omnibase_old.h | 34 ++++ 5 files changed, 419 insertions(+), 101 deletions(-) create mode 100644 src/Omnibase/omnibase_old.cpp create mode 100644 src/Omnibase/omnibase_old.h 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 e392647..c806738 100644 --- a/src/Omnibase/omnibase.cpp +++ b/src/Omnibase/omnibase.cpp @@ -1,53 +1,52 @@ #include "omnibase.h" #include "SimplexMotion.h" +#define RAI_OMNIBASE #ifdef RAI_OMNIBASE -//#define USE_FAKE +/* +TODO: +- change: motors(i)->runTorque(0.); +- should we keep this: if(P_compliance.N) u = P_compliance * u; +- clip velocities at setVelocities: clip(v_motors, -1., 1.); what does clip do? +- Is this ok? what units is it?: motors(i)->runSpeed(v_motors(i)); +*/ 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(const StringA& addresses, double Kp, double Kd){ -#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(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"; + 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;isetTarget(u_motors(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", .02){ + : RobotAbstraction(_cmd, _state), Thread("OmnibaseThread", .015){ init(robotID, _qIndices); + writeData = 10; } OmnibaseThread::~OmnibaseThread(){ @@ -139,16 +118,22 @@ OmnibaseThread::~OmnibaseThread(){ } void OmnibaseThread::init(uint _robotID, const uintA& _qIndices) { - robotID=_robotID; - qIndices=_qIndices; + robotID=_robotID; + qIndices=_qIndices; - CHECK_EQ(qIndices.N, 3, ""); + CHECK_EQ(qIndices.N, 3, ""); qIndices_max = rai::max(qIndices); //-- basic Kp Kd settings for reference control mode - Kp = rai::getParameter("Omnibase/Kp", .2); - Kd = rai::getParameter("Omnibase/Kd", .02); - LOG(0) << "Omnibase: Kp:" << Kp << " Kd:" << Kd; + 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); + + LOG(0) << "Omnibase: Kp:" << Kp << " Ki:" << Ki << " Kd:" << Kd << "KdDelay:" << KdDelay << "KiLimit:" << KiLimit << " Friction:" << Friction; //-- get robot address addresses = rai::getParameter("Omnibase/addresses", {"/dev/hidraw0", "/dev/hidraw1", "/dev/hidraw2"}); @@ -162,25 +147,25 @@ void OmnibaseThread::init(uint _robotID, const uintA& _qIndices) { void OmnibaseThread::open(){ // connect to robot - robot = make_shared(addresses, Kp, Kd); + 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(){ @@ -189,8 +174,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; @@ -265,46 +248,37 @@ void OmnibaseThread::step(){ } //-- compute torques from control message depending on the control type - arr u; + arr v; //-- initialize zero torques - u.resize(3).setZero(); - qDot_ref.setZero(); //REMOVE LATER!!! - + v.resize(3).setZero(); arr J = robot->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; + v += outer_Kp * delta_motor; } + if(qDot_ref.N==3){ - qDot_ref.setZero(); - arr delta_motor = Jinv * (qDot_ref - qDot_real); - u += Kd * delta_motor; + 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 a1af5fc..561895e 100644 --- a/src/Omnibase/omnibase.h +++ b/src/Omnibase/omnibase.h @@ -13,7 +13,8 @@ struct OmnibaseThread : rai::RobotAbstraction, Thread { private: int robotID=0; - double Kp, Kd; //read from rai.cfg + double outer_Kp; + int Kp, Ki, Kd, KdDelay, KiLimit, Friction; //read from rai.cfg StringA addresses; uintA qIndices; diff --git a/src/Omnibase/omnibase_old.cpp b/src/Omnibase/omnibase_old.cpp new file mode 100644 index 0000000..f4d9b6a --- /dev/null +++ b/src/Omnibase/omnibase_old.cpp @@ -0,0 +1,308 @@ +#include "omnibase.h" +#include "SimplexMotion.h" + +#define RAI_OMNIBASE + +#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 diff --git a/src/Omnibase/omnibase_old.h b/src/Omnibase/omnibase_old.h new file mode 100644 index 0000000..c8c0aca --- /dev/null +++ b/src/Omnibase/omnibase_old.h @@ -0,0 +1,34 @@ +#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(); +}; From 1478d3271b848718a73756dd0321f7cdda23e3ab Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Fri, 5 Jul 2024 09:51:09 +0200 Subject: [PATCH 08/30] Velocity controller done --- src/Omnibase/omnibase.cpp | 9 --------- src/Omnibase/omnibase_old.cpp | 3 ++- src/Omnibase/omnibase_old.h | 3 +++ 3 files changed, 5 insertions(+), 10 deletions(-) diff --git a/src/Omnibase/omnibase.cpp b/src/Omnibase/omnibase.cpp index c806738..aff8569 100644 --- a/src/Omnibase/omnibase.cpp +++ b/src/Omnibase/omnibase.cpp @@ -1,18 +1,9 @@ #include "omnibase.h" #include "SimplexMotion.h" -#define RAI_OMNIBASE #ifdef RAI_OMNIBASE -/* -TODO: -- change: motors(i)->runTorque(0.); -- should we keep this: if(P_compliance.N) u = P_compliance * u; -- clip velocities at setVelocities: clip(v_motors, -1., 1.); what does clip do? -- Is this ok? what units is it?: motors(i)->runSpeed(v_motors(i)); -*/ - struct OmnibaseController{ rai::Array> motors; //three motors arr qLast, sLast; //q: joint state; s: motor state diff --git a/src/Omnibase/omnibase_old.cpp b/src/Omnibase/omnibase_old.cpp index f4d9b6a..055bf25 100644 --- a/src/Omnibase/omnibase_old.cpp +++ b/src/Omnibase/omnibase_old.cpp @@ -1,7 +1,7 @@ +#if 0 #include "omnibase.h" #include "SimplexMotion.h" -#define RAI_OMNIBASE #ifdef RAI_OMNIBASE @@ -306,3 +306,4 @@ void OmnibaseThread::open(){ NICO } void OmnibaseThread::close(){ NICO } #endif +#endif diff --git a/src/Omnibase/omnibase_old.h b/src/Omnibase/omnibase_old.h index c8c0aca..555bbff 100644 --- a/src/Omnibase/omnibase_old.h +++ b/src/Omnibase/omnibase_old.h @@ -1,3 +1,4 @@ +#if 0 #pragma once #include @@ -32,3 +33,5 @@ struct OmnibaseThread : rai::RobotAbstraction, Thread { void step(); void close(); }; + +#endif From a06ccb2304f9956b2f96c60f450bf80039ff7587 Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Fri, 5 Jul 2024 09:56:02 +0200 Subject: [PATCH 09/30] gamepad test scripts --- test/24-gamepad/Makefile | 7 +++++++ test/24-gamepad/main.cpp | 30 ++++++++++++++++++++++++++++++ 2 files changed, 37 insertions(+) create mode 100644 test/24-gamepad/Makefile create mode 100644 test/24-gamepad/main.cpp 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) < Date: Fri, 5 Jul 2024 09:57:17 +0200 Subject: [PATCH 10/30] gamepad module added --- src/Gamepad/Makefile | 13 ++++++ src/Gamepad/gamepad.cpp | 101 ++++++++++++++++++++++++++++++++++++++++ src/Gamepad/gamepad.h | 47 +++++++++++++++++++ 3 files changed, 161 insertions(+) create mode 100644 src/Gamepad/Makefile create mode 100644 src/Gamepad/gamepad.cpp create mode 100644 src/Gamepad/gamepad.h 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..cc0d540 --- /dev/null +++ b/src/Gamepad/gamepad.cpp @@ -0,0 +1,101 @@ +#include "gamepad.h" + + +#define RAI_PLIB +#ifdef RAI_PLIB + +#include +#undef min +#undef max + +GamepadInterface::GamepadInterface() + : Thread("GamepadInterface", .01) { + threadLoop(true); +} + +GamepadInterface::~GamepadInterface(){ + threadClose(); +} + +void GamepadInterface::open(){ + jsInit(); + count = 0; + for(uint i=0;;i++) { + struct jsJoystick* joystick = new jsJoystick(i); + if(joystick->notWorking()) break; + uint n=joystick->getNumAxes(); + + std::cout << "name = " << joystick->getName() + << "\n#axis = " << joystick->getNumAxes() + << "\n#buttons = " << joystick->getNumButtons() + << "\nerror? = " << joystick->notWorking() + << "______________" << std::endl; + + if(n==2) { + joysticks[count] = joystick; + count++; + if (count >= 2) break; + } else { + delete joystick; + } + } + std::cout << "Found " << count << " gamepad(s)!" << std::endl; + step(); //clear the buffers... +} + +void GamepadInterface::close(){ + for (int i = 0; i < count; i++) { + delete joysticks[i]; + gamepadState[i].set()->clear(); + } +} + +void GamepadInterface::step(){ + arr pad_logs; + pad_logs.resize(count).setZero(); + ctrlTime += .01; + for (int i = 0; i < count; i++) { + if(joysticks[i]->notWorking()) return; + uint n=joysticks[i]->getNumAxes(); + floatA A(n); + int B; + joysticks[i]->rawRead(&B, A.p); + gamepadState[i].writeAccess(); + gamepadState[i]().resize(n+1); + gamepadState[i]()(0)=B; + for(uint j=0; j20 && stopButtons(gamepadState[i]())){ + LOG(1) <<"*** STOP button pressed"; + // moduleShutdown()->incrementStatus(); + quitSignal.set()=true; + } + gamepadState[i].deAccess(); + pad_logs.elem(i) = getButtonPressed(i); + } + if(writeData>0){ + if(!dataFile.is_open()) dataFile.open(STRING("z.gamepad.dat")); + dataFile <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..3968e2f --- /dev/null +++ b/src/Gamepad/gamepad.h @@ -0,0 +1,47 @@ +#ifndef RAI_gamepad_h +#define RAI_gamepad_h + +#include +#include + + +struct GamepadInterface : Thread { + int count; + struct jsJoystick* joysticks[2]; + Var gamepadState[2]; + Var quitSignal; + GamepadInterface(); + ~GamepadInterface(); + void open(); + void step(); + void close(); + int getButtonPressed(unsigned int controller_id=0); + // Logging + int writeData = 0; + double ctrlTime = 0.; + ofstream dataFile; +}; + +/// 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_X = 1, + BTN_A = 2, + BTN_B = 4, + BTN_Y = 8, + BTN_L = 16, + BTN_R = 32, + BTN_SELECT = 256, + BTN_START = 512, +}; + +inline bool stopButtons(const arr& gamepadState){ + if(!gamepadState.N) return false; + uint mode = uint(gamepadState(0)); + if(mode&BTN_START) return true; + return false; +} + +#endif From 5f28f8070d44c0e7e73e4ded4e10e04a18ef5192 Mon Sep 17 00:00:00 2001 From: Eckart Cobo Briesewitz Date: Sun, 22 Sep 2024 14:20:28 +0200 Subject: [PATCH 11/30] adjustable jacobian values --- src/Omnibase/omnibase.cpp | 15 +++++++++------ src/Omnibase/omnibase.h | 1 + 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/Omnibase/omnibase.cpp b/src/Omnibase/omnibase.cpp index aff8569..2afb571 100644 --- a/src/Omnibase/omnibase.cpp +++ b/src/Omnibase/omnibase.cpp @@ -42,16 +42,13 @@ struct OmnibaseController{ arr getJacobian(){ //return Jacobian based on qLast - double gears = 4.2; - double r = .06; - double R = .35; arr J = { .5, .5, -1., .5*sqrt(3.), -.5*sqrt(3.), 0., - 1/(3.*R), 1/(3.*R), 1./(3.*R) + 1/(3.*center2wheel), 1/(3.*center2wheel), 1./(3.*center2wheel) }; - J *= r/gears; + J *= wheel_radius/gear_ratio; J.reshape(3,3); double phi = qLast(2); @@ -124,7 +121,13 @@ void OmnibaseThread::init(uint _robotID, const uintA& _qIndices) { KdDelay = rai::getParameter("Omnibase/KdDelay", 2); Friction = rai::getParameter("Omnibase/Friction", 0); - LOG(0) << "Omnibase: Kp:" << Kp << " Ki:" << Ki << " Kd:" << Kd << "KdDelay:" << KdDelay << "KiLimit:" << KiLimit << " Friction:" << Friction; + // 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 addresses = rai::getParameter("Omnibase/addresses", {"/dev/hidraw0", "/dev/hidraw1", "/dev/hidraw2"}); diff --git a/src/Omnibase/omnibase.h b/src/Omnibase/omnibase.h index 561895e..0093735 100644 --- a/src/Omnibase/omnibase.h +++ b/src/Omnibase/omnibase.h @@ -15,6 +15,7 @@ struct OmnibaseThread : rai::RobotAbstraction, Thread { int robotID=0; 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; From 9d08080136434acb592e5df72bcdd7b019f288db Mon Sep 17 00:00:00 2001 From: Eckart Cobo Briesewitz Date: Sun, 22 Sep 2024 14:22:04 +0200 Subject: [PATCH 12/30] bug fix? --- src/Omnibase/omnibase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Omnibase/omnibase.cpp b/src/Omnibase/omnibase.cpp index 2afb571..b8c3568 100644 --- a/src/Omnibase/omnibase.cpp +++ b/src/Omnibase/omnibase.cpp @@ -37,7 +37,7 @@ struct OmnibaseController{ rai::wait(.1); for(uint i=0;irunOff(); rai::wait(.1); - for(uint i=0;ireset(); } arr getJacobian(){ From 794b19d29a8e8da68cea97c123a4a37873a86139 Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Wed, 22 Jan 2025 14:14:11 +0100 Subject: [PATCH 13/30] ranger code init --- src/Ranger/ranger.cpp | 290 ++++++++++++++++++++++++++++++++++++++++++ src/Ranger/ranger.h | 35 +++++ 2 files changed, 325 insertions(+) create mode 100644 src/Ranger/ranger.cpp create mode 100644 src/Ranger/ranger.h diff --git a/src/Ranger/ranger.cpp b/src/Ranger/ranger.cpp new file mode 100644 index 0000000..39e988a --- /dev/null +++ b/src/Ranger/ranger.cpp @@ -0,0 +1,290 @@ +#include "omnibase.h" +#include "SimplexMotion.h" + +#define RAI_RANGER +#ifdef RAI_RANGER + +struct RangerController{ + rai::Array> motors; //three motors + arr qLast, sLast; //q: joint state; s: motor state + int Kp, Ki, Kd, KiLimit, KdDelay, Friction; + + RangerController(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(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); + } + + ~RangerController(){ + 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 = { + .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){ + //-- get motor state + arr s(motors.N); + arr sDot(motors.N); + 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 setVelocities(const arr& v_motors){ + //-- send torques to motors + CHECK_EQ(v_motors.N, motors.N, "control signal has wrong dimensionality"); + + // clip(v_motors, -1., 1.); + for(uint i=0;isetPID(Kp, Ki, Kd, KiLimit, KdDelay, Friction); + motors(i)->runSpeed(v_motors(i)); + } + } +}; + +RangerThread::RangerThread(uint robotID, const uintA &_qIndices, const Var &_cmd, const Var &_state) + : RobotAbstraction(_cmd, _state), Thread("OmnibaseThread", .015){ + init(robotID, _qIndices); + writeData = 10; +} + +RangerThread::~RangerThread(){ + LOG(0) <<"shutting down Ranger " <("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 + addresses = rai::getParameter("Omnibase/addresses", {"/dev/hidraw0", "/dev/hidraw1", "/dev/hidraw2"}); + + //-- start thread and wait for first state signal + LOG(0) <<"launching Omnibase " <(address, Kp, Ki, Kd, KiLimit, KdDelay, Friction); + + //-- 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, 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); + + if(q_ref.N==3){ + arr delta_motor = Jinv * (q_ref - q_real); + v += outer_Kp * delta_motor; + } + + if(qDot_ref.N==3){ + v += Jinv * qDot_ref; + } + + //-- data log + if(writeData>0 && !(steps%1)){ + if(!dataFile.is_open()) dataFile.open(STRING("z.ranger"<1){ + qDot_real.modRaw().write(dataFile); //3 + qDot_ref.modRaw().write(dataFile); //3 + v.modRaw().write(dataFile); //3 + qDDot_ref.modRaw().write(dataFile); + } + dataFile <setVelocities(v); +} + +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..490c928 --- /dev/null +++ b/src/Ranger/ranger.h @@ -0,0 +1,35 @@ +#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 + int Kp, Ki, Kd; + 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(); +}; From 6da698831b1f1ba86f077e2452047fcfaadaf0bb Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Wed, 22 Jan 2025 14:51:49 +0100 Subject: [PATCH 14/30] updated ranger init --- src/Ranger/ranger.cpp | 159 ++++++++++++++++++++++++++++-------------- 1 file changed, 105 insertions(+), 54 deletions(-) diff --git a/src/Ranger/ranger.cpp b/src/Ranger/ranger.cpp index 39e988a..a2379e9 100644 --- a/src/Ranger/ranger.cpp +++ b/src/Ranger/ranger.cpp @@ -1,43 +1,75 @@ + #include "omnibase.h" #include "SimplexMotion.h" #define RAI_RANGER #ifdef RAI_RANGER + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include // Include for ioctl and SIOCGIFINDEX + struct RangerController{ - rai::Array> motors; //three motors + int socket_fd; + int ranger_mode; arr qLast, sLast; //q: joint state; s: motor state - int Kp, Ki, Kd, KiLimit, KdDelay, Friction; + struct ifreq ifr; + struct sockaddr_can addr; + uint32_t id_control_mode; + struct can_frame frame_control_mode; - RangerController(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(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"; + RangerController(const String& address, int Kp, int Ki, int Kd) + { + socket_fd = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (socket_fd < 0) { + std::cerr << "Error creating socket: " << strerror(errno) << std::endl; + return 1; + } + + std::strncpy(ifr.ifr_name, address, IFNAMSIZ - 1); + if (ioctl(socket_fd, SIOCGIFINDEX, &ifr) < 0) { + std::cerr << "Error getting interface index: " << strerror(errno) << std::endl; + close(socket_fd); + return 1; + } + + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + if (bind(socket_fd, (struct sockaddr*)&addr, sizeof(addr)) < 0) { + std::cerr << "Error binding socket: " << strerror(errno) << std::endl; + close(socket_fd); + return 1; + } + + // Send control mode message + ranger_mode = 0; + 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 the control mode message + if (write(socket_fd, &frame_control_mode, sizeof(frame_control_mode)) != sizeof(frame_control_mode)) { + std::cerr << "Error sending control mode message: " << strerror(errno) << std::endl; + close(socket_fd); + return 1; } - qLast = zeros(3); - sLast = zeros(motors.N); + std::cout << "Control mode message sent: ID = " << std::hex << id_control_mode << std::endl; } ~RangerController(){ - for(uint i=0;irunStop(); - rai::wait(.1); - for(uint i=0;irunOff(); - rai::wait(.1); - for(uint i=0;ireset(); + close(socket_fd); } arr getJacobian(){ @@ -82,20 +114,49 @@ struct RangerController{ sLast = s; } - void setVelocities(const arr& v_motors){ - //-- send torques to motors - CHECK_EQ(v_motors.N, motors.N, "control signal has wrong dimensionality"); - - // clip(v_motors, -1., 1.); - for(uint i=0;isetPID(Kp, Ki, Kd, KiLimit, KdDelay, Friction); - motors(i)->runSpeed(v_motors(i)); + void setVelocities(const arr& v) + { + // v[0] x vel + // v[1] y vel + // v[2] phi vel + // v[3] mode + + // Safety + v[0] = min(2000, v[0]); + v[1] = min(2000, v[1]); + v[2] = min(PI_M, v[2]); + + // Step 2: Define motion control command + uint32_t id_motion = 0x111; + int16_t linear_speed = v[0]; // Linear speed in mm/s + int16_t spin_speed = v[2]; // Spin speed in 0.001 rad/s + uint8_t reserved = 0; // Reserved byte + + // Create the data payload + struct can_frame frame_motion; + frame_motion.can_id = id_motion; + frame_motion.can_dlc = 8; // Data length code (number of bytes) + + // Convert linear speed and spin speed to bytes + frame_motion.data[0] = (linear_speed >> 8) & 0xFF; // High byte of linear speed + frame_motion.data[1] = linear_speed & 0xFF; // Low byte of linear speed + frame_motion.data[2] = (spin_speed >> 8) & 0xFF; // High byte of spin speed + frame_motion.data[3] = spin_speed & 0xFF; // Low byte of spin speed + frame_motion.data[4] = reserved; // Reserved byte + std::memset(frame_motion.data + 5, 0, 3); // Remaining reserved bytes + + // Send the motion command message + if (write(socket_fd, &frame_motion, sizeof(frame_motion)) != sizeof(frame_motion)) { + std::cerr << "Error sending motion command message: " << strerror(errno) << std::endl; + close(socket_fd); + return 1; } + std::cout << "Motion command message sent: ID = " << std::hex << id_motion << std::endl; } }; RangerThread::RangerThread(uint robotID, const uintA &_qIndices, const Var &_cmd, const Var &_state) - : RobotAbstraction(_cmd, _state), Thread("OmnibaseThread", .015){ + : RobotAbstraction(_cmd, _state), Thread("RangerThread", .015){ init(robotID, _qIndices); writeData = 10; } @@ -113,27 +174,17 @@ void RangerThread::init(uint _robotID, const uintA& _qIndices) { qIndices_max = rai::max(qIndices); //-- basic Kp Kd settings for reference control mode - 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; + Kp = rai::getParameter("Ranger/Kp", 20); + Ki = rai::getParameter("Ranger/Ki", 0); + Kd = rai::getParameter("Ranger/Kd", 10); + + LOG(0) << "Ranger/PID: Kp:" << Kp << " Ki:" << Ki << " Kd:" << Kd; //-- get robot address - addresses = rai::getParameter("Omnibase/addresses", {"/dev/hidraw0", "/dev/hidraw1", "/dev/hidraw2"}); + address = rai::getParameter("Ranger/address", "can0"); //-- start thread and wait for first state signal - LOG(0) <<"launching Omnibase " <(address, Kp, Ki, Kd, KiLimit, KdDelay, Friction); + robot = make_shared(address, Kp, Ki, Kd); //-- initialize state and ctrl with first state arr q_real, qDot_real; From 9f86b7aefbca1284a2346ff0955720229c1b43cc Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Tue, 28 Jan 2025 11:33:31 +0100 Subject: [PATCH 15/30] added ranger to bot.cpp --- src/BotOp/bot.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/BotOp/bot.cpp b/src/BotOp/bot.cpp index 5e0b027..d978865 100644 --- a/src/BotOp/bot.cpp +++ b/src/BotOp/bot.cpp @@ -13,6 +13,7 @@ #include #include "simulation.h" #include +#include #include #include #include @@ -93,6 +94,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; From 7c032cd860972f0633e3cd36a3c353bf5bfcfcd9 Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Tue, 28 Jan 2025 18:22:14 +0100 Subject: [PATCH 16/30] Ranger for TransX joint --- src/Ranger/ranger.cpp | 239 ++++++++++++++++++++++++------------------ 1 file changed, 136 insertions(+), 103 deletions(-) diff --git a/src/Ranger/ranger.cpp b/src/Ranger/ranger.cpp index a2379e9..20cc14c 100644 --- a/src/Ranger/ranger.cpp +++ b/src/Ranger/ranger.cpp @@ -1,10 +1,11 @@ -#include "omnibase.h" -#include "SimplexMotion.h" +#include "ranger.h" #define RAI_RANGER #ifdef RAI_RANGER +// #define FAKE +#define JOINT_DIM 1 #include #include @@ -17,6 +18,10 @@ #include #include #include // Include for ioctl and SIOCGIFINDEX +#include // For time tracking + + +const char* ranger_address = "can0"; struct RangerController{ int socket_fd; @@ -26,29 +31,34 @@ struct RangerController{ struct sockaddr_can addr; uint32_t id_control_mode; struct can_frame frame_control_mode; + double position; + std::chrono::time_point last_time; + - RangerController(const String& address, int Kp, int Ki, int Kd) + RangerController(const char* address, int Kp, int Ki, int Kd) { + position = 0.0; // Position in meters + last_time = std::chrono::steady_clock::now(); // For time tracking + #ifdef FAKE + LOG(0) << "Conneted to ranger ;)"; + #else socket_fd = socket(PF_CAN, SOCK_RAW, CAN_RAW); if (socket_fd < 0) { - std::cerr << "Error creating socket: " << strerror(errno) << std::endl; - return 1; + LOG(-1) << "Error creating socket: " << strerror(errno) << std::endl; } std::strncpy(ifr.ifr_name, address, IFNAMSIZ - 1); if (ioctl(socket_fd, SIOCGIFINDEX, &ifr) < 0) { - std::cerr << "Error getting interface index: " << strerror(errno) << std::endl; - close(socket_fd); - return 1; + 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) { - std::cerr << "Error binding socket: " << strerror(errno) << std::endl; - close(socket_fd); - return 1; + LOG(-1) << "Error binding socket: " << strerror(errno) << std::endl; + close(socket_fd); } // Send control mode message @@ -61,75 +71,106 @@ struct RangerController{ // Send the control mode message if (write(socket_fd, &frame_control_mode, sizeof(frame_control_mode)) != sizeof(frame_control_mode)) { - std::cerr << "Error sending control mode message: " << strerror(errno) << std::endl; - close(socket_fd); - return 1; + LOG(-1) << "Error sending control mode message: " << strerror(errno) << std::endl; + close(socket_fd); } std::cout << "Control mode message sent: ID = " << std::hex << id_control_mode << std::endl; + #endif + qLast = zeros(JOINT_DIM); + sLast = zeros(1); } ~RangerController(){ + #ifdef FAKE + #else close(socket_fd); + #endif } - arr getJacobian(){ + arr wheelVelToJointVel(arr s){ //return Jacobian based on qLast + double wheel_radius = .2; + arr q_vel(JOINT_DIM); + q_vel = s; + return q_vel; + } - 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; + arr jointVelToWheelVel(arr q){ + //return Jacobian based on qLast + double wheel_radius = .2; + arr s_vel(JOINT_DIM); + s_vel = q; + return s_vel; + } + + double getLinearVelocity(){ + while (true) { + struct can_frame recv_frame; + int nbytes = read(socket_fd, &recv_frame, sizeof(recv_frame)); + if (nbytes < 0) { + LOG(-1) << "Error reading message: " << strerror(errno) << std::endl; + break; + } + + if (nbytes < sizeof(struct can_frame)) { + LOG(-1) << "Incomplete CAN frame received" << std::endl; + continue; + } + + if (recv_frame.can_id == 0x221) { + // Extract the linear velocity (bytes 0 and 1) + int16_t linear_velocity = (recv_frame.data[0] << 8) | recv_frame.data[1]; + // Convert to m/s (unit in feedback is 0.001 m/s) + double linear_velocity_mps = linear_velocity / 1000.0; + + std::cout << "Current Linear Velocity: " << linear_velocity_mps << " m/s" << std::endl; + + return linear_velocity_mps; + } + } + return 0.0; } void getState(arr& q, arr& qDot){ //-- get motor state - arr s(motors.N); - arr sDot(motors.N); - for(uint i=0;igetMotorPosition(); - sDot(i) = motors(i)->getMotorSpeed(); - } + arr s(1); + arr sDot(1); + + double linear_vel = getLinearVelocity(); + + auto current_time = std::chrono::steady_clock::now(); + std::chrono::duration elapsed_time = current_time - last_time; + last_time = current_time; + + // Integrate velocity to estimate position + position += linear_vel * elapsed_time.count(); + + s = arr{position}; // Wheel positions + sDot = linear_vel; // Wheel velocities - //-- convert to joint state arr sDelta = s - sLast; + + //-- convert to joint state // DO MAGIC HERE: convert the sDelta (change in motor positions) to a qDelta (change in joint state) - arr Jacobian = getJacobian(); - arr qDelta = Jacobian * sDelta; + arr qDelta = wheelVelToJointVel(sDelta); q = qLast + qDelta; - qDot = Jacobian * sDot; + qDot = wheelVelToJointVel(sDot); qLast = q; sLast = s; } - void setVelocities(const arr& v) + void setVelocities(const arr& qDotTarget) { - // v[0] x vel - // v[1] y vel - // v[2] phi vel - // v[3] mode - + #ifdef FAKE + #else // Safety - v[0] = min(2000, v[0]); - v[1] = min(2000, v[1]); - v[2] = min(PI_M, v[2]); + // TODO: Check if velocities are too high // Step 2: Define motion control command uint32_t id_motion = 0x111; - int16_t linear_speed = v[0]; // Linear speed in mm/s - int16_t spin_speed = v[2]; // Spin speed in 0.001 rad/s + int16_t linear_speed = qDotTarget.elem(0) * 1000; // Linear speed in mm/s + int16_t spin_speed = 0; // Spin speed in 0.001 rad/s uint8_t reserved = 0; // Reserved byte // Create the data payload @@ -142,16 +183,16 @@ struct RangerController{ frame_motion.data[1] = linear_speed & 0xFF; // Low byte of linear speed frame_motion.data[2] = (spin_speed >> 8) & 0xFF; // High byte of spin speed frame_motion.data[3] = spin_speed & 0xFF; // Low byte of spin speed - frame_motion.data[4] = reserved; // Reserved byte + frame_motion.data[4] = reserved; // Reserved byte std::memset(frame_motion.data + 5, 0, 3); // Remaining reserved bytes // Send the motion command message if (write(socket_fd, &frame_motion, sizeof(frame_motion)) != sizeof(frame_motion)) { - std::cerr << "Error sending motion command message: " << strerror(errno) << std::endl; - close(socket_fd); - return 1; + LOG(-1) << "Error sending motion command message: " << strerror(errno) << std::endl; + close(socket_fd); } std::cout << "Motion command message sent: ID = " << std::hex << id_motion << std::endl; + #endif } }; @@ -162,15 +203,15 @@ RangerThread::RangerThread(uint robotID, const uintA &_qIndices, const Var("Ranger/address", "can0"); + // address = rai::getParameter("Ranger/address", "can0"); + address = ranger_address; //-- start thread and wait for first state signal LOG(0) <<"launching Ranger " <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++){ + for(uint i=0; iq.elem(qIndices(i)) = q_real(i); stateSet->qDot.elem(qIndices(i)) = qDot_real(i); stateSet->tauExternalIntegral.elem(qIndices(i)) = 0.; @@ -229,7 +271,7 @@ void RangerThread::step(){ else stateSet->stall--; } ctrlTime = stateSet->ctrlTime; - for(uint i=0;i<3;i++){ + for(uint i=0;iq.elem(qIndices(i)) = q_real.elem(i); stateSet->qDot.elem(qIndices(i)) = qDot_real.elem(i); } @@ -238,7 +280,7 @@ void RangerThread::step(){ } //-- get current ctrl command - arr q_ref, qDot_ref, qDDot_ref, Kp_ref, Kd_ref, P_compliance; + arr q_ref, qDot_ref, qDDot_ref, P_compliance; { auto cmdGet = cmd.get(); @@ -253,77 +295,68 @@ void RangerThread::step(){ //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)); + q_ref.resize(JOINT_DIM); + for(uint i=0; iKp.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)); + qDDot_ref.resize(JOINT_DIM); + for(uint i=0; iP_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)); + P_compliance.resize(JOINT_DIM,JOINT_DIM); + for(uint i=0; iP_compliance(qIndices(i), qIndices(j)); } - } + LOG(0) << "q_real" << q_real; + LOG(0) << "qDot_real" << qDot_real; + LOG(0) << "q_ref" << q_ref; + LOG(0) << "qDot_ref" << qDot_ref; //-- cap the reference difference - if(q_ref.N==3){ + if(q_ref.N==JOINT_DIM){ 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! + if(err>.3){ //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); - - if(q_ref.N==3){ - arr delta_motor = Jinv * (q_ref - q_real); - v += outer_Kp * delta_motor; - } - - if(qDot_ref.N==3){ - v += Jinv * qDot_ref; + arr sDotTarget; + sDotTarget.resize(3).setZero(); + + if(q_ref.N==JOINT_DIM && qDot_ref.N==JOINT_DIM) + { + arr p_term = Kp * (q_ref - q_real); + arr d_term = Kd * qDot_ref; + arr qDotTarget = p_term + d_term; + sDotTarget = robot->jointVelToWheelVel(qDotTarget); } //-- data log if(writeData>0 && !(steps%1)){ if(!dataFile.is_open()) dataFile.open(STRING("z.ranger"<1){ - qDot_real.modRaw().write(dataFile); //3 - qDot_ref.modRaw().write(dataFile); //3 - v.modRaw().write(dataFile); //3 + qDot_real.modRaw().write(dataFile); + qDot_ref.modRaw().write(dataFile); + sDotTarget.modRaw().write(dataFile); qDDot_ref.modRaw().write(dataFile); } dataFile <setVelocities(v); + + robot->setVelocities(sDotTarget); } void RangerThread::close(){ From d3343532a817a5f6024b72c5d5ccbc276241aa5d Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Fri, 14 Mar 2025 20:13:29 +0100 Subject: [PATCH 17/30] oblique and spin mode qorking with odometry --- src/Ranger/ranger.cpp | 462 ++++++++++++++++++++++++++++++------------ src/Ranger/ranger.h | 7 +- 2 files changed, 330 insertions(+), 139 deletions(-) diff --git a/src/Ranger/ranger.cpp b/src/Ranger/ranger.cpp index 20cc14c..9772410 100644 --- a/src/Ranger/ranger.cpp +++ b/src/Ranger/ranger.cpp @@ -4,9 +4,6 @@ #define RAI_RANGER #ifdef RAI_RANGER -// #define FAKE -#define JOINT_DIM 1 - #include #include #include @@ -18,30 +15,28 @@ #include #include #include // Include for ioctl and SIOCGIFINDEX -#include // For time tracking - +#define PI 3.1415926 +#define DEBUG 1 -const char* ranger_address = "can0"; -struct RangerController{ +struct RangerController +{ int socket_fd; - int ranger_mode; + int mode; // 0: Oblique, 1: Spin, 2: Ackermann arr qLast, sLast; //q: joint state; s: motor state - struct ifreq ifr; - struct sockaddr_can addr; - uint32_t id_control_mode; - struct can_frame frame_control_mode; - double position; - std::chrono::time_point last_time; - + double actual_steering_rad; - RangerController(const char* address, int Kp, int Ki, int Kd) + RangerController(const char* address) { - position = 0.0; // Position in meters - last_time = std::chrono::steady_clock::now(); // For time tracking - #ifdef FAKE - LOG(0) << "Conneted to ranger ;)"; - #else + mode = 0; + qLast = zeros(3); + actual_steering_rad = 0.0; // CAREFULL!! Only updates when you call "get_qDot_oblique". + // 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; @@ -61,138 +56,336 @@ struct RangerController{ close(socket_fd); } - // Send control mode message - ranger_mode = 0; - id_control_mode = 0x421; + // 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 the control mode message - if (write(socket_fd, &frame_control_mode, sizeof(frame_control_mode)) != sizeof(frame_control_mode)) { - LOG(-1) << "Error sending control mode message: " << strerror(errno) << std::endl; - close(socket_fd); - } + 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; - #endif - qLast = zeros(JOINT_DIM); - sLast = zeros(1); + + sLast = get_s(); } ~RangerController(){ - #ifdef FAKE - #else close(socket_fd); - #endif } - arr wheelVelToJointVel(arr s){ - //return Jacobian based on qLast - double wheel_radius = .2; - arr q_vel(JOINT_DIM); - q_vel = s; - return q_vel; + 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 jointVelToWheelVel(arr q){ - //return Jacobian based on qLast - double wheel_radius = .2; - arr s_vel(JOINT_DIM); - s_vel = q; - return s_vel; + 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}; } - double getLinearVelocity(){ - while (true) { - struct can_frame recv_frame; - int nbytes = read(socket_fd, &recv_frame, sizeof(recv_frame)); - if (nbytes < 0) { - LOG(-1) << "Error reading message: " << strerror(errno) << std::endl; - break; - } + 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 + actual_steering_rad = actual_steering_angle / 1000.0; // Convert mrad to rad + #if DEBUG + std::cout << "Actual Steering Angle before: " << actual_steering_rad << " rad" << std::endl; + #endif - if (nbytes < sizeof(struct can_frame)) { - LOG(-1) << "Incomplete CAN frame received" << std::endl; - continue; - } + actual_steering_rad *= -1; + if (actual_speed_mps < 0) { // Lower side + actual_steering_rad += PI; + } - if (recv_frame.can_id == 0x221) { - // Extract the linear velocity (bytes 0 and 1) - int16_t linear_velocity = (recv_frame.data[0] << 8) | recv_frame.data[1]; - // Convert to m/s (unit in feedback is 0.001 m/s) - double linear_velocity_mps = linear_velocity / 1000.0; + // Compute X-Y velocity components + double velocity_x = actual_speed_mps * cos(actual_steering_rad); + double velocity_y = actual_speed_mps * -sin(actual_steering_rad); - std::cout << "Current Linear Velocity: " << linear_velocity_mps << " m/s" << std::endl; + #if DEBUG + std::cout << "Actual Linear Speed: " << actual_speed_mps << " m/s" << std::endl; + std::cout << "Actual Steering Angle: " << actual_steering_rad << " 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 linear_velocity_mps; - } - } - return 0.0; + return arr{velocity_x, velocity_y, 0.}; } - void getState(arr& q, arr& qDot){ - //-- get motor state - arr s(1); - arr sDot(1); - - double linear_vel = getLinearVelocity(); + arr get_qDot_spin() + { + struct can_frame recv_frame = recv_package(0x221); - auto current_time = std::chrono::steady_clock::now(); - std::chrono::duration elapsed_time = current_time - last_time; - last_time = current_time; + // 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 - // Integrate velocity to estimate position - position += linear_vel * elapsed_time.count(); + #if DEBUG + std::cout << "Actual Angular Speed: " << actual_spin_speed_mps << " m/s" << std::endl; + #endif + return arr{0., 0., actual_spin_speed_mps}; + } - s = arr{position}; // Wheel positions - sDot = linear_vel; // Wheel velocities + 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; - //-- convert to joint state - // DO MAGIC HERE: convert the sDelta (change in motor positions) to a qDelta (change in joint state) - arr qDelta = wheelVelToJointVel(sDelta); + 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 + 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); + qDot = get_qDot_oblique(); + double xDelta = cos(actual_steering_rad) * mean_delta_s; + double yDelta = -sin(actual_steering_rad) * mean_delta_s; + qDelta = arr{xDelta, yDelta, 0.}; + break; + } + + default: { + break; + } + } q = qLast + qDelta; - qDot = wheelVelToJointVel(sDot); qLast = q; sLast = s; } - void setVelocities(const arr& qDotTarget) + void setVelocitiesOblique(double dir_angle, double dir_magnitude) { - #ifdef FAKE - #else - // Safety - // TODO: Check if velocities are too high + 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; + } + } + + #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 - // Step 2: Define motion control command - uint32_t id_motion = 0x111; - int16_t linear_speed = qDotTarget.elem(0) * 1000; // Linear speed in mm/s - int16_t spin_speed = 0; // Spin speed in 0.001 rad/s - uint8_t reserved = 0; // Reserved byte + 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 - // Create the data payload + 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_id = id_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 - // Convert linear speed and spin speed to bytes - frame_motion.data[0] = (linear_speed >> 8) & 0xFF; // High byte of linear speed - frame_motion.data[1] = linear_speed & 0xFF; // Low byte of linear speed - frame_motion.data[2] = (spin_speed >> 8) & 0xFF; // High byte of spin speed - frame_motion.data[3] = spin_speed & 0xFF; // Low byte of spin speed - frame_motion.data[4] = reserved; // Reserved byte - std::memset(frame_motion.data + 5, 0, 3); // Remaining reserved bytes + int16_t linear_speed = 0; + int16_t spin_speed_ = spin_speed * 1000; + int16_t steering_angle = 0; - // Send the motion command message + 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); } - std::cout << "Motion command message sent: ID = " << std::hex << id_motion << std::endl; - #endif + } + + 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; + + if (is_spin_mode) { + mode = 1; + } else if (is_oblique_mode) { + mode = 0; + } 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 oblique_dir = ::acos(qDotTarget.elem(0) / dir_magnitude); + if (qDotTarget.elem(1) > 0) { + oblique_dir = PI*2-oblique_dir; + } + + std::cout << "Mag: " << dir_magnitude << " m/s" << std::endl; + std::cout << "Angle: " << oblique_dir << " rad" << std::endl; + + setVelocitiesOblique(oblique_dir, dir_magnitude); + break; + } + + default: { + break; + } + } } }; @@ -211,19 +404,18 @@ void RangerThread::init(uint _robotID, const uintA& _qIndices) { robotID=_robotID; qIndices=_qIndices; - CHECK_EQ(qIndices.N, JOINT_DIM, ""); + CHECK_EQ(qIndices.N, 3, ""); qIndices_max = rai::max(qIndices); //-- basic Kp Kd settings for reference control mode - Kp = rai::getParameter("Ranger/Kp", 20); - Ki = rai::getParameter("Ranger/Ki", 0); - Kd = rai::getParameter("Ranger/Kd", 10); + Kp = rai::getParameter("Ranger/Kp", arr{20., 20., 20.}); + Ki = rai::getParameter("Ranger/Ki", arr{0., 0., 0.}); + Kd = rai::getParameter("Ranger/Kd", arr{10., 10., 10.}); LOG(0) << "Ranger/PID: Kp:" << Kp << " Ki:" << Ki << " Kd:" << Kd; //-- get robot address - // address = rai::getParameter("Ranger/address", "can0"); - address = ranger_address; + address = rai::getParameter("Ranger/address", "can0"); //-- start thread and wait for first state signal LOG(0) <<"launching Ranger " <(address, Kp, Ki, Kd); + robot = make_shared(address); //-- initialize state and ctrl with first state arr q_real, qDot_real; @@ -247,7 +439,7 @@ void RangerThread::open(){ while(stateSet->qDot.N<=qIndices_max) stateSet->qDot.append(0.); while(stateSet->tauExternalIntegral.N<=qIndices_max) stateSet->tauExternalIntegral.append(0.); - for(uint i=0; iq.elem(qIndices(i)) = q_real(i); stateSet->qDot.elem(qIndices(i)) = qDot_real(i); stateSet->tauExternalIntegral.elem(qIndices(i)) = 0.; @@ -271,7 +463,7 @@ void RangerThread::step(){ else stateSet->stall--; } ctrlTime = stateSet->ctrlTime; - for(uint i=0;iq.elem(qIndices(i)) = q_real.elem(i); stateSet->qDot.elem(qIndices(i)) = qDot_real.elem(i); } @@ -295,20 +487,20 @@ void RangerThread::step(){ //pick qIndices for this particular robot if(cmd_q_ref.N){ - q_ref.resize(JOINT_DIM); - for(uint i=0; iP_compliance.N) { - P_compliance.resize(JOINT_DIM,JOINT_DIM); - for(uint i=0; iP_compliance(qIndices(i), qIndices(j)); + 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)); } } LOG(0) << "q_real" << q_real; @@ -317,7 +509,7 @@ void RangerThread::step(){ LOG(0) << "qDot_ref" << qDot_ref; //-- cap the reference difference - if(q_ref.N==JOINT_DIM){ + if(q_ref.N==3){ double err = length(q_ref - q_real); if(P_compliance.N){ arr del = q_ref - q_real; @@ -330,15 +522,16 @@ void RangerThread::step(){ } //-- compute torques from control message depending on the control type - arr sDotTarget; - sDotTarget.resize(3).setZero(); + arr qDotTarget; + qDotTarget.resize(3).setZero(); - if(q_ref.N==JOINT_DIM && qDot_ref.N==JOINT_DIM) + if(q_ref.N==3 && qDot_ref.N==3) { - arr p_term = Kp * (q_ref - q_real); - arr d_term = Kd * qDot_ref; - arr qDotTarget = p_term + d_term; - sDotTarget = robot->jointVelToWheelVel(qDotTarget); + 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 @@ -350,13 +543,12 @@ void RangerThread::step(){ if(writeData>1){ qDot_real.modRaw().write(dataFile); qDot_ref.modRaw().write(dataFile); - sDotTarget.modRaw().write(dataFile); qDDot_ref.modRaw().write(dataFile); } dataFile <setVelocities(sDotTarget); + robot->setVelocities(qDotTarget); } void RangerThread::close(){ diff --git a/src/Ranger/ranger.h b/src/Ranger/ranger.h index 490c928..c60f163 100644 --- a/src/Ranger/ranger.h +++ b/src/Ranger/ranger.h @@ -15,8 +15,8 @@ struct RangerThread : rai::RobotAbstraction, Thread { int robotID=0; // Read from rai.cfg - int Kp, Ki, Kd; - String address; + arr Kp, Ki, Kd; + rai::String address; uintA qIndices; uint qIndices_max=0; @@ -25,8 +25,7 @@ struct RangerThread : rai::RobotAbstraction, Thread { ofstream dataFile; double ctrlTime=0.; - std::shared_ptr robot; - + std::shared_ptr robot; void init(uint _robotID, const uintA& _qIndices); void open(); From bf2af5675d189766f8406d6ab9a5c611a48aa9a2 Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Tue, 18 Mar 2025 11:03:06 +0100 Subject: [PATCH 18/30] ranger works on oblique and spin mode --- src/Ranger/ranger.cpp | 32 ++++++++++++++++++++++++-------- 1 file changed, 24 insertions(+), 8 deletions(-) diff --git a/src/Ranger/ranger.cpp b/src/Ranger/ranger.cpp index 9772410..00fbdd4 100644 --- a/src/Ranger/ranger.cpp +++ b/src/Ranger/ranger.cpp @@ -18,6 +18,7 @@ #define PI 3.1415926 #define DEBUG 1 +// TODO: setLights func in botop (also like flashing lights or so) struct RangerController { @@ -146,12 +147,14 @@ struct RangerController } // Compute X-Y velocity components - double velocity_x = actual_speed_mps * cos(actual_steering_rad); - double velocity_y = actual_speed_mps * -sin(actual_steering_rad); + double angle = actual_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: " << actual_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 @@ -201,9 +204,9 @@ struct RangerController case 1: { // Spin mode double mean_delta_s = (-1.*sDelta.elem(0) + - sDelta.elem(1) + + sDelta.elem(1) + -1.*sDelta.elem(2) + - sDelta.elem(3)) * .25; // m + sDelta.elem(3)) * .25; // m #if DEBUG std::cout << "Mean delta s: " << mean_delta_s << " mDelta" << std::endl; #endif @@ -222,8 +225,9 @@ struct RangerController sDelta.elem(3)) * .25; // m mean_delta_s = abs(mean_delta_s); qDot = get_qDot_oblique(); - double xDelta = cos(actual_steering_rad) * mean_delta_s; - double yDelta = -sin(actual_steering_rad) * mean_delta_s; + double steering_angle = actual_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; } @@ -375,8 +379,20 @@ struct RangerController oblique_dir = PI*2-oblique_dir; } - std::cout << "Mag: " << dir_magnitude << " m/s" << std::endl; - std::cout << "Angle: " << oblique_dir << " rad" << std::endl; + // Adjust for current rotation + oblique_dir += qLast.elem(2); + + if (oblique_dir < 0) { + oblique_dir += PI*2; + } else if (oblique_dir >= PI*2) { + oblique_dir -= 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: " << oblique_dir << " rad" << std::endl; + #endif setVelocitiesOblique(oblique_dir, dir_magnitude); break; From 45f77f27569fb7b6d418ab0f12f120b05beff690 Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Tue, 18 Mar 2025 11:04:47 +0100 Subject: [PATCH 19/30] Ranger test script --- test/25-ranger/Makefile | 8 +++ test/25-ranger/main.cpp | 129 ++++++++++++++++++++++++++++++++++++++++ test/25-ranger/rai.cfg | 23 +++++++ 3 files changed, 160 insertions(+) create mode 100644 test/25-ranger/Makefile create mode 100644 test/25-ranger/main.cpp create mode 100644 test/25-ranger/rai.cfg diff --git a/test/25-ranger/Makefile b/test/25-ranger/Makefile new file mode 100644 index 0000000..e553e71 --- /dev/null +++ b/test/25-ranger/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/25-ranger/main.cpp b/test/25-ranger/main.cpp new file mode 100644 index 0000000..4f43a44 --- /dev/null +++ b/test/25-ranger/main.cpp @@ -0,0 +1,129 @@ +#include +#include + +#define ON_REAL true + + +arr getTrianglePath(rai::Configuration& C) +{ + C.addFrame("way0")->setShape(rai::ST_marker, {.3}).setPosition({.3, 0., 1.}); + C.addFrame("way1")->setShape(rai::ST_marker, {.3}).setPosition({.3, -.1, 1.}); + C.addFrame("way2")->setShape(rai::ST_marker, {.3}).setPosition({0, 0, 1.}); + + KOMO komo(C, 3., 20, 2, false); + + komo.addControlObjective({}, 2, 1.); + + komo.addObjective({1.}, FS_positionDiff, {"ranger", "way0"}, OT_eq, {1e1, 1e1, 0}); + komo.addObjective({2.}, FS_positionDiff, {"ranger", "way1"}, OT_eq, {1e1, 1e1, 0}); + komo.addObjective({3.}, FS_positionDiff, {"ranger", "way2"}, OT_eq, {1e1, 1e1, 0}); + + komo.opt.verbose=-1; + komo.optimize(); + + return komo.getPath_qOrg(); +} + +arr getYPath(rai::Configuration& C) +{ + C.addFrame("way0")->setShape(rai::ST_marker, {.3}).setPosition({.3, 0., 1.}); + C.addFrame("way2")->setShape(rai::ST_marker, {.3}).setPosition({0, 0, 1.}); + + KOMO komo(C, 2., 20, 2, false); + + komo.addControlObjective({}, 2, 1.); + + komo.addObjective({1.}, FS_positionDiff, {"ranger", "way0"}, OT_eq, {1e1, 1e1, 0}); + komo.addObjective({2.}, FS_positionDiff, {"ranger", "way2"}, OT_eq, {1e1, 1e1, 0}); + + komo.opt.verbose=-1; + komo.optimize(); + + return komo.getPath_qOrg(); +} + +arr getPathToTarget(rai::Configuration& C, double target_x, double target_y) +{ + C.addFrame("way0")->setShape(rai::ST_marker, {.3}).setPosition({target_x, target_y, 1.}); + C.addFrame("way2")->setShape(rai::ST_marker, {.3}).setPosition({0, 0, 1.}); + + KOMO komo(C, 2., 20, 2, false); + + komo.addControlObjective({}, 2, 1.); + + komo.addObjective({1.}, FS_positionDiff, {"ranger", "way0"}, OT_eq, {1e1, 1e1, 0}); + komo.addObjective({2.}, FS_positionDiff, {"ranger", "way2"}, OT_eq, {1e1, 1e1, 0}); + + komo.opt.verbose=-1; + komo.optimize(); + + return komo.getPath_qOrg(); +} + +void moveRanger(){ + rai::Configuration C; + C.addFile(rai::raiPath("../rai-robotModels/ranger/ranger_simplified.g")); + + // arr q = getTrianglePath(C); + arr q = getYPath(C); + C.view(true); + + BotOp bot(C, ON_REAL); + + bot.move(q, {10.}); + bot.wait(C); +} + +void moveRangerLoop(){ + rai::Configuration C; + C.addFile(rai::raiPath("../rai-robotModels/ranger/ranger_simplified.g")); + C.view(false); + + int point_count = 6; + double offset = M_PI*.25; + double mag = .3; + for (int i = 0; i < point_count; i++) { + double angle = static_cast(i)/static_cast(point_count) * M_PI*2.0 + offset; + arr q = getPathToTarget(C, cos(angle)*mag, -sin(angle)*mag); + + BotOp bot(C, ON_REAL); + + bot.move(q, {5.}); + bot.wait(C); + } +} + +void justSpin(){ + rai::Configuration C; + C.addFile(rai::raiPath("../rai-robotModels/ranger/ranger_simplified.g")); + + BotOp bot(C, ON_REAL); + + double time = 5.; + + arr q = arr{0., 0., -1.57}; + bot.moveTo(q, {time}); + bot.wait(C); + + q = arr{.1, .2, -1.57}; + bot.moveTo(q, {time}); + bot.wait(C); + + q = arr{.1, .2, 0}; + bot.moveTo(q, {time}); + bot.wait(C); + + q = arr{0., 0., 0.}; + bot.moveTo(q, {time}); + bot.wait(C); +} + +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..d044b7f --- /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: [5, 5, 5] +Ranger/Kd: [1, 1, 1] From caf7b2107f94aecf61d52e0a65bd11aa99a5d5d3 Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Mon, 24 Mar 2025 20:46:08 +0100 Subject: [PATCH 20/30] livox init --- src/Livox/livox.cpp | 240 ++++++++++++++++++++++++++++++++++++++++++++ src/Livox/livox.h | 40 ++++++++ 2 files changed, 280 insertions(+) create mode 100644 src/Livox/livox.cpp create mode 100644 src/Livox/livox.h diff --git a/src/Livox/livox.cpp b/src/Livox/livox.cpp new file mode 100644 index 0000000..70785d6 --- /dev/null +++ b/src/Livox/livox.cpp @@ -0,0 +1,240 @@ +#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()); // Remove the first point + } + + // 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; + } +} + +void ImuDataCallback(uint32_t handle, const uint8_t dev_type, LivoxLidarEthernetPacket* data, void* client_data) { + if (data == nullptr) { + return; + } + printf("Imu data callback handle:%u, data_num:%u, data_type:%u, length:%u, frame_counter:%u.\n", + handle, data->dot_num, data->data_type, data->length, data->frame_cnt); +} + +void WorkModeCallback(livox_status status, uint32_t handle,LivoxLidarAsyncControlResponse *response, void *client_data) { + if (response == nullptr) { + return; + } + printf("WorkModeCallack, status:%u, handle:%u, ret_code:%u, error_key:%u", + status, handle, response->ret_code, response->error_key); + +} + +void RebootCallback(livox_status status, uint32_t handle, LivoxLidarRebootResponse* response, void* client_data) { + if (response == nullptr) { + return; + } + printf("RebootCallback, status:%u, handle:%u, ret_code:%u", + status, handle, response->ret_code); +} + +void SetIpInfoCallback(livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse *response, void *client_data) { + if (response == nullptr) { + return; + } + printf("LivoxLidarIpInfoCallback, status:%u, handle:%u, ret_code:%u, error_key:%u", + status, handle, response->ret_code, response->error_key); + + if (response->ret_code == 0 && response->error_key == 0) { + LivoxLidarRequestReboot(handle, RebootCallback, nullptr); + } +} + +void QueryInternalInfoCallback(livox_status status, uint32_t handle, + LivoxLidarDiagInternalInfoResponse* response, void* client_data) { + if (status != kLivoxLidarStatusSuccess) { + printf("Query lidar internal info failed.\n"); + QueryLivoxLidarInternalInfo(handle, QueryInternalInfoCallback, nullptr); + return; + } + + if (response == nullptr) { + return; + } + + uint8_t host_point_ipaddr[4] {0}; + uint16_t host_point_port = 0; + uint16_t lidar_point_port = 0; + + uint8_t host_imu_ipaddr[4] {0}; + uint16_t host_imu_data_port = 0; + uint16_t lidar_imu_data_port = 0; + + uint16_t off = 0; + for (uint8_t i = 0; i < response->param_num; ++i) { + LivoxLidarKeyValueParam* kv = (LivoxLidarKeyValueParam*)&response->data[off]; + if (kv->key == kKeyLidarPointDataHostIpCfg) { + memcpy(host_point_ipaddr, &(kv->value[0]), sizeof(uint8_t) * 4); + memcpy(&(host_point_port), &(kv->value[4]), sizeof(uint16_t)); + memcpy(&(lidar_point_port), &(kv->value[6]), sizeof(uint16_t)); + } else if (kv->key == kKeyLidarImuHostIpCfg) { + memcpy(host_imu_ipaddr, &(kv->value[0]), sizeof(uint8_t) * 4); + memcpy(&(host_imu_data_port), &(kv->value[4]), sizeof(uint16_t)); + memcpy(&(lidar_imu_data_port), &(kv->value[6]), sizeof(uint16_t)); + } + off += sizeof(uint16_t) * 2; + off += kv->length; + } + + printf("Host point cloud ip addr:%u.%u.%u.%u, host point cloud port:%u, lidar point cloud port:%u.\n", + host_point_ipaddr[0], host_point_ipaddr[1], host_point_ipaddr[2], host_point_ipaddr[3], host_point_port, lidar_point_port); + + printf("Host imu ip addr:%u.%u.%u.%u, host imu port:%u, lidar imu port:%u.\n", + host_imu_ipaddr[0], host_imu_ipaddr[1], host_imu_ipaddr[2], host_imu_ipaddr[3], host_imu_data_port, lidar_imu_data_port); + +} + +void LidarInfoChangeCallback(const uint32_t handle, const LivoxLidarInfo* info, void* client_data) { + if (info == nullptr) { + printf("lidar info change callback failed, the info is nullptr.\n"); + return; + } + printf("LidarInfoChangeCallback Lidar handle: %u SN: %s\n", handle, info->sn); + + // set the work mode to kLivoxLidarNormal, namely start the lidar + SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, WorkModeCallback, nullptr); + + QueryLivoxLidarInternalInfo(handle, QueryInternalInfoCallback, nullptr); + + // LivoxLidarIpInfo lidar_ip_info; + // strcpy(lidar_ip_info.ip_addr, "192.168.1.10"); + // strcpy(lidar_ip_info.net_mask, "255.255.255.0"); + // strcpy(lidar_ip_info.gw_addr, "192.168.1.1"); + // SetLivoxLidarLidarIp(handle, &lidar_ip_info, SetIpInfoCallback, nullptr); +} + +void LivoxLidarPushMsgCallback(const uint32_t handle, const uint8_t dev_type, const char* info, void* client_data) { + struct in_addr tmp_addr; + tmp_addr.s_addr = handle; + std::cout << "handle: " << handle << ", ip: " << inet_ntoa(tmp_addr) << ", push msg info: " << std::endl; + std::cout << info << std::endl; + return; +} + +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(); + } + + // REQUIRED, to get point cloud data via 'PointCloudCallback' + SetLivoxLidarPointCloudCallBack(PointCloudCallback, &points_message); + + // OPTIONAL, to get imu data via 'ImuDataCallback' + // some lidar types DO NOT contain an imu component + // SetLivoxLidarImuDataCallback(ImuDataCallback, nullptr); + + // SetLivoxLidarInfoCallback(LivoxLidarPushMsgCallback, nullptr); + + // REQUIRED, to get a handle to targeted lidar and set its work mode to NORMAL + // SetLivoxLidarInfoChangeCallback(LidarInfoChangeCallback, nullptr); + + threadLoop(); + } + + Livox::~Livox(){ + LivoxLidarSdkUninit(); + threadClose(); + } + + void Livox::pull(rai::Configuration& C){ + //-- Update configuration + const char* name = "livox_point_cloud"; + rai::Frame *base = C.getFrame(name, false); + if(!base){ + LOG(0) << "Creating new frame 'livox_point_cloud'"; + base = C.addFrame(name); + base->setColor({0., 1., 0.}); + } + + std::lock_guard lock(mux); + + base->setPointCloud(points); + } + + void Livox::step(){ + points = zeros(max_points*3); + 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..74b302d --- /dev/null +++ b/src/Livox/livox.h @@ -0,0 +1,40 @@ +#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 { + + int max_points; + arr points; + PointCloudData points_message; + + RAI_PARAM("livox/", double, filter, .9) + + Livox(); + ~Livox(); + + void pull(rai::Configuration& C); + + void step(); + + private: + std::mutex mux; + std::map poses; + }; + +} //namespace From be57c25a36008531e2a9cb25137ab3256564b890 Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Mon, 24 Mar 2025 20:46:44 +0100 Subject: [PATCH 21/30] livox init From 66204d4bb67b105494782f0bab7d7ad4dc02a63a Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Tue, 25 Mar 2025 11:38:10 +0100 Subject: [PATCH 22/30] Livox tests --- test/26-livox/Makefile | 8 ++++++++ test/26-livox/main.cpp | 24 ++++++++++++++++++++++++ test/26-livox/mid360_config.json | 22 ++++++++++++++++++++++ test/26-livox/rai.cfg | 1 + 4 files changed, 55 insertions(+) create mode 100644 test/26-livox/Makefile create mode 100644 test/26-livox/main.cpp create mode 100644 test/26-livox/mid360_config.json create mode 100644 test/26-livox/rai.cfg 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 < Date: Tue, 25 Mar 2025 17:51:42 +0100 Subject: [PATCH 23/30] removed extra functions --- src/Livox/livox.cpp | 131 +++----------------------------------------- 1 file changed, 8 insertions(+), 123 deletions(-) diff --git a/src/Livox/livox.cpp b/src/Livox/livox.cpp index 70785d6..455b4a0 100644 --- a/src/Livox/livox.cpp +++ b/src/Livox/livox.cpp @@ -40,9 +40,9 @@ void PointCloudCallback(uint32_t handle, const uint8_t dev_type, LivoxLidarEther 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 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()); // Remove the first point + pcData->points.erase(pcData->points.begin()); } // Add the point to the vector in the struct @@ -58,114 +58,6 @@ void PointCloudCallback(uint32_t handle, const uint8_t dev_type, LivoxLidarEther } } -void ImuDataCallback(uint32_t handle, const uint8_t dev_type, LivoxLidarEthernetPacket* data, void* client_data) { - if (data == nullptr) { - return; - } - printf("Imu data callback handle:%u, data_num:%u, data_type:%u, length:%u, frame_counter:%u.\n", - handle, data->dot_num, data->data_type, data->length, data->frame_cnt); -} - -void WorkModeCallback(livox_status status, uint32_t handle,LivoxLidarAsyncControlResponse *response, void *client_data) { - if (response == nullptr) { - return; - } - printf("WorkModeCallack, status:%u, handle:%u, ret_code:%u, error_key:%u", - status, handle, response->ret_code, response->error_key); - -} - -void RebootCallback(livox_status status, uint32_t handle, LivoxLidarRebootResponse* response, void* client_data) { - if (response == nullptr) { - return; - } - printf("RebootCallback, status:%u, handle:%u, ret_code:%u", - status, handle, response->ret_code); -} - -void SetIpInfoCallback(livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse *response, void *client_data) { - if (response == nullptr) { - return; - } - printf("LivoxLidarIpInfoCallback, status:%u, handle:%u, ret_code:%u, error_key:%u", - status, handle, response->ret_code, response->error_key); - - if (response->ret_code == 0 && response->error_key == 0) { - LivoxLidarRequestReboot(handle, RebootCallback, nullptr); - } -} - -void QueryInternalInfoCallback(livox_status status, uint32_t handle, - LivoxLidarDiagInternalInfoResponse* response, void* client_data) { - if (status != kLivoxLidarStatusSuccess) { - printf("Query lidar internal info failed.\n"); - QueryLivoxLidarInternalInfo(handle, QueryInternalInfoCallback, nullptr); - return; - } - - if (response == nullptr) { - return; - } - - uint8_t host_point_ipaddr[4] {0}; - uint16_t host_point_port = 0; - uint16_t lidar_point_port = 0; - - uint8_t host_imu_ipaddr[4] {0}; - uint16_t host_imu_data_port = 0; - uint16_t lidar_imu_data_port = 0; - - uint16_t off = 0; - for (uint8_t i = 0; i < response->param_num; ++i) { - LivoxLidarKeyValueParam* kv = (LivoxLidarKeyValueParam*)&response->data[off]; - if (kv->key == kKeyLidarPointDataHostIpCfg) { - memcpy(host_point_ipaddr, &(kv->value[0]), sizeof(uint8_t) * 4); - memcpy(&(host_point_port), &(kv->value[4]), sizeof(uint16_t)); - memcpy(&(lidar_point_port), &(kv->value[6]), sizeof(uint16_t)); - } else if (kv->key == kKeyLidarImuHostIpCfg) { - memcpy(host_imu_ipaddr, &(kv->value[0]), sizeof(uint8_t) * 4); - memcpy(&(host_imu_data_port), &(kv->value[4]), sizeof(uint16_t)); - memcpy(&(lidar_imu_data_port), &(kv->value[6]), sizeof(uint16_t)); - } - off += sizeof(uint16_t) * 2; - off += kv->length; - } - - printf("Host point cloud ip addr:%u.%u.%u.%u, host point cloud port:%u, lidar point cloud port:%u.\n", - host_point_ipaddr[0], host_point_ipaddr[1], host_point_ipaddr[2], host_point_ipaddr[3], host_point_port, lidar_point_port); - - printf("Host imu ip addr:%u.%u.%u.%u, host imu port:%u, lidar imu port:%u.\n", - host_imu_ipaddr[0], host_imu_ipaddr[1], host_imu_ipaddr[2], host_imu_ipaddr[3], host_imu_data_port, lidar_imu_data_port); - -} - -void LidarInfoChangeCallback(const uint32_t handle, const LivoxLidarInfo* info, void* client_data) { - if (info == nullptr) { - printf("lidar info change callback failed, the info is nullptr.\n"); - return; - } - printf("LidarInfoChangeCallback Lidar handle: %u SN: %s\n", handle, info->sn); - - // set the work mode to kLivoxLidarNormal, namely start the lidar - SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, WorkModeCallback, nullptr); - - QueryLivoxLidarInternalInfo(handle, QueryInternalInfoCallback, nullptr); - - // LivoxLidarIpInfo lidar_ip_info; - // strcpy(lidar_ip_info.ip_addr, "192.168.1.10"); - // strcpy(lidar_ip_info.net_mask, "255.255.255.0"); - // strcpy(lidar_ip_info.gw_addr, "192.168.1.1"); - // SetLivoxLidarLidarIp(handle, &lidar_ip_info, SetIpInfoCallback, nullptr); -} - -void LivoxLidarPushMsgCallback(const uint32_t handle, const uint8_t dev_type, const char* info, void* client_data) { - struct in_addr tmp_addr; - tmp_addr.s_addr = handle; - std::cout << "handle: " << handle << ", ip: " << inet_ntoa(tmp_addr) << ", push msg info: " << std::endl; - std::cout << info << std::endl; - return; -} - namespace rai{ Livox::Livox() : Thread("LivoxThread", 0.){ @@ -177,22 +69,15 @@ namespace rai{ const std::string path = "./mid360_config.json"; - if (!LivoxLidarSdkInit(path.c_str())) { + if (!LivoxLidarSdkInit(path.c_str())) + { printf("Livox Init Failed\n"); LivoxLidarSdkUninit(); } - - // REQUIRED, to get point cloud data via 'PointCloudCallback' - SetLivoxLidarPointCloudCallBack(PointCloudCallback, &points_message); - - // OPTIONAL, to get imu data via 'ImuDataCallback' - // some lidar types DO NOT contain an imu component - // SetLivoxLidarImuDataCallback(ImuDataCallback, nullptr); - - // SetLivoxLidarInfoCallback(LivoxLidarPushMsgCallback, nullptr); - - // REQUIRED, to get a handle to targeted lidar and set its work mode to NORMAL - // SetLivoxLidarInfoChangeCallback(LidarInfoChangeCallback, nullptr); + else + { + SetLivoxLidarPointCloudCallBack(PointCloudCallback, &points_message); + } threadLoop(); } From 7b856bdc5109d1b88306a815e98d96f2d9d7897f Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Wed, 26 Mar 2025 14:20:55 +0100 Subject: [PATCH 24/30] fixed stalling issues --- src/Ranger/ranger.cpp | 177 ++++++++++++++++++++++++++++++++++++------ 1 file changed, 153 insertions(+), 24 deletions(-) diff --git a/src/Ranger/ranger.cpp b/src/Ranger/ranger.cpp index 00fbdd4..fb1ea99 100644 --- a/src/Ranger/ranger.cpp +++ b/src/Ranger/ranger.cpp @@ -25,13 +25,17 @@ struct RangerController int socket_fd; int mode; // 0: Oblique, 1: Spin, 2: Ackermann arr qLast, sLast; //q: joint state; s: motor state - double actual_steering_rad; + double real_steering_rad; + double real_steering_rad_s; + double target_steering_rad_s; RangerController(const char* address) { mode = 0; qLast = zeros(3); - actual_steering_rad = 0.0; // CAREFULL!! Only updates when you call "get_qDot_oblique". + 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 @@ -136,24 +140,53 @@ struct RangerController 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 - actual_steering_rad = actual_steering_angle / 1000.0; // Convert mrad to rad + 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: " << actual_steering_rad << " rad" << std::endl; + std::cout << "Actual Steering Angle before: " << real_steering_rad << " rad" << std::endl; #endif - actual_steering_rad *= -1; + real_steering_rad *= -1; if (actual_speed_mps < 0) { // Lower side - actual_steering_rad += PI; + real_steering_rad += PI; } // Compute X-Y velocity components - double angle = actual_steering_rad - qLast.elem(2); + 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: " << actual_steering_rad << " rad" << 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_ackerman() + { + 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; + real_steering_rad = actual_steering_angle / 1000.0; + + // Compute X-Y velocity components + double angle = real_steering_rad - qLast.elem(2); + double velocity_x = actual_speed_mps * cos(real_steering_rad); + double velocity_y = actual_speed_mps * -sin(real_steering_rad); + double velocity_phi = + + #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; @@ -219,19 +252,35 @@ struct RangerController 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); - qDot = get_qDot_oblique(); - double steering_angle = actual_steering_rad - qLast.elem(2); + 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 + qDot = get_qDot_ackerman(); + 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; + double phiDelta = 0.0; + qDelta = arr{xDelta, yDelta, phiDelta}; + break; + } + default: { break; } @@ -268,6 +317,8 @@ struct RangerController } } + 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; @@ -290,6 +341,56 @@ struct RangerController 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 setVelocitiesAckerman(double linear_vel, double steering_angle) + { + struct can_frame frame_motion; + frame_motion.can_dlc = 8; + frame_motion.can_id = 0x111; + + #if DEBUG + std::cout << "---------- Ackerman mode inputs ----------" << std::endl; + std::cout << "Linear velocity: " << linear_vel << " m/s" << std::endl; + std::cout << "Steering angle: " << steering_angle << " m/s" << std::endl; + #endif + + int16_t linear_vel_ = linear_vel * 1000; + int16_t spin_speed_ = 0; + int16_t steering_angle_ = steering_angle * 1000; + + frame_motion.data[0] = (linear_vel_ >> 8) & 0xFF; + frame_motion.data[1] = linear_vel_ & 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; @@ -342,10 +443,14 @@ struct RangerController 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 { @@ -374,27 +479,40 @@ struct RangerController } double dir_magnitude = ::sqrt(::pow(qDotTarget.elem(0), 2) + ::pow(qDotTarget.elem(1), 2)); - double oblique_dir = ::acos(qDotTarget.elem(0) / dir_magnitude); + double target_steering_rad = ::acos(qDotTarget.elem(0) / dir_magnitude); if (qDotTarget.elem(1) > 0) { - oblique_dir = PI*2-oblique_dir; + target_steering_rad = PI*2-target_steering_rad; } // Adjust for current rotation - oblique_dir += qLast.elem(2); + target_steering_rad += qLast.elem(2); - if (oblique_dir < 0) { - oblique_dir += PI*2; - } else if (oblique_dir >= PI*2) { - oblique_dir -= PI*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: " << oblique_dir << " rad" << std::endl; + std::cout << "Angle: " << target_steering_rad << " rad" << std::endl; #endif - setVelocitiesOblique(oblique_dir, dir_magnitude); + setVelocitiesOblique(target_steering_rad, dir_magnitude); + break; + } + + case 2: { + // Ackerman mode + if (mode != prev_mode) { + set_control_mode(0x00); + } + + double linear_vel = sqrt(pow(qDotTarget.elem(0), 2) + pow(qDotTarget.elem(1), 2)); + double steering_angle = 0.0; + + setVelocitiesAckerman(linear_vel, steering_angle); break; } @@ -519,10 +637,12 @@ void RangerThread::step(){ 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)); } } - LOG(0) << "q_real" << q_real; - LOG(0) << "qDot_real" << qDot_real; - LOG(0) << "q_ref" << q_ref; - LOG(0) << "qDot_ref" << qDot_ref; + #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){ @@ -531,9 +651,18 @@ void RangerThread::step(){ arr del = q_ref - q_real; err = ::sqrt(scalarProduct(del, P_compliance*del)); } - if(err>.3){ //stall! + 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 + } } } From 28161d43f41073403764fa90a84c20428e231ce5 Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Wed, 26 Mar 2025 18:57:17 +0100 Subject: [PATCH 25/30] some bug fixes and added slam --- src/Livox/livox.cpp | 8 +++-- src/Livox/livox.h | 24 ++++++------- src/SLAM/slam.cpp | 85 +++++++++++++++++++++++++++++++++++++++++++++ src/SLAM/slam.h | 23 ++++++++++++ 4 files changed, 124 insertions(+), 16 deletions(-) create mode 100644 src/SLAM/slam.cpp create mode 100644 src/SLAM/slam.h diff --git a/src/Livox/livox.cpp b/src/Livox/livox.cpp index 455b4a0..975cb45 100644 --- a/src/Livox/livox.cpp +++ b/src/Livox/livox.cpp @@ -89,10 +89,10 @@ namespace rai{ void Livox::pull(rai::Configuration& C){ //-- Update configuration - const char* name = "livox_point_cloud"; + const char* name = "livox_lidar"; rai::Frame *base = C.getFrame(name, false); if(!base){ - LOG(0) << "Creating new frame 'livox_point_cloud'"; + LOG(0) << "Creating new frame 'livox_lidar'"; base = C.addFrame(name); base->setColor({0., 1., 0.}); } @@ -103,7 +103,9 @@ namespace rai{ } void Livox::step(){ - points = zeros(max_points*3); + + std::lock_guard lock(mux); + int counter = 0; for (const auto& point : points_message.points) { points.elem(counter) = point.x; diff --git a/src/Livox/livox.h b/src/Livox/livox.h index 74b302d..b922418 100644 --- a/src/Livox/livox.h +++ b/src/Livox/livox.h @@ -15,26 +15,24 @@ struct PointCloudData { int max_points; }; -namespace rai { - - struct Livox : Thread { - - int max_points; - arr points; - PointCloudData points_message; - +namespace rai +{ + struct Livox : Thread + { RAI_PARAM("livox/", double, filter, .9) - + Livox(); ~Livox(); - + void pull(rai::Configuration& C); - + void step(); - + private: std::mutex mux; - std::map poses; + int max_points; + arr points; + PointCloudData points_message; }; } //namespace diff --git a/src/SLAM/slam.cpp b/src/SLAM/slam.cpp new file mode 100644 index 0000000..57ed9fd --- /dev/null +++ b/src/SLAM/slam.cpp @@ -0,0 +1,85 @@ +#include "slam.h" + +#define RAI_SLAM +#ifdef RAI_SLAM + +#include + + +const char* slam_base_frame_name = "slam_graph_base"; +const char* mobile_base_frame = "ranger_coll"; + +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); +} + +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 From 9e666b421e7f2384521c1bf8c64eb4cefe274ae5 Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Tue, 8 Jul 2025 17:51:27 +0200 Subject: [PATCH 26/30] new ranger code --- src/Gamepad/gamepad.cpp | 80 +++++++++++++---------------------------- src/Gamepad/gamepad.h | 33 ++++++++--------- src/Livox/livox.cpp | 1 - src/Livox/livox.h | 2 +- src/Ranger/ranger.cpp | 76 --------------------------------------- src/SLAM/slam.cpp | 27 ++++++++++++++ 6 files changed, 70 insertions(+), 149 deletions(-) diff --git a/src/Gamepad/gamepad.cpp b/src/Gamepad/gamepad.cpp index cc0d540..aa97314 100644 --- a/src/Gamepad/gamepad.cpp +++ b/src/Gamepad/gamepad.cpp @@ -1,7 +1,5 @@ #include "gamepad.h" - -#define RAI_PLIB #ifdef RAI_PLIB #include @@ -19,80 +17,52 @@ GamepadInterface::~GamepadInterface(){ void GamepadInterface::open(){ jsInit(); - count = 0; - for(uint i=0;;i++) { - struct jsJoystick* joystick = new jsJoystick(i); + 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 = " << joystick->getNumButtons() - << "\nerror? = " << joystick->notWorking() - << "______________" << std::endl; + // << " \n#buttons = " <getNumButtons() + << "\nerror? = " <notWorking() + << std::endl; - if(n==2) { - joysticks[count] = joystick; - count++; - if (count >= 2) break; - } else { - delete joystick; - } + if(n>=4 && n<=8) break; + //iterate and try a new one + delete joystick; } - std::cout << "Found " << count << " gamepad(s)!" << std::endl; step(); //clear the buffers... } void GamepadInterface::close(){ - for (int i = 0; i < count; i++) { - delete joysticks[i]; - gamepadState[i].set()->clear(); - } + delete joystick; + gamepadState.set()->clear(); } void GamepadInterface::step(){ - arr pad_logs; - pad_logs.resize(count).setZero(); - ctrlTime += .01; - for (int i = 0; i < count; i++) { - if(joysticks[i]->notWorking()) return; - uint n=joysticks[i]->getNumAxes(); - floatA A(n); - int B; - joysticks[i]->rawRead(&B, A.p); - gamepadState[i].writeAccess(); - gamepadState[i]().resize(n+1); - gamepadState[i]()(0)=B; - for(uint j=0; j20 && stopButtons(gamepadState[i]())){ - LOG(1) <<"*** STOP button pressed"; - // moduleShutdown()->incrementStatus(); - quitSignal.set()=true; - } - gamepadState[i].deAccess(); - pad_logs.elem(i) = getButtonPressed(i); - } - if(writeData>0){ - if(!dataFile.is_open()) dataFile.open(STRING("z.gamepad.dat")); - dataFile <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(); -int GamepadInterface::getButtonPressed(unsigned int controller_id){ - gamepadState[controller_id].readAccess(); - int value = -1; - if(gamepadState[controller_id]().N) { - value = gamepadState[controller_id].get()().elem(0); - } - gamepadState[controller_id].deAccess(); - return value; } #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(){ } diff --git a/src/Gamepad/gamepad.h b/src/Gamepad/gamepad.h index 3968e2f..bb3ef0a 100644 --- a/src/Gamepad/gamepad.h +++ b/src/Gamepad/gamepad.h @@ -4,22 +4,15 @@ #include #include - struct GamepadInterface : Thread { - int count; - struct jsJoystick* joysticks[2]; - Var gamepadState[2]; + struct jsJoystick *joystick; + Var gamepadState; Var quitSignal; GamepadInterface(); ~GamepadInterface(); void open(); void step(); void close(); - int getButtonPressed(unsigned int controller_id=0); - // Logging - int writeData = 0; - double ctrlTime = 0.; - ofstream dataFile; }; /// The buttons on gamepad have the following values. @@ -27,21 +20,29 @@ struct GamepadInterface : Thread { /// button presses. enum BUTTON { BTN_NONE = 0, - BTN_X = 1, - BTN_A = 2, - BTN_B = 4, + BTN_A = 1, + BTN_B = 2, + BTN_X = 4, BTN_Y = 8, - BTN_L = 16, - BTN_R = 32, - BTN_SELECT = 256, + 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_START) return true; + 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 index 975cb45..e8a56c1 100644 --- a/src/Livox/livox.cpp +++ b/src/Livox/livox.cpp @@ -48,7 +48,6 @@ void PointCloudCallback(uint32_t handle, const uint8_t dev_type, LivoxLidarEther // Add the point to the vector in the struct pcData->points.push_back(point); } - } } else if (data->data_type == kLivoxLidarCartesianCoordinateLowData) { diff --git a/src/Livox/livox.h b/src/Livox/livox.h index b922418..57e4985 100644 --- a/src/Livox/livox.h +++ b/src/Livox/livox.h @@ -28,11 +28,11 @@ namespace rai void step(); + PointCloudData points_message; private: std::mutex mux; int max_points; arr points; - PointCloudData points_message; }; } //namespace diff --git a/src/Ranger/ranger.cpp b/src/Ranger/ranger.cpp index fb1ea99..7a3905f 100644 --- a/src/Ranger/ranger.cpp +++ b/src/Ranger/ranger.cpp @@ -167,34 +167,6 @@ struct RangerController return arr{velocity_x, velocity_y, 0.}; } - arr get_qDot_ackerman() - { - 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; - real_steering_rad = actual_steering_angle / 1000.0; - - // Compute X-Y velocity components - double angle = real_steering_rad - qLast.elem(2); - double velocity_x = actual_speed_mps * cos(real_steering_rad); - double velocity_y = actual_speed_mps * -sin(real_steering_rad); - double velocity_phi = - - #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); @@ -267,17 +239,6 @@ struct RangerController case 2: { // Ackerman mode - qDot = get_qDot_ackerman(); - 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; - double phiDelta = 0.0; - qDelta = arr{xDelta, yDelta, phiDelta}; break; } @@ -363,34 +324,6 @@ struct RangerController send_package(frame_motion); } - void setVelocitiesAckerman(double linear_vel, double steering_angle) - { - struct can_frame frame_motion; - frame_motion.can_dlc = 8; - frame_motion.can_id = 0x111; - - #if DEBUG - std::cout << "---------- Ackerman mode inputs ----------" << std::endl; - std::cout << "Linear velocity: " << linear_vel << " m/s" << std::endl; - std::cout << "Steering angle: " << steering_angle << " m/s" << std::endl; - #endif - - int16_t linear_vel_ = linear_vel * 1000; - int16_t spin_speed_ = 0; - int16_t steering_angle_ = steering_angle * 1000; - - frame_motion.data[0] = (linear_vel_ >> 8) & 0xFF; - frame_motion.data[1] = linear_vel_ & 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; @@ -504,15 +437,6 @@ struct RangerController } case 2: { - // Ackerman mode - if (mode != prev_mode) { - set_control_mode(0x00); - } - - double linear_vel = sqrt(pow(qDotTarget.elem(0), 2) + pow(qDotTarget.elem(1), 2)); - double steering_angle = 0.0; - - setVelocitiesAckerman(linear_vel, steering_angle); break; } diff --git a/src/SLAM/slam.cpp b/src/SLAM/slam.cpp index 57ed9fd..243d394 100644 --- a/src/SLAM/slam.cpp +++ b/src/SLAM/slam.cpp @@ -4,10 +4,14 @@ #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) { @@ -24,6 +28,29 @@ void set_new_node_frame(rai::Configuration& C, int node_id) 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) From 5f76f617e8b937cb4f7b7e5e637db91dec80427e Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Tue, 8 Jul 2025 17:53:23 +0200 Subject: [PATCH 27/30] ranger control and data collection --- test/31-gamepad/Makefile | 7 ++ test/31-gamepad/main.cpp | 26 +++++++ test/32-ranger_control/Makefile | 9 +++ test/32-ranger_control/main.cpp | 84 +++++++++++++++++++++++ test/32-ranger_control/main.py | 57 +++++++++++++++ test/32-ranger_control/mid360_config.json | 22 ++++++ test/32-ranger_control/rai.cfg | 27 ++++++++ 7 files changed, 232 insertions(+) create mode 100644 test/31-gamepad/Makefile create mode 100644 test/31-gamepad/main.cpp create mode 100644 test/32-ranger_control/Makefile create mode 100644 test/32-ranger_control/main.cpp create mode 100644 test/32-ranger_control/main.py create mode 100644 test/32-ranger_control/mid360_config.json create mode 100644 test/32-ranger_control/rai.cfg 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 From b89c758b23037415d7c88dfcd80b19717e40656c Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Mon, 15 Dec 2025 17:32:55 +0100 Subject: [PATCH 28/30] updated main --- src/BotOp/bot.cpp | 55 ++++++++++++++++++++++++++++++++++++----------- src/BotOp/bot.h | 10 +++++++-- 2 files changed, 51 insertions(+), 14 deletions(-) diff --git a/src/BotOp/bot.cpp b/src/BotOp/bot.cpp index d9621c3..91019a3 100644 --- a/src/BotOp/bot.cpp +++ b/src/BotOp/bot.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #ifdef RAI_VIVE # include @@ -43,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)){ @@ -62,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); @@ -95,9 +101,9 @@ BotOp::BotOp(rai::Configuration& C, bool useRealRobot){ } try{ - if(C.getFrame("ranger_world", false)){ + if(C.getFrame("ranger_base", false)){ LOG(0) <<"CONNECTING TO RANGER"; - robotL = make_shared(robotID++, uintA{0,1,2}, cmd, state); + robotR = make_shared(robotID++, uintA{0,1,2}, cmd, state); } } catch(const std::exception& ex) { LOG(-1) <<"Starting the ranger failed! Error msg: " <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)){ @@ -137,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(){ @@ -209,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 @@ -231,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); } } @@ -308,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; @@ -478,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(); From fc4094eecd528b8b2007d0e0cdf7d0d1b325fb76 Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Mon, 15 Dec 2025 17:33:36 +0100 Subject: [PATCH 29/30] hackathon updates --- src/Ranger/ranger.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/Ranger/ranger.cpp b/src/Ranger/ranger.cpp index 7a3905f..55a7900 100644 --- a/src/Ranger/ranger.cpp +++ b/src/Ranger/ranger.cpp @@ -16,7 +16,7 @@ #include #include // Include for ioctl and SIOCGIFINDEX #define PI 3.1415926 -#define DEBUG 1 +#define DEBUG 0 // TODO: setLights func in botop (also like flashing lights or so) @@ -262,19 +262,19 @@ struct RangerController if (dir_angle > PI) { // Lower left dir_angle -= PI; dir_angle *=-1; - std::cout << "Lower left" << std::endl; + // std::cout << "Lower left" << std::endl; } else { // Lower right dir_angle = PI - dir_angle; - std::cout << "Lower right" << std::endl; + // 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; + // std::cout << "Upper left" << std::endl; } else { // Upper right dir_angle *=-1; - std::cout << "Upper right" << std::endl; + // std::cout << "Upper right" << std::endl; } } @@ -466,9 +466,9 @@ void RangerThread::init(uint _robotID, const uintA& _qIndices) { qIndices_max = rai::max(qIndices); //-- basic Kp Kd settings for reference control mode - Kp = rai::getParameter("Ranger/Kp", arr{20., 20., 20.}); + Kp = rai::getParameter("Ranger/Kp", arr{1., 1., 1.}); Ki = rai::getParameter("Ranger/Ki", arr{0., 0., 0.}); - Kd = rai::getParameter("Ranger/Kd", arr{10., 10., 10.}); + Kd = rai::getParameter("Ranger/Kd", arr{0., 0., 0.}); LOG(0) << "Ranger/PID: Kp:" << Kp << " Ki:" << Ki << " Kd:" << Kd; From be60331794f6d8d57c3d7915d8c923e12450f30d Mon Sep 17 00:00:00 2001 From: Eckart Cobo-Briesewitz <68149355+Tupryk@users.noreply.github.com> Date: Mon, 15 Dec 2025 17:35:06 +0100 Subject: [PATCH 30/30] more updates from hackathon --- test/25-ranger/Makefile | 2 +- test/25-ranger/main.cpp | 141 +++++++----------------- test/25-ranger/rai.cfg | 4 +- test/27-livox_ranger/Makefile | 8 ++ test/27-livox_ranger/main.cpp | 62 +++++++++++ test/27-livox_ranger/mid360_config.json | 22 ++++ test/27-livox_ranger/rai.cfg | 2 + 7 files changed, 138 insertions(+), 103 deletions(-) create mode 100644 test/27-livox_ranger/Makefile create mode 100644 test/27-livox_ranger/main.cpp create mode 100644 test/27-livox_ranger/mid360_config.json create mode 100644 test/27-livox_ranger/rai.cfg diff --git a/test/25-ranger/Makefile b/test/25-ranger/Makefile index e553e71..fa3c617 100644 --- a/test/25-ranger/Makefile +++ b/test/25-ranger/Makefile @@ -1,4 +1,4 @@ -BASE = ../../../botop/rai +BASE = ../../../rai BASE2 = ../../../botop DEPEND = KOMO Core Geo Kin Gui Optim diff --git a/test/25-ranger/main.cpp b/test/25-ranger/main.cpp index 4f43a44..b58dae5 100644 --- a/test/25-ranger/main.cpp +++ b/test/25-ranger/main.cpp @@ -1,121 +1,62 @@ -#include #include #define ON_REAL true -arr getTrianglePath(rai::Configuration& C) -{ - C.addFrame("way0")->setShape(rai::ST_marker, {.3}).setPosition({.3, 0., 1.}); - C.addFrame("way1")->setShape(rai::ST_marker, {.3}).setPosition({.3, -.1, 1.}); - C.addFrame("way2")->setShape(rai::ST_marker, {.3}).setPosition({0, 0, 1.}); - - KOMO komo(C, 3., 20, 2, false); - - komo.addControlObjective({}, 2, 1.); - - komo.addObjective({1.}, FS_positionDiff, {"ranger", "way0"}, OT_eq, {1e1, 1e1, 0}); - komo.addObjective({2.}, FS_positionDiff, {"ranger", "way1"}, OT_eq, {1e1, 1e1, 0}); - komo.addObjective({3.}, FS_positionDiff, {"ranger", "way2"}, OT_eq, {1e1, 1e1, 0}); - - komo.opt.verbose=-1; - komo.optimize(); - - return komo.getPath_qOrg(); -} - -arr getYPath(rai::Configuration& C) -{ - C.addFrame("way0")->setShape(rai::ST_marker, {.3}).setPosition({.3, 0., 1.}); - C.addFrame("way2")->setShape(rai::ST_marker, {.3}).setPosition({0, 0, 1.}); - - KOMO komo(C, 2., 20, 2, false); - - komo.addControlObjective({}, 2, 1.); - - komo.addObjective({1.}, FS_positionDiff, {"ranger", "way0"}, OT_eq, {1e1, 1e1, 0}); - komo.addObjective({2.}, FS_positionDiff, {"ranger", "way2"}, OT_eq, {1e1, 1e1, 0}); - - komo.opt.verbose=-1; - komo.optimize(); - - return komo.getPath_qOrg(); -} - -arr getPathToTarget(rai::Configuration& C, double target_x, double target_y) -{ - C.addFrame("way0")->setShape(rai::ST_marker, {.3}).setPosition({target_x, target_y, 1.}); - C.addFrame("way2")->setShape(rai::ST_marker, {.3}).setPosition({0, 0, 1.}); - - KOMO komo(C, 2., 20, 2, false); - - komo.addControlObjective({}, 2, 1.); - - komo.addObjective({1.}, FS_positionDiff, {"ranger", "way0"}, OT_eq, {1e1, 1e1, 0}); - komo.addObjective({2.}, FS_positionDiff, {"ranger", "way2"}, OT_eq, {1e1, 1e1, 0}); - - komo.opt.verbose=-1; - komo.optimize(); - - return komo.getPath_qOrg(); -} - -void moveRanger(){ - rai::Configuration C; - C.addFile(rai::raiPath("../rai-robotModels/ranger/ranger_simplified.g")); - - // arr q = getTrianglePath(C); - arr q = getYPath(C); - C.view(true); - - BotOp bot(C, ON_REAL); - - bot.move(q, {10.}); - bot.wait(C); -} - -void moveRangerLoop(){ - rai::Configuration C; - C.addFile(rai::raiPath("../rai-robotModels/ranger/ranger_simplified.g")); - C.view(false); - - int point_count = 6; - double offset = M_PI*.25; - double mag = .3; - for (int i = 0; i < point_count; i++) { - double angle = static_cast(i)/static_cast(point_count) * M_PI*2.0 + offset; - arr q = getPathToTarget(C, cos(angle)*mag, -sin(angle)*mag); - - BotOp bot(C, ON_REAL); - - bot.move(q, {5.}); - bot.wait(C); - } -} - void justSpin(){ rai::Configuration C; - C.addFile(rai::raiPath("../rai-robotModels/ranger/ranger_simplified.g")); + // 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 = 5.; + double time = 1.; - arr q = arr{0., 0., -1.57}; - bot.moveTo(q, {time}); - bot.wait(C); - - q = arr{.1, .2, -1.57}; + arr 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, 0}; + + 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.}; + 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){ diff --git a/test/25-ranger/rai.cfg b/test/25-ranger/rai.cfg index d044b7f..7765380 100644 --- a/test/25-ranger/rai.cfg +++ b/test/25-ranger/rai.cfg @@ -19,5 +19,5 @@ KOMO/verbose: 4 #KOMO/useFCL: false -Ranger/Kp: [5, 5, 5] -Ranger/Kd: [1, 1, 1] +Ranger/Kp: [1, 1, 1] +Ranger/Kd: [0, 0, 0] diff --git a/test/27-livox_ranger/Makefile b/test/27-livox_ranger/Makefile new file mode 100644 index 0000000..a6d94e4 --- /dev/null +++ b/test/27-livox_ranger/Makefile @@ -0,0 +1,8 @@ +BASE = ../../../rai +BASE2 = ../../ + +EIGEN = 1 + +DEPEND = Core Gui OptiTrack + +include $(BASE)/_make/generic.mk diff --git a/test/27-livox_ranger/main.cpp b/test/27-livox_ranger/main.cpp new file mode 100644 index 0000000..632d09e --- /dev/null +++ b/test/27-livox_ranger/main.cpp @@ -0,0 +1,62 @@ +#include + +#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