EmbodiedAI
/
z1_sdk
mirror of https://github.com/unitreerobotics/z1_sdk.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

V2.1.0 

1. add thirdparty
2. delete src
3. add example_JointCtrl
4. add armModel
5. add comments
This commit is contained in:
Agnel Wang 2022-11-15 16:05:20 +08:00
parent 5578c69819
commit bb796755ba
28 changed files with 3820 additions and 1275 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
CMakeLists.txt
View File

@ -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)

1
README.md
View File

@ -1 +0,0 @@
see [unitree-z1-docs](http://dev-z1.unitree.com/)

96
examples/bigDemo.cpp
View File

@ -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 ------"<<std::endl;
std::cout<<"qState: "<<_ctrlComp->lowstate->getQ().transpose()<<std::endl;
std::cout<<"qdState: "<<_ctrlComp->lowstate->getQd().transpose()<<std::endl;
std::cout<<"tauState: "<<_ctrlComp->lowstate->getTau().transpose()<<std::endl;
std::cout<<"qState: "<<lowstate->getQ().transpose()<<std::endl;
std::cout<<"qdState: "<<lowstate->getQd().transpose()<<std::endl;
std::cout<<"tauState: "<<lowstate->getTau().transpose()<<std::endl;
std::cout<<"------ Endeffector Cartesian Posture ------"<<std::endl;
std::cout<<"roll pitch yaw x y z"<<std::endl;
std::cout<<_ctrlComp->lowstate->endPosture.transpose()<<std::endl;
std::cout<<lowstate->endPosture.transpose()<<std::endl;
}
int main() {
CtrlComponents *ctrlComp = new CtrlComponents(0.002);
Z1ARM arm(ctrlComp);
std::cout << std::fixed << std::setprecision(3);
Z1ARM arm;
arm.sendRecvThread->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;
}

4
examples/example_CtrlByFSM.cpp
View File

@ -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;
}

87
examples/example_CtrlByTraj.cpp
View File

@ -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;
}

13
examples/example_JointCtrl.cpp
View File

@ -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;
}

15
examples/example_lowCmd_send.cpp
View File

@ -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;
}

30
include/unitree_arm_sdk/control/ctrlComponents.h
View File

@ -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;
};

155
include/unitree_arm_sdk/control/unitreeArm.h
View File

