add python-armModel

examples/highcmd_basic.cpp

@@ -62,7 +62,7 @@ void Z1ARM::printState(){
     std::cout<<"tauState: "<<lowstate->getTau().transpose()<<std::endl;
 
     std::cout<<"------ Endeffector Cartesian Posture ------"<<std::endl;
-    std::cout<<"rx ry rz x y z"<<std::endl;
+    std::cout<<"roll pitch yaw x y z"<<std::endl;
     std::cout<<lowstate->endPosture.transpose()<<std::endl;
 }
 

examples/highcmd_development.cpp

@@ -14,11 +14,12 @@ int main(){
     for(int i=0; i<duration; i++)
     {
         arm.q = lastPos*(1-i/duration) + targetPos*(i/duration);
-        arm.qd = (targetPos-lastPos)/(duration*0.002);
+        arm.qd = (targetPos-lastPos)/(duration*arm._ctrlComp->dt);
         usleep(2000);
     }
 
     arm.backToStart();
     arm.setFsm(UNITREE_ARM::ArmFSMState::PASSIVE);
+    arm.sendRecvThread->shutdown();
     return 0;
 }

examples_py/arm_python_interface.cpp

@@ -23,6 +23,8 @@ public:
 
 namespace py = pybind11;
 PYBIND11_MODULE(unitree_arm_interface, m){
+    using rvp = py::return_value_policy;
+
     py::enum_<ArmFSMState>(m, "ArmFSMState")
         .value("INVALID", ArmFSMState::INVALID)
         .value("PASSIVE", ArmFSMState::PASSIVE)
@@ -32,14 +34,47 @@ PYBIND11_MODULE(unitree_arm_interface, m){
         ;
 
     py::class_<LowlevelState>(m, "LowlevelState")
-        .def("getQ", &LowlevelState::getQ, py::return_value_policy::reference_internal)
+        .def("getQ", &LowlevelState::getQ, rvp::reference_internal)
-        .def("getQd", &LowlevelState::getQd, py::return_value_policy::reference_internal)
+        .def("getQd", &LowlevelState::getQd, rvp::reference_internal)
-        .def("getQdd", &LowlevelState::getQdd, py::return_value_policy::reference_internal)
+        .def("getQdd", &LowlevelState::getQdd, rvp::reference_internal)
-        .def("getQTau", &LowlevelState::getTau, py::return_value_policy::reference_internal)
+        .def("getQTau", &LowlevelState::getTau, rvp::reference_internal)
+        ;
+
+    py::class_<CtrlComponents>(m, "CtrlComponents")
+        .def_readwrite("armModel", &CtrlComponents::armModel)
+        .def_readonly("dt", &CtrlComponents::dt)
+        ;
+
+    py::class_<Z1Model>(m, "Z1Model")
+        .def(py::init<Vec3, double, Vec3, Mat3>())
+        .def("checkInSingularity", &Z1Model::checkInSingularity)
+        // Pass-by-reference does not work in this method. 
+        // [incompatible function arguments.] Z1Model.jointProtect(arm.q, arm.qd)
+        //.def("jointProtect", &Z1Model::jointProtect) 
+        .def("jointProtect", [](Z1Model& self, Vec6 q, Vec6 qd){
+            self.jointProtect(q, qd);
+            return std::make_pair(q, qd);
+        })
+        .def("getJointQMax", &Z1Model::getJointQMax, rvp::reference_internal)
+        .def("getJointQMin", &Z1Model::getJointQMin, rvp::reference_internal)
+        .def("getJointSpeedMax", &Z1Model::getJointSpeedMax, rvp::reference_internal)
+        .def("inverseKinematics", [](Z1Model& self, HomoMat Tdes, Vec6 qPast, bool checkInWorkSpace){
+            Vec6 q_result;
+            bool hasIK = self.inverseKinematics(Tdes, qPast, q_result, checkInWorkSpace);
+            return std::make_pair(hasIK, q_result);
+        })
+        .def("forwardKinematics", &Z1Model::forwardKinematics)
+        .def("inverseDynamics", &Z1Model::inverseDynamics)
+        .def("CalcJacobian", &Z1Model::CalcJacobian)
+        .def("solveQP", [](Z1Model& self, Vec6 twist, Vec6 qPast, double dt){
+            Vec6 qd_result;
+            self.solveQP(twist, qPast, qd_result, dt);
+            return qd_result;
+        })
         ;
 
     py::class_<ArmInterface>(m, "ArmInterface")
-        .def(py::init<bool>())
+        .def(py::init<bool>(), py::arg("hasGripper")=true)
         .def_readwrite("q", &ArmInterface::q)
         .def_readwrite("qd", &ArmInterface::qd)
         .def_readwrite("tau", &ArmInterface::tau)

examples_py/example_highcmd.py

@@ -5,8 +5,7 @@ import time
 import numpy as np
 
 np.set_printoptions(precision=3, suppress=True)
-hasGripper = True
+arm =  unitree_arm_interface.ArmInterface(hasGripper=True)
-arm =  unitree_arm_interface.ArmInterface(hasGripper)
 armState = unitree_arm_interface.ArmFSMState
 arm.loopOn()
 
@@ -15,7 +14,7 @@ arm.labelRun("forward")
 arm.startTrack(armState.JOINTCTRL)
 for i in range(0, 1000):
     arm.jointCtrlCmd(np.array([0,0,0,-1,0,0,-1]), 0.5)
-    time.sleep(0.002)
+    time.sleep(arm._ctrlComp.dt)
 
 # 2. highcmd_basic : armCtrlByFSM
 arm.labelRun("forward")
@@ -28,7 +27,7 @@ arm.labelRun("forward")
 arm.startTrack(armState.CARTESIAN)
 for i in range(0, 1000):
     arm.cartesianCtrlCmd(np.array([0,0,0,0,0,-1,-1]), 0.3, 0.1)
-    time.sleep(0.002)
+    time.sleep(arm._ctrlComp.dt)
 
 arm.backToStart()
 arm.loopOff()

examples_py/example_lowcmd.py

@@ -5,8 +5,8 @@ import time
 import numpy as np
 
 np.set_printoptions(precision=3, suppress=True)
-hasGripper = True
+arm = unitree_arm_interface.ArmInterface(hasGripper=True)
-arm = unitree_arm_interface.ArmInterface(hasGripper)
+armModel = arm._ctrlComp.armModel
 arm.setFsmLowcmd()
 
 duration = 1000
@@ -16,11 +16,11 @@ targetPos = np.array([0.0, 1.5, -1.0, -0.54, 0.0, 0.0]) #forward
 for i in range(0, duration):
     arm.q = lastPos*(1-i/duration) + targetPos*(i/duration)# set position
     arm.qd = (targetPos-lastPos)/(duration*0.002) # set velocity
-    arm.tau = np.zeros(6) # set torque
+    arm.tau = armModel.inverseDynamics(arm.q, arm.qd, np.zeros(6), np.zeros(6)) # set torque
     arm.gripperQ = -1*(i/duration)
     arm.sendRecv()# udp connection
-    print(arm.lowstate.getQ())
+    # print(arm.lowstate.getQ())
-    time.sleep(0.002)
+    time.sleep(arm._ctrlComp.dt)
 
 arm.loopOn()
 arm.backToStart()

examples_py/example_model.py

@@ -0,0 +1,40 @@
+import sys
+sys.path.append("../lib")
+import unitree_arm_interface
+import numpy as np
+
+np.set_printoptions(precision=3, suppress=True)
+arm =  unitree_arm_interface.ArmInterface(hasGripper = True)
+armModel = arm._ctrlComp.armModel
+
+print('--------------------------FK & IK------------------------')
+q_FORWARD = np.array([0, 1.5, -1, 0.54, 0, 0])
+q_near_forward = np.array([0, 1.49, -1, 0.57, 0, 0])
+# 1. FK
+T_forward = armModel.forwardKinematics(q_FORWARD, 6)
+# 2. IK, q_result doesn't need to be near qPast
+hasIK, q_forward = armModel.inverseKinematics(T_forward, np.zeros(6), True)
+if hasIK:
+    print("The joint angles corresponding to position FORWRAD:")
+    print(q_forward)
+else:
+    print("no IK")
+# 2. IK, q_result should to be near qPast
+hasIK, q_forward = armModel.inverseKinematics(T_forward, np.zeros(6), False)
+print(hasIK)
+hasIK, q_forward = armModel.inverseKinematics(T_forward, q_near_forward, False)
+print(hasIK, '\n')
+
+print('--------------------------ID------------------------')
+tau = armModel.inverseDynamics(np.zeros(6), np.zeros(6), np.zeros(6), np.zeros(6))
+print("The torque required by the z1 arm at the homo position to resist gravity:")
+print(tau, '\n')
+
+print('--------------------------jacobian------------------------')
+print("compute: V = J * qd")
+J = armModel.CalcJacobian(q_FORWARD)
+spatialtwist = np.array([0, 0, 0, 1., 0, 0])
+qd = np.linalg.inv(J).dot(spatialtwist)
+print("solved by jacobian: ", qd)
+qd = armModel.solveQP(spatialtwist, q_near_forward, arm._ctrlComp.dt)
+print("solved by QP:       ", qd)

include/unitree_arm_sdk/control/ctrlComponents.h

@@ -13,8 +13,6 @@ struct CtrlComponents{
 public:
     CtrlComponents(double deltaT, bool hasUnitreeGripper);
     ~CtrlComponents();
-
-
 /*
  * Function: send udp message to z1_ctrl and receive udp message from it
  * Input:    None
@@ -23,8 +21,6 @@ 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
@@ -33,8 +29,6 @@ public:
  * Output:   None
  */
     void armCtrl(Vec6 q, Vec6 qd, Vec6 tau);
-
-    
 /*
  * Function: Set gripper commands to class lowcmd
  * Input:    q:  joint angle

include/unitree_arm_sdk/control/unitreeArm.h

@@ -10,6 +10,7 @@ public:
 unitreeArm(bool hasUnitreeGripper);
 ~unitreeArm();
 
+
 /*
  * Function: Change z1_ctrl state to fsm, wait until change complete
  * Input:    ArmFSMState
@@ -81,7 +82,7 @@ void calibration();
 
 /*
  * Function: Move the robot in a joint path
- * Input:    posture: target position, (rx ry rz x y z), unit: meter
+ * Input:    posture: target position, (roll pitch yaw x y z), unit: meter
  *           maxSpeed: the maximum joint speed when robot is moving, unit: radian/s
  *             range:[0, pi]
  * Output:   None
@@ -91,7 +92,7 @@ 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
+ * Input:    posture: target position, (roll pitch yaw x y z), unit: meter
  *           gripperPos: target angular
  *             uint: radian
  *             range:[-pi/2, 0]
@@ -105,7 +106,7 @@ 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
+ * Input:    posture: target position, (roll pitch yaw x y z), unit: meter
  *           maxSpeed: the maximum joint speed when robot is moving, unit: m/s
  * Output:   whether posture has inverse kinematics
  */
@@ -114,7 +115,7 @@ 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
+ * Input:    posture: target position, (roll pitch yaw x y z), unit: meter
  *           gripperPos: target angular, uint: radian
  *             range:[-pi/2, 0]
  *           maxSpeed: the maximum joint speed when robot is moving, unit: m/s
@@ -126,7 +127,7 @@ 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
- *           endPosture: target position, (rx ry rz x y z), unit: meter
+ *           endPosture: target position, (roll pitch yaw x y z), unit: meter
  *           maxSpeed: the maximum joint speed when robot is moving, unit: m/s
  * Output:   whether posture has inverse kinematics
  */
@@ -136,7 +137,7 @@ 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
- *           endPosture: target position, (rx ry rz x y z), unit: meter
+ *           endPosture: target position, (roll pitch yaw x y z), unit: meter
  *           gripperPos: target angular, uint: radian
  *             range:[-pi/2, 0]
  *           maxSpeed: the maximum joint speed when robot is moving, unit: m/s
@@ -220,7 +221,7 @@ void jointCtrlCmd(Vec7 directions, double jointSpeed);
 /*
  * Function: set spatial velocity command automatically by input parameters
  * Input:    directions: movement directions [include gripper], range:[-1,1]
- *                       rx, ry, rz, x, y, z, gripper
+ *                       roll, pitch, yaw, x, y, z, gripper
  *           oriSpeed: range: [0, 0.6]
  *           posSpeed: range: [0, 0.3]
  *                  gripper joint speed is set to 1.0

include/unitree_arm_sdk/model/ArmModel.h

@@ -12,6 +12,8 @@ class ArmModel{
 public:
     ArmModel(Vec3 endPosLocal, double endEffectorMass, Vec3 endEffectorCom, Mat3 endEffectorInertia);
     ~ArmModel(){};
+
+    
 /*
  * Function: compute end effector frame (used for current spatial position calculation)
  * Inputs: q: current joint angles
@@ -41,7 +43,7 @@ HomoMat forwardKinematics(Vec6 q, int index = 6);
  *                   number of maximum iterations without finding a solution
  *          q_result: Joint angles that achieve T within the specified tolerances,
  */
-virtual bool inverseKinematics(HomoMat TDes, Vec6 qPast, Vec6& q_result, bool checkInWorkSpace = false);
+virtual bool inverseKinematics(HomoMat TDes, Vec6 qPast, Eigen::Ref<Vec6> q_result, bool checkInWorkSpace = false);
 
 
 /*
@@ -61,7 +63,7 @@ Mat6 CalcJacobian(Vec6 q);
  * Returns: required joint forces/torques
  */
 Vec6 inverseDynamics(Vec6 q, Vec6 qd, Vec6 qdd, Vec6 Ftip);
-virtual void solveQP(Vec6 twist, Vec6 qPast, Vec6& qd_result, double dt) = 0;
+virtual void solveQP(Vec6 twist, Vec6 qPast, Eigen::Ref<Vec6> qd_result, double dt) = 0;
 virtual bool checkInSingularity(Vec6 q) = 0;
 
 
@@ -71,7 +73,7 @@ virtual bool checkInSingularity(Vec6 q) = 0;
  *         qd: set in range[-_jointSpeedMax, _jointSpeedMax]
  * Returns: None
  */
-void jointProtect(Vec6& q, Vec6& qd);
+void jointProtect(Eigen::Ref<Vec6> q, Eigen::Ref<Vec6> qd);
 std::vector<double> getJointQMax() {return _jointQMax;}
 std::vector<double> getJointQMin() {return _jointQMin;}
 std::vector<double> getJointSpeedMax() {return _jointSpeedMax;}
@@ -117,9 +119,11 @@ 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
@@ -146,7 +150,7 @@ bool checkInSingularity(Vec6 q);
  *                   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, Eigen::Ref<Vec6> q_result, bool checkInWorkSpace = false);
 
 
 /*
@@ -156,7 +160,7 @@ bool inverseKinematics(HomoMat TDes, Vec6 qPast, Vec6& q_result, bool checkInWor
  *         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, Eigen::Ref<Vec6> qd_result, double dt);
 
 
 private: