Reduce Cartesian space joint velocity

CMakeLists.txt

@@ -1,26 +1,26 @@
 cmake_minimum_required(VERSION 3.0)
 project(z1_sdk)
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -pthread")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -std=c++14 -pthread")
-set(CMAKE_CXX_STANDARD_REQUIRED on)
+
-set(CMAKE_CXX_STANDARD 14)
-set(EIGEN_PATH /usr/include/eigen3)
 include_directories(
     include
-    ${EIGEN_PATH}
 )
 
-set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/../lib)
 link_directories(lib)
 
-add_executable(highcmd_basic examples/highcmd_basic.cpp )
+set(EXAMPLES_FILES
-target_link_libraries(highcmd_basic Z1_SDK_${CMAKE_HOST_SYSTEM_PROCESSOR})
+    examples/highcmd_basic.cpp
-add_executable(highcmd_development examples/highcmd_development.cpp )
+    examples/highcmd_development.cpp
-target_link_libraries(highcmd_development Z1_SDK_${CMAKE_HOST_SYSTEM_PROCESSOR})
+    examples/lowcmd_development.cpp
-add_executable(lowcmd_development examples/lowcmd_development.cpp )
+    examples/lowcmd_multirobots.cpp
-target_link_libraries(lowcmd_development Z1_SDK_${CMAKE_HOST_SYSTEM_PROCESSOR})
+)
-add_executable(lowcmd_multirobots examples/lowcmd_multirobots.cpp )
+
-target_link_libraries(lowcmd_multirobots Z1_SDK_${CMAKE_HOST_SYSTEM_PROCESSOR})
+foreach(EXAMPLE_FILE IN LISTS EXAMPLES_FILES)
+  get_filename_component(EXAMPLE_NAME ${EXAMPLE_FILE} NAME_WE)
+  add_executable(${EXAMPLE_NAME} ${EXAMPLE_FILE})
+  target_link_libraries(${EXAMPLE_NAME} Z1_SDK_${CMAKE_SYSTEM_PROCESSOR})
+endforeach()
 
 add_subdirectory(pybind11)
 pybind11_add_module(unitree_arm_interface examples_py/arm_python_interface.cpp)
-target_link_libraries(unitree_arm_interface PRIVATE Z1_SDK_${CMAKE_HOST_SYSTEM_PROCESSOR})
+target_link_libraries(unitree_arm_interface PRIVATE Z1_SDK_${CMAKE_SYSTEM_PROCESSOR})

include/unitree_arm_sdk/control/ctrlComponents.h

@@ -22,29 +22,13 @@ 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
- *           qd: joint velocity
- *           tau: joint (Only used in State_LOWCMD)
- * Output:   None
- */
-    void armCtrl(Vec6 q, Vec6 qd, Vec6 tau);
-/*
- * Function: Set gripper commands to class lowcmd
- * Input:    q:  joint angle
- *           qd: joint velocity
- *           tau: joint (Only used in State_LOWCMD)
- * Output:   None
- */
-    void gripperCtrl(double gripperPos, double gripperW, double gripperTau);
 
     LowlevelCmd *lowcmd;
     LowlevelState *lowstate;
     double dt;// default: 0.002
     SendCmd sendCmd; // udp command to control the arm
     RecvState recvState; // the arm state receive from udp
-    ArmFSMState statePast;
     ArmModel* armModel;
     UDPPort *udp;
 };

include/unitree_arm_sdk/control/unitreeArm.h

@@ -223,7 +223,6 @@ 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]
@@ -247,6 +246,35 @@ void jointCtrlCmd(Vec7 directions, double jointSpeed);
 void cartesianCtrlCmd(Vec7 directions, double oriSpeed, double posSpeed);
 
 
+/*
+ * Function: Set six joints commands to class lowcmd
+ * Input:    q:  joint angle
+ *           qd: joint velocity
+ *           tau: joint (Only used in State_LOWCMD)
+ * Output:   None
+ */
+void setArmCmd(Vec6 q, Vec6 qd, Vec6 tau = Vec6::Zero())
+{
+    lowcmd->setQ(q);
+    lowcmd->setQd(qd);
+    lowcmd->setTau(tau);
+}
+
+
+/*
+ * Function: Set gripper commands to class lowcmd
+ * Input:    q:  joint angle
+ *           qd: joint velocity
+ *           tau: joint (Only used in State_LOWCMD)
+ * Output:   None
+ */
+void setGripperCmd(double gripperPos, double gripperW, double gripperTau = 0.)
+{
+    lowcmd->setGripperQ(gripperPos);
+    lowcmd->setGripperQd(gripperW);
+    lowcmd->setGripperTau(gripperTau);
+}
+
 //command parameters
 Vec6 q, qd, tau;
 Vec6 twist;//spatial velocity: [omega, v]'

include/unitree_arm_sdk/math/typeTrans.h

@@ -3,7 +3,7 @@
 
 #include <iostream>
 #include <vector>
-#include <Eigen/Dense>
+#include <eigen3/Eigen/Dense>
 
 namespace UNITREE_ARM {
 namespace typeTrans{

include/unitree_arm_sdk/message/udp.h

@@ -7,6 +7,7 @@
 #include <string.h>
 #include <vector>
 #include "unitree_arm_sdk/utilities/timer.h"
+#include <chrono>
 
 namespace UNITREE_ARM {
 enum class BlockYN{
@@ -53,7 +54,8 @@ private:
     int _on = 1;
     size_t _sentLength;
     fd_set _rSet;
-};
+    std::chrono::steady_clock::time_point last_recv_time_;
+};  
 }