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

V2.0.0

This commit is contained in:
Agnel Wang 2022-11-11 19:49:41 +08:00
parent ab2280d83f
commit 6e865f0f56
83 changed files with 1695 additions and 8337 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
CMakeLists.txt
View File

@ -11,36 +11,17 @@ set(COMMUNICATION UDP) # UDP
# ----------------------configurations----------------------
# COMMUNICATION
if(${COMMUNICATION} STREQUAL "UDP")
add_definitions(-DUDP)
set(SIMULATION OFF)
set(REAL_ROBOT ON)
elseif(${COMMUNICATION} STREQUAL "ROS")
add_definitions(-DROS)
add_definitions(-DCOMPILE_WITH_SIMULATION)
set(SIMULATION ON)
set(REAL_ROBOT OFF)
else()
message(FATAL_ERROR "[CMake ERROR] The COMMUNICATION is error")
endif()
# SIM OR REAL
if(((SIMULATION) AND (REAL_ROBOT)) OR ((NOT SIMULATION) AND (NOT REAL_ROBOT)))
message(FATAL_ERROR "[CMake ERROR] The SIMULATION and REAL_ROBOT can only be one ON one OFF")
endif()
if(REAL_ROBOT)
add_definitions(-DCOMPILE_WITH_REAL_ROBOT)
endif()
if(SIMULATION)
add_definitions(-DCOMPILE_WITH_SIMULATION)
add_definitions(-DRUN_ROS)
find_package(gazebo REQUIRED)
endif()
# ARM_MODEL
add_definitions(-DARM_Z1)
if(SIMULATION)
find_package(catkin REQUIRED COMPONENTS
controller_manager
genmsg
@ -67,24 +48,19 @@ if(SIMULATION)
)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
endif()
set(EIGEN_PATH /usr/include/eigen3)
include_directories(
include
thirdparty
../z1_sdk/include
${Boost_INCLUDE_DIR}
${EIGEN_PATH}
)
link_directories(lib)
# ----------------------add executable----------------------
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/../build) # EXECUTABLE:CMAKE_RUNTIME_OUTPUT_DIRECTORY
add_executable(z1_ctrl main.cpp)
# add_dependencies(z1_ctrl Z1_${COMMUNICATION}_Linux64)
target_link_libraries(z1_ctrl ${catkin_LIBRARIES} libUnitree_motor_SDK_Linux64_EPOLL.so libZ1_${COMMUNICATION}_Linux64.so)
# set(EXECUTABLE_OUTPUT_PATH /home/$ENV{USER}/)
target_link_libraries(z1_ctrl libZ1_${COMMUNICATION}_Linux64.so ${catkin_LIBRARIES} )

27
config.xml
View File

@ -1,26 +1,17 @@
<configurartion>
<control set="2">
<control set=2>
<option1>keyboard</option1>
<option2>sdk</option2>
<option3>joystick</option3> <!-- default aliengo_joystick -->
</control>
<gripper set="2">
<option1 name="none"/>
<option2 name="Unitree_gripper"/>
<option3 name="user_gripper">
<endPosLocalX>0.0</endPosLocalX>
<endPosLocalY>0.0</endPosLocalY>
<endPosLocalZ>0.0</endPosLocalZ>
<mass>0.0</mass>
<comX>0.0</comX>
<comY>0.0</comY>
<comZ>0.0</comZ>
<Ixx>0.0</Ixx>
<Iyy>0.0</Iyy>
<Izz>0.0</Izz>
</option3>
</gripper>
<!-- slave computer communication -->
<IP>192.168.123.110</IP>
<Port>8881</Port>
<collision>
<open>N</open> <!-- Y or N -->
<limitT>10.0</limitT> <!-- N*M -->
<load>0.0</load><!-- kg-->
</collision>
</configurartion>

27
config/savedArmStates.csv
View File

@ -1,14 +1,13 @@
forward, -0.000019, 1.542859, -1.037883, -0.531308, 0.002487, -0.012173, 0.999650, -0.002146, -0.026357, 0.389527, 0.002468, 0.999923, 0.012173, 0.000269, 0.026329, -0.012234, 0.999578, 0.402549, 0.000000, 0.000000, 0.000000, 1.000000,
start, -0.000336, 0.001634, 0.000000, 0.064640, 0.000248, 0.000230, 0.997805, 0.000104, 0.066225, 0.048696, -0.000087, 1.000000, -0.000252, 0.000011, -0.066225, 0.000246, 0.997805, 0.148729, 0.000000, 0.000000, 0.000000, 1.000000,
startFlat, 0.000000, 0.000000, -0.000012, -0.074351, 0.000000, 0.000006, 0.997236, -0.000000, -0.074294, 0.086594, 0.000000, 1.000000, -0.000006, 0.000000, 0.074294, 0.000006, 0.997236, 0.176788, 0.000000, 0.000000, 0.000000, 1.000000,
teachstart, -0.858000, 1.951000, -0.825000, 0.000000, -1.154000, 0.000000, 0.653696, 0.756536, -0.018308, 0.260474, -0.756240, 0.653952, 0.021180, -0.301334, 0.027996, 0.000000, 0.999608, 0.183326, 0.000000, 0.000000, 0.000000, 1.000000,
fulltest00, 2.316194, 0.013239, -0.001959, -0.000047, -0.278982, 0.221496, -0.654115, -0.677458, 0.336427, -0.022776, 0.708636, -0.704403, -0.040645, 0.024673, 0.264516, 0.211818, 0.940832, 0.174303, 0.000000, 0.000000, 0.000000, 1.000000,
fulltest01, 0.000194, 1.621239, -1.455959, 2.470727, 1.492796, 0.419496, 0.205219, 0.489994, 0.847225, 0.332273, 0.619814, -0.735004, 0.274956, 0.059687, 0.757440, 0.468696, -0.454542, 0.521459, 0.000000, 0.000000, 0.000000, 1.000000,
fulltest02, -2.336194, 3.117239, -3.099593, -2.478727, -1.516796, 0.419496, 0.415340, -0.220546, 0.882526, -0.404129, -0.454627, 0.789988, 0.411380, -0.505948, -0.787913, -0.572083, 0.227847, 0.075097, 0.000000, 0.000000, 0.000000, 1.000000,
001, 0.000000, 0.922204, -1.215402, 0.293198, 0.000000, 0.000000, 1.000000, 0.000000, -0.000000, 0.200000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.500000, 0.000000, 0.000000, 0.000000, 1.000000,
002, -1.035000, 0.922204, -1.017402, -0.285802, 0.000000, 0.000000, 0.473918, 0.859862, -0.189839, 0.103971, -0.798204, 0.510526, 0.319739, -0.175114, 0.371849, 0.000000, 0.928293, 0.541400, 0.000000, 0.000000, 0.000000, 1.000000,
003, 0.954000, 0.910204, -0.984402, -0.192802, 0.000000, 0.000000, 0.557929, -0.815736, -0.152611, 0.121384, 0.786832, 0.578425, -0.215223, 0.171185, 0.263839, 0.000000, 0.964567, 0.510707, 0.000000, 0.000000, 0.000000, 1.000000,
mohe1, 0.966000, 1.420800, -1.412412, -1.141151, 0.997200, 1.675206, -0.560087, -0.444472, 0.699105, 0.049195, 0.666938, -0.742508, 0.062250, 0.235148, 0.491423, 0.501125, 0.712304, 0.622679, 0.000000, 0.000000, 0.000000, 1.000000,
mohe2, -1.057200, 2.424000, -2.682793, 0.986449, -1.342800, -1.709994, -0.765519, -0.400523, 0.503550, 0.166634, -0.625604, 0.646204, -0.437081, -0.515483, -0.150335, -0.649616, -0.745251, 0.381296, 0.000000, 0.000000, 0.000000, 1.000000,
5kg1, 0.661051, 3.010641, -2.885593, -0.233059, 0.000065, 0.000206, 0.784707, -0.614016, -0.084965, 0.592966, 0.610421, 0.789294, -0.066347, 0.461215, 0.107801, 0.000198, 0.994173, 0.198126, 0.000000, 0.000000, 0.000000, 1.000000,
5kg2, -0.495523, 3.122645, -2.856166, -0.406859, 0.000065, -0.000378, 0.871097, 0.475482, -0.122910, 0.664711, -0.470757, 0.879726, 0.066863, -0.359274, 0.139919, -0.000383, 0.990163, 0.133116, 0.000000, 0.000000, 0.000000, 1.000000,
forward, 0.0, 1.5, -1.0, -0.54, 0.0, 0.0,
startFlat, 0.0, 0.0, -0.005, -0.074, 0.0, 0.0,
search, 0.0, 0.0, -0.005, -0.5, 0.0, 0.0,
show_left, -1.0, 0.9, -1., -0.3, 0.0, 0.0,
show_mid, 0.0, 0.9, -1.2, 0.3, 0.0, 0.0,
show_right, 1.0, 0.9, -1., -0.3, 0.0, 0.0,
5kg1, 0.661051, 3.010641, -2.885593, -0.233059, 0.0, 0.0,
5kg2, -0.495523, 3.122645, -2.856166, -0.406859, 0.0, 0.0,
mh1, 2.617994, 0.084000, -2.755093, 1.470296, 1.454343, 2.645527,
mh2, -2.485994, 2.451000, -0.102000, -1.611829, -1.421343, -2.265473,
01, 0.028072, 0.343213, -0.010000, -0.073124, 0.084670, -0.016248,
02, 2.024087, 0.760020, -0.816744, -1.190535, 1.175596, 2.064632,
03, -1.740466, 2.731333, -2.161190, 0.806278, -0.889325, -2.529499,
1 forward -0.000019 0.0 1.542859 1.5 -1.037883 -1.0 -0.531308 -0.54 0.002487 0.0 -0.012173 0.0 0.999650 -0.002146 -0.026357 0.389527 0.002468 0.999923 0.012173 0.000269 0.026329 -0.012234 0.999578 0.402549 0.000000 0.000000 0.000000 1.000000
2 start startFlat -0.000336 0.0 0.001634 0.0 0.000000 -0.005 0.064640 -0.074 0.000248 0.0 0.000230 0.0 0.997805 0.000104 0.066225 0.048696 -0.000087 1.000000 -0.000252 0.000011 -0.066225 0.000246 0.997805 0.148729 0.000000 0.000000 0.000000 1.000000
3 startFlat search 0.000000 0.0 0.000000 0.0 -0.000012 -0.005 -0.074351 -0.5 0.000000 0.0 0.000006 0.0 0.997236 -0.000000 -0.074294 0.086594 0.000000 1.000000 -0.000006 0.000000 0.074294 0.000006 0.997236 0.176788 0.000000 0.000000 0.000000 1.000000
4 teachstart show_left -0.858000 -1.0 1.951000 0.9 -0.825000 -1. 0.000000 -0.3 -1.154000 0.0 0.000000 0.0 0.653696 0.756536 -0.018308 0.260474 -0.756240 0.653952 0.021180 -0.301334 0.027996 0.000000 0.999608 0.183326 0.000000 0.000000 0.000000 1.000000
5 fulltest00 show_mid 2.316194 0.0 0.013239 0.9 -0.001959 -1.2 -0.000047 0.3 -0.278982 0.0 0.221496 0.0 -0.654115 -0.677458 0.336427 -0.022776 0.708636 -0.704403 -0.040645 0.024673 0.264516 0.211818 0.940832 0.174303 0.000000 0.000000 0.000000 1.000000
6 fulltest01 show_right 0.000194 1.0 1.621239 0.9 -1.455959 -1. 2.470727 -0.3 1.492796 0.0 0.419496 0.0 0.205219 0.489994 0.847225 0.332273 0.619814 -0.735004 0.274956 0.059687 0.757440 0.468696 -0.454542 0.521459 0.000000 0.000000 0.000000 1.000000
7 fulltest02 5kg1 -2.336194 0.661051 3.117239 3.010641 -3.099593 -2.885593 -2.478727 -0.233059 -1.516796 0.0 0.419496 0.0 0.415340 -0.220546 0.882526 -0.404129 -0.454627 0.789988 0.411380 -0.505948 -0.787913 -0.572083 0.227847 0.075097 0.000000 0.000000 0.000000 1.000000
8 001 5kg2 0.000000 -0.495523 0.922204 3.122645 -1.215402 -2.856166 0.293198 -0.406859 0.000000 0.0 0.000000 0.0 1.000000 0.000000 -0.000000 0.200000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.500000 0.000000 0.000000 0.000000 1.000000
9 002 mh1 -1.035000 2.617994 0.922204 0.084000 -1.017402 -2.755093 -0.285802 1.470296 0.000000 1.454343 0.000000 2.645527 0.473918 0.859862 -0.189839 0.103971 -0.798204 0.510526 0.319739 -0.175114 0.371849 0.000000 0.928293 0.541400 0.000000 0.000000 0.000000 1.000000
10 003 mh2 0.954000 -2.485994 0.910204 2.451000 -0.984402 -0.102000 -0.192802 -1.611829 0.000000 -1.421343 0.000000 -2.265473 0.557929 -0.815736 -0.152611 0.121384 0.786832 0.578425 -0.215223 0.171185 0.263839 0.000000 0.964567 0.510707 0.000000 0.000000 0.000000 1.000000
11 mohe1 01 0.966000 0.028072 1.420800 0.343213 -1.412412 -0.010000 -1.141151 -0.073124 0.997200 0.084670 1.675206 -0.016248 -0.560087 -0.444472 0.699105 0.049195 0.666938 -0.742508 0.062250 0.235148 0.491423 0.501125 0.712304 0.622679 0.000000 0.000000 0.000000 1.000000
12 mohe2 02 -1.057200 2.024087 2.424000 0.760020 -2.682793 -0.816744 0.986449 -1.190535 -1.342800 1.175596 -1.709994 2.064632 -0.765519 -0.400523 0.503550 0.166634 -0.625604 0.646204 -0.437081 -0.515483 -0.150335 -0.649616 -0.745251 0.381296 0.000000 0.000000 0.000000 1.000000
13 5kg1 03 0.661051 -1.740466 3.010641 2.731333 -2.885593 -2.161190 -0.233059 0.806278 0.000065 -0.889325 0.000206 -2.529499 0.784707 -0.614016 -0.084965 0.592966 0.610421 0.789294 -0.066347 0.461215 0.107801 0.000198 0.994173 0.198126 0.000000 0.000000 0.000000 1.000000
5kg2 -0.495523 3.122645 -2.856166 -0.406859 0.000065 -0.000378 0.871097 0.475482 -0.122910 0.664711 -0.470757 0.879726 0.066863 -0.359274 0.139919 -0.000383 0.990163 0.133116 0.000000 0.000000 0.000000 1.000000

2
include/FSM/BaseState.h
View File

@ -23,10 +23,10 @@ public:
}
}
std::string getStateName(){return _stateNameString;}
int getStateNameEnum(){return _stateNameEnum;};
protected:
int _stateNameEnum;
std::string _stateNameString;
Control _ctrl;
};
#endif

60
include/FSM/FSMState.h
View File

@ -5,15 +5,9 @@
#include <iostream>
#include <unistd.h>
#include "control/CtrlComponents.h"
#include "message/LowlevelCmd.h"
#include "message/LowlevelState.h"
#include "common/enumClass.h"
#include "common/math/mathTools.h"
#include "common/math/mathTypes.h"
#include "unitree_arm_sdk/timer.h"
#include "model/ArmDynKineModel.h"
#include "FSM/FiniteStateMachine.h"
#include "common/utilities/timer.h"
#include "FSM/BaseState.h"
class FSMState : public BaseState{
public:
@ -24,55 +18,23 @@ public:
virtual void run() = 0;
virtual void exit() = 0;
virtual int checkChange(int cmd) {return (int)ArmFSMStateName::INVALID;}
bool _collisionTest();
protected:
void _armCtrl();
void _tauDynForward();
void _tauFriction();
void _qdFeedBack();
bool _collisionTest();
void _jointPosProtect();
void _jointSpeedProtect();
void _stateUpdate();
void _gripperCmd();
void _gripperCtrl();
double _gripperPos;
double _gripperW;
double _gripperTau;
double _gripperPosStep;//keyboard
double _gripperTauStep;
void _recordData();
Vec6 _postureToVec6(Posture posture);
double _gripperLinearFriction;
double _gripperCoulombFriction;
static double _gripperCoulombDirection;
double _gripperFriction;
CtrlComponents *_ctrlComp;
ArmFSMStateName _nextStateName;
IOInterface *_ioInter;
LowlevelCmd *_lowCmd;
LowlevelState *_lowState;
UserValue _userValue;
ArmDynKineModel *_armModel;
IOInterface *_ioInter;
ArmModel *_armModel;
Vec6 _qPast, _qdPast, _q, _qd, _qdd;
Vec6 _tau, _endForce;
Vec6 _qPast, _qdPast, _q, _qd, _qdd, _tauf, _tauCmd, _g;
double _gripperPos, _gripperW, _gripperTau;
uint _collisionCnt;
Vec6 _coulombFriction;
static Vec6 _coulombDirection;
Vec6 _linearFriction;
Vec6 _kpDiag, _kdDiag;
Mat6 _Kp, _Kd;
std::vector<double> _jointQMax;
std::vector<double> _jointQMin;
std::vector<double> _jointSpeedMax;
SendCmd _sendCmd;
RecvState _recvState;
CtrlComponents *_ctrlComp;
};
#endif // FSMSTATE_H

32
include/FSM/FiniteStateMachine.h
View File

@ -2,40 +2,26 @@
#define FSM_H
#include <vector>
#include "FSM/BaseState.h"
#include "unitree_arm_sdk/cmdPanel.h"
#include "unitree_arm_sdk/loop.h"
#include "unitree_arm_sdk/keyboard.h"
#include "FSM/FSMState.h"
#include "common/utilities/loop.h"
#include "control/CtrlComponents.h"
enum class FSMRunMode{
NORMAL,
CHANGE
};
/*
状态机初始状态为states的第一个元素
为了支持多个状态机从同一个CmdPanel获取触发信号，
加入了cmdChannel，不同状态机之间的cmdChannel必须不同，
否则可能会漏过状态触发
*/
class FiniteStateMachine{
public:
FiniteStateMachine(std::vector<BaseState*> states,
CmdPanel *cmdPanel, size_t cmdChannel = 0, double dt=0.002);
FiniteStateMachine(std::vector<FSMState*> states, CtrlComponents *ctrlComp);
virtual ~FiniteStateMachine();
private:
void _run();
std::vector<BaseState*> _states;
std::vector<FSMState*> _states;
FSMRunMode _mode;
FSMMode _mode;
bool _running;
BaseState* _currentState;
BaseState* _nextState;
FSMState* _currentState;
FSMState* _nextState;
int _nextStateEnum;
size_t _cmdChannel;
CmdPanel *_cmdPanel;
CtrlComponents *_ctrlComp;
LoopFunc *_runThread;
};

15
include/FSM/State_BackToStart.h
View File

@ -1,14 +1,21 @@
#ifndef STATE_BACKTOSTART_H
#define STATE_BACKTOSTART_H
#include "FSM/State_Trajectory.h"
class State_BackToStart : public State_Trajectory{
#include "FSM/FSMState.h"
#include "trajectory/JointSpaceTraj.h"
class State_BackToStart : public FSMState{
public:
State_BackToStart(CtrlComponents *ctrlComp);
~State_BackToStart();
void enter();
void run();
void exit();
int checkChange(int cmd);
private:
void _setTraj();
void _setTrajSDK(){}
bool _reach, _pastReach;
JointSpaceTraj *_jointTraj;
};
#endif // STATE_BACKTOSTART_H

14
include/FSM/State_Cartesian.h
View File

@ -12,17 +12,15 @@ public:
void exit();
int checkChange(int cmd);
private:
double _posSpeed;
double _oriSpeed;
Vec3 _omega;
Vec3 _velocity;
double _oriSpeed = 0.4;// control by keyboard or joystick
double _posSpeed = 0.15;
double oriSpeedLimit = 0.3;// limits in SDK
double posSpeedLimit = 0.3;
VecX _changeDirections;
Vec6 _twist;
HomoMat _endHomoGoal, _endHomoGoalPast;
Vec6 _endPostureGoal, _endPosturePast, _endPostureDelta;
HomoMat _endHomoFeedback;
Vec6 _Pdes;
Vec6 _Pfd;
Vec6 _Pkp;
};
#endif // CARTESIAN_H

20
include/FSM/State_Dance.h
View File

@ -1,20 +0,0 @@
#ifndef STATE_DANCETRAJ_H
#define STATE_DANCETRAJ_H
#include "FSM/State_Trajectory.h"
class State_Dance : public State_Trajectory{
public:
State_Dance(CtrlComponents *ctrlComp,
TrajectoryManager *traj,
ArmFSMStateName stateEnum,
ArmFSMStateName nextState,
std::string stateString);
void exit();
int checkChange(int cmd);
private:
bool _freeBottom = false;
void _setTraj(){}
};
#endif

2
include/FSM/State_JointSpace.h
View File

@ -12,7 +12,7 @@ public:
void exit();
int checkChange(int cmd);
private:
double _angleStep;
std::vector<double> jointSpeedMax;
};
#endif // JOINTSPACE_H

22
include/FSM/State_MoveC.h
View File

@ -2,7 +2,7 @@
#define MOVEC_H
#include "FSM/FSMState.h"
#include "trajectory/CartesionSpaceTraj.h"
#include "trajectory/EndCircleTraj.h"
class State_MoveC : public FSMState{
public:
@ -13,22 +13,10 @@ public:
void exit();
int checkChange(int cmd);
private:
void quadprogArea();
double _posSpeed;
double _oriSpeed;
Vec3 _omega;
Vec3 _velocity;
Vec6 _twist;
Vec6 _pastPosture, _endPosture, _middlePostureGoal, _endPostureGoal, _endTwist;
HomoMat _endHomoFeedback;
Vec6 _Pdes;
Vec6 _Pfd;
Vec6 _Pkp;
Vec6 _endPostureError;
// 轨迹相关变量
CartesionSpaceTraj *_cartesionTraj;
bool _reached, _timeReached, _taskReached, _pastTaskReached;
double _speed;
std::vector<Vec6> _postures;
EndCircleTraj *_circleTraj;
bool _timeReached, _taskReached, _pastTaskReached, _finalReached;
};
#endif // CARTESIAN_H

5
include/FSM/State_MoveJ.h
View File

@ -13,11 +13,10 @@ public:
void exit();
int checkChange(int cmd);
private:
double _costTime;
HomoMat _goalHomo;
double _speed;
JointSpaceTraj *_jointTraj;
bool _reached, _pastReached, _finalReached;
std::vector<Vec6> _result;
std::vector<Vec6> _qCmd;
};
#endif // CARTESIAN_H

21
include/FSM/State_MoveL.h
View File

@ -2,7 +2,7 @@
#define MOVEL_H
#include "FSM/FSMState.h"
#include "trajectory/CartesionSpaceTraj.h"
#include "trajectory/EndLineTraj.h"
class State_MoveL : public FSMState{
public:
@ -13,22 +13,9 @@ public:
void exit();
int checkChange(int cmd);
private:
void quadprogArea();
double _posSpeed;
double _oriSpeed;
Vec3 _omega;
Vec3 _velocity;
Vec6 _twist;
Vec6 _pastPosture, _endPosture, _endTwist;
HomoMat _endHomoFeedback;
Vec6 _Pdes;
Vec6 _Pfd;
Vec6 _Pkp;
Vec6 _endPostureError;
// 轨迹相关变量
std::vector<std::vector<double> > _posture;
CartesionSpaceTraj *_cartesionTraj;
double _speed;
std::vector<Vec6> _postures;
EndLineTraj *_lineTraj;
bool _timeReached, _taskReached, _pastTaskReached, _finalReached;
};
#endif // CARTESIAN_H

2
include/FSM/State_Passive.h
View File

@ -1,7 +1,7 @@
#ifndef PASSIVE_H
#define PASSIVE_H
#include "FSMState.h"
#include "FSM/FSMState.h"
class State_Passive : public FSMState{
public:

3
include/FSM/State_SaveState.h
View File

@ -13,9 +13,6 @@ public:
void exit();
int checkChange(int cmd);
private:
CSVTool *_csv;
Vec6 _q;
HomoMat _endHomo;
};
#endif // SAVESTATE_H

1
include/FSM/State_Teach.h
View File

@ -2,7 +2,6 @@
#define STATETEACH_H
#include "FSM/FSMState.h"
#include "common/utilities/CSVTool.h"
class State_Teach : public FSMState{
public:

1
include/FSM/State_TeachRepeat.h
View File

@ -3,7 +3,6 @@
#include "FSM/FSMState.h"
#include "trajectory/TrajectoryManager.h"
#include "common/utilities/CSVTool.h"
class State_TeachRepeat : public FSMState{
public:

1
include/FSM/State_ToState.h
View File

@ -19,7 +19,6 @@ private:
JointSpaceTraj *_jointTraj;
bool _reach, _pastReach;
std::string _goalName;
// CSVTool *_csv;
};
#endif // TOSTATE_H

10
include/FSM/State_Trajectory.h
View File

@ -2,7 +2,6 @@
#define CARTESIANPATH_H
#include "FSM/FSMState.h"
#include "trajectory/EndHomoTraj.h"
#include "trajectory/TrajectoryManager.h"
class State_Trajectory : public FSMState{
@ -16,12 +15,12 @@ public:
void exit();
int checkChange(int cmd);
protected:
// EndHomoTraj *_traj;
virtual void _setTraj();
virtual void _setTrajSdk();
void _setTraj();
void _setTrajSDK();
TrajectoryManager *_traj;
HomoMat _goalHomo;
Vec6 _goalTwist;
double speedTemp;
JointSpaceTraj *_toStartTraj;
bool _reachedStart = false;
@ -31,11 +30,8 @@ protected:
std::vector<StopForTime*> _stopTraj;
std::vector<EndLineTraj*> _lineTraj;
long long startTime;
static std::vector<TrajCmd> _trajCmd;
Vec6 _posture[2];
static bool _isTrajFSM;
};
#endif // CARTESIANPATH_H

35
include/common/enumClass.h
View File

@ -1,13 +1,6 @@
#ifndef ENUMCLASS_H
#define ENUMCLASS_H
enum class FrameType{
BODY,
HIP,
ENDEFFECTOR,
GLOBAL
};
enum class FSMMode{
NORMAL,
CHANGE
@ -28,19 +21,7 @@ enum class ArmFSMStateName{
TEACHREPEAT,
CALIBRATION,
SETTRAJ,
DANCE00,
DANCE01,
DANCE02,
DANCE03,
DANCE04,
DANCE05,
DANCE06,
DANCE07,
DANCE08,
DANCE09,
BACKTOSTART,
GRIPPER_OPEN,
GRIPPER_CLOSE,
NEXT,
LOWCMD
};
@ -50,20 +31,10 @@ enum class JointMotorType{
DOUBLE_MOTOR
};
enum class MotorMountType{
STATOR_FIRST,
OUTPUT_FIRST
};
enum class HeadType{
TIGER,
DOG
};
enum class Control{
_KEYBOARD,
_SDK,
_JOYSTICK,
KEYBOARD,
SDK,
JOYSTICK
};
#endif // ENUMCLASS_H

6
include/common/utilities/LPFilter.h → include/common/math/Filter.h Executable file → Normal file
View File

@ -1,5 +1,5 @@
#ifndef LOWPASSFILTER
#define LOWPASSFILTER
#ifndef FILTER
#define FILTER
#include <vector>
#include <Eigen/Dense>
@ -26,4 +26,4 @@ private:
bool _start;
};
#endif // LOWPASSFILTER
#endif // FILTER

246
include/common/math/mathTools.h
View File

@ -2,18 +2,9 @@
#define MATHTOOLS_H
#include <iostream>
#include <math.h>
#include "common/math/mathTypes.h"
// template<typename T1, typename T2>
// inline T1 max(const T1 a, const T2 b){
// return (a > b ? a : b);
// }
// template<typename T1, typename T2>
// inline T1 min(const T1 a, const T2 b){
// return (a < b ? a : b);
// }
template<typename T>
T max(T value){
return value;
@ -34,21 +25,6 @@ inline T min(const T val0, const Args... restVal){
return val0 > (min<T>(restVal...)) ? val0 : min<T>(restVal...);
}
inline Mat2 skew(const double& w){
Mat2 mat; mat.setZero();
mat(0, 1) = -w;
mat(1, 0) = w;
return mat;
}
inline Mat3 skew(const Vec3& v) {
Mat3 m;
m << 0, -v(2), v(1),
v(2), 0, -v(0),
-v(1), v(0), 0;
return m;
}
enum class TurnDirection{
NOMATTER,
POSITIVE,
@ -150,11 +126,9 @@ inline double saturation(const double a, double limValue1, double limValue2){
if(a < lowLim){
return lowLim;
}
else if(a > highLim){
}else if(a > highLim){
return highLim;
}
else{
}else{
return a;
}
}
@ -180,21 +154,17 @@ inline T1 invNormalize(const T0 value, const T1 min, const T2 max, const double
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)){
// printf("[ERROR] The x of function windowFunc should between [0, xRange]\n");
std::cout << "[ERROR][windowFunc] The x=" << x << ", which should between [0, xRange]" << std::endl;
}
if((windowRatio <= 0)||(windowRatio >= 0.5)){
// printf("[ERROR] The windowRatio of function windowFunc should between (0, 0.5)\n");
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){
}else if(x/xRange > 1 - windowRatio){
return yRange * (xRange - x)/(xRange * windowRatio);
}
else{
}else{
return yRange;
}
}
@ -232,212 +202,6 @@ inline void updateAvgCov(T1 &cov, T2 &exp, T3 newValue, double n){
updateAverage(exp, newValue, n);
}
/* rotate matrix about x axis */
inline RotMat rotX(const double &theta) {
double s = std::sin(theta);
double c = std::cos(theta);
RotMat R;
R << 1, 0, 0, 0, c, -s, 0, s, c;
return R;
}
/* rotate matrix about y axis */
inline RotMat rotY(const double &theta) {
double s = std::sin(theta);
double c = std::cos(theta);
RotMat R;
R << c, 0, s, 0, 1, 0, -s, 0, c;
return R;
}
/* rotate matrix about z axis */
inline RotMat rotZ(const double &theta) {
double s = std::sin(theta);
double c = std::cos(theta);
RotMat R;
R << c, -s, 0, s, c, 0, 0, 0, 1;
return R;
}
inline RotMat so3ToRotMat(const Vec3& _rot){
RotMat R;
double theta = _rot.norm();
if (fabs(theta) <1e-6)
R = RotMat::Identity();
else{
Vec3 u_axis(_rot/theta);
double cos_theta = cos(theta);
R = cos_theta*RotMat::Identity()+sin(theta)*skew(u_axis) +(1-cos_theta)*(u_axis*u_axis.transpose());
}
return R;
}
/* row pitch yaw to rotate matrix */
inline RotMat rpyToRotMat(const double& row, const double& pitch, const double& yaw) {
RotMat m = rotZ(yaw) * rotY(pitch) * rotX(row);
return m;
}
inline RotMat quatToRotMat(const Quat& q) {
double e0 = q(0);
double e1 = q(1);
double e2 = q(2);
double e3 = q(3);
RotMat R;
R << 1 - 2 * (e2 * e2 + e3 * e3), 2 * (e1 * e2 - e0 * e3),
2 * (e1 * e3 + e0 * e2), 2 * (e1 * e2 + e0 * e3),
1 - 2 * (e1 * e1 + e3 * e3), 2 * (e2 * e3 - e0 * e1),
2 * (e1 * e3 - e0 * e2), 2 * (e2 * e3 + e0 * e1),
1 - 2 * (e1 * e1 + e2 * e2);
return R;
}
inline Vec3 rotMatToRPY(const Mat3& R) {
Vec3 rpy;
rpy(0) = atan2(R(2,1),R(2,2));//asin(R(2,1)/cos(rpy(1))); // atan2(R(2,1),R(2,2));
rpy(1) = asin(-R(2,0));
rpy(2) = atan2(R(1,0),R(0,0));
return rpy;
}
/* rotate matrix to exponential coordinate(omega*theta) */
inline Vec3 rotMatToExp(const RotMat& rm){
double cosValue = rm.trace()/2.0-1/2.0;
if(cosValue > 1.0f){
cosValue = 1.0f;
}else if(cosValue < -1.0f){
cosValue = -1.0f;
}
double angle = acos(cosValue);
Vec3 exp;
if (fabs(angle) < 1e-5){
exp=Vec3(0,0,0);
}
else if (fabs(angle - M_PI) < 1e-5){
exp = angle * Vec3(rm(0,0)+1, rm(0,1), rm(0,2)) / sqrt(2*(1+rm(0, 0)));
}
else{
exp=angle/(2.0f*sin(angle))*Vec3(rm(2,1)-rm(1,2),rm(0,2)-rm(2,0),rm(1,0)-rm(0,1));
}
return exp;
}
/* add 1 at the end of Vec3 */
inline Vec4 homoVec(Vec3 v3, double end = 1.0){
Vec4 v4;
v4.block(0, 0, 3, 1) = v3;
v4(3) = end;
return v4;
}
/* remove 1 at the end of Vec4 */
inline Vec3 noHomoVec(Vec4 v4){
Vec3 v3;
v3 = v4.block(0, 0, 3, 1);
return v3;
}
/* build Homogeneous Matrix by p and m */
inline HomoMat homoMatrix(Vec3 p, Mat3 m){
HomoMat homoM;
homoM.setZero();
homoM.topLeftCorner(3, 3) = m;
homoM.topRightCorner(3, 1) = p;
homoM(3, 3) = 1;
return homoM;
}
/* build Homogeneous Matrix by p and w */
inline HomoMat homoMatrix(Vec3 p, Vec3 w){
HomoMat homoM;
homoM.setZero();
homoM.topLeftCorner(3, 3) = so3ToRotMat(w);
homoM.topRightCorner(3, 1) = p;
homoM(3, 3) = 1;
return homoM;
}
/* build Homogeneous Matrix by x axis, y axis and p */
inline HomoMat homoMatrix(Vec3 x, Vec3 y, Vec3 p){
HomoMat homoM;
homoM.setZero();
Vec3 xN = x.normalized();
Vec3 yN = y.normalized();
homoM.block(0, 0, 3, 1) = xN;
homoM.block(0, 1, 3, 1) = yN;
homoM.block(0, 2, 3, 1) = skew(xN) * yN;
homoM.topRightCorner(3, 1) = p;
homoM(3, 3) = 1;
return homoM;
}
/* build Homogeneous Matrix of rotate joint */
inline HomoMat homoMatrixRotate(Vec3 q, Vec3 w){
HomoMat homoM;
Mat3 eye3 = Mat3::Identity();
Mat3 rotateM = so3ToRotMat(w);
homoM.setZero();
homoM.topLeftCorner(3, 3) = rotateM;
homoM.topRightCorner(3, 1) = (eye3 - rotateM) * q;
// homoM.topRightCorner(3, 1) = q;
homoM(3, 3) = 1;
return homoM;
}
/* build Homogeneous Matrix by posture */
inline HomoMat homoMatrixPosture(Vec6 posture){
HomoMat homoM;
homoM.setZero();
homoM.topLeftCorner(3, 3) = rpyToRotMat(posture(0),posture(1),posture(2));
homoM.topRightCorner(3, 1) << posture(3),posture(4),posture(5);
homoM(3, 3) = 1;
return homoM;
}
/* inverse matrix of homogeneous matrix */
inline HomoMat homoMatrixInverse(HomoMat homoM){
HomoMat homoInv;
homoInv.setZero();
homoInv.topLeftCorner(3, 3) = homoM.topLeftCorner(3, 3).transpose();
homoInv.topRightCorner(3, 1) = -homoM.topLeftCorner(3, 3).transpose() * homoM.topRightCorner(3, 1);
homoInv(3, 3) = 1;
return homoInv;
}
/* get position of Homogeneous Matrix */
inline Vec3 getHomoPosition(HomoMat mat){
return mat.block(0, 3, 3, 1);
}
/* get rotate matrix of Homogeneous Matrix */
inline RotMat getHomoRotMat(HomoMat mat){
return mat.block(0, 0, 3, 3);
}
/* convert homogeneous matrix to posture vector */
inline Vec6 homoToPosture(HomoMat mat){
Vec6 posture;
posture.block(0,0,3,1) = rotMatToRPY(getHomoRotMat(mat));
posture.block(3,0,3,1) = getHomoPosition(mat);
return posture;
}
/* convert posture vector matrix to homogeneous */
inline HomoMat postureToHomo(Vec6 posture){
HomoMat homoM;
homoM.setZero();
homoM.topLeftCorner(3, 3) = rpyToRotMat(posture(0), posture(1), posture(2));
homoM.topRightCorner(3, 1) << posture(3), posture(4), posture(5);
homoM(3, 3) = 1;
return homoM;
}
// Calculate average value and covariance
class AvgCov{
public:

10
include/common/math/mathTypes.h
View File

@ -2,6 +2,7 @@
#define MATHTYPES_H
#include <eigen3/Eigen/Dense>
#include <vector>
/************************/
/******** Vector ********/
@ -96,4 +97,13 @@ inline VecX stdVecToEigenVec(T stdVec){
VecX eigenVec = Eigen::VectorXd::Map(&stdVec[0], stdVec.size());
return eigenVec;
}
inline std::vector<double> EigenVectostdVec(VecX eigenVec){
std::vector<double> stdVec;
for(int i(0); i<eigenVec.size();i++){
stdVec.push_back(eigenVec(i));
}
return stdVec;
}
#endif // MATHTYPES_H

0
thirdparty/quadProgpp/include/Array.hh → include/common/math/quadProgpp/Array.hh Executable file → Normal file
View File

2
thirdparty/quadProgpp/include/QuadProg++.hh → include/common/math/quadProgpp/QuadProg++.hh Executable file → Normal file
View File

@ -62,7 +62,7 @@ s.t.
#ifndef _QUADPROGPP
#define _QUADPROGPP
#include "quadProgpp/include/Array.hh"
#include "common/math/quadProgpp/Array.hh"
#include <eigen3/Eigen/Dense>
namespace quadprogpp {

250
include/common/math/robotics.h
View File

@ -3,8 +3,7 @@
#include <vector>
#include "common/math/mathTools.h"
namespace mr {
namespace robo {
/*
* Function: Find if the value is negligible enough to consider 0
* Inputs: value to be checked as a double
@ -12,6 +11,39 @@ namespace mr {
*/
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)
@ -131,6 +163,21 @@ Eigen::MatrixXd MatrixExp6(const Eigen::MatrixXd&);
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
@ -459,72 +506,6 @@ Eigen::VectorXd ForwardDynamics(const Eigen::VectorXd&, const Eigen::VectorXd&,
const std::vector<Eigen::MatrixXd>&, const Eigen::MatrixXd&);
/*
* Function: Compute the joint angles and velocities at the next timestep using
first order Euler integration
* Inputs:
* thetalist[in]: n-vector of joint variables
* dthetalist[in]: n-vector of joint rates
* ddthetalist: n-vector of joint accelerations
* dt: The timestep delta t
*
* Outputs:
* thetalist[out]: Vector of joint variables after dt from first order Euler integration
* dthetalist[out]: Vector of joint rates after dt from first order Euler integration
*/
void EulerStep(Eigen::VectorXd&, Eigen::VectorXd&, const Eigen::VectorXd&, double);
/*
* Function: Compute the joint forces/torques required to move the serial chain along the given
* trajectory using inverse dynamics
* Inputs:
* thetamat: An N x n matrix of robot joint variables (N: no. of trajecoty time step points; n: no. of robot joints
* dthetamat: An N x n matrix of robot joint velocities
* ddthetamat: An N x n matrix of robot joint accelerations
* g: Gravity vector g
* Ftipmat: An N x 6 matrix of spatial forces applied by the end-effector (if there are no tip forces
* the user should input a zero matrix)
* 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:
* taumat: The N x n matrix of joint forces/torques for the specified trajectory, where each of the N rows is the vector
* of joint forces/torques at each time step
*/
Eigen::MatrixXd InverseDynamicsTrajectory(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&,
const Eigen::VectorXd&, const Eigen::MatrixXd&, const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&,
const Eigen::MatrixXd&);
/*
* Function: Compute the motion of a serial chain given an open-loop history of joint forces/torques
* Inputs:
* thetalist: n-vector of initial joint variables
* dthetalist: n-vector of initial joint rates
* taumat: An N x n matrix of joint forces/torques, where each row is is the joint effort at any time step
* g: Gravity vector g
* Ftipmat: An N x 6 matrix of spatial forces applied by the end-effector (if there are no tip forces
* the user should input a zero matrix)
* 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.
* dt: The timestep between consecutive joint forces/torques
* intRes: Integration resolution is the number of times integration (Euler) takes places between each time step.
* Must be an integer value greater than or equal to 1
*
* Outputs: std::vector of [thetamat, dthetamat]
* thetamat: The N x n matrix of joint angles resulting from the specified joint forces/torques
* dthetamat: The N x n matrix of joint velocities
*/
std::vector<Eigen::MatrixXd> ForwardDynamicsTrajectory(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::MatrixXd&,
const Eigen::VectorXd&, const Eigen::MatrixXd&, const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&,
const Eigen::MatrixXd&, double, int);
/*
* Function: Compute the joint control torques at a particular time instant
* Inputs:
@ -551,135 +532,4 @@ Eigen::VectorXd ComputedTorque(const Eigen::VectorXd&, const Eigen::VectorXd&, c
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);
/*
* Function: Compute s(t) for a cubic time scaling
* Inputs:
* Tf: Total time of the motion in seconds from rest to rest
* t: The current time t satisfying 0 < t < Tf
*
* Outputs:
* st: The path parameter corresponding to a third-order
* polynomial motion that begins and ends at zero velocity
*/
double CubicTimeScaling(double, double);
/*
* Function: Compute s(t) for a quintic time scaling
* Inputs:
* Tf: Total time of the motion in seconds from rest to rest
* t: The current time t satisfying 0 < t < Tf
*
* Outputs:
* st: The path parameter corresponding to a fifth-order
* polynomial motion that begins and ends at zero velocity
* and zero acceleration
*/
double QuinticTimeScaling(double, double);
/*
* Function: Compute a straight-line trajectory in joint space
* Inputs:
* thetastart: The initial joint variables
* thetaend: The final joint variables
* Tf: Total time of the motion in seconds from rest to rest
* N: The number of points N > 1 (Start and stop) in the discrete
* representation of the trajectory
* method: The time-scaling method, where 3 indicates cubic (third-
* order polynomial) time scaling and 5 indicates quintic
* (fifth-order polynomial) time scaling
*
* Outputs:
* traj: A trajectory as an N x n matrix, where each row is an n-vector
* of joint variables at an instant in time. The first row is
* thetastart and the Nth row is thetaend . The elapsed time
* between each row is Tf / (N - 1)
*/
Eigen::MatrixXd JointTrajectory(const Eigen::VectorXd&, const Eigen::VectorXd&, double, int, int);
/*
* Function: Compute a trajectory as a list of N SE(3) matrices corresponding to
* the screw motion about a space screw axis
* Inputs:
* Xstart: The initial end-effector configuration
* Xend: The final end-effector configuration
* Tf: Total time of the motion in seconds from rest to rest
* N: The number of points N > 1 (Start and stop) in the discrete
* representation of the trajectory
* method: The time-scaling method, where 3 indicates cubic (third-
* order polynomial) time scaling and 5 indicates quintic
* (fifth-order polynomial) time scaling
*
* Outputs:
* traj: The discretized trajectory as a list of N matrices in SE(3)
* separated in time by Tf/(N-1). The first in the list is Xstart
* and the Nth is Xend
*/
std::vector<Eigen::MatrixXd> ScrewTrajectory(const Eigen::MatrixXd&, const Eigen::MatrixXd&, double, int, int);
/*
* Function: Compute a trajectory as a list of N SE(3) matrices corresponding to
* the origin of the end-effector frame following a straight line
* Inputs:
* Xstart: The initial end-effector configuration
* Xend: The final end-effector configuration
* Tf: Total time of the motion in seconds from rest to rest
* N: The number of points N > 1 (Start and stop) in the discrete
* representation of the trajectory
* method: The time-scaling method, where 3 indicates cubic (third-
* order polynomial) time scaling and 5 indicates quintic
* (fifth-order polynomial) time scaling
*
* Outputs:
* traj: The discretized trajectory as a list of N matrices in SE(3)
* separated in time by Tf/(N-1). The first in the list is Xstart
* and the Nth is Xend
* Notes:
* This function is similar to ScrewTrajectory, except the origin of the
* end-effector frame follows a straight line, decoupled from the rotational
* motion.
*/
std::vector<Eigen::MatrixXd> CartesianTrajectory(const Eigen::MatrixXd&, const Eigen::MatrixXd&, double, int, int);
/*
* Function: Compute the motion of a serial chain given an open-loop history of joint forces/torques
* Inputs:
* thetalist: n-vector of initial joint variables
* dthetalist: n-vector of initial joint rates
* g: Gravity vector g
* Ftipmat: An N x 6 matrix of spatial forces applied by the end-effector (if there are no tip forces
* the user should input a zero matrix)
* 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.
* thetamatd: An Nxn matrix of desired joint variables from the reference trajectory
* dthetamatd: An Nxn matrix of desired joint velocities
* ddthetamatd: An Nxn matrix of desired joint accelerations
* gtilde: The gravity vector based on the model of the actual robot (actual values given above)
* Mtildelist: The link frame locations based on the model of the actual robot (actual values given above)
* Gtildelist: The link spatial inertias based on the model of the actual robot (actual values given above)
* 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)
* dt: The timestep between points on the reference trajectory
* intRes: Integration resolution is the number of times integration (Euler) takes places between each time step.
* Must be an integer value greater than or equal to 1
*
* Outputs: std::vector of [taumat, thetamat]
* taumat: An Nxn matrix of the controllers commanded joint forces/ torques, where each row of n forces/torques
* corresponds to a single time instant
* thetamat: The N x n matrix of actual joint angles
*/
std::vector<Eigen::MatrixXd> SimulateControl(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&,
const Eigen::MatrixXd&, const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&,
const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&,
const Eigen::VectorXd&, const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&,
double, double, double, double, int);
}
}

2
include/common/utilities/CSVTool.h
View File

@ -181,7 +181,7 @@ inline void CSVTool::readFile(){
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;
std::cout << "[ERROR] No such label: " << label << std::endl;
return false;
}else{
_lines.at(_labels[label])->getValues(values);

55
include/common/utilities/loop.h Normal file
View File

@ -0,0 +1,55 @@
#ifndef _UNITREE_ARM_LOOP_H_
#define _UNITREE_ARM_LOOP_H_
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <thread>
#include <pthread.h>
#include <vector>
#include <boost/shared_ptr.hpp>
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include "common/utilities/timer.h"
// constexpr int THREAD_PRIORITY = 99; // real-time priority
typedef boost::function<void ()> Callback;
class Loop {
public:
Loop(std::string name, float period, int bindCPU = -1);
~Loop();
void start();
void shutdown();
virtual void functionCB() = 0;
private:
void entryFunc();
std::string _name;
float _period;
int _bindCPU;
bool _bind_cpu_flag = false;
bool _isrunning = false;
std::thread _thread;
size_t _runTimes = 0;
size_t _timeOutTimes = 0;
AbsoluteTimer *_timer;
};
class LoopFunc : public Loop {
public:
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)
: Loop(name, period, bindCPU), _fp(_cb){}
void functionCB() { (_fp)(); }
private:
boost::function<void ()> _fp;
};
#endif // _UNITREE_ARM_LOOP_H_

56
include/common/utilities/timer.h Normal file
View File

@ -0,0 +1,56 @@
#ifndef _UNITREE_ARM_TIMER_H_
#define _UNITREE_ARM_TIMER_H_
#include <iostream>
#include <unistd.h>
#include <stdint.h>
#include <sys/time.h>
#include <sys/timerfd.h>
//时间戳 微秒级， 需要#include <sys/time.h>
inline long long getSystemTime(){
struct timeval t;
gettimeofday(&t, NULL);
return 1000000 * t.tv_sec + t.tv_usec;
}
//时间戳 秒级， 需要getSystemTime()
inline double getTimeSecond(){
double time = getSystemTime() * 0.000001;
return time;
}
/*
等待函数，微秒级，从startTime开始等待waitTime微秒
目前不推荐使用，建议使用AbsoluteTimer类
*/
inline void absoluteWait(long long startTime, long long waitTime){
if(getSystemTime() - startTime > waitTime){
std::cout << "[WARNING] The waitTime=" << waitTime << " of function absoluteWait is not enough!" << std::endl
<< "The program has already cost " << getSystemTime() - startTime << "us." << std::endl;
}
while(getSystemTime() - startTime < waitTime){
usleep(50);
}
}
/*
waitTimeS = 0 means do not care time out
*/
class AbsoluteTimer{
public:
AbsoluteTimer(double waitTimeS);
~AbsoluteTimer();
void start();
bool wait();
private:
void _updateWaitTime(double waitTimeS);
int _timerFd;
uint64_t _missed;
double _waitTime;
double _startTime;
double _leftTime;
double _nextWaitTime;
itimerspec _timerSpec;
};
#endif

0
thirdparty/tinyxml/include/tinystr.h → include/common/utilities/tinyxml/tinystr.h
View File

0
thirdparty/tinyxml/include/tinyxml.h → include/common/utilities/tinyxml/tinyxml.h
View File

46
include/control/CtrlComponents.h
View File

@ -1,47 +1,49 @@
#ifndef CTRLCOMPONENTS_H
#define CTRLCOMPONENTS_H
#include "message/LowlevelCmd.h"
#include "message/LowlevelState.h"
#include "interface/IOInterface.h"
#include "interface/IOROS.h"
#include "model/ArmDynKineModel.h"
#include "common/utilities/CSVTool.h"
#include <string>
#include <iostream>
#include "common/math/robotics.h"
#include "common/utilities/loop.h"
#include "message/arm_common.h"
#include "message/LowlevelCmd.h"
#include "message/LowlevelState.h"
#include "common/utilities/CSVTool.h"
#include "model/ArmModel.h"
#include "interface/IOUDP.h"
#include "interface/IOROS.h"
#include "control/armSDK.h"
using namespace std;
struct CtrlComponents{
public:
CtrlComponents();
~CtrlComponents();
int dof;
std::string armConfigPath;
LowlevelCmd *lowCmd;
LowlevelState *lowState;
CmdPanel *cmdPanel;
IOInterface *ioInter;
ArmDynKineModel *armModel;
ArmModel *armModel;
CSVTool *stateCSV;
SendCmd sendCmd; // cmd that receive from SDK
RecvState recvState;// state that send to SDK
//config
Control ctrl;
bool _hasGripper;
std::string ctrl_IP;
double dt;
bool *running;
Control ctrl;
bool hasGripper;
bool isCollisionOpen;
double collisionTLimit;
bool isPlot;
void sendRecv();
void geneObj();
bool isRecord;
void writeData();
private:
Vec3 _endPosLocal;
double _endEffectorMass;
Vec3 _endEffectorCom;
Mat3 _endEffectorInertia;
std::string ctrl_IP;
uint ctrl_port;
double _loadWeight;
};
#endif // CTRLCOMPONENTS_H

24
include/control/armSDK.h Normal file
View File

@ -0,0 +1,24 @@
#ifndef _SDK_H
#define _SDK_H
#include "message/udp.h"
#include "control/cmdPanel.h"
class ARMSDK : public CmdPanel{
public:
ARMSDK(std::vector<KeyAction*> events,
EmptyAction emptyAction, const char* IP, uint port, double dt = 0.002);
~ARMSDK();
SendCmd getSendCmd();
int getState(size_t channelID = 0);
void setRecvState(RecvState& recvState);
private:
void _sendRecv();
void _read(){};
UDPPort *_udp;
SendCmd _sendCmd, _sendCmdTemp;
RecvState _recvState;
size_t _recvLength;
};
#endif

144
include/control/cmdPanel.h Normal file
View File

@ -0,0 +1,144 @@
#ifndef _CMDPANEL_H_
#define _CMDPANEL_H_
#include <vector>
#include <deque>
#include "common/utilities/loop.h"
#include "message/arm_common.h"
enum class KeyPress{
RELEASE,
PRESS,
REPEAT
};
enum class ActionType{
EMPTY,
STATE,
VALUE
};
struct KeyCmd{
std::string c;
KeyPress keyPress;
};
class KeyAction{
public:
KeyAction(ActionType type);
virtual ~KeyAction(){};
ActionType getType(){return _type;}
protected:
ActionType _type;
};
class StateAction : public KeyAction{
public:
StateAction(std::string c, int state, KeyPress press = KeyPress::PRESS);
virtual ~StateAction(){};
int getState(){return _state;};
bool handleCmd(KeyCmd keyCmd, int &state);
protected:
int _state;
KeyCmd _keyCmdSet;
};
class EmptyAction : public StateAction{
public:
EmptyAction(int state);
~EmptyAction(){};
private:
};
/*
必须为长按, deltaValue为每秒改变量
valueAction允许共用按键，例如空格停止
但是正反转、停止键不可重复
*/
class ValueAction : public KeyAction{
public:
ValueAction(std::string cUp, std::string cDown, double deltaValue, double initValue = 0.0);
ValueAction(std::string cUp, std::string cDown, std::string cGoZero, double deltaValue, double initValue = 0.0);
ValueAction(std::string cUp, std::string cDown, double deltaValue, double limit1, double limit2, double initValue = 0.0);
ValueAction(std::string cUp, std::string cDown, std::string cGoZero, double deltaValue, double limit1, double limit2, double initValue = 0.0);
~ValueAction(){};
bool handleCmd(KeyCmd keyCmd);
void setDt(double dt);
double getValue();
double getDValue();
double getDirection(){return _changeDirection;};
void setValue(double value){_value = value;}
private:
double _value;
double _changeDirection;
double _dV = 0.0; //delta value per delta time
double _dt = 0.0;
double _dVdt = 0.0; // delta value per second
double _lim1, _lim2;
bool _hasLim = false;
bool _hasGoZero = false;
KeyCmd _upCmdSet;
KeyCmd _downCmdSet;
KeyCmd _goZeroCmdSet;
};
class CmdPanel{
public:
CmdPanel(std::vector<KeyAction*> events,
EmptyAction emptyAction, size_t channelNum = 1, double dt = 0.002);
virtual ~CmdPanel();
virtual int getState(size_t channelID = 0);
std::vector<double> getValues() {return _values;};
std::vector<double> getDValues() {return _dValues;};
std::vector<double> getDirections() {return _changeDirections;};
void setValue(std::vector<double> values);
void setValue(double value, size_t id);
virtual std::string getString(std::string slogan);
virtual std::vector<double> stringToArray(std::string slogan);
virtual std::vector<std::vector<double> > stringToMatrix(std::string slogan);
virtual SendCmd getSendCmd();
virtual void setRecvState(RecvState& recvState){};
protected:
virtual void _read() = 0;
void _run();
void _updateState();
void _pressKeyboard();
void _releaseKeyboard();
LoopFunc *_runThread;
LoopFunc *_readThread;
std::vector<StateAction> _stateEvents;
std::vector<ValueAction> _valueEvents;
EmptyAction _emptyAction;
size_t _actionNum = 0;
size_t _stateNum = 0;
size_t _valueNum = 0;
size_t _channelNum;
std::vector<double> _values;
std::vector<double> _dValues;
std::vector<double> _changeDirections;
int _state;
std::vector<std::deque<int>> _stateQueue;
std::vector<int> _outputState;
std::vector<bool> _getState;
double _dt;
KeyCmd _keyCmd;
std::string _cPast = "";
bool _running = true;
};
#endif

107
include/control/joystick.h Normal file
View File

@ -0,0 +1,107 @@
#ifndef _UNITREE_ARM_JOYSTICK_H_
#define _UNITREE_ARM_JOYSTICK_H_
#include <vector>
#include "message/udp.h"
#include "control/cmdPanel.h"
#include "message/joystick_common.h"
#include "message/aliengo_common.h"
#include "message/b1_common.h"
#include <math.h>
using namespace UNITREE_LEGGED_SDK_ALIENGO;
// using namespace UNITREE_LEGGED_SDK_B1;
class UnitreeJoystick : public CmdPanel{
public:
UnitreeJoystick(std::vector<KeyAction*> events,
EmptyAction emptyAction, size_t channelNum = 1,
double dt = 0.002)
: CmdPanel(events, emptyAction, channelNum, dt){
_udp = new UDPPort("dog", "192.168.123.220", 8082, 8081, HIGH_STATE_LENGTH, BlockYN::NO, 500000);
_udpCmd = {0};
_udpState = {0};
_readThread = new LoopFunc("JoyStickRead", 0.0, boost::bind(&UnitreeJoystick::_read, this));
_runThread = new LoopFunc("CmdPanelRun", _dt, boost::bind(&UnitreeJoystick::_run, this));
_readThread->start();
_runThread->start();
};
~UnitreeJoystick(){
delete _udp;
delete _runThread;
delete _readThread;
};
private:
void _read(){
_udp->send((uint8_t*)&_udpCmd, HIGH_CMD_LENGTH);
_udp->recv((uint8_t*)&_udpState);
#ifdef CTRL_BY_ALIENGO_JOYSTICK
memcpy(&_keyData, _udpState.wirelessRemote, 40);
#else
memcpy(&_keyData, &_udpState.wirelessRemote[0], 40);
#endif
_extractCmd();
_updateState();
};
void _extractCmd(){
float joyThre = 0.5; // 手柄数值范围 +-1
if(((int)_keyData.btn.components.L1 == 1) &&
((int)_keyData.btn.components.L2 == 1)){
_keyCmd.c = "l12";
}else if((int)_keyData.btn.components.R2 == 1){
_keyCmd.c = "r2";
if((int)_keyData.btn.components.X == 1){
_keyCmd.c = "r2x";
}
}else if((int)_keyData.btn.components.R1 == 1){
_keyCmd.c = "r1";
}else if(fabs(_keyData.lx) > joyThre){
_keyData.lx > joyThre?_keyCmd.c = "left_left":_keyCmd.c = "left_right";
}else if(fabs(_keyData.ly) > joyThre){
_keyData.ly > joyThre?_keyCmd.c = "left_up":_keyCmd.c = "left_down";
}else if((int)_keyData.btn.components.up == 1){
_keyCmd.c = "up";
}else if((int)_keyData.btn.components.down == 1){
_keyCmd.c = "down";
}else if(fabs(_keyData.rx) > joyThre){
_keyData.rx > joyThre?_keyCmd.c = "right_left":_keyCmd.c = "right_right";
}else if(fabs(_keyData.ry) > joyThre){
_keyData.ry > joyThre?_keyCmd.c = "right_up":_keyCmd.c = "right_down";
}else if((int)_keyData.btn.components.Y == 1){
_keyCmd.c = "Y";
}else if((int)_keyData.btn.components.A == 1){
_keyCmd.c = "A";
}else if((int)_keyData.btn.components.X == 1){
_keyCmd.c = "X";
}else if((int)_keyData.btn.components.B == 1){
_keyCmd.c = "B";
}else if((int)_keyData.btn.components.right == 1){
_keyCmd.c = "left";
}else if((int)_keyData.btn.components.left == 1){
_keyCmd.c = "right";
}else if((int)_keyData.btn.components.up == 1){
_keyCmd.c = "up";
}else if((int)_keyData.btn.components.down == 1){
_keyCmd.c = "down";
}else if((int)_keyData.btn.components.select == 1){
_keyCmd.c = "select";
}else{
_releaseKeyboard();
return;
}
_pressKeyboard();
};
xRockerBtnDataStruct _keyData;
UDPPort *_udp;
HighCmd _udpCmd;
HighState _udpState;
};
#endif // _UNITREE_ARM_JOYSTICK_H_

34
include/control/keyboard.h Normal file
View File

@ -0,0 +1,34 @@
#ifndef _UNITREE_ARM_KEYBOARD_H_
#define _UNITREE_ARM_KEYBOARD_H_
#include <string>
#include <vector>
#include <deque>
#include "message/udp.h"
#include "common/enumClass.h"
#include "control/cmdPanel.h"
class Keyboard : public CmdPanel{
public:
Keyboard(std::vector<KeyAction*> events,
EmptyAction emptyAction, size_t channelNum = 1, double dt = 0.002);
~Keyboard();
std::string getString(std::string slogan);
std::vector<double> stringToArray(std::string slogan);
std::vector<std::vector<double> > stringToMatrix(std::string slogan);
private:
void _read();
void _pauseKey();
void _startKey();
void _extractCmd();
fd_set _set;
char _c = '\0';
termios _oldSettings;
termios _newSettings;
timeval _tv;
};
#endif // _UNITREE_ARM_KEYBOARD_H_

33
include/interface/IOInterface.h
View File

@ -1,34 +1,29 @@
#ifndef IOINTERFACE_H
#define IOINTERFACE_H
#include <string>
#include "message/LowlevelCmd.h"
#include "message/LowlevelState.h"
#include "unitree_arm_sdk/cmdPanel.h"
#include "unitree_arm_sdk/keyboard.h"
#include <string>
#include "control/keyboard.h"
#include "control/joystick.h"
#include "common/math/robotics.h"
class IOInterface{
public:
IOInterface(CmdPanel *cmdPanel):_cmdPanel(cmdPanel){}
virtual ~IOInterface(){delete _cmdPanel;};
IOInterface(){}
~IOInterface(){
delete lowCmd;
delete lowState;
};
virtual bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state) = 0;
// void zeroCmdPanel(){_cmdPanel->setZero();}
std::string getString(std::string slogan){return _cmdPanel->getString(slogan);}
std::vector<double> stringToArray(std::string slogan){return _cmdPanel->stringToArray(slogan);}
std::vector<std::vector<double> > stringToMatrix(std::string slogan){return _cmdPanel->stringToMatrix(slogan);}
virtual bool calibration(){return false;};
SendCmd getSendCmd(){return _cmdPanel->getSendCmd();};
void getRecvState(RecvState recvState){ _cmdPanel->getRecvState(recvState);};
bool isDisConnect = false;// udp disconnection
bool _isDisConnect[7];// joint(motor) disconnection
bool checkGripper(){return hasGripper;};
LowlevelCmd *lowCmd;
LowlevelState *lowState;
virtual bool isDisconnect(){ return false;};
bool hasErrorState;
protected:
uint16_t _isDisConnectCnt[7];
CmdPanel *_cmdPanel;
bool hasGripper;
};
#endif //IOINTERFACE_H

15
include/interface/IOROS.h
View File

@ -5,29 +5,20 @@
#include <ros/ros.h>
#include "interface/IOInterface.h"
#include "unitree_arm_sdk/keyboard.h"
#include "message/unitree_legged_msgs/MotorCmd.h"
#include "message/unitree_legged_msgs/MotorState.h"
#include "message/MotorCmd.h"
#include "message/MotorState.h"
class IOROS : public IOInterface{
public:
IOROS(CmdPanel *cmdPanel, bool hasGripper);
IOROS();
~IOROS();
bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state);
private:
bool _hasGripper;
ros::NodeHandle _nm;
// #ifdef UNITREE_GRIPPER
ros::Subscriber _servo_sub[7];
ros::Publisher _servo_pub[7];
unitree_legged_msgs::MotorState _joint_state[7];
unitree_legged_msgs::MotorCmd _joint_cmd[7];
// #else
// ros::Subscriber _servo_sub[6];
// ros::Publisher _servo_pub[6];
// unitree_legged_msgs::MotorState _joint_state[6];
// unitree_legged_msgs::MotorCmd _joint_cmd[6];
// #endif
void _sendCmd(const LowlevelCmd *cmd);
void _recvState(LowlevelState *state);
void _initRecv();

11
include/interface/IOUDP.h
View File

@ -2,26 +2,23 @@
#define IOUDP_H
#include "interface/IOInterface.h"
// #include "/usr/include/x86_64-linux-gnu/bits/floatn-common.h"
class IOUDP : public IOInterface{
public:
IOUDP(CmdPanel *cmdPanel, const char* IP, bool hasGripper);
IOUDP(const char* IP, uint port);
~IOUDP();
bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state);
// void setGripper(int position, int force);
// void getGripper(int &position, int &force);
bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state);
bool calibration();
bool isDisconnect(){ return _ioPort->isDisConnect;}
private:
IOPort *_ioPort;
UDPSendCmd _cmd;
UDPRecvState _state;
UDPRecvStateOld _stateOld;
size_t _motorNum;
size_t _jointNum;
uint8_t _singleState;
uint8_t _selfCheck[10];
};

19
include/message/LowlevelCmd.h
View File

@ -7,8 +7,6 @@
#include <iostream>
struct LowlevelCmd{
private:
size_t _dof = 6;
public:
std::vector<double> q;
std::vector<double> dq;
@ -16,9 +14,10 @@ public:
std::vector<double> kp;
std::vector<double> kd;
std::vector<std::vector<double>> q_cmd_data;
std::vector<std::vector<double>> dq_cmd_data;
std::vector<std::vector<double>> tau_cmd_data;
std::vector<std::vector<double>> q_data;
std::vector<std::vector<double>> dq_data;
std::vector<std::vector<double>> tauf_data;
std::vector<std::vector<double>> tau_data;
LowlevelCmd();
~LowlevelCmd(){}
@ -27,13 +26,14 @@ public:
void setZeroTau();
void setZeroKp();
void setZeroKd();
void setControlGain();
void setControlGain(std::vector<float> KP, std::vector<float> KW);
void setQ(std::vector<double> qInput);
void setQ(VecX qInput);
void setQd(VecX qDInput);
void setTau(VecX tauInput);
void setControlGain();
void setControlGain(std::vector<float> KP, std::vector<float> KW);
void setPassive();
void setGripperGain();
void setGripperGain(float KP, float KW);
void setGripperZeroGain();
@ -43,10 +43,13 @@ public:
double getGripperQd();
void setGripperTau(double tauInput);
double getGripperTau();
Vec6 getQ();
Vec6 getQd();
void resizeGripper();
private:
size_t _dof = 6;
};

37
include/message/LowlevelState.h
View File

@ -3,55 +3,58 @@
#include <iostream>
#include <vector>
#include "common/math/mathTypes.h"
#include "common/math/mathTools.h"
#include "common/enumClass.h"
#include "common/utilities/LPFilter.h"
#include "message/UserValue.h"
#include "common/math/Filter.h"
struct LowlevelState{
private:
size_t _dof = 6;
public:
LowlevelState(double dt);
~LowlevelState();
// MotorState motorState[12];
std::vector<double> q;
std::vector<double> dq;
std::vector<double> ddq;
std::vector<double> tau;
std::vector<float> temperature;
std::vector<std::vector<double>> q_data;
std::vector<std::vector<double>> dq_data;
std::vector<std::vector<double>> ddq_data;
std::vector<std::vector<double>> tau_data;
std::vector<int> temperature;
std::vector<uint8_t> errorstate;
std::vector<uint8_t> isMotorConnected;
std::vector<double> qFiltered;
std::vector<double> dqFiltered;
std::vector<double> ddqFiltered;
std::vector<double> tauFiltered;
std::vector<std::vector<double>> q_state_data;
std::vector<std::vector<double>> dq_state_data;
std::vector<std::vector<double>> tau_state_data;
// ArmFSMStateName userCmd;
UserValue userValue;
LPFilter *qFilter;
LPFilter *dqFilter;
LPFilter *ddqFilter;
LPFilter *tauFilter;
void resizeGripper(double dt);
void runFilter();
bool checkError();
Vec6 getQ();
Vec6 getQd();
Vec6 getQdd();
Vec6 getTau();
Vec6 getQFiltered();
Vec6 getQdFiltered();
Vec6 getQddFiltered();
Vec6 getTauFiltered();
double getGripperQ() {return q.at(q.size()-1);}
double getGripperQd() {return dq.at(q.size()-1);}
double getGripperTau() {return tau.at(tau.size()-1);}
double getGripperTauFiltered() {return tauFiltered.at(tau.size()-1);}
double getGripperQ();
double getGripperQd();
double getGripperTau();
double getGripperTauFiltered();
private:
size_t _dof = 6;
int temporatureLimit = 80.0;// centigrade
std::vector<int> _isMotorConnectedCnt;
};
#endif //LOWLEVELSTATE_HPP

0
include/message/unitree_legged_msgs/MotorCmd.h → include/message/MotorCmd.h
View File

0
include/message/unitree_legged_msgs/MotorState.h → include/message/MotorState.h
View File

38
include/message/UserValue.h
View File

@ -1,38 +0,0 @@
#ifndef USERVALUE_H
#define USERVALUE_H
#include "common/math/mathTypes.h"
#include "common/utilities/typeTrans.h"
#include <vector>
template<typename T>
std::vector<T> cutVector(std::vector<T> vec, size_t startID, size_t length){
std::vector<T> result;
result.assign(vec.begin()+startID, vec.begin()+startID+length);
return result;
}
struct UserValue{
Vec6 moveAxis;
double gripperPos;
void setData(std::vector<double> rawData){
if(rawData.size() != 7){
std::cout << "[ERROR] UserValue::setData, the size of rawDate is " << rawData.size() << " but not 7" << std::endl;
}
gripperPos = rawData.at(6);
rawData = cutVector(rawData, 0, 6);
moveAxis = typeTrans::getValue(rawData, moveAxis);
}
UserValue(){
setZero();
}
void setZero(){
moveAxis.setZero();
gripperPos = 0;
// gripperTau = 0;
}
};
#endif

168
include/message/aliengo_common.h Normal file
View File

@ -0,0 +1,168 @@
#ifndef _UNITREE_ARM_ALIENGO_COMMON_H_
#define _UNITREE_ARM_ALIENGO_COMMON_H_
#include <stdint.h>
namespace UNITREE_LEGGED_SDK_ALIENGO
{
constexpr int HIGHLEVEL = 0x00;
constexpr int LOWLEVEL = 0xff;
constexpr double PosStopF = (2.146E+9f);
constexpr double VelStopF = (16000.0f);
#pragma pack(1)
// 12 bytes
typedef struct
{
float x;
float y;
float z;
} Cartesian;
// 53 bytes
typedef struct
{
float quaternion[4]; // quaternion, normalized, (w,x,y,z)
float gyroscope[3]; // angular velocity （unit: rad/s)
float accelerometer[3]; // m/(s2)
float rpy[3]; // euler angle（unit: rad)
int8_t temperature;
} IMU; // when under accelerated motion, the attitude of the robot calculated by IMU will drift.
// 3 bytes
typedef struct
{
uint8_t r;
uint8_t g;
uint8_t b;
} LED; // foot led brightness: 0~255
// 38 bytes
typedef struct
{
uint8_t mode; // motor working mode
float q; // current angle (unit: radian)
float dq; // current velocity (unit: radian/second)
float ddq; // current acc (unit: radian/second*second)
float tauEst; // current estimated output torque (unit: N.m)
float q_raw; // current angle (unit: radian)
float dq_raw; // current velocity (unit: radian/second)
float ddq_raw;
int8_t temperature; // current temperature (temperature conduction is slow that leads to lag)
uint32_t reserve[2]; // in reserve[1] returns the brake state.
} MotorState; // motor feedback
// 33 bytes
typedef struct
{
uint8_t mode; // desired working mode
float q; // desired angle (unit: radian)
float dq; // desired velocity (unit: radian/second)
float tau; // desired output torque (unit: N.m)
float Kp; // desired position stiffness (unit: N.m/rad )
float Kd; // desired velocity stiffness (unit: N.m/(rad/s) )
uint32_t reserve[3]; // in reserve[0] sends the brake cmd.
} MotorCmd; // motor control
// 891 bytes
typedef struct
{
uint8_t levelFlag; // flag to distinguish high level or low level
uint16_t commVersion;
uint16_t robotID;
uint32_t SN;
uint8_t bandWidth;
IMU imu;
MotorState motorState[20];
int16_t footForce[4]; // force sensors
int16_t footForceEst[4]; // force sensors
uint32_t tick; // reference real-time from motion controller (unit: us)
uint8_t wirelessRemote[40]; // wireless commands
uint32_t reserve;
uint32_t crc;
} LowState; // low level feedback
// 730 bytes
typedef struct
{
uint8_t levelFlag;
uint16_t commVersion;
uint16_t robotID;
uint32_t SN;
uint8_t bandWidth;
MotorCmd motorCmd[20];
LED led[4];
uint8_t wirelessRemote[40];
uint32_t reserve;
uint32_t crc;
} LowCmd; // low level control
// 244byte
typedef struct
{
uint8_t levelFlag;
uint16_t commVersion;
uint16_t robotID;
uint32_t SN;
uint8_t bandWidth;
uint8_t mode;
IMU imu;
float position[3]; // (unit: m), from robot own odometry in inertial frame, usually drift
float velocity[3]; // (unit: m/s), forwardSpeed, sideSpeed, updownSpeed in body frame
float yawSpeed; // (unit: rad/s), rotateSpeed in body frame.
Cartesian footPosition2Body[4]; // foot position relative to body
Cartesian footSpeed2Body[4]; // foot speed relative to body
int16_t footForce[4];
uint8_t wirelessRemote[40];
uint32_t reserve;
uint32_t crc;
} HighState; // high level feedback
//113byte
typedef struct
{
uint8_t levelFlag;
uint16_t commVersion;
uint16_t robotID;
uint32_t SN;
uint8_t bandWidth;
uint8_t mode; // 0.idle, default stand | 1.force stand (controlled by dBodyHeight + rpy)
// 2.target velocity walking (controlled by velocity + yawSpeed)
// 3.target position walking (controlled by position + rpy[2])
// 4. path mode walking (reserve for future release)
// 5. position stand down. |6. position stand up |7. damping mode | 8. recovery mode
uint8_t gaitType; // 0.trot | 1. trot running | 2.climb stair
uint8_t speedLevel; // 0. default low speed. 1. medium speed 2. high speed. during walking
float dFootRaiseHeight; // (unit: m), swing foot height adjustment from default swing height.
float dBodyHeight; // (unit: m), body height adjustment from default body height.
float position[2]; // (unit: m), desired x and y position in inertial frame.
float rpy[3]; // (unit: rad), desired yaw-pitch-roll euler angle, expressed in roll(rpy[0]) pitch(rpy[1]) yaw(rpy[2])
float velocity[2]; // (unit: m/s), forwardSpeed, sideSpeed in body frame.
float yawSpeed; // (unit: rad/s), rotateSpeed in body frame.
LED led[4];
uint8_t wirelessRemote[40];
uint32_t reserve;
uint32_t crc;
} HighCmd; // high level control uint8_t mode
#pragma pack()
typedef struct
{
unsigned long long TotalCount; // total loop count
unsigned long long SendCount; // total send count
unsigned long long RecvCount; // total receive count
unsigned long long SendError; // total send error
unsigned long long FlagError; // total flag error
unsigned long long RecvCRCError; // total reveive CRC error
unsigned long long RecvLoseError; // total lose package count
} UDPState; // UDP communication state
constexpr int HIGH_CMD_LENGTH = (sizeof(HighCmd));
constexpr int HIGH_STATE_LENGTH = (sizeof(HighState));
}
#endif

150
include/message/arm_common.h Normal file
View File

@ -0,0 +1,150 @@
#ifndef _UNITREE_ARM_ARM_COMMON_H_
#define _UNITREE_ARM_ARM_COMMON_H_
#include <stdint.h>
#pragma pack(1)
// 4 Byte
enum class ArmFSMState{
INVALID,
PASSIVE,
JOINTCTRL,
CARTESIAN,
MOVEJ,
MOVEL,
MOVEC,
TRAJECTORY,
TOSTATE,
SAVESTATE,
TEACH,
TEACHREPEAT,
CALIBRATION,
SETTRAJ,
BACKTOSTART,
NEXT,
LOWCMD
};
// 4 Byte
enum class ArmFSMValue{
INVALID,
Q,A,
W,S,
E,D,
R,F,
T,G,
Y,H,
DOWN,
UP
};
enum class TrajType{
MoveJ,
MoveL,
MoveC,
Stop
};
// 20 Byte
struct JointCmd{
float T;
float W;
float Pos;
float K_P;
float K_W;
};
typedef struct{
uint8_t reserved : 6 ;
uint8_t state : 2 ;//whether motor is connected; 0-ok, 1-disconnected, 2-CRC error
}Motor_Connected;
typedef struct{
int8_t temperature;
/* 0x01: phase current is too large
* 0x02: phase leakage
* 0x04: overheat(including the motor windings and the motor shell)
* 0x20: jumped
*/
uint8_t error;
Motor_Connected isConnected;
}Motor_State;
struct JointStateOld{//no error state return
float T;
float W;
float Acc;
float Pos;
};
struct JointState{
float T;
float W;
float Acc;
float Pos;
Motor_State state[2];
};
//140bytes
union UDPSendCmd{
uint8_t checkCmd;
JointCmd jointCmd[7];
};
struct UDPRecvStateOld{
JointStateOld jointStateOld[7];
};
struct UDPRecvState{
JointState jointState[7];
};
struct Posture{
double roll;
double pitch;
double yaw;
double x;
double y;
double z;
};
struct TrajCmd{
TrajType trajType;
Posture posture[2];
double gripperPos;
double maxSpeed;
double stopTime;
int trajOrder;
};
union ValueUnion{
char name[10];
JointCmd jointCmd[7];
TrajCmd trajCmd;
};
struct SendCmd{
uint8_t head[2];
ArmFSMState state;
bool track;// whether let arm track jointCmd in State_JOINTCTRL or posture[0] in State_CARTESIAN
ValueUnion valueUnion;
};
struct RecvState{
uint8_t head[2];
ArmFSMState state;
JointState jointState[7];
Posture cartesianState;
};
constexpr int SENDCMD_LENGTH = (sizeof(SendCmd));
constexpr int RECVSTATE_LENGTH = (sizeof(RecvState));
constexpr int JointCmd_LENGTH = (sizeof(JointCmd));
constexpr int JointState_LENGTH = (sizeof(JointState));
constexpr int JointStateOld_LENGTH = (sizeof(JointStateOld));
#pragma pack()
#endif // _UNITREE_ARM_ARM_MSG_H_

214
include/message/arm_motor_common.h Normal file
View File

