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

add spaces between comment of functions

This commit is contained in:
Agnel Wang 2023-02-01 20:24:51 +08:00
parent 756d06f7da
commit f584f1311c
5 changed files with 65 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/unitree_arm_sdk/control/ctrlComponents.h
View File

@ -13,6 +13,8 @@ struct CtrlComponents{
public:
CtrlComponents(double deltaT, bool hasUnitreeGripper);
~CtrlComponents();
/*
* Function: send udp message to z1_ctrl and receive udp message from it
* Input: None
@ -21,6 +23,8 @@ public:
* and call udp->recv() to store datas from z1_ctrl into lowstate
*/
void sendRecv();
/*
* Function: Set six joints commands to class lowcmd
* Input: q: joint angle
@ -29,6 +33,8 @@ public:
* Output: None
*/
void armCtrl(Vec6 q, Vec6 qd, Vec6 tau);
/*
* Function: Set gripper commands to class lowcmd
* Input: q: joint angle

18
include/unitree_arm_sdk/control/unitreeArm.h
View File

@ -18,6 +18,7 @@ unitreeArm(bool hasUnitreeGripper);
*/
bool setFsm(ArmFSMState fsm);
/*
* Function: Move arm to home position
* wait until arrival home position, and then switch to State_JointCtrl
@ -26,6 +27,7 @@ bool setFsm(ArmFSMState fsm);
*/
void backToStart();
/*
* Function: Move arm to label position
* wait until arrival label position, and then switch to State_JointCtrl
@ -36,6 +38,7 @@ void backToStart();
*/
void labelRun(std::string label);
/*
* Function: Save current position as a label to saveArmStates.csv
* Switch to State_JointCtrl when done
@ -46,6 +49,7 @@ void labelRun(std::string label);
*/
void labelSave(std::string label);
/*
* Function: Save current position as a label to saveArmStates.csv
* Switch to State_JointCtrl when done
@ -56,6 +60,7 @@ void labelSave(std::string label);
*/
void teach(std::string label);
/*
* Function: Switch to State_Teach
* Input: label
@ -65,6 +70,7 @@ void teach(std::string label);
*/
void teachRepeat(std::string label);
/*
* Function: Calibrate the motor, make current position as home position
* Input: None
@ -72,6 +78,7 @@ void teachRepeat(std::string label);
*/
void calibration();
/*
* Function: Move the robot in a joint path
* Input: posture: target position, (rx ry rz x y z), unit: meter
@ -81,6 +88,7 @@ void calibration();
*/
bool MoveJ(Vec6 posture, double maxSpeed);
/*
* Function: Move the robot in a joint path, and control the gripper at the same time
* Input: posture: target position, (rx ry rz x y z), unit: meter
@ -94,6 +102,7 @@ bool MoveJ(Vec6 posture, double maxSpeed);
*/
bool MoveJ(Vec6 posture, double gripperPos, double maxSpeed);
/*
* Function: Move the robot in a linear path
* Input: posture: target position, (rx ry rz x y z), unit: meter
@ -102,6 +111,7 @@ bool MoveJ(Vec6 posture, double gripperPos, double maxSpeed);
*/
bool MoveL(Vec6 posture, double maxSpeed);
/*
* Function: Move the robot in a linear path, and control the gripper at the same time
* Input: posture: target position, (rx ry rz x y z), unit: meter
@ -112,6 +122,7 @@ bool MoveL(Vec6 posture, double maxSpeed);
*/
bool MoveL(Vec6 posture, double gripperPos, double maxSpeed);
/*
* Function: Move the robot in a circular path
* Input: middle posture: determine the shape of the circular path
@ -121,6 +132,7 @@ bool MoveL(Vec6 posture, double gripperPos, double maxSpeed);
*/
bool MoveC(Vec6 middlePosutre, Vec6 endPosture, double maxSpeed);
/*
* Function: Move the robot in a circular path, and control the gripper at the same time
* Input: middle posture: determine the shape of the circular path
@ -132,6 +144,7 @@ bool MoveC(Vec6 middlePosutre, Vec6 endPosture, double maxSpeed);
*/
bool MoveC(Vec6 middlePosutre, Vec6 endPosture, double gripperPos, double maxSpeed);
/*
* Function: Control robot with q&qd command in joint space or posture command in cartesian space
* Input: fsm: ArmFSMState::JOINTCTRL or ArmFSMState::CARTESIAN
@ -152,6 +165,7 @@ bool MoveC(Vec6 middlePosutre, Vec6 endPosture, double gripperPos, double maxSpe
*/
void startTrack(ArmFSMState fsm);
/*
* Function: send udp message to z1_ctrl and receive udp message from it
* Input: None
@ -167,6 +181,7 @@ void startTrack(ArmFSMState fsm);
*/
void sendRecv();
/*
* Function: whether to wait for the command to finish
* Input: true or false
@ -185,6 +200,7 @@ void sendRecv();
*/
void setWait(bool Y_N);
/*
* Function: set q & qd command automatically by input parameters
* Input: directions: movement directions [include gripper], range:[-1,-1]
@ -200,6 +216,7 @@ void setWait(bool Y_N);
*/
void jointCtrlCmd(Vec7 directions, double jointSpeed);
/*
* Function: set spatial velocity command automatically by input parameters
* Input: directions: movement directions [include gripper], range:[-1,-1]
@ -222,6 +239,7 @@ void jointCtrlCmd(Vec7 directions, double jointSpeed);
*/
void cartesianCtrlCmd(Vec7 directions, double oriSpeed, double posSpeed);
//command parameters
Vec6 q, qd, tau;
Vec6 twist;//spatial velocity: [omega, v]'

17
include/unitree_arm_sdk/message/LowlevelCmd.h
View File

@ -20,30 +20,41 @@ public:
LowlevelCmd();
~LowlevelCmd(){}
/* dq = {0} */
void setZeroDq();
/* tau = {0} */
void setZeroTau();
/* kp = {0} */
void setZeroKp();
/* kd = {0} */
void setZeroKd();
/*
* set default arm kp & kd (Only used in State_LOWCMD)
* kp = [20, 30, 30, 20, 15, 10]
* kd = [2000, 2000, 2000, 2000, 2000, 2000]
*/
void setControlGain();
/* kp = KP, kd = KW */
void setControlGain(std::vector<double> KP, std::vector<double> KW);
/* q = qInput */
void setQ(std::vector<double> qInput);
/* q = qInput */
void setQ(VecX qInput);
/* dq = qDInput */
void setQd(VecX qDInput);
/* tau = tauInput */
void setTau(VecX tauInput);
/*
* setZeroDq()
* setZeroTau()
@ -51,19 +62,23 @@ void setTau(VecX tauInput);
* kd = [10, 10, 10, 10, 10, 10]
*/
void setPassive();
/*
* set default gripper kp & kd (Only used in State_LOWCMD)
* kp.at(kp.size()-1) = 20
* kd.at(kd.size()-1) = 2000
*/
void setGripperGain();
/*
* kp.at(kp.size()-1) = KP
* kd.at(kd.size()-1) = KW
*/
void setGripperGain(double KP, double KW);
/* set gripper kp&kd to zero */
void setGripperZeroGain();
/* q.at(q.size()-1) = qInput; */
void setGripperQ(double qInput);
double getGripperQ() {return q.at(q.size()-1);}
@ -71,8 +86,10 @@ void setGripperQd(double qdInput) {dq.at(dq.size()-1) = qdInput;}
double getGripperQd() {return dq.at(dq.size()-1);}
void setGripperTau(double tauInput) {tau.at(tau.size()-1) = tauInput;}
double getGripperTau() {return tau.at(tau.size()-1);}
/* return Vec6 from std::vector<double> q, without gripper */
Vec6 getQ();
/* return Vec6 from std::vector<double> dq, without gripper */
Vec6 getQd();
};

