add namespace
This commit is contained in:
parent
bb796755ba
commit
3af65a299e
|
@ -1,5 +1,7 @@
|
||||||
#include "control/unitreeArm.h"
|
#include "control/unitreeArm.h"
|
||||||
|
|
||||||
|
using namespace UNITREE_ARM;
|
||||||
|
|
||||||
class Z1ARM : public unitreeArm{
|
class Z1ARM : public unitreeArm{
|
||||||
public:
|
public:
|
||||||
Z1ARM():unitreeArm(true){};
|
Z1ARM():unitreeArm(true){};
|
||||||
|
|
|
@ -1,5 +1,7 @@
|
||||||
#include "control/unitreeArm.h"
|
#include "control/unitreeArm.h"
|
||||||
|
|
||||||
|
using namespace UNITREE_ARM;
|
||||||
|
|
||||||
int main() {
|
int main() {
|
||||||
Vec6 posture[2];
|
Vec6 posture[2];
|
||||||
unitreeArm arm(true);
|
unitreeArm arm(true);
|
||||||
|
|
|
@ -1,5 +1,7 @@
|
||||||
#include "control/unitreeArm.h"
|
#include "control/unitreeArm.h"
|
||||||
|
|
||||||
|
using namespace UNITREE_ARM;
|
||||||
|
|
||||||
class JointTraj{
|
class JointTraj{
|
||||||
public:
|
public:
|
||||||
JointTraj(){};
|
JointTraj(){};
|
||||||
|
|
|
@ -1,5 +1,7 @@
|
||||||
#include "control/unitreeArm.h"
|
#include "control/unitreeArm.h"
|
||||||
|
|
||||||
|
using namespace UNITREE_ARM;
|
||||||
|
|
||||||
class Z1ARM : public unitreeArm{
|
class Z1ARM : public unitreeArm{
|
||||||
public:
|
public:
|
||||||
Z1ARM():unitreeArm(true){
|
Z1ARM():unitreeArm(true){
|
||||||
|
|
|
@ -8,6 +8,7 @@
|
||||||
#include "utilities/loop.h"
|
#include "utilities/loop.h"
|
||||||
#include "model/ArmModel.h"
|
#include "model/ArmModel.h"
|
||||||
|
|
||||||
|
namespace UNITREE_ARM {
|
||||||
struct CtrlComponents{
|
struct CtrlComponents{
|
||||||
public:
|
public:
|
||||||
CtrlComponents(double deltaT, bool hasUnitreeGripper);
|
CtrlComponents(double deltaT, bool hasUnitreeGripper);
|
||||||
|
@ -48,4 +49,5 @@ private:
|
||||||
UDPPort *_udp;
|
UDPPort *_udp;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
}
|
||||||
#endif
|
#endif
|
|
@ -3,6 +3,8 @@
|
||||||
|
|
||||||
#include "control/ctrlComponents.h"
|
#include "control/ctrlComponents.h"
|
||||||
|
|
||||||
|
namespace UNITREE_ARM {
|
||||||
|
|
||||||
class unitreeArm{
|
class unitreeArm{
|
||||||
public:
|
public:
|
||||||
unitreeArm(bool hasUnitreeGripper);
|
unitreeArm(bool hasUnitreeGripper);
|
||||||
|
@ -162,4 +164,5 @@ LowlevelState *lowstate;//same as _ctrlComp->lowstate
|
||||||
CtrlComponents *_ctrlComp;
|
CtrlComponents *_ctrlComp;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -5,6 +5,8 @@
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
#include "math/mathTypes.h"
|
#include "math/mathTypes.h"
|
||||||
|
|
||||||
|
namespace UNITREE_ARM {
|
||||||
|
|
||||||
template<typename T>
|
template<typename T>
|
||||||
T max(T value){
|
T max(T value){
|
||||||
return value;
|
return value;
|
||||||
|
@ -241,4 +243,5 @@ private:
|
||||||
double _zoomFactor;
|
double _zoomFactor;
|
||||||
std::string _valueName;
|
std::string _valueName;
|
||||||
};
|
};
|
||||||
|
}
|
||||||
#endif
|
#endif
|
|
@ -5,6 +5,7 @@
|
||||||
#include <vector>
|
#include <vector>
|
||||||
#include <Eigen/Dense>
|
#include <Eigen/Dense>
|
||||||
|
|
||||||
|
namespace UNITREE_ARM {
|
||||||
namespace typeTrans{
|
namespace typeTrans{
|
||||||
|
|
||||||
inline void addValue(std::vector<double> &vec, double value){
|
inline void addValue(std::vector<double> &vec, double value){
|
||||||
|
@ -71,5 +72,5 @@ inline void extractVector(std::vector<double> &vec, T &t, Args&... rest){
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
}
|
||||||
#endif // TYPETRANS_H
|
#endif // TYPETRANS_H
|
|
@ -5,6 +5,7 @@
|
||||||
#include <vector>
|
#include <vector>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
|
|
||||||
|
namespace UNITREE_ARM {
|
||||||
struct LowlevelCmd{
|
struct LowlevelCmd{
|
||||||
private:
|
private:
|
||||||
size_t _dof = 6;
|
size_t _dof = 6;
|
||||||
|
@ -76,5 +77,5 @@ Vec6 getQ();
|
||||||
Vec6 getQd();
|
Vec6 getQd();
|
||||||
};
|
};
|
||||||
|
|
||||||
|
}
|
||||||
#endif //LOWLEVELCMD_H
|
#endif //LOWLEVELCMD_H
|
||||||
|
|
|
@ -6,6 +6,8 @@
|
||||||
#include "math/mathTypes.h"
|
#include "math/mathTypes.h"
|
||||||
#include "message/arm_common.h"
|
#include "message/arm_common.h"
|
||||||
|
|
||||||
|
namespace UNITREE_ARM {
|
||||||
|
|
||||||
Vec6 PosturetoVec6(const Posture p);
|
Vec6 PosturetoVec6(const Posture p);
|
||||||
Posture Vec6toPosture(const Vec6 p);
|
Posture Vec6toPosture(const Vec6 p);
|
||||||
|
|
||||||
|
@ -46,4 +48,5 @@ public:
|
||||||
double getGripperTau() {return tau.at(tau.size()-1);}
|
double getGripperTau() {return tau.at(tau.size()-1);}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
}
|
||||||
#endif //LOWLEVELSTATE_HPP
|
#endif //LOWLEVELSTATE_HPP
|
||||||
|
|
|
@ -5,6 +5,7 @@
|
||||||
|
|
||||||
#pragma pack(1)
|
#pragma pack(1)
|
||||||
|
|
||||||
|
namespace UNITREE_ARM {
|
||||||
// 4 Byte
|
// 4 Byte
|
||||||
enum class ArmFSMState{
|
enum class ArmFSMState{
|
||||||
INVALID,
|
INVALID,
|
||||||
|
@ -112,5 +113,5 @@ constexpr int JointCmd_LENGTH = (sizeof(JointCmd));
|
||||||
constexpr int JointState_LENGTH = (sizeof(JointState));
|
constexpr int JointState_LENGTH = (sizeof(JointState));
|
||||||
|
|
||||||
#pragma pack()
|
#pragma pack()
|
||||||
|
}
|
||||||
#endif // _UNITREE_ARM_ARM_MSG_H_
|
#endif // _UNITREE_ARM_ARM_MSG_H_
|
|
@ -8,6 +8,7 @@
|
||||||
#include <vector>
|
#include <vector>
|
||||||
#include "utilities/timer.h"
|
#include "utilities/timer.h"
|
||||||
|
|
||||||
|
namespace UNITREE_ARM {
|
||||||
enum class BlockYN{
|
enum class BlockYN{
|
||||||
YES,
|
YES,
|
||||||
NO
|
NO
|
||||||
|
@ -53,7 +54,7 @@ private:
|
||||||
size_t _sentLength;
|
size_t _sentLength;
|
||||||
fd_set _rSet;
|
fd_set _rSet;
|
||||||
};
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
#endif
|
#endif
|
|
@ -5,8 +5,9 @@
|
||||||
#include "thirdparty/robotics.h"
|
#include "thirdparty/robotics.h"
|
||||||
#include "thirdparty/quadProgpp/QuadProg++.hh"
|
#include "thirdparty/quadProgpp/QuadProg++.hh"
|
||||||
|
|
||||||
using namespace robo;
|
|
||||||
|
|
||||||
|
namespace UNITREE_ARM {
|
||||||
|
using namespace robo;
|
||||||
class ArmModel{
|
class ArmModel{
|
||||||
public:
|
public:
|
||||||
ArmModel(Vec3 endPosLocal, double endEffectorMass, Vec3 endEffectorCom, Mat3 endEffectorInertia);
|
ArmModel(Vec3 endPosLocal, double endEffectorMass, Vec3 endEffectorCom, Mat3 endEffectorInertia);
|
||||||
|
@ -146,5 +147,5 @@ private:
|
||||||
void setParam_V3_6();
|
void setParam_V3_6();
|
||||||
double _theta2Bias;
|
double _theta2Bias;
|
||||||
};
|
};
|
||||||
|
}
|
||||||
#endif
|
#endif
|
|
@ -12,6 +12,7 @@
|
||||||
#include <boost/bind.hpp>
|
#include <boost/bind.hpp>
|
||||||
#include "utilities/timer.h"
|
#include "utilities/timer.h"
|
||||||
|
|
||||||
|
namespace UNITREE_ARM {
|
||||||
typedef boost::function<void ()> Callback;
|
typedef boost::function<void ()> Callback;
|
||||||
|
|
||||||
class Loop {
|
class Loop {
|
||||||
|
@ -54,5 +55,5 @@ public:
|
||||||
private:
|
private:
|
||||||
boost::function<void ()> _fp;
|
boost::function<void ()> _fp;
|
||||||
};
|
};
|
||||||
|
}
|
||||||
#endif // _UNITREE_ARM_LOOP_H_
|
#endif // _UNITREE_ARM_LOOP_H_
|
|
@ -7,21 +7,19 @@
|
||||||
#include <sys/time.h>
|
#include <sys/time.h>
|
||||||
#include <sys/timerfd.h>
|
#include <sys/timerfd.h>
|
||||||
|
|
||||||
//时间戳 微秒级, 需要#include <sys/time.h>
|
namespace UNITREE_ARM {
|
||||||
|
|
||||||
inline long long getSystemTime(){
|
inline long long getSystemTime(){
|
||||||
struct timeval t;
|
struct timeval t;
|
||||||
gettimeofday(&t, NULL);
|
gettimeofday(&t, NULL);
|
||||||
return 1000000 * t.tv_sec + t.tv_usec;
|
return 1000000 * t.tv_sec + t.tv_usec;
|
||||||
}
|
}
|
||||||
//时间戳 秒级, 需要getSystemTime()
|
|
||||||
inline double getTimeSecond(){
|
inline double getTimeSecond(){
|
||||||
double time = getSystemTime() * 0.000001;
|
double time = getSystemTime() * 0.000001;
|
||||||
return time;
|
return time;
|
||||||
}
|
}
|
||||||
/*
|
|
||||||
等待函数,微秒级,从startTime开始等待waitTime微秒
|
|
||||||
目前不推荐使用,建议使用AbsoluteTimer类
|
|
||||||
*/
|
|
||||||
inline void absoluteWait(long long startTime, long long waitTime){
|
inline void absoluteWait(long long startTime, long long waitTime){
|
||||||
if(getSystemTime() - startTime > waitTime){
|
if(getSystemTime() - startTime > waitTime){
|
||||||
std::cout << "[WARNING] The waitTime=" << waitTime << " of function absoluteWait is not enough!" << std::endl
|
std::cout << "[WARNING] The waitTime=" << waitTime << " of function absoluteWait is not enough!" << std::endl
|
||||||
|
@ -52,5 +50,5 @@ private:
|
||||||
double _nextWaitTime;
|
double _nextWaitTime;
|
||||||
itimerspec _timerSpec;
|
itimerspec _timerSpec;
|
||||||
};
|
};
|
||||||
|
}
|
||||||
#endif
|
#endif
|
Binary file not shown.
Loading…
Reference in New Issue