@ -0,0 +1,214 @@
#ifndef _UNITREE_ARM_ARM_MOTOR_COMMON_H_
#define _UNITREE_ARM_ARM_MOTOR_COMMON_H_
#include <stdint.h>
typedef int16_t q15_t;
#pragma pack(1)
// 发送用单个数据数据结构
typedef union{
int32_t L;
uint8_t u8[4];
uint16_t u16[2];
uint32_t u32;
float F;
}COMData32;
typedef struct {
// 定义 数据包头
unsigned char start[2]; // 包头
unsigned char motorID; // 电机ID 0,1,2,3 ... 0xBB 表示向所有电机广播（此时无返回）
unsigned char reserved;
}COMHead;
#pragma pack()
#pragma pack(1)
typedef struct {
uint8_t fan_d; // 关节上的散热风扇转速
uint8_t Fmusic; // 电机发声频率 /64*1000 15.625f 频率分度
uint8_t Hmusic; // 电机发声强度 推荐值4 声音强度 0.1 分度
uint8_t reserved4;
uint8_t FRGB[4]; // 足端LED
}LowHzMotorCmd;
typedef struct { // 以 4个字节一组排列 ，不然编译器会凑整
// 定义 数据
uint8_t mode; // 关节模式选择
uint8_t ModifyBit; // 电机控制参数修改位
uint8_t ReadBit; // 电机控制参数发送位
uint8_t reserved;
COMData32 Modify; // 电机参数修改 的数据
//实际给FOC的指令力矩为：
//K_P*delta_Pos + K_W*delta_W + T
q15_t T; // 期望关节的输出力矩（电机本身的力矩）x256, 7 + 8 描述
q15_t W; // 期望关节速度 （电机本身的速度） x128, 8 + 7描述
int32_t Pos; // 期望关节位置 x 16384/6.2832, 14位编码器（主控0点修正，电机关节还是以编码器0点为准）
q15_t K_P; // 关节刚度系数 x2048 4+11 描述
q15_t K_W; // 关节速度系数 x1024 5+10 描述
uint8_t LowHzMotorCmdIndex; // 电机低频率控制命令的索引, 0-7, 分别代表LowHzMotorCmd中的8个字节
uint8_t LowHzMotorCmdByte; // 电机低频率控制命令的字节
COMData32 Res[1]; // 通讯 保留字节 用于实现别的一些通讯内容
}MasterComdV3; // 加上数据包的包头 和CRC 34字节
typedef struct {
// 定义 电机控制命令数据包
COMHead head;
MasterComdV3 Mdata;
COMData32 CRCdata;
}MasterComdDataV3;//返回数据
// typedef struct {
// // 定义 总得485 数据包
// MasterComdData M1;
// MasterComdData M2;
// MasterComdData M3;
// }DMA485TxDataV3;
#pragma pack()
#pragma pack(1)
typedef struct { // 以 4个字节一组排列 ，不然编译器会凑整
// 定义 数据
uint8_t mode; // 当前关节模式
uint8_t ReadBit; // 电机控制参数修改 是否成功位
int8_t Temp; // 电机当前平均温度
uint8_t MError; // 电机错误 标识
COMData32 Read; // 读取的当前 电机 的控制数据
int16_t T; // 当前实际电机输出力矩 7 + 8 描述
int16_t W; // 当前实际电机速度（高速） 8 + 7 描述
float LW; // 当前实际电机速度（低速）
int16_t W2; // 当前实际关节速度（高速） 8 + 7 描述
float LW2; // 当前实际关节速度（低速）
int16_t Acc; // 电机转子加速度 15+0 描述 惯量较小
int16_t OutAcc; // 输出轴加速度 12+3 描述 惯量较大
int32_t Pos; // 当前电机位置（主控0点修正，电机关节还是以编码器0点为准）
int32_t Pos2; // 关节编码器位置(输出编码器)
int16_t gyro[3]; // 电机驱动板6轴传感器数据
int16_t acc[3];
// 力传感器的数据
int16_t Fgyro[3]; //
int16_t Facc[3];
int16_t Fmag[3];
uint8_t Ftemp; // 8位表示的温度 7位（-28~100度） 1位0.5度分辨率
int16_t Force16; // 力传感器高16位数据
int8_t Force8; // 力传感器低8位数据
uint8_t FError; // 足端传感器错误标识
int8_t Res[1]; // 通讯 保留字节
}ServoComdV3; // 加上数据包的包头 和CRC 78字节（4+70+4）
typedef struct {
// 定义 电机控制命令数据包
COMHead head;
ServoComdV3 Mdata;
COMData32 CRCdata;
}ServoComdDataV3; //发送数据
// typedef struct {
// // 定义 总的485 接受数据包
// ServoComdDataV3 M[3];
// // uint8_t nullbyte1;
// }DMA485RxDataV3;
#pragma pack()
// 00 00 00 00 00
// 00 00 00 00 00
// 00 00 00 00 00
// 00 00 00
// 数据包默认初始化
// 主机发送的数据包
/*
Tx485Data[_FR][i].head.start[0] = 0xFE ; Tx485Data[_FR][i].head.start[1] = 0xEE; // 数据包头
Tx485Data[_FR][i].Mdata.ModifyBit = 0xFF; Tx485Data[_FR][i].Mdata.mode = 0; // 默认不修改数据 和 电机的默认工作模式
Tx485Data[_FR][i].head.motorID = i; 0 // 目标电机标号
Tx485Data[_FR][i].Mdata.T = 0.0f; // 默认目标关节输出力矩 motor1.Extra_Torque = motorRxData[1].Mdata.T*0.390625f; // N.M 转化为 N.CM IQ8描述
Tx485Data[_FR][i].Mdata.Pos = 0x7FE95C80; // 默认目标关节位置 不启用位置环 14位分辨率
Tx485Data[_FR][i].Mdata.W = 16000.0f; // 默认目标关节速度 不启用速度环 1+8+7描述 motor1.Target_Speed = motorRxData[1].Mdata.W*0.0078125f; // 单位 rad/s IQ7描述
Tx485Data[_FR][i].Mdata.K_P = (q15_t)(0.6f*(1<<11)); // 默认关节刚度系数 4+11 描述 motor1.K_Pos = ((float)motorRxData[1].Mdata.K_P)/(1<<11); // 描述刚度的通讯数据格式 4+11
Tx485Data[_FR][i].Mdata.K_W = (q15_t)(1.0f*(1<<10)); // 默认关节速度系数 5+10 描述 motor1.K_Speed = ((float)motorRxData[1].Mdata.K_W)/(1<<10); // 描述阻尼的通讯数据格式 5+10
*/
enum class MotorType{
A1Go1, // 4.8M baudrate, K_W x1024
B1 // 6.0M baudrate, K_W x512
};
struct MOTOR_send{
// 定义 发送格式化数据
MasterComdDataV3 motor_send_data; //电机控制数据结构体
MotorType motorType = MotorType::A1Go1;
int hex_len = 34; //发送的16进制命令数组长度, 34
// long long send_time; //发送该命令的时间, 微秒(us)
// 待发送的各项数据
unsigned short id; //电机ID，0xBB代表全部电机
unsigned short mode; //0:空闲, 5:开环转动, 10:闭环FOC控制
//实际给FOC的指令力矩为：
//K_P*delta_Pos + K_W*delta_W + T
float T; //期望关节的输出力矩（电机本身的力矩）（Nm）
float W; //期望关节速度（电机本身的速度）(rad/s)
float Pos; //期望关节位置（rad）
float K_P; //关节刚度系数
float K_W; //关节速度系数
COMData32 Res; // 通讯 保留字节 用于实现别的一些通讯内容
};
struct MOTOR_recv{
// 定义 接收数据
ServoComdDataV3 motor_recv_data; //电机接收数据结构体
MotorType motorType = MotorType::A1Go1;
int hex_len = 78; //接收的16进制命令数组长度, 78
// long long resv_time; //接收该命令的时间, 微秒(us)
bool correct = false; //接收数据是否完整（true完整，false不完整）
//解读得出的电机数据
unsigned char motor_id; //电机ID
unsigned char mode; //0:空闲, 5:开环转动, 10:闭环FOC控制
int Temp; //温度
unsigned char MError; //错误码
float T; // 当前实际电机输出力矩
float W; // 当前实际电机速度（高速）
float LW; // 当前实际电机速度（低速）
int Acc; // 电机转子加速度
float Pos; // 当前电机位置（主控0点修正，电机关节还是以编码器0点为准）
float gyro[3]; // 电机驱动板6轴传感器数据
float acc[3];
int8_t Res;
};
extern void modify_data(MOTOR_send* motor_s);
extern bool extract_data(MOTOR_recv* motor_r);
#endif

211
include/message/b1_common.h Normal file
View File

@ -0,0 +1,211 @@
#ifndef _UNITREE_ARM_B1_COMMON_H_
#define _UNITREE_ARM_B1_COMMON_H_
#include <stdint.h>
#include <array>
namespace UNITREE_LEGGED_SDK_B1
{
constexpr int HIGHLEVEL = 0xee;
constexpr int LOWLEVEL = 0xff;
constexpr int TRIGERLEVEL = 0xf0;
constexpr double PosStopF = (2.146E+9f);
constexpr double VelStopF = (16000.0f);
extern const int LOW_CMD_LENGTH; // shorter than sizeof(LowCmd), bytes compressed LowCmd length
extern const int LOW_STATE_LENGTH; // shorter than sizeof(LowState), bytes compressed LowState length
#pragma pack(1)
typedef struct
{
uint8_t off; // off 0xA5
std::array<uint8_t, 3> reserve;
} BmsCmd;
typedef struct
{
uint8_t version_h;
uint8_t version_l;
uint8_t bms_status;
uint8_t SOC; // SOC 0-100%
int32_t current; // mA
uint16_t cycle;
std::array<int8_t, 8> BQ_NTC; // x1 degrees centigrade
std::array<int8_t, 8> MCU_NTC; // x1 degrees centigrade
std::array<uint16_t, 30> cell_vol; // cell voltage mV
} BmsState;
typedef struct
{
float x;
float y;
float z;
} Cartesian;
typedef struct
{
std::array<float, 4> quaternion; // quaternion, normalized, (w,x,y,z)
std::array<float, 3> gyroscope; // angular velocity （unit: rad/s) (raw data)
std::array<float, 3> accelerometer; // m/(s2) (raw data)
std::array<float, 3> rpy; // euler angle（unit: rad)
int8_t temperature;
} IMU; // when under accelerated motion, the attitude of the robot calculated by IMU will drift.
typedef struct
{
uint8_t r;
uint8_t g;
uint8_t b;
} LED; // foot led brightness: 0~255
typedef struct
{
uint8_t mode; // motor working mode
float q; // current angle (unit: radian)
float dq; // current velocity (unit: radian/second)
float ddq; // current acc (unit: radian/second*second)
float tauEst; // current estimated output torque (unit: N.m)
float q_raw; // current angle (unit: radian)
float dq_raw; // current velocity (unit: radian/second)
float ddq_raw;
int8_t temperature; // current temperature (temperature conduction is slow that leads to lag)
std::array<uint32_t, 2> reserve;
} MotorState; // motor feedback
typedef struct
{
uint8_t mode; // desired working mode
float q; // desired angle (unit: radian)
float dq; // desired velocity (unit: radian/second)
float tau; // desired output torque (unit: N.m)
float Kp; // desired position stiffness (unit: N.m/rad )
float Kd; // desired velocity stiffness (unit: N.m/(rad/s) )
std::array<uint32_t, 3> reserve;
} MotorCmd; // motor control
typedef struct
{
std::array<uint8_t, 2> head;
uint8_t levelFlag;
uint8_t frameReserve;
std::array<uint32_t, 2> SN;
std::array<uint32_t, 2> version;
uint16_t bandWidth;
IMU imu;
std::array<MotorState, 20> motorState;
BmsState bms;
std::array<int16_t, 4> footForce; // force sensors
std::array<int16_t, 4> footForceEst; // force sensors
uint32_t tick; // reference real-time from motion controller (unit: us)
std::array<uint8_t, 40> wirelessRemote; // wireless commands
uint32_t reserve;
uint32_t crc;
} LowState; // low level feedback
typedef struct
{
std::array<uint8_t, 2> head;
uint8_t levelFlag;
uint8_t frameReserve;
std::array<uint32_t, 2> SN;
std::array<uint32_t, 2> version;
uint16_t bandWidth;
std::array<MotorCmd, 20> motorCmd;
BmsCmd bms;
std::array<uint8_t, 40> wirelessRemote;
uint32_t reserve;
uint32_t crc;
} LowCmd; // low level control
typedef struct
{
std::array<uint8_t, 2> head;
uint8_t levelFlag;
uint8_t frameReserve;
std::array<uint32_t, 2> SN;
std::array<uint32_t, 2> version;
uint16_t bandWidth;
IMU imu;
std::array<MotorState, 20> motorState;
BmsState bms;
std::array<int16_t, 4> footForce;
std::array<int16_t, 4> footForceEst;
uint8_t mode;
float progress;
uint8_t gaitType; // 0.idle 1.trot 2.trot running 3.climb stair 4.trot obstacle
float footRaiseHeight; // (unit: m, default: 0.08m), foot up height while walking
std::array<float, 3> position; // (unit: m), from own odometry in inertial frame, usually drift
float bodyHeight; // (unit: m, default: 0.28m),
std::array<float, 3> velocity; // (unit: m/s), forwardSpeed, sideSpeed, rotateSpeed in body frame
float yawSpeed; // (unit: rad/s), rotateSpeed in body frame
std::array<float, 4> rangeObstacle;
std::array<Cartesian, 4> footPosition2Body; // foot position relative to body
std::array<Cartesian, 4> footSpeed2Body; // foot speed relative to body
std::array<uint8_t, 40> wirelessRemote;
uint32_t reserve;
uint32_t crc;
} HighState; // high level feedback
typedef struct
{
std::array<uint8_t, 2> head;
uint8_t levelFlag;
uint8_t frameReserve;
std::array<uint32_t, 2> SN;
std::array<uint32_t, 2> version;
uint16_t bandWidth;
uint8_t mode; // 0. idle, default stand 1. force stand (controlled by dBodyHeight + ypr)
// 2. target velocity walking (controlled by velocity + yawSpeed)
// 3. target position walking (controlled by position + ypr[0])
// 4. path mode walking (reserve for future release)
// 5. position stand down.
// 6. position stand up
// 7. damping mode
// 8. recovery stand
// 9. backflip
// 10. jumpYaw
// 11. straightHand
// 12. dance1
// 13. dance2
uint8_t gaitType; // 0.idle 1.trot 2.trot running 3.climb stair 4.trot obstacle
uint8_t speedLevel; // 0. default low speed. 1. medium speed 2. high speed. during walking, only respond MODE 3
float footRaiseHeight; // (unit: m, default: 0.08m), foot up height while walking, delta value
float bodyHeight; // (unit: m, default: 0.28m), delta value
std::array<float, 2> position; // (unit: m), desired position in inertial frame
std::array<float, 3> euler; // (unit: rad), roll pitch yaw in stand mode
std::array<float, 2> velocity; // (unit: m/s), forwardSpeed, sideSpeed in body frame
float yawSpeed; // (unit: rad/s), rotateSpeed in body frame
BmsCmd bms;
std::array<LED, 4> led;
std::array<uint8_t, 40> wirelessRemote;
uint32_t reserve;
uint32_t crc;
} HighCmd; // high level control
#pragma pack()
typedef struct
{
unsigned long long TotalCount; // total loop count
unsigned long long SendCount; // total send count
unsigned long long RecvCount; // total receive count
unsigned long long SendError; // total send error
unsigned long long FlagError; // total flag error
unsigned long long RecvCRCError; // total reveive CRC error
unsigned long long RecvLoseError; // total lose package count
} UDPState; // UDP communication state
constexpr int HIGH_CMD_LENGTH = (sizeof(HighCmd));
constexpr int HIGH_STATE_LENGTH = (sizeof(HighState));
}
#endif

43
include/message/joystick_common.h Normal file
View File

@ -0,0 +1,43 @@
/*****************************************************************
Copyright (c) 2022, Unitree Robotics.Co.Ltd. All rights reserved.
*****************************************************************/
#ifndef _UNITREE_ARM_JOYSTICK_COMMON_H_
#define _UNITREE_ARM_JOYSTICK_COMMON_H_
#include <stdint.h>
// 16b
typedef union {
struct {
uint8_t R1 :1;
uint8_t L1 :1;
uint8_t start :1;
uint8_t select :1;
uint8_t R2 :1;
uint8_t L2 :1;
uint8_t F1 :1;
uint8_t F2 :1;
uint8_t A :1;
uint8_t B :1;
uint8_t X :1;
uint8_t Y :1;
uint8_t up :1;
uint8_t right :1;
uint8_t down :1;
uint8_t left :1;
} components;
uint16_t value;
} xKeySwitchUnion;
// 40 Byte (now used 24B)
typedef struct {
uint8_t head[2];
xKeySwitchUnion btn;
float lx;
float rx;
float ry;
float L2;
float ly;
uint8_t idle[16];
} xRockerBtnDataStruct;
#endif

69
include/message/udp.h Normal file
View File

@ -0,0 +1,69 @@
#ifndef _UDP_H
#define _UDP_H
#include <arpa/inet.h>
#include <termios.h>
#include <string>
#include <string.h>
#include <vector>
#include "common/utilities/timer.h"
#include "message/arm_motor_common.h"
enum class BlockYN{
YES,
NO
};
class IOPort{
public:
IOPort(std::string name, BlockYN blockYN, size_t recvLength, size_t timeOutUs)
:_name(name){
resetIO(blockYN, recvLength, timeOutUs);
}
virtual ~IOPort(){}
virtual size_t send(uint8_t *sendMsg, size_t sendLength) = 0;
virtual size_t recv(uint8_t *recvMsg, size_t recvLength) = 0;
virtual size_t recv(uint8_t *recvMsg) = 0;
virtual bool sendRecv(std::vector<MOTOR_send> &sendVec, std::vector<MOTOR_recv> &recvVec) = 0;
void resetIO(BlockYN blockYN, size_t recvLength, size_t timeOutUs);
bool isDisConnect = false;
protected:
std::string _name;
BlockYN _blockYN = BlockYN::NO;
size_t _recvLength;
timeval _timeout;
timeval _timeoutSaved;
uint16_t _isDisConnectCnt;
};
class UDPPort : public IOPort{
public:
UDPPort(std::string name, std::string toIP, uint toPort, uint ownPort,
size_t recvLength = 0,
BlockYN blockYN = BlockYN::NO,
size_t timeOutUs = 20000);
~UDPPort();
size_t send(uint8_t *sendMsg, size_t sendMsgLength);
size_t recv(uint8_t *recvMsg, size_t recvLength);
size_t recv(uint8_t *recvMsg);
bool sendRecv(std::vector<MOTOR_send> &sendVec, std::vector<MOTOR_recv> &recvVec);
private:
size_t _recvBlock(uint8_t *recvMsg, size_t recvLength);
size_t _recvUnBlock(uint8_t *recvMsg, size_t recvLength);
sockaddr_in _ownAddr, _toAddr, _fromAddr;
socklen_t _sockaddrSize;
int _sockfd;
int _on = 1;
size_t _sentLength;
uint8_t _sendBytes[238]; // 7 motors
uint8_t _recvBytes[546]; // 7 motors
fd_set _rSet;
};
#endif

215
include/message/unitree_legged_msgs/Cartesian.h
View File

