diff --git a/include/unitree_arm_sdk/control/ctrlComponents.h b/include/unitree_arm_sdk/control/ctrlComponents.h index 5e1f273..555f413 100644 --- a/include/unitree_arm_sdk/control/ctrlComponents.h +++ b/include/unitree_arm_sdk/control/ctrlComponents.h @@ -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 diff --git a/include/unitree_arm_sdk/control/unitreeArm.h b/include/unitree_arm_sdk/control/unitreeArm.h index cf823f4..90eedc3 100644 --- a/include/unitree_arm_sdk/control/unitreeArm.h +++ b/include/unitree_arm_sdk/control/unitreeArm.h @@ -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]' diff --git a/include/unitree_arm_sdk/message/LowlevelCmd.h b/include/unitree_arm_sdk/message/LowlevelCmd.h index 15a8d4e..ab9a431 100644 --- a/include/unitree_arm_sdk/message/LowlevelCmd.h +++ b/include/unitree_arm_sdk/message/LowlevelCmd.h @@ -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 KP, std::vector KW); + /* q = qInput */ void setQ(std::vector 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 q, without gripper */ Vec6 getQ(); + /* return Vec6 from std::vector dq, without gripper */ Vec6 getQd(); }; diff --git a/include/unitree_arm_sdk/message/LowlevelState.h b/include/unitree_arm_sdk/message/LowlevelState.h index 659f5b6..665564f 100644 --- a/include/unitree_arm_sdk/message/LowlevelState.h +++ b/include/unitree_arm_sdk/message/LowlevelState.h @@ -25,6 +25,7 @@ public: std::vector tau; std::vector 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 errorstate; + /* * 0: OK * 1: communication between lower computer and motor disconnect once diff --git a/include/unitree_arm_sdk/model/ArmModel.h b/include/unitree_arm_sdk/model/ArmModel.h index 9337bea..cd851d8 100644 --- a/include/unitree_arm_sdk/model/ArmModel.h +++ b/include/unitree_arm_sdk/model/ArmModel.h @@ -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 getJointQMax() {return _jointQMax;} std::vector getJointQMin() {return _jointQMin;} std::vector 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;