Remainder of changes from #86

Author: iamtesch

kits/arm/ex_AR_kit.cpp

@@ -11,7 +11,7 @@
 #include "arm/arm.hpp"
 #include "util/mobile_io.hpp"
 #include <chrono>
-#include <iostream>
+#include "hebi_util.hpp"
 
 using namespace hebi;
 using namespace experimental; // For all things mobileIO 
@@ -48,44 +48,48 @@ int main(int argc, char* argv[])
     return -1;
   }
 
+  // For this demo, we need the arm and mobile_io
+  std::unique_ptr<arm::Arm> arm;
+  std::unique_ptr<hebi::util::MobileIO> mobile_io;
+
   //////////////////////////
   ///// Arm Setup //////////
   //////////////////////////
 
-  // Create the arm object from the configuration, and keep retrying if arm not found
-  auto arm = arm::Arm::create(*example_config);
+  // Create the arm object from the configuration
+  arm = arm::Arm::create(*example_config);
+
+  // Keep retrying if arm not found
   while (!arm) {
     std::cerr << "Failed to create arm, retrying..." << std::endl;
+
+    // Retry
     arm = arm::Arm::create(*example_config);
   }
   std::cout << "Arm connected." << std::endl;
 
-
   //////////////////////////
   //// MobileIO Setup //////
   //////////////////////////
 
-  // Create the MobileIO object
-  std::unique_ptr<util::MobileIO> mobile_io = util::MobileIO::create("Arm", "mobileIO");
-  if (!mobile_io)
-  {
-    std::cout << "couldn't find mobile IO device!\n";
-    return 1;
-  }
+  // Create the mobile_io object from the configuration
+  std::cout << "Waiting for Mobile IO device to come online..." << std::endl;
+  mobile_io = createMobileIOFromConfig(*example_config);
 
-  // Clear any garbage on screen
-  mobile_io->resetUI();
+  // Keep retrying if Mobile IO not found
+  while (mobile_io == nullptr) {
+    std::cout << "Couldn't find Mobile IO. Check name, family, or device status..." << std::endl;
 
-  // Setup instructions for display
-  std::string instructions;
-  instructions = ("B1 - Home Position\nB3 - AR Control Mode\n"
-                  "B6 - Grav Comp Mode\nB8 - Quit\n");
+    // Retry
+    mobile_io = createMobileIOFromConfig(*example_config);
+  }
+  std::cout << "Mobile IO connected." << std::endl;
 
-  // Display instructions on screen
-  mobile_io->appendText(instructions);
+  // Clear any garbage on screen
+  mobile_io->clearText();
 
-  // Setup state variable for mobile device
-  auto last_mobile_state = mobile_io->update();
+  // Refresh mobile_io
+  auto last_state = mobile_io->update();
 
   //////////////////////////
   //// Main Control Loop ///
@@ -95,13 +99,15 @@ int main(int argc, char* argv[])
   bool softstart = true;
   bool ar_mode = false;
 
-  // Make sure we softstart the arm first.
+  // Load user data from config
   Eigen::VectorXd home_position(arm -> robotModel().getDoFCount());
-  home_position << 0, M_PI/3, 2*M_PI/3, 5*M_PI/6, -M_PI/2, 0; // Adjust depending on your DoFs
+  home_position = Eigen::Map<const Eigen::VectorXd>(example_config->getUserData().getFloatList("home_position").data(), example_config->getUserData().getFloatList("home_position").size());  
+  double soft_start_time = example_config->getUserData().getFloat("homing_duration");
+  double xyz_scale = example_config->getUserData().getFloat("xyz_scale");
 
   // Command the softstart to home position
   arm -> update();
-  arm -> setGoal(arm::Goal::createFromPosition(4, home_position)); // take 4 seconds
+  arm -> setGoal(arm::Goal::createFromPosition(soft_start_time, home_position)); // take 4 seconds
   arm -> send();
 
   // Get the cartesian position and rotation matrix @ home position
@@ -130,39 +136,36 @@ int main(int argc, char* argv[])
 
       // Stay in softstart, don't do any other behavior
       continue;
-    }
+    } 
 
     // Get latest mobile_state
     auto updated_mobile_io = mobile_io->update(0);
 
-    if (!updated_mobile_io)
-      std::cout << "Failed to get feedback from mobile I/O; check connection!\n";
-    else
+    if (updated_mobile_io)
     {
       // Button B1 - Return to home position
-      if (mobile_io->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) {
-        ar_mode = false;
-        arm -> setGoal(arm::Goal::createFromPosition(4, home_position));
+      if (mobile_io->getButtonDiff(1) == hebi::util::MobileIO::ButtonState::ToOn) {
+          ar_mode = false;
+          arm -> setGoal(arm::Goal::createFromPosition(soft_start_time, home_position));
       }
 
       // Button B3 - Start AR Control
-      if (mobile_io->getButtonDiff(3) == util::MobileIO::ButtonState::ToOn) {
+      if (mobile_io->getButtonDiff(3) == hebi::util::MobileIO::ButtonState::ToOn) {
         xyz_phone_init << mobile_io -> getLastFeedback().mobile().arPosition().get().getX(),
                           mobile_io -> getLastFeedback().mobile().arPosition().get().getY(),
                           mobile_io -> getLastFeedback().mobile().arPosition().get().getZ();
-        std::cout << xyz_phone_init << std::endl;
         rot_phone_init = makeRotationMatrix(mobile_io -> getLastFeedback().mobile().arOrientation().get());
         ar_mode = true;
       }
 
       // Button B6 - Grav Comp Mode
-      if (mobile_io->getButtonDiff(6) == util::MobileIO::ButtonState::ToOn) {
+      if (mobile_io->getButtonDiff(6) == hebi::util::MobileIO::ButtonState::ToOn) {
         arm -> cancelGoal();
         ar_mode = false;
       }
 
       // Button B8 - End Demo
-      if (mobile_io->getButtonDiff(8) == util::MobileIO::ButtonState::ToOn) {
+      if (mobile_io->getButtonDiff(8) == hebi::util::MobileIO::ButtonState::ToOn) {
         // Clear MobileIO text
         mobile_io->resetUI();
         return 1;
@@ -178,9 +181,9 @@ int main(int argc, char* argv[])
       auto rot_phone = makeRotationMatrix(mobile_io->getLastFeedback().mobile().arOrientation().get());
 
       // Calculate new targets
-      Eigen::Vector3d xyz_scale;
-      xyz_scale << 1, 1, 2;
-      Eigen::Vector3d xyz_target = xyz_home + (0.75 * xyz_scale.array() *
+      Eigen::Vector3d xyz_scale_vec;
+      xyz_scale_vec << 1, 1, 2;
+      Eigen::Vector3d xyz_target = xyz_home + (xyz_scale * xyz_scale_vec.array() *
                                 (rot_phone_init.transpose() * (xyz_phone - xyz_phone_init)).array()).matrix();
       Eigen::Matrix3d rot_target = rot_phone_init.transpose() * rot_phone * rot_home;
 
@@ -190,7 +193,7 @@ int main(int argc, char* argv[])
                                               rot_target);
 
       // Create and send new goal to the arm
-      arm -> setGoal(arm::Goal::createFromPosition(target_joints));
+      arm -> setGoal(arm::Goal::createFromPosition(example_config->getUserData().getFloat("latency"), target_joints));
     }
 
     // Send latest commands to the arm

kits/arm/ex_gravity_compensation_toggle.cpp

@@ -0,0 +1,146 @@
+/**
+ * This file is a barebones skeleton of how to setup an arm for use.
+ * It demonstrates gravity compensation behavior by commanding torques
+ * equal to the force from gravity on the links and joints of an arm.
+ * Note that this only approximately balances out gravity, as imperfections in
+ * the torque sensing and modeled system can lead to "drift".  Also, the
+ * particular choice of PID control gains can affect the performance of this
+ * demo.
+ */
+
+#include "group_command.hpp"
+#include "group_feedback.hpp"
+#include "robot_model.hpp"
+#include "arm/arm.hpp"
+#include "util/mobile_io.hpp"
+#include <chrono>
+#include "hebi_util.hpp"
+
+using namespace hebi;
+using namespace experimental;
+
+int main(int argc, char* argv[])
+{
+  //////////////////////////
+  ///// Config Setup ///////
+  //////////////////////////
+
+  // Config file path
+  const std::string example_config_file = "config/ex_gravity_compensation_toggle.cfg.yaml";
+  std::vector<std::string> errors;
+  
+  // Load the config
+  const auto example_config = RobotConfig::loadConfig(example_config_file, errors);
+  for (const auto& error : errors) {
+    std::cerr << error << std::endl;
+  }
+  if (!example_config) {
+    std::cerr << "Failed to load configuration from: " << example_config_file << std::endl;
+    return -1;
+  }
+
+  // For this demo, we need the arm and mobile_io
+  std::unique_ptr<arm::Arm> arm;
+  std::unique_ptr<hebi::util::MobileIO> mobile_io;
+
+  //////////////////////////
+  ///// Arm Setup //////////
+  //////////////////////////
+
+  // Create the arm object from the configuration
+  arm = arm::Arm::create(*example_config);
+
+  // Keep retrying if arm not found
+  while (!arm) {
+      std::cerr << "Failed to create arm, retrying..." << std::endl;
+
+      // Retry
+      arm = arm::Arm::create(*example_config);
+  }
+  std::cout << "Arm connected." << std::endl;
+
+  // Retrieve the gravcomp plugin from the arm
+
+  // Get a weak_ptr from the arm API, lock it as a shared_ptr, and then downcast it from a general plugin pointer to a specific plugin pointer
+  auto plugin_shared_ptr = arm->getPluginByType<arm::plugin::GravityCompensationEffort>().lock();
+  if (!plugin_shared_ptr) {
+    std::cerr << "Failed to lock plugin shared_ptr. The plugin may have been destroyed." << std::endl;
+    return -1;
+  }
+  auto gravcomp_plugin_ptr = std::dynamic_pointer_cast<arm::plugin::GravityCompensationEffort>(plugin_shared_ptr);
+  if (!gravcomp_plugin_ptr) {
+    std::cerr << "Failed to cast plugin to GravityCompensationEffort." << std::endl;
+    return -1;
+  }
+
+  //////////////////////////
+  //// MobileIO Setup //////
+  //////////////////////////
+
+  // Create the mobile_io object from the configuration
+  std::cout << "Waiting for Mobile IO device to come online..." << std::endl;
+  mobile_io = createMobileIOFromConfig(*example_config);
+
+  // Keep retrying if Mobile IO not found
+  while (mobile_io == nullptr) {
+      std::cout << "Couldn't find Mobile IO. Check name, family, or device status..." << std::endl;
+
+      // Retry
+      mobile_io = createMobileIOFromConfig(*example_config);
+  }
+  std::cout << "Mobile IO connected." << std::endl;
+
+  // Clear any garbage on screen
+  mobile_io->clearText(); 
+
+  // Refresh mobile_io
+  auto last_state = mobile_io->update();
+
+  std::cout <<  "Commanded gravity-compensated zero force to the arm.\n"
+            <<  "  🌍 (B2) - Toggles the gravity compensation on/off:\n"
+            <<  "            ON  - Apply controller \n"
+            <<  "            OFF - Disable  controller\n"
+            <<  "  ❌ (B1) - Exits the demo.\n";
+
+  //////////////////////////
+  //// Main Control Loop ///
+  //////////////////////////
+
+  while(arm->update())
+  {
+    auto updated_mobile_io = mobile_io->update(0);
+
+    if (updated_mobile_io)
+    {
+      /////////////////
+      // Button Presses
+      /////////////////
+
+      // Buttton B1 - End demo
+      if (mobile_io->getButtonDiff(1) == hebi::util::MobileIO::ButtonState::ToOn) {
+        // Clear MobileIO text
+        mobile_io->resetUI();
+        return 1;
+      }
+
+      // Button B2 - Set and unset gravcomp mode when button is pressed and released, respectively
+      if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOn) {
+        
+        // Enable gravcomp  
+        gravcomp_plugin_ptr->setEnabled(true);
+      }
+      else if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOff){
+
+        // Disable gravcomp
+        gravcomp_plugin_ptr->setEnabled(false);
+      }
+    }
+    // Send latest commands to the arm
+    arm->send();
+  }
+
+  return 0;
+}
+
+
+