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

V2.2.0 

1. delete quadrobot files ( remove joystick )
2. change cartesian control command from endposture to spatial twist
This commit is contained in:
Agnel Wang 2022-12-07 11:15:36 +08:00
parent 660c1e6cb2
commit 3053e2adce
17 changed files with 16 additions and 502 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
include/FSM/State_Cartesian.h
View File

@ -12,14 +12,14 @@ public:
void exit();
int checkChange(int cmd);
private:
double _oriSpeed = 0.4;// control by keyboard or joystick
double _posSpeed = 0.15;
double oriSpeedLimit = 0.3;// limits in SDK
double posSpeedLimit = 0.3;
double _oriSpeed = 0.4;// control by keyboard
double _posSpeed = 0.3;
double oriSpeedLimit = 0.5;// limits in SDK
double posSpeedLimit = 0.5;
VecX _changeDirections;
Vec6 _twist;
HomoMat _endHomoGoal, _endHomoGoalPast;
HomoMat _endHomoGoal, _endHomoPast, _endHomoDelta;
Vec6 _endPostureGoal, _endPosturePast, _endPostureDelta;
};

1
include/FSM/State_MoveC.h
View File

@ -17,6 +17,7 @@ private:
std::vector<Vec6> _postures;
EndCircleTraj *_circleTraj;
bool _timeReached, _taskReached, _pastTaskReached, _finalReached;
uint _taskCnt;
};
#endif // CARTESIAN_H

1
include/FSM/State_MoveL.h
View File

@ -17,5 +17,6 @@ private:
std::vector<Vec6> _postures;
EndLineTraj *_lineTraj;
bool _timeReached, _taskReached, _pastTaskReached, _finalReached;
uint _taskCnt;
};
#endif // CARTESIAN_H

3
include/common/enumClass.h
View File