2
include/unitree_arm_sdk/message/LowlevelState.h
View File

@ -25,6 +25,7 @@ public:
std::vector<double> tau;
std::vector<int> temperature;
/* 0x01 : phase current is too large
* 0x02 : phase leakage
* 0x04 : motor winding overheat or temperature is too large
@ -32,6 +33,7 @@ public:
* 0x40 : Ignore
*/
std::vector<uint8_t> errorstate;
/*
* 0: OK
* 1: communication between lower computer and motor disconnect once

26
include/unitree_arm_sdk/model/ArmModel.h
View File

@ -22,6 +22,8 @@ public:
* at the specified coordinates
*/
HomoMat forwardKinematics(Vec6 q, int index = 6);
/*
* Function: Computes inverse kinematics in the space frame with the irerative approach
* Inputs: TDes: The desired end-effector configuration
@ -40,12 +42,16 @@ HomoMat forwardKinematics(Vec6 q, int index = 6);
* q_result: Joint angles that achieve T within the specified tolerances,
*/
virtual bool inverseKinematics(HomoMat TDes, Vec6 qPast, Vec6& q_result, bool checkInWorkSpace = false);
/*
* Function: Gives the space Jacobian
* Inputs: q: current joint angles
* Returns: 6x6 Spatial Jacobian
*/
Mat6 CalcJacobian(Vec6 q);
/*
* Function: This function uses forward-backward Newton-Euler iterations to caculate inverse dynamics
* Inputs: q: joint angles
@ -57,6 +63,8 @@ Mat6 CalcJacobian(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;
virtual bool checkInSingularity(Vec6 q) = 0;
/*
* Function: Limit q & qd inputs to valid values
* Inputs: q: set in range[_jointQMin, _jointQMax]
@ -67,6 +75,8 @@ void jointProtect(Vec6& q, Vec6& qd);
std::vector<double> getJointQMax() {return _jointQMax;}
std::vector<double> getJointQMin() {return _jointQMin;}
std::vector<double> getJointSpeedMax() {return _jointSpeedMax;}
/*
* Function: The load is applied to the end joint in equal proportion
and caculates the correspoding dynamic parameters
@ -107,16 +117,18 @@ protected:
class Z1Model : public ArmModel{
public:
Z1Model(Vec3 endPosLocal = Vec3::Zero(), double endEffectorMass = 0.0,
Z1Model(Vec3 endPosLocal = Vec3::Zero(), double endEffectorMass = 0.0,
Vec3 endEffectorCom = Vec3::Zero(), Mat3 endEffectorInertia = Mat3::Zero());
~Z1Model(){};
~Z1Model(){};
/*
* Function: Check whether joint1 and joint5 is coaxial
* x5^2 + y5^2 < 0.1^2
* Inputs: q: current joint variables
* Returns: bool
*/
bool checkInSingularity(Vec6 q);
bool checkInSingularity(Vec6 q);
/*
* Function: Computes inverse kinematics in the space frame with the analytical approach
* Inputs: TDes: The desired end-effector configuration
@ -134,7 +146,9 @@ public:
* number of maximum iterations without finding a solution
* q_result: Joint angles that achieve T within the specified tolerances,
*/
bool inverseKinematics(HomoMat TDes, Vec6 qPast, Vec6& q_result, bool checkInWorkSpace = false);
bool inverseKinematics(HomoMat TDes, Vec6 qPast, Vec6& q_result, bool checkInWorkSpace = false);
/*
* Function: The function use quadprog++ to slove equation: qd = J.inverse() * twist, even if J has no inverse
* Inputs: twist: spatial velocity [R_dot, p_dot]
@ -142,7 +156,9 @@ public:
* dt : compute period
* Returns: qd_result: joint velocity that are corresponding to twist
*/
void solveQP(Vec6 twist, Vec6 qPast, Vec6& qd_result, double dt);
void solveQP(Vec6 twist, Vec6 qPast, Vec6& qd_result, double dt);
private:
void setParam_V3_6();
double _theta2Bias;