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