diff --git a/CMakeLists.txt b/CMakeLists.txt index 3aa5395..0093983 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,11 +10,6 @@ include_directories( set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/../lib) -file(GLOB_RECURSE ADD_SRC_LIST - src/*/*.cpp -) -add_library(Z1_SDK_Linux64 SHARED ${ADD_SRC_LIST}) - link_directories(lib) add_executable(bigDemo examples/bigDemo.cpp ) @@ -23,7 +18,5 @@ add_executable(example_lowCmd_send examples/example_lowCmd_send.cpp ) target_link_libraries(example_lowCmd_send Z1_SDK_Linux64) add_executable(example_CtrlByFSM examples/example_CtrlByFSM.cpp ) target_link_libraries(example_CtrlByFSM Z1_SDK_Linux64) -add_executable(example_CtrlByTraj examples/example_CtrlByTraj.cpp ) -target_link_libraries(example_CtrlByTraj Z1_SDK_Linux64) add_executable(example_JointCtrl examples/example_JointCtrl.cpp ) target_link_libraries(example_JointCtrl Z1_SDK_Linux64) \ No newline at end of file diff --git a/README.md b/README.md deleted file mode 100644 index 55e1d8f..0000000 --- a/README.md +++ /dev/null @@ -1 +0,0 @@ -see [unitree-z1-docs](http://dev-z1.unitree.com/) \ No newline at end of file diff --git a/examples/bigDemo.cpp b/examples/bigDemo.cpp index cacdfd7..472292e 100644 --- a/examples/bigDemo.cpp +++ b/examples/bigDemo.cpp @@ -2,13 +2,14 @@ class Z1ARM : public unitreeArm{ public: - Z1ARM(CtrlComponents * ctrlComp):unitreeArm(ctrlComp){}; + Z1ARM():unitreeArm(true){}; ~Z1ARM(){}; void armCtrlByFSM(); - void armCtrlByTraj(); void armCtrlTrackInJointCtrl(); void armCtrlTrackInCartesian(); void printState(); +private: + Vec6 qPast; }; @@ -35,73 +36,13 @@ void Z1ARM::armCtrlByFSM() { MoveC(posture[0], posture[1], -M_PI/3.0, 0.3); } -void Z1ARM::armCtrlByTraj(){ - Vec6 posture[2]; - int order = 1; - - labelRun("forward"); - - _trajCmd.trajOrder = 0;//if order == 0, clear traj - setTraj(); - - // No.1 trajectory - _trajCmd.trajOrder = order++; - _trajCmd.trajType = TrajType::MoveJ; - _trajCmd.maxSpeed = 1.0;// angular velocity, rad/s - _trajCmd.gripperPos = 0.0; - posture[0] << 0.5,0.1,0.1,0.5,-0.2,0.5; - _trajCmd.posture[0] = Vec6toPosture(posture[0]); - setTraj(); - - // No.2 trajectory - _trajCmd.trajOrder = order++; - _trajCmd.trajType = TrajType::Stop; - _trajCmd.stopTime = 1.0; - _trajCmd.gripperPos = -1.0; - setTraj(); - - - // No.3 trajectory - _trajCmd.trajOrder = order++; - _trajCmd.trajType = TrajType::MoveL; - _trajCmd.maxSpeed = 0.3; // Cartesian velocity , m/s - _trajCmd.gripperPos = 0.0; - posture[0] << 0,0,0,0.45,-0.2,0.2; - _trajCmd.posture[0] = Vec6toPosture(posture[0]); - setTraj(); - - // No.4 trajectory - _trajCmd.trajOrder = order++; - _trajCmd.trajType = TrajType::Stop; - _trajCmd.stopTime = 1.0; - _trajCmd.gripperPos = -1.0; - setTraj(); - - // No.5 trajectory - _trajCmd.trajOrder = order++; - _trajCmd.trajType = TrajType::MoveC; - _trajCmd.maxSpeed = 0.3; // Cartesian velocity - _trajCmd.gripperPos = 0.0; - posture[0] << 0,0,0,0.45,0,0.4; - posture[1] << 0,0,0,0.45,0.2,0.2; - _trajCmd.posture[0] = Vec6toPosture(posture[0]); - _trajCmd.posture[1] = Vec6toPosture(posture[1]); - setTraj(); - - startTraj(); - // wait for trajectory completion - while (_ctrlComp->recvState.state != ArmFSMState::JOINTCTRL){ - usleep(_ctrlComp->dt*1000000); - } -} - void Z1ARM::armCtrlTrackInJointCtrl(){ labelRun("forward"); startTrack(ArmFSMState::JOINTCTRL); for(;;){ q(3) -= _ctrlComp->dt * 1.0;//max dt*PI, rad/s - qPast = _ctrlComp->lowstate->getQ(); + qPast = lowstate->getQ(); // std::cout << "qCmd: " << q.transpose() << " qState: " << qPast.transpose() << std::endl; //The joint has reached limit, there is warning: joint cmd is far from state double error = fabs(q(3) - qPast(3)); @@ -117,11 +58,11 @@ void Z1ARM::armCtrlTrackInCartesian(){ labelRun("forward"); startTrack(ArmFSMState::CARTESIAN); for(;;){ - _ctrlComp->lowcmd->endPosture(5) -= _ctrlComp->dt * 0.2;//z, m/s + endPosture(5) -= _ctrlComp->dt * 0.2;//z axis, m/s // no inverse kinematics solution, the joint has reached limit - // std::cout << "postureCmd: " << _ctrlComp->lowcmd->endPosture.transpose() << " qState: " << _ctrlComp->lowstate->endPosture.transpose() << std::endl; - double error = fabs(_ctrlComp->lowcmd->endPosture(5) - _ctrlComp->lowstate->endPosture(5)); + std::cout << "postureCmd: " << endPosture.transpose() << " qState: " << lowstate->endPosture.transpose() << std::endl; + double error = fabs(endPosture(5) - lowstate->endPosture(5)); if( error > 0.1){ break; } @@ -132,37 +73,33 @@ void Z1ARM::armCtrlTrackInCartesian(){ void Z1ARM::printState(){ std::cout<<"------ joint State ------"<lowstate->getQ().transpose()<lowstate->getQd().transpose()<lowstate->getTau().transpose()<getQ().transpose()<getQd().transpose()<getTau().transpose()<lowstate->endPosture.transpose()<endPosture.transpose()<start(); arm.backToStart(); - // size_t demo = 2; - for(size_t demo = 1; demo < 5; demo++) - // for(;;) + // size_t demo = 3; + for(size_t demo = 1; demo < 4; demo++) switch (demo) { case 1: arm.armCtrlByFSM(); break; case 2: - arm.armCtrlByTraj(); - break; - case 3: arm.armCtrlTrackInJointCtrl(); break; - case 4: + case 3: arm.armCtrlTrackInCartesian(); break; default: @@ -172,6 +109,5 @@ int main() { arm.backToStart(); arm.setFsm(ArmFSMState::PASSIVE); arm.sendRecvThread->shutdown(); - delete ctrlComp; return 0; } \ No newline at end of file diff --git a/examples/example_CtrlByFSM.cpp b/examples/example_CtrlByFSM.cpp index ac2a9d0..c2d41bf 100644 --- a/examples/example_CtrlByFSM.cpp +++ b/examples/example_CtrlByFSM.cpp @@ -2,8 +2,7 @@ int main() { Vec6 posture[2]; - CtrlComponents *ctrlComp = new CtrlComponents(0.002); - unitreeArm arm(ctrlComp); + unitreeArm arm(true); arm.sendRecvThread->start(); arm.backToStart(); @@ -32,6 +31,5 @@ int main() { arm.backToStart(); arm.setFsm(ArmFSMState::PASSIVE); arm.sendRecvThread->shutdown(); - delete ctrlComp; return 0; } \ No newline at end of file diff --git a/examples/example_CtrlByTraj.cpp b/examples/example_CtrlByTraj.cpp deleted file mode 100644 index a3c0071..0000000 --- a/examples/example_CtrlByTraj.cpp +++ /dev/null @@ -1,87 +0,0 @@ -#include "control/unitreeArm.h" - -class Z1ARM : public unitreeArm{ -public: - Z1ARM(CtrlComponents * ctrlComp):unitreeArm(ctrlComp){}; - ~Z1ARM(){}; - void setJointTraj(); - void setLineTraj(); - void setCircleTraj(); - void armCtrlTrackInCart(); - void printState(); -}; - -int main() { - CtrlComponents *ctrlComp = new CtrlComponents(0.002); - unitreeArm arm(ctrlComp); - arm.sendRecvThread->start(); - - arm.backToStart(); - - //example - Vec6 posture[2]; - int order = 1; - - arm.labelRun("forward"); - - arm._trajCmd.trajOrder = 0;//if order == 0, clear traj - arm.setTraj(); - - // No.1 trajectory - arm._trajCmd.trajOrder = order++; - arm._trajCmd.trajType = TrajType::MoveJ; - arm._trajCmd.maxSpeed = 1.0;// angular velocity, rad/s - arm._trajCmd.gripperPos = 0.0; - posture[0] << 0.5,0.1,0.1,0.5,-0.2,0.5; - arm._trajCmd.posture[0] = Vec6toPosture(posture[0]); - arm.setTraj(); - - // No.2 trajectory - arm._trajCmd.trajOrder = order++; - arm._trajCmd.trajType = TrajType::Stop; - arm._trajCmd.stopTime = 1.0; - arm._trajCmd.gripperPos = -1.0; - arm.setTraj(); - - - // No.3 trajectory - arm._trajCmd.trajOrder = order++; - arm._trajCmd.trajType = TrajType::MoveL; - arm._trajCmd.maxSpeed = 0.3; // Cartesian velocity , m/s - arm._trajCmd.gripperPos = 0.0; - posture[0] << 0,0,0,0.45,-0.2,0.2; - arm._trajCmd.posture[0] = Vec6toPosture(posture[0]); - arm.setTraj(); - - // No.4 trajectory - arm._trajCmd.trajOrder = order++; - arm._trajCmd.trajType = TrajType::Stop; - arm._trajCmd.stopTime = 1.0; - arm._trajCmd.gripperPos = -1.0; - arm.setTraj(); - - // No.5 trajectory - arm._trajCmd.trajOrder = order++; - arm._trajCmd.trajType = TrajType::MoveC; - arm._trajCmd.maxSpeed = 0.3; // Cartesian velocity - arm._trajCmd.gripperPos = 0.0; - posture[0] << 0,0,0,0.45,0,0.4; - posture[1] << 0,0,0,0.45,0.2,0.2; - arm._trajCmd.posture[0] = Vec6toPosture(posture[0]); - arm._trajCmd.posture[1] = Vec6toPosture(posture[1]); - arm.setTraj(); - - arm.startTraj(); - // wait for trajectory completion - while (arm._ctrlComp->recvState.state != ArmFSMState::JOINTCTRL){ - usleep(arm._ctrlComp->dt*1000000); - } - - - //stop - arm.backToStart(); - arm.setFsm(ArmFSMState::PASSIVE); - arm.sendRecvThread->shutdown(); - delete ctrlComp; - return 0; -} \ No newline at end of file diff --git a/examples/example_JointCtrl.cpp b/examples/example_JointCtrl.cpp index b1e7a75..503ccc4 100644 --- a/examples/example_JointCtrl.cpp +++ b/examples/example_JointCtrl.cpp @@ -23,20 +23,17 @@ public: } bool getJointCmd(Vec6& q, Vec6& qd, double& gripperQ, double& gripperQd){ _runTime(); - if(!_reached){ _s = _a3*pow(_tCost,3) + _a4*pow(_tCost,4) + _a5*pow(_tCost,5); _sDot = 3*_a3*pow(_tCost,2) + 4*_a4*pow(_tCost,3) + 5*_a5*pow(_tCost,4); q = _startQ + _s*(_endQ - _startQ); qd = _sDot * (_endQ - _startQ); - gripperQ = _gripperStartQ + (_gripperEndQ-_gripperStartQ)*_s; gripperQd = (_gripperEndQ-_gripperStartQ) * _sDot; }else{ q = _endQ; qd.setZero(); } - return _reached; } private: @@ -62,7 +59,7 @@ private: _tCost = (_tCost>_pathTime) ? _pathTime : _tCost; } void _generateA345(){ - if(_pathTime == 0){ + if(NearZero(_pathTime)){ _a3 = 0; _a4 = 0; _a5 = 0; @@ -77,8 +74,8 @@ private: class Z1ARM : public unitreeArm{ public: - Z1ARM(CtrlComponents * ctrlComp):unitreeArm(ctrlComp){ - myThread = new LoopFunc("Z1Communication", _ctrlComp->dt, boost::bind(&Z1ARM::run, this)); + Z1ARM():unitreeArm(true){ + myThread = new LoopFunc("Z1Communication", 0.002, boost::bind(&Z1ARM::run, this)); }; ~Z1ARM(){delete myThread;}; void setJointTraj(Vec6 startQ,Vec6 endQ, double speed){ @@ -101,8 +98,7 @@ public: int main(){ std::cout << std::fixed << std::setprecision(3); - CtrlComponents *ctrlComp = new CtrlComponents(0.002); - Z1ARM arm(ctrlComp); + Z1ARM arm; arm.myThread->start(); Vec6 forward; @@ -119,6 +115,5 @@ int main(){ arm.backToStart(); arm.setFsm(ArmFSMState::PASSIVE); arm.myThread->shutdown(); - delete ctrlComp; return 0; } \ No newline at end of file diff --git a/examples/example_lowCmd_send.cpp b/examples/example_lowCmd_send.cpp index 1a07514..7978411 100644 --- a/examples/example_lowCmd_send.cpp +++ b/examples/example_lowCmd_send.cpp @@ -2,8 +2,8 @@ class Z1ARM : public unitreeArm{ public: - Z1ARM(CtrlComponents * ctrlComp):unitreeArm(ctrlComp){ - runThread = new LoopFunc("Z1LowCmd", _ctrlComp->dt, boost::bind(&Z1ARM::run, this)); + Z1ARM():unitreeArm(true){ + runThread = new LoopFunc("Z1LowCmd", 0.002, boost::bind(&Z1ARM::run, this)); }; ~Z1ARM(){delete runThread;}; void run(); @@ -23,12 +23,15 @@ void Z1ARM::run(){ qd(5) = direction*1.5;// velocity // gripper, if arm doesn't has gripper, it does noting. gripperQ -= direction*_ctrlComp->dt*velocity; + Vec6 gTemp = _ctrlComp->armModel->inverseDynamics(q, qd, Vec6::Zero(), Vec6::Zero()); + gTemp(0) = tau(0); + tau = gTemp; sendRecv(); } int main(int argc, char *argv[]) { - CtrlComponents *ctrlComp = new CtrlComponents(0.002); - Z1ARM arm(ctrlComp); + std::cout << std::fixed << std::setprecision(3); + Z1ARM arm; arm.sendRecvThread->start(); arm.backToStart(); @@ -51,6 +54,8 @@ int main(int argc, char *argv[]) { arm.runThread->start();// using runThread instead of sendRecvThread for(int i(0); i<1000; i++){ + // The robot arm will have vibration due to the rigid impact of the speed + // when the direction changes arm.direction = i < 600 ? 1. : -1.; usleep(arm._ctrlComp->dt*1000000); } @@ -58,9 +63,9 @@ int main(int argc, char *argv[]) { arm.runThread->shutdown(); arm.sendRecvThread->start(); + arm.setFsm(ArmFSMState::JOINTCTRL); arm.backToStart(); arm.setFsm(ArmFSMState::PASSIVE); arm.sendRecvThread->shutdown(); - delete ctrlComp; return 0; } \ No newline at end of file diff --git a/include/unitree_arm_sdk/control/ctrlComponents.h b/include/unitree_arm_sdk/control/ctrlComponents.h index d78d12d..4a60351 100644 --- a/include/unitree_arm_sdk/control/ctrlComponents.h +++ b/include/unitree_arm_sdk/control/ctrlComponents.h @@ -2,26 +2,48 @@ #define CTRLCOMPONENTS_H #include "message/arm_common.h" -#include "utilities/CSVTool.h" #include "message/LowlevelCmd.h" #include "message/LowlevelState.h" #include "message/udp.h" #include "utilities/loop.h" +#include "model/ArmModel.h" struct CtrlComponents{ public: - CtrlComponents(double deltaT); + CtrlComponents(double deltaT, bool hasUnitreeGripper); ~CtrlComponents(); +/* + * Function: send udp message to z1_ctrl and receive udp message from it + * Input: None + * Output: None + * Description: The function will call udp->send() to send datas in lowcmd to z1_ctrl + * and call udp->recv() to store datas from z1_ctrl into lowstate + */ void sendRecv(); +/* + * Function: Set six joints commands to class lowcmd + * Input: q: joint angle + * qd: joint velocity + * tau: joint (Only used in State_LOWCMD) + * Output: None + */ void armCtrl(Vec6 q, Vec6 qd, Vec6 tau); +/* + * Function: Set gripper commands to class lowcmd + * Input: q: joint angle + * qd: joint velocity + * tau: joint (Only used in State_LOWCMD) + * Output: None + */ void gripperCtrl(double gripperPos, double gripperW, double gripperTau); LowlevelCmd *lowcmd; LowlevelState *lowstate; - double dt; - SendCmd sendCmd; // command to control the arm + double dt;// default: 0.002 + SendCmd sendCmd; // udp command to control the arm RecvState recvState; // the arm state receive from udp ArmFSMState statePast; + ArmModel* armModel; private: UDPPort *_udp; }; diff --git a/include/unitree_arm_sdk/control/unitreeArm.h b/include/unitree_arm_sdk/control/unitreeArm.h index fe0566f..2a833b9 100644 --- a/include/unitree_arm_sdk/control/unitreeArm.h +++ b/include/unitree_arm_sdk/control/unitreeArm.h @@ -5,34 +5,161 @@ class unitreeArm{ public: - unitreeArm(CtrlComponents * ctrlComp); - ~unitreeArm(); - void setFsm(ArmFSMState fsm); - void backToStart(); - void labelRun(std::string label); - void labelSave(std::string label); - void teach(std::string label); - void teachRepeat(std::string label); - void calibration(); - void MoveJ(Vec6 posture, double maxSpeed ); - void MoveJ(Vec6 posture, double gripperPos, double maxSpeed); - void MoveL(Vec6 posture, double maxSpeed); - void MoveL(Vec6 posture, double gripperPos, double maxSpeed); - void MoveC(Vec6 middlePosutre, Vec6 endPosture, double maxSpeed); - void MoveC(Vec6 middlePosutre, Vec6 endPosture, double gripperPos, double maxSpeed); - void setTraj(); - void startTraj(); - void startTrack(ArmFSMState fsm); +unitreeArm(bool hasUnitreeGripper); +~unitreeArm(); +/* + * Function: Change z1_ctrl state to fsm, wait until change complete + * Input: ArmFSMState + * Output: Whether swtich to fsm correctly + * Note: eaxmple: Only State_Passive could switch to State_LowCmd + */ +bool setFsm(ArmFSMState fsm); +/* + * Function: Move arm to home position + * wait until arrival home position, and then switch to State_JointCtrl + * Input: None + * Output: None + */ +void backToStart(); +/* + * Function: Move arm to label position + * wait until arrival label position, and then switch to State_JointCtrl + * Input: label + * which should exist in z1_controller/config/saveArmStates.csv. + * The number of characters in label cannot be greater than 10.(char name[10]) + * Output: None + */ +void labelRun(std::string label); +/* + * Function: Save current position as a label to saveArmStates.csv + * Switch to State_JointCtrl when done + * Input: label + * name to save, which shouldn't exist in z1_controller/config/saveArmStates.csv. + * The number of characters in label cannot be greater than 10.(char name[10]) + * Output: None + */ +void labelSave(std::string label); +/* + * Function: Save current position as a label to saveArmStates.csv + * Switch to State_JointCtrl when done + * Input: label + * name to save, which shouldn't exist in z1_controller/config/saveArmStates.csv. + * The number of characters in label cannot be greater than 10.(char name[10]) + * Output: None + */ +void teach(std::string label); +/* + * Function: Switch to State_Teach + * Input: label + * Teach trajectory will be save as Traj_label.csv in directory z1_controller/config/ + * The number of characters in label cannot be greater than 10.(char name[10]) + * Output: None + */ +void teachRepeat(std::string label); +/* + * Function: Calibrate the motor, make current position as home position + * Input: None + * Output: None + */ +void calibration(); +/* + * Function: Move the robot in a joint path + * Input: posture: target position, (roll pitch yaw x y z), unit: meter + * maxSpeed: the maximum joint speed when robot is moving, unit: radian/s + * range:[0, pi] + * Output: None + */ +bool MoveJ(Vec6 posture, double maxSpeed); +/* + * Function: Move the robot in a joint path, and control the gripper at the same time + * Input: posture: target position, (roll pitch yaw x y z), unit: meter + * gripperPos: target angular + * uint: radian + * range:[0, pi/3] + * maxSpeed: the maximum joint speed when robot is moving + * unit: radian/s + * range:[0, pi] + * Output: whether posture has inverse kinematics + */ +bool MoveJ(Vec6 posture, double gripperPos, double maxSpeed); +/* + * Function: Move the robot in a linear path + * Input: posture: target position, (roll pitch yaw x y z), unit: meter + * maxSpeed: the maximum joint speed when robot is moving, unit: m/s + * Output: whether posture has inverse kinematics + */ +bool MoveL(Vec6 posture, double maxSpeed); +/* + * Function: Move the robot in a linear path, and control the gripper at the same time + * Input: posture: target position, (roll pitch yaw x y z), unit: meter + * gripperPos: target angular, uint: radian + * range:[0, pi/3] + * maxSpeed: the maximum joint speed when robot is moving, unit: m/s + * Output: whether posture has inverse kinematics + */ +bool MoveL(Vec6 posture, double gripperPos, double maxSpeed); +/* + * Function: Move the robot in a circular path + * Input: middle posture: determine the shape of the circular path + * endPosture: target position, (roll pitch yaw x y z), unit: meter + * maxSpeed: the maximum joint speed when robot is moving, unit: m/s + * Output: whether posture has inverse kinematics + */ +bool MoveC(Vec6 middlePosutre, Vec6 endPosture, double maxSpeed); +/* + * Function: Move the robot in a circular path, and control the gripper at the same time + * Input: middle posture: determine the shape of the circular path + * endPosture: target position, (roll pitch yaw x y z), unit: meter + * gripperPos: target angular, uint: radian + * range:[0, pi/3] + * maxSpeed: the maximum joint speed when robot is moving, unit: m/s + * Output: whether posture has inverse kinematics + */ +bool MoveC(Vec6 middlePosutre, Vec6 endPosture, double gripperPos, double maxSpeed); +/* + * Function: Control robot with q&qd command in joint space or posture command in cartesian space + * Input: fsm: ArmFSMState::JOINTCTRL or ArmFSMState::CARTESIAN + * Output: whether posture has inverse kinematics + * Description: 1. ArmFSMState::JOINTCTRL, + * if you run function startTrack(ArmFSMState::JOINTCTRL), + * firstly, the following parameters will be set at the first time: + * q : <---- lowstate->getQ() + * qd: <---- lowstate->getQd() + * gripperQ: <---- lowstate->getGripperQ() + * gripperQd: <---- lowstate->getGripperQd() + * then you can change these parameters to control robot + * 2. ArmFSMState::CARTESIAN, + * if you run function startTrack(ArmFSMState::JOINTCTRL), + * firstly, the following parameters will be set at the first time: + * endPosture: <---- lowstate->endPosture; + * gripperQ: <---- lowstate->getGripperQ() + * then you can change these parameters to control robot + */ +void startTrack(ArmFSMState fsm); +/* + * Function: send udp message to z1_ctrl and receive udp message from it + * Input: None + * Output: None + * Description: sendRecvThread will run sendRecv() at a frequency of 500Hz + * ctrlcomp.sendRecv() is called in unitreeArm.sendRecv(), + * and set command parameters in unitreeArm to lowcmd automatically + * If you want to control robot under JOINTCTRL, CARTESIAN or LOWCMD, + * instead of MovecJ, MoveL, MoveC, and so on + * it is recommended to create your own thread to process command parameters + * (see stratTrack() description) + * and execute sendRecv() at the end of thread + */ +void sendRecv(); - void sendRecv(); +//command parameters +Vec6 q, qd, tau; +Vec6 endPosture; +double gripperQ, gripperW, gripperTau; - Vec6 q, qd, tau; - Vec6 qPast, qdPast, tauPast; - double gripperQ, gripperW, gripperTau; - - TrajCmd _trajCmd; - CtrlComponents *_ctrlComp; - LoopFunc *sendRecvThread; +LoopFunc *sendRecvThread; +LowlevelCmd *lowcmd;//same as _ctrlComp->lowcmd +LowlevelState *lowstate;//same as _ctrlComp->lowstate +CtrlComponents *_ctrlComp; }; #endif diff --git a/include/unitree_arm_sdk/math/mathTools.h b/include/unitree_arm_sdk/math/mathTools.h new file mode 100644 index 0000000..05e1804 --- /dev/null +++ b/include/unitree_arm_sdk/math/mathTools.h @@ -0,0 +1,244 @@ +#ifndef MATHTOOLS_H +#define MATHTOOLS_H + +#include +#include +#include "math/mathTypes.h" + +template +T max(T value){ + return value; +} + +template +inline T max(const T val0, const Args... restVal){ + return val0 > (max(restVal...)) ? val0 : max(restVal...); +} + +template +T min(T value){ + return value; +} + +template +inline T min(const T val0, const Args... restVal){ + return val0 > (min(restVal...)) ? val0 : min(restVal...); +} + +enum class TurnDirection{ + NOMATTER, + POSITIVE, + NEGATIVE +}; + +/* first - second */ +inline double angleError(double first, double second, TurnDirection direction = TurnDirection::NOMATTER){ + double firstMod = fmod(first, 2.0*M_PI); + double secondMod = fmod(second, 2.0*M_PI); + + if(direction == TurnDirection::POSITIVE){ + if(firstMod - secondMod < 0.0){ + return 2*M_PI + firstMod - secondMod; + }else{ + return firstMod - secondMod; + } + } + else if(direction == TurnDirection::NEGATIVE){ + if(firstMod - secondMod > 0.0){ + return -2*M_PI + firstMod - secondMod; + }else{ + return firstMod - secondMod; + } + }else{//no matter + if(fabs(firstMod - secondMod) > fabs(secondMod - firstMod)){ + return secondMod - firstMod; + }else{ + return firstMod - secondMod; + } + } +} + +/* firstVec - secondVec */ +inline VecX angleError(VecX firstVec, VecX secondVec, TurnDirection directionMatter = TurnDirection::NOMATTER){ + if(firstVec.rows() != secondVec.rows()){ + std::cout << "[ERROR] angleError, the sizes of firstVec and secondVec are different!" << std::endl; + } + + VecX result = firstVec; + for(int i(0); itolerance){ + return false; + } + } + return true; +} + +inline bool inInterval(double value, double limValue1, double limValue2, bool canEqual1 = false, bool canEqual2 = false){ + double lowLim, highLim; + bool lowEqual, highEqual; + if(limValue1 >= limValue2){ + highLim = limValue1; + highEqual = canEqual1; + lowLim = limValue2; + lowEqual = canEqual2; + }else{ + lowLim = limValue1; + lowEqual = canEqual1; + highLim = limValue2; + highEqual = canEqual2; + } + + if((value > highLim) || (value < lowLim)){ + return false; + } + if((value == highLim) && !highEqual){ + return false; + } + if((value == lowLim) && !lowEqual){ + return false; + } + + return true; +} + +inline double saturation(const double a, double limValue1, double limValue2){ + double lowLim, highLim; + if(limValue1 >= limValue2){ + lowLim = limValue2; + highLim= limValue1; + }else{ + lowLim = limValue1; + highLim= limValue2; + } + + if(a < lowLim){ + return lowLim; + }else if(a > highLim){ + return highLim; + }else{ + return a; + } +} + +inline double saturation(const double a, Vec2 limits){ + return saturation(a, limits(0), limits(1)); +} + +template +inline T0 killZeroOffset(T0 a, const T1 limit){ + if((a > -limit) && (a < limit)){ + a = 0; + } + return a; +} + +template +inline T1 invNormalize(const T0 value, const T1 min, const T2 max, const double minLim = -1, const double maxLim = 1){ + return (value-minLim)*(max-min)/(maxLim-minLim) + min; +} + +// x: [0, 1], windowRatio: (0, 0.5) +template +inline T windowFunc(const T x, const T windowRatio, const T xRange=1.0, const T yRange=1.0){ + if((x < 0)||(x > xRange)){ + std::cout << "[ERROR][windowFunc] The x=" << x << ", which should between [0, xRange]" << std::endl; + } + if((windowRatio <= 0)||(windowRatio >= 0.5)){ + std::cout << "[ERROR][windowFunc] The windowRatio=" << windowRatio << ", which should between [0, 0.5]" << std::endl; + } + + if(x/xRange < windowRatio){ + return x * yRange / (xRange * windowRatio); + }else if(x/xRange > 1 - windowRatio){ + return yRange * (xRange - x)/(xRange * windowRatio); + }else{ + return yRange; + } +} + +template +inline void updateAverage(T1 &exp, T2 newValue, double n){ + if(exp.rows()!=newValue.rows()){ + std::cout << "The size of updateAverage is error" << std::endl; + exit(-1); + } + if(fabs(n - 1) < 0.001){ + exp = newValue; + }else{ + exp = exp + (newValue - exp)/n; + } +} + +template +inline void updateCovariance(T1 &cov, T2 expPast, T3 newValue, double n){ + if( (cov.rows()!=cov.cols()) || (cov.rows() != expPast.rows()) || (expPast.rows()!=newValue.rows())){ + std::cout << "The size of updateCovariance is error" << std::endl; + exit(-1); + } + if(fabs(n - 1) < 0.1){ + cov.setZero(); + }else{ + cov = cov*(n-1)/n + (newValue-expPast)*(newValue-expPast).transpose()*(n-1)/(n*n); + } +} + +template +inline void updateAvgCov(T1 &cov, T2 &exp, T3 newValue, double n){ + // The order matters!!! covariance first!!! + updateCovariance(cov, exp, newValue, n); + updateAverage(exp, newValue, n); +} + +// Calculate average value and covariance +class AvgCov{ +public: + AvgCov(unsigned int size, std::string name, bool avgOnly=false, unsigned int showPeriod=1000, unsigned int waitCount=5000, double zoomFactor=10000) + :_size(size), _showPeriod(showPeriod), _waitCount(waitCount), _zoomFactor(zoomFactor), _valueName(name), _avgOnly(avgOnly) { + _exp.resize(size); + _cov.resize(size, size); + _defaultWeight.resize(size, size); + _defaultWeight.setIdentity(); + _measureCount = 0; + } + void measure(VecX newValue){ + ++_measureCount; + + if(_measureCount > _waitCount){ + updateAvgCov(_cov, _exp, newValue, _measureCount-_waitCount); + if(_measureCount % _showPeriod == 0){ + // if(_measureCount < _waitCount + 5){ + std::cout << "******" << _valueName << " measured count: " << _measureCount-_waitCount << "******" << std::endl; + // std::cout << _zoomFactor << " Times newValue of " << _valueName << std::endl << (_zoomFactor*newValue).transpose() << std::endl; + std::cout << _zoomFactor << " Times Average of " << _valueName << std::endl << (_zoomFactor*_exp).transpose() << std::endl; + if(!_avgOnly){ + std::cout << _zoomFactor << " Times Covariance of " << _valueName << std::endl << _zoomFactor*_cov << std::endl; + } + } + } + } +private: + VecX _exp; + MatX _cov; + MatX _defaultWeight; + bool _avgOnly; + unsigned int _size; + unsigned int _measureCount; + unsigned int _showPeriod; + unsigned int _waitCount; + double _zoomFactor; + std::string _valueName; +}; +#endif \ No newline at end of file diff --git a/include/unitree_arm_sdk/message/LowlevelCmd.h b/include/unitree_arm_sdk/message/LowlevelCmd.h index 2991d32..899441a 100644 --- a/include/unitree_arm_sdk/message/LowlevelCmd.h +++ b/include/unitree_arm_sdk/message/LowlevelCmd.h @@ -19,29 +19,61 @@ public: LowlevelCmd(); ~LowlevelCmd(){} - - void setZeroDq(); - void setZeroTau(); - void setZeroKp(); - void setZeroKd(); - void setControlGain(); - void setControlGain(std::vector KP, std::vector KW); - void setQ(std::vector qInput); - void setQ(VecX qInput); - void setQd(VecX qDInput); - void setTau(VecX tauInput); - void setPassive(); - void setGripperGain(); - void setGripperGain(double KP, double KW); - void setGripperZeroGain(); - void setGripperQ(double qInput); - double getGripperQ(); - void setGripperQd(double qdInput); - double getGripperQd(); - void setGripperTau(double tauInput); - double getGripperTau(); - Vec6 getQ(); - Vec6 getQd(); +/* dq = {0} */ +void setZeroDq(); +/* tau = {0} */ +void setZeroTau(); +/* kp = {0} */ +void setZeroKp(); +/* kd = {0} */ +void setZeroKd(); +/* + * set default arm kp & kd (Only used in State_LOWCMD) + * kp = [20, 30, 30, 20, 15, 10] + * kd = [2000, 2000, 2000, 2000, 2000, 2000] + */ +void setControlGain(); +/* kp = KP, kd = KW */ +void setControlGain(std::vector KP, std::vector KW); +/* q = qInput */ +void setQ(std::vector qInput); +/* q = qInput */ +void setQ(VecX qInput); +/* dq = qDInput */ +void setQd(VecX qDInput); +/* tau = tauInput */ +void setTau(VecX tauInput); +/* + * setZeroDq() + * setZeroTau() + * setZeroKp() + * kd = [10, 10, 10, 10, 10, 10] + */ +void setPassive(); +/* + * set default gripper kp & kd (Only used in State_LOWCMD) + * kp.at(kp.size()-1) = 20 + * kd.at(kd.size()-1) = 2000 + */ +void setGripperGain(); +/* + * kp.at(kp.size()-1) = KP + * kd.at(kd.size()-1) = KW + */ +void setGripperGain(double KP, double KW); +/* set gripper kp&kd to zero */ +void setGripperZeroGain(); +/* q.at(q.size()-1) = qInput; */ +void setGripperQ(double qInput); +double getGripperQ() {return q.at(q.size()-1);} +void setGripperQd(double qdInput) {dq.at(dq.size()-1) = qdInput;} +double getGripperQd() {return dq.at(dq.size()-1);} +void setGripperTau(double tauInput) {tau.at(tau.size()-1) = tauInput;} +double getGripperTau() {return tau.at(tau.size()-1);} +/* return Vec6 from std::vector q, without gripper */ +Vec6 getQ(); +/* return Vec6 from std::vector dq, without gripper */ +Vec6 getQd(); }; diff --git a/include/unitree_arm_sdk/message/LowlevelState.h b/include/unitree_arm_sdk/message/LowlevelState.h index 552ae86..b0163fa 100644 --- a/include/unitree_arm_sdk/message/LowlevelState.h +++ b/include/unitree_arm_sdk/message/LowlevelState.h @@ -23,7 +23,18 @@ public: std::vector tau; std::vector temperature; + /* 0x01 : phase current is too large + * 0x02 : phase leakage + * 0x04 : motor winding overheat or temperature is too large + * 0x20 : parameters jump + * 0x40 : Ignore + */ std::vector errorstate; + /* + * 0: OK + * 1: communication between lower computer and motor disconnect once + * 2: communication between lower computer and motor has CRC erro once + */ std::vector isMotorConnected; Vec6 getQ(); diff --git a/include/unitree_arm_sdk/message/udp.h b/include/unitree_arm_sdk/message/udp.h index a434e28..df5a224 100644 --- a/include/unitree_arm_sdk/message/udp.h +++ b/include/unitree_arm_sdk/message/udp.h @@ -13,7 +13,6 @@ enum class BlockYN{ NO }; - class IOPort{ public: IOPort(BlockYN blockYN, size_t recvLength, size_t timeOutUs){ diff --git a/include/unitree_arm_sdk/model/ArmModel.h b/include/unitree_arm_sdk/model/ArmModel.h new file mode 100644 index 0000000..1b4d16e --- /dev/null +++ b/include/unitree_arm_sdk/model/ArmModel.h @@ -0,0 +1,150 @@ +#ifndef ARMMODEL_H +#define ARMMODEL_H + +#include +#include "thirdparty/robotics.h" +#include "thirdparty/quadProgpp/QuadProg++.hh" + +using namespace robo; + +class ArmModel{ +public: + ArmModel(Vec3 endPosLocal, double endEffectorMass, Vec3 endEffectorCom, Mat3 endEffectorInertia); + ~ArmModel(){}; +/* + * Function: compute end effector frame (used for current spatial position calculation) + * Inputs: q: current joint angles + * index: it can set as 0,1,...,6 + * if index == 6, then compute end efftor frame, + * else compute joint_i frame + * Returns: Transfomation matrix representing the end-effector frame when the joints are + * at the specified coordinates + */ +HomoMat forwardKinematics(Vec6 q, int index = 6); +/* + * Function: Computes inverse kinematics in the space frame with the irerative approach + * Inputs: TDes: The desired end-effector configuration + * qPast: An initial guess and result output of joint angles that are close to + * satisfying TDes + * checkInWorkSpace: whether q_result shoule be around qPast + * eaxmple: there is a postion defined by q_temp which is within the C-space + * if qPast == Vec6::Zero(), + * the function will return false while checkInWorkSpace is false, + * and return true while checkInWorkSpace is true. + * Normally, you can use qPast = Vec6::Zero() and checkInWorkSpace == true + * to check whether q_temp has inverse kinematics sloutions + * Returns: success: A logical value where TRUE means that the function found + * a solution and FALSE means that it ran through the set + * number of maximum iterations without finding a solution + * q_result: Joint angles that achieve T within the specified tolerances, + */ +virtual bool inverseKinematics(HomoMat TDes, Vec6 qPast, Vec6& q_result, bool checkInWorkSpace = false); +/* + * Function: Gives the space Jacobian + * Inputs: q: current joint angles + * Returns: 6x6 Spatial Jacobian + */ +Mat6 CalcJacobian(Vec6 q); +/* + * Function: This function uses forward-backward Newton-Euler iterations to caculate inverse dynamics + * Inputs: q: joint angles + * qd: joint velocities + * qdd: joint accelerations + * Ftip: Spatial force applied by the end-effector + * Returns: required joint forces/torques + */ +Vec6 inverseDynamics(Vec6 q, Vec6 qd, Vec6 qdd, Vec6 Ftip); +virtual void solveQP(Vec6 twist, Vec6 qPast, Vec6& qd_result, double dt) = 0; +virtual bool checkInSingularity(Vec6 q) = 0; +/* + * Function: Limit q & qd inputs to valid values + * Inputs: q: set in range[_jointQMin, _jointQMax] + * qd: set in range[-_jointSpeedMax, _jointSpeedMax] + * Returns: None + */ +void jointProtect(Vec6& q, Vec6& qd); +std::vector getJointQMax() {return _jointQMax;} +std::vector getJointQMin() {return _jointQMin;} +std::vector getJointSpeedMax() {return _jointSpeedMax;} +/* + * Function: The load is applied to the end joint in equal proportion + and caculates the correspoding dynamic parameters + * Inputs: load: unit:kg, set in z1_controller/config/config.xml + * Returns: None + */ +void addLoad(double load); + + const size_t dof = 6; +protected: + bool _checkAngle(Vec6 ); + void _buildModel(); + // initial parameters + HomoMat _M; //End posture at the home position + std::vector _Mlist;// List of link frames {i} relative to {i-1} at the home position + Vec3 _gravity; + Mat6 _Slist;// spatial twist at home position + std::vector _Glist; + std::vector _jointAxis;// omega + std::vector _jointPos;// p_0 + std::vector _jointQMax; + std::vector _jointQMin; + std::vector _jointSpeedMax; + Vec3 _endPosLocal; // based on mount postion + Vec3 _endMountPosGlobal; + + std::vector _linkPosLocal; + std::vector _disJoint;//distance between joint + std::vector _linkMass; + std::vector _linkCom; // center of mass + std::vector _linkInertia; + double _endEffectorMass; + Vec3 _endEffectorCom; + Mat3 _endEffectorInertia; + + double _load; +}; + +class Z1Model : public ArmModel{ +public: + Z1Model(Vec3 endPosLocal = Vec3::Zero(), double endEffectorMass = 0.0, + Vec3 endEffectorCom = Vec3::Zero(), Mat3 endEffectorInertia = Mat3::Zero()); + ~Z1Model(){}; +/* + * Function: Check whether joint1 and joint5 is coaxial + * x5^2 + y5^2 < 0.1^2 + * Inputs: q: current joint variables + * Returns: bool + */ + bool checkInSingularity(Vec6 q); +/* + * Function: Computes inverse kinematics in the space frame with the analytical approach + * Inputs: TDes: The desired end-effector configuration + * qPast: An initial guess and result output of joint angles that are close to + * satisfying TDes + * checkInWorkSpace: whether q_result shoule be around qPast + * eaxmple: there is a postion defined by q_temp which is within the C-space + * if qPast == Vec6::Zero(), + * the function will return false while checkInWorkSpace is false, + * and return true while checkInWorkSpace is true. + * Normally, you can use qPast = Vec6::Zero() and checkInWorkSpace = true + * to check whether q_temp has inverse kinematics sloutions + * Returns: success: A logical value where TRUE means that the function found + * a solution and FALSE means that it ran through the set + * number of maximum iterations without finding a solution + * q_result: Joint angles that achieve T within the specified tolerances, + */ + bool inverseKinematics(HomoMat TDes, Vec6 qPast, Vec6& q_result, bool checkInWorkSpace = false); +/* + * Function: The function use quadprog++ to slove equation: qd = J.inverse() * twist, even if J has no inverse + * Inputs: twist: spatial velocity [R_dot, p_dot] + * qPast: current joint angles + * dt : compute period + * Returns: qd_result: joint velocity that are corresponding to twist + */ + void solveQP(Vec6 twist, Vec6 qPast, Vec6& qd_result, double dt); +private: + void setParam_V3_6(); + double _theta2Bias; +}; + +#endif \ No newline at end of file diff --git a/include/unitree_arm_sdk/thirdparty/quadProgpp/Array.hh b/include/unitree_arm_sdk/thirdparty/quadProgpp/Array.hh new file mode 100644 index 0000000..29e494d --- /dev/null +++ b/include/unitree_arm_sdk/thirdparty/quadProgpp/Array.hh @@ -0,0 +1,2532 @@ +// $Id: Array.hh 249 2008-11-20 09:58:23Z schaerf $ +// This file is part of EasyLocalpp: a C++ Object-Oriented framework +// aimed at easing the development of Local Search algorithms. +// Copyright (C) 2001--2008 Andrea Schaerf, Luca Di Gaspero. +// +// This software may be modified and distributed under the terms +// of the MIT license. See the LICENSE file for details. + +#if !defined(_ARRAY_HH) +#define _ARRAY_HH + +#include +#include +#include +#include +#include +#include + +namespace quadprogpp { + +enum MType { DIAG }; + +template +class Vector +{ +public: + Vector(); + Vector(const unsigned int n); + Vector(const T& a, const unsigned int n); //initialize to constant value + Vector(const T* a, const unsigned int n); // Initialize to array + Vector(const Vector &rhs); // copy constructor + ~Vector(); // destructor + + inline void set(const T* a, const unsigned int n); + Vector extract(const std::set& indexes) const; + inline T& operator[](const unsigned int& i); //i-th element + inline const T& operator[](const unsigned int& i) const; + + inline unsigned int size() const; + inline void resize(const unsigned int n); + inline void resize(const T& a, const unsigned int n); + + Vector& operator=(const Vector& rhs); //assignment + Vector& operator=(const T& a); //assign a to every element + inline Vector& operator+=(const Vector& rhs); + inline Vector& operator-=(const Vector& rhs); + inline Vector& operator*=(const Vector& rhs); + inline Vector& operator/=(const Vector& rhs); + inline Vector& operator^=(const Vector& rhs); + inline Vector& operator+=(const T& a); + inline Vector& operator-=(const T& a); + inline Vector& operator*=(const T& a); + inline Vector& operator/=(const T& a); + inline Vector& operator^=(const T& a); +private: + unsigned int n; // size of array. upper index is n-1 + T* v; // storage for data +}; + +template +Vector::Vector() + : n(0), v(0) +{} + +template +Vector::Vector(const unsigned int n) + : v(new T[n]) +{ + this->n = n; +} + +template +Vector::Vector(const T& a, const unsigned int n) + : v(new T[n]) +{ + this->n = n; + for (unsigned int i = 0; i < n; i++) + v[i] = a; +} + +template +Vector::Vector(const T* a, const unsigned int n) + : v(new T[n]) +{ + this->n = n; + for (unsigned int i = 0; i < n; i++) + v[i] = *a++; +} + +template +Vector::Vector(const Vector& rhs) + : v(new T[rhs.n]) +{ + this->n = rhs.n; + for (unsigned int i = 0; i < n; i++) + v[i] = rhs[i]; +} + +template +Vector::~Vector() +{ + if (v != 0) + delete[] (v); +} + +template +void Vector::resize(const unsigned int n) +{ + if (n == this->n) + return; + if (v != 0) + delete[] (v); + v = new T[n]; + this->n = n; +} + +template +void Vector::resize(const T& a, const unsigned int n) +{ + resize(n); + for (unsigned int i = 0; i < n; i++) + v[i] = a; +} + + +template +inline Vector& Vector::operator=(const Vector& rhs) +// postcondition: normal assignment via copying has been performed; +// if vector and rhs were different sizes, vector +// has been resized to match the size of rhs +{ + if (this != &rhs) + { + resize(rhs.n); + for (unsigned int i = 0; i < n; i++) + v[i] = rhs[i]; + } + return *this; +} + +template +inline Vector & Vector::operator=(const T& a) //assign a to every element +{ + for (unsigned int i = 0; i < n; i++) + v[i] = a; + return *this; +} + +template +inline T & Vector::operator[](const unsigned int& i) //subscripting +{ + return v[i]; +} + +template +inline const T& Vector::operator[](const unsigned int& i) const //subscripting +{ + return v[i]; +} + +template +inline unsigned int Vector::size() const +{ + return n; +} + +template +inline void Vector::set(const T* a, unsigned int n) +{ + resize(n); + for (unsigned int i = 0; i < n; i++) + v[i] = a[i]; +} + +template +inline Vector Vector::extract(const std::set& indexes) const +{ + Vector tmp(indexes.size()); + unsigned int i = 0; + + for (std::set::const_iterator el = indexes.begin(); el != indexes.end(); el++) + { + if (*el >= n) + throw std::logic_error("Error extracting subvector: the indexes are out of vector bounds"); + tmp[i++] = v[*el]; + } + + return tmp; +} + +template +inline Vector& Vector::operator+=(const Vector& rhs) +{ + if (this->size() != rhs.size()) + throw std::logic_error("Operator+=: vectors have different sizes"); + for (unsigned int i = 0; i < n; i++) + v[i] += rhs[i]; + + return *this; +} + + +template +inline Vector& Vector::operator+=(const T& a) +{ + for (unsigned int i = 0; i < n; i++) + v[i] += a; + + return *this; +} + +template +inline Vector operator+(const Vector& rhs) +{ + return rhs; +} + +template +inline Vector operator+(const Vector& lhs, const Vector& rhs) +{ + if (lhs.size() != rhs.size()) + throw std::logic_error("Operator+: vectors have different sizes"); + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) + tmp[i] = lhs[i] + rhs[i]; + + return tmp; +} + +template +inline Vector operator+(const Vector& lhs, const T& a) +{ + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) + tmp[i] = lhs[i] + a; + + return tmp; +} + +template +inline Vector operator+(const T& a, const Vector& rhs) +{ + Vector tmp(rhs.size()); + for (unsigned int i = 0; i < rhs.size(); i++) + tmp[i] = a + rhs[i]; + + return tmp; +} + +template +inline Vector& Vector::operator-=(const Vector& rhs) +{ + if (this->size() != rhs.size()) + throw std::logic_error("Operator-=: vectors have different sizes"); + for (unsigned int i = 0; i < n; i++) + v[i] -= rhs[i]; + + return *this; +} + + +template +inline Vector& Vector::operator-=(const T& a) +{ + for (unsigned int i = 0; i < n; i++) + v[i] -= a; + + return *this; +} + +template +inline Vector operator-(const Vector& rhs) +{ + return (T)(-1) * rhs; +} + +template +inline Vector operator-(const Vector& lhs, const Vector& rhs) +{ + if (lhs.size() != rhs.size()) + throw std::logic_error("Operator-: vectors have different sizes"); + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) + tmp[i] = lhs[i] - rhs[i]; + + return tmp; +} + +template +inline Vector operator-(const Vector& lhs, const T& a) +{ + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) + tmp[i] = lhs[i] - a; + + return tmp; +} + +template +inline Vector operator-(const T& a, const Vector& rhs) +{ + Vector tmp(rhs.size()); + for (unsigned int i = 0; i < rhs.size(); i++) + tmp[i] = a - rhs[i]; + + return tmp; +} + +template +inline Vector& Vector::operator*=(const Vector& rhs) +{ + if (this->size() != rhs.size()) + throw std::logic_error("Operator*=: vectors have different sizes"); + for (unsigned int i = 0; i < n; i++) + v[i] *= rhs[i]; + + return *this; +} + + +template +inline Vector& Vector::operator*=(const T& a) +{ + for (unsigned int i = 0; i < n; i++) + v[i] *= a; + + return *this; +} + +template +inline Vector operator*(const Vector& lhs, const Vector& rhs) +{ + if (lhs.size() != rhs.size()) + throw std::logic_error("Operator*: vectors have different sizes"); + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) + tmp[i] = lhs[i] * rhs[i]; + + return tmp; +} + +template +inline Vector operator*(const Vector& lhs, const T& a) +{ + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) + tmp[i] = lhs[i] * a; + + return tmp; +} + +template +inline Vector operator*(const T& a, const Vector& rhs) +{ + Vector tmp(rhs.size()); + for (unsigned int i = 0; i < rhs.size(); i++) + tmp[i] = a * rhs[i]; + + return tmp; +} + +template +inline Vector& Vector::operator/=(const Vector& rhs) +{ + if (this->size() != rhs.size()) + throw std::logic_error("Operator/=: vectors have different sizes"); + for (unsigned int i = 0; i < n; i++) + v[i] /= rhs[i]; + + return *this; +} + + +template +inline Vector& Vector::operator/=(const T& a) +{ + for (unsigned int i = 0; i < n; i++) + v[i] /= a; + + return *this; +} + +template +inline Vector operator/(const Vector& lhs, const Vector& rhs) +{ + if (lhs.size() != rhs.size()) + throw std::logic_error("Operator/: vectors have different sizes"); + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) + tmp[i] = lhs[i] / rhs[i]; + + return tmp; +} + +template +inline Vector operator/(const Vector& lhs, const T& a) +{ + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) + tmp[i] = lhs[i] / a; + + return tmp; +} + +template +inline Vector operator/(const T& a, const Vector& rhs) +{ + Vector tmp(rhs.size()); + for (unsigned int i = 0; i < rhs.size(); i++) + tmp[i] = a / rhs[i]; + + return tmp; +} + +template +inline Vector operator^(const Vector& lhs, const Vector& rhs) +{ + if (lhs.size() != rhs.size()) + throw std::logic_error("Operator^: vectors have different sizes"); + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) + tmp[i] = pow(lhs[i], rhs[i]); + + return tmp; +} + +template +inline Vector operator^(const Vector& lhs, const T& a) +{ + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) + tmp[i] = pow(lhs[i], a); + + return tmp; +} + +template +inline Vector operator^(const T& a, const Vector& rhs) +{ + Vector tmp(rhs.size()); + for (unsigned int i = 0; i < rhs.size(); i++) + tmp[i] = pow(a, rhs[i]); + + return tmp; +} + +template +inline Vector& Vector::operator^=(const Vector& rhs) +{ + if (this->size() != rhs.size()) + throw std::logic_error("Operator^=: vectors have different sizes"); + for (unsigned int i = 0; i < n; i++) + v[i] = pow(v[i], rhs[i]); + + return *this; +} + +template +inline Vector& Vector::operator^=(const T& a) +{ + for (unsigned int i = 0; i < n; i++) + v[i] = pow(v[i], a); + + return *this; +} + +template +inline bool operator==(const Vector& v, const Vector& w) +{ + if (v.size() != w.size()) + throw std::logic_error("Vectors of different size are not confrontable"); + for (unsigned i = 0; i < v.size(); i++) + if (v[i] != w[i]) + return false; + return true; +} + +template +inline bool operator!=(const Vector& v, const Vector& w) +{ + if (v.size() != w.size()) + throw std::logic_error("Vectors of different size are not confrontable"); + for (unsigned i = 0; i < v.size(); i++) + if (v[i] != w[i]) + return true; + return false; +} + +template +inline bool operator<(const Vector& v, const Vector& w) +{ + if (v.size() != w.size()) + throw std::logic_error("Vectors of different size are not confrontable"); + for (unsigned i = 0; i < v.size(); i++) + if (v[i] >= w[i]) + return false; + return true; +} + +template +inline bool operator<=(const Vector& v, const Vector& w) +{ + if (v.size() != w.size()) + throw std::logic_error("Vectors of different size are not confrontable"); + for (unsigned i = 0; i < v.size(); i++) + if (v[i] > w[i]) + return false; + return true; +} + +template +inline bool operator>(const Vector& v, const Vector& w) +{ + if (v.size() != w.size()) + throw std::logic_error("Vectors of different size are not confrontable"); + for (unsigned i = 0; i < v.size(); i++) + if (v[i] <= w[i]) + return false; + return true; +} + +template +inline bool operator>=(const Vector& v, const Vector& w) +{ + if (v.size() != w.size()) + throw std::logic_error("Vectors of different size are not confrontable"); + for (unsigned i = 0; i < v.size(); i++) + if (v[i] < w[i]) + return false; + return true; +} + +/** + Input/Output +*/ +template +inline std::ostream& operator<<(std::ostream& os, const Vector& v) +{ + os << std::endl << v.size() << std::endl; + for (unsigned int i = 0; i < v.size() - 1; i++) + os << std::setw(20) << std::setprecision(16) << v[i] << ", "; + os << std::setw(20) << std::setprecision(16) << v[v.size() - 1] << std::endl; + + return os; +} + +template +std::istream& operator>>(std::istream& is, Vector& v) +{ + int elements; + char comma; + is >> elements; + v.resize(elements); + for (unsigned int i = 0; i < elements; i++) + is >> v[i] >> comma; + + return is; +} + +/** + Index utilities +*/ + +std::set seq(unsigned int s, unsigned int e); + +std::set singleton(unsigned int i); + +template +class CanonicalBaseVector : public Vector +{ +public: + CanonicalBaseVector(unsigned int i, unsigned int n); + inline void reset(unsigned int i); +private: + unsigned int e; +}; + +template +CanonicalBaseVector::CanonicalBaseVector(unsigned int i, unsigned int n) + : Vector((T)0, n), e(i) +{ (*this)[e] = (T)1; } + +template +inline void CanonicalBaseVector::reset(unsigned int i) +{ + (*this)[e] = (T)0; + e = i; + (*this)[e] = (T)1; +} + +#include + +template +inline T sum(const Vector& v) +{ + T tmp = (T)0; + for (unsigned int i = 0; i < v.size(); i++) + tmp += v[i]; + + return tmp; +} + +template +inline T prod(const Vector& v) +{ + T tmp = (T)1; + for (unsigned int i = 0; i < v.size(); i++) + tmp *= v[i]; + + return tmp; +} + +template +inline T mean(const Vector& v) +{ + T sum = (T)0; + for (unsigned int i = 0; i < v.size(); i++) + sum += v[i]; + return sum / v.size(); +} + +template +inline T median(const Vector& v) +{ + Vector tmp = sort(v); + if (v.size() % 2 == 1) // it is an odd-sized vector + return tmp[v.size() / 2]; + else + return 0.5 * (tmp[v.size() / 2 - 1] + tmp[v.size() / 2]); +} + +template +inline T stdev(const Vector& v, bool sample_correction = false) +{ + return sqrt(var(v, sample_correction)); +} + +template +inline T var(const Vector& v, bool sample_correction = false) +{ + T sum = (T)0, ssum = (T)0; + unsigned int n = v.size(); + for (unsigned int i = 0; i < n; i++) + { + sum += v[i]; + ssum += (v[i] * v[i]); + } + if (!sample_correction) + return (ssum / n) - (sum / n) * (sum / n); + else + return n * ((ssum / n) - (sum / n) * (sum / n)) / (n - 1); +} + +template +inline T max(const Vector& v) +{ + T value = v[0]; + for (unsigned int i = 1; i < v.size(); i++) + value = std::max(v[i], value); + + return value; +} + +template +inline T min(const Vector& v) +{ + T value = v[0]; + for (unsigned int i = 1; i < v.size(); i++) + value = std::min(v[i], value); + + return value; +} + +template +inline unsigned int index_max(const Vector& v) +{ + unsigned int max = 0; + for (unsigned int i = 1; i < v.size(); i++) + if (v[i] > v[max]) + max = i; + + return max; +} + +template +inline unsigned int index_min(const Vector& v) +{ + unsigned int min = 0; + for (unsigned int i = 1; i < v.size(); i++) + if (v[i] < v[min]) + min = i; + + return min; +} + + +template +inline T dot_prod(const Vector& a, const Vector& b) +{ + T sum = (T)0; + if (a.size() != b.size()) + throw std::logic_error("Dotprod error: the vectors are not the same size"); + for (unsigned int i = 0; i < a.size(); i++) + sum += a[i] * b[i]; + + return sum; +} + +/** + Single element mathematical functions +*/ + +template +inline Vector exp(const Vector& v) +{ + Vector tmp(v.size()); + for (unsigned int i = 0; i < v.size(); i++) + tmp[i] = exp(v[i]); + + return tmp; +} + +template +inline Vector log(const Vector& v) +{ + Vector tmp(v.size()); + for (unsigned int i = 0; i < v.size(); i++) + tmp[i] = log(v[i]); + + return tmp; +} + +template +inline Vector vec_sqrt(const Vector& v) +{ + Vector tmp(v.size()); + for (unsigned int i = 0; i < v.size(); i++) + tmp[i] = sqrt(v[i]); + + return tmp; +} + +template +inline Vector pow(const Vector& v, double a) +{ + Vector tmp(v.size()); + for (unsigned int i = 0; i < v.size(); i++) + tmp[i] = pow(v[i], a); + + return tmp; +} + +template +inline Vector abs(const Vector& v) +{ + Vector tmp(v.size()); + for (unsigned int i = 0; i < v.size(); i++) + tmp[i] = (T)fabs(v[i]); + + return tmp; +} + +template +inline Vector sign(const Vector& v) +{ + Vector tmp(v.size()); + for (unsigned int i = 0; i < v.size(); i++) + tmp[i] = v[i] > 0 ? +1 : v[i] == 0 ? 0 : -1; + + return tmp; +} + +template +inline unsigned int partition(Vector& v, unsigned int begin, unsigned int end) +{ + unsigned int i = begin + 1, j = begin + 1; + T pivot = v[begin]; + while (j <= end) + { + if (v[j] < pivot) { + std::swap(v[i], v[j]); + i++; + } + j++; + } + v[begin] = v[i - 1]; + v[i - 1] = pivot; + return i - 2; +} + + +template +inline void quicksort(Vector& v, unsigned int begin, unsigned int end) +{ + if (end > begin) + { + unsigned int index = partition(v, begin, end); + quicksort(v, begin, index); + quicksort(v, index + 2, end); + } +} + +template +inline Vector sort(const Vector& v) +{ + Vector tmp(v); + + quicksort(tmp, 0, tmp.size() - 1); + + return tmp; +} + +template +inline Vector rank(const Vector& v) +{ + Vector tmp(v); + Vector tmp_rank(0.0, v.size()); + + for (unsigned int i = 0; i < tmp.size(); i++) + { + unsigned int smaller = 0, equal = 0; + for (unsigned int j = 0; j < tmp.size(); j++) + if (i == j) + continue; + else + if (tmp[j] < tmp[i]) + smaller++; + else if (tmp[j] == tmp[i]) + equal++; + tmp_rank[i] = smaller + 1; + if (equal > 0) + { + for (unsigned int j = 1; j <= equal; j++) + tmp_rank[i] += smaller + 1 + j; + tmp_rank[i] /= (double)(equal + 1); + } + } + + return tmp_rank; +} + +//enum MType { DIAG }; + +template +class Matrix +{ +public: + Matrix(); // Default constructor + Matrix(const unsigned int n, const unsigned int m); // Construct a n x m matrix + Matrix(const T& a, const unsigned int n, const unsigned int m); // Initialize the content to constant a + Matrix(MType t, const T& a, const T& o, const unsigned int n, const unsigned int m); + Matrix(MType t, const Vector& v, const T& o, const unsigned int n, const unsigned int m); + Matrix(const T* a, const unsigned int n, const unsigned int m); // Initialize to array + Matrix(const Matrix& rhs); // Copy constructor + ~Matrix(); // destructor + + inline T* operator[](const unsigned int& i) { return v[i]; } // Subscripting: row i + inline const T* operator[](const unsigned int& i) const { return v[i]; }; // const subsctipting + + inline void resize(const unsigned int n, const unsigned int m); + inline void resize(const T& a, const unsigned int n, const unsigned int m); + + + inline Vector extractRow(const unsigned int i) const; + inline Vector extractColumn(const unsigned int j) const; + inline Vector extractDiag() const; + inline Matrix extractRows(const std::set& indexes) const; + inline Matrix extractColumns(const std::set& indexes) const; + inline Matrix extract(const std::set& r_indexes, const std::set& c_indexes) const; + + inline void set(const T* a, unsigned int n, unsigned int m); + inline void set(const std::set& r_indexes, const std::set& c_indexes, const Matrix& m); + inline void setRow(const unsigned int index, const Vector& v); + inline void setRow(const unsigned int index, const Matrix& v); + inline void setRows(const std::set& indexes, const Matrix& m); + inline void setColumn(const unsigned int index, const Vector& v); + inline void setColumn(const unsigned int index, const Matrix& v); + inline void setColumns(const std::set& indexes, const Matrix& m); + + + inline unsigned int nrows() const { return n; } // number of rows + inline unsigned int ncols() const { return m; } // number of columns + + inline Matrix& operator=(const Matrix& rhs); // Assignment operator + inline Matrix& operator=(const T& a); // Assign to every element value a + inline Matrix& operator+=(const Matrix& rhs); + inline Matrix& operator-=(const Matrix& rhs); + inline Matrix& operator*=(const Matrix& rhs); + inline Matrix& operator/=(const Matrix& rhs); + inline Matrix& operator^=(const Matrix& rhs); + inline Matrix& operator+=(const T& a); + inline Matrix& operator-=(const T& a); + inline Matrix& operator*=(const T& a); + inline Matrix& operator/=(const T& a); + inline Matrix& operator^=(const T& a); + inline operator Vector(); +private: + unsigned int n; // number of rows + unsigned int m; // number of columns + T **v; // storage for data +}; + +template +Matrix::Matrix() + : n(0), m(0), v(0) +{} + +template +Matrix::Matrix(unsigned int n, unsigned int m) + : v(new T*[n]) +{ + this->n = n; this->m = m; + v[0] = new T[m * n]; + for (unsigned int i = 1; i < n; i++) + v[i] = v[i - 1] + m; +} + +template +Matrix::Matrix(const T& a, unsigned int n, unsigned int m) + : v(new T*[n]) +{ + this->n = n; this->m = m; + v[0] = new T[m * n]; + for (unsigned int i = 1; i < n; i++) + v[i] = v[i - 1] + m; + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] = a; +} + +template +Matrix::Matrix(const T* a, unsigned int n, unsigned int m) + : v(new T*[n]) +{ + this->n = n; this->m = m; + v[0] = new T[m * n]; + for (unsigned int i = 1; i < n; i++) + v[i] = v[i - 1] + m; + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] = *a++; +} + +template +Matrix::Matrix(MType t, const T& a, const T& o, unsigned int n, unsigned int m) + : v(new T*[n]) +{ + this->n = n; this->m = m; + v[0] = new T[m * n]; + for (unsigned int i = 1; i < n; i++) + v[i] = v[i - 1] + m; + switch (t) + { + case DIAG: + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + if (i != j) + v[i][j] = o; + else + v[i][j] = a; + break; + default: + throw std::logic_error("Matrix type not supported"); + } +} + +template +Matrix::Matrix(MType t, const Vector& a, const T& o, unsigned int n, unsigned int m) + : v(new T*[n]) +{ + this->n = n; this->m = m; + v[0] = new T[m * n]; + for (unsigned int i = 1; i < n; i++) + v[i] = v[i - 1] + m; + switch (t) + { + case DIAG: + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + if (i != j) + v[i][j] = o; + else + v[i][j] = a[i]; + break; + default: + throw std::logic_error("Matrix type not supported"); + } +} + +template +Matrix::Matrix(const Matrix& rhs) + : v(new T*[rhs.n]) +{ + n = rhs.n; m = rhs.m; + v[0] = new T[m * n]; + for (unsigned int i = 1; i < n; i++) + v[i] = v[i - 1] + m; + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] = rhs[i][j]; +} + +template +Matrix::~Matrix() +{ + if (v != 0) { + delete[] (v[0]); + delete[] (v); + } +} + +template +inline Matrix& Matrix::operator=(const Matrix &rhs) +// postcondition: normal assignment via copying has been performed; +// if matrix and rhs were different sizes, matrix +// has been resized to match the size of rhs +{ + if (this != &rhs) + { + resize(rhs.n, rhs.m); + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] = rhs[i][j]; + } + return *this; +} + +template +inline Matrix& Matrix::operator=(const T& a) // assign a to every element +{ + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] = a; + return *this; +} + + +template +inline void Matrix::resize(const unsigned int n, const unsigned int m) +{ + if (n == this->n && m == this->m) + return; + if (v != 0) + { + delete[] (v[0]); + delete[] (v); + } + this->n = n; this->m = m; + v = new T*[n]; + v[0] = new T[m * n]; + for (unsigned int i = 1; i < n; i++) + v[i] = v[i - 1] + m; +} + +template +inline void Matrix::resize(const T& a, const unsigned int n, const unsigned int m) +{ + resize(n, m); + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] = a; +} + + + +template +inline Vector Matrix::extractRow(const unsigned int i) const +{ + if (i >= n) + throw std::logic_error("Error in extractRow: trying to extract a row out of matrix bounds"); + Vector tmp(v[i], m); + + return tmp; +} + +template +inline Vector Matrix::extractColumn(const unsigned int j) const +{ + if (j >= m) + throw std::logic_error("Error in extractRow: trying to extract a row out of matrix bounds"); + Vector tmp(n); + + for (unsigned int i = 0; i < n; i++) + tmp[i] = v[i][j]; + + return tmp; +} + +template +inline Vector Matrix::extractDiag() const +{ + unsigned int d = std::min(n, m); + + Vector tmp(d); + + for (unsigned int i = 0; i < d; i++) + tmp[i] = v[i][i]; + + return tmp; + +} + +template +inline Matrix Matrix::extractRows(const std::set& indexes) const +{ + Matrix tmp(indexes.size(), m); + unsigned int i = 0; + + for (std::set::const_iterator el = indexes.begin(); el != indexes.end(); el++) + { + for (unsigned int j = 0; j < m; j++) + { + if (*el >= n) + throw std::logic_error("Error extracting rows: the indexes are out of matrix bounds"); + tmp[i][j] = v[*el][j]; + } + i++; + } + + return tmp; +} + +template +inline Matrix Matrix::extractColumns(const std::set& indexes) const +{ + Matrix tmp(n, indexes.size()); + unsigned int j = 0; + + for (std::set::const_iterator el = indexes.begin(); el != indexes.end(); el++) + { + for (unsigned int i = 0; i < n; i++) + { + if (*el >= m) + throw std::logic_error("Error extracting columns: the indexes are out of matrix bounds"); + tmp[i][j] = v[i][*el]; + } + j++; + } + + return tmp; +} + +template +inline Matrix Matrix::extract(const std::set& r_indexes, const std::set& c_indexes) const +{ + Matrix tmp(r_indexes.size(), c_indexes.size()); + unsigned int i = 0, j; + + for (std::set::const_iterator r_el = r_indexes.begin(); r_el != r_indexes.end(); r_el++) + { + if (*r_el >= n) + throw std::logic_error("Error extracting submatrix: the indexes are out of matrix bounds"); + j = 0; + for (std::set::const_iterator c_el = c_indexes.begin(); c_el != c_indexes.end(); c_el++) + { + if (*c_el >= m) + throw std::logic_error("Error extracting rows: the indexes are out of matrix bounds"); + tmp[i][j] = v[*r_el][*c_el]; + j++; + } + i++; + } + + return tmp; +} + +template +inline void Matrix::setRow(unsigned int i, const Vector& a) +{ + if (i >= n) + throw std::logic_error("Error in setRow: trying to set a row out of matrix bounds"); + if (this->m != a.size()) + throw std::logic_error("Error setting matrix row: ranges are not compatible"); + for (unsigned int j = 0; j < ncols(); j++) + v[i][j] = a[j]; +} + +template +inline void Matrix::setRow(unsigned int i, const Matrix& a) +{ + if (i >= n) + throw std::logic_error("Error in setRow: trying to set a row out of matrix bounds"); + if (this->m != a.ncols()) + throw std::logic_error("Error setting matrix column: ranges are not compatible"); + if (a.nrows() != 1) + throw std::logic_error("Error setting matrix column with a non-row matrix"); + for (unsigned int j = 0; j < ncols(); j++) + v[i][j] = a[0][j]; +} + +template +inline void Matrix::setRows(const std::set& indexes, const Matrix& m) +{ + unsigned int i = 0; + + if (indexes.size() != m.nrows() || this->m != m.ncols()) + throw std::logic_error("Error setting matrix rows: ranges are not compatible"); + for (std::set::const_iterator el = indexes.begin(); el != indexes.end(); el++) + { + for (unsigned int j = 0; j < ncols(); j++) + { + if (*el >= n) + throw std::logic_error("Error in setRows: trying to set a row out of matrix bounds"); + v[*el][j] = m[i][j]; + } + i++; + } +} + +template +inline void Matrix::setColumn(unsigned int j, const Vector& a) +{ + if (j >= m) + throw std::logic_error("Error in setColumn: trying to set a column out of matrix bounds"); + if (this->n != a.size()) + throw std::logic_error("Error setting matrix column: ranges are not compatible"); + for (unsigned int i = 0; i < nrows(); i++) + v[i][j] = a[i]; +} + +template +inline void Matrix::setColumn(unsigned int j, const Matrix& a) +{ + if (j >= m) + throw std::logic_error("Error in setColumn: trying to set a column out of matrix bounds"); + if (this->n != a.nrows()) + throw std::logic_error("Error setting matrix column: ranges are not compatible"); + if (a.ncols() != 1) + throw std::logic_error("Error setting matrix column with a non-column matrix"); + for (unsigned int i = 0; i < nrows(); i++) + v[i][j] = a[i][0]; +} + + +template +inline void Matrix::setColumns(const std::set& indexes, const Matrix& a) +{ + unsigned int j = 0; + + if (indexes.size() != a.ncols() || this->n != a.nrows()) + throw std::logic_error("Error setting matrix columns: ranges are not compatible"); + for (std::set::const_iterator el = indexes.begin(); el != indexes.end(); el++) + { + for (unsigned int i = 0; i < nrows(); i++) + { + if (*el >= m) + throw std::logic_error("Error in setColumns: trying to set a column out of matrix bounds"); + v[i][*el] = a[i][j]; + } + j++; + } +} + +template +inline void Matrix::set(const std::set& r_indexes, const std::set& c_indexes, const Matrix& a) +{ + unsigned int i = 0, j; + if (c_indexes.size() != a.ncols() || r_indexes.size() != a.nrows()) + throw std::logic_error("Error setting matrix elements: ranges are not compatible"); + + for (std::set::const_iterator r_el = r_indexes.begin(); r_el != r_indexes.end(); r_el++) + { + if (*r_el >= n) + throw std::logic_error("Error in set: trying to set a row out of matrix bounds"); + j = 0; + for (std::set::const_iterator c_el = c_indexes.begin(); c_el != c_indexes.end(); c_el++) + { + if (*c_el >= m) + throw std::logic_error("Error in set: trying to set a column out of matrix bounds"); + v[*r_el][*c_el] = a[i][j]; + j++; + } + i++; + } +} + +template +inline void Matrix::set(const T* a, unsigned int n, unsigned int m) +{ + if (this->n != n || this->m != m) + resize(n, m); + unsigned int k = 0; + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] = a[k++]; +} + + +template +Matrix operator+(const Matrix& rhs) +{ + return rhs; +} + +template +Matrix operator+(const Matrix& lhs, const Matrix& rhs) +{ + if (lhs.ncols() != rhs.ncols() || lhs.nrows() != rhs.nrows()) + throw std::logic_error("Operator+: matrices have different sizes"); + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) + tmp[i][j] = lhs[i][j] + rhs[i][j]; + + return tmp; +} + +template +Matrix operator+(const Matrix& lhs, const T& a) +{ + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) + tmp[i][j] = lhs[i][j] + a; + + return tmp; +} + +template +Matrix operator+(const T& a, const Matrix& rhs) +{ + Matrix tmp(rhs.nrows(), rhs.ncols()); + for (unsigned int i = 0; i < rhs.nrows(); i++) + for (unsigned int j = 0; j < rhs.ncols(); j++) + tmp[i][j] = a + rhs[i][j]; + + return tmp; +} + +template +inline Matrix& Matrix::operator+=(const Matrix& rhs) +{ + if (m != rhs.ncols() || n != rhs.nrows()) + throw std::logic_error("Operator+=: matrices have different sizes"); + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] += rhs[i][j]; + + return *this; +} + +template +inline Matrix& Matrix::operator+=(const T& a) +{ + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] += a; + + return *this; +} + +template +Matrix operator-(const Matrix& rhs) +{ + return (T)(-1) * rhs; +} + +template +Matrix operator-(const Matrix& lhs, const Matrix& rhs) +{ + if (lhs.ncols() != rhs.ncols() || lhs.nrows() != rhs.nrows()) + throw std::logic_error("Operator-: matrices have different sizes"); + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) + tmp[i][j] = lhs[i][j] - rhs[i][j]; + + return tmp; +} + +template +Matrix operator-(const Matrix& lhs, const T& a) +{ + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) + tmp[i][j] = lhs[i][j] - a; + + return tmp; +} + +template +Matrix operator-(const T& a, const Matrix& rhs) +{ + Matrix tmp(rhs.nrows(), rhs.ncols()); + for (unsigned int i = 0; i < rhs.nrows(); i++) + for (unsigned int j = 0; j < rhs.ncols(); j++) + tmp[i][j] = a - rhs[i][j]; + + return tmp; +} + +template +inline Matrix& Matrix::operator-=(const Matrix& rhs) +{ + if (m != rhs.ncols() || n != rhs.nrows()) + throw std::logic_error("Operator-=: matrices have different sizes"); + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] -= rhs[i][j]; + + return *this; +} + +template +inline Matrix& Matrix::operator-=(const T& a) +{ + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] -= a; + + return *this; +} + +template +Matrix operator*(const Matrix& lhs, const Matrix& rhs) +{ + if (lhs.ncols() != rhs.ncols() || lhs.nrows() != rhs.nrows()) + throw std::logic_error("Operator*: matrices have different sizes"); + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) + tmp[i][j] = lhs[i][j] * rhs[i][j]; + + return tmp; +} + +template +Matrix operator*(const Matrix& lhs, const T& a) +{ + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) + tmp[i][j] = lhs[i][j] * a; + + return tmp; +} + +template +Matrix operator*(const T& a, const Matrix& rhs) +{ + Matrix tmp(rhs.nrows(), rhs.ncols()); + for (unsigned int i = 0; i < rhs.nrows(); i++) + for (unsigned int j = 0; j < rhs.ncols(); j++) + tmp[i][j] = a * rhs[i][j]; + + return tmp; +} + +template +inline Matrix& Matrix::operator*=(const Matrix& rhs) +{ + if (m != rhs.ncols() || n != rhs.nrows()) + throw std::logic_error("Operator*=: matrices have different sizes"); + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] *= rhs[i][j]; + + return *this; +} + +template +inline Matrix& Matrix::operator*=(const T& a) +{ + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] *= a; + + return *this; +} + +template +Matrix operator/(const Matrix& lhs, const Matrix& rhs) +{ + if (lhs.ncols() != rhs.ncols() || lhs.nrows() != rhs.nrows()) + throw std::logic_error("Operator+: matrices have different sizes"); + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) + tmp[i][j] = lhs[i][j] / rhs[i][j]; + + return tmp; +} + +template +Matrix operator/(const Matrix& lhs, const T& a) +{ + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) + tmp[i][j] = lhs[i][j] / a; + + return tmp; +} + +template +Matrix operator/(const T& a, const Matrix& rhs) +{ + Matrix tmp(rhs.nrows(), rhs.ncols()); + for (unsigned int i = 0; i < rhs.nrows(); i++) + for (unsigned int j = 0; j < rhs.ncols(); j++) + tmp[i][j] = a / rhs[i][j]; + + return tmp; +} + +template +inline Matrix& Matrix::operator/=(const Matrix& rhs) +{ + if (m != rhs.ncols() || n != rhs.nrows()) + throw std::logic_error("Operator+=: matrices have different sizes"); + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] /= rhs[i][j]; + + return *this; +} + +template +inline Matrix& Matrix::operator/=(const T& a) +{ + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] /= a; + + return *this; +} + +template +Matrix operator^(const Matrix& lhs, const T& a) +{ + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) + tmp[i][j] = pow(lhs[i][j], a); + + return tmp; +} + +template +inline Matrix& Matrix::operator^=(const Matrix& rhs) +{ + if (m != rhs.ncols() || n != rhs.nrows()) + throw std::logic_error("Operator^=: matrices have different sizes"); + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] = pow(v[i][j], rhs[i][j]); + + return *this; +} + + +template +inline Matrix& Matrix::operator^=(const T& a) +{ + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] = pow(v[i][j], a); + + return *this; +} + +template +inline Matrix::operator Vector() +{ + if (n > 1 && m > 1) + throw std::logic_error("Error matrix cast to vector: trying to cast a multi-dimensional matrix"); + if (n == 1) + return extractRow(0); + else + return extractColumn(0); +} + +template +inline bool operator==(const Matrix& a, const Matrix& b) +{ + if (a.nrows() != b.nrows() || a.ncols() != b.ncols()) + throw std::logic_error("Matrices of different size are not confrontable"); + for (unsigned i = 0; i < a.nrows(); i++) + for (unsigned j = 0; j < a.ncols(); j++) + if (a[i][j] != b[i][j]) + return false; + return true; +} + +template +inline bool operator!=(const Matrix& a, const Matrix& b) +{ + if (a.nrows() != b.nrows() || a.ncols() != b.ncols()) + throw std::logic_error("Matrices of different size are not confrontable"); + for (unsigned i = 0; i < a.nrows(); i++) + for (unsigned j = 0; j < a.ncols(); j++) + if (a[i][j] != b[i][j]) + return true; + return false; +} + + + +/** + Input/Output +*/ +template +std::ostream& operator<<(std::ostream& os, const Matrix& m) +{ + os << std::endl << m.nrows() << " " << m.ncols() << std::endl; + for (unsigned int i = 0; i < m.nrows(); i++) + { + for (unsigned int j = 0; j < m.ncols() - 1; j++) + os << std::setw(20) << std::setprecision(16) << m[i][j] << ", "; + os << std::setw(20) << std::setprecision(16) << m[i][m.ncols() - 1] << std::endl; + } + + return os; +} + +template +std::istream& operator>>(std::istream& is, Matrix& m) +{ + int rows, cols; + char comma; + is >> rows >> cols; + m.resize(rows, cols); + for (unsigned int i = 0; i < rows; i++) + for (unsigned int j = 0; j < cols; j++) + is >> m[i][j] >> comma; + + return is; +} + +template +T sign(const T& v) +{ + if (v >= (T)0.0) + return (T)1.0; + else + return (T)-1.0; +} + +template +T dist(const T& a, const T& b) +{ + T abs_a = (T)fabs(a), abs_b = (T)fabs(b); + if (abs_a > abs_b) + return abs_a * sqrt((T)1.0 + (abs_b / abs_a) * (abs_b / abs_a)); + else + return (abs_b == (T)0.0 ? (T)0.0 : abs_b * sqrt((T)1.0 + (abs_a / abs_b) * (abs_a / abs_b))); +} + +template +void svd(const Matrix& A, Matrix& U, Vector& W, Matrix& V) +{ + int m = A.nrows(), n = A.ncols(), i, j, k, l, jj, nm; + const unsigned int max_its = 30; + bool flag; + Vector rv1(n); + U = A; + W.resize(n); + V.resize(n, n); + T anorm, c, f, g, h, s, scale, x, y, z; + g = scale = anorm = (T)0.0; + + // Householder reduction to bidiagonal form + for (i = 0; i < n; i++) + { + l = i + 1; + rv1[i] = scale * g; + g = s = scale = (T)0.0; + if (i < m) + { + for (k = i; k < m; k++) + scale += fabs(U[k][i]); + if (scale != (T)0.0) + { + for (k = i; k < m; k++) + { + U[k][i] /= scale; + s += U[k][i] * U[k][i]; + } + f = U[i][i]; + g = -sign(f) * sqrt(s); + h = f * g - s; + U[i][i] = f - g; + for (j = l; j < n; j++) + { + s = (T)0.0; + for (k = i; k < m; k++) + s += U[k][i] * U[k][j]; + f = s / h; + for (k = i; k < m; k++) + U[k][j] += f * U[k][i]; + } + for (k = i; k < m; k++) + U[k][i] *= scale; + } + } + W[i] = scale * g; + g = s = scale = (T)0.0; + if (i < m && i != n - 1) + { + for (k = l; k < n; k++) + scale += fabs(U[i][k]); + if (scale != (T)0.0) + { + for (k = l; k < n; k++) + { + U[i][k] /= scale; + s += U[i][k] * U[i][k]; + } + f = U[i][l]; + g = -sign(f) * sqrt(s); + h = f * g - s; + U[i][l] = f - g; + for (k = l; k = 0; i--) + { + if (i < n - 1) + { + if (g != (T)0.0) + { + for (j = l; j < n; j++) + V[j][i] = (U[i][j] / U[i][l]) / g; + for (j = l; j < n; j++) + { + s = (T)0.0; + for (k = l; k < n; k++) + s += U[i][k] * V[k][j]; + for (k = l; k < n; k++) + V[k][j] += s * V[k][i]; + } + } + for (j = l; j < n; j++) + V[i][j] = V[j][i] = (T)0.0; + } + V[i][i] = (T)1.0; + g = rv1[i]; + l = i; + } + // Accumulation of left-hand transformations + for (i = std::min(m, n) - 1; i >= 0; i--) + { + l = i + 1; + g = W[i]; + for (j = l; j < n; j++) + U[i][j] = (T)0.0; + if (g != (T)0.0) + { + g = (T)1.0 / g; + for (j = l; j < n; j++) + { + s = (T)0.0; + for (k = l; k < m; k++) + s += U[k][i] * U[k][j]; + f = (s / U[i][i]) * g; + for (k = i; k < m; k++) + U[k][j] += f * U[k][i]; + } + for (j = i; j < m; j++) + U[j][i] *= g; + } + else + for (j = i; j < m; j++) + U[j][i] = (T)0.0; + U[i][i]++; + } + // Diagonalization of the bidiagonal form: loop over singular values, and over allowed iterations. + for (k = n - 1; k >= 0; k--) + { + for (unsigned int its = 0; its < max_its; its++) + { + flag = true; + for (l = k; l >= 0; l--) + { // Test for splitting + nm = l - 1; // Note that rV[0] is always zero + if ((T)(fabs(rv1[l]) + anorm) == anorm) + { + flag = false; + break; + } + if ((T)(fabs(W[nm]) + anorm) == anorm) + break; + } + if (flag) + { + // Cancellation of rv1[l], if l > 0 FIXME: it was l > 1 in NR + c = (T)0.0; + s = (T)1.0; + for (i = l; i <= k; i++) + { + f = s * rv1[i]; + rv1[i] *= c; + if ((T)(fabs(f) + anorm) == anorm) + break; + g = W[i]; + h = dist(f, g); + W[i] = h; + h = (T)1.0 / h; + c = g * h; + s = -f * h; + for (j = 0; j < m; j++) + { + y = U[j][nm]; + z = U[j][i]; + U[j][nm] = y * c + z * s; + U[j][i] = z * c - y * s; + } + } + } + z = W[k]; + if (l == k) + { // Convergence + if (z < (T)0.0) + { // Singular value is made nonnegative + W[k] = -z; + for (j = 0; j < n; j++) + V[j][k] = -V[j][k]; + } + break; + } + if (its == max_its) + throw std::logic_error("Error svd: no convergence in the maximum number of iterations"); + x = W[l]; + nm = k - 1; + y = W[nm]; + g = rv1[nm]; + h = rv1[k]; + f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y); + g = dist(f, (T)1.0); + f = ((x - z) * (x + z) + h * ((y / (f + sign(f)*fabs(g))) - h)) / x; + c = s = (T)1.0; // Next QR transformation + for (j = l; j <= nm; j++) + { + i = j + 1; + g = rv1[i]; + y = W[i]; + h = s * g; + g *= c; + z = dist(f, h); + rv1[j] = z; + c = f / z; + s = h / z; + f = x * c + g * s; + g = g * c - x * s; + h = y * s; + y *= c; + for (jj = 0; jj < n; jj++) + { + x = V[jj][j]; + z = V[jj][i]; + V[jj][j] = x * c + z * s; + V[jj][i] = z * c - x * s; + } + z = dist(f, h); + W[j] = z; + if (z != 0) // Rotation can be arbitrary if z = 0 + { + z = (T)1.0 / z; + c = f * z; + s = h * z; + } + f = c * g + s * y; + x = c * y - s * g; + for (jj = 0; jj < m; jj++) + { + y = U[jj][j]; + z = U[jj][i]; + U[jj][j] = y * c + z * s; + U[jj][i] = z * c - y * s; + } + } + rv1[l] = (T)0.0; + rv1[k] = f; + W[k] = x; + } + } +} + +template +Matrix pinv(const Matrix& A) +{ + Matrix U, V, x, tmp(A.ncols(), A.nrows()); + Vector W; + CanonicalBaseVector e(0, A.nrows()); + svd(A, U, W, V); + for (unsigned int i = 0; i < A.nrows(); i++) + { + e.reset(i); + tmp.setColumn(i, dot_prod(dot_prod(dot_prod(V, Matrix(DIAG, 1.0 / W, 0.0, W.size(), W.size())), t(U)), e)); + } + + return tmp; +} + +template +int lu(const Matrix& A, Matrix& LU, Vector& index) +{ + if (A.ncols() != A.nrows()) + throw std::logic_error("Error in LU decomposition: matrix must be squared"); + int i, p, j, k, n = A.ncols(), ex; + T val, tmp; + Vector d(n); + LU = A; + index.resize(n); + + ex = 1; + for (i = 0; i < n; i++) + { + index[i] = i; + val = (T)0.0; + for (j = 0; j < n; j++) + val = std::max(val, (T)fabs(LU[i][j])); + if (val == (T)0.0) + std::logic_error("Error in LU decomposition: matrix was singular"); + d[i] = val; + } + + for (k = 0; k < n - 1; k++) + { + p = k; + val = fabs(LU[k][k]) / d[k]; + for (i = k + 1; i < n; i++) + { + tmp = fabs(LU[i][k]) / d[i]; + if (tmp > val) + { + val = tmp; + p = i; + } + } + if (val == (T)0.0) + std::logic_error("Error in LU decomposition: matrix was singular"); + if (p > k) + { + ex = -ex; + std::swap(index[k], index[p]); + std::swap(d[k], d[p]); + for (j = 0; j < n; j++) + std::swap(LU[k][j], LU[p][j]); + } + + for (i = k + 1; i < n; i++) + { + LU[i][k] /= LU[k][k]; + for (j = k + 1; j < n; j++) + LU[i][j] -= LU[i][k] * LU[k][j]; + } + } + if (LU[n - 1][n - 1] == (T)0.0) + std::logic_error("Error in LU decomposition: matrix was singular"); + + return ex; +} + +template +Vector lu_solve(const Matrix& LU, const Vector& b, Vector& index) +{ + if (LU.ncols() != LU.nrows()) + throw std::logic_error("Error in LU solve: LU matrix should be squared"); + unsigned int n = LU.ncols(); + if (b.size() != n) + throw std::logic_error("Error in LU solve: b vector must be of the same dimensions of LU matrix"); + Vector x((T)0.0, n); + int i, j, p; + T sum; + + p = index[0]; + x[0] = b[p]; + + for (i = 1; i < n; i++) + { + sum = (T)0.0; + for (j = 0; j < i; j++) + sum += LU[i][j] * x[j]; + p = index[i]; + x[i] = b[p] - sum; + } + x[n - 1] /= LU[n - 1][n - 1]; + for (i = n - 2; i >= 0; i--) + { + sum = (T)0.0; + for (j = i + 1; j < n; j++) + sum += LU[i][j] * x[j]; + x[i] = (x[i] - sum) / LU[i][i]; + } + return x; +} + +template +void lu_solve(const Matrix& LU, Vector& x, const Vector& b, Vector& index) +{ + x = lu_solve(LU, b, index); +} + +template +Matrix lu_inverse(const Matrix& A) +{ + if (A.ncols() != A.nrows()) + throw std::logic_error("Error in LU invert: matrix must be squared"); + unsigned int n = A.ncols(); + Matrix A1(n, n), LU; + Vector index; + + lu(A, LU, index); + CanonicalBaseVector e(0, n); + for (unsigned i = 0; i < n; i++) + { + e.reset(i); + A1.setColumn(i, lu_solve(LU, e, index)); + } + + return A1; +} + +template +T lu_det(const Matrix& A) +{ + if (A.ncols() != A.nrows()) + throw std::logic_error("Error in LU determinant: matrix must be squared"); + unsigned int d; + Matrix LU; + Vector index; + + d = lu(A, LU, index); + + return d * prod(LU.extractDiag()); +} + +template +void cholesky(const Matrix A, Matrix& LL) +{ + if (A.ncols() != A.nrows()) + throw std::logic_error("Error in Cholesky decomposition: matrix must be squared"); + int n = A.ncols(); + double sum; + LL = A; + + for (unsigned int i = 0; i < n; i++) + { + for (unsigned int j = i; j < n; j++) + { + sum = LL[i][j]; + for (int k = i - 1; k >= 0; k--) + sum -= LL[i][k] * LL[j][k]; + if (i == j) + { + if (sum <= 0.0) + throw std::logic_error("Error in Cholesky decomposition: matrix is not postive definite"); + LL[i][i] = sqrt(sum); + } + else + LL[j][i] = sum / LL[i][i]; + } + for (unsigned int k = i + 1; k < n; k++) + LL[i][k] = LL[k][i]; + } +} + +template +Matrix cholesky(const Matrix A) +{ + Matrix LL; + cholesky(A, LL); + + return LL; +} + +template +Vector cholesky_solve(const Matrix& LL, const Vector& b) +{ + if (LL.ncols() != LL.nrows()) + throw std::logic_error("Error in Cholesky solve: matrix must be squared"); + unsigned int n = LL.ncols(); + if (b.size() != n) + throw std::logic_error("Error in Cholesky decomposition: b vector must be of the same dimensions of LU matrix"); + Vector x, y; + + /* Solve L * y = b */ + forward_elimination(LL, y, b); + /* Solve L^T * x = y */ + backward_elimination(LL, x, y); + + return x; +} + +template +void cholesky_solve(const Matrix& LL, Vector& x, const Vector& b) +{ + x = cholesky_solve(LL, b); +} + +template +void forward_elimination(const Matrix& L, Vector& y, const Vector b) +{ + if (L.ncols() != L.nrows()) + throw std::logic_error("Error in Forward elimination: matrix must be squared (lower triangular)"); + if (b.size() != L.nrows()) + throw std::logic_error("Error in Forward elimination: b vector must be of the same dimensions of L matrix"); + unsigned int n = b.size(); + y.resize(n); + + y[0] = b[0] / L[0][0]; + for (unsigned int i = 1; i < n; i++) + { + y[i] = b[i]; + for (unsigned int j = 0; j < i; j++) + y[i] -= L[i][j] * y[j]; + y[i] = y[i] / L[i][i]; + } +} + +template +Vector forward_elimination(const Matrix& L, const Vector b) +{ + Vector y; + forward_elimination(L, y, b); + + return y; +} + +template +void backward_elimination(const Matrix& U, Vector& x, const Vector& y) +{ + if (U.ncols() != U.nrows()) + throw std::logic_error("Error in Backward elimination: matrix must be squared (upper triangular)"); + if (y.size() != U.nrows()) + throw std::logic_error("Error in Backward elimination: b vector must be of the same dimensions of U matrix"); + int n = y.size(); + x.resize(n); + + x[n - 1] = y[n - 1] / U[n - 1][n - 1]; + for (int i = n - 2; i >= 0; i--) + { + x[i] = y[i]; + for (int j = i + 1; j < n; j++) + x[i] -= U[i][j] * x[j]; + x[i] = x[i] / U[i][i]; + } +} + +template +Vector backward_elimination(const Matrix& U, const Vector y) +{ + Vector x; + forward_elimination(U, x, y); + + return x; +} + +/* Setting default linear systems machinery */ + +// #define det lu_det +// #define inverse lu_inverse +// #define solve lu_solve + +/* Random */ + +template +void random(Matrix& m) +{ + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) + m[i][j] = (T)(rand() / double(RAND_MAX)); +} + +/** + Aggregate functions +*/ + +template +Vector sum(const Matrix& m) +{ + Vector tmp((T)0, m.ncols()); + for (unsigned int j = 0; j < m.ncols(); j++) + for (unsigned int i = 0; i < m.nrows(); i++) + tmp[j] += m[i][j]; + return tmp; +} + +template +Vector r_sum(const Matrix& m) +{ + Vector tmp((T)0, m.nrows()); + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) + tmp[i] += m[i][j]; + return tmp; +} + +template +T all_sum(const Matrix& m) +{ + T tmp = (T)0; + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) + tmp += m[i][j]; + return tmp; +} + +template +Vector prod(const Matrix& m) +{ + Vector tmp((T)1, m.ncols()); + for (unsigned int j = 0; j < m.ncols(); j++) + for (unsigned int i = 0; i < m.nrows(); i++) + tmp[j] *= m[i][j]; + return tmp; +} + +template +Vector r_prod(const Matrix& m) +{ + Vector tmp((T)1, m.nrows()); + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) + tmp[i] *= m[i][j]; + return tmp; +} + +template +T all_prod(const Matrix& m) +{ + T tmp = (T)1; + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) + tmp *= m[i][j]; + return tmp; +} + +template +Vector mean(const Matrix& m) +{ + Vector res((T)0, m.ncols()); + for (unsigned int j = 0; j < m.ncols(); j++) + { + for (unsigned int i = 0; i < m.nrows(); i++) + res[j] += m[i][j]; + res[j] /= m.nrows(); + } + + return res; +} + +template +Vector r_mean(const Matrix& m) +{ + Vector res((T)0, m.rows()); + for (unsigned int i = 0; i < m.nrows(); i++) + { + for (unsigned int j = 0; j < m.ncols(); j++) + res[i] += m[i][j]; + res[i] /= m.nrows(); + } + + return res; +} + +template +T all_mean(const Matrix& m) +{ + T tmp = (T)0; + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) + tmp += m[i][j]; + return tmp / (m.nrows() * m.ncols()); +} + +template +Vector var(const Matrix& m, bool sample_correction = false) +{ + Vector res((T)0, m.ncols()); + unsigned int n = m.nrows(); + double sum, ssum; + for (unsigned int j = 0; j < m.ncols(); j++) + { + sum = (T)0.0; ssum = (T)0.0; + for (unsigned int i = 0; i < m.nrows(); i++) + { + sum += m[i][j]; + ssum += (m[i][j] * m[i][j]); + } + if (!sample_correction) + res[j] = (ssum / n) - (sum / n) * (sum / n); + else + res[j] = n * ((ssum / n) - (sum / n) * (sum / n)) / (n - 1); + } + + return res; +} + +template +Vector stdev(const Matrix& m, bool sample_correction = false) +{ + return vec_sqrt(var(m, sample_correction)); +} + +template +Vector r_var(const Matrix& m, bool sample_correction = false) +{ + Vector res((T)0, m.nrows()); + double sum, ssum; + unsigned int n = m.ncols(); + for (unsigned int i = 0; i < m.nrows(); i++) + { + sum = 0.0; ssum = 0.0; + for (unsigned int j = 0; j < m.ncols(); j++) + { + sum += m[i][j]; + ssum += (m[i][j] * m[i][j]); + } + if (!sample_correction) + res[i] = (ssum / n) - (sum / n) * (sum / n); + else + res[i] = n * ((ssum / n) - (sum / n) * (sum / n)) / (n - 1); + } + + return res; +} + +template +Vector r_stdev(const Matrix& m, bool sample_correction = false) +{ + return vec_sqrt(r_var(m, sample_correction)); +} + +template +Vector max(const Matrix& m) +{ + Vector res(m.ncols()); + double value; + for (unsigned int j = 0; j < m.ncols(); j++) + { + value = m[0][j]; + for (unsigned int i = 1; i < m.nrows(); i++) + value = std::max(m[i][j], value); + res[j] = value; + } + + return res; +} + +template +Vector r_max(const Matrix& m) +{ + Vector res(m.nrows()); + double value; + for (unsigned int i = 0; i < m.nrows(); i++) + { + value = m[i][0]; + for (unsigned int j = 1; j < m.ncols(); j++) + value = std::max(m[i][j], value); + res[i] = value; + } + + return res; +} + +template +Vector min(const Matrix& m) +{ + Vector res(m.ncols()); + double value; + for (unsigned int j = 0; j < m.ncols(); j++) + { + value = m[0][j]; + for (unsigned int i = 1; i < m.nrows(); i++) + value = std::min(m[i][j], value); + res[j] = value; + } + + return res; +} + +template +Vector r_min(const Matrix& m) +{ + Vector res(m.nrows()); + double value; + for (unsigned int i = 0; i < m.nrows(); i++) + { + value = m[i][0]; + for (unsigned int j = 1; j < m.ncols(); j++) + value = std::min(m[i][j], value); + res[i] = value; + } + + return res; +} + + + +/** + Single element mathematical functions +*/ + +template +Matrix exp(const Matrix&m) +{ + Matrix tmp(m.nrows(), m.ncols()); + + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) + tmp[i][j] = exp(m[i][j]); + + return tmp; +} + +template +Matrix mat_sqrt(const Matrix&m) +{ + Matrix tmp(m.nrows(), m.ncols()); + + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) + tmp[i][j] = sqrt(m[i][j]); + + return tmp; +} + +/** + Matrix operators +*/ + +template +Matrix kron(const Vector& b, const Vector& a) +{ + Matrix tmp(b.size(), a.size()); + for (unsigned int i = 0; i < b.size(); i++) + for (unsigned int j = 0; j < a.size(); j++) + tmp[i][j] = a[j] * b[i]; + + return tmp; +} + +template +Matrix t(const Matrix& a) +{ + Matrix tmp(a.ncols(), a.nrows()); + for (unsigned int i = 0; i < a.nrows(); i++) + for (unsigned int j = 0; j < a.ncols(); j++) + tmp[j][i] = a[i][j]; + + return tmp; +} + +template +Matrix dot_prod(const Matrix& a, const Matrix& b) +{ + if (a.ncols() != b.nrows()) + throw std::logic_error("Error matrix dot product: dimensions of the matrices are not compatible"); + Matrix tmp(a.nrows(), b.ncols()); + for (unsigned int i = 0; i < tmp.nrows(); i++) + for (unsigned int j = 0; j < tmp.ncols(); j++) + { + tmp[i][j] = (T)0; + for (unsigned int k = 0; k < a.ncols(); k++) + tmp[i][j] += a[i][k] * b[k][j]; + } + + return tmp; +} + +template +Matrix dot_prod(const Matrix& a, const Vector& b) +{ + if (a.ncols() != b.size()) + throw std::logic_error("Error matrix dot product: dimensions of the matrix and the vector are not compatible"); + Matrix tmp(a.nrows(), 1); + for (unsigned int i = 0; i < tmp.nrows(); i++) + { + tmp[i][0] = (T)0; + for (unsigned int k = 0; k < a.ncols(); k++) + tmp[i][0] += a[i][k] * b[k]; + } + + return tmp; +} + +template +Matrix dot_prod(const Vector& a, const Matrix& b) +{ + if (a.size() != b.nrows()) + throw std::logic_error("Error matrix dot product: dimensions of the vector and matrix are not compatible"); + Matrix tmp(1, b.ncols()); + for (unsigned int j = 0; j < tmp.ncols(); j++) + { + tmp[0][j] = (T)0; + for (unsigned int k = 0; k < a.size(); k++) + tmp[0][j] += a[k] * b[k][j]; + } + + return tmp; +} + +template +inline Matrix rank(const Matrix m) +{ + Matrix tmp(m.nrows(), m.ncols()); + for (unsigned int j = 0; j < m.ncols(); j++) + tmp.setColumn(j, rank(m.extractColumn(j))); + + return tmp; +} + +template +inline Matrix r_rank(const Matrix m) +{ + Matrix tmp(m.nrows(), m.ncols()); + for (unsigned int i = 0; i < m.nrows(); i++) + tmp.setRow(i, rank(m.extractRow(i))); + + return tmp; +} + +} // namespace quadprogpp + +#endif // define _ARRAY_HH_ diff --git a/include/unitree_arm_sdk/thirdparty/quadProgpp/QuadProg++.hh b/include/unitree_arm_sdk/thirdparty/quadProgpp/QuadProg++.hh new file mode 100644 index 0000000..c2954d4 --- /dev/null +++ b/include/unitree_arm_sdk/thirdparty/quadProgpp/QuadProg++.hh @@ -0,0 +1,77 @@ +/* + File $Id: QuadProg++.hh 232 2007-06-21 12:29:00Z digasper $ + + The quadprog_solve() function implements the algorithm of Goldfarb and Idnani + for the solution of a (convex) Quadratic Programming problem + by means of an active-set dual method. + +The problem is in the form: + +min 0.5 * x G x + g0 x +s.t. + CE^T x + ce0 = 0 + CI^T x + ci0 >= 0 + + The matrix and vectors dimensions are as follows: + G: n * n + g0: n + + CE: n * p + ce0: p + + CI: n * m + ci0: m + + x: n + + The function will return the cost of the solution written in the x vector or + std::numeric_limits::infinity() if the problem is infeasible. In the latter case + the value of the x vector is not correct. + + References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving + strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33. + + Notes: + 1. pay attention in setting up the vectors ce0 and ci0. + If the constraints of your problem are specified in the form + A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d. + 2. The matrices have column dimension equal to MATRIX_DIM, + a constant set to 20 in this file (by means of a #define macro). + If the matrices are bigger than 20 x 20 the limit could be + increased by means of a -DMATRIX_DIM=n on the compiler command line. + 3. The matrix G is modified within the function since it is used to compute + the G = L^T L cholesky factorization for further computations inside the function. + If you need the original matrix G you should make a copy of it and pass the copy + to the function. + + Author: Luca Di Gaspero + DIEGM - University of Udine, Italy + luca.digaspero@uniud.it + http://www.diegm.uniud.it/digaspero/ + + The author will be grateful if the researchers using this software will + acknowledge the contribution of this function in their research papers. + + Copyright (c) 2007-2016 Luca Di Gaspero + + This software may be modified and distributed under the terms + of the MIT license. See the LICENSE file for details. +*/ + + +#ifndef _QUADPROGPP +#define _QUADPROGPP + +#include "thirdparty/quadProgpp/Array.hh" +#include + +namespace quadprogpp { + +double solve_quadprog(Matrix& G, Vector& g0, + const Matrix& CE, const Vector& ce0, + const Matrix& CI, const Vector& ci0, + Vector& x); + +} // namespace quadprogpp + +#endif // #define _QUADPROGPP diff --git a/include/unitree_arm_sdk/thirdparty/robotics.h b/include/unitree_arm_sdk/thirdparty/robotics.h new file mode 100644 index 0000000..3e96a54 --- /dev/null +++ b/include/unitree_arm_sdk/thirdparty/robotics.h @@ -0,0 +1,535 @@ +#pragma once + +#include +#include "math/mathTools.h" + +namespace robo { +/* + * Function: Find if the value is negligible enough to consider 0 + * Inputs: value to be checked as a double + * Returns: Boolean of true-ignore or false-can't ignore + */ +bool NearZero(const double); + +Mat6 rot(const Mat3& E); + +Mat6 xlt(const Vec3& r); + +/* rotate matrix about x axis */ +RotMat rotX(const double &); +RotMat rx(const double &xrot); + +/* rotate matrix about y axis */ +RotMat rotY(const double &); +RotMat ry(const double &yrot); + +/* rotate matrix about z axis */ +RotMat rotZ(const double &); +RotMat rz(const double &zrot); + +/* row pitch yaw to rotate matrix */ +RotMat rpyToRotMat(const double&, const double&, const double&); +RotMat rpyToRotMat(const Vec3& rpy); + +Vec3 rotMatToRPY(const Mat3& ); + +RotMat quatToRotMat(const Quat&); + +/* convert homogeneous matrix to posture vector */ +Vec6 homoToPosture(HomoMat); + +/* convert posture vector matrix to homogeneous */ +HomoMat postureToHomo(Vec6); + +RotMat getHomoRotMat(HomoMat T); +Vec3 getHomoPosition(HomoMat T); +HomoMat homoMatrix(Vec3 x, Vec3 y, Vec3 p); +/* + * Function: Calculate the 6x6 matrix [adV] of the given 6-vector + * Input: Eigen::VectorXd (6x1) + * Output: Eigen::MatrixXd (6x6) + * Note: Can be used to calculate the Lie bracket [V1, V2] = [adV1]V2 + */ +Eigen::MatrixXd ad(Eigen::VectorXd); + + +/* + * Function: Returns a normalized version of the input vector + * Input: Eigen::MatrixXd + * Output: Eigen::MatrixXd + * Note: MatrixXd is used instead of VectorXd for the case of row vectors + * Requires a copy + * Useful because of the MatrixXd casting + */ +Eigen::MatrixXd Normalize(Eigen::MatrixXd); + + +/* + * Function: Returns the skew symmetric matrix representation of an angular velocity vector + * Input: Eigen::Vector3d 3x1 angular velocity vector + * Returns: Eigen::MatrixXd 3x3 skew symmetric matrix + */ +Eigen::Matrix3d VecToso3(const Eigen::Vector3d&); + + +/* + * Function: Returns angular velocity vector represented by the skew symmetric matrix + * Inputs: Eigen::MatrixXd 3x3 skew symmetric matrix + * Returns: Eigen::Vector3d 3x1 angular velocity + */ +Eigen::Vector3d so3ToVec(const Eigen::MatrixXd&); + + +/* + * Function: Tranlates an exponential rotation into it's individual components + * Inputs: Exponential rotation (rotation matrix in terms of a rotation axis + * and the angle of rotation) + * Returns: The axis and angle of rotation as [x, y, z, theta] + */ +Eigen::Vector4d AxisAng3(const Eigen::Vector3d&); + + +/* + * Function: Translates an exponential rotation into a rotation matrix + * Inputs: exponenential representation of a rotation + * Returns: Rotation matrix + */ +Eigen::Matrix3d MatrixExp3(const Eigen::Matrix3d&); + + +/* Function: Computes the matrix logarithm of a rotation matrix + * Inputs: Rotation matrix + * Returns: matrix logarithm of a rotation + */ +Eigen::Matrix3d MatrixLog3(const Eigen::Matrix3d&); + + +/* + * Function: Combines a rotation matrix and position vector into a single + * Special Euclidian Group (SE3) homogeneous transformation matrix + * Inputs: Rotation Matrix (R), Position Vector (p) + * Returns: Matrix of T = [ [R, p], + * [0, 1] ] + */ +Eigen::MatrixXd RpToTrans(const Eigen::Matrix3d&, const Eigen::Vector3d&); + + +/* + * Function: Separates the rotation matrix and position vector from + * the transfomation matrix representation + * Inputs: Homogeneous transformation matrix + * Returns: std::vector of [rotation matrix, position vector] + */ +std::vector TransToRp(const Eigen::MatrixXd&); + + +/* + * Function: Translates a spatial velocity vector into a transformation matrix + * Inputs: Spatial velocity vector [angular velocity, linear velocity] + * Returns: Transformation matrix + */ +Eigen::MatrixXd VecTose3(const Eigen::VectorXd&); + + +/* Function: Translates a transformation matrix into a spatial velocity vector + * Inputs: Transformation matrix + * Returns: Spatial velocity vector [angular velocity, linear velocity] + */ +Eigen::VectorXd se3ToVec(const Eigen::MatrixXd&); + + +/* + * Function: Provides the adjoint representation of a transformation matrix + * Used to change the frame of reference for spatial velocity vectors + * Inputs: 4x4 Transformation matrix SE(3) + * Returns: 6x6 Adjoint Representation of the matrix + */ +Eigen::MatrixXd Adjoint(const Eigen::MatrixXd&); + + +/* + * Function: Rotation expanded for screw axis + * Inputs: se3 matrix representation of exponential coordinates (transformation matrix) + * Returns: 6x6 Matrix representing the rotation + */ +Eigen::MatrixXd MatrixExp6(const Eigen::MatrixXd&); + + +/* + * Function: Computes the matrix logarithm of a homogeneous transformation matrix + * Inputs: R: Transformation matrix in SE3 + * Returns: The matrix logarithm of R + */ +Eigen::MatrixXd MatrixLog6(const Eigen::MatrixXd&); + + +/* + * Functions: Tranforms 3D motion vector form A to B coordinates + * Input: T: the cordinate transform form A to B coordiantes for a motion vector + * Return : BX_A + */ +Mat6 CoordinateTransMotionVector(const HomoMat& T); + +/* + * Functions: Tranforms 3D force vector form A to B coordinates + * Input: T: the cordinate transform form A to B coordiantes for a force vector + * Return : {BX_A}* + */ +Mat6 CoordinateTransForceVector(const HomoMat& T); + + +/* + * Function: Compute end effector frame (used for current spatial position calculation) + * Inputs: Home configuration (position and orientation) of end-effector + * The joint screw axes in the space frame when the manipulator + * is at the home position + * A list of joint coordinates. + * Returns: Transfomation matrix representing the end-effector frame when the joints are + * at the specified coordinates + * Notes: FK means Forward Kinematics + */ +Eigen::MatrixXd FKinSpace(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::VectorXd&); + +/* + * Function: Compute end effector frame (used for current body position calculation) + * Inputs: Home configuration (position and orientation) of end-effector + * The joint screw axes in the body frame when the manipulator + * is at the home position + * A list of joint coordinates. + * Returns: Transfomation matrix representing the end-effector frame when the joints are + * at the specified coordinates + * Notes: FK means Forward Kinematics + */ +Eigen::MatrixXd FKinBody(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::VectorXd&); + + +/* + * Function: Gives the space Jacobian + * Inputs: Screw axis in home position, joint configuration + * Returns: 6xn Spatial Jacobian + */ +Eigen::MatrixXd JacobianSpace(const Eigen::MatrixXd&, const Eigen::MatrixXd&); + + +/* + * Function: Gives the body Jacobian + * Inputs: Screw axis in BODY position, joint configuration + * Returns: 6xn Bobdy Jacobian + */ +Eigen::MatrixXd JacobianBody(const Eigen::MatrixXd&, const Eigen::MatrixXd&); + + +/* + * Inverts a homogeneous transformation matrix + * Inputs: A homogeneous transformation Matrix T + * Returns: The inverse of T + */ +Eigen::MatrixXd TransInv(const Eigen::MatrixXd&); + +/* + * Inverts a rotation matrix + * Inputs: A rotation matrix R + * Returns: The inverse of R + */ +Eigen::MatrixXd RotInv(const Eigen::MatrixXd&); + +/* + * Takes a parametric description of a screw axis and converts it to a + * normalized screw axis + * Inputs: + * q: A point lying on the screw axis + * s: A unit vector in the direction of the screw axis + * h: The pitch of the screw axis + * Returns: A normalized screw axis described by the inputs + */ +Eigen::VectorXd ScrewToAxis(Eigen::Vector3d q, Eigen::Vector3d s, double h); + + +/* + * Function: Translates a 6-vector of exponential coordinates into screw + * axis-angle form + * Inputs: + * expc6: A 6-vector of exponential coordinates for rigid-body motion + S*theta + * Returns: The corresponding normalized screw axis S; The distance theta traveled + * along/about S in form [S, theta] + * Note: Is it better to return std::map? + */ +Eigen::VectorXd AxisAng6(const Eigen::VectorXd&); + + +/* + * Function: Returns projection of one matrix into SO(3) + * Inputs: + * M: A matrix near SO(3) to project to SO(3) + * Returns: The closest matrix R that is in SO(3) + * Projects a matrix mat to the closest matrix in SO(3) using singular-value decomposition + * (see http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review). + * This function is only appropriate for matrices close to SO(3). + */ +Eigen::MatrixXd ProjectToSO3(const Eigen::MatrixXd&); + + +/* + * Function: Returns projection of one matrix into SE(3) + * Inputs: + * M: A 4x4 matrix near SE(3) to project to SE(3) + * Returns: The closest matrix T that is in SE(3) + * Projects a matrix mat to the closest matrix in SO(3) using singular-value decomposition + * (see http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review). + * This function is only appropriate for matrices close to SE(3). + */ +Eigen::MatrixXd ProjectToSE3(const Eigen::MatrixXd&); + + +/* + * Function: Returns the Frobenius norm to describe the distance of M from the SO(3) manifold + * Inputs: + * M: A 3x3 matrix + * Outputs: + * the distance from mat to the SO(3) manifold using the following + * method: + * If det(M) <= 0, return a large number. + * If det(M) > 0, return norm(M^T*M - I). + */ +double DistanceToSO3(const Eigen::Matrix3d&); + + +/* + * Function: Returns the Frobenius norm to describe the distance of mat from the SE(3) manifold + * Inputs: + * T: A 4x4 matrix + * Outputs: + * the distance from T to the SE(3) manifold using the following + * method: + * Compute the determinant of matR, the top 3x3 submatrix of T. + * If det(matR) <= 0, return a large number. + * If det(matR) > 0, replace the top 3x3 submatrix of mat with matR^T*matR, + * and set the first three entries of the fourth column of mat to zero. Then + * return norm(T - I). + */ +double DistanceToSE3(const Eigen::Matrix4d&); + + +/* + * Function: Returns true if M is close to or on the manifold SO(3) + * Inputs: + * M: A 3x3 matrix + * Outputs: + * true if M is very close to or in SO(3), false otherwise + */ +bool TestIfSO3(const Eigen::Matrix3d&); + + +/* + * Function: Returns true if T is close to or on the manifold SE(3) + * Inputs: + * M: A 4x4 matrix + * Outputs: + * true if T is very close to or in SE(3), false otherwise + */ +bool TestIfSE3(const Eigen::Matrix4d&); + + +/* + * Function: Computes inverse kinematics in the body frame for an open chain robot + * Inputs: + * Blist: The joint screw axes in the end-effector frame when the + * manipulator is at the home position, in the format of a + * matrix with axes as the columns + * M: The home configuration of the end-effector + * T: The desired end-effector configuration Tsd + * thetalist[in][out]: An initial guess and result output of joint angles that are close to + * satisfying Tsd + * emog: A small positive tolerance on the end-effector orientation + * error. The returned joint angles must give an end-effector + * orientation error less than eomg + * ev: A small positive tolerance on the end-effector linear position + * error. The returned joint angles must give an end-effector + * position error less than ev + * Outputs: + * success: A logical value where TRUE means that the function found + * a solution and FALSE means that it ran through the set + * number of maximum iterations without finding a solution + * within the tolerances eomg and ev. + * thetalist[in][out]: Joint angles that achieve T within the specified tolerances, + */ +bool IKinBody(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&, Eigen::VectorXd&, double, double); + + +/* + * Function: Computes inverse kinematics in the space frame for an open chain robot + * Inputs: + * Slist: The joint screw axes in the space frame when the + * manipulator is at the home position, in the format of a + * matrix with axes as the columns + * M: The home configuration of the end-effector + * T: The desired end-effector configuration Tsd + * thetalist[in][out]: An initial guess and result output of joint angles that are close to + * satisfying Tsd + * emog: A small positive tolerance on the end-effector orientation + * error. The returned joint angles must give an end-effector + * orientation error less than eomg + * ev: A small positive tolerance on the end-effector linear position + * error. The returned joint angles must give an end-effector + * position error less than ev + * Outputs: + * success: A logical value where TRUE means that the function found + * a solution and FALSE means that it ran through the set + * number of maximum iterations without finding a solution + * within the tolerances eomg and ev. + * thetalist[in][out]: Joint angles that achieve T within the specified tolerances, + */ +bool IKinSpace(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&, Eigen::VectorXd&, double, double); + +/* + * Function: This function uses forward-backward Newton-Euler iterations to solve the + * equation: + * taulist = Mlist(thetalist) * ddthetalist + c(thetalist, dthetalist) ... + * + g(thetalist) + Jtr(thetalist) * Ftip + * Inputs: + * thetalist: n-vector of joint variables + * dthetalist: n-vector of joint rates + * ddthetalist: n-vector of joint accelerations + * g: Gravity vector g + * Ftip: Spatial force applied by the end-effector expressed in frame {n+1} + * Mlist: List of link frames {i} relative to {i-1} at the home position + * Glist: Spatial inertia matrices Gi of the links + * Slist: Screw axes Si of the joints in a space frame, in the format + * of a matrix with the screw axes as the columns. + * + * Outputs: + * taulist: The n-vector of required joint forces/torques + * + */ +Eigen::VectorXd InverseDynamics(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, + const Eigen::VectorXd&, const Eigen::VectorXd&, const std::vector&, + const std::vector&, const Eigen::MatrixXd&); + +/* + * Function: This function calls InverseDynamics with Ftip = 0, dthetalist = 0, and + * ddthetalist = 0. The purpose is to calculate one important term in the dynamics equation + * Inputs: + * thetalist: n-vector of joint variables + * g: Gravity vector g + * Mlist: List of link frames {i} relative to {i-1} at the home position + * Glist: Spatial inertia matrices Gi of the links + * Slist: Screw axes Si of the joints in a space frame, in the format + * of a matrix with the screw axes as the columns. + * + * Outputs: + * grav: The 3-vector showing the effect force of gravity to the dynamics + * + */ +Eigen::VectorXd GravityForces(const Eigen::VectorXd&, const Eigen::VectorXd&, + const std::vector&, const std::vector&, const Eigen::MatrixXd&); + +/* + * Function: This function calls InverseDynamics n times, each time passing a + * ddthetalist vector with a single element equal to one and all other + * inputs set to zero. Each call of InverseDynamics generates a single + * column, and these columns are assembled to create the inertia matrix. + * + * Inputs: + * thetalist: n-vector of joint variables + * Mlist: List of link frames {i} relative to {i-1} at the home position + * Glist: Spatial inertia matrices Gi of the links + * Slist: Screw axes Si of the joints in a space frame, in the format + * of a matrix with the screw axes as the columns. + * + * Outputs: + * M: The numerical inertia matrix M(thetalist) of an n-joint serial + * chain at the given configuration thetalist. + */ +Eigen::MatrixXd MassMatrix(const Eigen::VectorXd&, + const std::vector&, const std::vector&, const Eigen::MatrixXd&); + +/* + * Function: This function calls InverseDynamics with g = 0, Ftip = 0, and + * ddthetalist = 0. + * + * Inputs: + * thetalist: n-vector of joint variables + * dthetalist: A list of joint rates + * Mlist: List of link frames {i} relative to {i-1} at the home position + * Glist: Spatial inertia matrices Gi of the links + * Slist: Screw axes Si of the joints in a space frame, in the format + * of a matrix with the screw axes as the columns. + * + * Outputs: + * c: The vector c(thetalist,dthetalist) of Coriolis and centripetal + * terms for a given thetalist and dthetalist. + */ +Eigen::VectorXd VelQuadraticForces(const Eigen::VectorXd&, const Eigen::VectorXd&, + const std::vector&, const std::vector&, const Eigen::MatrixXd&); + +/* + * Function: This function calls InverseDynamics with g = 0, dthetalist = 0, and + * ddthetalist = 0. + * + * Inputs: + * thetalist: n-vector of joint variables + * Ftip: Spatial force applied by the end-effector expressed in frame {n+1} + * Mlist: List of link frames {i} relative to {i-1} at the home position + * Glist: Spatial inertia matrices Gi of the links + * Slist: Screw axes Si of the joints in a space frame, in the format + * of a matrix with the screw axes as the columns. + * + * Outputs: + * JTFtip: The joint forces and torques required only to create the + * end-effector force Ftip. + */ +Eigen::VectorXd EndEffectorForces(const Eigen::VectorXd&, const Eigen::VectorXd&, + const std::vector&, const std::vector&, const Eigen::MatrixXd&); + +/* + * Function: This function computes ddthetalist by solving: + * Mlist(thetalist) * ddthetalist = taulist - c(thetalist,dthetalist) + * - g(thetalist) - Jtr(thetalist) * Ftip + * Inputs: + * thetalist: n-vector of joint variables + * dthetalist: n-vector of joint rates + * taulist: An n-vector of joint forces/torques + * g: Gravity vector g + * Ftip: Spatial force applied by the end-effector expressed in frame {n+1} + * Mlist: List of link frames {i} relative to {i-1} at the home position + * Glist: Spatial inertia matrices Gi of the links + * Slist: Screw axes Si of the joints in a space frame, in the format + * of a matrix with the screw axes as the columns. + * + * Outputs: + * ddthetalist: The resulting joint accelerations + * + */ +Eigen::VectorXd ForwardDynamics(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, + const Eigen::VectorXd&, const Eigen::VectorXd&, const std::vector&, + const std::vector&, const Eigen::MatrixXd&); + + +/* + * Function: Compute the joint control torques at a particular time instant + * Inputs: + * thetalist: n-vector of joint variables + * dthetalist: n-vector of joint rates + * eint: n-vector of the time-integral of joint errors + * g: Gravity vector g + * Mlist: List of link frames {i} relative to {i-1} at the home position + * Glist: Spatial inertia matrices Gi of the links + * Slist: Screw axes Si of the joints in a space frame, in the format + * of a matrix with the screw axes as the columns. + * thetalistd: n-vector of reference joint variables + * dthetalistd: n-vector of reference joint rates + * ddthetalistd: n-vector of reference joint accelerations + * Kp: The feedback proportional gain (identical for each joint) + * Ki: The feedback integral gain (identical for each joint) + * Kd: The feedback derivative gain (identical for each joint) + * + * Outputs: + * tau_computed: The vector of joint forces/torques computed by the feedback + * linearizing controller at the current instant + */ +Eigen::VectorXd ComputedTorque(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, + const Eigen::VectorXd&, const std::vector&, const std::vector&, + const Eigen::MatrixXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, double, double, double); + +} \ No newline at end of file diff --git a/include/unitree_arm_sdk/utilities/CSVTool.h b/include/unitree_arm_sdk/utilities/CSVTool.h deleted file mode 100644 index 37556bf..0000000 --- a/include/unitree_arm_sdk/utilities/CSVTool.h +++ /dev/null @@ -1,246 +0,0 @@ -#ifndef CSVTOOL_H -#define CSVTOOL_H - -/* - personal tool for .csv reading and modifying. - only suitable for small .csv file. - for large .csv, try (read only) - */ -#include -#include -#include -#include -#include -#include -#include -#include -#include "math/typeTrans.h" - -enum class FileType{ - READ_WRITE, - CLEAR_DUMP -}; - -class CSVLine{ -public: - // CSVLine(std::string lineTemp, std::streampos filePos); - CSVLine(std::string lineTemp); - CSVLine(std::string label, std::vector values); - ~CSVLine(){} - - // void updateFilePos(std::streampos filePos){_filePos = filePos;} - void getValues(std::vector &values); - void changeValue(std::vector values); - void writeAtEnd(std::fstream &ioStream); - std::string getLabel(){return _label;} - -private: - // std::streampos _filePos; - std::string _label; - std::vector _values; -}; - -/* -FileType::READ_WRITE : must already exist such fileName -FileType::CLEAR_DUMP : if do not exist such file, will create one -*/ -class CSVTool{ -public: - CSVTool(std::string fileName, FileType type = FileType::READ_WRITE, int precision = 6); - ~CSVTool(){_ioStream.close();} - - bool getLine(std::string label, std::vector &values); - template - bool getLineDirect(std::string label, Args&... values); - - void modifyLine(std::string label, std::vector &values, bool addNew); - template - void modifyLineDirect(std::string label, bool addNew, Args&... values); - - void readFile(); - void saveFile(); - - bool _hasFile; -private: - std::string _fileName; - std::fstream _ioStream; - int _precision; - std::string _lineTemp; - std::map _labels; - std::vector _lines; - -}; - -/*************************/ -/* CSVLine */ -/*************************/ -// CSVLine::CSVLine(std::string lineTemp, std::streampos filePos) -// :_filePos(filePos){ - -// // std::cout << lineTemp << std::endl; -// } - -inline CSVLine::CSVLine(std::string lineTemp){ - // delete all spaces - lineTemp.erase(std::remove(lineTemp.begin(), lineTemp.end(), ' '), lineTemp.end()); - - std::stringstream ss(lineTemp); - std::string stringTemp; - - std::getline(ss, _label, ','); - - while(std::getline(ss, stringTemp, ',')){ - _values.push_back(stod(stringTemp)); - } - - // std::cout << "**********" << std::endl; - // std::cout << "_label: " << _label << std::fixed << std::setprecision(3) << std::endl; - // for(int i(0); i<_values.size(); ++i){ - // std::cout << _values.at(i) << ",,, "; - // } - // std::cout << std::endl; -} - -inline CSVLine::CSVLine(std::string label, std::vector values) - :_label(label), _values(values){ - -} - -inline void CSVLine::changeValue(std::vector values){ - if(values.size() != _values.size()){ - std::cout << "[WARNING] CSVLine::changeValue, the size changed" << std::endl; - } - _values = values; -} - -inline void CSVLine::getValues(std::vector &values){ - values = _values; -} - -inline void CSVLine::writeAtEnd(std::fstream &ioStream){ - ioStream << _label << ", "; - - for(int i(0); i<_values.size(); ++i){ - ioStream << _values.at(i) << ", "; - } - - ioStream << std::endl; -} - - -/*************************/ -/* CSVTool */ -/*************************/ -inline CSVTool::CSVTool(std::string fileName, FileType type, int precision) - : _fileName(fileName), _precision(precision){ - - if(type == FileType::READ_WRITE){ - _ioStream.open(_fileName, std::fstream::ate | std::fstream::in | std::fstream::out); - - if(!_ioStream.is_open()){ - std::cout << "[ERROR] CSVTool open file: " << fileName << " failed!" << std::endl; - // exit(-1); - _hasFile = false; - }else{ - readFile(); - _hasFile = true; - } - - } - else if(type == FileType::CLEAR_DUMP){ - _ioStream.open(_fileName, std::fstream::out); - } - -} - -inline void CSVTool::readFile(){ - if(!_ioStream.is_open()){ - // _ioStream.open(_fileName, std::fstream::ate | std::fstream::in | std::fstream::out); - std::cout << "[ERROR] CSVTool::readFile, file: " << _fileName << " has not been opened!" << std::endl; - return; - } - - _lines.clear(); - _labels.clear(); - - _ioStream.seekg(0, std::fstream::beg); - size_t lineNum = 0; - while(_ioStream && _ioStream.tellg() != std::fstream::end && getline(_ioStream, _lineTemp)){ - _lines.push_back( new CSVLine(_lineTemp) ); - - if(_labels.count(_lines.at(lineNum)->getLabel()) == 0){ - _labels.insert(std::pair(_lines.at(lineNum)->getLabel(), lineNum)); - ++lineNum; - }else{ - std::cout << "[ERROR] CSVTool::readFile, the label " - << _lines.at(lineNum)->getLabel() << " is repeated" << std::endl; - exit(-1); - } - } -} - -inline bool CSVTool::getLine(std::string label, std::vector &values){ - if(_labels.count(label) == 0){ - std::cout << "[ERROR] No such label: " << label << std::endl; - return false; - }else{ - _lines.at(_labels[label])->getValues(values); - return true; - } -} - -template -inline bool CSVTool::getLineDirect(std::string label, Args&... values){ - std::vector vec; - if(getLine(label, vec)){ - typeTrans::extractVector(vec, values...); - return true; - }else{ - return false; - } -} - -template -inline void CSVTool::modifyLineDirect(std::string label, bool addNew, Args&... values){ - std::vector vec; - typeTrans::combineToVector(vec, values...); - -// std::cout << "CSVTool::modifyLineDirect------" << std::endl; -// std::cout << "label: " << label << std::endl; -// std::cout << "vec: "; -// for(int i(0); iwriteAtEnd(_ioStream); - } - - _ioStream.close(); - _ioStream.open(_fileName, std::fstream::ate | std::fstream::in | std::fstream::out); -} - -inline void CSVTool::modifyLine(std::string label, std::vector &values, bool addNew =false){ - if(_labels.count(label) == 0){ - if(addNew){ - _labels.insert(std::pair(label, _labels.size())); - _lines.push_back(new CSVLine(label, values)); - }else{ - std::cout << "[ERROR] CSVTool::modifyLine, label " << label << "does not exist" << std::endl; - exit(-1); - } - }else{ - _lines.at(_labels[label])->changeValue(values); - } -} - - -#endif // CSVTOOL_H \ No newline at end of file diff --git a/include/unitree_arm_sdk/utilities/loop.h b/include/unitree_arm_sdk/utilities/loop.h index 8091aa2..f901a40 100644 --- a/include/unitree_arm_sdk/utilities/loop.h +++ b/include/unitree_arm_sdk/utilities/loop.h @@ -12,9 +12,6 @@ #include #include "utilities/timer.h" - -// constexpr int THREAD_PRIORITY = 99; // real-time priority - typedef boost::function Callback; class Loop { @@ -43,6 +40,12 @@ private: class LoopFunc : public Loop { public: +/* + * Function: create a thead run once every period + * Input: name: indicate what the thread aims to + * period : time, unit: second + * _cb : the function pointer + */ LoopFunc(std::string name, float period, const Callback& _cb) : Loop(name, period), _fp(_cb){} LoopFunc(std::string name, float period, int bindCPU, const Callback& _cb) diff --git a/lib/libZ1_SDK_Linux64.so b/lib/libZ1_SDK_Linux64.so new file mode 100644 index 0000000..6d8c4be Binary files /dev/null and b/lib/libZ1_SDK_Linux64.so differ diff --git a/src/control/ctrlComponents.cpp b/src/control/ctrlComponents.cpp deleted file mode 100644 index b06da7d..0000000 --- a/src/control/ctrlComponents.cpp +++ /dev/null @@ -1,86 +0,0 @@ -#include "control/ctrlComponents.h" - -CtrlComponents::CtrlComponents(double deltaT){ - dt = deltaT; - sendCmd = {0}; - sendCmd.head[0] = 0xFE; - sendCmd.head[1] = 0xFF; - - _udp = new UDPPort("127.0.0.1", 8071, 8072, RECVSTATE_LENGTH, BlockYN::NO, 500000); - lowcmd = new LowlevelCmd(); - lowstate = new LowlevelState(); -} - -CtrlComponents::~CtrlComponents(){ - delete _udp; - delete lowcmd; - delete lowstate; -} - -void CtrlComponents::sendRecv(){ - if(sendCmd.state == ArmFSMState::LOWCMD){ - for(int i(0); i<7; i++){ - sendCmd.valueUnion.jointCmd[i].Pos = lowcmd->q.at(i); - sendCmd.valueUnion.jointCmd[i].W = lowcmd->dq.at(i); - sendCmd.valueUnion.jointCmd[i].T = lowcmd->tau.at(i); - sendCmd.valueUnion.jointCmd[i].K_P = lowcmd->kp.at(i); - sendCmd.valueUnion.jointCmd[i].K_W = lowcmd->kd.at(i); - } - } - if(sendCmd.track){ - if(recvState.state == ArmFSMState::JOINTCTRL){ - for(int i(0); i<7; i++){//the arm will track joint cmd (Only q and dq) - sendCmd.valueUnion.jointCmd[i].Pos = lowcmd->q.at(i); - sendCmd.valueUnion.jointCmd[i].W = lowcmd->dq.at(i); - } - }else if(recvState.state == ArmFSMState::CARTESIAN){ - // the arm will track posture[0] cmd - sendCmd.valueUnion.trajCmd.posture[0] = Vec6toPosture(lowcmd->endPosture); - sendCmd.valueUnion.trajCmd.gripperPos = lowcmd->getGripperQ(); - } - } - _udp->send((uint8_t*)&sendCmd, SENDCMD_LENGTH); - statePast = recvState.state; - _udp->recv((uint8_t*)&recvState, RECVSTATE_LENGTH); - - for(int i(0); i<7; i++){ - lowstate->q.at(i) = recvState.jointState[i].Pos; - lowstate->dq.at(i) = recvState.jointState[i].W; - lowstate->ddq.at(i) = recvState.jointState[i].Acc; - lowstate->tau.at(i) = recvState.jointState[i].T; - } - lowstate->endPosture = PosturetoVec6(recvState.cartesianState); - //motor 0 - lowstate->temperature.at(0) = recvState.jointState[0].state[0].temperature; - lowstate->errorstate.at(0) = recvState.jointState[0].state[0].error; - lowstate->isMotorConnected.at(0) = recvState.jointState[0].state[0].isConnected.state; - //motor 1 - lowstate->temperature.at(1) = recvState.jointState[1].state[0].temperature; - lowstate->errorstate.at(1) = recvState.jointState[1].state[0].error; - lowstate->isMotorConnected.at(1) = recvState.jointState[1].state[0].isConnected.state; - //motor 2 - lowstate->temperature.at(2) = recvState.jointState[1].state[1].temperature; - lowstate->errorstate.at(2) = recvState.jointState[1].state[1].error; - lowstate->isMotorConnected.at(2) = recvState.jointState[1].state[1].isConnected.state; - //motor 3-7 - for(int i(2); i<7; i++){ - lowstate->temperature.at(i+1) = recvState.jointState[i].state[0].temperature; - lowstate->errorstate.at(i+1) = recvState.jointState[i].state[0].error; - lowstate->isMotorConnected.at(i+1) = recvState.jointState[i].state[0].isConnected.state; - } - for(int i(0); i<7; i++){ - lowstate->errorstate.at(i) = lowstate->errorstate.at(i) & 0xBF;// 0x40: nothing - } -} - -void CtrlComponents::armCtrl(Vec6 q, Vec6 qd, Vec6 tau){ - lowcmd->setQ(q); - lowcmd->setQd(qd); - lowcmd->setTau(tau); -} - -void CtrlComponents::gripperCtrl(double gripperPos, double gripperW, double gripperTau){ - lowcmd->setGripperQ(gripperPos); - lowcmd->setGripperQd(gripperW); - lowcmd->setGripperTau(gripperTau); -} \ No newline at end of file diff --git a/src/control/unitreeArm.cpp b/src/control/unitreeArm.cpp deleted file mode 100644 index 46d694d..0000000 --- a/src/control/unitreeArm.cpp +++ /dev/null @@ -1,220 +0,0 @@ -#include "control/unitreeArm.h" - -unitreeArm::unitreeArm(CtrlComponents * ctrlComp):_ctrlComp(ctrlComp){ - sendRecvThread = new LoopFunc("Z1Communication", _ctrlComp->dt, boost::bind(&unitreeArm::sendRecv, this)); -} - -unitreeArm::~unitreeArm() { - delete sendRecvThread; -} - -void unitreeArm::setFsm(ArmFSMState fsm){ - _ctrlComp->sendCmd.state = fsm; - if(fsm == ArmFSMState::LOWCMD){ - if(_ctrlComp->recvState.state != ArmFSMState::PASSIVE){ - _ctrlComp->sendCmd.state = ArmFSMState::PASSIVE; - std::cout << "[ERROR] Only state_passive could tranfer to state_lowcmd" << std::endl; - }else{ - _ctrlComp->lowcmd->setControlGain(); - _ctrlComp->lowcmd->setGripperGain(); - } - }else{ - while (_ctrlComp->recvState.state != fsm){ - usleep(_ctrlComp->dt * 1000000); - } - } -} - -void unitreeArm::backToStart(){ - setFsm(ArmFSMState::BACKTOSTART); - _ctrlComp->sendCmd.track = false; - while (_ctrlComp->recvState.state != ArmFSMState::JOINTCTRL){ - usleep(_ctrlComp->dt * 1000000); - } -} - -// move the arm to the posture indicated by the label -// the specific label name can be viewed in savedArmStates.csv -void unitreeArm::labelRun(std::string label){ - strcpy(_ctrlComp->sendCmd.valueUnion.name, label.c_str()); - setFsm(ArmFSMState::TOSTATE); - while (_ctrlComp->recvState.state != ArmFSMState::JOINTCTRL){ - usleep(_ctrlComp->dt * 1000000); - } -} - -// save current posture to a label -// the label name should be less than 10 chars -void unitreeArm::labelSave(std::string label){ - strcpy(_ctrlComp->sendCmd.valueUnion.name, label.c_str()); - setFsm(ArmFSMState::SAVESTATE); - while (_ctrlComp->recvState.state != ArmFSMState::JOINTCTRL){ - usleep(_ctrlComp->dt * 1000000); - } -} - -void unitreeArm::teach(std::string label){ - strcpy(_ctrlComp->sendCmd.valueUnion.name, label.c_str()); - setFsm(ArmFSMState::TEACH); -} - -void unitreeArm::teachRepeat(std::string label){ - strcpy(_ctrlComp->sendCmd.valueUnion.name, label.c_str()); - setFsm(ArmFSMState::TEACHREPEAT); - while (_ctrlComp->recvState.state != ArmFSMState::JOINTCTRL){ - usleep(_ctrlComp->dt * 1000000); - } -} - -void unitreeArm::calibration(){ - setFsm(ArmFSMState::CALIBRATION); - while (_ctrlComp->recvState.state != ArmFSMState::PASSIVE){ - usleep(_ctrlComp->dt * 1000000); - } -} - -// reach the target posture by directly moving the joint -void unitreeArm::MoveJ(Vec6 posture, double maxSpeed) { - _ctrlComp->sendCmd.valueUnion.trajCmd.posture[0] = Vec6toPosture(posture); - _ctrlComp->sendCmd.valueUnion.trajCmd.maxSpeed = maxSpeed; - setFsm(ArmFSMState::MOVEJ); - while (_ctrlComp->recvState.state != ArmFSMState::JOINTCTRL){ - usleep(_ctrlComp->dt * 1000000); - } -} - -void unitreeArm::MoveJ(Vec6 posture, double gripperPos, double maxSpeed) { - _ctrlComp->sendCmd.valueUnion.trajCmd.gripperPos = gripperPos; - MoveJ(posture, maxSpeed); -} - -// the end effector reaches the target point in a straight line trajectory -void unitreeArm::MoveL(Vec6 posture, double maxSpeed) { - _ctrlComp->sendCmd.valueUnion.trajCmd.posture[0] = Vec6toPosture(posture); - _ctrlComp->sendCmd.valueUnion.trajCmd.maxSpeed = maxSpeed; - setFsm(ArmFSMState::MOVEL); - while (_ctrlComp->recvState.state != ArmFSMState::JOINTCTRL){ - usleep(_ctrlComp->dt * 1000000); - } -} - -void unitreeArm::MoveL(Vec6 posture, double gripperPos, double maxSpeed) { - _ctrlComp->sendCmd.valueUnion.trajCmd.gripperPos = gripperPos; - MoveL(posture, maxSpeed); -} - -// the end effector reaches the target point in a circular arc trajectory -void unitreeArm::MoveC(Vec6 middlePosutre, Vec6 endPosture, double maxSpeed) { - _ctrlComp->sendCmd.valueUnion.trajCmd.posture[0] = Vec6toPosture(middlePosutre); - _ctrlComp->sendCmd.valueUnion.trajCmd.posture[1] = Vec6toPosture(endPosture); - _ctrlComp->sendCmd.valueUnion.trajCmd.maxSpeed = maxSpeed; - setFsm(ArmFSMState::MOVEC); - while (_ctrlComp->recvState.state != ArmFSMState::JOINTCTRL){ - usleep(_ctrlComp->dt * 1000000); - } -} - -void unitreeArm::MoveC(Vec6 middlePosutre, Vec6 endPosture, double gripperPos, double maxSpeed) { - _ctrlComp->sendCmd.valueUnion.trajCmd.gripperPos = gripperPos; - MoveC(middlePosutre, endPosture, maxSpeed); -} - -void unitreeArm::setTraj(){ - if(_ctrlComp->recvState.state != ArmFSMState::SETTRAJ){ - _ctrlComp->sendCmd.valueUnion.trajCmd.trajOrder = 0; - setFsm(ArmFSMState::SETTRAJ); - } - if(_ctrlComp->sendCmd.valueUnion.trajCmd.trajOrder == (_trajCmd.trajOrder - 1)){// make sure [order] is sequential - _ctrlComp->sendCmd.valueUnion.trajCmd = _trajCmd; - usleep(10000); - } -} - -void unitreeArm::startTraj(){ - setFsm(ArmFSMState::JOINTCTRL); - setFsm(ArmFSMState::TRAJECTORY); -} - -void unitreeArm::startTrack(ArmFSMState fsm){ - switch(fsm){ - case ArmFSMState::JOINTCTRL: - setFsm(ArmFSMState::JOINTCTRL); - usleep(10000);//wait for statePast changed , in case of setting false automatically - _ctrlComp->sendCmd.track = true; - q = _ctrlComp->lowstate->getQ(); - gripperQ = _ctrlComp->lowstate->getGripperQ(); - qd.setZero(); - gripperW = 0.; - break; - case ArmFSMState::CARTESIAN: - setFsm(ArmFSMState::CARTESIAN); - usleep(10000); - _ctrlComp->sendCmd.track = true; - // the current posture cmd must update to be consistent with the state - _ctrlComp->lowcmd->endPosture = _ctrlComp->lowstate->endPosture; - gripperQ = _ctrlComp->lowstate->getGripperQ(); - break; - default: - std::cout << "[ERROR] Please enter the state JOINTCTRL or CARTESION"<< std::endl; - break; - } -} - -void unitreeArm::sendRecv(){ - switch (_ctrlComp->sendCmd.state) - { - case ArmFSMState::SAVESTATE: - case ArmFSMState::TOSTATE: - case ArmFSMState::TEACHREPEAT: - case ArmFSMState::MOVEJ: - case ArmFSMState::MOVEL: - case ArmFSMState::MOVEC: - case ArmFSMState::CALIBRATION: - case ArmFSMState::TRAJECTORY: - case ArmFSMState::BACKTOSTART: - case ArmFSMState::TEACH: - case ArmFSMState::PASSIVE: - _ctrlComp->sendRecv(); - if(_ctrlComp->recvState.state == _ctrlComp->sendCmd.state){ - _ctrlComp->sendCmd.state = ArmFSMState::INVALID; - } - break; - case ArmFSMState::JOINTCTRL: - if(_ctrlComp->statePast != ArmFSMState::JOINTCTRL){ - _ctrlComp->sendCmd.track = false; - } - _ctrlComp->armCtrl(q, qd, Vec6::Zero()); - _ctrlComp->gripperCtrl(gripperQ, gripperW, 0.); - _ctrlComp->sendRecv(); - break; - case ArmFSMState::CARTESIAN: - if(_ctrlComp->statePast != ArmFSMState::CARTESIAN){ - _ctrlComp->sendCmd.track = false; - } - _ctrlComp->gripperCtrl(gripperQ, 0., 0.); - _ctrlComp->sendRecv(); - break; - case ArmFSMState::SETTRAJ: - _ctrlComp->sendCmd.valueUnion.trajCmd = _trajCmd; - _ctrlComp->sendRecv(); - break; - case ArmFSMState::LOWCMD: - if(_ctrlComp->statePast != ArmFSMState::LOWCMD){ - q = _ctrlComp->lowstate->getQ(); - qd.setZero(); - tau.setZero(); - gripperQ = _ctrlComp->lowstate->getGripperQ(); - gripperW = 0.0; - gripperTau = 0.0; - } - _ctrlComp->armCtrl(q, qd, tau); - _ctrlComp->gripperCtrl(gripperQ, gripperW, gripperTau); - _ctrlComp->sendRecv(); - break; - case ArmFSMState::INVALID: - _ctrlComp->sendRecv(); - break; - default: - break; - } -} \ No newline at end of file diff --git a/src/message/LowlevelCmd.cpp b/src/message/LowlevelCmd.cpp deleted file mode 100644 index 9fd570f..0000000 --- a/src/message/LowlevelCmd.cpp +++ /dev/null @@ -1,170 +0,0 @@ -#include "message/LowlevelCmd.h" - -LowlevelCmd::LowlevelCmd(){ - q.resize(_dof+1); - dq.resize(_dof+1); - tau.resize(_dof+1); - kp.resize(_dof+1); - kd.resize(_dof+1); -} - - -void LowlevelCmd::setZeroDq(){ - for(size_t i(0); i<_dof; ++i){ - dq.at(i) = 0.0; - } -} - -void LowlevelCmd::setZeroTau(){ - for(size_t i(0); i<_dof; ++i){ - tau.at(i) = 0.0; - } -} - -void LowlevelCmd::setZeroKp(){ - for(size_t i(0); i<_dof; ++i){ - kp.at(i) = 0.0; - } -} - -void LowlevelCmd::setZeroKd(){ - for(size_t i(0); i<_dof; ++i){ - kd.at(i) = 0.0; - } -} - -void LowlevelCmd::setControlGain(std::vector KP, std::vector KW){ - kp.at(0) = KP.at(0); - kp.at(1) = KP.at(1); - kp.at(2) = KP.at(2); - kp.at(3) = KP.at(3); - kp.at(4) = KP.at(4); - kp.at(5) = KP.at(5); - - kd.at(0) = KW.at(0); - kd.at(1) = KW.at(1); - kd.at(2) = KW.at(2); - kd.at(3) = KW.at(3); - kd.at(4) = KW.at(4); - kd.at(5) = KW.at(5); -} - -void LowlevelCmd::setControlGain(){ - kp.at(0) = 20; - kp.at(1) = 30; - kp.at(2) = 30; - kp.at(3) = 20; - kp.at(4) = 15; - kp.at(5) = 10; - - kd.at(0) = 2000; - kd.at(1) = 2000; - kd.at(2) = 2000; - kd.at(3) = 2000; - kd.at(4) = 2000; - kd.at(5) = 2000; -} - -void LowlevelCmd::setQ(std::vector qInput){ - if(qInput.size() != _dof){ - std::cout << "[ERROR] The qInput size of LowlevelCmd::setQ(std::vector) is not suitable" << std::endl; - } - for(size_t i(0); i<_dof; ++i){ - q.at(i) = qInput.at(i); - } -} - -void LowlevelCmd::setQ(VecX qInput){ - if(qInput.rows() != _dof){ - std::cout << "[ERROR] The qInput size of LowlevelCmd::setQ(VecX) is not suitable" << std::endl; - } - for(size_t i(0); i<_dof; ++i){ - q.at(i) = qInput(i); - } -} - -void LowlevelCmd::setQd(VecX qDInput){ - if(qDInput.rows() != _dof){ - std::cout << "[ERROR] The qDInput size of LowlevelCmd::setQd(VecX) is not suitable" << std::endl; - } - for(size_t i(0); i<_dof; ++i){ - dq.at(i) = qDInput(i); - } -} - -void LowlevelCmd::setTau(VecX tauInput){ - if(tauInput.rows() != _dof){ - std::cout << "[ERROR] The tauInput size of LowlevelCmd::setTau(VecX) is not suitable" << std::endl; - } - for(size_t i(0); i<_dof; ++i){ - tau.at(i) = tauInput(i); - } -} - -void LowlevelCmd::setPassive(){ - setZeroDq(); - setZeroTau(); - setZeroKp(); - for(size_t i; i<_dof; ++i){ - kd.at(i) = 10.0; - } -} - -void LowlevelCmd::setGripperGain(double KP, double KW){ - kp.at(kp.size()-1) = KP; - kd.at(kd.size()-1) = KW; -} - -//set gripper default gain -void LowlevelCmd::setGripperGain(){ - kp.at(kp.size()-1) = 20.0; - kd.at(kd.size()-1) = 100.0; -} - -//set gripper gain to 0 -void LowlevelCmd::setGripperZeroGain(){ - kp.at(kp.size()-1) = 0.0; - kd.at(kd.size()-1) = 0.0; -} - -void LowlevelCmd::setGripperQ(double qInput){ - q.at(q.size()-1) = qInput; -} - -double LowlevelCmd::getGripperQ(){ - return q.at(q.size()-1); -} - -void LowlevelCmd::setGripperQd(double qdInput){ - dq.at(dq.size()-1) = qdInput; -} - -double LowlevelCmd::getGripperQd(){ - return dq.at(dq.size()-1); -} - -void LowlevelCmd::setGripperTau(double tauInput){ - tau.at(tau.size()-1) = tauInput; -} - -double LowlevelCmd::getGripperTau(){ - return tau.at(tau.size()-1); -} - -//q of joints, without gripper -Vec6 LowlevelCmd::getQ(){ - Vec6 qReturn; - for(int i(0); i<6; ++i){ - qReturn(i) = q.at(i); - } - return qReturn; -} - -//qd of joints, without gripper -Vec6 LowlevelCmd::getQd(){ - Vec6 qdReturn; - for(int i(0); i<6; ++i){ - qdReturn(i) = dq.at(i); - } - return qdReturn; -} diff --git a/src/message/LowlevelState.cpp b/src/message/LowlevelState.cpp deleted file mode 100644 index bc1d3af..0000000 --- a/src/message/LowlevelState.cpp +++ /dev/null @@ -1,67 +0,0 @@ -#include "message/LowlevelState.h" - -LowlevelState::LowlevelState(){ -//set q, dq, tau parameters - q.resize(_dof+1); - dq.resize(_dof+1); - ddq.resize(_dof+1); - tau.resize(_dof+1); - - temperature.resize(_dof+2); - errorstate.resize(_dof+2); - isMotorConnected.resize(_dof+2); -} - -Vec6 LowlevelState::getQ(){ - Vec6 qReturn; - for(size_t i(0); i<_dof; ++i){ - qReturn(i) = q.at(i); - } - return qReturn; -} - -Vec6 LowlevelState::getQd(){ - Vec6 qdReturn; - for(size_t i(0); i<_dof; ++i){ - qdReturn(i) = dq.at(i); - } - return qdReturn; -} - -Vec6 LowlevelState::getQdd(){ - Vec6 qddReturn; - for(size_t i(0); i<_dof; ++i){ - qddReturn(i) = ddq.at(i); - } - return qddReturn; -} - -Vec6 LowlevelState::getTau(){ - Vec6 tauReturn; - for(int i(0); i < _dof; ++i){ - tauReturn(i) = tau.at(i); - } - return tauReturn; -} - -Vec6 PosturetoVec6(const Posture p){ - Vec6 posture; - posture(0) = p.roll; - posture(1) = p.pitch; - posture(2) = p.yaw; - posture(3) = p.x; - posture(4) = p.y; - posture(5) = p.z; - return posture; -} - -Posture Vec6toPosture(const Vec6 p){ - Posture posture; - posture.roll = p(0); - posture.pitch = p(1); - posture.yaw = p(2); - posture.x = p(3); - posture.y = p(4); - posture.z = p(5); - return posture; -} \ No newline at end of file diff --git a/src/message/udp.cpp b/src/message/udp.cpp deleted file mode 100644 index 89190e4..0000000 --- a/src/message/udp.cpp +++ /dev/null @@ -1,116 +0,0 @@ -#include "message/udp.h" - -void IOPort::resetIO(BlockYN blockYN, size_t recvLength, size_t timeOutUs){ - _blockYN = blockYN; - _recvLength = recvLength; - _timeout.tv_sec = timeOutUs / 1000000; - _timeout.tv_usec = timeOutUs % 1000000; - _timeoutSaved = _timeout; -} - -UDPPort::UDPPort(std::string toIP, uint toPort, uint ownPort, - size_t recvLength, - BlockYN blockYN, size_t timeOutUs) - :IOPort(blockYN, recvLength, timeOutUs){ - bzero(&_toAddr, sizeof(sockaddr_in)); - bzero(&_ownAddr, sizeof(sockaddr_in)); - bzero(&_fromAddr, sizeof(sockaddr_in)); - - _toAddr.sin_family = AF_INET; - _toAddr.sin_port = htons(toPort); - _toAddr.sin_addr.s_addr = inet_addr(toIP.c_str()); - - _ownAddr.sin_family = AF_INET; - _ownAddr.sin_port = htons(ownPort); - _ownAddr.sin_addr.s_addr = htonl(INADDR_ANY); - - _sockfd = socket(AF_INET, SOCK_DGRAM, 0); - if(_sockfd < 0){ - perror("[ERROR] UDPPort::UDPPort, create socket failed\n"); - exit(-1); - } - - setsockopt(_sockfd, SOL_SOCKET, SO_REUSEADDR, &_on, sizeof(_on)); - - if(bind(_sockfd, (struct sockaddr*)&_ownAddr, sizeof(struct sockaddr)) < 0){ - perror("[ERROR] UDPPort::UDPPort, bind failed"); - exit(-1); - } - - _sockaddrSize = sizeof(struct sockaddr); - -} - -UDPPort::~UDPPort(){ - close(_sockfd); -} - -size_t UDPPort::send(uint8_t *sendMsg, size_t sendMsgLength){ - _sentLength = sendto(_sockfd, sendMsg, sendMsgLength, 0, (struct sockaddr*)&_toAddr, _sockaddrSize); - if(_sentLength != sendMsgLength){ - std::cout << "[WARNING] UDPPort::send, sent " << _sentLength << " bytes, but not " << sendMsgLength << " bytes, " << strerror(errno) << std::endl; - } - return _sentLength; -} - -size_t UDPPort::recv(uint8_t *recvMsg){ - if(_blockYN == BlockYN::NO){ - return _recvUnBlock(recvMsg, _recvLength); - }else{ - return _recvBlock(recvMsg, _recvLength); - } -} - -size_t UDPPort::recv(uint8_t *recvMsg, size_t recvLength){ - if(_blockYN == BlockYN::NO){ - return _recvUnBlock(recvMsg, recvLength); - }else{ - return _recvBlock(recvMsg, recvLength); - } -} - -/* block, until received */ -size_t UDPPort::_recvBlock(uint8_t *recvMsg, size_t recvLength){ - size_t receivedLength = recvfrom(_sockfd, recvMsg, recvLength, MSG_WAITALL, (struct sockaddr*)&_fromAddr, &_sockaddrSize); - if(receivedLength != recvLength){ - std::cout << "[WARNING] UDPPort::recv, block version, received " << receivedLength << " bytes, but not " << recvLength << " bytes, " << strerror(errno) << std::endl; - } - tcflush(_sockfd, TCIOFLUSH); - return receivedLength; -} - -/* unblock, has time out, time unit is us */ -size_t UDPPort::_recvUnBlock(uint8_t *recvMsg, size_t recvLength){ - size_t receivedLength; - /* init every time */ - FD_ZERO(&_rSet); - FD_SET(_sockfd, &_rSet); - _timeout = _timeoutSaved; - - switch ( select(_sockfd+1, &_rSet, NULL, NULL, &_timeout) ){ - case -1: - std::cout << "[WARNING] UDPPort::recv, unblock version, receive error" << std::endl; - return 0; - case 0: - if((++_isDisConnectCnt) > 200){ - isDisConnect = true; - } - std::cout << "[WARNING] UDPPort::recv, unblock version, wait time out" << std::endl; - return 0; - default: - _isDisConnectCnt = 0; - if(isDisConnect){ - - } - isDisConnect = false; - - receivedLength = recvfrom(_sockfd, recvMsg, recvLength, 0, (struct sockaddr*)&_fromAddr, &_sockaddrSize); - if(receivedLength != recvLength){ - std::cout << "[WARNING] UDPPort::recv, unblock version, received " << receivedLength << " bytes, but not " << recvLength << " bytes, " << strerror(errno) << std::endl; - // std::cout << "IP: " << inet_ntoa(_fromAddr.sin_addr) << std::endl; - } - - tcflush(_sockfd, TCIOFLUSH); - return receivedLength; - } -} \ No newline at end of file diff --git a/src/utilities/loop.cpp b/src/utilities/loop.cpp deleted file mode 100644 index e61f983..0000000 --- a/src/utilities/loop.cpp +++ /dev/null @@ -1,63 +0,0 @@ -#include "utilities/loop.h" - -Loop::Loop(std::string name, float period, int bindCPU) - :_name(name), _period(period), _bindCPU(bindCPU){ - _timer = new AbsoluteTimer(_period); -} - -Loop::~Loop(){ - shutdown(); - delete _timer; -} - -void Loop::start() { - if (_isrunning) { - printf("[Error] Loop %s is already running.\n", _name.c_str()); - return; - } - - // printf("[Loop Start] named: %s, period: %d(ms), ", _name.c_str(), (int)(_period*1000.0)); - if(_bindCPU > 0){ - _bind_cpu_flag = true; - // printf("run at cpu: %d \n", _bindCPU); - } else { - _bind_cpu_flag = false; - // printf("cpu unspecified\n"); - } - - _isrunning = true; - _thread = std::thread(&Loop::entryFunc, this); - if(_bind_cpu_flag == true) { - cpu_set_t cpuset; - CPU_ZERO(&cpuset); - CPU_SET(_bindCPU, &cpuset); - if(0 != pthread_setaffinity_np(_thread.native_handle(), sizeof(cpu_set_t), &cpuset)){ - printf("Error: Set affinity failed.\n"); - } - } -} - -void Loop::shutdown() { - if (!_isrunning) { - // printf("[Warning] Loop %s shutdown nothing.\n", _name.c_str()); - return; - } - _isrunning = false; - _thread.join(); - // std::cout << "[REPORT] The time out rate of thread " << _name << " is " << 100.0*(double)_timeOutTimes/(double)_runTimes << "%" << std::endl; - return; -} - -void Loop::entryFunc() { - while (_isrunning) { - _timer->start(); - ++_runTimes; - functionCB(); - if(!_timer->wait()){ - ++_timeOutTimes; - // std::cout << "[WARNING] Loop " << _name << " time out." << std::endl; - } - } - - // printf("[Loop End] named: %s\n", _name.c_str()); -} diff --git a/src/utilities/timer.cpp b/src/utilities/timer.cpp deleted file mode 100644 index f862581..0000000 --- a/src/utilities/timer.cpp +++ /dev/null @@ -1,58 +0,0 @@ -#include -#include "utilities/timer.h" - -AbsoluteTimer::AbsoluteTimer(double waitTimeS) - :_waitTime(waitTimeS){ - _timerFd = timerfd_create(CLOCK_MONOTONIC, 0); -} - -AbsoluteTimer::~AbsoluteTimer(){ - close(_timerFd); -} - -void AbsoluteTimer::start(){ - _startTime = getTimeSecond(); -} - -bool AbsoluteTimer::wait(){ - if(_waitTime == 0.0){ - return true; - } - _leftTime = _waitTime - (getTimeSecond() - _startTime); - if(_leftTime < 0.0){ - // std::cout << "[WARNING] The wait time " << _waitTime * 1000.0 << "ms of AbsoluteTimer is not enough!" << std::endl - // << "The program has already cost " << (getTimeSecond() - _startTime) * 1000.0 << "ms." << std::endl; - return false; - } - - int m; - while(true){ - _leftTime = _waitTime - (getTimeSecond() - _startTime); - if(_leftTime < 1e-6){ - start(); - return true; - } - else if(_leftTime < 2e-3){ - _nextWaitTime = _leftTime / 2.0; - } - else{ - _nextWaitTime = 2e-3; - } - _updateWaitTime(_nextWaitTime); - m = read(_timerFd, &_missed, sizeof(_missed)); - } - - return false; -} - -void AbsoluteTimer::_updateWaitTime(double waitTimeS){ - int seconds = (int)waitTimeS; - int nanoseconds = (int)(1e9 * fmod(waitTimeS, 1.f)); - - _timerSpec.it_interval.tv_sec = seconds; - _timerSpec.it_interval.tv_nsec = nanoseconds; - _timerSpec.it_value.tv_sec = seconds; - _timerSpec.it_value.tv_nsec = nanoseconds; - - timerfd_settime(_timerFd, 0, &_timerSpec, nullptr); -} \ No newline at end of file diff --git a/thirdparty/eigen-3.3.9.zip b/thirdparty/eigen-3.3.9.zip deleted file mode 100644 index bc9412a..0000000 Binary files a/thirdparty/eigen-3.3.9.zip and /dev/null differ