add multirobot control

CMakeLists.txt

@@ -18,6 +18,8 @@ add_executable(highcmd_development examples/highcmd_development.cpp )
 target_link_libraries(highcmd_development Z1_SDK_Linux64)
 add_executable(lowcmd_development examples/lowcmd_development.cpp )
 target_link_libraries(lowcmd_development Z1_SDK_Linux64)
+add_executable(lowcmd_multirobots examples/lowcmd_multirobots.cpp )
+target_link_libraries(lowcmd_multirobots Z1_SDK_Linux64)
 
 add_subdirectory(pybind11)
 pybind11_add_module(unitree_arm_interface examples_py/arm_python_interface.cpp)

examples/lowcmd_multirobots.cpp

@@ -0,0 +1,67 @@
+#include "unitree_arm_sdk/control/unitreeArm.h"
+
+using namespace UNITREE_ARM;
+
+/* This program aims to control two z1 robots
+ * one named z1_ctrl1 uses UDP to communicate with robot
+ * the other named z1_ctrl2 communicates with ROS simulation
+ * and open main.cpp in z1_ctrl2, then change ARMSDK port to 
+ * ARMSDK(..., 8074, 8073, ...)
+ */
+
+int main()
+{
+    //robot 1
+    auto ctrlComp1 = new CtrlComponents();
+    ctrlComp1->dt = 0.002;//500HZ
+    ctrlComp1->udp = new UDPPort("127.0.0.1", 8071, 8072, RECVSTATE_LENGTH, BlockYN::NO, 500000);
+    ctrlComp1->armModel = new Z1Model();// no UnitreeGripper
+    ctrlComp1->armModel->addLoad(0.03);// add 0.03kg payload to the end joint
+    auto arm1 = new unitreeArm(ctrlComp1);
+
+    //robot 2
+    auto ctrlComp2 = new CtrlComponents();
+    ctrlComp2->dt = 0.002;//500HZ
+    ctrlComp2->udp = new UDPPort("127.0.0.1", 8073, 8074, RECVSTATE_LENGTH, BlockYN::NO, 500000);
+    ctrlComp2->armModel = new Z1Model(Vec3(0.0382, 0.0, 0.0),0.80225,
+        Vec3(0.0037, 0.0014, -0.0003), Vec3(0.00057593, 0.00099960, 0.00106337).asDiagonal());// no UnitreeGripper
+    ctrlComp2->armModel->addLoad(0.03);// add 0.03kg payload to the end joint
+    auto arm2 = new unitreeArm(ctrlComp2);
+
+    //switch to state_lowcmd
+    arm1->sendRecvThread->start();
+    arm1->setFsm(ArmFSMState::PASSIVE);
+    arm1->setFsm(ArmFSMState::LOWCMD);
+    arm1->sendRecvThread->shutdown();
+    arm2->sendRecvThread->start();
+    arm2->setFsm(ArmFSMState::PASSIVE);
+    arm2->setFsm(ArmFSMState::LOWCMD);
+    arm2->sendRecvThread->shutdown();
+
+
+    Vec6 targetQ, lastArm1Q, lastArm2Q;
+    lastArm1Q = arm1->lowstate->getQ();
+    lastArm2Q = arm2->lowstate->getQ();
+    targetQ << 1, 0, 0, 0, 0, 0;
+    int duration = 1000;
+
+    for(int i(0); i<duration; i++)
+    {
+        //robot1
+        arm1->q = lastArm1Q*(1-(double)i/duration) + targetQ*((double)i/duration);
+        arm1->qd = (targetQ-lastArm1Q)/(duration*0.002);
+        arm1->tau.setZero();
+        //robot2
+        arm2->q = lastArm2Q*(1-(double)i/duration) + targetQ*((double)i/duration);
+        arm2->qd = (targetQ-lastArm1Q)/(duration*0.002);
+        arm2->tau.setZero();
+
+        arm1->sendRecv();
+        arm2->sendRecv();
+        usleep(2000);
+    }
+
+    delete arm1;
+    delete arm2;
+    return 0;
+}

include/unitree_arm_sdk/control/ctrlComponents.h

@@ -11,6 +11,7 @@
 namespace UNITREE_ARM {
 struct CtrlComponents{
 public:
+    CtrlComponents();
     CtrlComponents(double deltaT, bool hasUnitreeGripper);
     ~CtrlComponents();
 /*
@@ -45,8 +46,7 @@ public:
     RecvState recvState; // the arm state receive from udp
     ArmFSMState statePast;
     ArmModel* armModel;
-private:
+    UDPPort *udp;
-    UDPPort *_udp;
 };
 
 }

include/unitree_arm_sdk/control/unitreeArm.h

@@ -7,7 +7,13 @@ namespace UNITREE_ARM {
 
 class unitreeArm{
 public:
+
+// few variable is set, the other need to be set manually
+// includes: dt, udp, armModel
 unitreeArm(bool hasUnitreeGripper);
+
+// the parameters set to default
+unitreeArm(CtrlComponents *ctrlComp);
 ~unitreeArm();
 
 

include/unitree_arm_sdk/thirdparty/robotics.h

@@ -1,5 +1,10 @@
 #pragma once
 
+/*
+ * Adapted from modern_robotics.py provided by modernrobotics.org
+ * Provides useful Jacobian and frame representation functions
+ */
+
 #include <vector>
 #include "unitree_arm_sdk/math/mathTools.h"