@ -33,8 +33,7 @@ enum class JointMotorType{
enum class Control{
KEYBOARD,
SDK,
JOYSTICK
SDK
};
#endif // ENUMCLASS_H

5
include/control/CtrlComponents.h
View File

@ -32,18 +32,19 @@ public:
//config
double dt;
bool *running;
Control ctrl;
Control ctrl = Control::SDK;
bool hasGripper;
bool isCollisionOpen;
double collisionTLimit;
bool isPlot;
int trajChoose = 1;
size_t dogType = 1;//1:Aliengo 2:B1
size_t armType = 36;
void geneObj();
void writeData();
private:
void inputProcess(int argc, char** argv);
void configProcess();
std::string ctrl_IP;
uint ctrl_port;

6
include/control/cmdPanel.h
View File

@ -56,12 +56,6 @@ private:
};
/*
必须为长按, deltaValue为每秒改变量
valueAction允许共用按键，例如空格停止
但是正反转、停止键不可重复
*/
class ValueAction : public KeyAction{
public:
ValueAction(std::string cUp, std::string cDown, double deltaValue, double initValue = 0.0);

30
include/control/joystick.h
View File

@ -1,30 +0,0 @@
#ifndef _UNITREE_ARM_JOYSTICK_H_
#define _UNITREE_ARM_JOYSTICK_H_
#include <vector>
#include "control/cmdPanel.h"
#include "message/joystick_common.h"
#include "message/aliengo_common.h"
#include "message/b1_common.h"
#include <math.h>
#include "message/udp.h"
class UnitreeJoystick : public CmdPanel{
public:
UnitreeJoystick( size_t dogType, std::vector<KeyAction*> events,
EmptyAction emptyAction, size_t channelNum = 1,
double dt = 0.002);
~UnitreeJoystick();
private:
void _read();
void _extractCmd();
size_t _dogType;
xRockerBtnDataStruct _keyData;
UDPPort *_udp;
UNITREE_LEGGED_SDK_ALIENGO::HighCmd _udpCmdAliengo;
UNITREE_LEGGED_SDK_ALIENGO::HighState _udpStateAliengo;
UNITREE_LEGGED_SDK_B1::HighCmd _udpCmdB1;
UNITREE_LEGGED_SDK_B1::HighState _udpStateB1;
};
#endif // _UNITREE_ARM_JOYSTICK_H_

1
include/interface/IOInterface.h
View File

@ -5,7 +5,6 @@
#include "message/LowlevelCmd.h"
#include "message/LowlevelState.h"
#include "control/keyboard.h"
#include "control/joystick.h"
#include "common/math/robotics.h"
class IOInterface{

168
include/message/aliengo_common.h
View File

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

13
include/message/arm_common.h
View File

@ -26,19 +26,6 @@ enum class ArmFSMState{
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,

211
include/message/b1_common.h
View File

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

43
include/message/joystick_common.h
View File

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

5
include/model/ArmModel.h
View File

@ -14,7 +14,8 @@ public:
HomoMat forwardKinematics(Vec6 q, int index = 6);
virtual bool inverseKinematics(HomoMat TDes, Vec6 qPast, Vec6& q_result, bool checkInWorkSpace = false);
Mat6 CalcJacobian(Vec6 q);
Mat6 CalcJacobianSpace(Vec6 q);
Mat6 CalcJacobianBody(Vec6 q);
Vec6 inverseDynamics(Vec6 q, Vec6 qd, Vec6 qdd, Vec6 Ftip);
virtual void solveQP(Vec6 twist, Vec6 qPast, Vec6& qd_result, double dt) = 0;
@ -58,7 +59,7 @@ protected:
class Z1Model : public ArmModel{
public:
Z1Model(Vec3 endPosLocal = Vec3::Zero(), double endEffectorMass = 0.0,
Z1Model(size_t armType = 36, Vec3 endPosLocal = Vec3::Zero(), double endEffectorMass = 0.0,
Vec3 endEffectorCom = Vec3::Zero(), Mat3 endEffectorInertia = Mat3::Zero());
~Z1Model(){};

1
include/trajectory/Trajectory.h
View File

@ -41,7 +41,6 @@ protected:
HomoMat _startHomo, _endHomo;
Vec6 _startPosture, _endPosture, _deltaPosture;
double _startGripperQ, _endGripperQ;
};
#endif // TRAJECTORY_H

BIN
lib/libZ1_ROS_Linux64.so
View File

Binary file not shown.

BIN
lib/libZ1_UDP_Linux64.so
View File

Binary file not shown.

20
main.cpp
View File

@ -39,7 +39,7 @@ int main(int argc, char **argv){
/* set real-time process */
setProcessScheduler();
/* set the print format */
std::cout << std::fixed << std::setprecision(3);
std::cout << std::fixed << std::setprecision(5);
EmptyAction emptyAction((int)ArmFSMStateName::INVALID);
@ -80,22 +80,6 @@ int main(int argc, char **argv){
events.push_back(new ValueAction("down", "up", 1.));
ctrlComp->cmdPanel = new Keyboard(events, emptyAction);
}else if(ctrlComp->ctrl == Control::JOYSTICK){
events.push_back(new StateAction("r2x", (int)ArmFSMStateName::TRAJECTORY));
events.push_back(new StateAction("l12", (int)ArmFSMStateName::PASSIVE));
events.push_back(new StateAction("r2", (int)ArmFSMStateName::JOINTCTRL));
events.push_back(new StateAction("r1", (int)ArmFSMStateName::CARTESIAN));
events.push_back(new StateAction("select", (int)ArmFSMStateName::BACKTOSTART));
events.push_back(new ValueAction("left_up", "left_down", 0.5));//Tran_Y
events.push_back(new ValueAction("left_right", "left_left", 0.5));//Tran_X, inverse
events.push_back(new ValueAction("up", "down", -0.5));//Tran_Z, inverse
events.push_back(new ValueAction("right_up", "right_down", 0.5));//Rot_Y
events.push_back(new ValueAction("right_left", "right_right", 0.5));//Rot_x
events.push_back(new ValueAction("Y", "A", 0.5));//Rot_Z
events.push_back(new ValueAction("right", "left", 1.0));//girpper, close-open
ctrlComp->cmdPanel = new UnitreeJoystick(ctrlComp->dogType, events, emptyAction);
}
std::vector<FSMState*> states;
states.push_back(new State_Passive(ctrlComp));
@ -112,7 +96,7 @@ int main(int argc, char **argv){
states.push_back(new State_ToState(ctrlComp));
states.push_back(new State_Trajectory(ctrlComp));
states.push_back(new State_Calibration(ctrlComp));
states.push_back(new State_SetTraj(ctrlComp));
// states.push_back(new State_SetTraj(ctrlComp));
FiniteStateMachine *fsm;
fsm = new FiniteStateMachine(states, ctrlComp);