@ -1,215 +0,0 @@
// Generated by gencpp from file unitree_legged_msgs/Cartesian.msg
// DO NOT EDIT!
#ifndef UNITREE_LEGGED_MSGS_MESSAGE_CARTESIAN_H
#define UNITREE_LEGGED_MSGS_MESSAGE_CARTESIAN_H
#include <string>
#include <vector>
#include <map>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
namespace unitree_legged_msgs
{
template <class ContainerAllocator>
struct Cartesian_
{
typedef Cartesian_<ContainerAllocator> Type;
Cartesian_()
: x(0.0)
, y(0.0)
, z(0.0) {
}
Cartesian_(const ContainerAllocator& _alloc)
: x(0.0)
, y(0.0)
, z(0.0) {
(void)_alloc;
}
typedef float _x_type;
_x_type x;
typedef float _y_type;
_y_type y;
typedef float _z_type;
_z_type z;
typedef boost::shared_ptr< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> const> ConstPtr;
}; // struct Cartesian_
typedef ::unitree_legged_msgs::Cartesian_<std::allocator<void> > Cartesian;
typedef boost::shared_ptr< ::unitree_legged_msgs::Cartesian > CartesianPtr;
typedef boost::shared_ptr< ::unitree_legged_msgs::Cartesian const> CartesianConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::Cartesian_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, "", v);
return s;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::unitree_legged_msgs::Cartesian_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::Cartesian_<ContainerAllocator2> & rhs)
{
return lhs.x == rhs.x &&
lhs.y == rhs.y &&
lhs.z == rhs.z;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::unitree_legged_msgs::Cartesian_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::Cartesian_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace unitree_legged_msgs
namespace ros
{
namespace message_traits
{
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> const>
: FalseType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
{
static const char* value()
{
return "cc153912f1453b708d221682bc23d9ac";
}
static const char* value(const ::unitree_legged_msgs::Cartesian_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0xcc153912f1453b70ULL;
static const uint64_t static_value2 = 0x8d221682bc23d9acULL;
};
template<class ContainerAllocator>
struct DataType< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
{
static const char* value()
{
return "unitree_legged_msgs/Cartesian";
}
static const char* value(const ::unitree_legged_msgs::Cartesian_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
{
static const char* value()
{
return "float32 x\n"
"float32 y\n"
"float32 z\n"
;
}
static const char* value(const ::unitree_legged_msgs::Cartesian_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.x);
stream.next(m.y);
stream.next(m.z);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct Cartesian_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::Cartesian_<ContainerAllocator>& v)
{
s << indent << "x: ";
Printer<float>::stream(s, indent + " ", v.x);
s << indent << "y: ";
Printer<float>::stream(s, indent + " ", v.y);
s << indent << "z: ";
Printer<float>::stream(s, indent + " ", v.z);
}
};
} // namespace message_operations
} // namespace ros
#endif // UNITREE_LEGGED_MSGS_MESSAGE_CARTESIAN_H

403
include/message/unitree_legged_msgs/HighCmd.h
View File

@ -1,403 +0,0 @@
// Generated by gencpp from file unitree_legged_msgs/HighCmd.msg
// DO NOT EDIT!
#ifndef UNITREE_LEGGED_MSGS_MESSAGE_HIGHCMD_H
#define UNITREE_LEGGED_MSGS_MESSAGE_HIGHCMD_H
#include <string>
#include <vector>
#include <map>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
#include <unitree_legged_msgs/LED.h>
namespace unitree_legged_msgs
{
template <class ContainerAllocator>
struct HighCmd_
{
typedef HighCmd_<ContainerAllocator> Type;
HighCmd_()
: levelFlag(0)
, commVersion(0)
, robotID(0)
, SN(0)
, bandWidth(0)
, mode(0)
, forwardSpeed(0.0)
, sideSpeed(0.0)
, rotateSpeed(0.0)
, bodyHeight(0.0)
, footRaiseHeight(0.0)
, yaw(0.0)
, pitch(0.0)
, roll(0.0)
, led()
, wirelessRemote()
, AppRemote()
, reserve(0)
, crc(0) {
wirelessRemote.assign(0);
AppRemote.assign(0);
}
HighCmd_(const ContainerAllocator& _alloc)
: levelFlag(0)
, commVersion(0)
, robotID(0)
, SN(0)
, bandWidth(0)
, mode(0)
, forwardSpeed(0.0)
, sideSpeed(0.0)
, rotateSpeed(0.0)
, bodyHeight(0.0)
, footRaiseHeight(0.0)
, yaw(0.0)
, pitch(0.0)
, roll(0.0)
, led()
, wirelessRemote()
, AppRemote()
, reserve(0)
, crc(0) {
(void)_alloc;
led.assign( ::unitree_legged_msgs::LED_<ContainerAllocator> (_alloc));
wirelessRemote.assign(0);
AppRemote.assign(0);
}
typedef uint8_t _levelFlag_type;
_levelFlag_type levelFlag;
typedef uint16_t _commVersion_type;
_commVersion_type commVersion;
typedef uint16_t _robotID_type;
_robotID_type robotID;
typedef uint32_t _SN_type;
_SN_type SN;
typedef uint8_t _bandWidth_type;
_bandWidth_type bandWidth;
typedef uint8_t _mode_type;
_mode_type mode;
typedef float _forwardSpeed_type;
_forwardSpeed_type forwardSpeed;
typedef float _sideSpeed_type;
_sideSpeed_type sideSpeed;
typedef float _rotateSpeed_type;
_rotateSpeed_type rotateSpeed;
typedef float _bodyHeight_type;
_bodyHeight_type bodyHeight;
typedef float _footRaiseHeight_type;
_footRaiseHeight_type footRaiseHeight;
typedef float _yaw_type;
_yaw_type yaw;
typedef float _pitch_type;
_pitch_type pitch;
typedef float _roll_type;
_roll_type roll;
typedef boost::array< ::unitree_legged_msgs::LED_<ContainerAllocator> , 4> _led_type;
_led_type led;
typedef boost::array<uint8_t, 40> _wirelessRemote_type;
_wirelessRemote_type wirelessRemote;
typedef boost::array<uint8_t, 40> _AppRemote_type;
_AppRemote_type AppRemote;
typedef uint32_t _reserve_type;
_reserve_type reserve;
typedef int32_t _crc_type;
_crc_type crc;
typedef boost::shared_ptr< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> const> ConstPtr;
}; // struct HighCmd_
typedef ::unitree_legged_msgs::HighCmd_<std::allocator<void> > HighCmd;
typedef boost::shared_ptr< ::unitree_legged_msgs::HighCmd > HighCmdPtr;
typedef boost::shared_ptr< ::unitree_legged_msgs::HighCmd const> HighCmdConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::HighCmd_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >::stream(s, "", v);
return s;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::unitree_legged_msgs::HighCmd_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::HighCmd_<ContainerAllocator2> & rhs)
{
return lhs.levelFlag == rhs.levelFlag &&
lhs.commVersion == rhs.commVersion &&
lhs.robotID == rhs.robotID &&
lhs.SN == rhs.SN &&
lhs.bandWidth == rhs.bandWidth &&
lhs.mode == rhs.mode &&
lhs.forwardSpeed == rhs.forwardSpeed &&
lhs.sideSpeed == rhs.sideSpeed &&
lhs.rotateSpeed == rhs.rotateSpeed &&
lhs.bodyHeight == rhs.bodyHeight &&
lhs.footRaiseHeight == rhs.footRaiseHeight &&
lhs.yaw == rhs.yaw &&
lhs.pitch == rhs.pitch &&
lhs.roll == rhs.roll &&
lhs.led == rhs.led &&
lhs.wirelessRemote == rhs.wirelessRemote &&
lhs.AppRemote == rhs.AppRemote &&
lhs.reserve == rhs.reserve &&
lhs.crc == rhs.crc;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::unitree_legged_msgs::HighCmd_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::HighCmd_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace unitree_legged_msgs
namespace ros
{
namespace message_traits
{
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> const>
: FalseType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
{
static const char* value()
{
return "1a655499a3f64905db59ceed65ca774a";
}
static const char* value(const ::unitree_legged_msgs::HighCmd_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0x1a655499a3f64905ULL;
static const uint64_t static_value2 = 0xdb59ceed65ca774aULL;
};
template<class ContainerAllocator>
struct DataType< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
{
static const char* value()
{
return "unitree_legged_msgs/HighCmd";
}
static const char* value(const ::unitree_legged_msgs::HighCmd_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
{
static const char* value()
{
return "uint8 levelFlag\n"
"uint16 commVersion # Old version Aliengo does not have\n"
"uint16 robotID # Old version Aliengo does not have\n"
"uint32 SN # Old version Aliengo does not have\n"
"uint8 bandWidth # Old version Aliengo does not have\n"
"uint8 mode\n"
"float32 forwardSpeed\n"
"float32 sideSpeed\n"
"float32 rotateSpeed \n"
"float32 bodyHeight\n"
"float32 footRaiseHeight\n"
"float32 yaw\n"
"float32 pitch\n"
"float32 roll\n"
"LED[4] led\n"
"uint8[40] wirelessRemote\n"
"uint8[40] AppRemote # Old version Aliengo does not have\n"
"uint32 reserve # Old version Aliengo does not have\n"
"int32 crc\n"
"================================================================================\n"
"MSG: unitree_legged_msgs/LED\n"
"uint8 r\n"
"uint8 g\n"
"uint8 b\n"
;
}
static const char* value(const ::unitree_legged_msgs::HighCmd_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.levelFlag);
stream.next(m.commVersion);
stream.next(m.robotID);
stream.next(m.SN);
stream.next(m.bandWidth);
stream.next(m.mode);
stream.next(m.forwardSpeed);
stream.next(m.sideSpeed);
stream.next(m.rotateSpeed);
stream.next(m.bodyHeight);
stream.next(m.footRaiseHeight);
stream.next(m.yaw);
stream.next(m.pitch);
stream.next(m.roll);
stream.next(m.led);
stream.next(m.wirelessRemote);
stream.next(m.AppRemote);
stream.next(m.reserve);
stream.next(m.crc);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct HighCmd_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::HighCmd_<ContainerAllocator>& v)
{
s << indent << "levelFlag: ";
Printer<uint8_t>::stream(s, indent + " ", v.levelFlag);
s << indent << "commVersion: ";
Printer<uint16_t>::stream(s, indent + " ", v.commVersion);
s << indent << "robotID: ";
Printer<uint16_t>::stream(s, indent + " ", v.robotID);
s << indent << "SN: ";
Printer<uint32_t>::stream(s, indent + " ", v.SN);
s << indent << "bandWidth: ";
Printer<uint8_t>::stream(s, indent + " ", v.bandWidth);
s << indent << "mode: ";
Printer<uint8_t>::stream(s, indent + " ", v.mode);
s << indent << "forwardSpeed: ";
Printer<float>::stream(s, indent + " ", v.forwardSpeed);
s << indent << "sideSpeed: ";
Printer<float>::stream(s, indent + " ", v.sideSpeed);
s << indent << "rotateSpeed: ";
Printer<float>::stream(s, indent + " ", v.rotateSpeed);
s << indent << "bodyHeight: ";
Printer<float>::stream(s, indent + " ", v.bodyHeight);
s << indent << "footRaiseHeight: ";
Printer<float>::stream(s, indent + " ", v.footRaiseHeight);
s << indent << "yaw: ";
Printer<float>::stream(s, indent + " ", v.yaw);
s << indent << "pitch: ";
Printer<float>::stream(s, indent + " ", v.pitch);
s << indent << "roll: ";
Printer<float>::stream(s, indent + " ", v.roll);
s << indent << "led[]" << std::endl;
for (size_t i = 0; i < v.led.size(); ++i)
{
s << indent << " led[" << i << "]: ";
s << std::endl;
s << indent;
Printer< ::unitree_legged_msgs::LED_<ContainerAllocator> >::stream(s, indent + " ", v.led[i]);
}
s << indent << "wirelessRemote[]" << std::endl;
for (size_t i = 0; i < v.wirelessRemote.size(); ++i)
{
s << indent << " wirelessRemote[" << i << "]: ";
Printer<uint8_t>::stream(s, indent + " ", v.wirelessRemote[i]);
}
s << indent << "AppRemote[]" << std::endl;
for (size_t i = 0; i < v.AppRemote.size(); ++i)
{
s << indent << " AppRemote[" << i << "]: ";
Printer<uint8_t>::stream(s, indent + " ", v.AppRemote[i]);
}
s << indent << "reserve: ";
Printer<uint32_t>::stream(s, indent + " ", v.reserve);
s << indent << "crc: ";
Printer<int32_t>::stream(s, indent + " ", v.crc);
}
};
} // namespace message_operations
} // namespace ros
#endif // UNITREE_LEGGED_MSGS_MESSAGE_HIGHCMD_H

497
include/message/unitree_legged_msgs/HighState.h
View File

@ -1,497 +0,0 @@
// Generated by gencpp from file unitree_legged_msgs/HighState.msg
// DO NOT EDIT!
#ifndef UNITREE_LEGGED_MSGS_MESSAGE_HIGHSTATE_H
#define UNITREE_LEGGED_MSGS_MESSAGE_HIGHSTATE_H
#include <string>
#include <vector>
#include <map>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
#include <unitree_legged_msgs/IMU.h>
#include <unitree_legged_msgs/Cartesian.h>
#include <unitree_legged_msgs/Cartesian.h>
#include <unitree_legged_msgs/Cartesian.h>
namespace unitree_legged_msgs
{
template <class ContainerAllocator>
struct HighState_
{
typedef HighState_<ContainerAllocator> Type;
HighState_()
: levelFlag(0)
, commVersion(0)
, robotID(0)
, SN(0)
, bandWidth(0)
, mode(0)
, imu()
, forwardSpeed(0.0)
, sideSpeed(0.0)
, rotateSpeed(0.0)
, bodyHeight(0.0)
, updownSpeed(0.0)
, forwardPosition(0.0)
, sidePosition(0.0)
, footPosition2Body()
, footSpeed2Body()
, footForce()
, footForceEst()
, tick(0)
, wirelessRemote()
, reserve(0)
, crc(0)
, eeForce()
, jointP() {
footForce.assign(0);
footForceEst.assign(0);
wirelessRemote.assign(0);
jointP.assign(0.0);
}
HighState_(const ContainerAllocator& _alloc)
: levelFlag(0)
, commVersion(0)
, robotID(0)
, SN(0)
, bandWidth(0)
, mode(0)
, imu(_alloc)
, forwardSpeed(0.0)
, sideSpeed(0.0)
, rotateSpeed(0.0)
, bodyHeight(0.0)
, updownSpeed(0.0)
, forwardPosition(0.0)
, sidePosition(0.0)
, footPosition2Body()
, footSpeed2Body()
, footForce()
, footForceEst()
, tick(0)
, wirelessRemote()
, reserve(0)
, crc(0)
, eeForce()
, jointP() {
(void)_alloc;
footPosition2Body.assign( ::unitree_legged_msgs::Cartesian_<ContainerAllocator> (_alloc));
footSpeed2Body.assign( ::unitree_legged_msgs::Cartesian_<ContainerAllocator> (_alloc));
footForce.assign(0);
footForceEst.assign(0);
wirelessRemote.assign(0);
eeForce.assign( ::unitree_legged_msgs::Cartesian_<ContainerAllocator> (_alloc));
jointP.assign(0.0);
}
typedef uint8_t _levelFlag_type;
_levelFlag_type levelFlag;
typedef uint16_t _commVersion_type;
_commVersion_type commVersion;
typedef uint16_t _robotID_type;
_robotID_type robotID;
typedef uint32_t _SN_type;
_SN_type SN;
typedef uint8_t _bandWidth_type;
_bandWidth_type bandWidth;
typedef uint8_t _mode_type;
_mode_type mode;
typedef ::unitree_legged_msgs::IMU_<ContainerAllocator> _imu_type;
_imu_type imu;
typedef float _forwardSpeed_type;
_forwardSpeed_type forwardSpeed;
typedef float _sideSpeed_type;
_sideSpeed_type sideSpeed;
typedef float _rotateSpeed_type;
_rotateSpeed_type rotateSpeed;
typedef float _bodyHeight_type;
_bodyHeight_type bodyHeight;
typedef float _updownSpeed_type;
_updownSpeed_type updownSpeed;
typedef float _forwardPosition_type;
_forwardPosition_type forwardPosition;
typedef float _sidePosition_type;
_sidePosition_type sidePosition;
typedef boost::array< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> , 4> _footPosition2Body_type;
_footPosition2Body_type footPosition2Body;
typedef boost::array< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> , 4> _footSpeed2Body_type;
_footSpeed2Body_type footSpeed2Body;
typedef boost::array<int16_t, 4> _footForce_type;
_footForce_type footForce;
typedef boost::array<int16_t, 4> _footForceEst_type;
_footForceEst_type footForceEst;
typedef uint32_t _tick_type;
_tick_type tick;
typedef boost::array<uint8_t, 40> _wirelessRemote_type;
_wirelessRemote_type wirelessRemote;
typedef uint32_t _reserve_type;
_reserve_type reserve;
typedef uint32_t _crc_type;
_crc_type crc;
typedef boost::array< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> , 4> _eeForce_type;
_eeForce_type eeForce;
typedef boost::array<float, 12> _jointP_type;
_jointP_type jointP;
typedef boost::shared_ptr< ::unitree_legged_msgs::HighState_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::unitree_legged_msgs::HighState_<ContainerAllocator> const> ConstPtr;
}; // struct HighState_
typedef ::unitree_legged_msgs::HighState_<std::allocator<void> > HighState;
typedef boost::shared_ptr< ::unitree_legged_msgs::HighState > HighStatePtr;
typedef boost::shared_ptr< ::unitree_legged_msgs::HighState const> HighStateConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::HighState_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::unitree_legged_msgs::HighState_<ContainerAllocator> >::stream(s, "", v);
return s;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::unitree_legged_msgs::HighState_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::HighState_<ContainerAllocator2> & rhs)
{
return lhs.levelFlag == rhs.levelFlag &&
lhs.commVersion == rhs.commVersion &&
lhs.robotID == rhs.robotID &&
lhs.SN == rhs.SN &&
lhs.bandWidth == rhs.bandWidth &&
lhs.mode == rhs.mode &&
lhs.imu == rhs.imu &&
lhs.forwardSpeed == rhs.forwardSpeed &&
lhs.sideSpeed == rhs.sideSpeed &&
lhs.rotateSpeed == rhs.rotateSpeed &&
lhs.bodyHeight == rhs.bodyHeight &&
lhs.updownSpeed == rhs.updownSpeed &&
lhs.forwardPosition == rhs.forwardPosition &&
lhs.sidePosition == rhs.sidePosition &&
lhs.footPosition2Body == rhs.footPosition2Body &&
lhs.footSpeed2Body == rhs.footSpeed2Body &&
lhs.footForce == rhs.footForce &&
lhs.footForceEst == rhs.footForceEst &&
lhs.tick == rhs.tick &&
lhs.wirelessRemote == rhs.wirelessRemote &&
lhs.reserve == rhs.reserve &&
lhs.crc == rhs.crc &&
lhs.eeForce == rhs.eeForce &&
lhs.jointP == rhs.jointP;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::unitree_legged_msgs::HighState_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::HighState_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace unitree_legged_msgs
namespace ros
{
namespace message_traits
{
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::HighState_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::HighState_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::HighState_<ContainerAllocator> const>
: FalseType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
{
static const char* value()
{
return "a12e8b22df896c82203810ec6d521dad";
}
static const char* value(const ::unitree_legged_msgs::HighState_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0xa12e8b22df896c82ULL;
static const uint64_t static_value2 = 0x203810ec6d521dadULL;
};
template<class ContainerAllocator>
struct DataType< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
{
static const char* value()
{
return "unitree_legged_msgs/HighState";
}
static const char* value(const ::unitree_legged_msgs::HighState_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
{
static const char* value()
{
return "uint8 levelFlag\n"
"uint16 commVersion # Old version Aliengo does not have\n"
"uint16 robotID # Old version Aliengo does not have\n"
"uint32 SN # Old version Aliengo does not have\n"
"uint8 bandWidth # Old version Aliengo does not have\n"
"uint8 mode\n"
"IMU imu\n"
"float32 forwardSpeed\n"
"float32 sideSpeed\n"
"float32 rotateSpeed\n"
"float32 bodyHeight\n"
"float32 updownSpeed\n"
"float32 forwardPosition # (will be float type next version) # Old version Aliengo is different\n"
"float32 sidePosition # (will be float type next version) # Old version Aliengo is different\n"
"Cartesian[4] footPosition2Body\n"
"Cartesian[4] footSpeed2Body\n"
"int16[4] footForce # Old version Aliengo is different\n"
"int16[4] footForceEst # Old version Aliengo does not have\n"
"uint32 tick \n"
"uint8[40] wirelessRemote\n"
"uint32 reserve # Old version Aliengo does not have\n"
"uint32 crc\n"
"\n"
"# Under are not defined in SDK yet. # Old version Aliengo does not have\n"
"Cartesian[4] eeForce # It's a 1-DOF force in real robot, but 3-DOF is better for visualization.\n"
"float32[12] jointP # for visualization\n"
"================================================================================\n"
"MSG: unitree_legged_msgs/IMU\n"
"float32[4] quaternion\n"
"float32[3] gyroscope\n"
"float32[3] accelerometer\n"
"int8 temperature\n"
"================================================================================\n"
"MSG: unitree_legged_msgs/Cartesian\n"
"float32 x\n"
"float32 y\n"
"float32 z\n"
;
}
static const char* value(const ::unitree_legged_msgs::HighState_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.levelFlag);
stream.next(m.commVersion);
stream.next(m.robotID);
stream.next(m.SN);
stream.next(m.bandWidth);
stream.next(m.mode);
stream.next(m.imu);
stream.next(m.forwardSpeed);
stream.next(m.sideSpeed);
stream.next(m.rotateSpeed);
stream.next(m.bodyHeight);
stream.next(m.updownSpeed);
stream.next(m.forwardPosition);
stream.next(m.sidePosition);
stream.next(m.footPosition2Body);
stream.next(m.footSpeed2Body);
stream.next(m.footForce);
stream.next(m.footForceEst);
stream.next(m.tick);
stream.next(m.wirelessRemote);
stream.next(m.reserve);
stream.next(m.crc);
stream.next(m.eeForce);
stream.next(m.jointP);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct HighState_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::HighState_<ContainerAllocator>& v)
{
s << indent << "levelFlag: ";
Printer<uint8_t>::stream(s, indent + " ", v.levelFlag);
s << indent << "commVersion: ";
Printer<uint16_t>::stream(s, indent + " ", v.commVersion);
s << indent << "robotID: ";
Printer<uint16_t>::stream(s, indent + " ", v.robotID);
s << indent << "SN: ";
Printer<uint32_t>::stream(s, indent + " ", v.SN);
s << indent << "bandWidth: ";
Printer<uint8_t>::stream(s, indent + " ", v.bandWidth);
s << indent << "mode: ";
Printer<uint8_t>::stream(s, indent + " ", v.mode);
s << indent << "imu: ";
s << std::endl;
Printer< ::unitree_legged_msgs::IMU_<ContainerAllocator> >::stream(s, indent + " ", v.imu);
s << indent << "forwardSpeed: ";
Printer<float>::stream(s, indent + " ", v.forwardSpeed);
s << indent << "sideSpeed: ";
Printer<float>::stream(s, indent + " ", v.sideSpeed);
s << indent << "rotateSpeed: ";
Printer<float>::stream(s, indent + " ", v.rotateSpeed);
s << indent << "bodyHeight: ";
Printer<float>::stream(s, indent + " ", v.bodyHeight);
s << indent << "updownSpeed: ";
Printer<float>::stream(s, indent + " ", v.updownSpeed);
s << indent << "forwardPosition: ";
Printer<float>::stream(s, indent + " ", v.forwardPosition);
s << indent << "sidePosition: ";
Printer<float>::stream(s, indent + " ", v.sidePosition);
s << indent << "footPosition2Body[]" << std::endl;
for (size_t i = 0; i < v.footPosition2Body.size(); ++i)
{
s << indent << " footPosition2Body[" << i << "]: ";
s << std::endl;
s << indent;
Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + " ", v.footPosition2Body[i]);
}
s << indent << "footSpeed2Body[]" << std::endl;
for (size_t i = 0; i < v.footSpeed2Body.size(); ++i)
{
s << indent << " footSpeed2Body[" << i << "]: ";
s << std::endl;
s << indent;
Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + " ", v.footSpeed2Body[i]);
}
s << indent << "footForce[]" << std::endl;
for (size_t i = 0; i < v.footForce.size(); ++i)
{
s << indent << " footForce[" << i << "]: ";
Printer<int16_t>::stream(s, indent + " ", v.footForce[i]);
}
s << indent << "footForceEst[]" << std::endl;
for (size_t i = 0; i < v.footForceEst.size(); ++i)
{
s << indent << " footForceEst[" << i << "]: ";
Printer<int16_t>::stream(s, indent + " ", v.footForceEst[i]);
}
s << indent << "tick: ";
Printer<uint32_t>::stream(s, indent + " ", v.tick);
s << indent << "wirelessRemote[]" << std::endl;
for (size_t i = 0; i < v.wirelessRemote.size(); ++i)
{
s << indent << " wirelessRemote[" << i << "]: ";
Printer<uint8_t>::stream(s, indent + " ", v.wirelessRemote[i]);
}
s << indent << "reserve: ";
Printer<uint32_t>::stream(s, indent + " ", v.reserve);
s << indent << "crc: ";
Printer<uint32_t>::stream(s, indent + " ", v.crc);
s << indent << "eeForce[]" << std::endl;
for (size_t i = 0; i < v.eeForce.size(); ++i)
{
s << indent << " eeForce[" << i << "]: ";
s << std::endl;
s << indent;
Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + " ", v.eeForce[i]);
}
s << indent << "jointP[]" << std::endl;
for (size_t i = 0; i < v.jointP.size(); ++i)
{
s << indent << " jointP[" << i << "]: ";
Printer<float>::stream(s, indent + " ", v.jointP[i]);
}
}
};
} // namespace message_operations
} // namespace ros
#endif // UNITREE_LEGGED_MSGS_MESSAGE_HIGHSTATE_H

247
include/message/unitree_legged_msgs/IMU.h
View File

@ -1,247 +0,0 @@
// Generated by gencpp from file unitree_legged_msgs/IMU.msg
// DO NOT EDIT!
#ifndef UNITREE_LEGGED_MSGS_MESSAGE_IMU_H
#define UNITREE_LEGGED_MSGS_MESSAGE_IMU_H
#include <string>
#include <vector>
#include <map>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
namespace unitree_legged_msgs
{
template <class ContainerAllocator>
struct IMU_
{
typedef IMU_<ContainerAllocator> Type;
IMU_()
: quaternion()
, gyroscope()
, accelerometer()
, temperature(0) {
quaternion.assign(0.0);
gyroscope.assign(0.0);
accelerometer.assign(0.0);
}
IMU_(const ContainerAllocator& _alloc)
: quaternion()
, gyroscope()
, accelerometer()
, temperature(0) {
(void)_alloc;
quaternion.assign(0.0);
gyroscope.assign(0.0);
accelerometer.assign(0.0);
}
typedef boost::array<float, 4> _quaternion_type;
_quaternion_type quaternion;
typedef boost::array<float, 3> _gyroscope_type;
_gyroscope_type gyroscope;
typedef boost::array<float, 3> _accelerometer_type;
_accelerometer_type accelerometer;
typedef int8_t _temperature_type;
_temperature_type temperature;
typedef boost::shared_ptr< ::unitree_legged_msgs::IMU_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::unitree_legged_msgs::IMU_<ContainerAllocator> const> ConstPtr;
}; // struct IMU_
typedef ::unitree_legged_msgs::IMU_<std::allocator<void> > IMU;
typedef boost::shared_ptr< ::unitree_legged_msgs::IMU > IMUPtr;
typedef boost::shared_ptr< ::unitree_legged_msgs::IMU const> IMUConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::IMU_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::unitree_legged_msgs::IMU_<ContainerAllocator> >::stream(s, "", v);
return s;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::unitree_legged_msgs::IMU_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::IMU_<ContainerAllocator2> & rhs)
{
return lhs.quaternion == rhs.quaternion &&
lhs.gyroscope == rhs.gyroscope &&
lhs.accelerometer == rhs.accelerometer &&
lhs.temperature == rhs.temperature;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::unitree_legged_msgs::IMU_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::IMU_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace unitree_legged_msgs
namespace ros
{
namespace message_traits
{
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::IMU_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::IMU_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::IMU_<ContainerAllocator> const>
: FalseType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
{
static const char* value()
{
return "dd4bb4e42aa2f15aa1fb1b6a3c3752cb";
}
static const char* value(const ::unitree_legged_msgs::IMU_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0xdd4bb4e42aa2f15aULL;
static const uint64_t static_value2 = 0xa1fb1b6a3c3752cbULL;
};
template<class ContainerAllocator>
struct DataType< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
{
static const char* value()
{
return "unitree_legged_msgs/IMU";
}
static const char* value(const ::unitree_legged_msgs::IMU_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
{
static const char* value()
{
return "float32[4] quaternion\n"
"float32[3] gyroscope\n"
"float32[3] accelerometer\n"
"int8 temperature\n"
;
}
static const char* value(const ::unitree_legged_msgs::IMU_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.quaternion);
stream.next(m.gyroscope);
stream.next(m.accelerometer);
stream.next(m.temperature);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct IMU_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::IMU_<ContainerAllocator>& v)
{
s << indent << "quaternion[]" << std::endl;
for (size_t i = 0; i < v.quaternion.size(); ++i)
{
s << indent << " quaternion[" << i << "]: ";
Printer<float>::stream(s, indent + " ", v.quaternion[i]);
}
s << indent << "gyroscope[]" << std::endl;
for (size_t i = 0; i < v.gyroscope.size(); ++i)
{
s << indent << " gyroscope[" << i << "]: ";
Printer<float>::stream(s, indent + " ", v.gyroscope[i]);
}
s << indent << "accelerometer[]" << std::endl;
for (size_t i = 0; i < v.accelerometer.size(); ++i)
{
s << indent << " accelerometer[" << i << "]: ";
Printer<float>::stream(s, indent + " ", v.accelerometer[i]);
}
s << indent << "temperature: ";
Printer<int8_t>::stream(s, indent + " ", v.temperature);
}
};
} // namespace message_operations
} // namespace ros
#endif // UNITREE_LEGGED_MSGS_MESSAGE_IMU_H

215
include/message/unitree_legged_msgs/LED.h
View File

@ -1,215 +0,0 @@
// Generated by gencpp from file unitree_legged_msgs/LED.msg
// DO NOT EDIT!
#ifndef UNITREE_LEGGED_MSGS_MESSAGE_LED_H
#define UNITREE_LEGGED_MSGS_MESSAGE_LED_H
#include <string>
#include <vector>
#include <map>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
namespace unitree_legged_msgs
{
template <class ContainerAllocator>
struct LED_
{
typedef LED_<ContainerAllocator> Type;
LED_()
: r(0)
, g(0)
, b(0) {
}
LED_(const ContainerAllocator& _alloc)
: r(0)
, g(0)
, b(0) {
(void)_alloc;
}
typedef uint8_t _r_type;
_r_type r;
typedef uint8_t _g_type;
_g_type g;
typedef uint8_t _b_type;
_b_type b;
typedef boost::shared_ptr< ::unitree_legged_msgs::LED_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::unitree_legged_msgs::LED_<ContainerAllocator> const> ConstPtr;
}; // struct LED_
typedef ::unitree_legged_msgs::LED_<std::allocator<void> > LED;
typedef boost::shared_ptr< ::unitree_legged_msgs::LED > LEDPtr;
typedef boost::shared_ptr< ::unitree_legged_msgs::LED const> LEDConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::LED_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::unitree_legged_msgs::LED_<ContainerAllocator> >::stream(s, "", v);
return s;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::unitree_legged_msgs::LED_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::LED_<ContainerAllocator2> & rhs)
{
return lhs.r == rhs.r &&
lhs.g == rhs.g &&
lhs.b == rhs.b;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::unitree_legged_msgs::LED_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::LED_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace unitree_legged_msgs
namespace ros
{
namespace message_traits
{
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::LED_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::LED_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::LED_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::LED_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::LED_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::LED_<ContainerAllocator> const>
: FalseType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::unitree_legged_msgs::LED_<ContainerAllocator> >
{
static const char* value()
{
return "353891e354491c51aabe32df673fb446";
}
static const char* value(const ::unitree_legged_msgs::LED_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0x353891e354491c51ULL;
static const uint64_t static_value2 = 0xaabe32df673fb446ULL;
};
template<class ContainerAllocator>
struct DataType< ::unitree_legged_msgs::LED_<ContainerAllocator> >
{
static const char* value()
{
return "unitree_legged_msgs/LED";
}
static const char* value(const ::unitree_legged_msgs::LED_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::unitree_legged_msgs::LED_<ContainerAllocator> >
{
static const char* value()
{
return "uint8 r\n"
"uint8 g\n"
"uint8 b\n"
;
}
static const char* value(const ::unitree_legged_msgs::LED_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::LED_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.r);
stream.next(m.g);
stream.next(m.b);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct LED_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::unitree_legged_msgs::LED_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::LED_<ContainerAllocator>& v)
{
s << indent << "r: ";
Printer<uint8_t>::stream(s, indent + " ", v.r);
s << indent << "g: ";
Printer<uint8_t>::stream(s, indent + " ", v.g);
s << indent << "b: ";
Printer<uint8_t>::stream(s, indent + " ", v.b);
}
};
} // namespace message_operations
} // namespace ros
#endif // UNITREE_LEGGED_MSGS_MESSAGE_LED_H

348
include/message/unitree_legged_msgs/LowCmd.h
View File

@ -1,348 +0,0 @@
// Generated by gencpp from file unitree_legged_msgs/LowCmd.msg
// DO NOT EDIT!
#ifndef UNITREE_LEGGED_MSGS_MESSAGE_LOWCMD_H
#define UNITREE_LEGGED_MSGS_MESSAGE_LOWCMD_H
#include <string>
#include <vector>
#include <map>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
#include <unitree_legged_msgs/MotorCmd.h>
#include <unitree_legged_msgs/LED.h>
#include <unitree_legged_msgs/Cartesian.h>
namespace unitree_legged_msgs
{
template <class ContainerAllocator>
struct LowCmd_
{
typedef LowCmd_<ContainerAllocator> Type;
LowCmd_()
: levelFlag(0)
, commVersion(0)
, robotID(0)
, SN(0)
, bandWidth(0)
, motorCmd()
, led()
, wirelessRemote()
, reserve(0)
, crc(0)
, ff() {
wirelessRemote.assign(0);
}
LowCmd_(const ContainerAllocator& _alloc)
: levelFlag(0)
, commVersion(0)
, robotID(0)
, SN(0)
, bandWidth(0)
, motorCmd()
, led()
, wirelessRemote()
, reserve(0)
, crc(0)
, ff() {
(void)_alloc;
motorCmd.assign( ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> (_alloc));
led.assign( ::unitree_legged_msgs::LED_<ContainerAllocator> (_alloc));
wirelessRemote.assign(0);
ff.assign( ::unitree_legged_msgs::Cartesian_<ContainerAllocator> (_alloc));
}
typedef uint8_t _levelFlag_type;
_levelFlag_type levelFlag;
typedef uint16_t _commVersion_type;
_commVersion_type commVersion;
typedef uint16_t _robotID_type;
_robotID_type robotID;
typedef uint32_t _SN_type;
_SN_type SN;
typedef uint8_t _bandWidth_type;
_bandWidth_type bandWidth;
typedef boost::array< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> , 20> _motorCmd_type;
_motorCmd_type motorCmd;
typedef boost::array< ::unitree_legged_msgs::LED_<ContainerAllocator> , 4> _led_type;
_led_type led;
typedef boost::array<uint8_t, 40> _wirelessRemote_type;
_wirelessRemote_type wirelessRemote;
typedef uint32_t _reserve_type;
_reserve_type reserve;
typedef uint32_t _crc_type;
_crc_type crc;
typedef boost::array< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> , 4> _ff_type;
_ff_type ff;
typedef boost::shared_ptr< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> const> ConstPtr;
}; // struct LowCmd_
typedef ::unitree_legged_msgs::LowCmd_<std::allocator<void> > LowCmd;
typedef boost::shared_ptr< ::unitree_legged_msgs::LowCmd > LowCmdPtr;
typedef boost::shared_ptr< ::unitree_legged_msgs::LowCmd const> LowCmdConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::LowCmd_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >::stream(s, "", v);
return s;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::unitree_legged_msgs::LowCmd_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::LowCmd_<ContainerAllocator2> & rhs)
{
return lhs.levelFlag == rhs.levelFlag &&
lhs.commVersion == rhs.commVersion &&
lhs.robotID == rhs.robotID &&
lhs.SN == rhs.SN &&
lhs.bandWidth == rhs.bandWidth &&
lhs.motorCmd == rhs.motorCmd &&
lhs.led == rhs.led &&
lhs.wirelessRemote == rhs.wirelessRemote &&
lhs.reserve == rhs.reserve &&
lhs.crc == rhs.crc &&
lhs.ff == rhs.ff;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::unitree_legged_msgs::LowCmd_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::LowCmd_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace unitree_legged_msgs
namespace ros
{
namespace message_traits
{
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> const>
: FalseType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
{
static const char* value()
{
return "357432b2562edd0a8e89b9c9f5fc4821";
}
static const char* value(const ::unitree_legged_msgs::LowCmd_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0x357432b2562edd0aULL;
static const uint64_t static_value2 = 0x8e89b9c9f5fc4821ULL;
};
template<class ContainerAllocator>
struct DataType< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
{
static const char* value()
{
return "unitree_legged_msgs/LowCmd";
}
static const char* value(const ::unitree_legged_msgs::LowCmd_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
{
static const char* value()
{
return "uint8 levelFlag \n"
"uint16 commVersion # Old version Aliengo does not have\n"
"uint16 robotID # Old version Aliengo does not have\n"
"uint32 SN # Old version Aliengo does not have\n"
"uint8 bandWidth # Old version Aliengo does not have\n"
"MotorCmd[20] motorCmd\n"
"LED[4] led\n"
"uint8[40] wirelessRemote\n"
"uint32 reserve # Old version Aliengo does not have\n"
"uint32 crc\n"
"\n"
"Cartesian[4] ff # will delete # Old version Aliengo does not have\n"
"================================================================================\n"
"MSG: unitree_legged_msgs/MotorCmd\n"
"uint8 mode # motor target mode\n"
"float32 q # motor target position\n"
"float32 dq # motor target velocity\n"
"float32 tau # motor target torque\n"
"float32 Kp # motor spring stiffness coefficient\n"
"float32 Kd # motor damper coefficient\n"
"uint32[3] reserve # motor target torque\n"
"================================================================================\n"
"MSG: unitree_legged_msgs/LED\n"
"uint8 r\n"
"uint8 g\n"
"uint8 b\n"
"================================================================================\n"
"MSG: unitree_legged_msgs/Cartesian\n"
"float32 x\n"
"float32 y\n"
"float32 z\n"
;
}
static const char* value(const ::unitree_legged_msgs::LowCmd_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.levelFlag);
stream.next(m.commVersion);
stream.next(m.robotID);
stream.next(m.SN);
stream.next(m.bandWidth);
stream.next(m.motorCmd);
stream.next(m.led);
stream.next(m.wirelessRemote);
stream.next(m.reserve);
stream.next(m.crc);
stream.next(m.ff);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct LowCmd_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::LowCmd_<ContainerAllocator>& v)
{
s << indent << "levelFlag: ";
Printer<uint8_t>::stream(s, indent + " ", v.levelFlag);
s << indent << "commVersion: ";
Printer<uint16_t>::stream(s, indent + " ", v.commVersion);
s << indent << "robotID: ";
Printer<uint16_t>::stream(s, indent + " ", v.robotID);
s << indent << "SN: ";
Printer<uint32_t>::stream(s, indent + " ", v.SN);
s << indent << "bandWidth: ";
Printer<uint8_t>::stream(s, indent + " ", v.bandWidth);
s << indent << "motorCmd[]" << std::endl;
for (size_t i = 0; i < v.motorCmd.size(); ++i)
{
s << indent << " motorCmd[" << i << "]: ";
s << std::endl;
s << indent;
Printer< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >::stream(s, indent + " ", v.motorCmd[i]);
}
s << indent << "led[]" << std::endl;
for (size_t i = 0; i < v.led.size(); ++i)
{
s << indent << " led[" << i << "]: ";
s << std::endl;
s << indent;
Printer< ::unitree_legged_msgs::LED_<ContainerAllocator> >::stream(s, indent + " ", v.led[i]);
}
s << indent << "wirelessRemote[]" << std::endl;
for (size_t i = 0; i < v.wirelessRemote.size(); ++i)
{
s << indent << " wirelessRemote[" << i << "]: ";
Printer<uint8_t>::stream(s, indent + " ", v.wirelessRemote[i]);
}
s << indent << "reserve: ";
Printer<uint32_t>::stream(s, indent + " ", v.reserve);
s << indent << "crc: ";
Printer<uint32_t>::stream(s, indent + " ", v.crc);
s << indent << "ff[]" << std::endl;
for (size_t i = 0; i < v.ff.size(); ++i)
{
s << indent << " ff[" << i << "]: ";
s << std::endl;
s << indent;
Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + " ", v.ff[i]);
}
}
};
} // namespace message_operations
} // namespace ros
#endif // UNITREE_LEGGED_MSGS_MESSAGE_LOWCMD_H

448
include/message/unitree_legged_msgs/LowState.h
View File

@ -1,448 +0,0 @@
// Generated by gencpp from file unitree_legged_msgs/LowState.msg
// DO NOT EDIT!
#ifndef UNITREE_LEGGED_MSGS_MESSAGE_LOWSTATE_H
#define UNITREE_LEGGED_MSGS_MESSAGE_LOWSTATE_H
#include <string>
#include <vector>
#include <map>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
#include <unitree_legged_msgs/IMU.h>
#include <unitree_legged_msgs/MotorState.h>
#include <unitree_legged_msgs/Cartesian.h>
#include <unitree_legged_msgs/Cartesian.h>
#include <unitree_legged_msgs/Cartesian.h>
#include <unitree_legged_msgs/Cartesian.h>
#include <unitree_legged_msgs/Cartesian.h>
namespace unitree_legged_msgs
{
template <class ContainerAllocator>
struct LowState_
{
typedef LowState_<ContainerAllocator> Type;
LowState_()
: levelFlag(0)
, commVersion(0)
, robotID(0)
, SN(0)
, bandWidth(0)
, imu()
, motorState()
, footForce()
, footForceEst()
, tick(0)
, wirelessRemote()
, reserve(0)
, crc(0)
, eeForceRaw()
, eeForce()
, position()
, velocity()
, velocity_w() {
footForce.assign(0);
footForceEst.assign(0);
wirelessRemote.assign(0);
}
LowState_(const ContainerAllocator& _alloc)
: levelFlag(0)
, commVersion(0)
, robotID(0)
, SN(0)
, bandWidth(0)
, imu(_alloc)
, motorState()
, footForce()
, footForceEst()
, tick(0)
, wirelessRemote()
, reserve(0)
, crc(0)
, eeForceRaw()
, eeForce()
, position(_alloc)
, velocity(_alloc)
, velocity_w(_alloc) {
(void)_alloc;
motorState.assign( ::unitree_legged_msgs::MotorState_<ContainerAllocator> (_alloc));
footForce.assign(0);
footForceEst.assign(0);
wirelessRemote.assign(0);
eeForceRaw.assign( ::unitree_legged_msgs::Cartesian_<ContainerAllocator> (_alloc));
eeForce.assign( ::unitree_legged_msgs::Cartesian_<ContainerAllocator> (_alloc));
}
typedef uint8_t _levelFlag_type;
_levelFlag_type levelFlag;
typedef uint16_t _commVersion_type;
_commVersion_type commVersion;
typedef uint16_t _robotID_type;
_robotID_type robotID;
typedef uint32_t _SN_type;
_SN_type SN;
typedef uint8_t _bandWidth_type;
_bandWidth_type bandWidth;
typedef ::unitree_legged_msgs::IMU_<ContainerAllocator> _imu_type;
_imu_type imu;
typedef boost::array< ::unitree_legged_msgs::MotorState_<ContainerAllocator> , 20> _motorState_type;
_motorState_type motorState;
typedef boost::array<int16_t, 4> _footForce_type;
_footForce_type footForce;
typedef boost::array<int16_t, 4> _footForceEst_type;
_footForceEst_type footForceEst;
typedef uint32_t _tick_type;
_tick_type tick;
typedef boost::array<uint8_t, 40> _wirelessRemote_type;
_wirelessRemote_type wirelessRemote;
typedef uint32_t _reserve_type;
_reserve_type reserve;
typedef uint32_t _crc_type;
_crc_type crc;
typedef boost::array< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> , 4> _eeForceRaw_type;
_eeForceRaw_type eeForceRaw;
typedef boost::array< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> , 4> _eeForce_type;
_eeForce_type eeForce;
typedef ::unitree_legged_msgs::Cartesian_<ContainerAllocator> _position_type;
_position_type position;
typedef ::unitree_legged_msgs::Cartesian_<ContainerAllocator> _velocity_type;
_velocity_type velocity;
typedef ::unitree_legged_msgs::Cartesian_<ContainerAllocator> _velocity_w_type;
_velocity_w_type velocity_w;
typedef boost::shared_ptr< ::unitree_legged_msgs::LowState_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::unitree_legged_msgs::LowState_<ContainerAllocator> const> ConstPtr;
}; // struct LowState_
typedef ::unitree_legged_msgs::LowState_<std::allocator<void> > LowState;
typedef boost::shared_ptr< ::unitree_legged_msgs::LowState > LowStatePtr;
typedef boost::shared_ptr< ::unitree_legged_msgs::LowState const> LowStateConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::LowState_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::unitree_legged_msgs::LowState_<ContainerAllocator> >::stream(s, "", v);
return s;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::unitree_legged_msgs::LowState_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::LowState_<ContainerAllocator2> & rhs)
{
return lhs.levelFlag == rhs.levelFlag &&
lhs.commVersion == rhs.commVersion &&
lhs.robotID == rhs.robotID &&
lhs.SN == rhs.SN &&
lhs.bandWidth == rhs.bandWidth &&
lhs.imu == rhs.imu &&
lhs.motorState == rhs.motorState &&
lhs.footForce == rhs.footForce &&
lhs.footForceEst == rhs.footForceEst &&
lhs.tick == rhs.tick &&
lhs.wirelessRemote == rhs.wirelessRemote &&
lhs.reserve == rhs.reserve &&
lhs.crc == rhs.crc &&
lhs.eeForceRaw == rhs.eeForceRaw &&
lhs.eeForce == rhs.eeForce &&
lhs.position == rhs.position &&
lhs.velocity == rhs.velocity &&
lhs.velocity_w == rhs.velocity_w;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::unitree_legged_msgs::LowState_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::LowState_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace unitree_legged_msgs
namespace ros
{
namespace message_traits
{
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::LowState_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::LowState_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::LowState_<ContainerAllocator> const>
: FalseType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
{
static const char* value()
{
return "cef9d4f6b5a89bd6330896affb1bca88";
}
static const char* value(const ::unitree_legged_msgs::LowState_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0xcef9d4f6b5a89bd6ULL;
static const uint64_t static_value2 = 0x330896affb1bca88ULL;
};
template<class ContainerAllocator>
struct DataType< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
{
static const char* value()
{
return "unitree_legged_msgs/LowState";
}
static const char* value(const ::unitree_legged_msgs::LowState_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
{
static const char* value()
{
return "uint8 levelFlag\n"
"uint16 commVersion # Old version Aliengo does not have\n"
"uint16 robotID # Old version Aliengo does not have\n"
"uint32 SN # Old version Aliengo does not have\n"
"uint8 bandWidth # Old version Aliengo does not have\n"
"IMU imu\n"
"MotorState[20] motorState\n"
"int16[4] footForce # force sensors # Old version Aliengo is different\n"
"int16[4] footForceEst # force sensors # Old version Aliengo does not have\n"
"uint32 tick # reference real-time from motion controller (unit: us)\n"
"uint8[40] wirelessRemote # wireless commands\n"
"uint32 reserve # Old version Aliengo does not have\n"
"uint32 crc\n"
"\n"
"# Old version Aliengo does not have:\n"
"Cartesian[4] eeForceRaw\n"
"Cartesian[4] eeForce #it's a 1-DOF force infact, but we use 3-DOF here just for visualization \n"
"Cartesian position # will delete\n"
"Cartesian velocity # will delete\n"
"Cartesian velocity_w # will delete\n"
"\n"
"================================================================================\n"
"MSG: unitree_legged_msgs/IMU\n"
"float32[4] quaternion\n"
"float32[3] gyroscope\n"
"float32[3] accelerometer\n"
"int8 temperature\n"
"================================================================================\n"
"MSG: unitree_legged_msgs/MotorState\n"
"uint8 mode # motor current mode \n"
"float32 q # motor current position（rad）\n"
"float32 dq # motor current speed（rad/s）\n"
"float32 ddq # motor current speed（rad/s）\n"
"float32 tauEst # current estimated output torque（N*m）\n"
"float32 q_raw # motor current position（rad）\n"
"float32 dq_raw # motor current speed（rad/s）\n"
"float32 ddq_raw # motor current speed（rad/s）\n"
"int8 temperature # motor temperature（slow conduction of temperature leads to lag）\n"
"uint32[2] reserve\n"
"================================================================================\n"
"MSG: unitree_legged_msgs/Cartesian\n"
"float32 x\n"
"float32 y\n"
"float32 z\n"
;
}
static const char* value(const ::unitree_legged_msgs::LowState_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.levelFlag);
stream.next(m.commVersion);
stream.next(m.robotID);
stream.next(m.SN);
stream.next(m.bandWidth);
stream.next(m.imu);
stream.next(m.motorState);
stream.next(m.footForce);
stream.next(m.footForceEst);
stream.next(m.tick);
stream.next(m.wirelessRemote);
stream.next(m.reserve);
stream.next(m.crc);
stream.next(m.eeForceRaw);
stream.next(m.eeForce);
stream.next(m.position);
stream.next(m.velocity);
stream.next(m.velocity_w);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct LowState_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::LowState_<ContainerAllocator>& v)
{
s << indent << "levelFlag: ";
Printer<uint8_t>::stream(s, indent + " ", v.levelFlag);
s << indent << "commVersion: ";
Printer<uint16_t>::stream(s, indent + " ", v.commVersion);
s << indent << "robotID: ";
Printer<uint16_t>::stream(s, indent + " ", v.robotID);
s << indent << "SN: ";
Printer<uint32_t>::stream(s, indent + " ", v.SN);
s << indent << "bandWidth: ";
Printer<uint8_t>::stream(s, indent + " ", v.bandWidth);
s << indent << "imu: ";
s << std::endl;
Printer< ::unitree_legged_msgs::IMU_<ContainerAllocator> >::stream(s, indent + " ", v.imu);
s << indent << "motorState[]" << std::endl;
for (size_t i = 0; i < v.motorState.size(); ++i)
{
s << indent << " motorState[" << i << "]: ";
s << std::endl;
s << indent;
Printer< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >::stream(s, indent + " ", v.motorState[i]);
}
s << indent << "footForce[]" << std::endl;
for (size_t i = 0; i < v.footForce.size(); ++i)
{
s << indent << " footForce[" << i << "]: ";
Printer<int16_t>::stream(s, indent + " ", v.footForce[i]);
}
s << indent << "footForceEst[]" << std::endl;
for (size_t i = 0; i < v.footForceEst.size(); ++i)
{
s << indent << " footForceEst[" << i << "]: ";
Printer<int16_t>::stream(s, indent + " ", v.footForceEst[i]);
}
s << indent << "tick: ";
Printer<uint32_t>::stream(s, indent + " ", v.tick);
s << indent << "wirelessRemote[]" << std::endl;
for (size_t i = 0; i < v.wirelessRemote.size(); ++i)
{
s << indent << " wirelessRemote[" << i << "]: ";
Printer<uint8_t>::stream(s, indent + " ", v.wirelessRemote[i]);
}
s << indent << "reserve: ";
Printer<uint32_t>::stream(s, indent + " ", v.reserve);
s << indent << "crc: ";
Printer<uint32_t>::stream(s, indent + " ", v.crc);
s << indent << "eeForceRaw[]" << std::endl;
for (size_t i = 0; i < v.eeForceRaw.size(); ++i)
{
s << indent << " eeForceRaw[" << i << "]: ";
s << std::endl;
s << indent;
Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + " ", v.eeForceRaw[i]);
}
s << indent << "eeForce[]" << std::endl;
for (size_t i = 0; i < v.eeForce.size(); ++i)
{
s << indent << " eeForce[" << i << "]: ";
s << std::endl;
s << indent;
Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + " ", v.eeForce[i]);
}
s << indent << "position: ";
s << std::endl;
Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + " ", v.position);
s << indent << "velocity: ";
s << std::endl;
Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + " ", v.velocity);
s << indent << "velocity_w: ";
s << std::endl;
Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + " ", v.velocity_w);
}
};
} // namespace message_operations
} // namespace ros
#endif // UNITREE_LEGGED_MSGS_MESSAGE_LOWSTATE_H

124
include/model/ArmDynKineModel.h
View File

@ -1,124 +0,0 @@
#ifndef ARMDYNCKINEMODEL_H
#define ARMDYNCKINEMODEL_H
#include <rbdl/rbdl.h>
#include <vector>
#include "common/math/mathTypes.h"
#include "quadProgpp/include/QuadProg++.hh"
/* Kinematics only suitable for 6 DOF arm */
class ArmDynKineModel{
public:
ArmDynKineModel(int dof, Vec3 endPosLocal,
double endEffectorMass,
Vec3 endEffectorCom, Mat3 endEffectorInertia);
virtual ~ArmDynKineModel();
Vec6 forDerivativeKinematice(Vec6 q, Vec6 qd);
Vec6 invDerivativeKinematice(Vec6 q, Vec6 twistSpatial);
Vec6 invDynamics(Vec6 q, Vec6 qd, Vec6 qdd, Vec6 endForce);
int getDOF(){return _dof;}
std::vector<double> getJointQMax(){return _jointQMax;}
std::vector<double> getJointQMin(){return _jointQMin;}
std::vector<double> getJointSpeedMax(){return _jointSpeedMax;}
void solveQP(Vec6 twist, Vec6 pastAngle, Vec6 &result, double dt, bool setJ0QdZero, bool setQdZero);
bool invKinematics(HomoMat gDes, Vec6 pastAngle, Vec6 &result, bool checkInWorkSpace = false);
HomoMat forwardKinematics(Vec6 q, int index=6, bool isExp = false); // index from 0 to 5, used for fourth version
bool _useIKSolver = false;
protected:
void _dofCheck();
void _buildModel();
int _dof;
RigidBodyDynamics::Model *_model;
std::vector<unsigned int> _linkID;
std::vector<RigidBodyDynamics::Body> _body;
std::vector<double> _linkMass;
std::vector<Vec3> _linkCom;
std::vector<RigidBodyDynamics::Math::Matrix3d> _linkInertia;
double _endEffectorMass;
Vec3 _endEffectorCom;
Mat3 _endEffectorInertia;
std::vector<RigidBodyDynamics::Joint> _joint;
std::vector<Vec3> _jointAxis;
std::vector<Vec3> _jointPos;
std::vector<double> _jointQMax;
std::vector<double> _jointQMin;
std::vector<double> _jointSpeedMax;
Vec3 _endPosGlobal, _endPosLocal; // based on mount pos
Vec3 _endMountPosGlobal, _endMountPosLocal;
std::vector<Vec6> _theta;
/* inverse derivative kinematics */
RigidBodyDynamics::Math::MatrixNd _Jacobian;
/* inverse dynamics */
std::vector<RigidBodyDynamics::Math::SpatialVector> _endForceVec;
/* inverse kinematics */
bool _checkAngle(Vec6 angle); // check all angles once
Vec6 JointQMax;
Vec6 JointQMin;
Vec6 JointQdMax;
Vec6 JointQdMin;
double theta0[2], theta1[4], theta2[4], theta3[4], theta4[4], theta5[4];
double theta2Bias;
std::vector<Vec3> _linkPosLocal;
std::vector<double> _disJoint;
double _disP13;
Vec3 _vecP45;
Vec3 _vecP14;
Vec3 _vecP13;
Vec3 _vecP34;
Vec3 _joint4Axis;
Vec3 _joint5Axis;
Vec3 _plane324X, _plane324Z;
double _vec34ProjX, _vec34ProjZ;
Vec3 _plane1234Y, _plane1234_X;
Vec3 _vecP04ProjXY;
double _angle213, _angle123, _angle132;
double _angle_x13;
HomoMat _T46;
/* quadprog */
quadprogpp::Matrix<double> G, CE, CI;
quadprogpp::Vector<double> g0, ce0, ci0, x;
Mat6 _G;
Vec6 _g0;
VecX _ci0;
MatX _CI;
};
class Z1DynKineModel : public ArmDynKineModel{
public:
Z1DynKineModel(Vec3 endPosLocal = Vec3::Zero(),
double endEffectorMass = 0.0,
Vec3 endEffectorCom = Vec3::Zero(),
Mat3 endEffectorInertia = Mat3::Zero());
~Z1DynKineModel(){}
};
class Z1PlusDynKineModel : public ArmDynKineModel{
public:
Z1PlusDynKineModel(Vec3 endPosLocal = Vec3::Zero(),
double endEffectorMass = 0.0,
Vec3 endEffectorCom = Vec3::Zero(),
Mat3 endEffectorInertia = Mat3::Zero());
~Z1PlusDynKineModel(){}
};
#endif

75
include/model/ArmModel.h Normal file
View File

@ -0,0 +1,75 @@
#ifndef ARMMODEL_H
#define ARMMODEL_H
#include <vector>
#include "common/math/robotics.h"
#include "common/math/quadProgpp/QuadProg++.hh"
using namespace robo;
class ArmModel{
public:
ArmModel(Vec3 endPosLocal, double endEffectorMass, Vec3 endEffectorCom, Mat3 endEffectorInertia);
~ArmModel(){};
HomoMat forwardKinematics(Vec6 q, int index = 6);
virtual bool inverseKinematics(HomoMat TDes, Vec6 qPast, Vec6& q_result, bool checkInWorkSpace = false);
Mat6 CalcJacobian(Vec6 q);
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;
void jointProtect(Vec6& q, Vec6& qd);
std::vector<double> getJointQMax() {return _jointQMax;}
std::vector<double> getJointQMin() {return _jointQMin;}
std::vector<double> getJointSpeedMax() {return _jointSpeedMax;}
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(){};
bool checkInSingularity(Vec6 q);
bool inverseKinematics(HomoMat TDes, Vec6 qPast, Vec6& q_result, bool checkInWorkSpace = false);
void solveQP(Vec6 twist, Vec6 qPast, Vec6& qd_result, double dt);
private:
void setParam_V3_6();
void setParam_V3_7();//long
void setParam_V3_8();
double _theta2Bias;
};
#endif

82
include/model/ArmReal.h
View File

@ -1,82 +0,0 @@
#ifndef ARM_H
#define ARM_H
#include <vector>
#include "common/math/mathTypes.h"
#include "model/Joint.h"
#include "model/Motor.h"
#include "unitree_arm_sdk/udp.h"
class ArmReal{
public:
ArmReal(int dof, int motorNum, IOPort *ioPort);
virtual ~ArmReal();
void setJointKp(std::vector<double> jointKp);
void setJointKd(std::vector<double> jointKd);
void setJointQ(std::vector<double> jointQ);
void setJointDq(std::vector<double> jointDq);
void setJointTau(std::vector<double> jointTau);
void getJointQ(std::vector<double> &jointQState);
void getJointDq(std::vector<double> &jointDqState);
void getJointDDq(std::vector<double> &jointDDqState);
void getJointTau(std::vector<double> &jointTauState);
void getMotorQ(std::vector<double> &motorQ);
void getMotorDq(std::vector<double> &motorDq);
void getMotorTau(std::vector<double> &motorTau);
void armCalibration();
void armCalibration(std::vector<double> motorAngleBias);
bool armComm();
size_t getDof(){return _dof;}
size_t getMotorNum(){return _motorNum;}
// int getMotorNum(){return _motorNum;}
protected:
void _init();
void _getCmd(std::vector<MOTOR_send> &cmd);
void _setState(std::vector<MOTOR_recv> &motorState);
void _getMotorQBias();
// void _setComm();
void _setCaliBias();
int _motorNum;
int _dof;
int _commReturn;
bool _commYN, _motorCommYN;
std::vector<Joint*> _joints;
std::vector<double> _jointCaliAngle;
std::vector<double> _motorAngleBias;
std::vector<MOTOR_send> _motorCmd;
std::vector<MOTOR_recv> _motorState;
/* serial or UDP Communication */
IOPort *_ioPort;
};
class z1Real : public ArmReal{
public:
z1Real(IOPort *ioPort);
~z1Real(){}
};
class TigerHeadReal : public ArmReal{
public:
TigerHeadReal(IOPort *ioPort);
~TigerHeadReal(){}
};
class DogHeadReal : public ArmReal{
public:
DogHeadReal(IOPort *ioPort);
~DogHeadReal(){}
};
class TestSerial : public ArmReal{
public:
TestSerial(IOPort *ioPort);
~TestSerial(){}
};
#endif // ARM_H

55
include/model/Joint.h
View File

@ -1,55 +0,0 @@
#ifndef JOINT_H
#define JOINT_H
#include <vector>
#include "model/Motor.h"
#include "common/enumClass.h"
class Joint{
public:
Joint(){}
virtual ~Joint(){}
void setJointKp (double jointKp);
void setJointKd (double jointKd);
void setJointQ (double jointQ);
void setJointDq (double jointDq);
void setJointTau(double jointTau);
double getJointQ();
double getJointDq();
double getJointDDq();
double getJointTau();
void getMotorQ(std::vector<double> &motorQ);
void getMotorDq(std::vector<double> &motorDq);
void getMotorDDq(std::vector<double> &motorDDq);
void getMotorTau(std::vector<double> &motorTau);
void getCmd(std::vector<MOTOR_send> &cmd);
void setState(std::vector<MOTOR_recv> &state);
void setCaliBias(double cali, std::vector<double> bias);
protected:
JointMotorType _motorType;
int _motorNum;
double _jointInitAngle;
std::vector<Motor*> _motor;
};
class Jointz1 : public Joint{
public:
Jointz1(int motorID, double direction);
Jointz1(int motorID0, int motorID1, double direction0, double direction1);
Jointz1(int motorID, int motorMsgOrder, double direction);
Jointz1(int motorID0, int motorID1, int motorMsgOrder0, int motorMsgOrder1, double direction0, double direction1);
~Jointz1(){}
};
class JointTigerHead : public Joint{
public:
JointTigerHead(int motorID, double direction);
JointTigerHead(int motorID, int motorMsgOrder, double direction);
~JointTigerHead(){}
};
#endif // JOINT_H

56
include/model/Motor.h
View File

@ -1,56 +0,0 @@
#ifndef MOTOR_H
#define MOTOR_H
#include <vector>
#include "common/enumClass.h"
#include "unitree_arm_sdk/common/arm_motor_common.h"
class Motor{
public:
Motor(int id, int motorMsgOrder, double reductionRatio, MotorMountType type, double direction);
virtual ~Motor(){}
void setMotorKp (double jointKp);
void setMotorKd (double jointKd);
void setMotorQ (double jointQ);
void setMotorDq (double jointDq);
void setMotorTau(double jointTau);
double getJointQ(){return _qJoint;}
double getJointDq(){return _dqJoint;}
double getJointDDq(){return _ddqJoint;}
double getJointTau(){return _tauJoint;}
void getMotorQ(std::vector<double> &motorQ);
void getMotorDq(std::vector<double> &motorDq);
void getMotorDDq(std::vector<double> &motorDDq);
void getMotorTau(std::vector<double> &motorTau);
void setQBias(std::vector<double> qBias);
void getCmd(std::vector<MOTOR_send> &cmd);
void setState(std::vector<MOTOR_recv> &state);
protected:
int _id;
int _motorMsgOrder;
double _kp, _kd, _q, _dq, _tau;
double _qJoint, _dqJoint, _ddqJoint, _tauJoint;
double _qMotor, _dqMotor, _ddqMotor, _tauMotor;
double _reductionRatio;
double _directon;
double _qBias;
MotorMountType _type;
};
class Motorz1 : public Motor{
public:
Motorz1(int id, int motorMsgOrder, MotorMountType type, double direction);
~Motorz1(){}
};
class MotorGo1 : public Motor{
public:
MotorGo1(int id, int motorMsgOrder, MotorMountType type, double direction);
~MotorGo1(){}
};
#endif // MOTOR_H

45
include/trajectory/CartesionSpaceTraj.h
View File

@ -1,45 +0,0 @@
#ifndef CartesionSpaceTraj_H
#define CartesionSpaceTraj_H
#include <vector>
#include <trajectory/Trajectory.h>
#include <string>
#include "control/CtrlComponents.h"
class CartesionSpaceTraj : public Trajectory{
public:
CartesionSpaceTraj(CtrlComponents *ctrlComp);
CartesionSpaceTraj(ArmDynKineModel *armModel);
CartesionSpaceTraj(ArmDynKineModel *armModel, CSVTool *csvState);
~CartesionSpaceTraj(){}
void setCartesionTraj(Vec6 startP, Vec6 endP, double oriSpeed, double posSpeed);
void setCartesionTraj(Vec6 startP, Vec6 middleP, double middleS, Vec6 endP, double oriSpeed, double posSpeed);
bool getCartesionCmd(Vec6 pastPosture, Vec6 &endPosture, Vec6 &endTwist);
void setCartesionTrajMoveC(Vec6 startP, Vec6 middleP, Vec6 endP, double oriSpeed, double posSpeed);
bool getCartesionCmdMoveC(Vec6 pastPosture, Vec6 &endPosture, Vec6 &endTwist);
double _radius;
private:
void _centerCircle(Vec3 p1, Vec3 p2, Vec3 p3);
bool _checkPostureValid(Vec6 endPosture, int pointOrder);
void _generateA345();
double _pathTimeTemp; // path total time
double _s, _sDot; // [0, 1]
double _a3, _a4, _a5; // parameters of path
int _trajOrder; // The order of trajectory curve
std::vector<Vec6> _curveParam;
/* MoveC instruct */
Vec3 _center;
double _theta;
Vec3 _axis_x;
Vec3 _axis_y;
Vec3 _axis_z;
};
#endif // JOINTSPACETRAJ_H

16
include/trajectory/EndCircleTraj.h
View File

@ -1,13 +1,11 @@
#ifndef ENDCIRCLETRAJ_H
#define ENDCIRCLETRAJ_H
#include "model/ArmDynKineModel.h"
#include "trajectory/EndHomoTraj.h"
class EndCircleTraj: public EndHomoTraj{
public:
EndCircleTraj(CtrlComponents *ctrlComp);
EndCircleTraj(ArmDynKineModel *armModel);
~EndCircleTraj(){}
void setEndRoundTraj(HomoMat startHomo, Vec3 axisPointFromInit,
@ -16,21 +14,17 @@ public:
void setEndRoundTraj(std::string stateName, Vec3 axisPointFromInit,
Vec3 axisDirection, double maxSpeed, double angle,
bool keepOrientation = true);
void setEndRoundTraj(Vec6 startP, Vec6 middleP, Vec6 endP,
double maxSpeed);
void setEndRoundTraj(Vec6 startP, Vec6 middleP, Vec6 endP, double speed);
private:
void _centerCircle(Vec3 p1, Vec3 p2, Vec3 p3);
bool _getEndTraj(HomoMat &homo, Vec6 &twist);
Vec3 _center;
double _radius;
Vec3 _center, _omegaAxis;
double _radius, _theta;
Vec6 _middlePosture;
Vec6 _middleQ;
HomoMat _middleHomo;
HomoMat _centerHomo;
HomoMat _initHomoToCenter;
HomoMat _initHomoToCenter, _middleHomo, _centerHomo;
RotMat _initOri;
double _maxSpeed, _goalAngle, _speed, _angle;
Vec3 _omegaAxis;
bool _keepOrientation;
};

14
include/trajectory/EndHomoTraj.h
View File

@ -1,14 +1,13 @@
#ifndef ENDHOMOTRAJ_H
#define ENDHOMOTRAJ_H
#include "model/ArmDynKineModel.h"
#include "model/ArmModel.h"
#include "trajectory/Trajectory.h"
#include "trajectory/SCurve.h"
class EndHomoTraj : public Trajectory{
public:
EndHomoTraj(CtrlComponents *ctrlComp);
EndHomoTraj(ArmDynKineModel *armModel);
virtual ~EndHomoTraj();
bool getJointCmd(Vec6 &q, Vec6 &qd);
bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd);
@ -19,16 +18,7 @@ protected:
HomoMat _cmdHomo;
Vec6 _cmdTwist;
Vec6 _deltaPosture;
Vec3 _omgtheta;
double _theta;
Mat3 _startR, _endR, _delatR, _currentR;
Vec3 _startp, _endp, _currentp;
SCurve *_sCurve;
private:
bool checkInSingularity();
void _checkAngleValid(const Vec6 &q, int pointOrder);
bool _checkJointAngleValid(const double &q, int jointOrder);
SCurve _sCurve;
};
#endif // ENDHOMOTRAJ_H

20
include/trajectory/EndLineTraj.h
View File

@ -7,24 +7,18 @@
class EndLineTraj : public EndHomoTraj{
public:
EndLineTraj(CtrlComponents *ctrlComp);
EndLineTraj(ArmDynKineModel *armModel);
~EndLineTraj(){}
void setEndLineTraj(HomoMat startHomo, Vec3 deltaPos, Vec3 deltaOri, double maxMovingSpeed, double maxTurningSpeed);
void setEndLineTraj(std::string stateName, Vec3 deltaPos, Vec3 deltaOri, double maxMovingSpeed, double maxTurningSpeed);
void setEndLineTraj(Vec6 startP, Vec6 endP, double maxMovingSpeed, double maxTurningSpeed);
void setEndLineTraj(std::string startName, std::string endName, double maxMovingSpeed, double maxTurningSpeed);
bool setEndLineTraj(Vec6 startP, Vec6 endP, double speed);
bool setEndLineTraj(HomoMat startHomo, Vec3 deltaPos, Vec3 deltaOri, double speed);
bool setEndLineTraj(std::string stateName, Vec3 deltaPos, Vec3 deltaOri, double speed);
bool setEndLineTraj(std::string startName, std::string endName, double speed);
private:
bool _getEndTraj(HomoMat &homo, Vec6 &twist);
// SCurve _sCurves[2]; // 0: position, 1: orientation
Vec3 _movingAxis, _turningAxis;
double _movingDistance, _turningAngle;
Vec3 _currentDistance, _currentAngle;
Vec3 _currentVelocity, _currentOmega;
SCurve _posCurve;
SCurve _oriCurve;
Vec3 _currentDistance, _currentAngle, _omgtheta;
Mat3 _startR, _endR, _delatR, _currentR;
Vec3 _startp, _endp, _deltap, _currentp;
};
#endif

27
include/trajectory/JointSpaceTraj.h
View File

@ -10,36 +10,19 @@
class JointSpaceTraj : public Trajectory{
public:
JointSpaceTraj(CtrlComponents *ctrlComp);
JointSpaceTraj(ArmDynKineModel *armModel);
JointSpaceTraj(ArmDynKineModel *armModel, CSVTool *csvState);
~JointSpaceTraj(){}
void setGripper(double startQ, double endQ);
bool getJointCmd(Vec6 &q, Vec6 &qd);
bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd);
void setGripper(double startQ, double endQ, double speed = M_PI);
void setJointTraj(Vec6 startQ, Vec6 endQ, double speed);
bool setJointTraj(Vec6 startQ, std::string endName, double speed);
void setJointTraj(std::string startName, std::string endName, double speed);
void setJointTraj(Vec6 startQ, Vec6 middleQ, double middleS, Vec6 endQ, double speed);
void setJointTraj(std::string startName, std::string middleName, double middleS, std::string endName, double speed);
void setJointTraj(std::vector<Vec6> stateQ, std::vector<double> stateS, double pathTime);
void setJointTraj(std::vector<std::string> stateName, std::vector<double> stateS, double pathTime);
bool setJointTraj(std::string startName, std::string endName, double speed);
private:
void _checkAngleValid(const Vec6 &q, int pointOrder);
bool _checkJointAngleValid(const double &q, int jointOrder);
void _generateA345();
SCurve _jointCurve;
int _jointNum;
double _pathTimeTemp; // path total time
double _s, _sDot; // [0, 1]
double _a3, _a4, _a5; // parameters of path
int _trajOrder; // The order of trajectory curve
std::vector<Vec6> _curveParam;
double ddQMax;
double dddQMax;
};
#endif // JOINTSPACETRAJ_H

1
include/trajectory/SCurve.h
View File

@ -3,7 +3,6 @@
/* 归一化后的s曲线 */
#include <math.h>
#include <iostream>
class SCurve{

7
include/trajectory/StopForTime.h
View File

@ -6,19 +6,12 @@
class StopForTime : public Trajectory{
public:
StopForTime(CtrlComponents *ctrlComp);
StopForTime(ArmDynKineModel *armModel);
StopForTime(ArmDynKineModel *armModel, CSVTool *csvState);
~StopForTime(){}
bool getJointCmd(Vec6 &q, Vec6 &qd);
bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd);
void setStop(Vec6 stopQ, double stopTime);
void setStop(std::string stateName, double stopTime);
private:
// Vec6 _stopQ;
// double _stopTime;
};
#endif // STOPFORTIME_H

113
include/trajectory/Trajectory.h
View File

@ -1,124 +1,47 @@
#ifndef TRAJECTORY_H
#define TRAJECTORY_H
#include "common/math/mathTypes.h"
#include "common/math/mathTools.h"
#include "control/CtrlComponents.h"
#include "unitree_arm_sdk/timer.h"
#include "common/utilities/timer.h"
class Trajectory{
public:
Trajectory(CtrlComponents *ctrlComp){
_ctrlComp = ctrlComp;
_armModel = ctrlComp->armModel;
_csvState = ctrlComp->stateCSV;
_dof = _armModel->getDOF();
_hasKinematic = true;
_jointMaxQ = _armModel->getJointQMax();
_jointMinQ = _armModel->getJointQMin();
_jointMaxSpeed = _armModel->getJointSpeedMax();
}
Trajectory(ArmDynKineModel *armModel){
_ctrlComp = nullptr;
_armModel = armModel;
_csvState = nullptr;
_dof = armModel->getDOF();
if(_dof == 6){
_hasKinematic = true;
}
_jointMaxQ = _armModel->getJointQMax();
_jointMinQ = _armModel->getJointQMin();
_jointMaxSpeed = _armModel->getJointSpeedMax();
}
Trajectory(ArmDynKineModel *armModel, CSVTool *csvState):Trajectory(armModel){
_csvState = csvState;
}
Trajectory(CtrlComponents *ctrlComp);
virtual ~Trajectory(){}
virtual bool getJointCmd(Vec6 &q, Vec6 &qd){return false;};
virtual bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd){return false;};
virtual bool getCartesionCmd(Vec6 pastPosture, Vec6 &endPosture, Vec6 &endTwist){return false;};
void restart(){
_pathStarted = false;
_reached = false;
_qPast = _startQ;
}
virtual void setGripper(double startQ, double endQ){
_gripperStartQ = startQ;
_gripperEndQ = endQ;
}
void restart();
virtual void setGripper(double startQ, double endQ, double speed = M_PI);
bool correctYN(){return _settingCorrect;}
Vec6 getStartQ(){return _startQ;}
Vec6 getEndQ(){return _endQ;}
double getEndGripperQ(){return _gripperEndQ;};
double getStartGripperQ(){return _gripperStartQ;};
HomoMat getStartHomo(){
if(_hasKinematic){
return _startHomo;
}else{
std::cout << "[ERROR] This trajectory do not have kinematics" << std::endl;
exit(-1);
}
}
HomoMat getEndHomo(){
if(_hasKinematic){
return _endHomo;
}else{
std::cout << "[ERROR] This trajectory do not have kinematics" << std::endl;
exit(-1);
}
}
Vec6 getEndPosture(){
if(_hasKinematic){
return _endPosture;
}else{
std::cout << "[ERROR] This trajectory do not have kinematics" << std::endl;
exit(-1);
}
}
double getEndGripperQ(){return _endGripperQ;};
double getStartGripperQ(){return _startGripperQ;};
HomoMat getStartHomo(){return _startHomo;};
HomoMat getEndHomo(){return _endHomo;};
Vec6 getEndPosture(){return _endPosture;};
double getPathTime(){return _pathTime;}
protected:
void _runTime();
CtrlComponents *_ctrlComp;
ArmDynKineModel *_armModel;
ArmModel *_armModel;
CSVTool *_csvState;
bool _pathStarted = false;
bool _reached = false;
bool _paused = false;
bool _settingCorrect = true;
double _startTime;
double _currentTime;
double _pathTime;
double _tCost;
double _startTime, _currentTime, _pathTime, _tCost;
Vec6 _qPast;
Vec6 _startPosture, _endPosture;
Vec6 _startQ, _endQ;
Vec6 _startQ, _endQ, _deltaQ;
HomoMat _startHomo, _endHomo;
Vec6 _startPosture, _endPosture, _deltaPosture;
double _startGripperQ, _endGripperQ;
double _gripperStartQ;
double _gripperEndQ;
size_t _dof;
bool _hasKinematic;
std::vector<double> _jointMaxQ;
std::vector<double> _jointMinQ;
std::vector<double> _jointMaxSpeed;
void _runTime(){
_currentTime = getTimeSecond();
if(!_pathStarted){
_pathStarted = true;
_startTime = _currentTime;
_tCost = 0;
}
_tCost = _currentTime - _startTime;
_reached = (_tCost>_pathTime) ? true : false;
_tCost = (_tCost>_pathTime) ? _pathTime : _tCost;
}
};
#endif // TRAJECTORY_H

6
include/trajectory/TrajectoryManager.h
View File

@ -11,8 +11,6 @@
class TrajectoryManager{
public:
TrajectoryManager(CtrlComponents *ctrlComp);
TrajectoryManager(ArmDynKineModel *armModel);
TrajectoryManager(ArmDynKineModel *armModel, CSVTool *csvState);
~TrajectoryManager(){}
bool getJointCmd(Vec6 &q, Vec6 &qd);
bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd);
@ -23,10 +21,10 @@ public:
Vec6 getStartQ();
Vec6 getEndQ();
Vec6 getEndPosture();
double getEndGripperQ();
double getStartGripperQ();
double getEndGripperQ();
HomoMat getEndHomo();
// bool checkTrajectoryContinuous();
size_t size() {return _trajVec.size();} ;
private:
CtrlComponents *_ctrlComp;
JointSpaceTraj *_trajBack;

BIN
lib/libUnitree_motor_SDK_Linux64.so
View File

Binary file not shown.

BIN
lib/libUnitree_motor_SDK_Linux64_EPOLL.so
View File

Binary file not shown.

BIN
lib/libZ1_ROS_Linux64.so
View File

Binary file not shown.

BIN
lib/libZ1_UDP_Linux64.so
View File

Binary file not shown.

185
main.cpp
View File

@ -1,38 +1,29 @@
#include <iostream>
#include <unistd.h>
#include <csignal>
#include <sched.h>
#include <iomanip>
#include "control/CtrlComponents.h"
#include "model/ArmDynKineModel.h"
#include "interface/IOUDP.h"
#include "FSM/FiniteStateMachine.h"
#include "FSM/State_Passive.h"
#include "FSM/State_BackToStart.h"
#include "FSM/State_Calibration.h"
#include "FSM/State_Cartesian.h"
#include "FSM/State_JointSpace.h"
#include "FSM/State_MoveJ.h"
#include "FSM/State_MoveL.h"
#include "FSM/State_MoveC.h"
#include "FSM/State_Dance.h"
#include "FSM/State_JointSpace.h"
#include "FSM/State_Passive.h"
#include "FSM/State_ToState.h"
#include "FSM/State_SaveState.h"
#include "FSM/State_Teach.h"
#include "FSM/State_TeachRepeat.h"
#include "FSM/State_ToState.h"
#include "FSM/State_Trajectory.h"
#include "FSM/State_Calibration.h"
#include "FSM/State_LowCmd.h"
#include "FSM/FiniteStateMachine.h"
#include "FSM/State_SetTraj.h"
#include "unitree_arm_sdk/unitree_arm_sdk.h"
const std::string Z1_CTRL_VERSION = "2022.11.11";
bool running = true;
// over watch the ctrl+c command
void ShutDown(int sig){
std::cout << "[STATE] stop the controller" << std::endl;
running = false;
std::cout << "[STATE] stop the controller" << std::endl;
}
//set real-time program
@ -46,105 +37,88 @@ void setProcessScheduler(){
}
int main(int argc, char **argv){
if(argc == 2) {
if((strcmp(argv[1], "-v") == 0) || (strcmp(argv[1], "--version") == 0)){
std::cout << "Version z1_controller: "<< Z1_CTRL_VERSION<<std::endl;
return 0;
}
}
/* set real-time process */
setProcessScheduler();
/* set the print format */
std::cout << std::fixed << std::setprecision(3);
EmptyAction emptyAction((int)ArmFSMStateName::INVALID);
std::vector<KeyAction*> events;
CmdPanel *cmdPanel;
CtrlComponents *ctrlComp = new CtrlComponents();
if(ctrlComp->ctrl == Control::_SDK)
{
events.push_back(new StateAction("`", (int)ArmFSMStateName::BACKTOSTART));
events.push_back(new StateAction("1", (int)ArmFSMStateName::PASSIVE));
events.push_back(new StateAction("2", (int)ArmFSMStateName::JOINTCTRL));
events.push_back(new StateAction("3", (int)ArmFSMStateName::CARTESIAN));
events.push_back(new StateAction("4", (int)ArmFSMStateName::MOVEJ));
events.push_back(new StateAction("5", (int)ArmFSMStateName::MOVEL));
events.push_back(new StateAction("6", (int)ArmFSMStateName::MOVEC));
events.push_back(new StateAction("7", (int)ArmFSMStateName::TEACH));
events.push_back(new StateAction("8", (int)ArmFSMStateName::TEACHREPEAT));
events.push_back(new StateAction("9", (int)ArmFSMStateName::SAVESTATE));
events.push_back(new StateAction("0", (int)ArmFSMStateName::TOSTATE));
events.push_back(new StateAction("-", (int)ArmFSMStateName::TRAJECTORY));
events.push_back(new StateAction("=", (int)ArmFSMStateName::CALIBRATION));
events.push_back(new StateAction("]", (int)ArmFSMStateName::NEXT));
events.push_back(new StateAction("/", (int)ArmFSMStateName::LOWCMD));
events.push_back(new StateAction("l", (int)ArmFSMStateName::SETTRAJ));
events.push_back(new ValueAction("q", "a", 1.0));
events.push_back(new ValueAction("w", "s", 1.0));
events.push_back(new ValueAction("e", "d", 1.0));
events.push_back(new ValueAction("r", "f", 1.0));
events.push_back(new ValueAction("t", "g", 1.0));
events.push_back(new ValueAction("y", "h", 1.0));
events.push_back(new ValueAction("down", "up", 1.0));
cmdPanel = new UnitreeKeyboardUDPRecv(events, emptyAction);
}else if(ctrlComp->ctrl == Control::_KEYBOARD){
events.push_back(new StateAction("`", (int)ArmFSMStateName::BACKTOSTART));
events.push_back(new StateAction("1", (int)ArmFSMStateName::PASSIVE));
events.push_back(new StateAction("2", (int)ArmFSMStateName::JOINTCTRL));
events.push_back(new StateAction("3", (int)ArmFSMStateName::CARTESIAN));
events.push_back(new StateAction("4", (int)ArmFSMStateName::MOVEJ));
events.push_back(new StateAction("5", (int)ArmFSMStateName::MOVEL));
events.push_back(new StateAction("6", (int)ArmFSMStateName::MOVEC));
events.push_back(new StateAction("7", (int)ArmFSMStateName::TEACH));
events.push_back(new StateAction("8", (int)ArmFSMStateName::TEACHREPEAT));
events.push_back(new StateAction("9", (int)ArmFSMStateName::SAVESTATE));
events.push_back(new StateAction("0", (int)ArmFSMStateName::TOSTATE));
events.push_back(new StateAction("-", (int)ArmFSMStateName::TRAJECTORY));
events.push_back(new StateAction("=", (int)ArmFSMStateName::CALIBRATION));
events.push_back(new StateAction("]", (int)ArmFSMStateName::NEXT));
events.push_back(new ValueAction("q", "a", 1.0));
events.push_back(new ValueAction("w", "s", 1.0));
events.push_back(new ValueAction("e", "d", 1.0));
events.push_back(new ValueAction("r", "f", 1.0));
events.push_back(new ValueAction("t", "g", 1.0));
events.push_back(new ValueAction("y", "h", 1.0));
events.push_back(new ValueAction("down", "up", 1.0));
cmdPanel = new Keyboard(events, emptyAction);
}else if(ctrlComp->ctrl == Control::_JOYSTICK){
events.push_back(new StateAction("r2x", (int)ArmFSMStateName::TRAJECTORY));
events.push_back(new StateAction("l12", (int)ArmFSMStateName::PASSIVE));
events.push_back(new StateAction("r2", (int)ArmFSMStateName::JOINTCTRL));
events.push_back(new StateAction("r1", (int)ArmFSMStateName::CARTESIAN));
events.push_back(new StateAction("select", (int)ArmFSMStateName::BACKTOSTART));
events.push_back(new ValueAction("left_up", "left_down", 1.0));//Tran_Y
events.push_back(new ValueAction("left_left", "left_right", -1.0));//Tran_X, inverse
events.push_back(new ValueAction("up", "down", 1.0));//Tran_Z
events.push_back(new ValueAction("right_up", "right_down", 1.0));//Rot_Y
events.push_back(new ValueAction("right_left", "right_right", 1.0));//Rot_x
events.push_back(new ValueAction("Y", "A", 1.0));//Rot_Z
events.push_back(new ValueAction("right", "left", 1.0));//girpper, close-open
cmdPanel = new UnitreeJoystick(events, emptyAction);
// control method
if(argc == 2) {
if(argv[1][0] == 'k'){
ctrlComp->ctrl = Control::KEYBOARD;
}else if(argv[1][0] == 's'){
ctrlComp->ctrl = Control::SDK;
}else if(argv[1][0] == 'j'){
ctrlComp->ctrl = Control::JOYSTICK;
}
}
#ifdef RUN_ROS
#ifdef COMPILE_WITH_SIMULATION
ros::init(argc, argv, "z1_controller");
#endif // RUN_ROS
#endif
ctrlComp->dof = 6;
// ctrlComp->isPlot = true;
ctrlComp->dt = 1.0/250.;
ctrlComp->armConfigPath = "../config/";
ctrlComp->stateCSV = new CSVTool("../config/savedArmStates.csv");
ctrlComp->running = &running;
#ifdef UDP
ctrlComp->ioInter = new IOUDP(cmdPanel, ctrlComp->ctrl_IP.c_str(), ctrlComp->_hasGripper);
#elif defined ROS
ctrlComp->ioInter = new IOROS(cmdPanel, ctrlComp->_hasGripper);
#endif
ctrlComp->dt = 0.004;
ctrlComp->geneObj();
if(ctrlComp->ctrl == Control::SDK){
ctrlComp->cmdPanel = new ARMSDK(events, emptyAction, "127.0.0.1", 8072, 0.002);
}else if(ctrlComp->ctrl == Control::KEYBOARD){
events.push_back(new StateAction("`", (int)ArmFSMStateName::BACKTOSTART));
events.push_back(new StateAction("1", (int)ArmFSMStateName::PASSIVE));
events.push_back(new StateAction("2", (int)ArmFSMStateName::JOINTCTRL));
events.push_back(new StateAction("3", (int)ArmFSMStateName::CARTESIAN));
events.push_back(new StateAction("4", (int)ArmFSMStateName::MOVEJ));
events.push_back(new StateAction("5", (int)ArmFSMStateName::MOVEL));
events.push_back(new StateAction("6", (int)ArmFSMStateName::MOVEC));
events.push_back(new StateAction("7", (int)ArmFSMStateName::TEACH));
events.push_back(new StateAction("8", (int)ArmFSMStateName::TEACHREPEAT));
events.push_back(new StateAction("9", (int)ArmFSMStateName::SAVESTATE));
events.push_back(new StateAction("0", (int)ArmFSMStateName::TOSTATE));
events.push_back(new StateAction("-", (int)ArmFSMStateName::TRAJECTORY));
events.push_back(new StateAction("=", (int)ArmFSMStateName::CALIBRATION));
std::vector<BaseState*> states;
states.push_back(new State_Passive(ctrlComp)); // First state in states is the begining state for FSM
events.push_back(new ValueAction("q", "a", 0.5));
events.push_back(new ValueAction("w", "s", 0.5));
events.push_back(new ValueAction("e", "d", 0.5));
events.push_back(new ValueAction("r", "f", 0.5));
events.push_back(new ValueAction("t", "g", 0.5));
events.push_back(new ValueAction("y", "h", 0.5));
events.push_back(new ValueAction("down", "up", 1.));
ctrlComp->cmdPanel = new Keyboard(events, emptyAction);
}else if(ctrlComp->ctrl == Control::JOYSTICK){
events.push_back(new StateAction("r2x", (int)ArmFSMStateName::TRAJECTORY));
events.push_back(new StateAction("l12", (int)ArmFSMStateName::PASSIVE));
events.push_back(new StateAction("r2", (int)ArmFSMStateName::JOINTCTRL));
events.push_back(new StateAction("r1", (int)ArmFSMStateName::CARTESIAN));
events.push_back(new StateAction("select", (int)ArmFSMStateName::BACKTOSTART));
events.push_back(new ValueAction("left_up", "left_down", 0.5));//Tran_Y
events.push_back(new ValueAction("left_right", "left_left", 0.5));//Tran_X, inverse
events.push_back(new ValueAction("up", "down", -0.5));//Tran_Z, inverse
events.push_back(new ValueAction("right_up", "right_down", 0.5));//Rot_Y
events.push_back(new ValueAction("right_left", "right_right", 0.5));//Rot_x
events.push_back(new ValueAction("Y", "A", 0.5));//Rot_Z
events.push_back(new ValueAction("right", "left", 1.0));//girpper, close-open
ctrlComp->cmdPanel = new UnitreeJoystick(events, emptyAction);
}
std::vector<FSMState*> states;
states.push_back(new State_Passive(ctrlComp));
states.push_back(new State_BackToStart(ctrlComp));
states.push_back(new State_JointSpace(ctrlComp));
states.push_back(new State_Cartesian(ctrlComp));
@ -160,19 +134,16 @@ int main(int argc, char **argv){
states.push_back(new State_Calibration(ctrlComp));
states.push_back(new State_SetTraj(ctrlComp));
FiniteStateMachine fsm(states, cmdPanel, 0, ctrlComp->dt);
FiniteStateMachine *fsm;
fsm = new FiniteStateMachine(states, ctrlComp);
ctrlComp->running = &running;
signal(SIGINT, ShutDown);
while(running){
usleep(100000);
}
#ifdef COMPILE_DEBUG
ctrlComp->writeData();
#endif
delete fsm;
delete ctrlComp;
return 0;
}
}

3
package.xml
View File

@ -10,15 +10,12 @@
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>urdf</build_depend>
<build_depend>xacro</build_depend>
<build_export_depend>roscp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>urdf</build_export_depend>
<build_export_depend>xacro</build_export_depend>
<exec_depend>roscp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>urdf</exec_depend>
<exec_depend>xacro</exec_depend>

33
thirdparty/quadProgpp/src/Array.cc vendored
View File

@ -1,33 +0,0 @@
// $Id: Array.cc 201 2008-05-18 19:47:38Z digasper $
// This file is part of QuadProg++:
// Copyright (C) 2006--2009 Luca Di Gaspero.
//
// This software may be modified and distributed under the terms
// of the MIT license. See the LICENSE file for details.
#include "quadProgpp/include/Array.hh"
/**
Index utilities
*/
namespace quadprogpp {
std::set<unsigned int> seq(unsigned int s, unsigned int e)
{
std::set<unsigned int> tmp;
for (unsigned int i = s; i <= e; i++)
tmp.insert(i);
return tmp;
}
std::set<unsigned int> singleton(unsigned int i)
{
std::set<unsigned int> tmp;
tmp.insert(i);
return tmp;
}
} // namespace quadprogpp

790
thirdparty/quadProgpp/src/QuadProg++.cc vendored
View File

@ -1,790 +0,0 @@
/*
File $Id: QuadProg++.cc 232 2007-06-21 12:29:00Z digasper $
Author: Luca Di Gaspero
DIEGM - University of Udine, Italy
luca.digaspero@uniud.it
http://www.diegm.uniud.it/digaspero/
This software may be modified and distributed under the terms
of the MIT license. See the LICENSE file for details.
*/
#include <iostream>
#include <algorithm>
#include <cmath>
#include <limits>
#include <sstream>
#include <stdexcept>
#include "quadProgpp/include/QuadProg++.hh"
//#define TRACE_SOLVER
namespace quadprogpp {
// Utility functions for updating some data needed by the solution method
void compute_d(Vector<double>& d, const Matrix<double>& J, const Vector<double>& np);
void update_z(Vector<double>& z, const Matrix<double>& J, const Vector<double>& d, int iq);
void update_r(const Matrix<double>& R, Vector<double>& r, const Vector<double>& d, int iq);
bool add_constraint(Matrix<double>& R, Matrix<double>& J, Vector<double>& d, unsigned int& iq, double& rnorm);
void delete_constraint(Matrix<double>& R, Matrix<double>& J, Vector<int>& A, Vector<double>& u, unsigned int n, int p, unsigned int& iq, int l);
// Utility functions for computing the Cholesky decomposition and solving
// linear systems
void cholesky_decomposition(Matrix<double>& A);
void cholesky_solve(const Matrix<double>& L, Vector<double>& x, const Vector<double>& b);
void forward_elimination(const Matrix<double>& L, Vector<double>& y, const Vector<double>& b);
void backward_elimination(const Matrix<double>& U, Vector<double>& x, const Vector<double>& y);
// Utility functions for computing the scalar product and the euclidean
// distance between two numbers
double scalar_product(const Vector<double>& x, const Vector<double>& y);
double distance(double a, double b);
// Utility functions for printing vectors and matrices
void print_matrix(const char* name, const Matrix<double>& A, int n = -1, int m = -1);
template<typename T>
void print_vector(const char* name, const Vector<T>& v, int n = -1);
// The Solving function, implementing the Goldfarb-Idnani method
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)
{
std::ostringstream msg;
unsigned int n = G.ncols(), p = CE.ncols(), m = CI.ncols();
if (G.nrows() != n)
{
msg << "The matrix G is not a squared matrix (" << G.nrows() << " x " << G.ncols() << ")";
throw std::logic_error(msg.str());
}
if (CE.nrows() != n)
{
msg << "The matrix CE is incompatible (incorrect number of rows " << CE.nrows() << " , expecting " << n << ")";
throw std::logic_error(msg.str());
}
if (ce0.size() != p)
{
msg << "The vector ce0 is incompatible (incorrect dimension " << ce0.size() << ", expecting " << p << ")";
throw std::logic_error(msg.str());
}
if (CI.nrows() != n)
{
msg << "The matrix CI is incompatible (incorrect number of rows " << CI.nrows() << " , expecting " << n << ")";
throw std::logic_error(msg.str());
}
if (ci0.size() != m)
{
msg << "The vector ci0 is incompatible (incorrect dimension " << ci0.size() << ", expecting " << m << ")";
throw std::logic_error(msg.str());
}
x.resize(n);
register unsigned int i, j, k, l; /* indices */
int ip; // this is the index of the constraint to be added to the active set
Matrix<double> R(n, n), J(n, n);
Vector<double> s(m + p), z(n), r(m + p), d(n), np(n), u(m + p), x_old(n), u_old(m + p);
double f_value, psi, c1, c2, sum, ss, R_norm;
double inf;
if (std::numeric_limits<double>::has_infinity)
inf = std::numeric_limits<double>::infinity();
else
inf = 1.0E300;
double t, t1, t2; /* t is the step lenght, which is the minimum of the partial step length t1
* and the full step length t2 */
Vector<int> A(m + p), A_old(m + p), iai(m + p);
unsigned int iq, iter = 0;
Vector<bool> iaexcl(m + p);
/* p is the number of equality constraints */
/* m is the number of inequality constraints */
#ifdef TRACE_SOLVER
std::cout << std::endl << "Starting solve_quadprog" << std::endl;
print_matrix("G", G);
print_vector("g0", g0);
print_matrix("CE", CE);
print_vector("ce0", ce0);
print_matrix("CI", CI);
print_vector("ci0", ci0);
#endif
/*
* Preprocessing phase
*/
/* compute the trace of the original matrix G */
c1 = 0.0;
for (i = 0; i < n; i++)
{
c1 += G[i][i];
}
/* decompose the matrix G in the form L^T L */
cholesky_decomposition(G);
#ifdef TRACE_SOLVER
print_matrix("G", G);
#endif
/* initialize the matrix R */
for (i = 0; i < n; i++)
{
d[i] = 0.0;
for (j = 0; j < n; j++)
R[i][j] = 0.0;
}
R_norm = 1.0; /* this variable will hold the norm of the matrix R */
/* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
c2 = 0.0;
for (i = 0; i < n; i++)
{
d[i] = 1.0;
forward_elimination(G, z, d);
for (j = 0; j < n; j++)
J[i][j] = z[j];
c2 += z[i];
d[i] = 0.0;
}
#ifdef TRACE_SOLVER
print_matrix("J", J);
#endif
/* c1 * c2 is an estimate for cond(G) */
/*
* Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x
* this is a feasible point in the dual space
* x = G^-1 * g0
*/
cholesky_solve(G, x, g0);
for (i = 0; i < n; i++)
x[i] = -x[i];
/* and compute the current solution value */
f_value = 0.5 * scalar_product(g0, x);
#ifdef TRACE_SOLVER
std::cout << "Unconstrained solution: " << f_value << std::endl;
print_vector("x", x);
#endif
/* Add equality constraints to the working set A */
iq = 0;
for (i = 0; i < p; i++)
{
for (j = 0; j < n; j++)
np[j] = CE[j][i];
compute_d(d, J, np);
update_z(z, J, d, iq);
update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
print_matrix("R", R, n, iq);
print_vector("z", z);
print_vector("r", r, iq);
print_vector("d", d);
#endif
/* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
becomes feasible */
t2 = 0.0;
if (fabs(scalar_product(z, z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = (-scalar_product(np, x) - ce0[i]) / scalar_product(z, np);
/* set x = x + t2 * z */
for (k = 0; k < n; k++)
x[k] += t2 * z[k];
/* set u = u+ */
u[iq] = t2;
for (k = 0; k < iq; k++)
u[k] -= t2 * r[k];
/* compute the new solution value */
f_value += 0.5 * (t2 * t2) * scalar_product(z, np);
A[i] = -i - 1;
if (!add_constraint(R, J, d, iq, R_norm))
{
// Equality constraints are linearly dependent
throw std::runtime_error("Constraints are linearly dependent");
return f_value;
}
}
/* set iai = K \ A */
for (i = 0; i < m; i++)
iai[i] = i;
l1: iter++;
#ifdef TRACE_SOLVER
print_vector("x", x);
#endif
/* step 1: choose a violated constraint */
for (i = p; i < iq; i++)
{
ip = A[i];
iai[ip] = -1;
}
/* compute s[x] = ci^T * x + ci0 for all elements of K \ A */
ss = 0.0;
psi = 0.0; /* this value will contain the sum of all infeasibilities */
ip = 0; /* ip will be the index of the chosen violated constraint */
for (i = 0; i < m; i++)
{
iaexcl[i] = true;
sum = 0.0;
for (j = 0; j < n; j++)
sum += CI[j][i] * x[j];
sum += ci0[i];
s[i] = sum;
psi += std::min(0.0, sum);
}
#ifdef TRACE_SOLVER
print_vector("s", s, m);
#endif
if (fabs(psi) <= m * std::numeric_limits<double>::epsilon() * c1 * c2* 100.0)
{
/* numerically there are not infeasibilities anymore */
return f_value;
}
/* save old values for u and A */
for (i = 0; i < iq; i++)
{
u_old[i] = u[i];
A_old[i] = A[i];
}
/* and for x */
for (i = 0; i < n; i++)
x_old[i] = x[i];
l2: /* Step 2: check for feasibility and determine a new S-pair */
for (i = 0; i < m; i++)
{
if (s[i] < ss && iai[i] != -1 && iaexcl[i])
{
ss = s[i];
ip = i;
}
}
if (ss >= 0.0)
{
return f_value;
}
/* set np = n[ip] */
for (i = 0; i < n; i++)
np[i] = CI[i][ip];
/* set u = [u 0]^T */
u[iq] = 0.0;
/* add ip to the active set A */
A[iq] = ip;
#ifdef TRACE_SOLVER
std::cout << "Trying with constraint " << ip << std::endl;
print_vector("np", np);
#endif
l2a:/* Step 2a: determine step direction */
/* compute z = H np: the step direction in the primal space (through J, see the paper) */
compute_d(d, J, np);
update_z(z, J, d, iq);
/* compute N* np (if q > 0): the negative of the step direction in the dual space */
update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
std::cout << "Step direction z" << std::endl;
print_vector("z", z);
print_vector("r", r, iq + 1);
print_vector("u", u, iq + 1);
print_vector("d", d);
print_vector("A", A, iq + 1);
#endif
/* Step 2b: compute step length */
l = 0;
/* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
t1 = inf; /* +inf */
/* find the index l s.t. it reaches the minimum of u+[x] / r */
for (k = p; k < iq; k++)
{
if (r[k] > 0.0)
{
if (u[k] / r[k] < t1)
{
t1 = u[k] / r[k];
l = A[k];
}
}
}
/* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
if (fabs(scalar_product(z, z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
{
t2 = -s[ip] / scalar_product(z, np);
if (t2 < 0) // patch suggested by Takano Akio for handling numerical inconsistencies
t2 = inf;
}
else
t2 = inf; /* +inf */
/* the step is chosen as the minimum of t1 and t2 */
t = std::min(t1, t2);
#ifdef TRACE_SOLVER
std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
#endif
/* Step 2c: determine new S-pair and take step: */
/* case (i): no step in primal or dual space */
if (t >= inf)
{
/* QPP is infeasible */
return inf;
}
/* case (ii): step in dual space */
if (t2 >= inf)
{
/* set u = u + t * [-r 1] and drop constraint l from the active set A */
for (k = 0; k < iq; k++)
u[k] -= t * r[k];
u[iq] += t;
iai[l] = l;
delete_constraint(R, J, A, u, n, p, iq, l);
#ifdef TRACE_SOLVER
std::cout << " in dual space: "
<< f_value << std::endl;
print_vector("x", x);
print_vector("z", z);
print_vector("A", A, iq + 1);
#endif
goto l2a;
}
/* case (iii): step in primal and dual space */
/* set x = x + t * z */
for (k = 0; k < n; k++)
x[k] += t * z[k];
/* update the solution value */
f_value += t * scalar_product(z, np) * (0.5 * t + u[iq]);
/* u = u + t * [-r 1] */
for (k = 0; k < iq; k++)
u[k] -= t * r[k];
u[iq] += t;
#ifdef TRACE_SOLVER
std::cout << " in both spaces: "
<< f_value << std::endl;
print_vector("x", x);
print_vector("u", u, iq + 1);
print_vector("r", r, iq + 1);
print_vector("A", A, iq + 1);
#endif
if (fabs(t - t2) < std::numeric_limits<double>::epsilon())
{
#ifdef TRACE_SOLVER
std::cout << "Full step has taken " << t << std::endl;
print_vector("x", x);
#endif
/* full step has taken */
/* add constraint ip to the active set*/
if (!add_constraint(R, J, d, iq, R_norm))
{
iaexcl[ip] = false;
delete_constraint(R, J, A, u, n, p, iq, ip);
#ifdef TRACE_SOLVER
print_matrix("R", R);
print_vector("A", A, iq);
print_vector("iai", iai);
#endif
for (i = 0; i < m; i++)
iai[i] = i;
for (i = p; i < iq; i++)
{
A[i] = A_old[i];
u[i] = u_old[i];
iai[A[i]] = -1;
}
for (i = 0; i < n; i++)
x[i] = x_old[i];
goto l2; /* go to step 2 */
}
else
iai[ip] = -1;
#ifdef TRACE_SOLVER
print_matrix("R", R);
print_vector("A", A, iq);
print_vector("iai", iai);
#endif
goto l1;
}
/* a patial step has taken */
#ifdef TRACE_SOLVER
std::cout << "Partial step has taken " << t << std::endl;
print_vector("x", x);
#endif
/* drop constraint l */
iai[l] = l;
delete_constraint(R, J, A, u, n, p, iq, l);
#ifdef TRACE_SOLVER
print_matrix("R", R);
print_vector("A", A, iq);
#endif
/* update s[ip] = CI * x + ci0 */
sum = 0.0;
for (k = 0; k < n; k++)
sum += CI[k][ip] * x[k];
s[ip] = sum + ci0[ip];
#ifdef TRACE_SOLVER
print_vector("s", s, m);
#endif
goto l2a;
}
inline void compute_d(Vector<double>& d, const Matrix<double>& J, const Vector<double>& np)
{
register int i, j, n = d.size();
register double sum;
/* compute d = H^T * np */
for (i = 0; i < n; i++)
{
sum = 0.0;
for (j = 0; j < n; j++)
sum += J[j][i] * np[j];
d[i] = sum;
}
}
inline void update_z(Vector<double>& z, const Matrix<double>& J, const Vector<double>& d, int iq)
{
register int i, j, n = z.size();
/* setting of z = H * d */
for (i = 0; i < n; i++)
{
z[i] = 0.0;
for (j = iq; j < n; j++)
z[i] += J[i][j] * d[j];
}
}
inline void update_r(const Matrix<double>& R, Vector<double>& r, const Vector<double>& d, int iq)
{
register int i, j;
register double sum;
/* setting of r = R^-1 d */
for (i = iq - 1; i >= 0; i--)
{
sum = 0.0;
for (j = i + 1; j < iq; j++)
sum += R[i][j] * r[j];
r[i] = (d[i] - sum) / R[i][i];
}
}
bool add_constraint(Matrix<double>& R, Matrix<double>& J, Vector<double>& d, unsigned int& iq, double& R_norm)
{
unsigned int n = d.size();
#ifdef TRACE_SOLVER
std::cout << "Add constraint " << iq << '/';
#endif
register unsigned int i, j, k;
double cc, ss, h, t1, t2, xny;
/* we have to find the Givens rotation which will reduce the element
d[j] to zero.
if it is already zero we don't have to do anything, except of
decreasing j */
for (j = n - 1; j >= iq + 1; j--)
{
/* The Givens rotation is done with the matrix (cc cs, cs -cc).
If cc is one, then element (j) of d is zero compared with element
(j - 1). Hence we don't have to do anything.
If cc is zero, then we just have to switch column (j) and column (j - 1)
of J. Since we only switch columns in J, we have to be careful how we
update d depending on the sign of gs.
Otherwise we have to apply the Givens rotation to these columns.
The i - 1 element of d has to be updated to h. */
cc = d[j - 1];
ss = d[j];
h = distance(cc, ss);
if (fabs(h) < std::numeric_limits<double>::epsilon()) // h == 0
continue;
d[j] = 0.0;
ss = ss / h;
cc = cc / h;
if (cc < 0.0)
{
cc = -cc;
ss = -ss;
d[j - 1] = -h;
}
else
d[j - 1] = h;
xny = ss / (1.0 + cc);
for (k = 0; k < n; k++)
{
t1 = J[k][j - 1];
t2 = J[k][j];
J[k][j - 1] = t1 * cc + t2 * ss;
J[k][j] = xny * (t1 + J[k][j - 1]) - t2;
}
}
/* update the number of constraints added*/
iq++;
/* To update R we have to put the iq components of the d vector
into column iq - 1 of R
*/
for (i = 0; i < iq; i++)
R[i][iq - 1] = d[i];
#ifdef TRACE_SOLVER
std::cout << iq << std::endl;
print_matrix("R", R, iq, iq);
print_matrix("J", J);
print_vector("d", d, iq);
#endif
if (fabs(d[iq - 1]) <= std::numeric_limits<double>::epsilon() * R_norm)
{
// problem degenerate
return false;
}
R_norm = std::max<double>(R_norm, fabs(d[iq - 1]));
return true;
}
void delete_constraint(Matrix<double>& R, Matrix<double>& J, Vector<int>& A, Vector<double>& u, unsigned int n, int p, unsigned int& iq, int l)
{
#ifdef TRACE_SOLVER
std::cout << "Delete constraint " << l << ' ' << iq;
#endif
register unsigned int i, j, k, qq = 0; // just to prevent warnings from smart compilers
double cc, ss, h, xny, t1, t2;
bool found = false;
/* Find the index qq for active constraint l to be removed */
for (i = p; i < iq; i++)
if (A[i] == l)
{
qq = i;
found = true;
break;
}
if(!found)
{
std::ostringstream os;
os << "Attempt to delete non existing constraint, constraint: " << l;
throw std::invalid_argument(os.str());
}
/* remove the constraint from the active set and the duals */
for (i = qq; i < iq - 1; i++)
{
A[i] = A[i + 1];
u[i] = u[i + 1];
for (j = 0; j < n; j++)
R[j][i] = R[j][i + 1];
}
A[iq - 1] = A[iq];
u[iq - 1] = u[iq];
A[iq] = 0;
u[iq] = 0.0;
for (j = 0; j < iq; j++)
R[j][iq - 1] = 0.0;
/* constraint has been fully removed */
iq--;
#ifdef TRACE_SOLVER
std::cout << '/' << iq << std::endl;
#endif
if (iq == 0)
return;
for (j = qq; j < iq; j++)
{
cc = R[j][j];
ss = R[j + 1][j];
h = distance(cc, ss);
if (fabs(h) < std::numeric_limits<double>::epsilon()) // h == 0
continue;
cc = cc / h;
ss = ss / h;
R[j + 1][j] = 0.0;
if (cc < 0.0)
{
R[j][j] = -h;
cc = -cc;
ss = -ss;
}
else
R[j][j] = h;
xny = ss / (1.0 + cc);
for (k = j + 1; k < iq; k++)
{
t1 = R[j][k];
t2 = R[j + 1][k];
R[j][k] = t1 * cc + t2 * ss;
R[j + 1][k] = xny * (t1 + R[j][k]) - t2;
}
for (k = 0; k < n; k++)
{
t1 = J[k][j];
t2 = J[k][j + 1];
J[k][j] = t1 * cc + t2 * ss;
J[k][j + 1] = xny * (J[k][j] + t1) - t2;
}
}
}
inline double distance(double a, double b)
{
register double a1, b1, t;
a1 = fabs(a);
b1 = fabs(b);
if (a1 > b1)
{
t = (b1 / a1);
return a1 * sqrt(1.0 + t * t);
}
else
if (b1 > a1)
{
t = (a1 / b1);
return b1 * sqrt(1.0 + t * t);
}
return a1 * sqrt(2.0);
}
inline double scalar_product(const Vector<double>& x, const Vector<double>& y)
{
register int i, n = x.size();
register double sum;
sum = 0.0;
for (i = 0; i < n; i++)
sum += x[i] * y[i];
return sum;
}
void cholesky_decomposition(Matrix<double>& A)
{
register int i, j, k, n = A.nrows();
register double sum;
for (i = 0; i < n; i++)
{
for (j = i; j < n; j++)
{
sum = A[i][j];
for (k = i - 1; k >= 0; k--)
sum -= A[i][k]*A[j][k];
if (i == j)
{
if (sum <= 0.0)
{
std::ostringstream os;
// raise error
print_matrix("A", A);
os << "Error in cholesky decomposition, sum: " << sum;
throw std::logic_error(os.str());
exit(-1);
}
A[i][i] = sqrt(sum);
}
else
A[j][i] = sum / A[i][i];
}
for (k = i + 1; k < n; k++)
A[i][k] = A[k][i];
}
}
void cholesky_solve(const Matrix<double>& L, Vector<double>& x, const Vector<double>& b)
{
int n = L.nrows();
Vector<double> y(n);
/* Solve L * y = b */
forward_elimination(L, y, b);
/* Solve L^T * x = y */
backward_elimination(L, x, y);
}
inline void forward_elimination(const Matrix<double>& L, Vector<double>& y, const Vector<double>& b)
{
register int i, j, n = L.nrows();
y[0] = b[0] / L[0][0];
for (i = 1; i < n; i++)
{
y[i] = b[i];
for (j = 0; j < i; j++)
y[i] -= L[i][j] * y[j];
y[i] = y[i] / L[i][i];
}
}
inline void backward_elimination(const Matrix<double>& U, Vector<double>& x, const Vector<double>& y)
{
register int i, j, n = U.nrows();
x[n - 1] = y[n - 1] / U[n - 1][n - 1];
for (i = n - 2; i >= 0; i--)
{
x[i] = y[i];
for (j = i + 1; j < n; j++)
x[i] -= U[i][j] * x[j];
x[i] = x[i] / U[i][i];
}
}
void print_matrix(const char* name, const Matrix<double>& A, int n, int m)
{
std::ostringstream s;
std::string t;
if (n == -1)
n = A.nrows();
if (m == -1)
m = A.ncols();
s << name << ": " << std::endl;
for (int i = 0; i < n; i++)
{
s << " ";
for (int j = 0; j < m; j++)
s << A[i][j] << ", ";
s << std::endl;
}
t = s.str();
t = t.substr(0, t.size() - 3); // To remove the trailing space, comma and newline
std::cout << t << std::endl;
}
template<typename T>
void print_vector(const char* name, const Vector<T>& v, int n)
{
std::ostringstream s;
std::string t;
if (n == -1)
n = v.size();
s << name << ": " << std::endl << " ";
for (int i = 0; i < n; i++)
{
s << v[i] << ", ";
}
t = s.str();
t = t.substr(0, t.size() - 2); // To remove the trailing space and comma
std::cout << t << std::endl;
}
} // namespace quadprogpp

111
thirdparty/tinyxml/src/tinystr.cpp vendored
View File

@ -1,111 +0,0 @@
/*
www.sourceforge.net/projects/tinyxml
This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any
damages arising from the use of this software.
Permission is granted to anyone to use this software for any
purpose, including commercial applications, and to alter it and
redistribute it freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must
not claim that you wrote the original software. If you use this
software in a product, an acknowledgment in the product documentation
would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and
must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source
distribution.
*/
#ifndef TIXML_USE_STL
#include "tinyxml/include/tinystr.h"
// Error value for find primitive
const TiXmlString::size_type TiXmlString::npos = static_cast< TiXmlString::size_type >(-1);
// Null rep.
TiXmlString::Rep TiXmlString::nullrep_ = { 0, 0, { '\0' } };
void TiXmlString::reserve (size_type cap)
{
if (cap > capacity())
{
TiXmlString tmp;
tmp.init(length(), cap);
memcpy(tmp.start(), data(), length());
swap(tmp);
}
}
TiXmlString& TiXmlString::assign(const char* str, size_type len)
{
size_type cap = capacity();
if (len > cap || cap > 3*(len + 8))
{
TiXmlString tmp;
tmp.init(len);
memcpy(tmp.start(), str, len);
swap(tmp);
}
else
{
memmove(start(), str, len);
set_size(len);
}
return *this;
}
TiXmlString& TiXmlString::append(const char* str, size_type len)
{
size_type newsize = length() + len;
if (newsize > capacity())
{
reserve (newsize + capacity());
}
memmove(finish(), str, len);
set_size(newsize);
return *this;
}
TiXmlString operator + (const TiXmlString & a, const TiXmlString & b)
{
TiXmlString tmp;
tmp.reserve(a.length() + b.length());
tmp += a;
tmp += b;
return tmp;
}
TiXmlString operator + (const TiXmlString & a, const char* b)
{
TiXmlString tmp;
TiXmlString::size_type b_len = static_cast<TiXmlString::size_type>( strlen(b) );
tmp.reserve(a.length() + b_len);
tmp += a;
tmp.append(b, b_len);
return tmp;
}
TiXmlString operator + (const char* a, const TiXmlString & b)
{
TiXmlString tmp;
TiXmlString::size_type a_len = static_cast<TiXmlString::size_type>( strlen(a) );
tmp.reserve(a_len + b.length());
tmp.append(a, a_len);
tmp += b;
return tmp;
}
#endif // TIXML_USE_STL

1886
thirdparty/tinyxml/src/tinyxml.cpp vendored
View File

File diff suppressed because it is too large Load Diff

52
thirdparty/tinyxml/src/tinyxmlerror.cpp vendored
View File

@ -1,52 +0,0 @@
/*
www.sourceforge.net/projects/tinyxml
Original code (2.0 and earlier )copyright (c) 2000-2006 Lee Thomason (www.grinninglizard.com)
This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any
damages arising from the use of this software.
Permission is granted to anyone to use this software for any
purpose, including commercial applications, and to alter it and
redistribute it freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must
not claim that you wrote the original software. If you use this
software in a product, an acknowledgment in the product documentation
would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and
must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source
distribution.
*/
#include "tinyxml/include/tinyxml.h"
// The goal of the seperate error file is to make the first
// step towards localization. tinyxml (currently) only supports
// english error messages, but the could now be translated.
//
// It also cleans up the code a bit.
//
const char* TiXmlBase::errorString[ TiXmlBase::TIXML_ERROR_STRING_COUNT ] =
{
"No error",
"Error",
"Failed to open file",
"Error parsing Element.",
"Failed to read Element name",
"Error reading Element value.",
"Error reading Attributes.",
"Error: empty tag.",
"Error reading end tag.",
"Error parsing Unknown.",
"Error parsing Comment.",
"Error parsing Declaration.",
"Error document empty.",
"Error null (0) or unexpected EOF found in input stream.",
"Error parsing CDATA.",
"Error when TiXmlDocument added to document, because TiXmlDocument can only be at the root.",
};

1638
thirdparty/tinyxml/src/tinyxmlparser.cpp vendored
View File

File diff suppressed because it is too large Load Diff