diff --git a/examples/highcmd_basic.cpp b/examples/highcmd_basic.cpp index d66605b..4d54e12 100644 --- a/examples/highcmd_basic.cpp +++ b/examples/highcmd_basic.cpp @@ -62,7 +62,7 @@ void Z1ARM::printState(){ std::cout<<"tauState: "<getTau().transpose()<endPosture.transpose()<dt); usleep(2000); } arm.backToStart(); arm.setFsm(UNITREE_ARM::ArmFSMState::PASSIVE); + arm.sendRecvThread->shutdown(); return 0; } \ No newline at end of file diff --git a/examples_py/arm_python_interface.cpp b/examples_py/arm_python_interface.cpp index 2fafc5b..af91607 100644 --- a/examples_py/arm_python_interface.cpp +++ b/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_(m, "ArmFSMState") .value("INVALID", ArmFSMState::INVALID) .value("PASSIVE", ArmFSMState::PASSIVE) @@ -32,14 +34,47 @@ PYBIND11_MODULE(unitree_arm_interface, m){ ; py::class_(m, "LowlevelState") - .def("getQ", &LowlevelState::getQ, py::return_value_policy::reference_internal) - .def("getQd", &LowlevelState::getQd, py::return_value_policy::reference_internal) - .def("getQdd", &LowlevelState::getQdd, py::return_value_policy::reference_internal) - .def("getQTau", &LowlevelState::getTau, py::return_value_policy::reference_internal) + .def("getQ", &LowlevelState::getQ, rvp::reference_internal) + .def("getQd", &LowlevelState::getQd, rvp::reference_internal) + .def("getQdd", &LowlevelState::getQdd, rvp::reference_internal) + .def("getQTau", &LowlevelState::getTau, rvp::reference_internal) + ; + + py::class_(m, "CtrlComponents") + .def_readwrite("armModel", &CtrlComponents::armModel) + .def_readonly("dt", &CtrlComponents::dt) + ; + + py::class_(m, "Z1Model") + .def(py::init()) + .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_(m, "ArmInterface") - .def(py::init()) + .def(py::init(), py::arg("hasGripper")=true) .def_readwrite("q", &ArmInterface::q) .def_readwrite("qd", &ArmInterface::qd) .def_readwrite("tau", &ArmInterface::tau) diff --git a/examples_py/example_highcmd.py b/examples_py/example_highcmd.py index ff4ab8b..111ec19 100644 --- a/examples_py/example_highcmd.py +++ b/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) +arm = unitree_arm_interface.ArmInterface(hasGripper=True) 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() diff --git a/examples_py/example_lowcmd.py b/examples_py/example_lowcmd.py index d3ac998..2b7723d 100644 --- a/examples_py/example_lowcmd.py +++ b/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) +arm = unitree_arm_interface.ArmInterface(hasGripper=True) +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()) - time.sleep(0.002) + # print(arm.lowstate.getQ()) + time.sleep(arm._ctrlComp.dt) arm.loopOn() arm.backToStart() diff --git a/examples_py/example_model.py b/examples_py/example_model.py new file mode 100644 index 0000000..7478a39 --- /dev/null +++ b/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) diff --git a/include/unitree_arm_sdk/control/ctrlComponents.h b/include/unitree_arm_sdk/control/ctrlComponents.h index 555f413..5e1f273 100644 --- a/include/unitree_arm_sdk/control/ctrlComponents.h +++ b/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 diff --git a/include/unitree_arm_sdk/control/unitreeArm.h b/include/unitree_arm_sdk/control/unitreeArm.h index cec22b6..627d6a5 100644 --- a/include/unitree_arm_sdk/control/unitreeArm.h +++ b/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 diff --git a/include/unitree_arm_sdk/message/LowlevelState.h b/include/unitree_arm_sdk/message/LowlevelState.h index 665564f..cbcecf5 100644 --- a/include/unitree_arm_sdk/message/LowlevelState.h +++ b/include/unitree_arm_sdk/message/LowlevelState.h @@ -25,7 +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 @@ -33,7 +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 cd851d8..1882a4a 100644 --- a/include/unitree_arm_sdk/model/ArmModel.h +++ b/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 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 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 q, Eigen::Ref qd); std::vector getJointQMax() {return _jointQMax;} std::vector getJointQMin() {return _jointQMin;} std::vector getJointSpeedMax() {return _jointSpeedMax;} @@ -117,9 +119,11 @@ 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 @@ -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 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 qd_result, double dt); private: diff --git a/lib/libZ1_SDK_Linux64.so b/lib/libZ1_SDK_Linux64.so index 908ae87..23f3eb7 100644 Binary files a/lib/libZ1_SDK_Linux64.so and b/lib/libZ1_SDK_Linux64.so differ