From 6e865f0f56f004ea46e5372afb689a3a9d3d3b0b Mon Sep 17 00:00:00 2001
From: Agnel Wang <59766821+Agnel-Wang@users.noreply.github.com>
Date: Fri, 11 Nov 2022 19:49:41 +0800
Subject: [PATCH] V2.0.0
---
CMakeLists.txt | 30 +-
config.xml | 27 +-
config/savedArmStates.csv | 27 +-
include/FSM/BaseState.h | 2 +-
include/FSM/FSMState.h | 60 +-
include/FSM/FiniteStateMachine.h | 32 +-
include/FSM/State_BackToStart.h | 15 +-
include/FSM/State_Cartesian.h | 14 +-
include/FSM/State_Dance.h | 20 -
include/FSM/State_JointSpace.h | 2 +-
include/FSM/State_MoveC.h | 22 +-
include/FSM/State_MoveJ.h | 5 +-
include/FSM/State_MoveL.h | 21 +-
include/FSM/State_Passive.h | 2 +-
include/FSM/State_SaveState.h | 3 -
include/FSM/State_Teach.h | 1 -
include/FSM/State_TeachRepeat.h | 1 -
include/FSM/State_ToState.h | 1 -
include/FSM/State_Trajectory.h | 10 +-
include/common/enumClass.h | 35 +-
.../{utilities/LPFilter.h => math/Filter.h} | 6 +-
include/common/math/mathTools.h | 246 +--
include/common/math/mathTypes.h | 10 +
.../common/math/quadProgpp}/Array.hh | 0
.../common/math/quadProgpp}/QuadProg++.hh | 2 +-
include/common/math/robotics.h | 250 +--
include/common/utilities/CSVTool.h | 2 +-
include/common/utilities/loop.h | 55 +
include/common/utilities/timer.h | 56 +
.../common/utilities/tinyxml}/tinystr.h | 0
.../common/utilities/tinyxml}/tinyxml.h | 0
include/control/CtrlComponents.h | 46 +-
include/control/armSDK.h | 24 +
include/control/cmdPanel.h | 144 ++
include/control/joystick.h | 107 +
include/control/keyboard.h | 34 +
include/interface/IOInterface.h | 33 +-
include/interface/IOROS.h | 15 +-
include/interface/IOUDP.h | 11 +-
include/message/LowlevelCmd.h | 19 +-
include/message/LowlevelState.h | 37 +-
.../{unitree_legged_msgs => }/MotorCmd.h | 0
.../{unitree_legged_msgs => }/MotorState.h | 0
include/message/UserValue.h | 38 -
include/message/aliengo_common.h | 168 ++
include/message/arm_common.h | 150 ++
include/message/arm_motor_common.h | 214 ++
include/message/b1_common.h | 211 ++
include/message/joystick_common.h | 43 +
include/message/udp.h | 69 +
.../message/unitree_legged_msgs/Cartesian.h | 215 --
include/message/unitree_legged_msgs/HighCmd.h | 403 ----
.../message/unitree_legged_msgs/HighState.h | 497 -----
include/message/unitree_legged_msgs/IMU.h | 247 ---
include/message/unitree_legged_msgs/LED.h | 215 --
include/message/unitree_legged_msgs/LowCmd.h | 348 ---
.../message/unitree_legged_msgs/LowState.h | 448 ----
include/model/ArmDynKineModel.h | 124 --
include/model/ArmModel.h | 75 +
include/model/ArmReal.h | 82 -
include/model/Joint.h | 55 -
include/model/Motor.h | 56 -
include/trajectory/CartesionSpaceTraj.h | 45 -
include/trajectory/EndCircleTraj.h | 16 +-
include/trajectory/EndHomoTraj.h | 14 +-
include/trajectory/EndLineTraj.h | 20 +-
include/trajectory/JointSpaceTraj.h | 27 +-
include/trajectory/SCurve.h | 1 -
include/trajectory/StopForTime.h | 7 -
include/trajectory/Trajectory.h | 113 +-
include/trajectory/TrajectoryManager.h | 6 +-
lib/libUnitree_motor_SDK_Linux64.so | Bin 23896 -> 0 bytes
lib/libUnitree_motor_SDK_Linux64_EPOLL.so | Bin 13176 -> 0 bytes
lib/libZ1_ROS_Linux64.so | Bin 1733872 -> 1606208 bytes
lib/libZ1_UDP_Linux64.so | Bin 1633200 -> 1515136 bytes
main.cpp | 185 +-
package.xml | 3 -
thirdparty/quadProgpp/src/Array.cc | 33 -
thirdparty/quadProgpp/src/QuadProg++.cc | 790 -------
thirdparty/tinyxml/src/tinystr.cpp | 111 -
thirdparty/tinyxml/src/tinyxml.cpp | 1886 -----------------
thirdparty/tinyxml/src/tinyxmlerror.cpp | 52 -
thirdparty/tinyxml/src/tinyxmlparser.cpp | 1638 --------------
83 files changed, 1695 insertions(+), 8337 deletions(-)
delete mode 100644 include/FSM/State_Dance.h
rename include/common/{utilities/LPFilter.h => math/Filter.h} (88%)
mode change 100755 => 100644
rename {thirdparty/quadProgpp/include => include/common/math/quadProgpp}/Array.hh (100%)
mode change 100755 => 100644
rename {thirdparty/quadProgpp/include => include/common/math/quadProgpp}/QuadProg++.hh (98%)
mode change 100755 => 100644
create mode 100644 include/common/utilities/loop.h
create mode 100644 include/common/utilities/timer.h
rename {thirdparty/tinyxml/include => include/common/utilities/tinyxml}/tinystr.h (100%)
rename {thirdparty/tinyxml/include => include/common/utilities/tinyxml}/tinyxml.h (100%)
create mode 100644 include/control/armSDK.h
create mode 100644 include/control/cmdPanel.h
create mode 100644 include/control/joystick.h
create mode 100644 include/control/keyboard.h
rename include/message/{unitree_legged_msgs => }/MotorCmd.h (100%)
rename include/message/{unitree_legged_msgs => }/MotorState.h (100%)
delete mode 100644 include/message/UserValue.h
create mode 100644 include/message/aliengo_common.h
create mode 100644 include/message/arm_common.h
create mode 100644 include/message/arm_motor_common.h
create mode 100644 include/message/b1_common.h
create mode 100644 include/message/joystick_common.h
create mode 100644 include/message/udp.h
delete mode 100644 include/message/unitree_legged_msgs/Cartesian.h
delete mode 100644 include/message/unitree_legged_msgs/HighCmd.h
delete mode 100644 include/message/unitree_legged_msgs/HighState.h
delete mode 100644 include/message/unitree_legged_msgs/IMU.h
delete mode 100644 include/message/unitree_legged_msgs/LED.h
delete mode 100644 include/message/unitree_legged_msgs/LowCmd.h
delete mode 100644 include/message/unitree_legged_msgs/LowState.h
delete mode 100755 include/model/ArmDynKineModel.h
create mode 100644 include/model/ArmModel.h
delete mode 100755 include/model/ArmReal.h
delete mode 100755 include/model/Joint.h
delete mode 100755 include/model/Motor.h
delete mode 100755 include/trajectory/CartesionSpaceTraj.h
delete mode 100755 lib/libUnitree_motor_SDK_Linux64.so
delete mode 100755 lib/libUnitree_motor_SDK_Linux64_EPOLL.so
delete mode 100755 thirdparty/quadProgpp/src/Array.cc
delete mode 100755 thirdparty/quadProgpp/src/QuadProg++.cc
delete mode 100644 thirdparty/tinyxml/src/tinystr.cpp
delete mode 100644 thirdparty/tinyxml/src/tinyxml.cpp
delete mode 100644 thirdparty/tinyxml/src/tinyxmlerror.cpp
delete mode 100644 thirdparty/tinyxml/src/tinyxmlparser.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 192c216..01ba915 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -11,36 +11,17 @@ set(COMMUNICATION UDP) # UDP
# ----------------------configurations----------------------
# COMMUNICATION
if(${COMMUNICATION} STREQUAL "UDP")
- add_definitions(-DUDP)
set(SIMULATION OFF)
- set(REAL_ROBOT ON)
elseif(${COMMUNICATION} STREQUAL "ROS")
- add_definitions(-DROS)
+ add_definitions(-DCOMPILE_WITH_SIMULATION)
set(SIMULATION ON)
- set(REAL_ROBOT OFF)
else()
message(FATAL_ERROR "[CMake ERROR] The COMMUNICATION is error")
endif()
-# SIM OR REAL
-if(((SIMULATION) AND (REAL_ROBOT)) OR ((NOT SIMULATION) AND (NOT REAL_ROBOT)))
- message(FATAL_ERROR "[CMake ERROR] The SIMULATION and REAL_ROBOT can only be one ON one OFF")
-endif()
-
-if(REAL_ROBOT)
- add_definitions(-DCOMPILE_WITH_REAL_ROBOT)
-endif()
if(SIMULATION)
- add_definitions(-DCOMPILE_WITH_SIMULATION)
- add_definitions(-DRUN_ROS)
find_package(gazebo REQUIRED)
-endif()
-
-# ARM_MODEL
-add_definitions(-DARM_Z1)
-
-if(SIMULATION)
find_package(catkin REQUIRED COMPONENTS
controller_manager
genmsg
@@ -67,24 +48,19 @@ if(SIMULATION)
)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
-
endif()
set(EIGEN_PATH /usr/include/eigen3)
include_directories(
include
- thirdparty
- ../z1_sdk/include
${Boost_INCLUDE_DIR}
${EIGEN_PATH}
)
link_directories(lib)
+
# ----------------------add executable----------------------
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/../build) # EXECUTABLE:CMAKE_RUNTIME_OUTPUT_DIRECTORY
add_executable(z1_ctrl main.cpp)
-# add_dependencies(z1_ctrl Z1_${COMMUNICATION}_Linux64)
-target_link_libraries(z1_ctrl ${catkin_LIBRARIES} libUnitree_motor_SDK_Linux64_EPOLL.so libZ1_${COMMUNICATION}_Linux64.so)
-
-# set(EXECUTABLE_OUTPUT_PATH /home/$ENV{USER}/)
+target_link_libraries(z1_ctrl libZ1_${COMMUNICATION}_Linux64.so ${catkin_LIBRARIES} )
diff --git a/config.xml b/config.xml
index 6547186..b3b6a9a 100644
--- a/config.xml
+++ b/config.xml
@@ -1,26 +1,17 @@
-
+
keyboard
sdk
joystick
-
-
-
-
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
-
-
-
+
192.168.123.110
+ 8881
+
+
+ N
+ 10.0
+ 0.0
+
diff --git a/config/savedArmStates.csv b/config/savedArmStates.csv
index 0201ef6..c2a67bb 100755
--- a/config/savedArmStates.csv
+++ b/config/savedArmStates.csv
@@ -1,14 +1,13 @@
-forward, -0.000019, 1.542859, -1.037883, -0.531308, 0.002487, -0.012173, 0.999650, -0.002146, -0.026357, 0.389527, 0.002468, 0.999923, 0.012173, 0.000269, 0.026329, -0.012234, 0.999578, 0.402549, 0.000000, 0.000000, 0.000000, 1.000000,
-start, -0.000336, 0.001634, 0.000000, 0.064640, 0.000248, 0.000230, 0.997805, 0.000104, 0.066225, 0.048696, -0.000087, 1.000000, -0.000252, 0.000011, -0.066225, 0.000246, 0.997805, 0.148729, 0.000000, 0.000000, 0.000000, 1.000000,
-startFlat, 0.000000, 0.000000, -0.000012, -0.074351, 0.000000, 0.000006, 0.997236, -0.000000, -0.074294, 0.086594, 0.000000, 1.000000, -0.000006, 0.000000, 0.074294, 0.000006, 0.997236, 0.176788, 0.000000, 0.000000, 0.000000, 1.000000,
-teachstart, -0.858000, 1.951000, -0.825000, 0.000000, -1.154000, 0.000000, 0.653696, 0.756536, -0.018308, 0.260474, -0.756240, 0.653952, 0.021180, -0.301334, 0.027996, 0.000000, 0.999608, 0.183326, 0.000000, 0.000000, 0.000000, 1.000000,
-fulltest00, 2.316194, 0.013239, -0.001959, -0.000047, -0.278982, 0.221496, -0.654115, -0.677458, 0.336427, -0.022776, 0.708636, -0.704403, -0.040645, 0.024673, 0.264516, 0.211818, 0.940832, 0.174303, 0.000000, 0.000000, 0.000000, 1.000000,
-fulltest01, 0.000194, 1.621239, -1.455959, 2.470727, 1.492796, 0.419496, 0.205219, 0.489994, 0.847225, 0.332273, 0.619814, -0.735004, 0.274956, 0.059687, 0.757440, 0.468696, -0.454542, 0.521459, 0.000000, 0.000000, 0.000000, 1.000000,
-fulltest02, -2.336194, 3.117239, -3.099593, -2.478727, -1.516796, 0.419496, 0.415340, -0.220546, 0.882526, -0.404129, -0.454627, 0.789988, 0.411380, -0.505948, -0.787913, -0.572083, 0.227847, 0.075097, 0.000000, 0.000000, 0.000000, 1.000000,
-001, 0.000000, 0.922204, -1.215402, 0.293198, 0.000000, 0.000000, 1.000000, 0.000000, -0.000000, 0.200000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.500000, 0.000000, 0.000000, 0.000000, 1.000000,
-002, -1.035000, 0.922204, -1.017402, -0.285802, 0.000000, 0.000000, 0.473918, 0.859862, -0.189839, 0.103971, -0.798204, 0.510526, 0.319739, -0.175114, 0.371849, 0.000000, 0.928293, 0.541400, 0.000000, 0.000000, 0.000000, 1.000000,
-003, 0.954000, 0.910204, -0.984402, -0.192802, 0.000000, 0.000000, 0.557929, -0.815736, -0.152611, 0.121384, 0.786832, 0.578425, -0.215223, 0.171185, 0.263839, 0.000000, 0.964567, 0.510707, 0.000000, 0.000000, 0.000000, 1.000000,
-mohe1, 0.966000, 1.420800, -1.412412, -1.141151, 0.997200, 1.675206, -0.560087, -0.444472, 0.699105, 0.049195, 0.666938, -0.742508, 0.062250, 0.235148, 0.491423, 0.501125, 0.712304, 0.622679, 0.000000, 0.000000, 0.000000, 1.000000,
-mohe2, -1.057200, 2.424000, -2.682793, 0.986449, -1.342800, -1.709994, -0.765519, -0.400523, 0.503550, 0.166634, -0.625604, 0.646204, -0.437081, -0.515483, -0.150335, -0.649616, -0.745251, 0.381296, 0.000000, 0.000000, 0.000000, 1.000000,
-5kg1, 0.661051, 3.010641, -2.885593, -0.233059, 0.000065, 0.000206, 0.784707, -0.614016, -0.084965, 0.592966, 0.610421, 0.789294, -0.066347, 0.461215, 0.107801, 0.000198, 0.994173, 0.198126, 0.000000, 0.000000, 0.000000, 1.000000,
-5kg2, -0.495523, 3.122645, -2.856166, -0.406859, 0.000065, -0.000378, 0.871097, 0.475482, -0.122910, 0.664711, -0.470757, 0.879726, 0.066863, -0.359274, 0.139919, -0.000383, 0.990163, 0.133116, 0.000000, 0.000000, 0.000000, 1.000000,
+forward, 0.0, 1.5, -1.0, -0.54, 0.0, 0.0,
+startFlat, 0.0, 0.0, -0.005, -0.074, 0.0, 0.0,
+search, 0.0, 0.0, -0.005, -0.5, 0.0, 0.0,
+show_left, -1.0, 0.9, -1., -0.3, 0.0, 0.0,
+show_mid, 0.0, 0.9, -1.2, 0.3, 0.0, 0.0,
+show_right, 1.0, 0.9, -1., -0.3, 0.0, 0.0,
+5kg1, 0.661051, 3.010641, -2.885593, -0.233059, 0.0, 0.0,
+5kg2, -0.495523, 3.122645, -2.856166, -0.406859, 0.0, 0.0,
+mh1, 2.617994, 0.084000, -2.755093, 1.470296, 1.454343, 2.645527,
+mh2, -2.485994, 2.451000, -0.102000, -1.611829, -1.421343, -2.265473,
+01, 0.028072, 0.343213, -0.010000, -0.073124, 0.084670, -0.016248,
+02, 2.024087, 0.760020, -0.816744, -1.190535, 1.175596, 2.064632,
+03, -1.740466, 2.731333, -2.161190, 0.806278, -0.889325, -2.529499,
\ No newline at end of file
diff --git a/include/FSM/BaseState.h b/include/FSM/BaseState.h
index b070c9b..43d9071 100755
--- a/include/FSM/BaseState.h
+++ b/include/FSM/BaseState.h
@@ -23,10 +23,10 @@ public:
}
}
std::string getStateName(){return _stateNameString;}
+ int getStateNameEnum(){return _stateNameEnum;};
protected:
int _stateNameEnum;
std::string _stateNameString;
- Control _ctrl;
};
#endif
\ No newline at end of file
diff --git a/include/FSM/FSMState.h b/include/FSM/FSMState.h
index 920f531..a983cc8 100755
--- a/include/FSM/FSMState.h
+++ b/include/FSM/FSMState.h
@@ -5,15 +5,9 @@
#include
#include
#include "control/CtrlComponents.h"
-#include "message/LowlevelCmd.h"
-#include "message/LowlevelState.h"
-#include "common/enumClass.h"
#include "common/math/mathTools.h"
-#include "common/math/mathTypes.h"
-#include "unitree_arm_sdk/timer.h"
-#include "model/ArmDynKineModel.h"
-
-#include "FSM/FiniteStateMachine.h"
+#include "common/utilities/timer.h"
+#include "FSM/BaseState.h"
class FSMState : public BaseState{
public:
@@ -24,55 +18,23 @@ public:
virtual void run() = 0;
virtual void exit() = 0;
virtual int checkChange(int cmd) {return (int)ArmFSMStateName::INVALID;}
+ bool _collisionTest();
protected:
void _armCtrl();
- void _tauDynForward();
- void _tauFriction();
- void _qdFeedBack();
- bool _collisionTest();
- void _jointPosProtect();
- void _jointSpeedProtect();
- void _stateUpdate();
-
- void _gripperCmd();
- void _gripperCtrl();
- double _gripperPos;
- double _gripperW;
- double _gripperTau;
- double _gripperPosStep;//keyboard
- double _gripperTauStep;
+ void _recordData();
+ Vec6 _postureToVec6(Posture posture);
- double _gripperLinearFriction;
- double _gripperCoulombFriction;
- static double _gripperCoulombDirection;
- double _gripperFriction;
-
- CtrlComponents *_ctrlComp;
- ArmFSMStateName _nextStateName;
-
- IOInterface *_ioInter;
LowlevelCmd *_lowCmd;
LowlevelState *_lowState;
- UserValue _userValue;
- ArmDynKineModel *_armModel;
+ IOInterface *_ioInter;
+ ArmModel *_armModel;
- Vec6 _qPast, _qdPast, _q, _qd, _qdd;
- Vec6 _tau, _endForce;
+ Vec6 _qPast, _qdPast, _q, _qd, _qdd, _tauf, _tauCmd, _g;
+ double _gripperPos, _gripperW, _gripperTau;
+ uint _collisionCnt;
- Vec6 _coulombFriction;
- static Vec6 _coulombDirection;
- Vec6 _linearFriction;
-
- Vec6 _kpDiag, _kdDiag;
- Mat6 _Kp, _Kd;
-
- std::vector _jointQMax;
- std::vector _jointQMin;
- std::vector _jointSpeedMax;
-
- SendCmd _sendCmd;
- RecvState _recvState;
+ CtrlComponents *_ctrlComp;
};
#endif // FSMSTATE_H
diff --git a/include/FSM/FiniteStateMachine.h b/include/FSM/FiniteStateMachine.h
index 71fdbe8..e6d1df7 100755
--- a/include/FSM/FiniteStateMachine.h
+++ b/include/FSM/FiniteStateMachine.h
@@ -2,40 +2,26 @@
#define FSM_H
#include
-#include "FSM/BaseState.h"
-#include "unitree_arm_sdk/cmdPanel.h"
-#include "unitree_arm_sdk/loop.h"
-#include "unitree_arm_sdk/keyboard.h"
+#include "FSM/FSMState.h"
+#include "common/utilities/loop.h"
+#include "control/CtrlComponents.h"
-enum class FSMRunMode{
- NORMAL,
- CHANGE
-};
-
-/*
- 状态机初始状态为states的第一个元素
- 为了支持多个状态机从同一个CmdPanel获取触发信号,
- 加入了cmdChannel,不同状态机之间的cmdChannel必须不同,
- 否则可能会漏过状态触发
-*/
class FiniteStateMachine{
public:
- FiniteStateMachine(std::vector states,
- CmdPanel *cmdPanel, size_t cmdChannel = 0, double dt=0.002);
+ FiniteStateMachine(std::vector states, CtrlComponents *ctrlComp);
virtual ~FiniteStateMachine();
private:
void _run();
- std::vector _states;
+ std::vector _states;
- FSMRunMode _mode;
+ FSMMode _mode;
bool _running;
- BaseState* _currentState;
- BaseState* _nextState;
+ FSMState* _currentState;
+ FSMState* _nextState;
int _nextStateEnum;
- size_t _cmdChannel;
- CmdPanel *_cmdPanel;
+ CtrlComponents *_ctrlComp;
LoopFunc *_runThread;
};
diff --git a/include/FSM/State_BackToStart.h b/include/FSM/State_BackToStart.h
index a77434a..9399e13 100644
--- a/include/FSM/State_BackToStart.h
+++ b/include/FSM/State_BackToStart.h
@@ -1,14 +1,21 @@
#ifndef STATE_BACKTOSTART_H
#define STATE_BACKTOSTART_H
-#include "FSM/State_Trajectory.h"
-class State_BackToStart : public State_Trajectory{
+#include "FSM/FSMState.h"
+#include "trajectory/JointSpaceTraj.h"
+
+class State_BackToStart : public FSMState{
public:
State_BackToStart(CtrlComponents *ctrlComp);
+ ~State_BackToStart();
+ void enter();
+ void run();
+ void exit();
+ int checkChange(int cmd);
private:
- void _setTraj();
- void _setTrajSDK(){}
+ bool _reach, _pastReach;
+ JointSpaceTraj *_jointTraj;
};
#endif // STATE_BACKTOSTART_H
\ No newline at end of file
diff --git a/include/FSM/State_Cartesian.h b/include/FSM/State_Cartesian.h
index 43ac293..bc88ec8 100755
--- a/include/FSM/State_Cartesian.h
+++ b/include/FSM/State_Cartesian.h
@@ -12,17 +12,15 @@ public:
void exit();
int checkChange(int cmd);
private:
- double _posSpeed;
- double _oriSpeed;
- Vec3 _omega;
- Vec3 _velocity;
+ double _oriSpeed = 0.4;// control by keyboard or joystick
+ double _posSpeed = 0.15;
+ double oriSpeedLimit = 0.3;// limits in SDK
+ double posSpeedLimit = 0.3;
+ VecX _changeDirections;
+
Vec6 _twist;
HomoMat _endHomoGoal, _endHomoGoalPast;
Vec6 _endPostureGoal, _endPosturePast, _endPostureDelta;
- HomoMat _endHomoFeedback;
- Vec6 _Pdes;
- Vec6 _Pfd;
- Vec6 _Pkp;
};
#endif // CARTESIAN_H
\ No newline at end of file
diff --git a/include/FSM/State_Dance.h b/include/FSM/State_Dance.h
deleted file mode 100644
index 1ef5039..0000000
--- a/include/FSM/State_Dance.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef STATE_DANCETRAJ_H
-#define STATE_DANCETRAJ_H
-
-#include "FSM/State_Trajectory.h"
-
-class State_Dance : public State_Trajectory{
-public:
- State_Dance(CtrlComponents *ctrlComp,
- TrajectoryManager *traj,
- ArmFSMStateName stateEnum,
- ArmFSMStateName nextState,
- std::string stateString);
- void exit();
- int checkChange(int cmd);
-private:
- bool _freeBottom = false;
- void _setTraj(){}
-};
-
-#endif
\ No newline at end of file
diff --git a/include/FSM/State_JointSpace.h b/include/FSM/State_JointSpace.h
index 75a4b29..4b7bbdd 100755
--- a/include/FSM/State_JointSpace.h
+++ b/include/FSM/State_JointSpace.h
@@ -12,7 +12,7 @@ public:
void exit();
int checkChange(int cmd);
private:
- double _angleStep;
+ std::vector jointSpeedMax;
};
#endif // JOINTSPACE_H
\ No newline at end of file
diff --git a/include/FSM/State_MoveC.h b/include/FSM/State_MoveC.h
index 9b77aab..c27a5f7 100755
--- a/include/FSM/State_MoveC.h
+++ b/include/FSM/State_MoveC.h
@@ -2,7 +2,7 @@
#define MOVEC_H
#include "FSM/FSMState.h"
-#include "trajectory/CartesionSpaceTraj.h"
+#include "trajectory/EndCircleTraj.h"
class State_MoveC : public FSMState{
public:
@@ -13,22 +13,10 @@ public:
void exit();
int checkChange(int cmd);
private:
- void quadprogArea();
- double _posSpeed;
- double _oriSpeed;
- Vec3 _omega;
- Vec3 _velocity;
- Vec6 _twist;
- Vec6 _pastPosture, _endPosture, _middlePostureGoal, _endPostureGoal, _endTwist;
- HomoMat _endHomoFeedback;
- Vec6 _Pdes;
- Vec6 _Pfd;
- Vec6 _Pkp;
- Vec6 _endPostureError;
-
- // 轨迹相关变量
- CartesionSpaceTraj *_cartesionTraj;
- bool _reached, _timeReached, _taskReached, _pastTaskReached;
+ double _speed;
+ std::vector _postures;
+ EndCircleTraj *_circleTraj;
+ bool _timeReached, _taskReached, _pastTaskReached, _finalReached;
};
#endif // CARTESIAN_H
\ No newline at end of file
diff --git a/include/FSM/State_MoveJ.h b/include/FSM/State_MoveJ.h
index def1e20..48cffac 100755
--- a/include/FSM/State_MoveJ.h
+++ b/include/FSM/State_MoveJ.h
@@ -13,11 +13,10 @@ public:
void exit();
int checkChange(int cmd);
private:
- double _costTime;
- HomoMat _goalHomo;
+ double _speed;
JointSpaceTraj *_jointTraj;
bool _reached, _pastReached, _finalReached;
- std::vector _result;
+ std::vector _qCmd;
};
#endif // CARTESIAN_H
\ No newline at end of file
diff --git a/include/FSM/State_MoveL.h b/include/FSM/State_MoveL.h
index 6bde0e4..fc16132 100755
--- a/include/FSM/State_MoveL.h
+++ b/include/FSM/State_MoveL.h
@@ -2,7 +2,7 @@
#define MOVEL_H
#include "FSM/FSMState.h"
-#include "trajectory/CartesionSpaceTraj.h"
+#include "trajectory/EndLineTraj.h"
class State_MoveL : public FSMState{
public:
@@ -13,22 +13,9 @@ public:
void exit();
int checkChange(int cmd);
private:
- void quadprogArea();
- double _posSpeed;
- double _oriSpeed;
- Vec3 _omega;
- Vec3 _velocity;
- Vec6 _twist;
- Vec6 _pastPosture, _endPosture, _endTwist;
- HomoMat _endHomoFeedback;
- Vec6 _Pdes;
- Vec6 _Pfd;
- Vec6 _Pkp;
- Vec6 _endPostureError;
-
- // 轨迹相关变量
- std::vector > _posture;
- CartesionSpaceTraj *_cartesionTraj;
+ double _speed;
+ std::vector _postures;
+ EndLineTraj *_lineTraj;
bool _timeReached, _taskReached, _pastTaskReached, _finalReached;
};
#endif // CARTESIAN_H
\ No newline at end of file
diff --git a/include/FSM/State_Passive.h b/include/FSM/State_Passive.h
index 506e215..b4c4306 100755
--- a/include/FSM/State_Passive.h
+++ b/include/FSM/State_Passive.h
@@ -1,7 +1,7 @@
#ifndef PASSIVE_H
#define PASSIVE_H
-#include "FSMState.h"
+#include "FSM/FSMState.h"
class State_Passive : public FSMState{
public:
diff --git a/include/FSM/State_SaveState.h b/include/FSM/State_SaveState.h
index cdae1a0..5393fc7 100755
--- a/include/FSM/State_SaveState.h
+++ b/include/FSM/State_SaveState.h
@@ -13,9 +13,6 @@ public:
void exit();
int checkChange(int cmd);
private:
- CSVTool *_csv;
- Vec6 _q;
- HomoMat _endHomo;
};
#endif // SAVESTATE_H
\ No newline at end of file
diff --git a/include/FSM/State_Teach.h b/include/FSM/State_Teach.h
index 533a3cd..0bfcbb9 100755
--- a/include/FSM/State_Teach.h
+++ b/include/FSM/State_Teach.h
@@ -2,7 +2,6 @@
#define STATETEACH_H
#include "FSM/FSMState.h"
-#include "common/utilities/CSVTool.h"
class State_Teach : public FSMState{
public:
diff --git a/include/FSM/State_TeachRepeat.h b/include/FSM/State_TeachRepeat.h
index aad6bbf..e79802e 100755
--- a/include/FSM/State_TeachRepeat.h
+++ b/include/FSM/State_TeachRepeat.h
@@ -3,7 +3,6 @@
#include "FSM/FSMState.h"
#include "trajectory/TrajectoryManager.h"
-#include "common/utilities/CSVTool.h"
class State_TeachRepeat : public FSMState{
public:
diff --git a/include/FSM/State_ToState.h b/include/FSM/State_ToState.h
index 7b1a82e..066ceb3 100755
--- a/include/FSM/State_ToState.h
+++ b/include/FSM/State_ToState.h
@@ -19,7 +19,6 @@ private:
JointSpaceTraj *_jointTraj;
bool _reach, _pastReach;
std::string _goalName;
- // CSVTool *_csv;
};
#endif // TOSTATE_H
\ No newline at end of file
diff --git a/include/FSM/State_Trajectory.h b/include/FSM/State_Trajectory.h
index 8bad4c7..ea5c47d 100755
--- a/include/FSM/State_Trajectory.h
+++ b/include/FSM/State_Trajectory.h
@@ -2,7 +2,6 @@
#define CARTESIANPATH_H
#include "FSM/FSMState.h"
-#include "trajectory/EndHomoTraj.h"
#include "trajectory/TrajectoryManager.h"
class State_Trajectory : public FSMState{
@@ -16,12 +15,12 @@ public:
void exit();
int checkChange(int cmd);
protected:
- // EndHomoTraj *_traj;
- virtual void _setTraj();
- virtual void _setTrajSdk();
+ void _setTraj();
+ void _setTrajSDK();
TrajectoryManager *_traj;
HomoMat _goalHomo;
Vec6 _goalTwist;
+ double speedTemp;
JointSpaceTraj *_toStartTraj;
bool _reachedStart = false;
@@ -31,11 +30,8 @@ protected:
std::vector _stopTraj;
std::vector _lineTraj;
- long long startTime;
static std::vector _trajCmd;
Vec6 _posture[2];
-
- static bool _isTrajFSM;
};
#endif // CARTESIANPATH_H
\ No newline at end of file
diff --git a/include/common/enumClass.h b/include/common/enumClass.h
index 9786c46..b6542d0 100755
--- a/include/common/enumClass.h
+++ b/include/common/enumClass.h
@@ -1,13 +1,6 @@
#ifndef ENUMCLASS_H
#define ENUMCLASS_H
-enum class FrameType{
- BODY,
- HIP,
- ENDEFFECTOR,
- GLOBAL
-};
-
enum class FSMMode{
NORMAL,
CHANGE
@@ -28,19 +21,7 @@ enum class ArmFSMStateName{
TEACHREPEAT,
CALIBRATION,
SETTRAJ,
- DANCE00,
- DANCE01,
- DANCE02,
- DANCE03,
- DANCE04,
- DANCE05,
- DANCE06,
- DANCE07,
- DANCE08,
- DANCE09,
BACKTOSTART,
- GRIPPER_OPEN,
- GRIPPER_CLOSE,
NEXT,
LOWCMD
};
@@ -50,20 +31,10 @@ enum class JointMotorType{
DOUBLE_MOTOR
};
-enum class MotorMountType{
- STATOR_FIRST,
- OUTPUT_FIRST
-};
-
-enum class HeadType{
- TIGER,
- DOG
-};
-
enum class Control{
- _KEYBOARD,
- _SDK,
- _JOYSTICK,
+ KEYBOARD,
+ SDK,
+ JOYSTICK
};
#endif // ENUMCLASS_H
\ No newline at end of file
diff --git a/include/common/utilities/LPFilter.h b/include/common/math/Filter.h
old mode 100755
new mode 100644
similarity index 88%
rename from include/common/utilities/LPFilter.h
rename to include/common/math/Filter.h
index f9dc43c..ec421da
--- a/include/common/utilities/LPFilter.h
+++ b/include/common/math/Filter.h
@@ -1,5 +1,5 @@
-#ifndef LOWPASSFILTER
-#define LOWPASSFILTER
+#ifndef FILTER
+#define FILTER
#include
#include
@@ -26,4 +26,4 @@ private:
bool _start;
};
-#endif // LOWPASSFILTER
\ No newline at end of file
+#endif // FILTER
\ No newline at end of file
diff --git a/include/common/math/mathTools.h b/include/common/math/mathTools.h
index 59d35a8..11f812a 100755
--- a/include/common/math/mathTools.h
+++ b/include/common/math/mathTools.h
@@ -2,18 +2,9 @@
#define MATHTOOLS_H
#include
+#include
#include "common/math/mathTypes.h"
-// template
-// inline T1 max(const T1 a, const T2 b){
-// return (a > b ? a : b);
-// }
-
-// template
-// inline T1 min(const T1 a, const T2 b){
-// return (a < b ? a : b);
-// }
-
template
T max(T value){
return value;
@@ -34,21 +25,6 @@ inline T min(const T val0, const Args... restVal){
return val0 > (min(restVal...)) ? val0 : min(restVal...);
}
-inline Mat2 skew(const double& w){
- Mat2 mat; mat.setZero();
- mat(0, 1) = -w;
- mat(1, 0) = w;
- return mat;
-}
-
-inline Mat3 skew(const Vec3& v) {
- Mat3 m;
- m << 0, -v(2), v(1),
- v(2), 0, -v(0),
- -v(1), v(0), 0;
- return m;
-}
-
enum class TurnDirection{
NOMATTER,
POSITIVE,
@@ -150,11 +126,9 @@ inline double saturation(const double a, double limValue1, double limValue2){
if(a < lowLim){
return lowLim;
- }
- else if(a > highLim){
+ }else if(a > highLim){
return highLim;
- }
- else{
+ }else{
return a;
}
}
@@ -180,21 +154,17 @@ inline T1 invNormalize(const T0 value, const T1 min, const T2 max, const double
template
inline T windowFunc(const T x, const T windowRatio, const T xRange=1.0, const T yRange=1.0){
if((x < 0)||(x > xRange)){
- // printf("[ERROR] The x of function windowFunc should between [0, xRange]\n");
std::cout << "[ERROR][windowFunc] The x=" << x << ", which should between [0, xRange]" << std::endl;
}
if((windowRatio <= 0)||(windowRatio >= 0.5)){
- // printf("[ERROR] The windowRatio of function windowFunc should between (0, 0.5)\n");
std::cout << "[ERROR][windowFunc] The windowRatio=" << windowRatio << ", which should between [0, 0.5]" << std::endl;
}
if(x/xRange < windowRatio){
return x * yRange / (xRange * windowRatio);
- }
- else if(x/xRange > 1 - windowRatio){
+ }else if(x/xRange > 1 - windowRatio){
return yRange * (xRange - x)/(xRange * windowRatio);
- }
- else{
+ }else{
return yRange;
}
}
@@ -232,212 +202,6 @@ inline void updateAvgCov(T1 &cov, T2 &exp, T3 newValue, double n){
updateAverage(exp, newValue, n);
}
-/* rotate matrix about x axis */
-inline RotMat rotX(const double &theta) {
- double s = std::sin(theta);
- double c = std::cos(theta);
-
- RotMat R;
- R << 1, 0, 0, 0, c, -s, 0, s, c;
- return R;
-}
-
-/* rotate matrix about y axis */
-inline RotMat rotY(const double &theta) {
- double s = std::sin(theta);
- double c = std::cos(theta);
-
- RotMat R;
- R << c, 0, s, 0, 1, 0, -s, 0, c;
- return R;
-}
-
-/* rotate matrix about z axis */
-inline RotMat rotZ(const double &theta) {
- double s = std::sin(theta);
- double c = std::cos(theta);
-
- RotMat R;
- R << c, -s, 0, s, c, 0, 0, 0, 1;
- return R;
-}
-
-inline RotMat so3ToRotMat(const Vec3& _rot){
- RotMat R;
- double theta = _rot.norm();
- if (fabs(theta) <1e-6)
- R = RotMat::Identity();
- else{
- Vec3 u_axis(_rot/theta);
- double cos_theta = cos(theta);
- R = cos_theta*RotMat::Identity()+sin(theta)*skew(u_axis) +(1-cos_theta)*(u_axis*u_axis.transpose());
- }
- return R;
-}
-
-/* row pitch yaw to rotate matrix */
-inline RotMat rpyToRotMat(const double& row, const double& pitch, const double& yaw) {
- RotMat m = rotZ(yaw) * rotY(pitch) * rotX(row);
- return m;
-}
-
-inline RotMat quatToRotMat(const Quat& q) {
- double e0 = q(0);
- double e1 = q(1);
- double e2 = q(2);
- double e3 = q(3);
-
- RotMat R;
- R << 1 - 2 * (e2 * e2 + e3 * e3), 2 * (e1 * e2 - e0 * e3),
- 2 * (e1 * e3 + e0 * e2), 2 * (e1 * e2 + e0 * e3),
- 1 - 2 * (e1 * e1 + e3 * e3), 2 * (e2 * e3 - e0 * e1),
- 2 * (e1 * e3 - e0 * e2), 2 * (e2 * e3 + e0 * e1),
- 1 - 2 * (e1 * e1 + e2 * e2);
- return R;
-}
-
-inline Vec3 rotMatToRPY(const Mat3& R) {
- Vec3 rpy;
- rpy(0) = atan2(R(2,1),R(2,2));//asin(R(2,1)/cos(rpy(1))); // atan2(R(2,1),R(2,2));
- rpy(1) = asin(-R(2,0));
- rpy(2) = atan2(R(1,0),R(0,0));
- return rpy;
-}
-
-/* rotate matrix to exponential coordinate(omega*theta) */
-inline Vec3 rotMatToExp(const RotMat& rm){
- double cosValue = rm.trace()/2.0-1/2.0;
- if(cosValue > 1.0f){
- cosValue = 1.0f;
- }else if(cosValue < -1.0f){
- cosValue = -1.0f;
- }
-
- double angle = acos(cosValue);
- Vec3 exp;
- if (fabs(angle) < 1e-5){
- exp=Vec3(0,0,0);
- }
- else if (fabs(angle - M_PI) < 1e-5){
- exp = angle * Vec3(rm(0,0)+1, rm(0,1), rm(0,2)) / sqrt(2*(1+rm(0, 0)));
- }
- else{
- exp=angle/(2.0f*sin(angle))*Vec3(rm(2,1)-rm(1,2),rm(0,2)-rm(2,0),rm(1,0)-rm(0,1));
- }
- return exp;
-}
-
-/* add 1 at the end of Vec3 */
-inline Vec4 homoVec(Vec3 v3, double end = 1.0){
- Vec4 v4;
- v4.block(0, 0, 3, 1) = v3;
- v4(3) = end;
- return v4;
-}
-
-/* remove 1 at the end of Vec4 */
-inline Vec3 noHomoVec(Vec4 v4){
- Vec3 v3;
- v3 = v4.block(0, 0, 3, 1);
- return v3;
-}
-
-/* build Homogeneous Matrix by p and m */
-inline HomoMat homoMatrix(Vec3 p, Mat3 m){
- HomoMat homoM;
- homoM.setZero();
- homoM.topLeftCorner(3, 3) = m;
- homoM.topRightCorner(3, 1) = p;
- homoM(3, 3) = 1;
- return homoM;
-}
-
-/* build Homogeneous Matrix by p and w */
-inline HomoMat homoMatrix(Vec3 p, Vec3 w){
- HomoMat homoM;
- homoM.setZero();
- homoM.topLeftCorner(3, 3) = so3ToRotMat(w);
- homoM.topRightCorner(3, 1) = p;
- homoM(3, 3) = 1;
- return homoM;
-}
-
-/* build Homogeneous Matrix by x axis, y axis and p */
-inline HomoMat homoMatrix(Vec3 x, Vec3 y, Vec3 p){
- HomoMat homoM;
- homoM.setZero();
- Vec3 xN = x.normalized();
- Vec3 yN = y.normalized();
- homoM.block(0, 0, 3, 1) = xN;
- homoM.block(0, 1, 3, 1) = yN;
- homoM.block(0, 2, 3, 1) = skew(xN) * yN;
- homoM.topRightCorner(3, 1) = p;
- homoM(3, 3) = 1;
- return homoM;
-}
-
-/* build Homogeneous Matrix of rotate joint */
-inline HomoMat homoMatrixRotate(Vec3 q, Vec3 w){
- HomoMat homoM;
- Mat3 eye3 = Mat3::Identity();
- Mat3 rotateM = so3ToRotMat(w);
- homoM.setZero();
- homoM.topLeftCorner(3, 3) = rotateM;
- homoM.topRightCorner(3, 1) = (eye3 - rotateM) * q;
- // homoM.topRightCorner(3, 1) = q;
- homoM(3, 3) = 1;
-
- return homoM;
-}
-
-/* build Homogeneous Matrix by posture */
-inline HomoMat homoMatrixPosture(Vec6 posture){
- HomoMat homoM;
- homoM.setZero();
- homoM.topLeftCorner(3, 3) = rpyToRotMat(posture(0),posture(1),posture(2));
- homoM.topRightCorner(3, 1) << posture(3),posture(4),posture(5);
- homoM(3, 3) = 1;
- return homoM;
-}
-
-/* inverse matrix of homogeneous matrix */
-inline HomoMat homoMatrixInverse(HomoMat homoM){
- HomoMat homoInv;
- homoInv.setZero();
- homoInv.topLeftCorner(3, 3) = homoM.topLeftCorner(3, 3).transpose();
- homoInv.topRightCorner(3, 1) = -homoM.topLeftCorner(3, 3).transpose() * homoM.topRightCorner(3, 1);
- homoInv(3, 3) = 1;
- return homoInv;
-}
-
-/* get position of Homogeneous Matrix */
-inline Vec3 getHomoPosition(HomoMat mat){
- return mat.block(0, 3, 3, 1);
-}
-
-/* get rotate matrix of Homogeneous Matrix */
-inline RotMat getHomoRotMat(HomoMat mat){
- return mat.block(0, 0, 3, 3);
-}
-
-/* convert homogeneous matrix to posture vector */
-inline Vec6 homoToPosture(HomoMat mat){
- Vec6 posture;
- posture.block(0,0,3,1) = rotMatToRPY(getHomoRotMat(mat));
- posture.block(3,0,3,1) = getHomoPosition(mat);
- return posture;
-}
-
-/* convert posture vector matrix to homogeneous */
-inline HomoMat postureToHomo(Vec6 posture){
- HomoMat homoM;
- homoM.setZero();
- homoM.topLeftCorner(3, 3) = rpyToRotMat(posture(0), posture(1), posture(2));
- homoM.topRightCorner(3, 1) << posture(3), posture(4), posture(5);
- homoM(3, 3) = 1;
- return homoM;
-}
-
// Calculate average value and covariance
class AvgCov{
public:
diff --git a/include/common/math/mathTypes.h b/include/common/math/mathTypes.h
index 2ca09b1..b74c7d7 100755
--- a/include/common/math/mathTypes.h
+++ b/include/common/math/mathTypes.h
@@ -2,6 +2,7 @@
#define MATHTYPES_H
#include
+#include
/************************/
/******** Vector ********/
@@ -96,4 +97,13 @@ inline VecX stdVecToEigenVec(T stdVec){
VecX eigenVec = Eigen::VectorXd::Map(&stdVec[0], stdVec.size());
return eigenVec;
}
+
+inline std::vector EigenVectostdVec(VecX eigenVec){
+ std::vector stdVec;
+ for(int i(0); i
namespace quadprogpp {
diff --git a/include/common/math/robotics.h b/include/common/math/robotics.h
index 98cbd0d..3b30766 100644
--- a/include/common/math/robotics.h
+++ b/include/common/math/robotics.h
@@ -3,8 +3,7 @@
#include
#include "common/math/mathTools.h"
-namespace mr {
-
+namespace robo {
/*
* Function: Find if the value is negligible enough to consider 0
* Inputs: value to be checked as a double
@@ -12,6 +11,39 @@ namespace mr {
*/
bool NearZero(const double);
+Mat6 rot(const Mat3& E);
+
+Mat6 xlt(const Vec3& r);
+
+/* rotate matrix about x axis */
+RotMat rotX(const double &);
+RotMat rx(const double &xrot);
+
+/* rotate matrix about y axis */
+RotMat rotY(const double &);
+RotMat ry(const double &yrot);
+
+/* rotate matrix about z axis */
+RotMat rotZ(const double &);
+RotMat rz(const double &zrot);
+
+/* row pitch yaw to rotate matrix */
+RotMat rpyToRotMat(const double&, const double&, const double&);
+RotMat rpyToRotMat(const Vec3& rpy);
+
+Vec3 rotMatToRPY(const Mat3& );
+
+RotMat quatToRotMat(const Quat&);
+
+/* convert homogeneous matrix to posture vector */
+Vec6 homoToPosture(HomoMat);
+
+/* convert posture vector matrix to homogeneous */
+HomoMat postureToHomo(Vec6);
+
+RotMat getHomoRotMat(HomoMat T);
+Vec3 getHomoPosition(HomoMat T);
+HomoMat homoMatrix(Vec3 x, Vec3 y, Vec3 p);
/*
* Function: Calculate the 6x6 matrix [adV] of the given 6-vector
* Input: Eigen::VectorXd (6x1)
@@ -131,6 +163,21 @@ Eigen::MatrixXd MatrixExp6(const Eigen::MatrixXd&);
Eigen::MatrixXd MatrixLog6(const Eigen::MatrixXd&);
+/*
+ * Functions: Tranforms 3D motion vector form A to B coordinates
+ * Input: T: the cordinate transform form A to B coordiantes for a motion vector
+ * Return : BX_A
+ */
+Mat6 CoordinateTransMotionVector(const HomoMat& T);
+
+/*
+ * Functions: Tranforms 3D force vector form A to B coordinates
+ * Input: T: the cordinate transform form A to B coordiantes for a force vector
+ * Return : {BX_A}*
+ */
+Mat6 CoordinateTransForceVector(const HomoMat& T);
+
+
/*
* Function: Compute end effector frame (used for current spatial position calculation)
* Inputs: Home configuration (position and orientation) of end-effector
@@ -459,72 +506,6 @@ Eigen::VectorXd ForwardDynamics(const Eigen::VectorXd&, const Eigen::VectorXd&,
const std::vector&, const Eigen::MatrixXd&);
-/*
- * Function: Compute the joint angles and velocities at the next timestep using
- first order Euler integration
- * Inputs:
- * thetalist[in]: n-vector of joint variables
- * dthetalist[in]: n-vector of joint rates
- * ddthetalist: n-vector of joint accelerations
- * dt: The timestep delta t
- *
- * Outputs:
- * thetalist[out]: Vector of joint variables after dt from first order Euler integration
- * dthetalist[out]: Vector of joint rates after dt from first order Euler integration
- */
-void EulerStep(Eigen::VectorXd&, Eigen::VectorXd&, const Eigen::VectorXd&, double);
-
-
-/*
- * Function: Compute the joint forces/torques required to move the serial chain along the given
- * trajectory using inverse dynamics
- * Inputs:
- * thetamat: An N x n matrix of robot joint variables (N: no. of trajecoty time step points; n: no. of robot joints
- * dthetamat: An N x n matrix of robot joint velocities
- * ddthetamat: An N x n matrix of robot joint accelerations
- * g: Gravity vector g
- * Ftipmat: An N x 6 matrix of spatial forces applied by the end-effector (if there are no tip forces
- * the user should input a zero matrix)
- * Mlist: List of link frames {i} relative to {i-1} at the home position
- * Glist: Spatial inertia matrices Gi of the links
- * Slist: Screw axes Si of the joints in a space frame, in the format
- * of a matrix with the screw axes as the columns.
- *
- * Outputs:
- * taumat: The N x n matrix of joint forces/torques for the specified trajectory, where each of the N rows is the vector
- * of joint forces/torques at each time step
- */
-Eigen::MatrixXd InverseDynamicsTrajectory(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&,
- const Eigen::VectorXd&, const Eigen::MatrixXd&, const std::vector&, const std::vector&,
- const Eigen::MatrixXd&);
-
-
-/*
- * Function: Compute the motion of a serial chain given an open-loop history of joint forces/torques
- * Inputs:
- * thetalist: n-vector of initial joint variables
- * dthetalist: n-vector of initial joint rates
- * taumat: An N x n matrix of joint forces/torques, where each row is is the joint effort at any time step
- * g: Gravity vector g
- * Ftipmat: An N x 6 matrix of spatial forces applied by the end-effector (if there are no tip forces
- * the user should input a zero matrix)
- * Mlist: List of link frames {i} relative to {i-1} at the home position
- * Glist: Spatial inertia matrices Gi of the links
- * Slist: Screw axes Si of the joints in a space frame, in the format
- * of a matrix with the screw axes as the columns.
- * dt: The timestep between consecutive joint forces/torques
- * intRes: Integration resolution is the number of times integration (Euler) takes places between each time step.
- * Must be an integer value greater than or equal to 1
- *
- * Outputs: std::vector of [thetamat, dthetamat]
- * thetamat: The N x n matrix of joint angles resulting from the specified joint forces/torques
- * dthetamat: The N x n matrix of joint velocities
- */
-std::vector ForwardDynamicsTrajectory(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::MatrixXd&,
- const Eigen::VectorXd&, const Eigen::MatrixXd&, const std::vector&, const std::vector&,
- const Eigen::MatrixXd&, double, int);
-
-
/*
* Function: Compute the joint control torques at a particular time instant
* Inputs:
@@ -551,135 +532,4 @@ Eigen::VectorXd ComputedTorque(const Eigen::VectorXd&, const Eigen::VectorXd&, c
const Eigen::VectorXd&, const std::vector&, const std::vector&,
const Eigen::MatrixXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, double, double, double);
-
-/*
- * Function: Compute s(t) for a cubic time scaling
- * Inputs:
- * Tf: Total time of the motion in seconds from rest to rest
- * t: The current time t satisfying 0 < t < Tf
- *
- * Outputs:
- * st: The path parameter corresponding to a third-order
- * polynomial motion that begins and ends at zero velocity
- */
-double CubicTimeScaling(double, double);
-
-
-/*
- * Function: Compute s(t) for a quintic time scaling
- * Inputs:
- * Tf: Total time of the motion in seconds from rest to rest
- * t: The current time t satisfying 0 < t < Tf
- *
- * Outputs:
- * st: The path parameter corresponding to a fifth-order
- * polynomial motion that begins and ends at zero velocity
- * and zero acceleration
- */
-double QuinticTimeScaling(double, double);
-
-
-/*
- * Function: Compute a straight-line trajectory in joint space
- * Inputs:
- * thetastart: The initial joint variables
- * thetaend: The final joint variables
- * Tf: Total time of the motion in seconds from rest to rest
- * N: The number of points N > 1 (Start and stop) in the discrete
- * representation of the trajectory
- * method: The time-scaling method, where 3 indicates cubic (third-
- * order polynomial) time scaling and 5 indicates quintic
- * (fifth-order polynomial) time scaling
- *
- * Outputs:
- * traj: A trajectory as an N x n matrix, where each row is an n-vector
- * of joint variables at an instant in time. The first row is
- * thetastart and the Nth row is thetaend . The elapsed time
- * between each row is Tf / (N - 1)
- */
-Eigen::MatrixXd JointTrajectory(const Eigen::VectorXd&, const Eigen::VectorXd&, double, int, int);
-
-
-/*
- * Function: Compute a trajectory as a list of N SE(3) matrices corresponding to
- * the screw motion about a space screw axis
- * Inputs:
- * Xstart: The initial end-effector configuration
- * Xend: The final end-effector configuration
- * Tf: Total time of the motion in seconds from rest to rest
- * N: The number of points N > 1 (Start and stop) in the discrete
- * representation of the trajectory
- * method: The time-scaling method, where 3 indicates cubic (third-
- * order polynomial) time scaling and 5 indicates quintic
- * (fifth-order polynomial) time scaling
- *
- * Outputs:
- * traj: The discretized trajectory as a list of N matrices in SE(3)
- * separated in time by Tf/(N-1). The first in the list is Xstart
- * and the Nth is Xend
- */
-std::vector ScrewTrajectory(const Eigen::MatrixXd&, const Eigen::MatrixXd&, double, int, int);
-
-
-/*
- * Function: Compute a trajectory as a list of N SE(3) matrices corresponding to
- * the origin of the end-effector frame following a straight line
- * Inputs:
- * Xstart: The initial end-effector configuration
- * Xend: The final end-effector configuration
- * Tf: Total time of the motion in seconds from rest to rest
- * N: The number of points N > 1 (Start and stop) in the discrete
- * representation of the trajectory
- * method: The time-scaling method, where 3 indicates cubic (third-
- * order polynomial) time scaling and 5 indicates quintic
- * (fifth-order polynomial) time scaling
- *
- * Outputs:
- * traj: The discretized trajectory as a list of N matrices in SE(3)
- * separated in time by Tf/(N-1). The first in the list is Xstart
- * and the Nth is Xend
- * Notes:
- * This function is similar to ScrewTrajectory, except the origin of the
- * end-effector frame follows a straight line, decoupled from the rotational
- * motion.
- */
-std::vector CartesianTrajectory(const Eigen::MatrixXd&, const Eigen::MatrixXd&, double, int, int);
-
-
-/*
- * Function: Compute the motion of a serial chain given an open-loop history of joint forces/torques
- * Inputs:
- * thetalist: n-vector of initial joint variables
- * dthetalist: n-vector of initial joint rates
- * g: Gravity vector g
- * Ftipmat: An N x 6 matrix of spatial forces applied by the end-effector (if there are no tip forces
- * the user should input a zero matrix)
- * Mlist: List of link frames {i} relative to {i-1} at the home position
- * Glist: Spatial inertia matrices Gi of the links
- * Slist: Screw axes Si of the joints in a space frame, in the format
- * of a matrix with the screw axes as the columns.
- * thetamatd: An Nxn matrix of desired joint variables from the reference trajectory
- * dthetamatd: An Nxn matrix of desired joint velocities
- * ddthetamatd: An Nxn matrix of desired joint accelerations
- * gtilde: The gravity vector based on the model of the actual robot (actual values given above)
- * Mtildelist: The link frame locations based on the model of the actual robot (actual values given above)
- * Gtildelist: The link spatial inertias based on the model of the actual robot (actual values given above)
- * Kp: The feedback proportional gain (identical for each joint)
- * Ki: The feedback integral gain (identical for each joint)
- * Kd: The feedback derivative gain (identical for each joint)
- * dt: The timestep between points on the reference trajectory
- * intRes: Integration resolution is the number of times integration (Euler) takes places between each time step.
- * Must be an integer value greater than or equal to 1
- *
- * Outputs: std::vector of [taumat, thetamat]
- * taumat: An Nxn matrix of the controllers commanded joint forces/ torques, where each row of n forces/torques
- * corresponds to a single time instant
- * thetamat: The N x n matrix of actual joint angles
- */
-std::vector SimulateControl(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&,
- const Eigen::MatrixXd&, const std::vector&, const std::vector&,
- const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&,
- const Eigen::VectorXd&, const std::vector&, const std::vector&,
- double, double, double, double, int);
-
-}
+}
\ No newline at end of file
diff --git a/include/common/utilities/CSVTool.h b/include/common/utilities/CSVTool.h
index bc27544..456a182 100755
--- a/include/common/utilities/CSVTool.h
+++ b/include/common/utilities/CSVTool.h
@@ -181,7 +181,7 @@ inline void CSVTool::readFile(){
inline bool CSVTool::getLine(std::string label, std::vector &values){
if(_labels.count(label) == 0){
- // std::cout << "[ERROR] No such label: " << label << std::endl;
+ std::cout << "[ERROR] No such label: " << label << std::endl;
return false;
}else{
_lines.at(_labels[label])->getValues(values);
diff --git a/include/common/utilities/loop.h b/include/common/utilities/loop.h
new file mode 100644
index 0000000..8369c7a
--- /dev/null
+++ b/include/common/utilities/loop.h
@@ -0,0 +1,55 @@
+#ifndef _UNITREE_ARM_LOOP_H_
+#define _UNITREE_ARM_LOOP_H_
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include "common/utilities/timer.h"
+
+
+// constexpr int THREAD_PRIORITY = 99; // real-time priority
+
+typedef boost::function Callback;
+
+class Loop {
+public:
+ Loop(std::string name, float period, int bindCPU = -1);
+ ~Loop();
+ void start();
+ void shutdown();
+ virtual void functionCB() = 0;
+
+private:
+ void entryFunc();
+
+ std::string _name;
+ float _period;
+ int _bindCPU;
+ bool _bind_cpu_flag = false;
+ bool _isrunning = false;
+ std::thread _thread;
+
+ size_t _runTimes = 0;
+ size_t _timeOutTimes = 0;
+
+ AbsoluteTimer *_timer;
+};
+
+class LoopFunc : public Loop {
+public:
+ LoopFunc(std::string name, float period, const Callback& _cb)
+ : Loop(name, period), _fp(_cb){}
+ LoopFunc(std::string name, float period, int bindCPU, const Callback& _cb)
+ : Loop(name, period, bindCPU), _fp(_cb){}
+ void functionCB() { (_fp)(); }
+private:
+ boost::function _fp;
+};
+
+#endif // _UNITREE_ARM_LOOP_H_
\ No newline at end of file
diff --git a/include/common/utilities/timer.h b/include/common/utilities/timer.h
new file mode 100644
index 0000000..40701da
--- /dev/null
+++ b/include/common/utilities/timer.h
@@ -0,0 +1,56 @@
+#ifndef _UNITREE_ARM_TIMER_H_
+#define _UNITREE_ARM_TIMER_H_
+
+#include
+#include
+#include
+#include
+#include
+
+//时间戳 微秒级, 需要#include
+inline long long getSystemTime(){
+ struct timeval t;
+ gettimeofday(&t, NULL);
+ return 1000000 * t.tv_sec + t.tv_usec;
+}
+//时间戳 秒级, 需要getSystemTime()
+inline double getTimeSecond(){
+ double time = getSystemTime() * 0.000001;
+ return time;
+}
+/*
+等待函数,微秒级,从startTime开始等待waitTime微秒
+目前不推荐使用,建议使用AbsoluteTimer类
+*/
+inline void absoluteWait(long long startTime, long long waitTime){
+ if(getSystemTime() - startTime > waitTime){
+ std::cout << "[WARNING] The waitTime=" << waitTime << " of function absoluteWait is not enough!" << std::endl
+ << "The program has already cost " << getSystemTime() - startTime << "us." << std::endl;
+ }
+ while(getSystemTime() - startTime < waitTime){
+ usleep(50);
+ }
+}
+
+
+/*
+waitTimeS = 0 means do not care time out
+*/
+class AbsoluteTimer{
+public:
+ AbsoluteTimer(double waitTimeS);
+ ~AbsoluteTimer();
+ void start();
+ bool wait();
+private:
+ void _updateWaitTime(double waitTimeS);
+ int _timerFd;
+ uint64_t _missed;
+ double _waitTime;
+ double _startTime;
+ double _leftTime;
+ double _nextWaitTime;
+ itimerspec _timerSpec;
+};
+
+#endif
\ No newline at end of file
diff --git a/thirdparty/tinyxml/include/tinystr.h b/include/common/utilities/tinyxml/tinystr.h
similarity index 100%
rename from thirdparty/tinyxml/include/tinystr.h
rename to include/common/utilities/tinyxml/tinystr.h
diff --git a/thirdparty/tinyxml/include/tinyxml.h b/include/common/utilities/tinyxml/tinyxml.h
similarity index 100%
rename from thirdparty/tinyxml/include/tinyxml.h
rename to include/common/utilities/tinyxml/tinyxml.h
diff --git a/include/control/CtrlComponents.h b/include/control/CtrlComponents.h
index 68c2c0b..f3a4ab4 100755
--- a/include/control/CtrlComponents.h
+++ b/include/control/CtrlComponents.h
@@ -1,47 +1,49 @@
#ifndef CTRLCOMPONENTS_H
#define CTRLCOMPONENTS_H
-#include "message/LowlevelCmd.h"
-#include "message/LowlevelState.h"
-#include "interface/IOInterface.h"
-#include "interface/IOROS.h"
-#include "model/ArmDynKineModel.h"
-#include "common/utilities/CSVTool.h"
#include
#include
-#include "common/math/robotics.h"
+#include "common/utilities/loop.h"
+#include "message/arm_common.h"
+#include "message/LowlevelCmd.h"
+#include "message/LowlevelState.h"
+#include "common/utilities/CSVTool.h"
+#include "model/ArmModel.h"
+#include "interface/IOUDP.h"
+#include "interface/IOROS.h"
+#include "control/armSDK.h"
+
+using namespace std;
struct CtrlComponents{
public:
CtrlComponents();
~CtrlComponents();
- int dof;
std::string armConfigPath;
- LowlevelCmd *lowCmd;
- LowlevelState *lowState;
+ CmdPanel *cmdPanel;
IOInterface *ioInter;
- ArmDynKineModel *armModel;
+ ArmModel *armModel;
CSVTool *stateCSV;
+
+ SendCmd sendCmd; // cmd that receive from SDK
+ RecvState recvState;// state that send to SDK
//config
- Control ctrl;
- bool _hasGripper;
- std::string ctrl_IP;
-
double dt;
bool *running;
+ Control ctrl;
+ bool hasGripper;
+ bool isCollisionOpen;
+ double collisionTLimit;
+ bool isPlot;
- void sendRecv();
void geneObj();
-
- bool isRecord;
void writeData();
private:
- Vec3 _endPosLocal;
- double _endEffectorMass;
- Vec3 _endEffectorCom;
- Mat3 _endEffectorInertia;
+ std::string ctrl_IP;
+ uint ctrl_port;
+ double _loadWeight;
};
#endif // CTRLCOMPONENTS_H
\ No newline at end of file
diff --git a/include/control/armSDK.h b/include/control/armSDK.h
new file mode 100644
index 0000000..b60c215
--- /dev/null
+++ b/include/control/armSDK.h
@@ -0,0 +1,24 @@
+#ifndef _SDK_H
+#define _SDK_H
+
+#include "message/udp.h"
+#include "control/cmdPanel.h"
+
+class ARMSDK : public CmdPanel{
+public:
+ ARMSDK(std::vector events,
+ EmptyAction emptyAction, const char* IP, uint port, double dt = 0.002);
+ ~ARMSDK();
+ SendCmd getSendCmd();
+ int getState(size_t channelID = 0);
+ void setRecvState(RecvState& recvState);
+private:
+ void _sendRecv();
+ void _read(){};
+ UDPPort *_udp;
+ SendCmd _sendCmd, _sendCmdTemp;
+ RecvState _recvState;
+ size_t _recvLength;
+};
+
+#endif
\ No newline at end of file
diff --git a/include/control/cmdPanel.h b/include/control/cmdPanel.h
new file mode 100644
index 0000000..66b9e96
--- /dev/null
+++ b/include/control/cmdPanel.h
@@ -0,0 +1,144 @@
+#ifndef _CMDPANEL_H_
+#define _CMDPANEL_H_
+
+#include
+#include
+#include "common/utilities/loop.h"
+#include "message/arm_common.h"
+
+
+enum class KeyPress{
+ RELEASE,
+ PRESS,
+ REPEAT
+};
+
+
+enum class ActionType{
+ EMPTY,
+ STATE,
+ VALUE
+};
+
+
+struct KeyCmd{
+ std::string c;
+ KeyPress keyPress;
+};
+
+
+class KeyAction{
+public:
+ KeyAction(ActionType type);
+ virtual ~KeyAction(){};
+ ActionType getType(){return _type;}
+protected:
+ ActionType _type;
+};
+
+
+class StateAction : public KeyAction{
+public:
+ StateAction(std::string c, int state, KeyPress press = KeyPress::PRESS);
+ virtual ~StateAction(){};
+ int getState(){return _state;};
+ bool handleCmd(KeyCmd keyCmd, int &state);
+protected:
+ int _state;
+ KeyCmd _keyCmdSet;
+};
+
+class EmptyAction : public StateAction{
+public:
+ EmptyAction(int state);
+ ~EmptyAction(){};
+private:
+
+};
+
+
+/*
+必须为长按, deltaValue为每秒改变量
+valueAction允许共用按键,例如空格停止
+但是正反转、停止键不可重复
+*/
+class ValueAction : public KeyAction{
+public:
+ ValueAction(std::string cUp, std::string cDown, double deltaValue, double initValue = 0.0);
+ ValueAction(std::string cUp, std::string cDown, std::string cGoZero, double deltaValue, double initValue = 0.0);
+ ValueAction(std::string cUp, std::string cDown, double deltaValue, double limit1, double limit2, double initValue = 0.0);
+ ValueAction(std::string cUp, std::string cDown, std::string cGoZero, double deltaValue, double limit1, double limit2, double initValue = 0.0);
+
+ ~ValueAction(){};
+ bool handleCmd(KeyCmd keyCmd);
+ void setDt(double dt);
+ double getValue();
+ double getDValue();
+ double getDirection(){return _changeDirection;};
+ void setValue(double value){_value = value;}
+private:
+ double _value;
+ double _changeDirection;
+ double _dV = 0.0; //delta value per delta time
+ double _dt = 0.0;
+ double _dVdt = 0.0; // delta value per second
+ double _lim1, _lim2;
+ bool _hasLim = false;
+ bool _hasGoZero = false;
+
+ KeyCmd _upCmdSet;
+ KeyCmd _downCmdSet;
+ KeyCmd _goZeroCmdSet;
+
+};
+
+
+class CmdPanel{
+public:
+ CmdPanel(std::vector events,
+ EmptyAction emptyAction, size_t channelNum = 1, double dt = 0.002);
+ virtual ~CmdPanel();
+ virtual int getState(size_t channelID = 0);
+ std::vector getValues() {return _values;};
+ std::vector getDValues() {return _dValues;};
+ std::vector getDirections() {return _changeDirections;};
+ void setValue(std::vector values);
+ void setValue(double value, size_t id);
+ virtual std::string getString(std::string slogan);
+ virtual std::vector stringToArray(std::string slogan);
+ virtual std::vector > stringToMatrix(std::string slogan);
+ virtual SendCmd getSendCmd();
+ virtual void setRecvState(RecvState& recvState){};
+protected:
+ virtual void _read() = 0;
+ void _run();
+ void _updateState();
+ void _pressKeyboard();
+ void _releaseKeyboard();
+
+ LoopFunc *_runThread;
+ LoopFunc *_readThread;
+
+ std::vector _stateEvents;
+ std::vector _valueEvents;
+
+ EmptyAction _emptyAction;
+ size_t _actionNum = 0;
+ size_t _stateNum = 0;
+ size_t _valueNum = 0;
+ size_t _channelNum;
+ std::vector _values;
+ std::vector _dValues;
+ std::vector _changeDirections;
+ int _state;
+ std::vector> _stateQueue;
+ std::vector _outputState;
+ std::vector _getState;
+ double _dt;
+ KeyCmd _keyCmd;
+ std::string _cPast = "";
+
+ bool _running = true;
+};
+
+#endif
\ No newline at end of file
diff --git a/include/control/joystick.h b/include/control/joystick.h
new file mode 100644
index 0000000..e8b7ff1
--- /dev/null
+++ b/include/control/joystick.h
@@ -0,0 +1,107 @@
+#ifndef _UNITREE_ARM_JOYSTICK_H_
+#define _UNITREE_ARM_JOYSTICK_H_
+
+#include
+#include "message/udp.h"
+#include "control/cmdPanel.h"
+#include "message/joystick_common.h"
+#include "message/aliengo_common.h"
+#include "message/b1_common.h"
+#include
+
+using namespace UNITREE_LEGGED_SDK_ALIENGO;
+// using namespace UNITREE_LEGGED_SDK_B1;
+
+class UnitreeJoystick : public CmdPanel{
+public:
+ UnitreeJoystick(std::vector events,
+ EmptyAction emptyAction, size_t channelNum = 1,
+ double dt = 0.002)
+ : CmdPanel(events, emptyAction, channelNum, dt){
+ _udp = new UDPPort("dog", "192.168.123.220", 8082, 8081, HIGH_STATE_LENGTH, BlockYN::NO, 500000);
+
+ _udpCmd = {0};
+ _udpState = {0};
+ _readThread = new LoopFunc("JoyStickRead", 0.0, boost::bind(&UnitreeJoystick::_read, this));
+ _runThread = new LoopFunc("CmdPanelRun", _dt, boost::bind(&UnitreeJoystick::_run, this));
+ _readThread->start();
+ _runThread->start();
+ };
+ ~UnitreeJoystick(){
+ delete _udp;
+ delete _runThread;
+ delete _readThread;
+
+ };
+private:
+ void _read(){
+ _udp->send((uint8_t*)&_udpCmd, HIGH_CMD_LENGTH);
+ _udp->recv((uint8_t*)&_udpState);
+
+ #ifdef CTRL_BY_ALIENGO_JOYSTICK
+ memcpy(&_keyData, _udpState.wirelessRemote, 40);
+ #else
+ memcpy(&_keyData, &_udpState.wirelessRemote[0], 40);
+ #endif
+
+ _extractCmd();
+ _updateState();
+ };
+ void _extractCmd(){
+ float joyThre = 0.5; // 手柄数值范围 +-1
+
+ if(((int)_keyData.btn.components.L1 == 1) &&
+ ((int)_keyData.btn.components.L2 == 1)){
+ _keyCmd.c = "l12";
+ }else if((int)_keyData.btn.components.R2 == 1){
+ _keyCmd.c = "r2";
+ if((int)_keyData.btn.components.X == 1){
+ _keyCmd.c = "r2x";
+ }
+ }else if((int)_keyData.btn.components.R1 == 1){
+ _keyCmd.c = "r1";
+ }else if(fabs(_keyData.lx) > joyThre){
+ _keyData.lx > joyThre?_keyCmd.c = "left_left":_keyCmd.c = "left_right";
+ }else if(fabs(_keyData.ly) > joyThre){
+ _keyData.ly > joyThre?_keyCmd.c = "left_up":_keyCmd.c = "left_down";
+ }else if((int)_keyData.btn.components.up == 1){
+ _keyCmd.c = "up";
+ }else if((int)_keyData.btn.components.down == 1){
+ _keyCmd.c = "down";
+ }else if(fabs(_keyData.rx) > joyThre){
+ _keyData.rx > joyThre?_keyCmd.c = "right_left":_keyCmd.c = "right_right";
+ }else if(fabs(_keyData.ry) > joyThre){
+ _keyData.ry > joyThre?_keyCmd.c = "right_up":_keyCmd.c = "right_down";
+ }else if((int)_keyData.btn.components.Y == 1){
+ _keyCmd.c = "Y";
+ }else if((int)_keyData.btn.components.A == 1){
+ _keyCmd.c = "A";
+ }else if((int)_keyData.btn.components.X == 1){
+ _keyCmd.c = "X";
+ }else if((int)_keyData.btn.components.B == 1){
+ _keyCmd.c = "B";
+ }else if((int)_keyData.btn.components.right == 1){
+ _keyCmd.c = "left";
+ }else if((int)_keyData.btn.components.left == 1){
+ _keyCmd.c = "right";
+ }else if((int)_keyData.btn.components.up == 1){
+ _keyCmd.c = "up";
+ }else if((int)_keyData.btn.components.down == 1){
+ _keyCmd.c = "down";
+ }else if((int)_keyData.btn.components.select == 1){
+ _keyCmd.c = "select";
+ }else{
+ _releaseKeyboard();
+ return;
+ }
+
+ _pressKeyboard();
+ };
+
+ xRockerBtnDataStruct _keyData;
+ UDPPort *_udp;
+ HighCmd _udpCmd;
+ HighState _udpState;
+};
+
+#endif // _UNITREE_ARM_JOYSTICK_H_
diff --git a/include/control/keyboard.h b/include/control/keyboard.h
new file mode 100644
index 0000000..c0d0f08
--- /dev/null
+++ b/include/control/keyboard.h
@@ -0,0 +1,34 @@
+#ifndef _UNITREE_ARM_KEYBOARD_H_
+#define _UNITREE_ARM_KEYBOARD_H_
+
+#include
+#include
+#include
+
+#include "message/udp.h"
+#include "common/enumClass.h"
+#include "control/cmdPanel.h"
+
+class Keyboard : public CmdPanel{
+public:
+ Keyboard(std::vector events,
+ EmptyAction emptyAction, size_t channelNum = 1, double dt = 0.002);
+ ~Keyboard();
+ std::string getString(std::string slogan);
+ std::vector stringToArray(std::string slogan);
+ std::vector > stringToMatrix(std::string slogan);
+private:
+ void _read();
+ void _pauseKey();
+ void _startKey();
+ void _extractCmd();
+
+ fd_set _set;
+ char _c = '\0';
+
+ termios _oldSettings;
+ termios _newSettings;
+ timeval _tv;
+};
+
+#endif // _UNITREE_ARM_KEYBOARD_H_
\ No newline at end of file
diff --git a/include/interface/IOInterface.h b/include/interface/IOInterface.h
index 3829e82..50571dc 100755
--- a/include/interface/IOInterface.h
+++ b/include/interface/IOInterface.h
@@ -1,34 +1,29 @@
#ifndef IOINTERFACE_H
#define IOINTERFACE_H
+#include
#include "message/LowlevelCmd.h"
#include "message/LowlevelState.h"
-#include "unitree_arm_sdk/cmdPanel.h"
-#include "unitree_arm_sdk/keyboard.h"
-#include
+#include "control/keyboard.h"
+#include "control/joystick.h"
#include "common/math/robotics.h"
class IOInterface{
public:
- IOInterface(CmdPanel *cmdPanel):_cmdPanel(cmdPanel){}
-
- virtual ~IOInterface(){delete _cmdPanel;};
+ IOInterface(){}
+ ~IOInterface(){
+ delete lowCmd;
+ delete lowState;
+ };
virtual bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state) = 0;
-
- // void zeroCmdPanel(){_cmdPanel->setZero();}
- std::string getString(std::string slogan){return _cmdPanel->getString(slogan);}
- std::vector stringToArray(std::string slogan){return _cmdPanel->stringToArray(slogan);}
- std::vector > stringToMatrix(std::string slogan){return _cmdPanel->stringToMatrix(slogan);}
virtual bool calibration(){return false;};
-
- SendCmd getSendCmd(){return _cmdPanel->getSendCmd();};
- void getRecvState(RecvState recvState){ _cmdPanel->getRecvState(recvState);};
-
- bool isDisConnect = false;// udp disconnection
- bool _isDisConnect[7];// joint(motor) disconnection
+ bool checkGripper(){return hasGripper;};
+ LowlevelCmd *lowCmd;
+ LowlevelState *lowState;
+ virtual bool isDisconnect(){ return false;};
+ bool hasErrorState;
protected:
- uint16_t _isDisConnectCnt[7];
- CmdPanel *_cmdPanel;
+ bool hasGripper;
};
#endif //IOINTERFACE_H
\ No newline at end of file
diff --git a/include/interface/IOROS.h b/include/interface/IOROS.h
index 4683553..7eb13c0 100755
--- a/include/interface/IOROS.h
+++ b/include/interface/IOROS.h
@@ -5,29 +5,20 @@
#include
#include "interface/IOInterface.h"
-#include "unitree_arm_sdk/keyboard.h"
-#include "message/unitree_legged_msgs/MotorCmd.h"
-#include "message/unitree_legged_msgs/MotorState.h"
+#include "message/MotorCmd.h"
+#include "message/MotorState.h"
class IOROS : public IOInterface{
public:
- IOROS(CmdPanel *cmdPanel, bool hasGripper);
+ IOROS();
~IOROS();
bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state);
private:
- bool _hasGripper;
ros::NodeHandle _nm;
-// #ifdef UNITREE_GRIPPER
ros::Subscriber _servo_sub[7];
ros::Publisher _servo_pub[7];
unitree_legged_msgs::MotorState _joint_state[7];
unitree_legged_msgs::MotorCmd _joint_cmd[7];
-// #else
-// ros::Subscriber _servo_sub[6];
-// ros::Publisher _servo_pub[6];
-// unitree_legged_msgs::MotorState _joint_state[6];
-// unitree_legged_msgs::MotorCmd _joint_cmd[6];
-// #endif
void _sendCmd(const LowlevelCmd *cmd);
void _recvState(LowlevelState *state);
void _initRecv();
diff --git a/include/interface/IOUDP.h b/include/interface/IOUDP.h
index 2697889..992faa5 100644
--- a/include/interface/IOUDP.h
+++ b/include/interface/IOUDP.h
@@ -2,26 +2,23 @@
#define IOUDP_H
#include "interface/IOInterface.h"
-// #include "/usr/include/x86_64-linux-gnu/bits/floatn-common.h"
class IOUDP : public IOInterface{
public:
- IOUDP(CmdPanel *cmdPanel, const char* IP, bool hasGripper);
+ IOUDP(const char* IP, uint port);
~IOUDP();
- bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state);
- // void setGripper(int position, int force);
- // void getGripper(int &position, int &force);
+ bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state);
bool calibration();
+ bool isDisconnect(){ return _ioPort->isDisConnect;}
private:
IOPort *_ioPort;
UDPSendCmd _cmd;
UDPRecvState _state;
-
+ UDPRecvStateOld _stateOld;
size_t _motorNum;
size_t _jointNum;
-
uint8_t _singleState;
uint8_t _selfCheck[10];
};
diff --git a/include/message/LowlevelCmd.h b/include/message/LowlevelCmd.h
index ed61262..ac8ae44 100755
--- a/include/message/LowlevelCmd.h
+++ b/include/message/LowlevelCmd.h
@@ -7,8 +7,6 @@
#include
struct LowlevelCmd{
-private:
- size_t _dof = 6;
public:
std::vector q;
std::vector dq;
@@ -16,9 +14,10 @@ public:
std::vector kp;
std::vector kd;
- std::vector> q_cmd_data;
- std::vector> dq_cmd_data;
- std::vector> tau_cmd_data;
+ std::vector> q_data;
+ std::vector> dq_data;
+ std::vector> tauf_data;
+ std::vector> tau_data;
LowlevelCmd();
~LowlevelCmd(){}
@@ -27,13 +26,14 @@ public:
void setZeroTau();
void setZeroKp();
void setZeroKd();
- void setControlGain();
- void setControlGain(std::vector KP, std::vector KW);
- void setQ(std::vector qInput);
void setQ(VecX qInput);
void setQd(VecX qDInput);
void setTau(VecX tauInput);
+
+ void setControlGain();
+ void setControlGain(std::vector KP, std::vector KW);
void setPassive();
+
void setGripperGain();
void setGripperGain(float KP, float KW);
void setGripperZeroGain();
@@ -43,10 +43,13 @@ public:
double getGripperQd();
void setGripperTau(double tauInput);
double getGripperTau();
+
Vec6 getQ();
Vec6 getQd();
void resizeGripper();
+private:
+ size_t _dof = 6;
};
diff --git a/include/message/LowlevelState.h b/include/message/LowlevelState.h
index 2117cc8..9dee0ff 100755
--- a/include/message/LowlevelState.h
+++ b/include/message/LowlevelState.h
@@ -3,55 +3,58 @@
#include
#include
-#include "common/math/mathTypes.h"
#include "common/math/mathTools.h"
#include "common/enumClass.h"
-#include "common/utilities/LPFilter.h"
-#include "message/UserValue.h"
+#include "common/math/Filter.h"
struct LowlevelState{
-private:
- size_t _dof = 6;
public:
LowlevelState(double dt);
~LowlevelState();
- // MotorState motorState[12];
std::vector q;
std::vector dq;
std::vector ddq;
std::vector tau;
- std::vector temperature;
+ std::vector> q_data;
+ std::vector> dq_data;
+ std::vector> ddq_data;
+ std::vector> tau_data;
+
+ std::vector temperature;
std::vector errorstate;
+ std::vector isMotorConnected;
std::vector qFiltered;
std::vector dqFiltered;
+ std::vector ddqFiltered;
std::vector tauFiltered;
- std::vector> q_state_data;
- std::vector> dq_state_data;
- std::vector> tau_state_data;
-
- // ArmFSMStateName userCmd;
- UserValue userValue;
LPFilter *qFilter;
LPFilter *dqFilter;
+ LPFilter *ddqFilter;
LPFilter *tauFilter;
void resizeGripper(double dt);
void runFilter();
+ bool checkError();
Vec6 getQ();
Vec6 getQd();
Vec6 getQdd();
Vec6 getTau();
Vec6 getQFiltered();
Vec6 getQdFiltered();
+ Vec6 getQddFiltered();
Vec6 getTauFiltered();
- double getGripperQ() {return q.at(q.size()-1);}
- double getGripperQd() {return dq.at(q.size()-1);}
- double getGripperTau() {return tau.at(tau.size()-1);}
- double getGripperTauFiltered() {return tauFiltered.at(tau.size()-1);}
+ double getGripperQ();
+ double getGripperQd();
+ double getGripperTau();
+ double getGripperTauFiltered();
+private:
+ size_t _dof = 6;
+ int temporatureLimit = 80.0;// centigrade
+ std::vector _isMotorConnectedCnt;
};
#endif //LOWLEVELSTATE_HPP
diff --git a/include/message/unitree_legged_msgs/MotorCmd.h b/include/message/MotorCmd.h
similarity index 100%
rename from include/message/unitree_legged_msgs/MotorCmd.h
rename to include/message/MotorCmd.h
diff --git a/include/message/unitree_legged_msgs/MotorState.h b/include/message/MotorState.h
similarity index 100%
rename from include/message/unitree_legged_msgs/MotorState.h
rename to include/message/MotorState.h
diff --git a/include/message/UserValue.h b/include/message/UserValue.h
deleted file mode 100644
index e69956a..0000000
--- a/include/message/UserValue.h
+++ /dev/null
@@ -1,38 +0,0 @@
-#ifndef USERVALUE_H
-#define USERVALUE_H
-
-#include "common/math/mathTypes.h"
-#include "common/utilities/typeTrans.h"
-#include
-
-template
-std::vector cutVector(std::vector vec, size_t startID, size_t length){
- std::vector result;
- result.assign(vec.begin()+startID, vec.begin()+startID+length);
- return result;
-}
-
-struct UserValue{
- Vec6 moveAxis;
- double gripperPos;
-
- void setData(std::vector rawData){
- if(rawData.size() != 7){
- std::cout << "[ERROR] UserValue::setData, the size of rawDate is " << rawData.size() << " but not 7" << std::endl;
- }
- gripperPos = rawData.at(6);
- rawData = cutVector(rawData, 0, 6);
- moveAxis = typeTrans::getValue(rawData, moveAxis);
- }
-
- UserValue(){
- setZero();
- }
- void setZero(){
- moveAxis.setZero();
- gripperPos = 0;
- // gripperTau = 0;
- }
-};
-
-#endif
\ No newline at end of file
diff --git a/include/message/aliengo_common.h b/include/message/aliengo_common.h
new file mode 100644
index 0000000..b82e925
--- /dev/null
+++ b/include/message/aliengo_common.h
@@ -0,0 +1,168 @@
+#ifndef _UNITREE_ARM_ALIENGO_COMMON_H_
+#define _UNITREE_ARM_ALIENGO_COMMON_H_
+
+#include
+
+namespace UNITREE_LEGGED_SDK_ALIENGO
+{
+
+ constexpr int HIGHLEVEL = 0x00;
+ constexpr int LOWLEVEL = 0xff;
+ constexpr double PosStopF = (2.146E+9f);
+ constexpr double VelStopF = (16000.0f);
+
+#pragma pack(1)
+
+ // 12 bytes
+ typedef struct
+ {
+ float x;
+ float y;
+ float z;
+ } Cartesian;
+
+ // 53 bytes
+ typedef struct
+ {
+ float quaternion[4]; // quaternion, normalized, (w,x,y,z)
+ float gyroscope[3]; // angular velocity (unit: rad/s)
+ float accelerometer[3]; // m/(s2)
+ float rpy[3]; // euler angle(unit: rad)
+ int8_t temperature;
+ } IMU; // when under accelerated motion, the attitude of the robot calculated by IMU will drift.
+
+ // 3 bytes
+ typedef struct
+ {
+ uint8_t r;
+ uint8_t g;
+ uint8_t b;
+ } LED; // foot led brightness: 0~255
+
+ // 38 bytes
+ typedef struct
+ {
+ uint8_t mode; // motor working mode
+ float q; // current angle (unit: radian)
+ float dq; // current velocity (unit: radian/second)
+ float ddq; // current acc (unit: radian/second*second)
+ float tauEst; // current estimated output torque (unit: N.m)
+ float q_raw; // current angle (unit: radian)
+ float dq_raw; // current velocity (unit: radian/second)
+ float ddq_raw;
+ int8_t temperature; // current temperature (temperature conduction is slow that leads to lag)
+ uint32_t reserve[2]; // in reserve[1] returns the brake state.
+ } MotorState; // motor feedback
+
+ // 33 bytes
+ typedef struct
+ {
+ uint8_t mode; // desired working mode
+ float q; // desired angle (unit: radian)
+ float dq; // desired velocity (unit: radian/second)
+ float tau; // desired output torque (unit: N.m)
+ float Kp; // desired position stiffness (unit: N.m/rad )
+ float Kd; // desired velocity stiffness (unit: N.m/(rad/s) )
+ uint32_t reserve[3]; // in reserve[0] sends the brake cmd.
+ } MotorCmd; // motor control
+
+ // 891 bytes
+ typedef struct
+ {
+ uint8_t levelFlag; // flag to distinguish high level or low level
+ uint16_t commVersion;
+ uint16_t robotID;
+ uint32_t SN;
+ uint8_t bandWidth;
+ IMU imu;
+ MotorState motorState[20];
+ int16_t footForce[4]; // force sensors
+ int16_t footForceEst[4]; // force sensors
+ uint32_t tick; // reference real-time from motion controller (unit: us)
+ uint8_t wirelessRemote[40]; // wireless commands
+ uint32_t reserve;
+ uint32_t crc;
+ } LowState; // low level feedback
+
+ // 730 bytes
+ typedef struct
+ {
+ uint8_t levelFlag;
+ uint16_t commVersion;
+ uint16_t robotID;
+ uint32_t SN;
+ uint8_t bandWidth;
+ MotorCmd motorCmd[20];
+ LED led[4];
+ uint8_t wirelessRemote[40];
+ uint32_t reserve;
+ uint32_t crc;
+ } LowCmd; // low level control
+
+ // 244byte
+ typedef struct
+ {
+ uint8_t levelFlag;
+ uint16_t commVersion;
+ uint16_t robotID;
+ uint32_t SN;
+ uint8_t bandWidth;
+ uint8_t mode;
+ IMU imu;
+ float position[3]; // (unit: m), from robot own odometry in inertial frame, usually drift
+ float velocity[3]; // (unit: m/s), forwardSpeed, sideSpeed, updownSpeed in body frame
+ float yawSpeed; // (unit: rad/s), rotateSpeed in body frame.
+ Cartesian footPosition2Body[4]; // foot position relative to body
+ Cartesian footSpeed2Body[4]; // foot speed relative to body
+ int16_t footForce[4];
+ uint8_t wirelessRemote[40];
+ uint32_t reserve;
+ uint32_t crc;
+ } HighState; // high level feedback
+
+ //113byte
+ typedef struct
+ {
+ uint8_t levelFlag;
+ uint16_t commVersion;
+ uint16_t robotID;
+ uint32_t SN;
+ uint8_t bandWidth;
+ uint8_t mode; // 0.idle, default stand | 1.force stand (controlled by dBodyHeight + rpy)
+ // 2.target velocity walking (controlled by velocity + yawSpeed)
+ // 3.target position walking (controlled by position + rpy[2])
+ // 4. path mode walking (reserve for future release)
+ // 5. position stand down. |6. position stand up |7. damping mode | 8. recovery mode
+ uint8_t gaitType; // 0.trot | 1. trot running | 2.climb stair
+ uint8_t speedLevel; // 0. default low speed. 1. medium speed 2. high speed. during walking
+ float dFootRaiseHeight; // (unit: m), swing foot height adjustment from default swing height.
+ float dBodyHeight; // (unit: m), body height adjustment from default body height.
+ float position[2]; // (unit: m), desired x and y position in inertial frame.
+ float rpy[3]; // (unit: rad), desired yaw-pitch-roll euler angle, expressed in roll(rpy[0]) pitch(rpy[1]) yaw(rpy[2])
+ float velocity[2]; // (unit: m/s), forwardSpeed, sideSpeed in body frame.
+ float yawSpeed; // (unit: rad/s), rotateSpeed in body frame.
+ LED led[4];
+ uint8_t wirelessRemote[40];
+ uint32_t reserve;
+ uint32_t crc;
+ } HighCmd; // high level control uint8_t mode
+
+#pragma pack()
+
+ typedef struct
+ {
+ unsigned long long TotalCount; // total loop count
+ unsigned long long SendCount; // total send count
+ unsigned long long RecvCount; // total receive count
+ unsigned long long SendError; // total send error
+ unsigned long long FlagError; // total flag error
+ unsigned long long RecvCRCError; // total reveive CRC error
+ unsigned long long RecvLoseError; // total lose package count
+ } UDPState; // UDP communication state
+
+ constexpr int HIGH_CMD_LENGTH = (sizeof(HighCmd));
+ constexpr int HIGH_STATE_LENGTH = (sizeof(HighState));
+
+}
+
+#endif
\ No newline at end of file
diff --git a/include/message/arm_common.h b/include/message/arm_common.h
new file mode 100644
index 0000000..e8e15b1
--- /dev/null
+++ b/include/message/arm_common.h
@@ -0,0 +1,150 @@
+#ifndef _UNITREE_ARM_ARM_COMMON_H_
+#define _UNITREE_ARM_ARM_COMMON_H_
+
+#include
+
+#pragma pack(1)
+
+// 4 Byte
+enum class ArmFSMState{
+ INVALID,
+ PASSIVE,
+ JOINTCTRL,
+ CARTESIAN,
+ MOVEJ,
+ MOVEL,
+ MOVEC,
+ TRAJECTORY,
+ TOSTATE,
+ SAVESTATE,
+ TEACH,
+ TEACHREPEAT,
+ CALIBRATION,
+ SETTRAJ,
+ BACKTOSTART,
+ NEXT,
+ LOWCMD
+};
+
+// 4 Byte
+enum class ArmFSMValue{
+ INVALID,
+ Q,A,
+ W,S,
+ E,D,
+ R,F,
+ T,G,
+ Y,H,
+ DOWN,
+ UP
+};
+
+enum class TrajType{
+ MoveJ,
+ MoveL,
+ MoveC,
+ Stop
+};
+
+// 20 Byte
+struct JointCmd{
+ float T;
+ float W;
+ float Pos;
+ float K_P;
+ float K_W;
+};
+
+typedef struct{
+ uint8_t reserved : 6 ;
+ uint8_t state : 2 ;//whether motor is connected; 0-ok, 1-disconnected, 2-CRC error
+}Motor_Connected;
+
+typedef struct{
+ int8_t temperature;
+ /* 0x01: phase current is too large
+ * 0x02: phase leakage
+ * 0x04: overheat(including the motor windings and the motor shell)
+ * 0x20: jumped
+ */
+ uint8_t error;
+ Motor_Connected isConnected;
+}Motor_State;
+
+struct JointStateOld{//no error state return
+ float T;
+ float W;
+ float Acc;
+ float Pos;
+};
+
+struct JointState{
+ float T;
+ float W;
+ float Acc;
+ float Pos;
+ Motor_State state[2];
+};
+
+//140bytes
+union UDPSendCmd{
+ uint8_t checkCmd;
+ JointCmd jointCmd[7];
+};
+
+struct UDPRecvStateOld{
+ JointStateOld jointStateOld[7];
+};
+
+struct UDPRecvState{
+ JointState jointState[7];
+};
+
+struct Posture{
+ double roll;
+ double pitch;
+ double yaw;
+ double x;
+ double y;
+ double z;
+};
+
+struct TrajCmd{
+ TrajType trajType;
+ Posture posture[2];
+ double gripperPos;
+ double maxSpeed;
+ double stopTime;
+ int trajOrder;
+};
+
+union ValueUnion{
+ char name[10];
+ JointCmd jointCmd[7];
+ TrajCmd trajCmd;
+};
+
+struct SendCmd{
+ uint8_t head[2];
+ ArmFSMState state;
+ bool track;// whether let arm track jointCmd in State_JOINTCTRL or posture[0] in State_CARTESIAN
+ ValueUnion valueUnion;
+};
+
+
+struct RecvState{
+ uint8_t head[2];
+ ArmFSMState state;
+ JointState jointState[7];
+ Posture cartesianState;
+};
+
+constexpr int SENDCMD_LENGTH = (sizeof(SendCmd));
+constexpr int RECVSTATE_LENGTH = (sizeof(RecvState));
+constexpr int JointCmd_LENGTH = (sizeof(JointCmd));
+constexpr int JointState_LENGTH = (sizeof(JointState));
+constexpr int JointStateOld_LENGTH = (sizeof(JointStateOld));
+
+#pragma pack()
+
+#endif // _UNITREE_ARM_ARM_MSG_H_
\ No newline at end of file
diff --git a/include/message/arm_motor_common.h b/include/message/arm_motor_common.h
new file mode 100644
index 0000000..e248955
--- /dev/null
+++ b/include/message/arm_motor_common.h
@@ -0,0 +1,214 @@
+#ifndef _UNITREE_ARM_ARM_MOTOR_COMMON_H_
+#define _UNITREE_ARM_ARM_MOTOR_COMMON_H_
+
+#include
+typedef int16_t q15_t;
+
+#pragma pack(1)
+
+// 发送用单个数据数据结构
+typedef union{
+ int32_t L;
+ uint8_t u8[4];
+ uint16_t u16[2];
+ uint32_t u32;
+ float F;
+}COMData32;
+
+typedef struct {
+ // 定义 数据包头
+ unsigned char start[2]; // 包头
+ unsigned char motorID; // 电机ID 0,1,2,3 ... 0xBB 表示向所有电机广播(此时无返回)
+ unsigned char reserved;
+}COMHead;
+
+#pragma pack()
+
+#pragma pack(1)
+
+typedef struct {
+
+ uint8_t fan_d; // 关节上的散热风扇转速
+ uint8_t Fmusic; // 电机发声频率 /64*1000 15.625f 频率分度
+ uint8_t Hmusic; // 电机发声强度 推荐值4 声音强度 0.1 分度
+ uint8_t reserved4;
+
+ uint8_t FRGB[4]; // 足端LED
+
+}LowHzMotorCmd;
+
+typedef struct { // 以 4个字节一组排列 ,不然编译器会凑整
+ // 定义 数据
+ uint8_t mode; // 关节模式选择
+ uint8_t ModifyBit; // 电机控制参数修改位
+ uint8_t ReadBit; // 电机控制参数发送位
+ uint8_t reserved;
+
+ COMData32 Modify; // 电机参数修改 的数据
+ //实际给FOC的指令力矩为:
+ //K_P*delta_Pos + K_W*delta_W + T
+ q15_t T; // 期望关节的输出力矩(电机本身的力矩)x256, 7 + 8 描述
+ q15_t W; // 期望关节速度 (电机本身的速度) x128, 8 + 7描述
+ int32_t Pos; // 期望关节位置 x 16384/6.2832, 14位编码器(主控0点修正,电机关节还是以编码器0点为准)
+
+ q15_t K_P; // 关节刚度系数 x2048 4+11 描述
+ q15_t K_W; // 关节速度系数 x1024 5+10 描述
+
+ uint8_t LowHzMotorCmdIndex; // 电机低频率控制命令的索引, 0-7, 分别代表LowHzMotorCmd中的8个字节
+ uint8_t LowHzMotorCmdByte; // 电机低频率控制命令的字节
+
+ COMData32 Res[1]; // 通讯 保留字节 用于实现别的一些通讯内容
+
+}MasterComdV3; // 加上数据包的包头 和CRC 34字节
+
+typedef struct {
+ // 定义 电机控制命令数据包
+ COMHead head;
+ MasterComdV3 Mdata;
+ COMData32 CRCdata;
+}MasterComdDataV3;//返回数据
+
+// typedef struct {
+// // 定义 总得485 数据包
+
+// MasterComdData M1;
+// MasterComdData M2;
+// MasterComdData M3;
+
+// }DMA485TxDataV3;
+
+#pragma pack()
+
+#pragma pack(1)
+
+typedef struct { // 以 4个字节一组排列 ,不然编译器会凑整
+ // 定义 数据
+ uint8_t mode; // 当前关节模式
+ uint8_t ReadBit; // 电机控制参数修改 是否成功位
+ int8_t Temp; // 电机当前平均温度
+ uint8_t MError; // 电机错误 标识
+
+ COMData32 Read; // 读取的当前 电机 的控制数据
+ int16_t T; // 当前实际电机输出力矩 7 + 8 描述
+
+ int16_t W; // 当前实际电机速度(高速) 8 + 7 描述
+ float LW; // 当前实际电机速度(低速)
+
+ int16_t W2; // 当前实际关节速度(高速) 8 + 7 描述
+ float LW2; // 当前实际关节速度(低速)
+
+ int16_t Acc; // 电机转子加速度 15+0 描述 惯量较小
+ int16_t OutAcc; // 输出轴加速度 12+3 描述 惯量较大
+
+ int32_t Pos; // 当前电机位置(主控0点修正,电机关节还是以编码器0点为准)
+ int32_t Pos2; // 关节编码器位置(输出编码器)
+
+ int16_t gyro[3]; // 电机驱动板6轴传感器数据
+ int16_t acc[3];
+
+ // 力传感器的数据
+ int16_t Fgyro[3]; //
+ int16_t Facc[3];
+ int16_t Fmag[3];
+ uint8_t Ftemp; // 8位表示的温度 7位(-28~100度) 1位0.5度分辨率
+
+ int16_t Force16; // 力传感器高16位数据
+ int8_t Force8; // 力传感器低8位数据
+
+ uint8_t FError; // 足端传感器错误标识
+
+ int8_t Res[1]; // 通讯 保留字节
+
+}ServoComdV3; // 加上数据包的包头 和CRC 78字节(4+70+4)
+
+typedef struct {
+ // 定义 电机控制命令数据包
+ COMHead head;
+ ServoComdV3 Mdata;
+
+ COMData32 CRCdata;
+
+}ServoComdDataV3; //发送数据
+
+// typedef struct {
+// // 定义 总的485 接受数据包
+
+// ServoComdDataV3 M[3];
+// // uint8_t nullbyte1;
+
+// }DMA485RxDataV3;
+
+
+#pragma pack()
+
+// 00 00 00 00 00
+// 00 00 00 00 00
+// 00 00 00 00 00
+// 00 00 00
+// 数据包默认初始化
+// 主机发送的数据包
+/*
+ Tx485Data[_FR][i].head.start[0] = 0xFE ; Tx485Data[_FR][i].head.start[1] = 0xEE; // 数据包头
+ Tx485Data[_FR][i].Mdata.ModifyBit = 0xFF; Tx485Data[_FR][i].Mdata.mode = 0; // 默认不修改数据 和 电机的默认工作模式
+ Tx485Data[_FR][i].head.motorID = i; 0 // 目标电机标号
+ Tx485Data[_FR][i].Mdata.T = 0.0f; // 默认目标关节输出力矩 motor1.Extra_Torque = motorRxData[1].Mdata.T*0.390625f; // N.M 转化为 N.CM IQ8描述
+ Tx485Data[_FR][i].Mdata.Pos = 0x7FE95C80; // 默认目标关节位置 不启用位置环 14位分辨率
+ Tx485Data[_FR][i].Mdata.W = 16000.0f; // 默认目标关节速度 不启用速度环 1+8+7描述 motor1.Target_Speed = motorRxData[1].Mdata.W*0.0078125f; // 单位 rad/s IQ7描述
+ Tx485Data[_FR][i].Mdata.K_P = (q15_t)(0.6f*(1<<11)); // 默认关节刚度系数 4+11 描述 motor1.K_Pos = ((float)motorRxData[1].Mdata.K_P)/(1<<11); // 描述刚度的通讯数据格式 4+11
+ Tx485Data[_FR][i].Mdata.K_W = (q15_t)(1.0f*(1<<10)); // 默认关节速度系数 5+10 描述 motor1.K_Speed = ((float)motorRxData[1].Mdata.K_W)/(1<<10); // 描述阻尼的通讯数据格式 5+10
+*/
+
+enum class MotorType{
+ A1Go1, // 4.8M baudrate, K_W x1024
+ B1 // 6.0M baudrate, K_W x512
+};
+
+struct MOTOR_send{
+ // 定义 发送格式化数据
+ MasterComdDataV3 motor_send_data; //电机控制数据结构体
+ MotorType motorType = MotorType::A1Go1;
+ int hex_len = 34; //发送的16进制命令数组长度, 34
+ // long long send_time; //发送该命令的时间, 微秒(us)
+ // 待发送的各项数据
+ unsigned short id; //电机ID,0xBB代表全部电机
+ unsigned short mode; //0:空闲, 5:开环转动, 10:闭环FOC控制
+ //实际给FOC的指令力矩为:
+ //K_P*delta_Pos + K_W*delta_W + T
+ float T; //期望关节的输出力矩(电机本身的力矩)(Nm)
+ float W; //期望关节速度(电机本身的速度)(rad/s)
+ float Pos; //期望关节位置(rad)
+ float K_P; //关节刚度系数
+ float K_W; //关节速度系数
+ COMData32 Res; // 通讯 保留字节 用于实现别的一些通讯内容
+};
+
+struct MOTOR_recv{
+ // 定义 接收数据
+ ServoComdDataV3 motor_recv_data; //电机接收数据结构体
+ MotorType motorType = MotorType::A1Go1;
+ int hex_len = 78; //接收的16进制命令数组长度, 78
+ // long long resv_time; //接收该命令的时间, 微秒(us)
+ bool correct = false; //接收数据是否完整(true完整,false不完整)
+ //解读得出的电机数据
+ unsigned char motor_id; //电机ID
+ unsigned char mode; //0:空闲, 5:开环转动, 10:闭环FOC控制
+ int Temp; //温度
+ unsigned char MError; //错误码
+
+ float T; // 当前实际电机输出力矩
+ float W; // 当前实际电机速度(高速)
+ float LW; // 当前实际电机速度(低速)
+ int Acc; // 电机转子加速度
+ float Pos; // 当前电机位置(主控0点修正,电机关节还是以编码器0点为准)
+
+ float gyro[3]; // 电机驱动板6轴传感器数据
+ float acc[3];
+
+ int8_t Res;
+};
+
+
+extern void modify_data(MOTOR_send* motor_s);
+extern bool extract_data(MOTOR_recv* motor_r);
+
+#endif
\ No newline at end of file
diff --git a/include/message/b1_common.h b/include/message/b1_common.h
new file mode 100644
index 0000000..24debcd
--- /dev/null
+++ b/include/message/b1_common.h
@@ -0,0 +1,211 @@
+#ifndef _UNITREE_ARM_B1_COMMON_H_
+#define _UNITREE_ARM_B1_COMMON_H_
+
+#include
+#include
+
+namespace UNITREE_LEGGED_SDK_B1
+{
+
+ constexpr int HIGHLEVEL = 0xee;
+ constexpr int LOWLEVEL = 0xff;
+ constexpr int TRIGERLEVEL = 0xf0;
+ constexpr double PosStopF = (2.146E+9f);
+ constexpr double VelStopF = (16000.0f);
+ extern const int LOW_CMD_LENGTH; // shorter than sizeof(LowCmd), bytes compressed LowCmd length
+ extern const int LOW_STATE_LENGTH; // shorter than sizeof(LowState), bytes compressed LowState length
+
+#pragma pack(1)
+
+ typedef struct
+ {
+ uint8_t off; // off 0xA5
+ std::array reserve;
+ } BmsCmd;
+
+ typedef struct
+ {
+ uint8_t version_h;
+ uint8_t version_l;
+ uint8_t bms_status;
+ uint8_t SOC; // SOC 0-100%
+ int32_t current; // mA
+ uint16_t cycle;
+ std::array BQ_NTC; // x1 degrees centigrade
+ std::array MCU_NTC; // x1 degrees centigrade
+ std::array cell_vol; // cell voltage mV
+ } BmsState;
+
+ typedef struct
+ {
+ float x;
+ float y;
+ float z;
+ } Cartesian;
+
+ typedef struct
+ {
+ std::array quaternion; // quaternion, normalized, (w,x,y,z)
+ std::array gyroscope; // angular velocity (unit: rad/s) (raw data)
+ std::array accelerometer; // m/(s2) (raw data)
+ std::array rpy; // euler angle(unit: rad)
+ int8_t temperature;
+ } IMU; // when under accelerated motion, the attitude of the robot calculated by IMU will drift.
+
+ typedef struct
+ {
+ uint8_t r;
+ uint8_t g;
+ uint8_t b;
+ } LED; // foot led brightness: 0~255
+
+ typedef struct
+ {
+ uint8_t mode; // motor working mode
+ float q; // current angle (unit: radian)
+ float dq; // current velocity (unit: radian/second)
+ float ddq; // current acc (unit: radian/second*second)
+ float tauEst; // current estimated output torque (unit: N.m)
+ float q_raw; // current angle (unit: radian)
+ float dq_raw; // current velocity (unit: radian/second)
+ float ddq_raw;
+ int8_t temperature; // current temperature (temperature conduction is slow that leads to lag)
+ std::array reserve;
+ } MotorState; // motor feedback
+
+ typedef struct
+ {
+ uint8_t mode; // desired working mode
+ float q; // desired angle (unit: radian)
+ float dq; // desired velocity (unit: radian/second)
+ float tau; // desired output torque (unit: N.m)
+ float Kp; // desired position stiffness (unit: N.m/rad )
+ float Kd; // desired velocity stiffness (unit: N.m/(rad/s) )
+ std::array reserve;
+ } MotorCmd; // motor control
+
+ typedef struct
+ {
+ std::array head;
+ uint8_t levelFlag;
+ uint8_t frameReserve;
+
+ std::array SN;
+ std::array version;
+ uint16_t bandWidth;
+ IMU imu;
+ std::array motorState;
+ BmsState bms;
+ std::array footForce; // force sensors
+ std::array footForceEst; // force sensors
+ uint32_t tick; // reference real-time from motion controller (unit: us)
+ std::array wirelessRemote; // wireless commands
+ uint32_t reserve;
+
+ uint32_t crc;
+ } LowState; // low level feedback
+
+ typedef struct
+ {
+ std::array head;
+ uint8_t levelFlag;
+ uint8_t frameReserve;
+
+ std::array SN;
+ std::array version;
+ uint16_t bandWidth;
+ std::array motorCmd;
+ BmsCmd bms;
+ std::array wirelessRemote;
+ uint32_t reserve;
+
+ uint32_t crc;
+ } LowCmd; // low level control
+
+ typedef struct
+ {
+ std::array head;
+ uint8_t levelFlag;
+ uint8_t frameReserve;
+
+ std::array SN;
+ std::array version;
+ uint16_t bandWidth;
+ IMU imu;
+ std::array motorState;
+ BmsState bms;
+ std::array footForce;
+ std::array footForceEst;
+ uint8_t mode;
+ float progress;
+ uint8_t gaitType; // 0.idle 1.trot 2.trot running 3.climb stair 4.trot obstacle
+ float footRaiseHeight; // (unit: m, default: 0.08m), foot up height while walking
+ std::array position; // (unit: m), from own odometry in inertial frame, usually drift
+ float bodyHeight; // (unit: m, default: 0.28m),
+ std::array velocity; // (unit: m/s), forwardSpeed, sideSpeed, rotateSpeed in body frame
+ float yawSpeed; // (unit: rad/s), rotateSpeed in body frame
+ std::array rangeObstacle;
+ std::array footPosition2Body; // foot position relative to body
+ std::array footSpeed2Body; // foot speed relative to body
+ std::array wirelessRemote;
+ uint32_t reserve;
+
+ uint32_t crc;
+ } HighState; // high level feedback
+
+ typedef struct
+ {
+ std::array head;
+ uint8_t levelFlag;
+ uint8_t frameReserve;
+
+ std::array SN;
+ std::array version;
+ uint16_t bandWidth;
+ uint8_t mode; // 0. idle, default stand 1. force stand (controlled by dBodyHeight + ypr)
+ // 2. target velocity walking (controlled by velocity + yawSpeed)
+ // 3. target position walking (controlled by position + ypr[0])
+ // 4. path mode walking (reserve for future release)
+ // 5. position stand down.
+ // 6. position stand up
+ // 7. damping mode
+ // 8. recovery stand
+ // 9. backflip
+ // 10. jumpYaw
+ // 11. straightHand
+ // 12. dance1
+ // 13. dance2
+
+ uint8_t gaitType; // 0.idle 1.trot 2.trot running 3.climb stair 4.trot obstacle
+ uint8_t speedLevel; // 0. default low speed. 1. medium speed 2. high speed. during walking, only respond MODE 3
+ float footRaiseHeight; // (unit: m, default: 0.08m), foot up height while walking, delta value
+ float bodyHeight; // (unit: m, default: 0.28m), delta value
+ std::array position; // (unit: m), desired position in inertial frame
+ std::array euler; // (unit: rad), roll pitch yaw in stand mode
+ std::array velocity; // (unit: m/s), forwardSpeed, sideSpeed in body frame
+ float yawSpeed; // (unit: rad/s), rotateSpeed in body frame
+ BmsCmd bms;
+ std::array led;
+ std::array wirelessRemote;
+ uint32_t reserve;
+
+ uint32_t crc;
+ } HighCmd; // high level control
+
+#pragma pack()
+
+ typedef struct
+ {
+ unsigned long long TotalCount; // total loop count
+ unsigned long long SendCount; // total send count
+ unsigned long long RecvCount; // total receive count
+ unsigned long long SendError; // total send error
+ unsigned long long FlagError; // total flag error
+ unsigned long long RecvCRCError; // total reveive CRC error
+ unsigned long long RecvLoseError; // total lose package count
+ } UDPState; // UDP communication state
+ constexpr int HIGH_CMD_LENGTH = (sizeof(HighCmd));
+ constexpr int HIGH_STATE_LENGTH = (sizeof(HighState));
+}
+
+#endif
\ No newline at end of file
diff --git a/include/message/joystick_common.h b/include/message/joystick_common.h
new file mode 100644
index 0000000..b9c1e06
--- /dev/null
+++ b/include/message/joystick_common.h
@@ -0,0 +1,43 @@
+/*****************************************************************
+Copyright (c) 2022, Unitree Robotics.Co.Ltd. All rights reserved.
+*****************************************************************/
+#ifndef _UNITREE_ARM_JOYSTICK_COMMON_H_
+#define _UNITREE_ARM_JOYSTICK_COMMON_H_
+#include
+
+// 16b
+typedef union {
+ struct {
+ uint8_t R1 :1;
+ uint8_t L1 :1;
+ uint8_t start :1;
+ uint8_t select :1;
+ uint8_t R2 :1;
+ uint8_t L2 :1;
+ uint8_t F1 :1;
+ uint8_t F2 :1;
+ uint8_t A :1;
+ uint8_t B :1;
+ uint8_t X :1;
+ uint8_t Y :1;
+ uint8_t up :1;
+ uint8_t right :1;
+ uint8_t down :1;
+ uint8_t left :1;
+ } components;
+ uint16_t value;
+} xKeySwitchUnion;
+
+// 40 Byte (now used 24B)
+typedef struct {
+ uint8_t head[2];
+ xKeySwitchUnion btn;
+ float lx;
+ float rx;
+ float ry;
+ float L2;
+ float ly;
+ uint8_t idle[16];
+} xRockerBtnDataStruct;
+
+#endif
\ No newline at end of file
diff --git a/include/message/udp.h b/include/message/udp.h
new file mode 100644
index 0000000..6b2c2a6
--- /dev/null
+++ b/include/message/udp.h
@@ -0,0 +1,69 @@
+#ifndef _UDP_H
+#define _UDP_H
+
+#include
+#include
+#include
+#include
+#include
+#include "common/utilities/timer.h"
+#include "message/arm_motor_common.h"
+
+enum class BlockYN{
+ YES,
+ NO
+};
+
+
+class IOPort{
+public:
+ IOPort(std::string name, BlockYN blockYN, size_t recvLength, size_t timeOutUs)
+ :_name(name){
+ resetIO(blockYN, recvLength, timeOutUs);
+ }
+ virtual ~IOPort(){}
+ virtual size_t send(uint8_t *sendMsg, size_t sendLength) = 0;
+ virtual size_t recv(uint8_t *recvMsg, size_t recvLength) = 0;
+ virtual size_t recv(uint8_t *recvMsg) = 0;
+ virtual bool sendRecv(std::vector &sendVec, std::vector &recvVec) = 0;
+ void resetIO(BlockYN blockYN, size_t recvLength, size_t timeOutUs);
+ bool isDisConnect = false;
+protected:
+ std::string _name;
+ BlockYN _blockYN = BlockYN::NO;
+ size_t _recvLength;
+ timeval _timeout;
+ timeval _timeoutSaved;
+ uint16_t _isDisConnectCnt;
+};
+
+
+class UDPPort : public IOPort{
+public:
+ UDPPort(std::string name, std::string toIP, uint toPort, uint ownPort,
+ size_t recvLength = 0,
+ BlockYN blockYN = BlockYN::NO,
+ size_t timeOutUs = 20000);
+ ~UDPPort();
+ size_t send(uint8_t *sendMsg, size_t sendMsgLength);
+ size_t recv(uint8_t *recvMsg, size_t recvLength);
+ size_t recv(uint8_t *recvMsg);
+ bool sendRecv(std::vector &sendVec, std::vector &recvVec);
+private:
+ size_t _recvBlock(uint8_t *recvMsg, size_t recvLength);
+ size_t _recvUnBlock(uint8_t *recvMsg, size_t recvLength);
+ sockaddr_in _ownAddr, _toAddr, _fromAddr;
+ socklen_t _sockaddrSize;
+ int _sockfd;
+ int _on = 1;
+ size_t _sentLength;
+
+ uint8_t _sendBytes[238]; // 7 motors
+ uint8_t _recvBytes[546]; // 7 motors
+
+ fd_set _rSet;
+};
+
+
+
+#endif
\ No newline at end of file
diff --git a/include/message/unitree_legged_msgs/Cartesian.h b/include/message/unitree_legged_msgs/Cartesian.h
deleted file mode 100644
index 064a47e..0000000
--- a/include/message/unitree_legged_msgs/Cartesian.h
+++ /dev/null
@@ -1,215 +0,0 @@
-// Generated by gencpp from file unitree_legged_msgs/Cartesian.msg
-// DO NOT EDIT!
-
-
-#ifndef UNITREE_LEGGED_MSGS_MESSAGE_CARTESIAN_H
-#define UNITREE_LEGGED_MSGS_MESSAGE_CARTESIAN_H
-
-
-#include
-#include
-#include