z1_sdk/examples_py/arm_python_interface.cpp at 4172cad2d4d230221a6d20955fed9db9c2f8bd8e · unitreerobotics/z1_sdk · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/stl.h>
#include "unitree_arm_sdk/control/unitreeArm.h"

using namespace UNITREE_ARM;

class ArmInterface : public unitreeArm
{
public:
    ArmInterface(bool hasGripper):unitreeArm(hasGripper){};
    ~ArmInterface(){};
    void loopOn() { sendRecvThread->start();}
    void loopOff() { sendRecvThread->shutdown();}
    void setFsmLowcmd()
    {
        sendRecvThread->start();
        setFsm(ArmFSMState::PASSIVE);
        setFsm(ArmFSMState::LOWCMD);
        sendRecvThread->shutdown();
    }
};

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)
        .value("LOWCMD", ArmFSMState::LOWCMD)
        .value("JOINTCTRL", ArmFSMState::JOINTCTRL)
        .value("CARTESIAN", ArmFSMState::CARTESIAN)
        ;

    py::class_<LowlevelState>(m, "LowlevelState")
        .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_<UDPPort>(m, "UDPPort")
            .def(py::init<std::string, uint, uint>(),
                    py::arg("IP") = "127.0.0.1",
                    py::arg("toPort") = 8071,
                    py::arg("ownPort") = 8072
                    )
            ;

    py::class_<CtrlComponents>(m, "CtrlComponents")
        .def_readwrite("armModel", &CtrlComponents::armModel)
        .def_readwrite("udp", &CtrlComponents::udp)
        .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>(), py::arg("hasGripper")=true)
        .def_readwrite("q", &ArmInterface::q)
        .def_readwrite("qd", &ArmInterface::qd)
        .def_readwrite("tau", &ArmInterface::tau)
        .def_readwrite("gripperQ", &ArmInterface::gripperQ)
        .def_readwrite("gripperQd", &ArmInterface::gripperW)
        .def_readwrite("gripperTau", &ArmInterface::gripperTau)
        .def_readwrite("lowstate", &ArmInterface::lowstate)
        .def_readwrite("_ctrlComp", &ArmInterface::_ctrlComp)
        .def("setFsmLowcmd", &ArmInterface::setFsmLowcmd)
        .def("loopOn", &ArmInterface::loopOn)
        .def("loopOff", &ArmInterface::loopOff)
        .def("setFsm", &ArmInterface::setFsm)
        .def("backToStart", &ArmInterface::backToStart)
        .def("labelRun", &ArmInterface::labelRun)
        .def("labelSave", &ArmInterface::labelSave)
        .def("teach", &ArmInterface::teach)
        .def("teachRepeat", &ArmInterface::teachRepeat)
        .def("calibration", &ArmInterface::calibration)
        .def("MoveJ", py::overload_cast<Vec6, double>(&ArmInterface::MoveJ))
        .def("MoveJ", py::overload_cast<Vec6, double, double>(&ArmInterface::MoveJ))
        .def("MoveL", py::overload_cast<Vec6, double>(&ArmInterface::MoveL))
        .def("MoveL", py::overload_cast<Vec6, double, double>(&ArmInterface::MoveL))
        .def("MoveC", py::overload_cast<Vec6, Vec6, double>(&ArmInterface::MoveC))
        .def("MoveC", py::overload_cast<Vec6, Vec6, double, double>(&ArmInterface::MoveC))
        .def("startTrack", &ArmInterface::startTrack)
        .def("sendRecv", &ArmInterface::sendRecv)
        .def("setWait", &ArmInterface::setWait)
        .def("jointCtrlCmd", &ArmInterface::jointCtrlCmd)
        .def("cartesianCtrlCmd", &ArmInterface::cartesianCtrlCmd)
        ;
}