add waist control in g1 arm sdk example

unitreerobotics · Oct 30, 2024 · c02ac39 · c02ac39
1 parent c51f807
commit c02ac39
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 12 deletions.
diff --git a/example/g1/high_level/g1_arm5_sdk_dds_example.cpp b/example/g1/high_level/g1_arm5_sdk_dds_example.cpp
@@ -70,13 +70,16 @@ int main(int argc, char const *argv[]) {
           kTopicArmSDK));
   arm_sdk_publisher->InitChannel();
 
-  std::array<JointIndex, 10> arm_joints = {
+  std::array<JointIndex, 13> arm_joints = {
       JointIndex::kLeftShoulderPitch,  JointIndex::kLeftShoulderRoll,
       JointIndex::kLeftShoulderYaw,    JointIndex::kLeftElbowPitch,
       JointIndex::kLeftElbowRoll,
       JointIndex::kRightShoulderPitch, JointIndex::kRightShoulderRoll,
       JointIndex::kRightShoulderYaw,   JointIndex::kRightElbowPitch,
-      JointIndex::kRightElbowRoll};
+      JointIndex::kRightElbowRoll,
+      JointIndex::kWaistYaw,
+      JointIndex::kWaistRoll,
+      JointIndex::kWaistPitch};
 
   float weight = 0.f;
   float weight_rate = 0.2f;
@@ -94,10 +97,11 @@ int main(int argc, char const *argv[]) {
   auto sleep_time =
       std::chrono::milliseconds(static_cast<int>(control_dt / 0.001f));
 
-  std::array<float, 10> init_pos{};
+  std::array<float, 13> init_pos{};
 
-  std::array<float, 10> target_pos = {0.f, kPi_2,  0.f, kPi_2, 0.f,
-                                     0.f, -kPi_2, 0.f, kPi_2, 0.f};
+  std::array<float, 13> target_pos = {0.f, kPi_2,  0.f, kPi_2, 0.f,
+                                     0.f, -kPi_2, 0.f, kPi_2, 0.f, 
+                                     0.f, 0.f, 0.f};
 
   // wait for init
   std::cout << "Press ENTER to init arms ...";
@@ -144,7 +148,7 @@ int main(int argc, char const *argv[]) {
   float period = 5.f;
   int num_time_steps = static_cast<int>(period / control_dt);
 
-  std::array<float, 10> current_jpos_des{};
+  std::array<float, 13> current_jpos_des{};
 
   // lift arms up
   for (int i = 0; i < num_time_steps; ++i) {

diff --git a/example/g1/high_level/g1_arm7_sdk_dds_example.cpp b/example/g1/high_level/g1_arm7_sdk_dds_example.cpp
@@ -73,15 +73,18 @@ int main(int argc, char const *argv[]) {
           kTopicArmSDK));
   arm_sdk_publisher->InitChannel();
 
-  std::array<JointIndex, 14> arm_joints = {
+  std::array<JointIndex, 17> arm_joints = {
       JointIndex::kLeftShoulderPitch,  JointIndex::kLeftShoulderRoll,
       JointIndex::kLeftShoulderYaw,    JointIndex::kLeftElbow,
       JointIndex::kLeftWistRoll,       JointIndex::kLeftWistYaw,
       JointIndex::kLeftWistPitch,
       JointIndex::kRightShoulderPitch, JointIndex::kRightShoulderRoll,
       JointIndex::kRightShoulderYaw,   JointIndex::kRightElbow,
       JointIndex::kRightWistRoll,      JointIndex::kRightWistYaw,
-      JointIndex::kRightWistPitch};
+      JointIndex::kRightWistPitch,
+      JointIndex::kWaistYaw,
+      JointIndex::kWaistRoll,
+      JointIndex::kWaistPitch};
 
   float weight = 0.f;
   float weight_rate = 0.2f;
@@ -99,10 +102,11 @@ int main(int argc, char const *argv[]) {
   auto sleep_time =
       std::chrono::milliseconds(static_cast<int>(control_dt / 0.001f));
 
-  std::array<float, 14> init_pos{};
+  std::array<float, 17> init_pos{};
 
-  std::array<float, 14> target_pos = {0.f, kPi_2,  0.f, kPi_2, 0.f, 0.f, 0.f,
-                                     0.f, -kPi_2, 0.f, kPi_2, 0.f, 0.f, 0.f};
+  std::array<float, 17> target_pos = {0.f, kPi_2,  0.f, kPi_2, 0.f, 0.f, 0.f,
+                                     0.f, -kPi_2, 0.f, kPi_2, 0.f, 0.f, 0.f, 
+                                     0, 0, 0};
 
   // wait for init
   std::cout << "Press ENTER to init arms ...";
@@ -149,7 +153,7 @@ int main(int argc, char const *argv[]) {
   float period = 5.f;
   int num_time_steps = static_cast<int>(period / control_dt);
 
-  std::array<float, 14> current_jpos_des{};
+  std::array<float, 17> current_jpos_des{};
 
   // lift arms up
   for (int i = 0; i < num_time_steps; ++i) {