update a new example

CMakeLists.txt

@@ -17,6 +17,9 @@ endif()
 add_executable(example_a1_motor example/example_a1_motor.cpp)
 target_link_libraries(example_a1_motor ${EXTRA_LIBS})
 
+add_executable(example_a1_motor_output example/example_a1_motor_output.cpp)
+target_link_libraries(example_a1_motor_output ${EXTRA_LIBS})
+
 add_executable(example_b1_motor example/example_b1_motor.cpp)
 target_link_libraries(example_b1_motor ${EXTRA_LIBS})
 
@@ -30,4 +33,4 @@ set(LIBRARY_OUTPUT_PATH "../lib")
 add_subdirectory(thirdparty/pybind11)
 pybind11_add_module(unitree_actuator_sdk thirdparty/python_wrapper/wrapper.cpp)
 target_link_libraries(unitree_actuator_sdk PRIVATE ${EXTRA_LIBS})
-set_target_properties(unitree_actuator_sdk PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${LIBRARY_OUTPUT_PATH}")
+set_target_properties(unitree_actuator_sdk PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${LIBRARY_OUTPUT_PATH}")

README.md

@@ -30,3 +30,30 @@ If you need to run the Python example, please enter the "python" folder. Then ru
 ```python
 sudo python3 example_a1_motor.py
 ```
+
+### Tip
+
+The code snippet below demonstrates an example of assigning values to the command structure `cmd` in `example_a1_motor.cpp`. It should be noted that the commands are all for the **rotor** side. However, the commands we usually compute are for the **output** side. Therefore, when assigning values, we need to consider the conversion between them.
+
+```c++
+cmd.motorType = MotorType::A1;
+data.motorType = MotorType::A1;
+cmd.mode  = queryMotorMode(MotorType::A1,MotorMode::FOC);
+cmd.id    = 0;
+cmd.kp    = 0.0;
+cmd.kd    = 2;
+cmd.q     = 0.0;
+cmd.dq    = -6.28*queryGearRatio(MotorType::A1);
+cmd.tau   = 0.0;
+serial.sendRecv(&cmd,&data);
+```
+
+![Simple Diagram of A1 Motor](Simple_Diagram_of_A1_Motor.png)
+
+Typically, for kp and kd, assuming the motor's gear ratio is r, when calculating kp and kd on the rotor side, we need to convert kp and kd on the output side by dividing them by the square of r. 
+
+$$kp_{\text{rotor}} = \frac{kp_{\text{output}}}{r^2}$$
+
+$$kd_{\text{rotor}} = \frac{kd_{\text{output}}}{r^2}$$
+
+This conversion relationship is demonstrated in the example `example_a1_motor_output.cpp`.By the way, in the example `example_a1_motor_output.cpp`, the kp on the rotor side is additionally divided by 26.07, and the kd on the rotor side is additionally multiplied by 100.0. These are magic numbers for the A1 and B1 motor. When controlling other motors, there is no need to consider these additional steps.

example/example_a1_motor_output.cpp

@@ -0,0 +1,65 @@
+#include <unistd.h>
+#include "serialPort/SerialPort.h"
+#include "unitreeMotor/unitreeMotor.h"
+#include <math.h>
+#define PI 3.1415926
+
+int main() {
+
+  SerialPort  serial("/dev/ttyUSB0");
+  MotorCmd    cmd;
+  MotorData   data;
+
+  float output_kp = 25;
+  float output_kd = 0.6;
+  float rotor_kp = 0;
+  float rotor_kd = 0;
+  float gear_ratio = queryGearRatio(MotorType::A1);
+  float sin_counter = 0.0;
+
+  rotor_kp = (output_kp / (gear_ratio * gear_ratio)) / 26.07;
+  rotor_kd = (output_kd / (gear_ratio * gear_ratio)) * 100.0;
+
+  cmd.motorType = MotorType::A1;
+  data.motorType = MotorType::A1;
+  cmd.mode  = queryMotorMode(MotorType::A1,MotorMode::FOC);
+  cmd.id    = 0;
+  cmd.kp    = 0.0;
+  cmd.kd    = 0.0;
+  cmd.q     = 0.0;
+  cmd.dq    = 0.0;
+  cmd.tau   = 0.0;
+  serial.sendRecv(&cmd,&data);
+  float output_angle_c;
+  output_angle_c = (data.q / gear_ratio) * (180/PI);
+  while(true) 
+  {
+    sin_counter+=0.0001;
+    float output_angle_d;
+    output_angle_d = output_angle_c + 90 * sin(2*PI*sin_counter);
+    float rotor_angle_d = (output_angle_d * (PI/180)) * gear_ratio;
+    cmd.motorType = MotorType::A1;
+    data.motorType = MotorType::A1;
+    cmd.mode  = queryMotorMode(MotorType::A1,MotorMode::FOC);
+    cmd.id    = 0;
+    cmd.kp    = rotor_kp;
+    cmd.kd    = rotor_kd;
+    cmd.q     = rotor_angle_d;
+    cmd.dq    = 0.0;
+    cmd.tau   = 0.0;
+    serial.sendRecv(&cmd,&data);
+
+
+    std::cout <<  std::endl;
+    std::cout <<  "motor.q: "    << data.q / gear_ratio    <<  std::endl;
+    std::cout <<  "motor.temp: "   << data.temp   <<  std::endl;
+    std::cout <<  "motor.W: "      << data.dq / gear_ratio     <<  std::endl;
+    std::cout <<  "motor.merror: " << data.merror <<  std::endl;
+    std::cout <<  "rotor_kp: " << rotor_kp <<  std::endl;
+    std::cout <<  "rotor_kd: " << rotor_kd <<  std::endl;
+    std::cout <<  std::endl;
+
+    usleep(200);
+  }
+
+}