support for arm64
This commit is contained in:
Agnel Wang
parent
cae64c9ee6
commit
fd5a63f09e
|
|
@ -0,0 +1,2 @@
|
|||
.vscode
|
||||
build
|
||||
|
|
@ -64,4 +64,4 @@ link_directories(lib)
|
|||
# ----------------------add executable----------------------
|
||||
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR})
|
||||
add_executable(z1_ctrl main.cpp)
|
||||
target_link_libraries(z1_ctrl libZ1_${COMMUNICATION}_Linux64.so ${catkin_LIBRARIES} )
|
||||
target_link_libraries(z1_ctrl libZ1_${COMMUNICATION}_${CMAKE_HOST_SYSTEM_PROCESSOR}.so ${catkin_LIBRARIES} )
|
||||
|
|
|
|||
|
|
@ -1,32 +1,32 @@
|
|||
#ifndef BASESTATE_H
|
||||
#define BASESTATE_H
|
||||
|
||||
#include <string>
|
||||
#include "common/enumClass.h"
|
||||
|
||||
class BaseState{
|
||||
public:
|
||||
BaseState(int stateNameEnum, std::string stateNameString)
|
||||
: _stateNameEnum(stateNameEnum), _stateNameString(stateNameString){}
|
||||
virtual ~BaseState(){};
|
||||
|
||||
virtual void enter() = 0;
|
||||
virtual void run() = 0;
|
||||
virtual void exit() = 0;
|
||||
virtual int checkChange(int cmd) = 0;
|
||||
|
||||
bool isState(int stateEnum){
|
||||
if(_stateNameEnum == stateEnum){
|
||||
return true;
|
||||
}else{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
std::string getStateName(){return _stateNameString;}
|
||||
int getStateNameEnum(){return _stateNameEnum;};
|
||||
protected:
|
||||
int _stateNameEnum;
|
||||
std::string _stateNameString;
|
||||
};
|
||||
|
||||
#ifndef BASESTATE_H
|
||||
#define BASESTATE_H
|
||||
|
||||
#include <string>
|
||||
#include "common/enumClass.h"
|
||||
|
||||
class BaseState{
|
||||
public:
|
||||
BaseState(int stateNameEnum, std::string stateNameString)
|
||||
: _stateNameEnum(stateNameEnum), _stateNameString(stateNameString){}
|
||||
virtual ~BaseState(){};
|
||||
|
||||
virtual void enter() = 0;
|
||||
virtual void run() = 0;
|
||||
virtual void exit() = 0;
|
||||
virtual int checkChange(int cmd) = 0;
|
||||
|
||||
bool isState(int stateEnum){
|
||||
if(_stateNameEnum == stateEnum){
|
||||
return true;
|
||||
}else{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
std::string getStateName(){return _stateNameString;}
|
||||
int getStateNameEnum(){return _stateNameEnum;};
|
||||
protected:
|
||||
int _stateNameEnum;
|
||||
std::string _stateNameString;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -1,49 +1,49 @@
|
|||
#ifndef FSMSTATE_H
|
||||
#define FSMSTATE_H
|
||||
|
||||
#include <string>
|
||||
#include <iostream>
|
||||
#include <unistd.h>
|
||||
#include "control/CtrlComponents.h"
|
||||
#include "common/math/mathTools.h"
|
||||
#include "common/utilities/timer.h"
|
||||
#include "FSM/BaseState.h"
|
||||
|
||||
class FSMState : public BaseState{
|
||||
public:
|
||||
FSMState(CtrlComponents *ctrlComp, ArmFSMStateName stateName, std::string stateNameString);
|
||||
virtual ~FSMState(){}
|
||||
|
||||
virtual void enter() = 0;
|
||||
virtual void run() = 0;
|
||||
virtual void exit() = 0;
|
||||
virtual int checkChange(int cmd) {return (int)ArmFSMStateName::INVALID;}
|
||||
bool _collisionTest();
|
||||
|
||||
protected:
|
||||
void _armCtrl();
|
||||
void _recordData();
|
||||
Vec6 _postureToVec6(Posture posture);
|
||||
void _tauFriction();
|
||||
|
||||
LowlevelCmd *_lowCmd;
|
||||
LowlevelState *_lowState;
|
||||
IOInterface *_ioInter;
|
||||
ArmModel *_armModel;
|
||||
|
||||
Vec6 _qPast, _qdPast, _q, _qd, _qdd, _tauForward;
|
||||
double _gripperPos, _gripperW, _gripperTau;
|
||||
|
||||
CtrlComponents *_ctrlComp;
|
||||
Vec6 _g, _tauCmd, _tauFric;
|
||||
|
||||
private:
|
||||
|
||||
uint _collisionCnt;
|
||||
|
||||
Vec6 _mLinearFriction;
|
||||
Vec6 _mCoulombFriction;
|
||||
|
||||
};
|
||||
|
||||
#endif // FSMSTATE_H
|
||||
#ifndef FSMSTATE_H
|
||||
#define FSMSTATE_H
|
||||
|
||||
#include <string>
|
||||
#include <iostream>
|
||||
#include <unistd.h>
|
||||
#include "control/CtrlComponents.h"
|
||||
#include "common/math/mathTools.h"
|
||||
#include "common/utilities/timer.h"
|
||||
#include "FSM/BaseState.h"
|
||||
|
||||
class FSMState : public BaseState{
|
||||
public:
|
||||
FSMState(CtrlComponents *ctrlComp, ArmFSMStateName stateName, std::string stateNameString);
|
||||
virtual ~FSMState(){}
|
||||
|
||||
virtual void enter() = 0;
|
||||
virtual void run() = 0;
|
||||
virtual void exit() = 0;
|
||||
virtual int checkChange(int cmd) {return (int)ArmFSMStateName::INVALID;}
|
||||
bool _collisionTest();
|
||||
|
||||
protected:
|
||||
void _armCtrl();
|
||||
void _recordData();
|
||||
Vec6 _postureToVec6(Posture posture);
|
||||
void _tauFriction();
|
||||
|
||||
LowlevelCmd *_lowCmd;
|
||||
LowlevelState *_lowState;
|
||||
IOInterface *_ioInter;
|
||||
ArmModel *_armModel;
|
||||
|
||||
Vec6 _qPast, _qdPast, _q, _qd, _qdd, _tauForward;
|
||||
double _gripperPos, _gripperW, _gripperTau;
|
||||
|
||||
CtrlComponents *_ctrlComp;
|
||||
Vec6 _g, _tauCmd, _tauFric;
|
||||
|
||||
private:
|
||||
|
||||
uint _collisionCnt;
|
||||
|
||||
Vec6 _mLinearFriction;
|
||||
Vec6 _mCoulombFriction;
|
||||
|
||||
};
|
||||
|
||||
#endif // FSMSTATE_H
|
||||
|
|
|
|||
|
|
@ -1,28 +1,28 @@
|
|||
#ifndef FSM_H
|
||||
#define FSM_H
|
||||
|
||||
#include <vector>
|
||||
#include "FSM/FSMState.h"
|
||||
#include "common/utilities/loop.h"
|
||||
#include "control/CtrlComponents.h"
|
||||
|
||||
class FiniteStateMachine{
|
||||
public:
|
||||
FiniteStateMachine(std::vector<FSMState*> states, CtrlComponents *ctrlComp);
|
||||
virtual ~FiniteStateMachine();
|
||||
|
||||
private:
|
||||
void _run();
|
||||
std::vector<FSMState*> _states;
|
||||
|
||||
FSMMode _mode;
|
||||
bool _running;
|
||||
FSMState* _currentState;
|
||||
FSMState* _nextState;
|
||||
int _nextStateEnum;
|
||||
|
||||
CtrlComponents *_ctrlComp;
|
||||
LoopFunc *_runThread;
|
||||
};
|
||||
|
||||
#ifndef FSM_H
|
||||
#define FSM_H
|
||||
|
||||
#include <vector>
|
||||
#include "FSM/FSMState.h"
|
||||
#include "common/utilities/loop.h"
|
||||
#include "control/CtrlComponents.h"
|
||||
|
||||
class FiniteStateMachine{
|
||||
public:
|
||||
FiniteStateMachine(std::vector<FSMState*> states, CtrlComponents *ctrlComp);
|
||||
virtual ~FiniteStateMachine();
|
||||
|
||||
private:
|
||||
void _run();
|
||||
std::vector<FSMState*> _states;
|
||||
|
||||
FSMMode _mode;
|
||||
bool _running;
|
||||
FSMState* _currentState;
|
||||
FSMState* _nextState;
|
||||
int _nextStateEnum;
|
||||
|
||||
CtrlComponents *_ctrlComp;
|
||||
LoopFunc *_runThread;
|
||||
};
|
||||
|
||||
#endif // FSM_H
|
||||
|
|
@ -1,22 +1,22 @@
|
|||
#ifndef STATE_BACKTOSTART_H
|
||||
#define STATE_BACKTOSTART_H
|
||||
|
||||
|
||||
#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:
|
||||
bool _reach, _pastReach;
|
||||
JointSpaceTraj *_jointTraj;
|
||||
Vec6 _pos_startFlat;
|
||||
};
|
||||
|
||||
#ifndef STATE_BACKTOSTART_H
|
||||
#define STATE_BACKTOSTART_H
|
||||
|
||||
|
||||
#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:
|
||||
bool _reach, _pastReach;
|
||||
JointSpaceTraj *_jointTraj;
|
||||
Vec6 _pos_startFlat;
|
||||
};
|
||||
|
||||
#endif // STATE_BACKTOSTART_H
|
||||
|
|
@ -1,18 +1,18 @@
|
|||
#ifndef STATE_CALIBRATION_H
|
||||
#define STATE_CALIBRATION_H
|
||||
|
||||
#include "FSM/FSMState.h"
|
||||
|
||||
class State_Calibration : public FSMState{
|
||||
public:
|
||||
State_Calibration(CtrlComponents *ctrlComp);
|
||||
~State_Calibration(){}
|
||||
void enter();
|
||||
void run(){};
|
||||
void exit(){};
|
||||
int checkChange(int cmd);
|
||||
private:
|
||||
|
||||
};
|
||||
|
||||
#ifndef STATE_CALIBRATION_H
|
||||
#define STATE_CALIBRATION_H
|
||||
|
||||
#include "FSM/FSMState.h"
|
||||
|
||||
class State_Calibration : public FSMState{
|
||||
public:
|
||||
State_Calibration(CtrlComponents *ctrlComp);
|
||||
~State_Calibration(){}
|
||||
void enter();
|
||||
void run(){};
|
||||
void exit(){};
|
||||
int checkChange(int cmd);
|
||||
private:
|
||||
|
||||
};
|
||||
|
||||
#endif // STATE_CALIBRATION_H
|
||||
|
|
@ -1,18 +1,18 @@
|
|||
#ifndef JOINTSPACE_H
|
||||
#define JOINTSPACE_H
|
||||
|
||||
#include "FSM/FSMState.h"
|
||||
|
||||
class State_JointSpace : public FSMState{
|
||||
public:
|
||||
State_JointSpace(CtrlComponents *ctrlComp);
|
||||
~State_JointSpace(){}
|
||||
void enter();
|
||||
void run();
|
||||
void exit();
|
||||
int checkChange(int cmd);
|
||||
private:
|
||||
std::vector<double> jointSpeedMax;
|
||||
};
|
||||
|
||||
#ifndef JOINTSPACE_H
|
||||
#define JOINTSPACE_H
|
||||
|
||||
#include "FSM/FSMState.h"
|
||||
|
||||
class State_JointSpace : public FSMState{
|
||||
public:
|
||||
State_JointSpace(CtrlComponents *ctrlComp);
|
||||
~State_JointSpace(){}
|
||||
void enter();
|
||||
void run();
|
||||
void exit();
|
||||
int checkChange(int cmd);
|
||||
private:
|
||||
std::vector<double> jointSpeedMax;
|
||||
};
|
||||
|
||||
#endif // JOINTSPACE_H
|
||||
|
|
@ -1,18 +1,18 @@
|
|||
#ifndef LOWCMD_H
|
||||
#define LOWCMD_H
|
||||
|
||||
#include "FSMState.h"
|
||||
|
||||
class State_LowCmd : public FSMState{
|
||||
public:
|
||||
State_LowCmd(CtrlComponents *ctrlComp);
|
||||
void enter();
|
||||
void run();
|
||||
void exit();
|
||||
int checkChange(int cmd);
|
||||
private:
|
||||
std::vector<float> _kp;
|
||||
std::vector<float> _kw;
|
||||
};
|
||||
|
||||
#ifndef LOWCMD_H
|
||||
#define LOWCMD_H
|
||||
|
||||
#include "FSMState.h"
|
||||
|
||||
class State_LowCmd : public FSMState{
|
||||
public:
|
||||
State_LowCmd(CtrlComponents *ctrlComp);
|
||||
void enter();
|
||||
void run();
|
||||
void exit();
|
||||
int checkChange(int cmd);
|
||||
private:
|
||||
std::vector<float> _kp;
|
||||
std::vector<float> _kw;
|
||||
};
|
||||
|
||||
#endif // LOWCMD_H
|
||||
|
|
@ -1,23 +1,23 @@
|
|||
#ifndef MOVEC_H
|
||||
#define MOVEC_H
|
||||
|
||||
#include "FSM/FSMState.h"
|
||||
#include "trajectory/EndCircleTraj.h"
|
||||
|
||||
class State_MoveC : public FSMState{
|
||||
public:
|
||||
State_MoveC(CtrlComponents *ctrlComp);
|
||||
~State_MoveC();
|
||||
void enter();
|
||||
void run();
|
||||
void exit();
|
||||
int checkChange(int cmd);
|
||||
private:
|
||||
double _speed;
|
||||
std::vector<Vec6> _postures;
|
||||
EndCircleTraj *_circleTraj;
|
||||
bool _timeReached, _taskReached, _pastTaskReached, _finalReached;
|
||||
uint _taskCnt;
|
||||
};
|
||||
|
||||
#ifndef MOVEC_H
|
||||
#define MOVEC_H
|
||||
|
||||
#include "FSM/FSMState.h"
|
||||
#include "trajectory/EndCircleTraj.h"
|
||||
|
||||
class State_MoveC : public FSMState{
|
||||
public:
|
||||
State_MoveC(CtrlComponents *ctrlComp);
|
||||
~State_MoveC();
|
||||
void enter();
|
||||
void run();
|
||||
void exit();
|
||||
int checkChange(int cmd);
|
||||
private:
|
||||
double _speed;
|
||||
std::vector<Vec6> _postures;
|
||||
EndCircleTraj *_circleTraj;
|
||||
bool _timeReached, _taskReached, _pastTaskReached, _finalReached;
|
||||
uint _taskCnt;
|
||||
};
|
||||
|
||||
#endif // CARTESIAN_H
|
||||
|
|
@ -1,15 +1,15 @@
|
|||
#ifndef PASSIVE_H
|
||||
#define PASSIVE_H
|
||||
|
||||
#include "FSM/FSMState.h"
|
||||
|
||||
class State_Passive : public FSMState{
|
||||
public:
|
||||
State_Passive(CtrlComponents *ctrlComp);
|
||||
void enter();
|
||||
void run();
|
||||
void exit();
|
||||
int checkChange(int cmd);
|
||||
};
|
||||
|
||||
#ifndef PASSIVE_H
|
||||
#define PASSIVE_H
|
||||
|
||||
#include "FSM/FSMState.h"
|
||||
|
||||
class State_Passive : public FSMState{
|
||||
public:
|
||||
State_Passive(CtrlComponents *ctrlComp);
|
||||
void enter();
|
||||
void run();
|
||||
void exit();
|
||||
int checkChange(int cmd);
|
||||
};
|
||||
|
||||
#endif // PASSIVE_H
|
||||
|
|
@ -1,18 +1,18 @@
|
|||
#ifndef SAVESTATE_H
|
||||
#define SAVESTATE_H
|
||||
|
||||
#include "FSMState.h"
|
||||
#include "common/utilities/CSVTool.h"
|
||||
|
||||
class State_SaveState : public FSMState{
|
||||
public:
|
||||
State_SaveState(CtrlComponents *ctrlComp);
|
||||
~State_SaveState();
|
||||
void enter();
|
||||
void run();
|
||||
void exit();
|
||||
int checkChange(int cmd);
|
||||
private:
|
||||
};
|
||||
|
||||
#ifndef SAVESTATE_H
|
||||
#define SAVESTATE_H
|
||||
|
||||
#include "FSMState.h"
|
||||
#include "common/utilities/CSVTool.h"
|
||||
|
||||
class State_SaveState : public FSMState{
|
||||
public:
|
||||
State_SaveState(CtrlComponents *ctrlComp);
|
||||
~State_SaveState();
|
||||
void enter();
|
||||
void run();
|
||||
void exit();
|
||||
int checkChange(int cmd);
|
||||
private:
|
||||
};
|
||||
|
||||
#endif // SAVESTATE_H
|
||||
|
|
@ -1,24 +1,24 @@
|
|||
#ifndef STATETEACH_H
|
||||
#define STATETEACH_H
|
||||
|
||||
#include "FSM/FSMState.h"
|
||||
|
||||
class State_Teach : public FSMState{
|
||||
public:
|
||||
State_Teach(CtrlComponents *ctrlComp);
|
||||
~State_Teach();
|
||||
void enter();
|
||||
void run();
|
||||
void exit();
|
||||
int checkChange(int cmd);
|
||||
private:
|
||||
CSVTool *_trajCSV;
|
||||
size_t _stateID = 0;
|
||||
|
||||
Vec6 _KdDiag;
|
||||
Vec6 _kpForStable;
|
||||
Mat6 _Kd;
|
||||
double _errorBias;
|
||||
};
|
||||
|
||||
#ifndef STATETEACH_H
|
||||
#define STATETEACH_H
|
||||
|
||||
#include "FSM/FSMState.h"
|
||||
|
||||
class State_Teach : public FSMState{
|
||||
public:
|
||||
State_Teach(CtrlComponents *ctrlComp);
|
||||
~State_Teach();
|
||||
void enter();
|
||||
void run();
|
||||
void exit();
|
||||
int checkChange(int cmd);
|
||||
private:
|
||||
CSVTool *_trajCSV;
|
||||
size_t _stateID = 0;
|
||||
|
||||
Vec6 _KdDiag;
|
||||
Vec6 _kpForStable;
|
||||
Mat6 _Kd;
|
||||
double _errorBias;
|
||||
};
|
||||
|
||||
#endif // STATETEACH_H
|
||||
|
|
@ -1,28 +1,28 @@
|
|||
#ifndef STATE_TEACHREPEAT_H
|
||||
#define STATE_TEACHREPEAT_H
|
||||
|
||||
#include "FSM/FSMState.h"
|
||||
#include "trajectory/TrajectoryManager.h"
|
||||
|
||||
class State_TeachRepeat : public FSMState{
|
||||
public:
|
||||
State_TeachRepeat(CtrlComponents *ctrlComp);
|
||||
~State_TeachRepeat();
|
||||
void enter();
|
||||
void run();
|
||||
void exit();
|
||||
int checkChange(int cmd);
|
||||
private:
|
||||
bool _setCorrectly;
|
||||
JointSpaceTraj *_toStartTraj;
|
||||
bool _reachedStart = false;
|
||||
bool _finishedRepeat = false;
|
||||
size_t _index = 0;
|
||||
size_t _indexPast;
|
||||
Vec6 _trajStartQ, _trajStartQd;
|
||||
double _trajStartGripperQ, _trajStartGripperQd;
|
||||
|
||||
CSVTool *_csvFile;
|
||||
};
|
||||
|
||||
#ifndef STATE_TEACHREPEAT_H
|
||||
#define STATE_TEACHREPEAT_H
|
||||
|
||||
#include "FSM/FSMState.h"
|
||||
#include "trajectory/TrajectoryManager.h"
|
||||
|
||||
class State_TeachRepeat : public FSMState{
|
||||
public:
|
||||
State_TeachRepeat(CtrlComponents *ctrlComp);
|
||||
~State_TeachRepeat();
|
||||
void enter();
|
||||
void run();
|
||||
void exit();
|
||||
int checkChange(int cmd);
|
||||
private:
|
||||
bool _setCorrectly;
|
||||
JointSpaceTraj *_toStartTraj;
|
||||
bool _reachedStart = false;
|
||||
bool _finishedRepeat = false;
|
||||
size_t _index = 0;
|
||||
size_t _indexPast;
|
||||
Vec6 _trajStartQ, _trajStartQd;
|
||||
double _trajStartGripperQ, _trajStartGripperQd;
|
||||
|
||||
CSVTool *_csvFile;
|
||||
};
|
||||
|
||||
#endif // STATE_TEACHREPEAT_H
|
||||
|
|
@ -1,24 +1,24 @@
|
|||
#ifndef TOSTATE_H
|
||||
#define TOSTATE_H
|
||||
|
||||
#include "FSM/FSMState.h"
|
||||
#include "trajectory/JointSpaceTraj.h"
|
||||
|
||||
class State_ToState : public FSMState{
|
||||
public:
|
||||
State_ToState(CtrlComponents *ctrlComp);
|
||||
~State_ToState();
|
||||
void enter();
|
||||
void run();
|
||||
void exit();
|
||||
int checkChange(int cmd);
|
||||
private:
|
||||
bool _setCorrectly;
|
||||
double _costTime;
|
||||
HomoMat _goalHomo;
|
||||
JointSpaceTraj *_jointTraj;
|
||||
bool _reach, _pastReach;
|
||||
std::string _goalName;
|
||||
};
|
||||
|
||||
#ifndef TOSTATE_H
|
||||
#define TOSTATE_H
|
||||
|
||||
#include "FSM/FSMState.h"
|
||||
#include "trajectory/JointSpaceTraj.h"
|
||||
|
||||
class State_ToState : public FSMState{
|
||||
public:
|
||||
State_ToState(CtrlComponents *ctrlComp);
|
||||
~State_ToState();
|
||||
void enter();
|
||||
void run();
|
||||
void exit();
|
||||
int checkChange(int cmd);
|
||||
private:
|
||||
bool _setCorrectly;
|
||||
double _costTime;
|
||||
HomoMat _goalHomo;
|
||||
JointSpaceTraj *_jointTraj;
|
||||
bool _reach, _pastReach;
|
||||
std::string _goalName;
|
||||
};
|
||||
|
||||
#endif // TOSTATE_H
|
||||
|
|
@ -1,37 +1,37 @@
|
|||
#ifndef CARTESIANPATH_H
|
||||
#define CARTESIANPATH_H
|
||||
|
||||
#include "FSM/FSMState.h"
|
||||
#include "trajectory/TrajectoryManager.h"
|
||||
|
||||
class State_Trajectory : public FSMState{
|
||||
public:
|
||||
State_Trajectory(CtrlComponents *ctrlComp,
|
||||
ArmFSMStateName stateEnum = ArmFSMStateName::TRAJECTORY,
|
||||
std::string stateString = "trajectory");
|
||||
~State_Trajectory();
|
||||
void enter();
|
||||
void run();
|
||||
void exit();
|
||||
int checkChange(int cmd);
|
||||
protected:
|
||||
void _setTraj();
|
||||
|
||||
TrajectoryManager *_traj;
|
||||
HomoMat _goalHomo;
|
||||
Vec6 _goalTwist;
|
||||
double speedTemp;
|
||||
|
||||
JointSpaceTraj *_toStartTraj;
|
||||
bool _reachedStart = false;
|
||||
bool _finishedTraj = false;
|
||||
std::vector<JointSpaceTraj*> _jointTraj;
|
||||
std::vector<EndCircleTraj*> _circleTraj;
|
||||
std::vector<StopForTime*> _stopTraj;
|
||||
std::vector<EndLineTraj*> _lineTraj;
|
||||
|
||||
static std::vector<TrajCmd> _trajCmd;
|
||||
Vec6 _posture[2];
|
||||
};
|
||||
|
||||
#ifndef CARTESIANPATH_H
|
||||
#define CARTESIANPATH_H
|
||||
|
||||
#include "FSM/FSMState.h"
|
||||
#include "trajectory/TrajectoryManager.h"
|
||||
|
||||
class State_Trajectory : public FSMState{
|
||||
public:
|
||||
State_Trajectory(CtrlComponents *ctrlComp,
|
||||
ArmFSMStateName stateEnum = ArmFSMStateName::TRAJECTORY,
|
||||
std::string stateString = "trajectory");
|
||||
~State_Trajectory();
|
||||
void enter();
|
||||
void run();
|
||||
void exit();
|
||||
int checkChange(int cmd);
|
||||
protected:
|
||||
void _setTraj();
|
||||
|
||||
TrajectoryManager *_traj;
|
||||
HomoMat _goalHomo;
|
||||
Vec6 _goalTwist;
|
||||
double speedTemp;
|
||||
|
||||
JointSpaceTraj *_toStartTraj;
|
||||
bool _reachedStart = false;
|
||||
bool _finishedTraj = false;
|
||||
std::vector<JointSpaceTraj*> _jointTraj;
|
||||
std::vector<EndCircleTraj*> _circleTraj;
|
||||
std::vector<StopForTime*> _stopTraj;
|
||||
std::vector<EndLineTraj*> _lineTraj;
|
||||
|
||||
static std::vector<TrajCmd> _trajCmd;
|
||||
Vec6 _posture[2];
|
||||
};
|
||||
|
||||
#endif // CARTESIANPATH_H
|
||||
|
|
@ -1,39 +1,39 @@
|
|||
#ifndef ENUMCLASS_H
|
||||
#define ENUMCLASS_H
|
||||
|
||||
enum class FSMMode{
|
||||
NORMAL,
|
||||
CHANGE
|
||||
};
|
||||
|
||||
enum class ArmFSMStateName{
|
||||
INVALID,
|
||||
PASSIVE,
|
||||
JOINTCTRL,
|
||||
CARTESIAN,
|
||||
MOVEJ,
|
||||
MOVEL,
|
||||
MOVEC,
|
||||
TRAJECTORY,
|
||||
TOSTATE,
|
||||
SAVESTATE,
|
||||
TEACH,
|
||||
TEACHREPEAT,
|
||||
CALIBRATION,
|
||||
SETTRAJ,//no longer used
|
||||
BACKTOSTART,
|
||||
NEXT,
|
||||
LOWCMD
|
||||
};
|
||||
|
||||
enum class JointMotorType{
|
||||
SINGLE_MOTOR,
|
||||
DOUBLE_MOTOR
|
||||
};
|
||||
|
||||
enum class Control{
|
||||
KEYBOARD,
|
||||
SDK
|
||||
};
|
||||
|
||||
#ifndef ENUMCLASS_H
|
||||
#define ENUMCLASS_H
|
||||
|
||||
enum class FSMMode{
|
||||
NORMAL,
|
||||
CHANGE
|
||||
};
|
||||
|
||||
enum class ArmFSMStateName{
|
||||
INVALID,
|
||||
PASSIVE,
|
||||
JOINTCTRL,
|
||||
CARTESIAN,
|
||||
MOVEJ,
|
||||
MOVEL,
|
||||
MOVEC,
|
||||
TRAJECTORY,
|
||||
TOSTATE,
|
||||
SAVESTATE,
|
||||
TEACH,
|
||||
TEACHREPEAT,
|
||||
CALIBRATION,
|
||||
SETTRAJ,//no longer used
|
||||
BACKTOSTART,
|
||||
NEXT,
|
||||
LOWCMD
|
||||
};
|
||||
|
||||
enum class JointMotorType{
|
||||
SINGLE_MOTOR,
|
||||
DOUBLE_MOTOR
|
||||
};
|
||||
|
||||
enum class Control{
|
||||
KEYBOARD,
|
||||
SDK
|
||||
};
|
||||
|
||||
#endif // ENUMCLASS_H
|
||||
|
|
@ -1,29 +1,29 @@
|
|||
#ifndef FILTER
|
||||
#define FILTER
|
||||
|
||||
#include <vector>
|
||||
#include <Eigen/Dense>
|
||||
|
||||
/*
|
||||
Low pass filter
|
||||
*/
|
||||
class LPFilter{
|
||||
public:
|
||||
LPFilter(double samplePeriod, double cutFrequency, size_t valueCount);
|
||||
~LPFilter(){};
|
||||
void clear();
|
||||
void addValue(double &newValue);
|
||||
void addValue(std::vector<double> &vec);
|
||||
|
||||
template <typename T>
|
||||
void addValue(Eigen::MatrixBase<T> &eigenVec);
|
||||
|
||||
// double getValue();
|
||||
private:
|
||||
size_t _valueCount;
|
||||
double _weight;
|
||||
std::vector<double> _pastValue;
|
||||
bool _start;
|
||||
};
|
||||
|
||||
#ifndef FILTER
|
||||
#define FILTER
|
||||
|
||||
#include <vector>
|
||||
#include <Eigen/Dense>
|
||||
|
||||
/*
|
||||
Low pass filter
|
||||
*/
|
||||
class LPFilter{
|
||||
public:
|
||||
LPFilter(double samplePeriod, double cutFrequency, size_t valueCount);
|
||||
~LPFilter(){};
|
||||
void clear();
|
||||
void addValue(double &newValue);
|
||||
void addValue(std::vector<double> &vec);
|
||||
|
||||
template <typename T>
|
||||
void addValue(Eigen::MatrixBase<T> &eigenVec);
|
||||
|
||||
// double getValue();
|
||||
private:
|
||||
size_t _valueCount;
|
||||
double _weight;
|
||||
std::vector<double> _pastValue;
|
||||
bool _start;
|
||||
};
|
||||
|
||||
#endif // FILTER
|
||||
|
|
@ -1,109 +1,109 @@
|
|||
#ifndef MATHTYPES_H
|
||||
#define MATHTYPES_H
|
||||
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <vector>
|
||||
|
||||
/************************/
|
||||
/******** Vector ********/
|
||||
/************************/
|
||||
// 2x1 Vector
|
||||
using Vec2 = typename Eigen::Matrix<double, 2, 1>;
|
||||
|
||||
// 3x1 Vector
|
||||
using Vec3 = typename Eigen::Matrix<double, 3, 1>;
|
||||
|
||||
// 4x1 Vector
|
||||
using Vec4 = typename Eigen::Matrix<double, 4, 1>;
|
||||
|
||||
// 6x1 Vector
|
||||
using Vec6 = typename Eigen::Matrix<double, 6, 1>;
|
||||
|
||||
// Quaternion
|
||||
using Quat = typename Eigen::Matrix<double, 4, 1>;
|
||||
|
||||
// 4x1 Integer Vector
|
||||
using VecInt4 = typename Eigen::Matrix<int, 4, 1>;
|
||||
|
||||
// 12x1 Vector
|
||||
using Vec12 = typename Eigen::Matrix<double, 12, 1>;
|
||||
|
||||
// 18x1 Vector
|
||||
using Vec18 = typename Eigen::Matrix<double, 18, 1>;
|
||||
|
||||
// Dynamic Length Vector
|
||||
using VecX = typename Eigen::Matrix<double, Eigen::Dynamic, 1>;
|
||||
|
||||
/************************/
|
||||
/******** Matrix ********/
|
||||
/************************/
|
||||
// Rotation Matrix
|
||||
using RotMat = typename Eigen::Matrix<double, 3, 3>;
|
||||
|
||||
// Homogenous Matrix
|
||||
using HomoMat = typename Eigen::Matrix<double, 4, 4>;
|
||||
|
||||
// 2x2 Matrix
|
||||
using Mat2 = typename Eigen::Matrix<double, 2, 2>;
|
||||
|
||||
// 3x3 Matrix
|
||||
using Mat3 = typename Eigen::Matrix<double, 3, 3>;
|
||||
|
||||
// 3x4 Matrix, each column is a 3x1 vector
|
||||
using Vec34 = typename Eigen::Matrix<double, 3, 4>;
|
||||
|
||||
// 6x6 Matrix
|
||||
using Mat6 = typename Eigen::Matrix<double, 6, 6>;
|
||||
|
||||
// 12x12 Matrix
|
||||
using Mat12 = typename Eigen::Matrix<double, 12, 12>;
|
||||
|
||||
// 3x3 Identity Matrix
|
||||
#define I3 Eigen::MatrixXd::Identity(3, 3)
|
||||
|
||||
// 6x6 Identity Matrix
|
||||
#define I6 Eigen::MatrixXd::Identity(6, 6)
|
||||
|
||||
// 12x12 Identity Matrix
|
||||
#define I12 Eigen::MatrixXd::Identity(12, 12)
|
||||
|
||||
// 18x18 Identity Matrix
|
||||
#define I18 Eigen::MatrixXd::Identity(18, 18)
|
||||
|
||||
// Dynamic Size Matrix
|
||||
using MatX = typename Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
|
||||
|
||||
/************************/
|
||||
/****** Functions *******/
|
||||
/************************/
|
||||
inline Vec34 vec12ToVec34(Vec12 vec12){
|
||||
Vec34 vec34;
|
||||
for(int i(0); i < 4; ++i){
|
||||
vec34.col(i) = vec12.segment(3*i, 3);
|
||||
}
|
||||
return vec34;
|
||||
}
|
||||
|
||||
inline Vec12 vec34ToVec12(Vec34 vec34){
|
||||
Vec12 vec12;
|
||||
for(int i(0); i < 4; ++i){
|
||||
vec12.segment(3*i, 3) = vec34.col(i);
|
||||
}
|
||||
return vec12;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
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;
|
||||
}
|
||||
|
||||
#ifndef MATHTYPES_H
|
||||
#define MATHTYPES_H
|
||||
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <vector>
|
||||
|
||||
/************************/
|
||||
/******** Vector ********/
|
||||
/************************/
|
||||
// 2x1 Vector
|
||||
using Vec2 = typename Eigen::Matrix<double, 2, 1>;
|
||||
|
||||
// 3x1 Vector
|
||||
using Vec3 = typename Eigen::Matrix<double, 3, 1>;
|
||||
|
||||
// 4x1 Vector
|
||||
using Vec4 = typename Eigen::Matrix<double, 4, 1>;
|
||||
|
||||
// 6x1 Vector
|
||||
using Vec6 = typename Eigen::Matrix<double, 6, 1>;
|
||||
|
||||
// Quaternion
|
||||
using Quat = typename Eigen::Matrix<double, 4, 1>;
|
||||
|
||||
// 4x1 Integer Vector
|
||||
using VecInt4 = typename Eigen::Matrix<int, 4, 1>;
|
||||
|
||||
// 12x1 Vector
|
||||
using Vec12 = typename Eigen::Matrix<double, 12, 1>;
|
||||
|
||||
// 18x1 Vector
|
||||
using Vec18 = typename Eigen::Matrix<double, 18, 1>;
|
||||
|
||||
// Dynamic Length Vector
|
||||
using VecX = typename Eigen::Matrix<double, Eigen::Dynamic, 1>;
|
||||
|
||||
/************************/
|
||||
/******** Matrix ********/
|
||||
/************************/
|
||||
// Rotation Matrix
|
||||
using RotMat = typename Eigen::Matrix<double, 3, 3>;
|
||||
|
||||
// Homogenous Matrix
|
||||
using HomoMat = typename Eigen::Matrix<double, 4, 4>;
|
||||
|
||||
// 2x2 Matrix
|
||||
using Mat2 = typename Eigen::Matrix<double, 2, 2>;
|
||||
|
||||
// 3x3 Matrix
|
||||
using Mat3 = typename Eigen::Matrix<double, 3, 3>;
|
||||
|
||||
// 3x4 Matrix, each column is a 3x1 vector
|
||||
using Vec34 = typename Eigen::Matrix<double, 3, 4>;
|
||||
|
||||
// 6x6 Matrix
|
||||
using Mat6 = typename Eigen::Matrix<double, 6, 6>;
|
||||
|
||||
// 12x12 Matrix
|
||||
using Mat12 = typename Eigen::Matrix<double, 12, 12>;
|
||||
|
||||
// 3x3 Identity Matrix
|
||||
#define I3 Eigen::MatrixXd::Identity(3, 3)
|
||||
|
||||
// 6x6 Identity Matrix
|
||||
#define I6 Eigen::MatrixXd::Identity(6, 6)
|
||||
|
||||
// 12x12 Identity Matrix
|
||||
#define I12 Eigen::MatrixXd::Identity(12, 12)
|
||||
|
||||
// 18x18 Identity Matrix
|
||||
#define I18 Eigen::MatrixXd::Identity(18, 18)
|
||||
|
||||
// Dynamic Size Matrix
|
||||
using MatX = typename Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
|
||||
|
||||
/************************/
|
||||
/****** Functions *******/
|
||||
/************************/
|
||||
inline Vec34 vec12ToVec34(Vec12 vec12){
|
||||
Vec34 vec34;
|
||||
for(int i(0); i < 4; ++i){
|
||||
vec34.col(i) = vec12.segment(3*i, 3);
|
||||
}
|
||||
return vec34;
|
||||
}
|
||||
|
||||
inline Vec12 vec34ToVec12(Vec34 vec34){
|
||||
Vec12 vec12;
|
||||
for(int i(0); i < 4; ++i){
|
||||
vec12.segment(3*i, 3) = vec34.col(i);
|
||||
}
|
||||
return vec12;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
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
|
||||
|
|
@ -1,246 +1,246 @@
|
|||
#ifndef CSVTOOL_H
|
||||
#define CSVTOOL_H
|
||||
|
||||
/*
|
||||
personal tool for .csv reading and modifying.
|
||||
only suitable for small .csv file.
|
||||
for large .csv, try <https://github.com/ben-strasser/fast-cpp-csv-parser> (read only)
|
||||
*/
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <map>
|
||||
#include <fstream>
|
||||
#include <sstream>
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
#include <algorithm>
|
||||
#include "common/utilities/typeTrans.h"
|
||||
|
||||
enum class FileType{
|
||||
READ_WRITE,
|
||||
CLEAR_DUMP
|
||||
};
|
||||
|
||||
class CSVLine{
|
||||
public:
|
||||
// CSVLine(std::string lineTemp, std::streampos filePos);
|
||||
CSVLine(std::string lineTemp);
|
||||
CSVLine(std::string label, std::vector<double> values);
|
||||
~CSVLine(){}
|
||||
|
||||
// void updateFilePos(std::streampos filePos){_filePos = filePos;}
|
||||
void getValues(std::vector<double> &values);
|
||||
void changeValue(std::vector<double> values);
|
||||
void writeAtEnd(std::fstream &ioStream);
|
||||
std::string getLabel(){return _label;}
|
||||
|
||||
private:
|
||||
// std::streampos _filePos;
|
||||
std::string _label;
|
||||
std::vector<double> _values;
|
||||
};
|
||||
|
||||
/*
|
||||
FileType::READ_WRITE : must already exist such fileName
|
||||
FileType::CLEAR_DUMP : if do not exist such file, will create one
|
||||
*/
|
||||
class CSVTool{
|
||||
public:
|
||||
CSVTool(std::string fileName, FileType type = FileType::READ_WRITE, int precision = 6);
|
||||
~CSVTool(){_ioStream.close();}
|
||||
|
||||
bool getLine(std::string label, std::vector<double> &values);
|
||||
template<typename... Args>
|
||||
bool getLineDirect(std::string label, Args&... values);
|
||||
|
||||
void modifyLine(std::string label, std::vector<double> &values, bool addNew);
|
||||
template<typename... Args>
|
||||
void modifyLineDirect(std::string label, bool addNew, Args&... values);
|
||||
|
||||
void readFile();
|
||||
void saveFile();
|
||||
|
||||
bool _hasFile;
|
||||
private:
|
||||
std::string _fileName;
|
||||
std::fstream _ioStream;
|
||||
int _precision;
|
||||
std::string _lineTemp;
|
||||
std::map<std::string, size_t> _labels;
|
||||
std::vector<CSVLine*> _lines;
|
||||
|
||||
};
|
||||
|
||||
/*************************/
|
||||
/* CSVLine */
|
||||
/*************************/
|
||||
// CSVLine::CSVLine(std::string lineTemp, std::streampos filePos)
|
||||
// :_filePos(filePos){
|
||||
|
||||
// // std::cout << lineTemp << std::endl;
|
||||
// }
|
||||
|
||||
inline CSVLine::CSVLine(std::string lineTemp){
|
||||
// delete all spaces
|
||||
lineTemp.erase(std::remove(lineTemp.begin(), lineTemp.end(), ' '), lineTemp.end());
|
||||
|
||||
std::stringstream ss(lineTemp);
|
||||
std::string stringTemp;
|
||||
|
||||
std::getline(ss, _label, ',');
|
||||
|
||||
while(std::getline(ss, stringTemp, ',')){
|
||||
_values.push_back(stod(stringTemp));
|
||||
}
|
||||
|
||||
// std::cout << "**********" << std::endl;
|
||||
// std::cout << "_label: " << _label << std::fixed << std::setprecision(3) << std::endl;
|
||||
// for(int i(0); i<_values.size(); ++i){
|
||||
// std::cout << _values.at(i) << ",,, ";
|
||||
// }
|
||||
// std::cout << std::endl;
|
||||
}
|
||||
|
||||
inline CSVLine::CSVLine(std::string label, std::vector<double> values)
|
||||
:_label(label), _values(values){
|
||||
|
||||
}
|
||||
|
||||
inline void CSVLine::changeValue(std::vector<double> values){
|
||||
if(values.size() != _values.size()){
|
||||
std::cout << "[WARNING] CSVLine::changeValue, the size changed" << std::endl;
|
||||
}
|
||||
_values = values;
|
||||
}
|
||||
|
||||
inline void CSVLine::getValues(std::vector<double> &values){
|
||||
values = _values;
|
||||
}
|
||||
|
||||
inline void CSVLine::writeAtEnd(std::fstream &ioStream){
|
||||
ioStream << _label << ", ";
|
||||
|
||||
for(int i(0); i<_values.size(); ++i){
|
||||
ioStream << _values.at(i) << ", ";
|
||||
}
|
||||
|
||||
ioStream << std::endl;
|
||||
}
|
||||
|
||||
|
||||
/*************************/
|
||||
/* CSVTool */
|
||||
/*************************/
|
||||
inline CSVTool::CSVTool(std::string fileName, FileType type, int precision)
|
||||
: _fileName(fileName), _precision(precision){
|
||||
|
||||
if(type == FileType::READ_WRITE){
|
||||
_ioStream.open(_fileName, std::fstream::ate | std::fstream::in | std::fstream::out);
|
||||
|
||||
if(!_ioStream.is_open()){
|
||||
std::cout << "[ERROR] CSVTool open file: " << fileName << " failed!" << std::endl;
|
||||
// exit(-1);
|
||||
_hasFile = false;
|
||||
}else{
|
||||
readFile();
|
||||
_hasFile = true;
|
||||
}
|
||||
|
||||
}
|
||||
else if(type == FileType::CLEAR_DUMP){
|
||||
_ioStream.open(_fileName, std::fstream::out);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
inline void CSVTool::readFile(){
|
||||
if(!_ioStream.is_open()){
|
||||
// _ioStream.open(_fileName, std::fstream::ate | std::fstream::in | std::fstream::out);
|
||||
std::cout << "[ERROR] CSVTool::readFile, file: " << _fileName << " has not been opened!" << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
_lines.clear();
|
||||
_labels.clear();
|
||||
|
||||
_ioStream.seekg(0, std::fstream::beg);
|
||||
size_t lineNum = 0;
|
||||
while(_ioStream && _ioStream.tellg() != std::fstream::end && getline(_ioStream, _lineTemp)){
|
||||
_lines.push_back( new CSVLine(_lineTemp) );
|
||||
|
||||
if(_labels.count(_lines.at(lineNum)->getLabel()) == 0){
|
||||
_labels.insert(std::pair<std::string, size_t>(_lines.at(lineNum)->getLabel(), lineNum));
|
||||
++lineNum;
|
||||
}else{
|
||||
std::cout << "[ERROR] CSVTool::readFile, the label "
|
||||
<< _lines.at(lineNum)->getLabel() << " is repeated" << std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
inline bool CSVTool::getLine(std::string label, std::vector<double> &values){
|
||||
if(_labels.count(label) == 0){
|
||||
std::cout << "[ERROR] No such label: " << label << std::endl;
|
||||
return false;
|
||||
}else{
|
||||
_lines.at(_labels[label])->getValues(values);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
template<typename... Args>
|
||||
inline bool CSVTool::getLineDirect(std::string label, Args&... values){
|
||||
std::vector<double> vec;
|
||||
if(getLine(label, vec)){
|
||||
typeTrans::extractVector(vec, values...);
|
||||
return true;
|
||||
}else{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
template<typename... Args>
|
||||
inline void CSVTool::modifyLineDirect(std::string label, bool addNew, Args&... values){
|
||||
std::vector<double> vec;
|
||||
typeTrans::combineToVector(vec, values...);
|
||||
|
||||
// std::cout << "CSVTool::modifyLineDirect------" << std::endl;
|
||||
// std::cout << "label: " << label << std::endl;
|
||||
// std::cout << "vec: ";
|
||||
// for(int i(0); i<vec.size(); ++i){
|
||||
// std::cout << vec.at(i) << ", ";
|
||||
// }std::cout << std::endl;
|
||||
|
||||
modifyLine(label, vec, addNew);
|
||||
}
|
||||
|
||||
|
||||
inline void CSVTool::saveFile(){
|
||||
_ioStream.close();
|
||||
_ioStream.open(_fileName, std::fstream::out);
|
||||
_ioStream << std::fixed << std::setprecision(_precision);
|
||||
for(int i(0); i<_lines.size(); ++i){
|
||||
_lines.at(i)->writeAtEnd(_ioStream);
|
||||
}
|
||||
|
||||
_ioStream.close();
|
||||
_ioStream.open(_fileName, std::fstream::ate | std::fstream::in | std::fstream::out);
|
||||
}
|
||||
|
||||
inline void CSVTool::modifyLine(std::string label, std::vector<double> &values, bool addNew =false){
|
||||
if(_labels.count(label) == 0){
|
||||
if(addNew){
|
||||
_labels.insert(std::pair<std::string, size_t>(label, _labels.size()));
|
||||
_lines.push_back(new CSVLine(label, values));
|
||||
}else{
|
||||
std::cout << "[ERROR] CSVTool::modifyLine, label " << label << "does not exist" << std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
}else{
|
||||
_lines.at(_labels[label])->changeValue(values);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#ifndef CSVTOOL_H
|
||||
#define CSVTOOL_H
|
||||
|
||||
/*
|
||||
personal tool for .csv reading and modifying.
|
||||
only suitable for small .csv file.
|
||||
for large .csv, try <https://github.com/ben-strasser/fast-cpp-csv-parser> (read only)
|
||||
*/
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <map>
|
||||
#include <fstream>
|
||||
#include <sstream>
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
#include <algorithm>
|
||||
#include "common/utilities/typeTrans.h"
|
||||
|
||||
enum class FileType{
|
||||
READ_WRITE,
|
||||
CLEAR_DUMP
|
||||
};
|
||||
|
||||
class CSVLine{
|
||||
public:
|
||||
// CSVLine(std::string lineTemp, std::streampos filePos);
|
||||
CSVLine(std::string lineTemp);
|
||||
CSVLine(std::string label, std::vector<double> values);
|
||||
~CSVLine(){}
|
||||
|
||||
// void updateFilePos(std::streampos filePos){_filePos = filePos;}
|
||||
void getValues(std::vector<double> &values);
|
||||
void changeValue(std::vector<double> values);
|
||||
void writeAtEnd(std::fstream &ioStream);
|
||||
std::string getLabel(){return _label;}
|
||||
|
||||
private:
|
||||
// std::streampos _filePos;
|
||||
std::string _label;
|
||||
std::vector<double> _values;
|
||||
};
|
||||
|
||||
/*
|
||||
FileType::READ_WRITE : must already exist such fileName
|
||||
FileType::CLEAR_DUMP : if do not exist such file, will create one
|
||||
*/
|
||||
class CSVTool{
|
||||
public:
|
||||
CSVTool(std::string fileName, FileType type = FileType::READ_WRITE, int precision = 6);
|
||||
~CSVTool(){_ioStream.close();}
|
||||
|
||||
bool getLine(std::string label, std::vector<double> &values);
|
||||
template<typename... Args>
|
||||
bool getLineDirect(std::string label, Args&... values);
|
||||
|
||||
void modifyLine(std::string label, std::vector<double> &values, bool addNew);
|
||||
template<typename... Args>
|
||||
void modifyLineDirect(std::string label, bool addNew, Args&... values);
|
||||
|
||||
void readFile();
|
||||
void saveFile();
|
||||
|
||||
bool _hasFile;
|
||||
private:
|
||||
std::string _fileName;
|
||||
std::fstream _ioStream;
|
||||
int _precision;
|
||||
std::string _lineTemp;
|
||||
std::map<std::string, size_t> _labels;
|
||||
std::vector<CSVLine*> _lines;
|
||||
|
||||
};
|
||||
|
||||
/*************************/
|
||||
/* CSVLine */
|
||||
/*************************/
|
||||
// CSVLine::CSVLine(std::string lineTemp, std::streampos filePos)
|
||||
// :_filePos(filePos){
|
||||
|
||||
// // std::cout << lineTemp << std::endl;
|
||||
// }
|
||||
|
||||
inline CSVLine::CSVLine(std::string lineTemp){
|
||||
// delete all spaces
|
||||
lineTemp.erase(std::remove(lineTemp.begin(), lineTemp.end(), ' '), lineTemp.end());
|
||||
|
||||
std::stringstream ss(lineTemp);
|
||||
std::string stringTemp;
|
||||
|
||||
std::getline(ss, _label, ',');
|
||||
|
||||
while(std::getline(ss, stringTemp, ',')){
|
||||
_values.push_back(stod(stringTemp));
|
||||
}
|
||||
|
||||
// std::cout << "**********" << std::endl;
|
||||
// std::cout << "_label: " << _label << std::fixed << std::setprecision(3) << std::endl;
|
||||
// for(int i(0); i<_values.size(); ++i){
|
||||
// std::cout << _values.at(i) << ",,, ";
|
||||
// }
|
||||
// std::cout << std::endl;
|
||||
}
|
||||
|
||||
inline CSVLine::CSVLine(std::string label, std::vector<double> values)
|
||||
:_label(label), _values(values){
|
||||
|
||||
}
|
||||
|
||||
inline void CSVLine::changeValue(std::vector<double> values){
|
||||
if(values.size() != _values.size()){
|
||||
std::cout << "[WARNING] CSVLine::changeValue, the size changed" << std::endl;
|
||||
}
|
||||
_values = values;
|
||||
}
|
||||
|
||||
inline void CSVLine::getValues(std::vector<double> &values){
|
||||
values = _values;
|
||||
}
|
||||
|
||||
inline void CSVLine::writeAtEnd(std::fstream &ioStream){
|
||||
ioStream << _label << ", ";
|
||||
|
||||
for(int i(0); i<_values.size(); ++i){
|
||||
ioStream << _values.at(i) << ", ";
|
||||
}
|
||||
|
||||
ioStream << std::endl;
|
||||
}
|
||||
|
||||
|
||||
/*************************/
|
||||
/* CSVTool */
|
||||
/*************************/
|
||||
inline CSVTool::CSVTool(std::string fileName, FileType type, int precision)
|
||||
: _fileName(fileName), _precision(precision){
|
||||
|
||||
if(type == FileType::READ_WRITE){
|
||||
_ioStream.open(_fileName, std::fstream::ate | std::fstream::in | std::fstream::out);
|
||||
|
||||
if(!_ioStream.is_open()){
|
||||
std::cout << "[ERROR] CSVTool open file: " << fileName << " failed!" << std::endl;
|
||||
// exit(-1);
|
||||
_hasFile = false;
|
||||
}else{
|
||||
readFile();
|
||||
_hasFile = true;
|
||||
}
|
||||
|
||||
}
|
||||
else if(type == FileType::CLEAR_DUMP){
|
||||
_ioStream.open(_fileName, std::fstream::out);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
inline void CSVTool::readFile(){
|
||||
if(!_ioStream.is_open()){
|
||||
// _ioStream.open(_fileName, std::fstream::ate | std::fstream::in | std::fstream::out);
|
||||
std::cout << "[ERROR] CSVTool::readFile, file: " << _fileName << " has not been opened!" << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
_lines.clear();
|
||||
_labels.clear();
|
||||
|
||||
_ioStream.seekg(0, std::fstream::beg);
|
||||
size_t lineNum = 0;
|
||||
while(_ioStream && _ioStream.tellg() != std::fstream::end && getline(_ioStream, _lineTemp)){
|
||||
_lines.push_back( new CSVLine(_lineTemp) );
|
||||
|
||||
if(_labels.count(_lines.at(lineNum)->getLabel()) == 0){
|
||||
_labels.insert(std::pair<std::string, size_t>(_lines.at(lineNum)->getLabel(), lineNum));
|
||||
++lineNum;
|
||||
}else{
|
||||
std::cout << "[ERROR] CSVTool::readFile, the label "
|
||||
<< _lines.at(lineNum)->getLabel() << " is repeated" << std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
inline bool CSVTool::getLine(std::string label, std::vector<double> &values){
|
||||
if(_labels.count(label) == 0){
|
||||
std::cout << "[ERROR] No such label: " << label << std::endl;
|
||||
return false;
|
||||
}else{
|
||||
_lines.at(_labels[label])->getValues(values);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
template<typename... Args>
|
||||
inline bool CSVTool::getLineDirect(std::string label, Args&... values){
|
||||
std::vector<double> vec;
|
||||
if(getLine(label, vec)){
|
||||
typeTrans::extractVector(vec, values...);
|
||||
return true;
|
||||
}else{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
template<typename... Args>
|
||||
inline void CSVTool::modifyLineDirect(std::string label, bool addNew, Args&... values){
|
||||
std::vector<double> vec;
|
||||
typeTrans::combineToVector(vec, values...);
|
||||
|
||||
// std::cout << "CSVTool::modifyLineDirect------" << std::endl;
|
||||
// std::cout << "label: " << label << std::endl;
|
||||
// std::cout << "vec: ";
|
||||
// for(int i(0); i<vec.size(); ++i){
|
||||
// std::cout << vec.at(i) << ", ";
|
||||
// }std::cout << std::endl;
|
||||
|
||||
modifyLine(label, vec, addNew);
|
||||
}
|
||||
|
||||
|
||||
inline void CSVTool::saveFile(){
|
||||
_ioStream.close();
|
||||
_ioStream.open(_fileName, std::fstream::out);
|
||||
_ioStream << std::fixed << std::setprecision(_precision);
|
||||
for(int i(0); i<_lines.size(); ++i){
|
||||
_lines.at(i)->writeAtEnd(_ioStream);
|
||||
}
|
||||
|
||||
_ioStream.close();
|
||||
_ioStream.open(_fileName, std::fstream::ate | std::fstream::in | std::fstream::out);
|
||||
}
|
||||
|
||||
inline void CSVTool::modifyLine(std::string label, std::vector<double> &values, bool addNew =false){
|
||||
if(_labels.count(label) == 0){
|
||||
if(addNew){
|
||||
_labels.insert(std::pair<std::string, size_t>(label, _labels.size()));
|
||||
_lines.push_back(new CSVLine(label, values));
|
||||
}else{
|
||||
std::cout << "[ERROR] CSVTool::modifyLine, label " << label << "does not exist" << std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
}else{
|
||||
_lines.at(_labels[label])->changeValue(values);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#endif // CSVTOOL_H
|
||||
|
|
@ -1,75 +1,75 @@
|
|||
#ifndef TYPETRANS_H
|
||||
#define TYPETRANS_H
|
||||
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <Eigen/Dense>
|
||||
|
||||
namespace typeTrans{
|
||||
|
||||
inline void addValue(std::vector<double> &vec, double value){
|
||||
vec.push_back(value);
|
||||
}
|
||||
|
||||
inline double getValue(std::vector<double> &vec, double value){
|
||||
value = vec.at(0);
|
||||
std::vector<double>::iterator begin = vec.begin();
|
||||
vec.erase(begin);
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
inline void addValue(std::vector<double> &vec, Eigen::MatrixXd value){
|
||||
for(int i(0); i<value.rows(); ++i){
|
||||
for(int j(0); j<value.cols(); ++j){
|
||||
vec.push_back(value(i, j));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
inline Eigen::MatrixXd getValue(std::vector<double> &vec, Eigen::MatrixXd value){
|
||||
std::vector<double>::iterator begin = vec.begin();
|
||||
std::vector<double>::iterator end = begin;
|
||||
|
||||
for(int i(0); i<value.rows(); ++i){
|
||||
for(int j(0); j<value.cols(); ++j){
|
||||
value(i, j) = *end;
|
||||
++end;
|
||||
}
|
||||
}
|
||||
|
||||
vec.erase(begin, end);
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
/* combine different type variables to vector */
|
||||
template<typename T>
|
||||
inline void combineToVector(std::vector<double> &vec, T value){
|
||||
addValue(vec, value);
|
||||
}
|
||||
|
||||
template<typename T, typename... Args>
|
||||
inline void combineToVector(std::vector<double> &vec, const T t, const Args... rest){
|
||||
combineToVector(vec, t);
|
||||
|
||||
combineToVector(vec, rest...);
|
||||
}
|
||||
|
||||
|
||||
/* extract different type variables from vector */
|
||||
template<typename T>
|
||||
inline void extractVector(std::vector<double> &vec, T &value){
|
||||
value = getValue(vec, value);
|
||||
}
|
||||
|
||||
template<typename T, typename... Args>
|
||||
inline void extractVector(std::vector<double> &vec, T &t, Args&... rest){
|
||||
extractVector(vec, t);
|
||||
|
||||
extractVector(vec, rest...);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#ifndef TYPETRANS_H
|
||||
#define TYPETRANS_H
|
||||
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <Eigen/Dense>
|
||||
|
||||
namespace typeTrans{
|
||||
|
||||
inline void addValue(std::vector<double> &vec, double value){
|
||||
vec.push_back(value);
|
||||
}
|
||||
|
||||
inline double getValue(std::vector<double> &vec, double value){
|
||||
value = vec.at(0);
|
||||
std::vector<double>::iterator begin = vec.begin();
|
||||
vec.erase(begin);
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
inline void addValue(std::vector<double> &vec, Eigen::MatrixXd value){
|
||||
for(int i(0); i<value.rows(); ++i){
|
||||
for(int j(0); j<value.cols(); ++j){
|
||||
vec.push_back(value(i, j));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
inline Eigen::MatrixXd getValue(std::vector<double> &vec, Eigen::MatrixXd value){
|
||||
std::vector<double>::iterator begin = vec.begin();
|
||||
std::vector<double>::iterator end = begin;
|
||||
|
||||
for(int i(0); i<value.rows(); ++i){
|
||||
for(int j(0); j<value.cols(); ++j){
|
||||
value(i, j) = *end;
|
||||
++end;
|
||||
}
|
||||
}
|
||||
|
||||
vec.erase(begin, end);
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
/* combine different type variables to vector */
|
||||
template<typename T>
|
||||
inline void combineToVector(std::vector<double> &vec, T value){
|
||||
addValue(vec, value);
|
||||
}
|
||||
|
||||
template<typename T, typename... Args>
|
||||
inline void combineToVector(std::vector<double> &vec, const T t, const Args... rest){
|
||||
combineToVector(vec, t);
|
||||
|
||||
combineToVector(vec, rest...);
|
||||
}
|
||||
|
||||
|
||||
/* extract different type variables from vector */
|
||||
template<typename T>
|
||||
inline void extractVector(std::vector<double> &vec, T &value){
|
||||
value = getValue(vec, value);
|
||||
}
|
||||
|
||||
template<typename T, typename... Args>
|
||||
inline void extractVector(std::vector<double> &vec, T &t, Args&... rest){
|
||||
extractVector(vec, t);
|
||||
|
||||
extractVector(vec, rest...);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif // TYPETRANS_H
|
||||
|
|
@ -1,53 +1,53 @@
|
|||
#ifndef CTRLCOMPONENTS_H
|
||||
#define CTRLCOMPONENTS_H
|
||||
|
||||
#include <string>
|
||||
#include <iostream>
|
||||
#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(int argc, char**argv);
|
||||
~CtrlComponents();
|
||||
|
||||
std::string armConfigPath;
|
||||
CmdPanel *cmdPanel;
|
||||
IOInterface *ioInter;
|
||||
Z1Model *armModel;
|
||||
CSVTool *stateCSV;
|
||||
|
||||
SendCmd sendCmd; // cmd that receive from SDK
|
||||
RecvState recvState;// state that send to SDK
|
||||
|
||||
//config
|
||||
double dt;
|
||||
bool *running;
|
||||
Control ctrl = Control::SDK;
|
||||
bool hasGripper;
|
||||
bool isCollisionOpen;
|
||||
double collisionTLimit;
|
||||
bool isPlot;
|
||||
int trajChoose = 1;
|
||||
size_t armType = 36;
|
||||
|
||||
void geneObj();
|
||||
void writeData();
|
||||
private:
|
||||
void configProcess(int argc, char** argv);
|
||||
|
||||
std::string ctrl_IP;
|
||||
uint ctrl_port;
|
||||
double _loadWeight;
|
||||
};
|
||||
|
||||
#ifndef CTRLCOMPONENTS_H
|
||||
#define CTRLCOMPONENTS_H
|
||||
|
||||
#include <string>
|
||||
#include <iostream>
|
||||
#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(int argc, char**argv);
|
||||
~CtrlComponents();
|
||||
|
||||
std::string armConfigPath;
|
||||
CmdPanel *cmdPanel;
|
||||
IOInterface *ioInter;
|
||||
Z1Model *armModel;
|
||||
CSVTool *stateCSV;
|
||||
|
||||
SendCmd sendCmd; // cmd that receive from SDK
|
||||
RecvState recvState;// state that send to SDK
|
||||
|
||||
//config
|
||||
double dt;
|
||||
bool *running;
|
||||
Control ctrl = Control::SDK;
|
||||
bool hasGripper;
|
||||
bool isCollisionOpen;
|
||||
double collisionTLimit;
|
||||
bool isPlot;
|
||||
int trajChoose = 1;
|
||||
size_t armType = 36;
|
||||
|
||||
void geneObj();
|
||||
void writeData();
|
||||
private:
|
||||
void configProcess(int argc, char** argv);
|
||||
|
||||
std::string ctrl_IP;
|
||||
uint ctrl_port;
|
||||
double _loadWeight;
|
||||
};
|
||||
|
||||
#endif // CTRLCOMPONENTS_H
|
||||
|
|
@ -1,28 +1,28 @@
|
|||
#ifndef IOINTERFACE_H
|
||||
#define IOINTERFACE_H
|
||||
|
||||
#include <string>
|
||||
#include "message/LowlevelCmd.h"
|
||||
#include "message/LowlevelState.h"
|
||||
#include "control/keyboard.h"
|
||||
#include "common/math/robotics.h"
|
||||
|
||||
class IOInterface{
|
||||
public:
|
||||
IOInterface(){}
|
||||
~IOInterface(){
|
||||
delete lowCmd;
|
||||
delete lowState;
|
||||
};
|
||||
virtual bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state) = 0;
|
||||
virtual bool calibration(){return false;};
|
||||
bool checkGripper(){return hasGripper;};
|
||||
LowlevelCmd *lowCmd;
|
||||
LowlevelState *lowState;
|
||||
virtual bool isDisconnect(){ return false;};
|
||||
bool hasErrorState;
|
||||
protected:
|
||||
bool hasGripper;
|
||||
};
|
||||
|
||||
#ifndef IOINTERFACE_H
|
||||
#define IOINTERFACE_H
|
||||
|
||||
#include <string>
|
||||
#include "message/LowlevelCmd.h"
|
||||
#include "message/LowlevelState.h"
|
||||
#include "control/keyboard.h"
|
||||
#include "common/math/robotics.h"
|
||||
|
||||
class IOInterface{
|
||||
public:
|
||||
IOInterface(){}
|
||||
~IOInterface(){
|
||||
delete lowCmd;
|
||||
delete lowState;
|
||||
};
|
||||
virtual bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state) = 0;
|
||||
virtual bool calibration(){return false;};
|
||||
bool checkGripper(){return hasGripper;};
|
||||
LowlevelCmd *lowCmd;
|
||||
LowlevelState *lowState;
|
||||
virtual bool isDisconnect(){ return false;};
|
||||
bool hasErrorState;
|
||||
protected:
|
||||
bool hasGripper;
|
||||
};
|
||||
|
||||
#endif //IOINTERFACE_H
|
||||
|
|
@ -1,26 +1,26 @@
|
|||
#ifndef IOUDP_H
|
||||
#define IOUDP_H
|
||||
|
||||
#include "interface/IOInterface.h"
|
||||
|
||||
class IOUDP : public IOInterface{
|
||||
public:
|
||||
IOUDP(const char* IP, uint port, size_t timeOutUs = 100000, bool showInfo = true);
|
||||
~IOUDP();
|
||||
|
||||
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];
|
||||
};
|
||||
|
||||
#ifndef IOUDP_H
|
||||
#define IOUDP_H
|
||||
|
||||
#include "interface/IOInterface.h"
|
||||
|
||||
class IOUDP : public IOInterface{
|
||||
public:
|
||||
IOUDP(const char* IP, uint port, size_t timeOutUs = 100000, bool showInfo = true);
|
||||
~IOUDP();
|
||||
|
||||
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];
|
||||
};
|
||||
|
||||
#endif // IOUDP_H
|
||||
|
|
@ -1,56 +1,56 @@
|
|||
#ifndef LOWLEVELCMD_H
|
||||
#define LOWLEVELCMD_H
|
||||
|
||||
#include "common/math/mathTypes.h"
|
||||
#include "common/math/mathTools.h"
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
|
||||
struct LowlevelCmd{
|
||||
public:
|
||||
std::vector<double> q;
|
||||
std::vector<double> dq;
|
||||
std::vector<double> tau;
|
||||
std::vector<double> kp;
|
||||
std::vector<double> kd;
|
||||
|
||||
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(){}
|
||||
|
||||
void setZeroDq();
|
||||
void setZeroTau();
|
||||
void setZeroKp();
|
||||
void setZeroKd();
|
||||
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();
|
||||
void setGripperQ(double qInput);
|
||||
double getGripperQ();
|
||||
void setGripperQd(double qdInput);
|
||||
double getGripperQd();
|
||||
void setGripperTau(double tauInput);
|
||||
double getGripperTau();
|
||||
|
||||
Vec6 getQ();
|
||||
Vec6 getQd();
|
||||
|
||||
void resizeGripper();
|
||||
private:
|
||||
size_t _dof = 6;
|
||||
};
|
||||
|
||||
|
||||
#endif //LOWLEVELCMD_H
|
||||
#ifndef LOWLEVELCMD_H
|
||||
#define LOWLEVELCMD_H
|
||||
|
||||
#include "common/math/mathTypes.h"
|
||||
#include "common/math/mathTools.h"
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
|
||||
struct LowlevelCmd{
|
||||
public:
|
||||
std::vector<double> q;
|
||||
std::vector<double> dq;
|
||||
std::vector<double> tau;
|
||||
std::vector<double> kp;
|
||||
std::vector<double> kd;
|
||||
|
||||
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(){}
|
||||
|
||||
void setZeroDq();
|
||||
void setZeroTau();
|
||||
void setZeroKp();
|
||||
void setZeroKd();
|
||||
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();
|
||||
void setGripperQ(double qInput);
|
||||
double getGripperQ();
|
||||
void setGripperQd(double qdInput);
|
||||
double getGripperQd();
|
||||
void setGripperTau(double tauInput);
|
||||
double getGripperTau();
|
||||
|
||||
Vec6 getQ();
|
||||
Vec6 getQd();
|
||||
|
||||
void resizeGripper();
|
||||
private:
|
||||
size_t _dof = 6;
|
||||
};
|
||||
|
||||
|
||||
#endif //LOWLEVELCMD_H
|
||||
|
|
|
|||
|
|
@ -1,61 +1,61 @@
|
|||
#ifndef LOWLEVELSTATE_HPP
|
||||
#define LOWLEVELSTATE_HPP
|
||||
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include "common/math/mathTools.h"
|
||||
#include "common/enumClass.h"
|
||||
#include "common/math/Filter.h"
|
||||
|
||||
struct LowlevelState{
|
||||
public:
|
||||
LowlevelState(double dt);
|
||||
~LowlevelState();
|
||||
|
||||
std::vector<double> q;
|
||||
std::vector<double> dq;
|
||||
std::vector<double> ddq;
|
||||
std::vector<double> tau;
|
||||
|
||||
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;
|
||||
|
||||
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();
|
||||
double getGripperQd();
|
||||
double getGripperTau();
|
||||
double getGripperTauFiltered();
|
||||
private:
|
||||
size_t _dof = 6;
|
||||
int temporatureLimit = 80.0;// centigrade
|
||||
std::vector<int> _isMotorConnectedCnt;
|
||||
std::vector<bool> _isMotorLostConnection;
|
||||
};
|
||||
|
||||
#endif //LOWLEVELSTATE_HPP
|
||||
#ifndef LOWLEVELSTATE_HPP
|
||||
#define LOWLEVELSTATE_HPP
|
||||
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include "common/math/mathTools.h"
|
||||
#include "common/enumClass.h"
|
||||
#include "common/math/Filter.h"
|
||||
|
||||
struct LowlevelState{
|
||||
public:
|
||||
LowlevelState(double dt);
|
||||
~LowlevelState();
|
||||
|
||||
std::vector<double> q;
|
||||
std::vector<double> dq;
|
||||
std::vector<double> ddq;
|
||||
std::vector<double> tau;
|
||||
|
||||
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;
|
||||
|
||||
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();
|
||||
double getGripperQd();
|
||||
double getGripperTau();
|
||||
double getGripperTauFiltered();
|
||||
private:
|
||||
size_t _dof = 6;
|
||||
int temporatureLimit = 80.0;// centigrade
|
||||
std::vector<int> _isMotorConnectedCnt;
|
||||
std::vector<bool> _isMotorLostConnection;
|
||||
};
|
||||
|
||||
#endif //LOWLEVELSTATE_HPP
|
||||
|
|
|
|||
|
|
@ -1,261 +1,261 @@
|
|||
// Generated by gencpp from file unitree_legged_msgs/MotorCmd.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef UNITREE_LEGGED_MSGS_MESSAGE_MOTORCMD_H
|
||||
#define UNITREE_LEGGED_MSGS_MESSAGE_MOTORCMD_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 MotorCmd_
|
||||
{
|
||||
typedef MotorCmd_<ContainerAllocator> Type;
|
||||
|
||||
MotorCmd_()
|
||||
: mode(0)
|
||||
, q(0.0)
|
||||
, dq(0.0)
|
||||
, tau(0.0)
|
||||
, Kp(0.0)
|
||||
, Kd(0.0)
|
||||
, reserve() {
|
||||
reserve.assign(0);
|
||||
}
|
||||
MotorCmd_(const ContainerAllocator& _alloc)
|
||||
: mode(0)
|
||||
, q(0.0)
|
||||
, dq(0.0)
|
||||
, tau(0.0)
|
||||
, Kp(0.0)
|
||||
, Kd(0.0)
|
||||
, reserve() {
|
||||
(void)_alloc;
|
||||
reserve.assign(0);
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef uint8_t _mode_type;
|
||||
_mode_type mode;
|
||||
|
||||
typedef float _q_type;
|
||||
_q_type q;
|
||||
|
||||
typedef float _dq_type;
|
||||
_dq_type dq;
|
||||
|
||||
typedef float _tau_type;
|
||||
_tau_type tau;
|
||||
|
||||
typedef float _Kp_type;
|
||||
_Kp_type Kp;
|
||||
|
||||
typedef float _Kd_type;
|
||||
_Kd_type Kd;
|
||||
|
||||
typedef boost::array<uint32_t, 3> _reserve_type;
|
||||
_reserve_type reserve;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> > Ptr;
|
||||
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct MotorCmd_
|
||||
|
||||
typedef ::unitree_legged_msgs::MotorCmd_<std::allocator<void> > MotorCmd;
|
||||
|
||||
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd > MotorCmdPtr;
|
||||
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd const> MotorCmdConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator>
|
||||
std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> & v)
|
||||
{
|
||||
ros::message_operations::Printer< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >::stream(s, "", v);
|
||||
return s;
|
||||
}
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.mode == rhs.mode &&
|
||||
lhs.q == rhs.q &&
|
||||
lhs.dq == rhs.dq &&
|
||||
lhs.tau == rhs.tau &&
|
||||
lhs.Kp == rhs.Kp &&
|
||||
lhs.Kd == rhs.Kd &&
|
||||
lhs.reserve == rhs.reserve;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace unitree_legged_msgs
|
||||
|
||||
namespace ros
|
||||
{
|
||||
namespace message_traits
|
||||
{
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct IsFixedSize< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
|
||||
: TrueType
|
||||
{ };
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct IsFixedSize< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> const>
|
||||
: TrueType
|
||||
{ };
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct IsMessage< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
|
||||
: TrueType
|
||||
{ };
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct IsMessage< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> const>
|
||||
: TrueType
|
||||
{ };
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct HasHeader< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
|
||||
: FalseType
|
||||
{ };
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct HasHeader< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> const>
|
||||
: FalseType
|
||||
{ };
|
||||
|
||||
|
||||
template<class ContainerAllocator>
|
||||
struct MD5Sum< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
|
||||
{
|
||||
static const char* value()
|
||||
{
|
||||
return "bbb3b7d91319c3a1b99055f0149ba221";
|
||||
}
|
||||
|
||||
static const char* value(const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator>&) { return value(); }
|
||||
static const uint64_t static_value1 = 0xbbb3b7d91319c3a1ULL;
|
||||
static const uint64_t static_value2 = 0xb99055f0149ba221ULL;
|
||||
};
|
||||
|
||||
template<class ContainerAllocator>
|
||||
struct DataType< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
|
||||
{
|
||||
static const char* value()
|
||||
{
|
||||
return "unitree_legged_msgs/MotorCmd";
|
||||
}
|
||||
|
||||
static const char* value(const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator>&) { return value(); }
|
||||
};
|
||||
|
||||
template<class ContainerAllocator>
|
||||
struct Definition< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
|
||||
{
|
||||
static const char* value()
|
||||
{
|
||||
return "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"
|
||||
;
|
||||
}
|
||||
|
||||
static const char* value(const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator>&) { return value(); }
|
||||
};
|
||||
|
||||
} // namespace message_traits
|
||||
} // namespace ros
|
||||
|
||||
namespace ros
|
||||
{
|
||||
namespace serialization
|
||||
{
|
||||
|
||||
template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
|
||||
{
|
||||
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
|
||||
{
|
||||
stream.next(m.mode);
|
||||
stream.next(m.q);
|
||||
stream.next(m.dq);
|
||||
stream.next(m.tau);
|
||||
stream.next(m.Kp);
|
||||
stream.next(m.Kd);
|
||||
stream.next(m.reserve);
|
||||
}
|
||||
|
||||
ROS_DECLARE_ALLINONE_SERIALIZER
|
||||
}; // struct MotorCmd_
|
||||
|
||||
} // namespace serialization
|
||||
} // namespace ros
|
||||
|
||||
namespace ros
|
||||
{
|
||||
namespace message_operations
|
||||
{
|
||||
|
||||
template<class ContainerAllocator>
|
||||
struct Printer< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
|
||||
{
|
||||
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator>& v)
|
||||
{
|
||||
s << indent << "mode: ";
|
||||
Printer<uint8_t>::stream(s, indent + " ", v.mode);
|
||||
s << indent << "q: ";
|
||||
Printer<float>::stream(s, indent + " ", v.q);
|
||||
s << indent << "dq: ";
|
||||
Printer<float>::stream(s, indent + " ", v.dq);
|
||||
s << indent << "tau: ";
|
||||
Printer<float>::stream(s, indent + " ", v.tau);
|
||||
s << indent << "Kp: ";
|
||||
Printer<float>::stream(s, indent + " ", v.Kp);
|
||||
s << indent << "Kd: ";
|
||||
Printer<float>::stream(s, indent + " ", v.Kd);
|
||||
s << indent << "reserve[]" << std::endl;
|
||||
for (size_t i = 0; i < v.reserve.size(); ++i)
|
||||
{
|
||||
s << indent << " reserve[" << i << "]: ";
|
||||
Printer<uint32_t>::stream(s, indent + " ", v.reserve[i]);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace message_operations
|
||||
} // namespace ros
|
||||
|
||||
#endif // UNITREE_LEGGED_MSGS_MESSAGE_MOTORCMD_H
|
||||
// Generated by gencpp from file unitree_legged_msgs/MotorCmd.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef UNITREE_LEGGED_MSGS_MESSAGE_MOTORCMD_H
|
||||
#define UNITREE_LEGGED_MSGS_MESSAGE_MOTORCMD_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 MotorCmd_
|
||||
{
|
||||
typedef MotorCmd_<ContainerAllocator> Type;
|
||||
|
||||
MotorCmd_()
|
||||
: mode(0)
|
||||
, q(0.0)
|
||||
, dq(0.0)
|
||||
, tau(0.0)
|
||||
, Kp(0.0)
|
||||
, Kd(0.0)
|
||||
, reserve() {
|
||||
reserve.assign(0);
|
||||
}
|
||||
MotorCmd_(const ContainerAllocator& _alloc)
|
||||
: mode(0)
|
||||
, q(0.0)
|
||||
, dq(0.0)
|
||||
, tau(0.0)
|
||||
, Kp(0.0)
|
||||
, Kd(0.0)
|
||||
, reserve() {
|
||||
(void)_alloc;
|
||||
reserve.assign(0);
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef uint8_t _mode_type;
|
||||
_mode_type mode;
|
||||
|
||||
typedef float _q_type;
|
||||
_q_type q;
|
||||
|
||||
typedef float _dq_type;
|
||||
_dq_type dq;
|
||||
|
||||
typedef float _tau_type;
|
||||
_tau_type tau;
|
||||
|
||||
typedef float _Kp_type;
|
||||
_Kp_type Kp;
|
||||
|
||||
typedef float _Kd_type;
|
||||
_Kd_type Kd;
|
||||
|
||||
typedef boost::array<uint32_t, 3> _reserve_type;
|
||||
_reserve_type reserve;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> > Ptr;
|
||||
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct MotorCmd_
|
||||
|
||||
typedef ::unitree_legged_msgs::MotorCmd_<std::allocator<void> > MotorCmd;
|
||||
|
||||
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd > MotorCmdPtr;
|
||||
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd const> MotorCmdConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator>
|
||||
std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> & v)
|
||||
{
|
||||
ros::message_operations::Printer< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >::stream(s, "", v);
|
||||
return s;
|
||||
}
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.mode == rhs.mode &&
|
||||
lhs.q == rhs.q &&
|
||||
lhs.dq == rhs.dq &&
|
||||
lhs.tau == rhs.tau &&
|
||||
lhs.Kp == rhs.Kp &&
|
||||
lhs.Kd == rhs.Kd &&
|
||||
lhs.reserve == rhs.reserve;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace unitree_legged_msgs
|
||||
|
||||
namespace ros
|
||||
{
|
||||
namespace message_traits
|
||||
{
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct IsFixedSize< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
|
||||
: TrueType
|
||||
{ };
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct IsFixedSize< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> const>
|
||||
: TrueType
|
||||
{ };
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct IsMessage< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
|
||||
: TrueType
|
||||
{ };
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct IsMessage< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> const>
|
||||
: TrueType
|
||||
{ };
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct HasHeader< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
|
||||
: FalseType
|
||||
{ };
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct HasHeader< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> const>
|
||||
: FalseType
|
||||
{ };
|
||||
|
||||
|
||||
template<class ContainerAllocator>
|
||||
struct MD5Sum< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
|
||||
{
|
||||
static const char* value()
|
||||
{
|
||||
return "bbb3b7d91319c3a1b99055f0149ba221";
|
||||
}
|
||||
|
||||
static const char* value(const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator>&) { return value(); }
|
||||
static const uint64_t static_value1 = 0xbbb3b7d91319c3a1ULL;
|
||||
static const uint64_t static_value2 = 0xb99055f0149ba221ULL;
|
||||
};
|
||||
|
||||
template<class ContainerAllocator>
|
||||
struct DataType< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
|
||||
{
|
||||
static const char* value()
|
||||
{
|
||||
return "unitree_legged_msgs/MotorCmd";
|
||||
}
|
||||
|
||||
static const char* value(const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator>&) { return value(); }
|
||||
};
|
||||
|
||||
template<class ContainerAllocator>
|
||||
struct Definition< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
|
||||
{
|
||||
static const char* value()
|
||||
{
|
||||
return "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"
|
||||
;
|
||||
}
|
||||
|
||||
static const char* value(const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator>&) { return value(); }
|
||||
};
|
||||
|
||||
} // namespace message_traits
|
||||
} // namespace ros
|
||||
|
||||
namespace ros
|
||||
{
|
||||
namespace serialization
|
||||
{
|
||||
|
||||
template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
|
||||
{
|
||||
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
|
||||
{
|
||||
stream.next(m.mode);
|
||||
stream.next(m.q);
|
||||
stream.next(m.dq);
|
||||
stream.next(m.tau);
|
||||
stream.next(m.Kp);
|
||||
stream.next(m.Kd);
|
||||
stream.next(m.reserve);
|
||||
}
|
||||
|
||||
ROS_DECLARE_ALLINONE_SERIALIZER
|
||||
}; // struct MotorCmd_
|
||||
|
||||
} // namespace serialization
|
||||
} // namespace ros
|
||||
|
||||
namespace ros
|
||||
{
|
||||
namespace message_operations
|
||||
{
|
||||
|
||||
template<class ContainerAllocator>
|
||||
struct Printer< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
|
||||
{
|
||||
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator>& v)
|
||||
{
|
||||
s << indent << "mode: ";
|
||||
Printer<uint8_t>::stream(s, indent + " ", v.mode);
|
||||
s << indent << "q: ";
|
||||
Printer<float>::stream(s, indent + " ", v.q);
|
||||
s << indent << "dq: ";
|
||||
Printer<float>::stream(s, indent + " ", v.dq);
|
||||
s << indent << "tau: ";
|
||||
Printer<float>::stream(s, indent + " ", v.tau);
|
||||
s << indent << "Kp: ";
|
||||
Printer<float>::stream(s, indent + " ", v.Kp);
|
||||
s << indent << "Kd: ";
|
||||
Printer<float>::stream(s, indent + " ", v.Kd);
|
||||
s << indent << "reserve[]" << std::endl;
|
||||
for (size_t i = 0; i < v.reserve.size(); ++i)
|
||||
{
|
||||
s << indent << " reserve[" << i << "]: ";
|
||||
Printer<uint32_t>::stream(s, indent + " ", v.reserve[i]);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace message_operations
|
||||
} // namespace ros
|
||||
|
||||
#endif // UNITREE_LEGGED_MSGS_MESSAGE_MOTORCMD_H
|
||||
|
|
|
|||
|
|
@ -1,291 +1,291 @@
|
|||
// Generated by gencpp from file unitree_legged_msgs/MotorState.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef UNITREE_LEGGED_MSGS_MESSAGE_MOTORSTATE_H
|
||||
#define UNITREE_LEGGED_MSGS_MESSAGE_MOTORSTATE_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 MotorState_
|
||||
{
|
||||
typedef MotorState_<ContainerAllocator> Type;
|
||||
|
||||
MotorState_()
|
||||
: mode(0)
|
||||
, q(0.0)
|
||||
, dq(0.0)
|
||||
, ddq(0.0)
|
||||
, tauEst(0.0)
|
||||
, q_raw(0.0)
|
||||
, dq_raw(0.0)
|
||||
, ddq_raw(0.0)
|
||||
, temperature(0)
|
||||
, reserve() {
|
||||
reserve.assign(0);
|
||||
}
|
||||
MotorState_(const ContainerAllocator& _alloc)
|
||||
: mode(0)
|
||||
, q(0.0)
|
||||
, dq(0.0)
|
||||
, ddq(0.0)
|
||||
, tauEst(0.0)
|
||||
, q_raw(0.0)
|
||||
, dq_raw(0.0)
|
||||
, ddq_raw(0.0)
|
||||
, temperature(0)
|
||||
, reserve() {
|
||||
(void)_alloc;
|
||||
reserve.assign(0);
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef uint8_t _mode_type;
|
||||
_mode_type mode;
|
||||
|
||||
typedef float _q_type;
|
||||
_q_type q;
|
||||
|
||||
typedef float _dq_type;
|
||||
_dq_type dq;
|
||||
|
||||
typedef float _ddq_type;
|
||||
_ddq_type ddq;
|
||||
|
||||
typedef float _tauEst_type;
|
||||
_tauEst_type tauEst;
|
||||
|
||||
typedef float _q_raw_type;
|
||||
_q_raw_type q_raw;
|
||||
|
||||
typedef float _dq_raw_type;
|
||||
_dq_raw_type dq_raw;
|
||||
|
||||
typedef float _ddq_raw_type;
|
||||
_ddq_raw_type ddq_raw;
|
||||
|
||||
typedef int8_t _temperature_type;
|
||||
_temperature_type temperature;
|
||||
|
||||
typedef boost::array<uint32_t, 2> _reserve_type;
|
||||
_reserve_type reserve;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState_<ContainerAllocator> > Ptr;
|
||||
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct MotorState_
|
||||
|
||||
typedef ::unitree_legged_msgs::MotorState_<std::allocator<void> > MotorState;
|
||||
|
||||
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState > MotorStatePtr;
|
||||
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState const> MotorStateConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator>
|
||||
std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::MotorState_<ContainerAllocator> & v)
|
||||
{
|
||||
ros::message_operations::Printer< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >::stream(s, "", v);
|
||||
return s;
|
||||
}
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::unitree_legged_msgs::MotorState_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::MotorState_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.mode == rhs.mode &&
|
||||
lhs.q == rhs.q &&
|
||||
lhs.dq == rhs.dq &&
|
||||
lhs.ddq == rhs.ddq &&
|
||||
lhs.tauEst == rhs.tauEst &&
|
||||
lhs.q_raw == rhs.q_raw &&
|
||||
lhs.dq_raw == rhs.dq_raw &&
|
||||
lhs.ddq_raw == rhs.ddq_raw &&
|
||||
lhs.temperature == rhs.temperature &&
|
||||
lhs.reserve == rhs.reserve;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::unitree_legged_msgs::MotorState_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::MotorState_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace unitree_legged_msgs
|
||||
|
||||
namespace ros
|
||||
{
|
||||
namespace message_traits
|
||||
{
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct IsFixedSize< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
|
||||
: TrueType
|
||||
{ };
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct IsFixedSize< ::unitree_legged_msgs::MotorState_<ContainerAllocator> const>
|
||||
: TrueType
|
||||
{ };
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct IsMessage< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
|
||||
: TrueType
|
||||
{ };
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct IsMessage< ::unitree_legged_msgs::MotorState_<ContainerAllocator> const>
|
||||
: TrueType
|
||||
{ };
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct HasHeader< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
|
||||
: FalseType
|
||||
{ };
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct HasHeader< ::unitree_legged_msgs::MotorState_<ContainerAllocator> const>
|
||||
: FalseType
|
||||
{ };
|
||||
|
||||
|
||||
template<class ContainerAllocator>
|
||||
struct MD5Sum< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
|
||||
{
|
||||
static const char* value()
|
||||
{
|
||||
return "94c55ee3b7852be2bd437b20ce12a254";
|
||||
}
|
||||
|
||||
static const char* value(const ::unitree_legged_msgs::MotorState_<ContainerAllocator>&) { return value(); }
|
||||
static const uint64_t static_value1 = 0x94c55ee3b7852be2ULL;
|
||||
static const uint64_t static_value2 = 0xbd437b20ce12a254ULL;
|
||||
};
|
||||
|
||||
template<class ContainerAllocator>
|
||||
struct DataType< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
|
||||
{
|
||||
static const char* value()
|
||||
{
|
||||
return "unitree_legged_msgs/MotorState";
|
||||
}
|
||||
|
||||
static const char* value(const ::unitree_legged_msgs::MotorState_<ContainerAllocator>&) { return value(); }
|
||||
};
|
||||
|
||||
template<class ContainerAllocator>
|
||||
struct Definition< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
|
||||
{
|
||||
static const char* value()
|
||||
{
|
||||
return "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"
|
||||
;
|
||||
}
|
||||
|
||||
static const char* value(const ::unitree_legged_msgs::MotorState_<ContainerAllocator>&) { return value(); }
|
||||
};
|
||||
|
||||
} // namespace message_traits
|
||||
} // namespace ros
|
||||
|
||||
namespace ros
|
||||
{
|
||||
namespace serialization
|
||||
{
|
||||
|
||||
template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
|
||||
{
|
||||
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
|
||||
{
|
||||
stream.next(m.mode);
|
||||
stream.next(m.q);
|
||||
stream.next(m.dq);
|
||||
stream.next(m.ddq);
|
||||
stream.next(m.tauEst);
|
||||
stream.next(m.q_raw);
|
||||
stream.next(m.dq_raw);
|
||||
stream.next(m.ddq_raw);
|
||||
stream.next(m.temperature);
|
||||
stream.next(m.reserve);
|
||||
}
|
||||
|
||||
ROS_DECLARE_ALLINONE_SERIALIZER
|
||||
}; // struct MotorState_
|
||||
|
||||
} // namespace serialization
|
||||
} // namespace ros
|
||||
|
||||
namespace ros
|
||||
{
|
||||
namespace message_operations
|
||||
{
|
||||
|
||||
template<class ContainerAllocator>
|
||||
struct Printer< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
|
||||
{
|
||||
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::MotorState_<ContainerAllocator>& v)
|
||||
{
|
||||
s << indent << "mode: ";
|
||||
Printer<uint8_t>::stream(s, indent + " ", v.mode);
|
||||
s << indent << "q: ";
|
||||
Printer<float>::stream(s, indent + " ", v.q);
|
||||
s << indent << "dq: ";
|
||||
Printer<float>::stream(s, indent + " ", v.dq);
|
||||
s << indent << "ddq: ";
|
||||
Printer<float>::stream(s, indent + " ", v.ddq);
|
||||
s << indent << "tauEst: ";
|
||||
Printer<float>::stream(s, indent + " ", v.tauEst);
|
||||
s << indent << "q_raw: ";
|
||||
Printer<float>::stream(s, indent + " ", v.q_raw);
|
||||
s << indent << "dq_raw: ";
|
||||
Printer<float>::stream(s, indent + " ", v.dq_raw);
|
||||
s << indent << "ddq_raw: ";
|
||||
Printer<float>::stream(s, indent + " ", v.ddq_raw);
|
||||
s << indent << "temperature: ";
|
||||
Printer<int8_t>::stream(s, indent + " ", v.temperature);
|
||||
s << indent << "reserve[]" << std::endl;
|
||||
for (size_t i = 0; i < v.reserve.size(); ++i)
|
||||
{
|
||||
s << indent << " reserve[" << i << "]: ";
|
||||
Printer<uint32_t>::stream(s, indent + " ", v.reserve[i]);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace message_operations
|
||||
} // namespace ros
|
||||
|
||||
#endif // UNITREE_LEGGED_MSGS_MESSAGE_MOTORSTATE_H
|
||||
// Generated by gencpp from file unitree_legged_msgs/MotorState.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef UNITREE_LEGGED_MSGS_MESSAGE_MOTORSTATE_H
|
||||
#define UNITREE_LEGGED_MSGS_MESSAGE_MOTORSTATE_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 MotorState_
|
||||
{
|
||||
typedef MotorState_<ContainerAllocator> Type;
|
||||
|
||||
MotorState_()
|
||||
: mode(0)
|
||||
, q(0.0)
|
||||
, dq(0.0)
|
||||
, ddq(0.0)
|
||||
, tauEst(0.0)
|
||||
, q_raw(0.0)
|
||||
, dq_raw(0.0)
|
||||
, ddq_raw(0.0)
|
||||
, temperature(0)
|
||||
, reserve() {
|
||||
reserve.assign(0);
|
||||
}
|
||||
MotorState_(const ContainerAllocator& _alloc)
|
||||
: mode(0)
|
||||
, q(0.0)
|
||||
, dq(0.0)
|
||||
, ddq(0.0)
|
||||
, tauEst(0.0)
|
||||
, q_raw(0.0)
|
||||
, dq_raw(0.0)
|
||||
, ddq_raw(0.0)
|
||||
, temperature(0)
|
||||
, reserve() {
|
||||
(void)_alloc;
|
||||
reserve.assign(0);
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef uint8_t _mode_type;
|
||||
_mode_type mode;
|
||||
|
||||
typedef float _q_type;
|
||||
_q_type q;
|
||||
|
||||
typedef float _dq_type;
|
||||
_dq_type dq;
|
||||
|
||||
typedef float _ddq_type;
|
||||
_ddq_type ddq;
|
||||
|
||||
typedef float _tauEst_type;
|
||||
_tauEst_type tauEst;
|
||||
|
||||
typedef float _q_raw_type;
|
||||
_q_raw_type q_raw;
|
||||
|
||||
typedef float _dq_raw_type;
|
||||
_dq_raw_type dq_raw;
|
||||
|
||||
typedef float _ddq_raw_type;
|
||||
_ddq_raw_type ddq_raw;
|
||||
|
||||
typedef int8_t _temperature_type;
|
||||
_temperature_type temperature;
|
||||
|
||||
typedef boost::array<uint32_t, 2> _reserve_type;
|
||||
_reserve_type reserve;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState_<ContainerAllocator> > Ptr;
|
||||
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct MotorState_
|
||||
|
||||
typedef ::unitree_legged_msgs::MotorState_<std::allocator<void> > MotorState;
|
||||
|
||||
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState > MotorStatePtr;
|
||||
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState const> MotorStateConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator>
|
||||
std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::MotorState_<ContainerAllocator> & v)
|
||||
{
|
||||
ros::message_operations::Printer< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >::stream(s, "", v);
|
||||
return s;
|
||||
}
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::unitree_legged_msgs::MotorState_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::MotorState_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.mode == rhs.mode &&
|
||||
lhs.q == rhs.q &&
|
||||
lhs.dq == rhs.dq &&
|
||||
lhs.ddq == rhs.ddq &&
|
||||
lhs.tauEst == rhs.tauEst &&
|
||||
lhs.q_raw == rhs.q_raw &&
|
||||
lhs.dq_raw == rhs.dq_raw &&
|
||||
lhs.ddq_raw == rhs.ddq_raw &&
|
||||
lhs.temperature == rhs.temperature &&
|
||||
lhs.reserve == rhs.reserve;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::unitree_legged_msgs::MotorState_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::MotorState_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace unitree_legged_msgs
|
||||
|
||||
namespace ros
|
||||
{
|
||||
namespace message_traits
|
||||
{
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct IsFixedSize< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
|
||||
: TrueType
|
||||
{ };
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct IsFixedSize< ::unitree_legged_msgs::MotorState_<ContainerAllocator> const>
|
||||
: TrueType
|
||||
{ };
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct IsMessage< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
|
||||
: TrueType
|
||||
{ };
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct IsMessage< ::unitree_legged_msgs::MotorState_<ContainerAllocator> const>
|
||||
: TrueType
|
||||
{ };
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct HasHeader< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
|
||||
: FalseType
|
||||
{ };
|
||||
|
||||
template <class ContainerAllocator>
|
||||
struct HasHeader< ::unitree_legged_msgs::MotorState_<ContainerAllocator> const>
|
||||
: FalseType
|
||||
{ };
|
||||
|
||||
|
||||
template<class ContainerAllocator>
|
||||
struct MD5Sum< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
|
||||
{
|
||||
static const char* value()
|
||||
{
|
||||
return "94c55ee3b7852be2bd437b20ce12a254";
|
||||
}
|
||||
|
||||
static const char* value(const ::unitree_legged_msgs::MotorState_<ContainerAllocator>&) { return value(); }
|
||||
static const uint64_t static_value1 = 0x94c55ee3b7852be2ULL;
|
||||
static const uint64_t static_value2 = 0xbd437b20ce12a254ULL;
|
||||
};
|
||||
|
||||
template<class ContainerAllocator>
|
||||
struct DataType< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
|
||||
{
|
||||
static const char* value()
|
||||
{
|
||||
return "unitree_legged_msgs/MotorState";
|
||||
}
|
||||
|
||||
static const char* value(const ::unitree_legged_msgs::MotorState_<ContainerAllocator>&) { return value(); }
|
||||
};
|
||||
|
||||
template<class ContainerAllocator>
|
||||
struct Definition< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
|
||||
{
|
||||
static const char* value()
|
||||
{
|
||||
return "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"
|
||||
;
|
||||
}
|
||||
|
||||
static const char* value(const ::unitree_legged_msgs::MotorState_<ContainerAllocator>&) { return value(); }
|
||||
};
|
||||
|
||||
} // namespace message_traits
|
||||
} // namespace ros
|
||||
|
||||
namespace ros
|
||||
{
|
||||
namespace serialization
|
||||
{
|
||||
|
||||
template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
|
||||
{
|
||||
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
|
||||
{
|
||||
stream.next(m.mode);
|
||||
stream.next(m.q);
|
||||
stream.next(m.dq);
|
||||
stream.next(m.ddq);
|
||||
stream.next(m.tauEst);
|
||||
stream.next(m.q_raw);
|
||||
stream.next(m.dq_raw);
|
||||
stream.next(m.ddq_raw);
|
||||
stream.next(m.temperature);
|
||||
stream.next(m.reserve);
|
||||
}
|
||||
|
||||
ROS_DECLARE_ALLINONE_SERIALIZER
|
||||
}; // struct MotorState_
|
||||
|
||||
} // namespace serialization
|
||||
} // namespace ros
|
||||
|
||||
namespace ros
|
||||
{
|
||||
namespace message_operations
|
||||
{
|
||||
|
||||
template<class ContainerAllocator>
|
||||
struct Printer< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
|
||||
{
|
||||
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::MotorState_<ContainerAllocator>& v)
|
||||
{
|
||||
s << indent << "mode: ";
|
||||
Printer<uint8_t>::stream(s, indent + " ", v.mode);
|
||||
s << indent << "q: ";
|
||||
Printer<float>::stream(s, indent + " ", v.q);
|
||||
s << indent << "dq: ";
|
||||
Printer<float>::stream(s, indent + " ", v.dq);
|
||||
s << indent << "ddq: ";
|
||||
Printer<float>::stream(s, indent + " ", v.ddq);
|
||||
s << indent << "tauEst: ";
|
||||
Printer<float>::stream(s, indent + " ", v.tauEst);
|
||||
s << indent << "q_raw: ";
|
||||
Printer<float>::stream(s, indent + " ", v.q_raw);
|
||||
s << indent << "dq_raw: ";
|
||||
Printer<float>::stream(s, indent + " ", v.dq_raw);
|
||||
s << indent << "ddq_raw: ";
|
||||
Printer<float>::stream(s, indent + " ", v.ddq_raw);
|
||||
s << indent << "temperature: ";
|
||||
Printer<int8_t>::stream(s, indent + " ", v.temperature);
|
||||
s << indent << "reserve[]" << std::endl;
|
||||
for (size_t i = 0; i < v.reserve.size(); ++i)
|
||||
{
|
||||
s << indent << " reserve[" << i << "]: ";
|
||||
Printer<uint32_t>::stream(s, indent + " ", v.reserve[i]);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace message_operations
|
||||
} // namespace ros
|
||||
|
||||
#endif // UNITREE_LEGGED_MSGS_MESSAGE_MOTORSTATE_H
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -1,77 +1,77 @@
|
|||
/*
|
||||
File $Id: QuadProg++.hh 232 2007-06-21 12:29:00Z digasper $
|
||||
|
||||
The quadprog_solve() function implements the algorithm of Goldfarb and Idnani
|
||||
for the solution of a (convex) Quadratic Programming problem
|
||||
by means of an active-set dual method.
|
||||
|
||||
The problem is in the form:
|
||||
|
||||
min 0.5 * x G x + g0 x
|
||||
s.t.
|
||||
CE^T x + ce0 = 0
|
||||
CI^T x + ci0 >= 0
|
||||
|
||||
The matrix and vectors dimensions are as follows:
|
||||
G: n * n
|
||||
g0: n
|
||||
|
||||
CE: n * p
|
||||
ce0: p
|
||||
|
||||
CI: n * m
|
||||
ci0: m
|
||||
|
||||
x: n
|
||||
|
||||
The function will return the cost of the solution written in the x vector or
|
||||
std::numeric_limits::infinity() if the problem is infeasible. In the latter case
|
||||
the value of the x vector is not correct.
|
||||
|
||||
References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving
|
||||
strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33.
|
||||
|
||||
Notes:
|
||||
1. pay attention in setting up the vectors ce0 and ci0.
|
||||
If the constraints of your problem are specified in the form
|
||||
A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d.
|
||||
2. The matrices have column dimension equal to MATRIX_DIM,
|
||||
a constant set to 20 in this file (by means of a #define macro).
|
||||
If the matrices are bigger than 20 x 20 the limit could be
|
||||
increased by means of a -DMATRIX_DIM=n on the compiler command line.
|
||||
3. The matrix G is modified within the function since it is used to compute
|
||||
the G = L^T L cholesky factorization for further computations inside the function.
|
||||
If you need the original matrix G you should make a copy of it and pass the copy
|
||||
to the function.
|
||||
|
||||
Author: Luca Di Gaspero
|
||||
DIEGM - University of Udine, Italy
|
||||
luca.digaspero@uniud.it
|
||||
http://www.diegm.uniud.it/digaspero/
|
||||
|
||||
The author will be grateful if the researchers using this software will
|
||||
acknowledge the contribution of this function in their research papers.
|
||||
|
||||
Copyright (c) 2007-2016 Luca Di Gaspero
|
||||
|
||||
This software may be modified and distributed under the terms
|
||||
of the MIT license. See the LICENSE file for details.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef _QUADPROGPP
|
||||
#define _QUADPROGPP
|
||||
|
||||
#include "thirdparty/quadProgpp/Array.hh"
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
namespace quadprogpp {
|
||||
|
||||
double solve_quadprog(Matrix<double>& G, Vector<double>& g0,
|
||||
const Matrix<double>& CE, const Vector<double>& ce0,
|
||||
const Matrix<double>& CI, const Vector<double>& ci0,
|
||||
Vector<double>& x);
|
||||
|
||||
} // namespace quadprogpp
|
||||
|
||||
#endif // #define _QUADPROGPP
|
||||
/*
|
||||
File $Id: QuadProg++.hh 232 2007-06-21 12:29:00Z digasper $
|
||||
|
||||
The quadprog_solve() function implements the algorithm of Goldfarb and Idnani
|
||||
for the solution of a (convex) Quadratic Programming problem
|
||||
by means of an active-set dual method.
|
||||
|
||||
The problem is in the form:
|
||||
|
||||
min 0.5 * x G x + g0 x
|
||||
s.t.
|
||||
CE^T x + ce0 = 0
|
||||
CI^T x + ci0 >= 0
|
||||
|
||||
The matrix and vectors dimensions are as follows:
|
||||
G: n * n
|
||||
g0: n
|
||||
|
||||
CE: n * p
|
||||
ce0: p
|
||||
|
||||
CI: n * m
|
||||
ci0: m
|
||||
|
||||
x: n
|
||||
|
||||
The function will return the cost of the solution written in the x vector or
|
||||
std::numeric_limits::infinity() if the problem is infeasible. In the latter case
|
||||
the value of the x vector is not correct.
|
||||
|
||||
References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving
|
||||
strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33.
|
||||
|
||||
Notes:
|
||||
1. pay attention in setting up the vectors ce0 and ci0.
|
||||
If the constraints of your problem are specified in the form
|
||||
A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d.
|
||||
2. The matrices have column dimension equal to MATRIX_DIM,
|
||||
a constant set to 20 in this file (by means of a #define macro).
|
||||
If the matrices are bigger than 20 x 20 the limit could be
|
||||
increased by means of a -DMATRIX_DIM=n on the compiler command line.
|
||||
3. The matrix G is modified within the function since it is used to compute
|
||||
the G = L^T L cholesky factorization for further computations inside the function.
|
||||
If you need the original matrix G you should make a copy of it and pass the copy
|
||||
to the function.
|
||||
|
||||
Author: Luca Di Gaspero
|
||||
DIEGM - University of Udine, Italy
|
||||
luca.digaspero@uniud.it
|
||||
http://www.diegm.uniud.it/digaspero/
|
||||
|
||||
The author will be grateful if the researchers using this software will
|
||||
acknowledge the contribution of this function in their research papers.
|
||||
|
||||
Copyright (c) 2007-2016 Luca Di Gaspero
|
||||
|
||||
This software may be modified and distributed under the terms
|
||||
of the MIT license. See the LICENSE file for details.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef _QUADPROGPP
|
||||
#define _QUADPROGPP
|
||||
|
||||
#include "thirdparty/quadProgpp/Array.hh"
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
namespace quadprogpp {
|
||||
|
||||
double solve_quadprog(Matrix<double>& G, Vector<double>& g0,
|
||||
const Matrix<double>& CE, const Vector<double>& ce0,
|
||||
const Matrix<double>& CI, const Vector<double>& ci0,
|
||||
Vector<double>& x);
|
||||
|
||||
} // namespace quadprogpp
|
||||
|
||||
#endif // #define _QUADPROGPP
|
||||
|
|
|
|||
|
|
@ -1,305 +1,305 @@
|
|||
/*
|
||||
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
|
||||
|
||||
#ifndef TIXML_STRING_INCLUDED
|
||||
#define TIXML_STRING_INCLUDED
|
||||
|
||||
#include <assert.h>
|
||||
#include <string.h>
|
||||
|
||||
/* The support for explicit isn't that universal, and it isn't really
|
||||
required - it is used to check that the TiXmlString class isn't incorrectly
|
||||
used. Be nice to old compilers and macro it here:
|
||||
*/
|
||||
#if defined(_MSC_VER) && (_MSC_VER >= 1200 )
|
||||
// Microsoft visual studio, version 6 and higher.
|
||||
#define TIXML_EXPLICIT explicit
|
||||
#elif defined(__GNUC__) && (__GNUC__ >= 3 )
|
||||
// GCC version 3 and higher.s
|
||||
#define TIXML_EXPLICIT explicit
|
||||
#else
|
||||
#define TIXML_EXPLICIT
|
||||
#endif
|
||||
|
||||
|
||||
/*
|
||||
TiXmlString is an emulation of a subset of the std::string template.
|
||||
Its purpose is to allow compiling TinyXML on compilers with no or poor STL support.
|
||||
Only the member functions relevant to the TinyXML project have been implemented.
|
||||
The buffer allocation is made by a simplistic power of 2 like mechanism : if we increase
|
||||
a string and there's no more room, we allocate a buffer twice as big as we need.
|
||||
*/
|
||||
class TiXmlString
|
||||
{
|
||||
public :
|
||||
// The size type used
|
||||
typedef size_t size_type;
|
||||
|
||||
// Error value for find primitive
|
||||
static const size_type npos; // = -1;
|
||||
|
||||
|
||||
// TiXmlString empty constructor
|
||||
TiXmlString () : rep_(&nullrep_)
|
||||
{
|
||||
}
|
||||
|
||||
// TiXmlString copy constructor
|
||||
TiXmlString ( const TiXmlString & copy) : rep_(0)
|
||||
{
|
||||
init(copy.length());
|
||||
memcpy(start(), copy.data(), length());
|
||||
}
|
||||
|
||||
// TiXmlString constructor, based on a string
|
||||
TIXML_EXPLICIT TiXmlString ( const char * copy) : rep_(0)
|
||||
{
|
||||
init( static_cast<size_type>( strlen(copy) ));
|
||||
memcpy(start(), copy, length());
|
||||
}
|
||||
|
||||
// TiXmlString constructor, based on a string
|
||||
TIXML_EXPLICIT TiXmlString ( const char * str, size_type len) : rep_(0)
|
||||
{
|
||||
init(len);
|
||||
memcpy(start(), str, len);
|
||||
}
|
||||
|
||||
// TiXmlString destructor
|
||||
~TiXmlString ()
|
||||
{
|
||||
quit();
|
||||
}
|
||||
|
||||
TiXmlString& operator = (const char * copy)
|
||||
{
|
||||
return assign( copy, (size_type)strlen(copy));
|
||||
}
|
||||
|
||||
TiXmlString& operator = (const TiXmlString & copy)
|
||||
{
|
||||
return assign(copy.start(), copy.length());
|
||||
}
|
||||
|
||||
|
||||
// += operator. Maps to append
|
||||
TiXmlString& operator += (const char * suffix)
|
||||
{
|
||||
return append(suffix, static_cast<size_type>( strlen(suffix) ));
|
||||
}
|
||||
|
||||
// += operator. Maps to append
|
||||
TiXmlString& operator += (char single)
|
||||
{
|
||||
return append(&single, 1);
|
||||
}
|
||||
|
||||
// += operator. Maps to append
|
||||
TiXmlString& operator += (const TiXmlString & suffix)
|
||||
{
|
||||
return append(suffix.data(), suffix.length());
|
||||
}
|
||||
|
||||
|
||||
// Convert a TiXmlString into a null-terminated char *
|
||||
const char * c_str () const { return rep_->str; }
|
||||
|
||||
// Convert a TiXmlString into a char * (need not be null terminated).
|
||||
const char * data () const { return rep_->str; }
|
||||
|
||||
// Return the length of a TiXmlString
|
||||
size_type length () const { return rep_->size; }
|
||||
|
||||
// Alias for length()
|
||||
size_type size () const { return rep_->size; }
|
||||
|
||||
// Checks if a TiXmlString is empty
|
||||
bool empty () const { return rep_->size == 0; }
|
||||
|
||||
// Return capacity of string
|
||||
size_type capacity () const { return rep_->capacity; }
|
||||
|
||||
|
||||
// single char extraction
|
||||
const char& at (size_type index) const
|
||||
{
|
||||
assert( index < length() );
|
||||
return rep_->str[ index ];
|
||||
}
|
||||
|
||||
// [] operator
|
||||
char& operator [] (size_type index) const
|
||||
{
|
||||
assert( index < length() );
|
||||
return rep_->str[ index ];
|
||||
}
|
||||
|
||||
// find a char in a string. Return TiXmlString::npos if not found
|
||||
size_type find (char lookup) const
|
||||
{
|
||||
return find(lookup, 0);
|
||||
}
|
||||
|
||||
// find a char in a string from an offset. Return TiXmlString::npos if not found
|
||||
size_type find (char tofind, size_type offset) const
|
||||
{
|
||||
if (offset >= length()) return npos;
|
||||
|
||||
for (const char* p = c_str() + offset; *p != '\0'; ++p)
|
||||
{
|
||||
if (*p == tofind) return static_cast< size_type >( p - c_str() );
|
||||
}
|
||||
return npos;
|
||||
}
|
||||
|
||||
void clear ()
|
||||
{
|
||||
//Lee:
|
||||
//The original was just too strange, though correct:
|
||||
// TiXmlString().swap(*this);
|
||||
//Instead use the quit & re-init:
|
||||
quit();
|
||||
init(0,0);
|
||||
}
|
||||
|
||||
/* Function to reserve a big amount of data when we know we'll need it. Be aware that this
|
||||
function DOES NOT clear the content of the TiXmlString if any exists.
|
||||
*/
|
||||
void reserve (size_type cap);
|
||||
|
||||
TiXmlString& assign (const char* str, size_type len);
|
||||
|
||||
TiXmlString& append (const char* str, size_type len);
|
||||
|
||||
void swap (TiXmlString& other)
|
||||
{
|
||||
Rep* r = rep_;
|
||||
rep_ = other.rep_;
|
||||
other.rep_ = r;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
void init(size_type sz) { init(sz, sz); }
|
||||
void set_size(size_type sz) { rep_->str[ rep_->size = sz ] = '\0'; }
|
||||
char* start() const { return rep_->str; }
|
||||
char* finish() const { return rep_->str + rep_->size; }
|
||||
|
||||
struct Rep
|
||||
{
|
||||
size_type size, capacity;
|
||||
char str[1];
|
||||
};
|
||||
|
||||
void init(size_type sz, size_type cap)
|
||||
{
|
||||
if (cap)
|
||||
{
|
||||
// Lee: the original form:
|
||||
// rep_ = static_cast<Rep*>(operator new(sizeof(Rep) + cap));
|
||||
// doesn't work in some cases of new being overloaded. Switching
|
||||
// to the normal allocation, although use an 'int' for systems
|
||||
// that are overly picky about structure alignment.
|
||||
const size_type bytesNeeded = sizeof(Rep) + cap;
|
||||
const size_type intsNeeded = ( bytesNeeded + sizeof(int) - 1 ) / sizeof( int );
|
||||
rep_ = reinterpret_cast<Rep*>( new int[ intsNeeded ] );
|
||||
|
||||
rep_->str[ rep_->size = sz ] = '\0';
|
||||
rep_->capacity = cap;
|
||||
}
|
||||
else
|
||||
{
|
||||
rep_ = &nullrep_;
|
||||
}
|
||||
}
|
||||
|
||||
void quit()
|
||||
{
|
||||
if (rep_ != &nullrep_)
|
||||
{
|
||||
// The rep_ is really an array of ints. (see the allocator, above).
|
||||
// Cast it back before delete, so the compiler won't incorrectly call destructors.
|
||||
delete [] ( reinterpret_cast<int*>( rep_ ) );
|
||||
}
|
||||
}
|
||||
|
||||
Rep * rep_;
|
||||
static Rep nullrep_;
|
||||
|
||||
} ;
|
||||
|
||||
|
||||
inline bool operator == (const TiXmlString & a, const TiXmlString & b)
|
||||
{
|
||||
return ( a.length() == b.length() ) // optimization on some platforms
|
||||
&& ( strcmp(a.c_str(), b.c_str()) == 0 ); // actual compare
|
||||
}
|
||||
inline bool operator < (const TiXmlString & a, const TiXmlString & b)
|
||||
{
|
||||
return strcmp(a.c_str(), b.c_str()) < 0;
|
||||
}
|
||||
|
||||
inline bool operator != (const TiXmlString & a, const TiXmlString & b) { return !(a == b); }
|
||||
inline bool operator > (const TiXmlString & a, const TiXmlString & b) { return b < a; }
|
||||
inline bool operator <= (const TiXmlString & a, const TiXmlString & b) { return !(b < a); }
|
||||
inline bool operator >= (const TiXmlString & a, const TiXmlString & b) { return !(a < b); }
|
||||
|
||||
inline bool operator == (const TiXmlString & a, const char* b) { return strcmp(a.c_str(), b) == 0; }
|
||||
inline bool operator == (const char* a, const TiXmlString & b) { return b == a; }
|
||||
inline bool operator != (const TiXmlString & a, const char* b) { return !(a == b); }
|
||||
inline bool operator != (const char* a, const TiXmlString & b) { return !(b == a); }
|
||||
|
||||
TiXmlString operator + (const TiXmlString & a, const TiXmlString & b);
|
||||
TiXmlString operator + (const TiXmlString & a, const char* b);
|
||||
TiXmlString operator + (const char* a, const TiXmlString & b);
|
||||
|
||||
|
||||
/*
|
||||
TiXmlOutStream is an emulation of std::ostream. It is based on TiXmlString.
|
||||
Only the operators that we need for TinyXML have been developped.
|
||||
*/
|
||||
class TiXmlOutStream : public TiXmlString
|
||||
{
|
||||
public :
|
||||
|
||||
// TiXmlOutStream << operator.
|
||||
TiXmlOutStream & operator << (const TiXmlString & in)
|
||||
{
|
||||
*this += in;
|
||||
return *this;
|
||||
}
|
||||
|
||||
// TiXmlOutStream << operator.
|
||||
TiXmlOutStream & operator << (const char * in)
|
||||
{
|
||||
*this += in;
|
||||
return *this;
|
||||
}
|
||||
|
||||
} ;
|
||||
|
||||
#endif // TIXML_STRING_INCLUDED
|
||||
#endif // TIXML_USE_STL
|
||||
/*
|
||||
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
|
||||
|
||||
#ifndef TIXML_STRING_INCLUDED
|
||||
#define TIXML_STRING_INCLUDED
|
||||
|
||||
#include <assert.h>
|
||||
#include <string.h>
|
||||
|
||||
/* The support for explicit isn't that universal, and it isn't really
|
||||
required - it is used to check that the TiXmlString class isn't incorrectly
|
||||
used. Be nice to old compilers and macro it here:
|
||||
*/
|
||||
#if defined(_MSC_VER) && (_MSC_VER >= 1200 )
|
||||
// Microsoft visual studio, version 6 and higher.
|
||||
#define TIXML_EXPLICIT explicit
|
||||
#elif defined(__GNUC__) && (__GNUC__ >= 3 )
|
||||
// GCC version 3 and higher.s
|
||||
#define TIXML_EXPLICIT explicit
|
||||
#else
|
||||
#define TIXML_EXPLICIT
|
||||
#endif
|
||||
|
||||
|
||||
/*
|
||||
TiXmlString is an emulation of a subset of the std::string template.
|
||||
Its purpose is to allow compiling TinyXML on compilers with no or poor STL support.
|
||||
Only the member functions relevant to the TinyXML project have been implemented.
|
||||
The buffer allocation is made by a simplistic power of 2 like mechanism : if we increase
|
||||
a string and there's no more room, we allocate a buffer twice as big as we need.
|
||||
*/
|
||||
class TiXmlString
|
||||
{
|
||||
public :
|
||||
// The size type used
|
||||
typedef size_t size_type;
|
||||
|
||||
// Error value for find primitive
|
||||
static const size_type npos; // = -1;
|
||||
|
||||
|
||||
// TiXmlString empty constructor
|
||||
TiXmlString () : rep_(&nullrep_)
|
||||
{
|
||||
}
|
||||
|
||||
// TiXmlString copy constructor
|
||||
TiXmlString ( const TiXmlString & copy) : rep_(0)
|
||||
{
|
||||
init(copy.length());
|
||||
memcpy(start(), copy.data(), length());
|
||||
}
|
||||
|
||||
// TiXmlString constructor, based on a string
|
||||
TIXML_EXPLICIT TiXmlString ( const char * copy) : rep_(0)
|
||||
{
|
||||
init( static_cast<size_type>( strlen(copy) ));
|
||||
memcpy(start(), copy, length());
|
||||
}
|
||||
|
||||
// TiXmlString constructor, based on a string
|
||||
TIXML_EXPLICIT TiXmlString ( const char * str, size_type len) : rep_(0)
|
||||
{
|
||||
init(len);
|
||||
memcpy(start(), str, len);
|
||||
}
|
||||
|
||||
// TiXmlString destructor
|
||||
~TiXmlString ()
|
||||
{
|
||||
quit();
|
||||
}
|
||||
|
||||
TiXmlString& operator = (const char * copy)
|
||||
{
|
||||
return assign( copy, (size_type)strlen(copy));
|
||||
}
|
||||
|
||||
TiXmlString& operator = (const TiXmlString & copy)
|
||||
{
|
||||
return assign(copy.start(), copy.length());
|
||||
}
|
||||
|
||||
|
||||
// += operator. Maps to append
|
||||
TiXmlString& operator += (const char * suffix)
|
||||
{
|
||||
return append(suffix, static_cast<size_type>( strlen(suffix) ));
|
||||
}
|
||||
|
||||
// += operator. Maps to append
|
||||
TiXmlString& operator += (char single)
|
||||
{
|
||||
return append(&single, 1);
|
||||
}
|
||||
|
||||
// += operator. Maps to append
|
||||
TiXmlString& operator += (const TiXmlString & suffix)
|
||||
{
|
||||
return append(suffix.data(), suffix.length());
|
||||
}
|
||||
|
||||
|
||||
// Convert a TiXmlString into a null-terminated char *
|
||||
const char * c_str () const { return rep_->str; }
|
||||
|
||||
// Convert a TiXmlString into a char * (need not be null terminated).
|
||||
const char * data () const { return rep_->str; }
|
||||
|
||||
// Return the length of a TiXmlString
|
||||
size_type length () const { return rep_->size; }
|
||||
|
||||
// Alias for length()
|
||||
size_type size () const { return rep_->size; }
|
||||
|
||||
// Checks if a TiXmlString is empty
|
||||
bool empty () const { return rep_->size == 0; }
|
||||
|
||||
// Return capacity of string
|
||||
size_type capacity () const { return rep_->capacity; }
|
||||
|
||||
|
||||
// single char extraction
|
||||
const char& at (size_type index) const
|
||||
{
|
||||
assert( index < length() );
|
||||
return rep_->str[ index ];
|
||||
}
|
||||
|
||||
// [] operator
|
||||
char& operator [] (size_type index) const
|
||||
{
|
||||
assert( index < length() );
|
||||
return rep_->str[ index ];
|
||||
}
|
||||
|
||||
// find a char in a string. Return TiXmlString::npos if not found
|
||||
size_type find (char lookup) const
|
||||
{
|
||||
return find(lookup, 0);
|
||||
}
|
||||
|
||||
// find a char in a string from an offset. Return TiXmlString::npos if not found
|
||||
size_type find (char tofind, size_type offset) const
|
||||
{
|
||||
if (offset >= length()) return npos;
|
||||
|
||||
for (const char* p = c_str() + offset; *p != '\0'; ++p)
|
||||
{
|
||||
if (*p == tofind) return static_cast< size_type >( p - c_str() );
|
||||
}
|
||||
return npos;
|
||||
}
|
||||
|
||||
void clear ()
|
||||
{
|
||||
//Lee:
|
||||
//The original was just too strange, though correct:
|
||||
// TiXmlString().swap(*this);
|
||||
//Instead use the quit & re-init:
|
||||
quit();
|
||||
init(0,0);
|
||||
}
|
||||
|
||||
/* Function to reserve a big amount of data when we know we'll need it. Be aware that this
|
||||
function DOES NOT clear the content of the TiXmlString if any exists.
|
||||
*/
|
||||
void reserve (size_type cap);
|
||||
|
||||
TiXmlString& assign (const char* str, size_type len);
|
||||
|
||||
TiXmlString& append (const char* str, size_type len);
|
||||
|
||||
void swap (TiXmlString& other)
|
||||
{
|
||||
Rep* r = rep_;
|
||||
rep_ = other.rep_;
|
||||
other.rep_ = r;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
void init(size_type sz) { init(sz, sz); }
|
||||
void set_size(size_type sz) { rep_->str[ rep_->size = sz ] = '\0'; }
|
||||
char* start() const { return rep_->str; }
|
||||
char* finish() const { return rep_->str + rep_->size; }
|
||||
|
||||
struct Rep
|
||||
{
|
||||
size_type size, capacity;
|
||||
char str[1];
|
||||
};
|
||||
|
||||
void init(size_type sz, size_type cap)
|
||||
{
|
||||
if (cap)
|
||||
{
|
||||
// Lee: the original form:
|
||||
// rep_ = static_cast<Rep*>(operator new(sizeof(Rep) + cap));
|
||||
// doesn't work in some cases of new being overloaded. Switching
|
||||
// to the normal allocation, although use an 'int' for systems
|
||||
// that are overly picky about structure alignment.
|
||||
const size_type bytesNeeded = sizeof(Rep) + cap;
|
||||
const size_type intsNeeded = ( bytesNeeded + sizeof(int) - 1 ) / sizeof( int );
|
||||
rep_ = reinterpret_cast<Rep*>( new int[ intsNeeded ] );
|
||||
|
||||
rep_->str[ rep_->size = sz ] = '\0';
|
||||
rep_->capacity = cap;
|
||||
}
|
||||
else
|
||||
{
|
||||
rep_ = &nullrep_;
|
||||
}
|
||||
}
|
||||
|
||||
void quit()
|
||||
{
|
||||
if (rep_ != &nullrep_)
|
||||
{
|
||||
// The rep_ is really an array of ints. (see the allocator, above).
|
||||
// Cast it back before delete, so the compiler won't incorrectly call destructors.
|
||||
delete [] ( reinterpret_cast<int*>( rep_ ) );
|
||||
}
|
||||
}
|
||||
|
||||
Rep * rep_;
|
||||
static Rep nullrep_;
|
||||
|
||||
} ;
|
||||
|
||||
|
||||
inline bool operator == (const TiXmlString & a, const TiXmlString & b)
|
||||
{
|
||||
return ( a.length() == b.length() ) // optimization on some platforms
|
||||
&& ( strcmp(a.c_str(), b.c_str()) == 0 ); // actual compare
|
||||
}
|
||||
inline bool operator < (const TiXmlString & a, const TiXmlString & b)
|
||||
{
|
||||
return strcmp(a.c_str(), b.c_str()) < 0;
|
||||
}
|
||||
|
||||
inline bool operator != (const TiXmlString & a, const TiXmlString & b) { return !(a == b); }
|
||||
inline bool operator > (const TiXmlString & a, const TiXmlString & b) { return b < a; }
|
||||
inline bool operator <= (const TiXmlString & a, const TiXmlString & b) { return !(b < a); }
|
||||
inline bool operator >= (const TiXmlString & a, const TiXmlString & b) { return !(a < b); }
|
||||
|
||||
inline bool operator == (const TiXmlString & a, const char* b) { return strcmp(a.c_str(), b) == 0; }
|
||||
inline bool operator == (const char* a, const TiXmlString & b) { return b == a; }
|
||||
inline bool operator != (const TiXmlString & a, const char* b) { return !(a == b); }
|
||||
inline bool operator != (const char* a, const TiXmlString & b) { return !(b == a); }
|
||||
|
||||
TiXmlString operator + (const TiXmlString & a, const TiXmlString & b);
|
||||
TiXmlString operator + (const TiXmlString & a, const char* b);
|
||||
TiXmlString operator + (const char* a, const TiXmlString & b);
|
||||
|
||||
|
||||
/*
|
||||
TiXmlOutStream is an emulation of std::ostream. It is based on TiXmlString.
|
||||
Only the operators that we need for TinyXML have been developped.
|
||||
*/
|
||||
class TiXmlOutStream : public TiXmlString
|
||||
{
|
||||
public :
|
||||
|
||||
// TiXmlOutStream << operator.
|
||||
TiXmlOutStream & operator << (const TiXmlString & in)
|
||||
{
|
||||
*this += in;
|
||||
return *this;
|
||||
}
|
||||
|
||||
// TiXmlOutStream << operator.
|
||||
TiXmlOutStream & operator << (const char * in)
|
||||
{
|
||||
*this += in;
|
||||
return *this;
|
||||
}
|
||||
|
||||
} ;
|
||||
|
||||
#endif // TIXML_STRING_INCLUDED
|
||||
#endif // TIXML_USE_STL
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -1,32 +1,32 @@
|
|||
#ifndef ENDCIRCLETRAJ_H
|
||||
#define ENDCIRCLETRAJ_H
|
||||
|
||||
#include "trajectory/EndHomoTraj.h"
|
||||
|
||||
class EndCircleTraj: public EndHomoTraj{
|
||||
public:
|
||||
EndCircleTraj(CtrlComponents *ctrlComp);
|
||||
~EndCircleTraj(){}
|
||||
|
||||
void setEndRoundTraj(HomoMat startHomo, Vec3 axisPointFromInit,
|
||||
Vec3 axisDirection, double maxSpeed, double angle,
|
||||
bool keepOrientation = true);
|
||||
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 speed);
|
||||
private:
|
||||
void _centerCircle(Vec3 p1, Vec3 p2, Vec3 p3);
|
||||
bool _getEndTraj(HomoMat &homo, Vec6 &twist);
|
||||
Vec3 _center, _omegaAxis;
|
||||
double _radius, _theta;
|
||||
|
||||
Vec6 _middlePosture;
|
||||
Vec6 _middleQ;
|
||||
HomoMat _initHomoToCenter, _middleHomo, _centerHomo;
|
||||
RotMat _initOri;
|
||||
|
||||
bool _keepOrientation;
|
||||
};
|
||||
|
||||
#ifndef ENDCIRCLETRAJ_H
|
||||
#define ENDCIRCLETRAJ_H
|
||||
|
||||
#include "trajectory/EndHomoTraj.h"
|
||||
|
||||
class EndCircleTraj: public EndHomoTraj{
|
||||
public:
|
||||
EndCircleTraj(CtrlComponents *ctrlComp);
|
||||
~EndCircleTraj(){}
|
||||
|
||||
void setEndRoundTraj(HomoMat startHomo, Vec3 axisPointFromInit,
|
||||
Vec3 axisDirection, double maxSpeed, double angle,
|
||||
bool keepOrientation = true);
|
||||
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 speed);
|
||||
private:
|
||||
void _centerCircle(Vec3 p1, Vec3 p2, Vec3 p3);
|
||||
bool _getEndTraj(HomoMat &homo, Vec6 &twist);
|
||||
Vec3 _center, _omegaAxis;
|
||||
double _radius, _theta;
|
||||
|
||||
Vec6 _middlePosture;
|
||||
Vec6 _middleQ;
|
||||
HomoMat _initHomoToCenter, _middleHomo, _centerHomo;
|
||||
RotMat _initOri;
|
||||
|
||||
bool _keepOrientation;
|
||||
};
|
||||
|
||||
#endif // ENDCIRCLETRAJ_H
|
||||
|
|
@ -1,33 +1,33 @@
|
|||
#ifndef JOINTSPACETRAJ_H
|
||||
#define JOINTSPACETRAJ_H
|
||||
|
||||
#include <vector>
|
||||
#include <trajectory/Trajectory.h>
|
||||
#include <string>
|
||||
#include "control/CtrlComponents.h"
|
||||
#include "trajectory/SCurve.h"
|
||||
|
||||
|
||||
class JointSpaceTraj : public Trajectory{
|
||||
public:
|
||||
JointSpaceTraj(CtrlComponents *ctrlComp);
|
||||
~JointSpaceTraj(){}
|
||||
|
||||
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);
|
||||
bool setJointTraj(std::string startName, std::string endName, double speed);
|
||||
private:
|
||||
void _generateA345(double pathTime);
|
||||
|
||||
SCurve _jointCurve;
|
||||
const double ddQMax = 15.0;
|
||||
const double dddQMax = 30.0;
|
||||
|
||||
double _a3, _a4, _a5, _s, _sDot;
|
||||
};
|
||||
|
||||
#ifndef JOINTSPACETRAJ_H
|
||||
#define JOINTSPACETRAJ_H
|
||||
|
||||
#include <vector>
|
||||
#include <trajectory/Trajectory.h>
|
||||
#include <string>
|
||||
#include "control/CtrlComponents.h"
|
||||
#include "trajectory/SCurve.h"
|
||||
|
||||
|
||||
class JointSpaceTraj : public Trajectory{
|
||||
public:
|
||||
JointSpaceTraj(CtrlComponents *ctrlComp);
|
||||
~JointSpaceTraj(){}
|
||||
|
||||
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);
|
||||
bool setJointTraj(std::string startName, std::string endName, double speed);
|
||||
private:
|
||||
void _generateA345(double pathTime);
|
||||
|
||||
SCurve _jointCurve;
|
||||
const double ddQMax = 15.0;
|
||||
const double dddQMax = 30.0;
|
||||
|
||||
double _a3, _a4, _a5, _s, _sDot;
|
||||
};
|
||||
|
||||
#endif // JOINTSPACETRAJ_H
|
||||
|
|
@ -1,49 +1,49 @@
|
|||
#ifndef SCURVE_H
|
||||
#define SCURVE_H
|
||||
|
||||
/* 归一化后的s曲线 */
|
||||
|
||||
#include <iostream>
|
||||
|
||||
class SCurve{
|
||||
public:
|
||||
SCurve(){}
|
||||
~SCurve(){}
|
||||
void setSCurve(double deltaQ, double dQMax, double ddQMax, double dddQMax);
|
||||
void restart();
|
||||
/*
|
||||
考虑到归一化，在实际使用时，物理量为：
|
||||
加速度 = deltaQ * getDDs();
|
||||
*/
|
||||
double getDDs();
|
||||
double getDDs(double t);
|
||||
/*
|
||||
考虑到归一化，在实际使用时，物理量为：
|
||||
速度 = deltaQ * getDs();
|
||||
*/
|
||||
double getDs();
|
||||
double getDs(double t);
|
||||
/*
|
||||
考虑到归一化，在实际使用时，物理量为：
|
||||
位置 = deltaQ * gets();
|
||||
*/
|
||||
double gets();
|
||||
double gets(double t);
|
||||
|
||||
double getT();
|
||||
private:
|
||||
int _getSegment(double t);
|
||||
void _setFunc();
|
||||
double _runTime();
|
||||
|
||||
bool _started = false;
|
||||
double _startTime;
|
||||
|
||||
double _J, _aMax, _vMax;
|
||||
double _T[7]; // period
|
||||
double _t[7]; // moment
|
||||
double _v0, _v1; // ds at _t[0], _t[1]
|
||||
double _s0, _s1, _s2, _s3, _s4, _s5, _s6; // s at _t[0], _t[1]
|
||||
};
|
||||
|
||||
#ifndef SCURVE_H
|
||||
#define SCURVE_H
|
||||
|
||||
/* 归一化后的s曲线 */
|
||||
|
||||
#include <iostream>
|
||||
|
||||
class SCurve{
|
||||
public:
|
||||
SCurve(){}
|
||||
~SCurve(){}
|
||||
void setSCurve(double deltaQ, double dQMax, double ddQMax, double dddQMax);
|
||||
void restart();
|
||||
/*
|
||||
考虑到归一化，在实际使用时，物理量为：
|
||||
加速度 = deltaQ * getDDs();
|
||||
*/
|
||||
double getDDs();
|
||||
double getDDs(double t);
|
||||
/*
|
||||
考虑到归一化，在实际使用时，物理量为：
|
||||
速度 = deltaQ * getDs();
|
||||
*/
|
||||
double getDs();
|
||||
double getDs(double t);
|
||||
/*
|
||||
考虑到归一化，在实际使用时，物理量为：
|
||||
位置 = deltaQ * gets();
|
||||
*/
|
||||
double gets();
|
||||
double gets(double t);
|
||||
|
||||
double getT();
|
||||
private:
|
||||
int _getSegment(double t);
|
||||
void _setFunc();
|
||||
double _runTime();
|
||||
|
||||
bool _started = false;
|
||||
double _startTime;
|
||||
|
||||
double _J, _aMax, _vMax;
|
||||
double _T[7]; // period
|
||||
double _t[7]; // moment
|
||||
double _v0, _v1; // ds at _t[0], _t[1]
|
||||
double _s0, _s1, _s2, _s3, _s4, _s5, _s6; // s at _t[0], _t[1]
|
||||
};
|
||||
|
||||
#endif // SCURVE_H
|
||||
|
|
@ -1,17 +1,17 @@
|
|||
#ifndef STOPFORTIME_H
|
||||
#define STOPFORTIME_H
|
||||
|
||||
#include "trajectory/Trajectory.h"
|
||||
|
||||
class StopForTime : public Trajectory{
|
||||
public:
|
||||
StopForTime(CtrlComponents *ctrlComp);
|
||||
~StopForTime(){}
|
||||
|
||||
bool getJointCmd(Vec6 &q, Vec6 &qd);
|
||||
bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd);
|
||||
void setStop(Vec6 stopQ, double stopTime);
|
||||
private:
|
||||
};
|
||||
|
||||
#ifndef STOPFORTIME_H
|
||||
#define STOPFORTIME_H
|
||||
|
||||
#include "trajectory/Trajectory.h"
|
||||
|
||||
class StopForTime : public Trajectory{
|
||||
public:
|
||||
StopForTime(CtrlComponents *ctrlComp);
|
||||
~StopForTime(){}
|
||||
|
||||
bool getJointCmd(Vec6 &q, Vec6 &qd);
|
||||
bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd);
|
||||
void setStop(Vec6 stopQ, double stopTime);
|
||||
private:
|
||||
};
|
||||
|
||||
#endif // STOPFORTIME_H
|
||||
|
|
@ -1,38 +1,38 @@
|
|||
#ifndef TRAJECTORY_MANAGER_H
|
||||
#define TRAJECTORY_MANAGER_H
|
||||
|
||||
#include <vector>
|
||||
#include "control/CtrlComponents.h"
|
||||
#include "trajectory/JointSpaceTraj.h"
|
||||
#include "trajectory/EndLineTraj.h"
|
||||
#include "trajectory/EndCircleTraj.h"
|
||||
#include "trajectory/StopForTime.h"
|
||||
|
||||
class TrajectoryManager{
|
||||
public:
|
||||
TrajectoryManager(CtrlComponents *ctrlComp);
|
||||
~TrajectoryManager(){}
|
||||
bool getJointCmd(Vec6 &q, Vec6 &qd);
|
||||
bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd);
|
||||
void addTrajectory(Trajectory* traj);
|
||||
void setLoop(double backSpeed = 0.7);
|
||||
void restartTraj();
|
||||
void emptyTraj();
|
||||
Vec6 getStartQ();
|
||||
Vec6 getEndQ();
|
||||
Vec6 getEndPosture();
|
||||
double getStartGripperQ();
|
||||
double getEndGripperQ();
|
||||
HomoMat getEndHomo();
|
||||
size_t size() {return _trajVec.size();} ;
|
||||
private:
|
||||
CtrlComponents *_ctrlComp;
|
||||
JointSpaceTraj *_trajBack;
|
||||
std::vector<Trajectory*> _trajVec;
|
||||
int _trajID = 0;
|
||||
double _jointErr = 0.05;
|
||||
bool _trajCorrect = true;
|
||||
bool _loop = false;
|
||||
};
|
||||
|
||||
#ifndef TRAJECTORY_MANAGER_H
|
||||
#define TRAJECTORY_MANAGER_H
|
||||
|
||||
#include <vector>
|
||||
#include "control/CtrlComponents.h"
|
||||
#include "trajectory/JointSpaceTraj.h"
|
||||
#include "trajectory/EndLineTraj.h"
|
||||
#include "trajectory/EndCircleTraj.h"
|
||||
#include "trajectory/StopForTime.h"
|
||||
|
||||
class TrajectoryManager{
|
||||
public:
|
||||
TrajectoryManager(CtrlComponents *ctrlComp);
|
||||
~TrajectoryManager(){}
|
||||
bool getJointCmd(Vec6 &q, Vec6 &qd);
|
||||
bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd);
|
||||
void addTrajectory(Trajectory* traj);
|
||||
void setLoop(double backSpeed = 0.7);
|
||||
void restartTraj();
|
||||
void emptyTraj();
|
||||
Vec6 getStartQ();
|
||||
Vec6 getEndQ();
|
||||
Vec6 getEndPosture();
|
||||
double getStartGripperQ();
|
||||
double getEndGripperQ();
|
||||
HomoMat getEndHomo();
|
||||
size_t size() {return _trajVec.size();} ;
|
||||
private:
|
||||
CtrlComponents *_ctrlComp;
|
||||
JointSpaceTraj *_trajBack;
|
||||
std::vector<Trajectory*> _trajVec;
|
||||
int _trajID = 0;
|
||||
double _jointErr = 0.05;
|
||||
bool _trajCorrect = true;
|
||||
bool _loop = false;
|
||||
};
|
||||
|
||||
#endif // TRAJECTORY_MANAGER_H
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Loading…
Reference in New Issue