@ -5,34 +5,161 @@
class unitreeArm{
public:
unitreeArm(CtrlComponents * ctrlComp);
unitreeArm(bool hasUnitreeGripper);
~unitreeArm();
void setFsm(ArmFSMState fsm);
/*
* 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();
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();
/*
* 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();
//command parameters
Vec6 q, qd, tau;
Vec6 qPast, qdPast, tauPast;
Vec6 endPosture;
double gripperQ, gripperW, gripperTau;
TrajCmd _trajCmd;
CtrlComponents *_ctrlComp;
LoopFunc *sendRecvThread;
LowlevelCmd *lowcmd;//same as _ctrlComp->lowcmd
LowlevelState *lowstate;//same as _ctrlComp->lowstate
CtrlComponents *_ctrlComp;
};
#endif

244
include/unitree_arm_sdk/math/mathTools.h Normal file
View File

@ -0,0 +1,244 @@
#ifndef MATHTOOLS_H
#define MATHTOOLS_H
#include <iostream>
#include <math.h>
#include "math/mathTypes.h"
template<typename T>
T max(T value){
return value;
}
template<typename T, typename... Args>
inline T max(const T val0, const Args... restVal){
return val0 > (max<T>(restVal...)) ? val0 : max<T>(restVal...);
}
template<typename T>
T min(T value){
return value;
}
template<typename T, typename... Args>
inline T min(const T val0, const Args... restVal){
return val0 > (min<T>(restVal...)) ? val0 : min<T>(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); i<firstVec.rows(); ++i){
result(i) = angleError(firstVec(i), secondVec(i), directionMatter);
}
return result;
}
inline bool vectorEqual(VecX v1, VecX v2, double tolerance){
if(v1.rows() != v2.rows()){
std::cout << "[WARNING] vectorEqual, the size of two vectors is not equal, v1 is "
<< v1.rows() << ", v2 is " << v2.rows() << std::endl;
return false;
}
for(int i(0); i<v1.rows(); ++i){
if(fabs(v1(i)-v2(i))>tolerance){
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<typename T0, typename T1>
inline T0 killZeroOffset(T0 a, const T1 limit){
if((a > -limit) && (a < limit)){
a = 0;
}
return a;
}
template<typename T0, typename T1, typename T2>
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<typename T>
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<typename T1, typename T2>
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<typename T1, typename T2, typename T3>
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<typename T1, typename T2, typename T3>
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

44
include/unitree_arm_sdk/message/LowlevelCmd.h
View File

@ -19,28 +19,60 @@ public:
LowlevelCmd();
~LowlevelCmd(){}
/* 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<double> KP, std::vector<double> KW);
/* q = qInput */
void setQ(std::vector<double> 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();
void setGripperQd(double qdInput);
double getGripperQd();
void setGripperTau(double tauInput);
double getGripperTau();
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<double> q, without gripper */
Vec6 getQ();
/* return Vec6 from std::vector<double> dq, without gripper */
Vec6 getQd();
};

11
include/unitree_arm_sdk/message/LowlevelState.h
View File

@ -23,7 +23,18 @@ public:
std::vector<double> tau;
std::vector<int> 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<uint8_t> 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<uint8_t> isMotorConnected;
Vec6 getQ();

1
include/unitree_arm_sdk/message/udp.h
View File

@ -13,7 +13,6 @@ enum class BlockYN{
NO
};
class IOPort{
public:
IOPort(BlockYN blockYN, size_t recvLength, size_t timeOutUs){

150
include/unitree_arm_sdk/model/ArmModel.h Normal file
View File

@ -0,0 +1,150 @@
#ifndef ARMMODEL_H
#define ARMMODEL_H
#include <vector>
#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<double> getJointQMax() {return _jointQMax;}
std::vector<double> getJointQMin() {return _jointQMin;}
std::vector<double> 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<HomoMat> _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<Mat6> _Glist;
std::vector<Vec3> _jointAxis;// omega
std::vector<Vec3> _jointPos;// p_0
std::vector<double> _jointQMax;
std::vector<double> _jointQMin;
std::vector<double> _jointSpeedMax;
Vec3 _endPosLocal; // based on mount postion
Vec3 _endMountPosGlobal;
std::vector<Vec3> _linkPosLocal;
std::vector<double> _disJoint;//distance between joint
std::vector<double> _linkMass;
std::vector<Vec3> _linkCom; // center of mass
std::vector<Mat3> _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

2532
include/unitree_arm_sdk/thirdparty/quadProgpp/Array.hh vendored Normal file
View File

File diff suppressed because it is too large Load Diff

77
include/unitree_arm_sdk/thirdparty/quadProgpp/QuadProg++.hh vendored Normal file
View File

@ -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 <eigen3/Eigen/Dense>
namespace quadprogpp {
double solve_quadprog(Matrix<double>& G, Vector<double>& g0,
const Matrix<double>& CE, const Vector<double>& ce0,
const Matrix<double>& CI, const Vector<double>& ci0,
Vector<double>& x);
} // namespace quadprogpp
#endif // #define _QUADPROGPP

535
include/unitree_arm_sdk/thirdparty/robotics.h vendored Normal file
View File

@ -0,0 +1,535 @@
#pragma once
#include <vector>
#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<Eigen::MatrixXd> 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<S, theta>?
*/
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<Eigen::MatrixXd>&,
const std::vector<Eigen::MatrixXd>&, 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<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&, 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<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&, 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<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&, 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<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&, 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<Eigen::MatrixXd>&,
const std::vector<Eigen::MatrixXd>&, 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<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&,
const Eigen::MatrixXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, double, double, double);
}

246
include/unitree_arm_sdk/utilities/CSVTool.h
View File

@ -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 <https://github.com/ben-strasser/fast-cpp-csv-parser> (read only)
*/
#include <string>
#include <vector>
#include <map>
#include <fstream>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <algorithm>
#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<double> values);
~CSVLine(){}
// void updateFilePos(std::streampos filePos){_filePos = filePos;}
void getValues(std::vector<double> &values);
void changeValue(std::vector<double> values);
void writeAtEnd(std::fstream &ioStream);
std::string getLabel(){return _label;}
private:
// std::streampos _filePos;
std::string _label;
std::vector<double> _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<double> &values);
template<typename... Args>
bool getLineDirect(std::string label, Args&... values);
void modifyLine(std::string label, std::vector<double> &values, bool addNew);
template<typename... Args>
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<std::string, size_t> _labels;
std::vector<CSVLine*> _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<double> values)
:_label(label), _values(values){
}
inline void CSVLine::changeValue(std::vector<double> values){
if(values.size() != _values.size()){
std::cout << "[WARNING] CSVLine::changeValue, the size changed" << std::endl;
}
_values = values;
}
inline void CSVLine::getValues(std::vector<double> &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<std::string, size_t>(_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<double> &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<typename... Args>
inline bool CSVTool::getLineDirect(std::string label, Args&... values){
std::vector<double> vec;
if(getLine(label, vec)){
typeTrans::extractVector(vec, values...);
return true;
}else{
return false;
}
}
template<typename... Args>
inline void CSVTool::modifyLineDirect(std::string label, bool addNew, Args&... values){
std::vector<double> vec;
typeTrans::combineToVector(vec, values...);
// std::cout << "CSVTool::modifyLineDirect------" << std::endl;
// std::cout << "label: " << label << std::endl;
// std::cout << "vec: ";
// for(int i(0); i<vec.size(); ++i){
// std::cout << vec.at(i) << ", ";
// }std::cout << std::endl;
modifyLine(label, vec, addNew);
}
inline void CSVTool::saveFile(){
_ioStream.close();
_ioStream.open(_fileName, std::fstream::out);
_ioStream << std::fixed << std::setprecision(_precision);
for(int i(0); i<_lines.size(); ++i){
_lines.at(i)->writeAtEnd(_ioStream);
}
_ioStream.close();
_ioStream.open(_fileName, std::fstream::ate | std::fstream::in | std::fstream::out);
}
inline void CSVTool::modifyLine(std::string label, std::vector<double> &values, bool addNew =false){
if(_labels.count(label) == 0){
if(addNew){
_labels.insert(std::pair<std::string, size_t>(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

9
include/unitree_arm_sdk/utilities/loop.h
View File

@ -12,9 +12,6 @@
#include <boost/bind.hpp>
#include "utilities/timer.h"
// constexpr int THREAD_PRIORITY = 99; // real-time priority
typedef boost::function<void ()> 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)

BIN
lib/libZ1_SDK_Linux64.so Normal file
View File

Binary file not shown.

86
src/control/ctrlComponents.cpp
View File

@ -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);
}

220
src/control/unitreeArm.cpp
View File

@ -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;
}
}

170
src/message/LowlevelCmd.cpp
View File

@ -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<double> KP, std::vector<double> 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<double> qInput){
if(qInput.size() != _dof){
std::cout << "[ERROR] The qInput size of LowlevelCmd::setQ(std::vector<double>) 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;
}

67
src/message/LowlevelState.cpp
View File

@ -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;
}

116
src/message/udp.cpp
View File

@ -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;
}
}

63
src/utilities/loop.cpp
View File

@ -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());
}

58
src/utilities/timer.cpp
View File

@ -1,58 +0,0 @@
#include <math.h>
#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);
}

BIN
thirdparty/eigen-3.3.9.zip vendored
View File

Binary file not shown.