add spaces between comment of functions
This commit is contained in:
Agnel Wang
parent
756d06f7da
commit
f584f1311c
|
|
@ -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,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]'
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
Vec3 endEffectorCom = Vec3::Zero(), Mat3 endEffectorInertia = Mat3::Zero());
|
||||
~Z1Model(){};
|
||||
Z1Model(Vec3 endPosLocal = Vec3::Zero(), double endEffectorMass = 0.0,
|
||||
Vec3 endEffectorCom = Vec3::Zero(), Mat3 endEffectorInertia = Mat3::Zero());
|
||||
~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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue