diff --git a/kits/arm/ex_AR_kit.cpp b/kits/arm/ex_AR_kit.cpp index 24c1e5d8..2e7d7992 100644 --- a/kits/arm/ex_AR_kit.cpp +++ b/kits/arm/ex_AR_kit.cpp @@ -11,7 +11,7 @@ #include "arm/arm.hpp" #include "util/mobile_io.hpp" #include -#include +#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; + std::unique_ptr 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 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(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 diff --git a/kits/arm/ex_gravity_compensation_toggle.cpp b/kits/arm/ex_gravity_compensation_toggle.cpp new file mode 100644 index 00000000..cc2656c1 --- /dev/null +++ b/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 +#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 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; + std::unique_ptr 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().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(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; +} + + + diff --git a/kits/arm/ex_impedance_control_cartesian.cpp b/kits/arm/ex_impedance_control_cartesian.cpp new file mode 100644 index 00000000..a8940e09 --- /dev/null +++ b/kits/arm/ex_impedance_control_cartesian.cpp @@ -0,0 +1,181 @@ +/** +In these examples we will implement various hybrid motion-force controllers using the impedance control plugin, which can be used for a wide variety of +applications. +Impedance control is BEST SUITED for assigning free, rigid and springy behaviour, along/about each different axis. +While this is perfectly useful for: +- Having a selectively compliant end-effector, +- Switching between fixed and free behaviour to simulate (mostly) rigid constraints, and +- Allowing human intervention for automated operations by separating controls across different axes, +any applications involving more salient control of the forces (as more complex functions with flexible inputs) should use our force control plugin. See ex_force_control_demoname.cpp. + +This comprises the following demos: +- Fixed: A task-space pose controller implemented entirely using force control via the (PID) impedance controller. +- Cartesian: Locks onto a particular end-effector position while having some compliant orientation. +- Gimbal: A gimbal that locks a specific end-effector orientation, while keeping the rest of the arm compliant. +- Floor: The end-effector is free to move but can't travel below a virtual floor. To further simulate sliding on the floor, see force_control example. +- Damping: The end-effector behaves as 3-different damped systems (overdamped, critically damped, and underdamped), at 3 different heights. + +The following example is for the "Cartesian" demo: +*/ + +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" +#include "util/mobile_io.hpp" +#include +#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_impedance_control_cartesian.cfg.yaml"; + std::vector 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; + std::unique_ptr 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; + + // Ideally, in the impedance control demos, positions and velocities must not be commanded + + // Get the pending command pointer + hebi::GroupCommand& command = arm->pendingCommand(); + + // Clear gains for all position and velocity commands + auto num_joints = arm->robotModel().getDoFCount(); + for (size_t i = 0; i < num_joints; ++i) + { + command[i].settings().actuator().positionGains().kP().set(0); + command[i].settings().actuator().velocityGains().kP().set(0); + } + while (!arm->send()) + { + std::cerr << "Couldn't set actuator position gains to zero!" << std::endl; + } + command.clear(); + + ////////////////////////// + //// 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 an impedance controller on/off:\n" + << " ON - Apply controller based on current position\n" + << " OFF - Go back to gravity-compensated mode\n" + << " ❌ (B1) - Exits the demo.\n"; + + // NOTE: Angle wraparound is an unresolved issue which can lead to unstable behaviour for any case involving rotational positional control. + // Make sure that the rotational gains are either all zero, or are high enough to prevent large angular errors (greater than pi/2). The gains provided in these examples are well behaved. + // Interacting with the end-effector in these examples is perfectly safe. + // However, ensure that nothing prevents the wrist's actuators from moving, and DO NOT place your fingers between them. + + ///////////////////////////// + // Control Variables Setup // + ///////////////////////////// + + // Flag to indicate when impedance controller is on + bool controller_on = false; + + ////////////////////////// + //// 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 impedance mode when button is pressed and released, respectively + if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOn) { + + controller_on = true; + + arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPosition())); + } + else if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOff){ + + controller_on = false; + } + } + + if (!controller_on) + { + arm->cancelGoal(); + } + + // Send latest commands to the arm + arm->send(); + } + + // Clear MobileIO text + mobile_io->clearText(); + + return 0; +} + + + diff --git a/kits/arm/ex_impedance_control_damping.cpp b/kits/arm/ex_impedance_control_damping.cpp new file mode 100644 index 00000000..6acdf0be --- /dev/null +++ b/kits/arm/ex_impedance_control_damping.cpp @@ -0,0 +1,239 @@ +/** +In these examples we will implement various hybrid motion-force controllers using the impedance control plugin, which can be used for a wide variety of +applications. +Impedance control is BEST SUITED for assigning free, rigid and springy behaviour, along/about each different axis. +While this is perfectly useful for: +- Having a selectively compliant end-effector, +- Switching between fixed and free behaviour to simulate (mostly) rigid constraints, and +- Allowing human intervention for automated operations by separating controls across different axes, +any applications involving more salient control of the forces (as more complex functions with flexible inputs) should use our force control plugin. See ex_force_control_demoname.cpp. + +This comprises the following demos: +- Fixed: A task-space pose controller implemented entirely using force control via the (PID) impedance controller. +- Cartesian: Locks onto a particular end-effector position while having some compliant orientation. +- Gimbal: A gimbal that locks a specific end-effector orientation, while keeping the rest of the arm compliant. +- Floor: The end-effector is free to move but can't travel below a virtual floor. To further simulate sliding on the floor, see force_control example. +- Damping: The end-effector behaves as 3-different damped systems (overdamped, critically damped, and underdamped), at 3 different heights. + +The following example is for the "Damping" demo: +*/ + +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" +#include "util/mobile_io.hpp" +#include +#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_impedance_control_damping.cfg.yaml"; + std::vector 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; + std::unique_ptr 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; + + // Ideally, in the impedance control demos, positions and velocities must not be commanded + + // Get the pending command pointer + hebi::GroupCommand& command = arm->pendingCommand(); + + // Clear gains for all position and velocity commands + auto num_joints = arm->robotModel().getDoFCount(); + for (size_t i = 0; i < num_joints; ++i) + { + command[i].settings().actuator().positionGains().kP().set(0); + command[i].settings().actuator().velocityGains().kP().set(0); + } + while (!arm->send()) + { + std::cerr << "Couldn't set actuator position gains to zero!" << std::endl; + } + command.clear(); + + // Retrieve the impedance controller 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().lock(); + if (!plugin_shared_ptr) { + std::cerr << "Failed to lock plugin shared_ptr. The plugin may have been destroyed." << std::endl; + return -1; + } + auto impedance_plugin_ptr = std::dynamic_pointer_cast(plugin_shared_ptr); + if (!impedance_plugin_ptr) { + std::cerr << "Failed to cast plugin to ImpedanceController." << 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 an impedance controller on/off:\n" + << " ON - Apply controller based on current position\n" + << " OFF - Go back to gravity-compensated mode\n" + << " ❌ (B1) - Exits the demo.\n"; + + ///////////////////////////// + // Control Variables Setup // + ///////////////////////////// + + // Meters above the base for overdamped, critically damped, and underdamped cases respectively + std::vector lower_limits = example_config->getUserData().getFloatList("lower_limits"); + + // State variable for current mode: 0 for overdamped, 1 for crtically damped, 2 for underdamped, -1 for free + int mode = -1; + int prevmode = -1; + + // Kp and Kd in different modes + std::vector damping_kp, damping_kd; + + // 0: Overdamped + damping_kp.push_back(Eigen::Map(example_config->getUserData().getFloatList("overdamped_kp").data(), example_config->getUserData().getFloatList("overdamped_kp").size())); + damping_kd.push_back(Eigen::Map(example_config->getUserData().getFloatList("overdamped_kd").data(), example_config->getUserData().getFloatList("overdamped_kd").size())); + + // 1: Critically damped + damping_kp.push_back(Eigen::Map(example_config->getUserData().getFloatList("critically_damped_kp").data(), example_config->getUserData().getFloatList("critically_damped_kp").size())); + damping_kd.push_back(Eigen::Map(example_config->getUserData().getFloatList("critically_damped_kd").data(), example_config->getUserData().getFloatList("critically_damped_kd").size())); + + // 2: Underdamped + damping_kp.push_back(Eigen::Map(example_config->getUserData().getFloatList("underdamped_kp").data(), example_config->getUserData().getFloatList("underdamped_kp").size())); + damping_kd.push_back(Eigen::Map(example_config->getUserData().getFloatList("underdamped_kd").data(), example_config->getUserData().getFloatList("underdamped_kd").size())); + + // Flag to indicate when impedance controller is on + bool controller_on = false; + + ////////////////////////// + //// 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 impedance mode when button is pressed and released, respectively + if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOn) { + + controller_on = true; + + arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPosition())); + } + else if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOff){ + + controller_on = false; + } + } + + if (!controller_on) + { + arm->cancelGoal(); + mode = -1; // Free + prevmode = -1; + } + else + { + // Use forward kinematics to calculate pose of end-effector + Eigen::Vector3d ee_position_curr; + Eigen::Matrix3d ee_orientation_curr; + arm->FK(arm->lastFeedback().getPosition(), ee_position_curr, ee_orientation_curr); + + // Assign mode based on current position + for (int i = 0; i < static_cast(lower_limits.size()); i++) + { + if (ee_position_curr(2) > lower_limits[i]) + { + mode = i; + } + } + + // Change gains only upon mode switches + if (mode != prevmode && mode >= 0) + { + impedance_plugin_ptr->setKp(damping_kp.at(mode)); + impedance_plugin_ptr->setKd(damping_kd.at(mode)); + std::cout << "Mode: " << mode << std::endl; + } + prevmode = mode; + } + + // Send latest commands to the arm + arm->send(); + } + + // Clear MobileIO text + mobile_io->clearText(); + + return 0; +} + + + diff --git a/kits/arm/ex_impedance_control_fixed.cpp b/kits/arm/ex_impedance_control_fixed.cpp new file mode 100644 index 00000000..0408b4ee --- /dev/null +++ b/kits/arm/ex_impedance_control_fixed.cpp @@ -0,0 +1,192 @@ +/** +In these examples we will implement various hybrid motion-force controllers using the impedance control plugin, which can be used for a wide variety of +applications. +Impedance control is BEST SUITED for assigning free, rigid and springy behaviour, along/about each different axis. +While this is perfectly useful for: +- Having a selectively compliant end-effector, +- Switching between fixed and free behaviour to simulate (mostly) rigid constraints, and +- Allowing human intervention for automated operations by separating controls across different axes, +any applications involving more salient control of the forces (as more complex functions with flexible inputs) should use our force control plugin. See ex_force_control_demoname.cpp. + +This comprises the following demos: +- Fixed: A task-space pose controller implemented entirely using force control via the (PID) impedance controller. +- Cartesian: Locks onto a particular end-effector position while having some compliant orientation. +- Gimbal: A gimbal that locks a specific end-effector orientation, while keeping the rest of the arm compliant. +- Floor: The end-effector is free to move but can't travel below a virtual floor. To further simulate sliding on the floor, see force_control example. +- Damping: The end-effector behaves as 3-different damped systems (overdamped, critically damped, and underdamped), at 3 different heights. + +The following example is for the "Fixed" demo: +*/ + +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" +#include "util/mobile_io.hpp" +#include +#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_impedance_control_fixed.cfg.yaml"; + std::vector 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; + std::unique_ptr 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; + + // Ideally, in the impedance control demos, positions and velocities must not be commanded + + // Get the pending command pointer + hebi::GroupCommand& command = arm->pendingCommand(); + + // Clear gains for all position and velocity commands + auto num_joints = arm->robotModel().getDoFCount(); + for (size_t i = 0; i < num_joints; ++i) + { + command[i].settings().actuator().positionGains().kP().set(0); + command[i].settings().actuator().velocityGains().kP().set(0); + } + while (!arm->send()) + { + std::cerr << "Couldn't set actuator position gains to zero!" << std::endl; + } + command.clear(); + + ////////////////////////// + //// 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 an impedance controller on/off:\n" + << " ON - Apply controller based on current position\n" + << " OFF - Go back to gravity-compensated mode\n" + << " ❌ (B1) - Exits the demo.\n"; + + // NOTE: Angle wraparound is an unresolved issue which can lead to unstable behaviour for any case involving rotational positional control. + // Make sure that the rotational gains are either all zero, or are high enough to prevent large angular errors (greater than pi/2). The gains provided in these examples are well behaved. + // Interacting with the end-effector in these examples is perfectly safe. + // However, ensure that nothing prevents the wrist's actuators from moving, and DO NOT place your fingers between them. + + ///////////////////////////// + // Control Variables Setup // + ///////////////////////////// + + // Flag to indicate when impedance controller is on + bool controller_on = false; + + for (size_t i = 0; i < num_joints; ++i) + { + command[i].settings().actuator().positionGains().kP().set(0); + command[i].settings().actuator().velocityGains().kP().set(0); + } + while (!arm->send()) + { + std::cerr << "Couldn't set actuator position gains to zero!" << std::endl; + } + command.clear(); + + ////////////////////////// + //// 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 impedance mode when button is pressed and released, respectively + if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOn) { + + controller_on = true; + + arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPosition())); + } + else if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOff){ + + controller_on = false; + } + } + + if (!controller_on) + { + arm->cancelGoal(); + } + + // Send latest commands to the arm + arm->send(); + } + + // Clear MobileIO text + mobile_io->clearText(); + + return 0; +} + + + diff --git a/kits/arm/ex_impedance_control_floor.cpp b/kits/arm/ex_impedance_control_floor.cpp new file mode 100644 index 00000000..ef567ce8 --- /dev/null +++ b/kits/arm/ex_impedance_control_floor.cpp @@ -0,0 +1,237 @@ +/** +In these examples we will implement various hybrid motion-force controllers using the impedance control plugin, which can be used for a wide variety of +applications. +Impedance control is BEST SUITED for assigning free, rigid and springy behaviour, along/about each different axis. +While this is perfectly useful for: +- Having a selectively compliant end-effector, +- Switching between fixed and free behaviour to simulate (mostly) rigid constraints, and +- Allowing human intervention for automated operations by separating controls across different axes, +any applications involving more salient control of the forces (as more complex functions with flexible inputs) should use our force control plugin. See ex_force_control_demoname.cpp. + +This comprises the following demos: +- Fixed: A task-space pose controller implemented entirely using force control via the (PID) impedance controller. +- Cartesian: Locks onto a particular end-effector position while having some compliant orientation. +- Gimbal: A gimbal that locks a specific end-effector orientation, while keeping the rest of the arm compliant. +- Floor: The end-effector is free to move but can't travel below a virtual floor. To further simulate sliding on the floor, see force_control example. +- Damping: The end-effector behaves as 3-different damped systems (overdamped, critically damped, and underdamped), at 3 different heights. + +The following example is for the "Floor" demo: +*/ + +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" +#include "util/mobile_io.hpp" +#include +#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_impedance_control_floor.cfg.yaml"; + std::vector 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; + std::unique_ptr 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; + + // Ideally, in the impedance control demos, positions and velocities must not be commanded + + // Get the pending command pointer + hebi::GroupCommand& command = arm->pendingCommand(); + + // Clear gains for all position and velocity commands + auto num_joints = arm->robotModel().getDoFCount(); + for (size_t i = 0; i < num_joints; ++i) + { + command[i].settings().actuator().positionGains().kP().set(0); + command[i].settings().actuator().velocityGains().kP().set(0); + } + while (!arm->send()) + { + std::cerr << "Couldn't set actuator position gains to zero!" << std::endl; + } + command.clear(); + + ////////////////////////// + //// 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 an impedance controller on/off:\n" + << " ON - Apply controller based on current position\n" + << " OFF - Go back to gravity-compensated mode\n" + << " ❌ (B1) - Exits the demo.\n"; + + ///////////////////////////// + // Control Variables Setup // + ///////////////////////////// + + // Initialize floor demo variables + double floor_level = 0.0; + double floor_buffer = 0.01; // 1cm + + // Initialize floor demo flags + bool floor_command_flag = false; // Indicates whether or not to command floor stiffness goals + bool cancel_command_flag = false; // Indicates whether or not to cancel goals + + // Flag to indicate when impedance controller is on + bool controller_on = false; + + ////////////////////////// + //// 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 impedance mode when button is pressed and released, respectively + if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOn) { + + controller_on = true; + + arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPosition())); + + // Store current height as floor level, for floor demo + + // Use forward kinematics to find end-effector pose + Eigen::Vector3d ee_position0; + Eigen::Matrix3d ee_orientation0; + arm->FK(arm->lastFeedback().getPosition(), ee_position0, ee_orientation0); + + // Give a little margin to floor level + floor_level = ee_position0(2) - floor_buffer; + + // Update flags to indicate having left the floor + cancel_command_flag = true; + } + else if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOff){ + + controller_on = false; + } + } + + if (!controller_on) + { + arm->cancelGoal(); + } + else + { + // Use forward kinematics to calculate pose of end-effector + Eigen::Vector3d ee_position_curr, ee_position_floor; + Eigen::VectorXd joint_position_floor(num_joints); + Eigen::Matrix3d ee_orientation_curr; + arm->FK(arm->lastFeedback().getPosition(), ee_position_curr, ee_orientation_curr); + + // Snap goal to floor if end-effector is at or below floor, only when it first reaches the floor + if(ee_position_curr(2) <= floor_level && floor_command_flag) + { + // Snap current pose to floor + ee_position_floor = ee_position_curr; + ee_position_floor(2) = floor_level; + + // Use inverse kinematics to calculate appropriate joint positions + joint_position_floor = arm->solveIK(arm->lastFeedback().getPosition(), ee_position_floor, ee_orientation_curr); + + // Set snapped pose as goal + arm->setGoal(arm::Goal::createFromPosition(0.001, joint_position_floor)); // Time is very small to make less complex trajectories + + std::cout << "Hit floor!\n"; + + // Update flags to indicate being in contact with the floor + floor_command_flag = false; + cancel_command_flag = true; + } + // Cancel goal if end-effector is above the floor, only when it leaves the floor + else if (ee_position_curr(2) > floor_level and cancel_command_flag) + { + // Cancel goal to move freely + arm->cancelGoal(); + + std::cout << "Left floor!\n"; + + // Update flags to indicate having left the floor + cancel_command_flag = false; + floor_command_flag = true; + } + } + + // Send latest commands to the arm + arm->send(); + } + + // Clear MobileIO text + mobile_io->clearText(); + + return 0; +} + + + diff --git a/kits/arm/ex_impedance_control_gimbal.cpp b/kits/arm/ex_impedance_control_gimbal.cpp new file mode 100644 index 00000000..2a304995 --- /dev/null +++ b/kits/arm/ex_impedance_control_gimbal.cpp @@ -0,0 +1,181 @@ +/** +In these examples we will implement various hybrid motion-force controllers using the impedance control plugin, which can be used for a wide variety of +applications. +Impedance control is BEST SUITED for assigning free, rigid and springy behaviour, along/about each different axis. +While this is perfectly useful for: +- Having a selectively compliant end-effector, +- Switching between fixed and free behaviour to simulate (mostly) rigid constraints, and +- Allowing human intervention for automated operations by separating controls across different axes, +any applications involving more salient control of the forces (as more complex functions with flexible inputs) should use our force control plugin. See ex_force_control_demoname.cpp. + +This comprises the following demos: +- Fixed: A task-space pose controller implemented entirely using force control via the (PID) impedance controller. +- Cartesian: Locks onto a particular end-effector position while having some compliant orientation. +- Gimbal: A gimbal that locks a specific end-effector orientation, while keeping the rest of the arm compliant. +- Floor: The end-effector is free to move but can't travel below a virtual floor. To further simulate sliding on the floor, see force_control example. +- Damping: The end-effector behaves as 3-different damped systems (overdamped, critically damped, and underdamped), at 3 different heights. + +The following example is for the "Gimbal" demo: +*/ + +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" +#include "util/mobile_io.hpp" +#include +#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_impedance_control_gimbal.cfg.yaml"; + std::vector 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; + std::unique_ptr 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; + + // Ideally, in the impedance control demos, positions and velocities must not be commanded + + // Get the pending command pointer + hebi::GroupCommand& command = arm->pendingCommand(); + + // Clear gains for all position and velocity commands + auto num_joints = arm->robotModel().getDoFCount(); + for (size_t i = 0; i < num_joints; ++i) + { + command[i].settings().actuator().positionGains().kP().set(0); + command[i].settings().actuator().velocityGains().kP().set(0); + } + while (!arm->send()) + { + std::cerr << "Couldn't set actuator position gains to zero!" << std::endl; + } + command.clear(); + + ////////////////////////// + //// 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 an impedance controller on/off:\n" + << " ON - Apply controller based on current position\n" + << " OFF - Go back to gravity-compensated mode\n" + << " ❌ (B1) - Exits the demo.\n"; + + // NOTE: Angle wraparound is an unresolved issue which can lead to unstable behaviour for any case involving rotational positional control. + // Make sure that the rotational gains are either all zero, or are high enough to prevent large angular errors (greater than pi/2). The gains provided in these examples are well behaved. + // Interacting with the end-effector in these examples is perfectly safe. + // However, ensure that nothing prevents the wrist's actuators from moving, and DO NOT place your fingers between them. + + ///////////////////////////// + // Control Variables Setup // + ///////////////////////////// + + // Flag to indicate when impedance controller is on + bool controller_on = false; + + ////////////////////////// + //// 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 impedance mode when button is pressed and released, respectively + if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOn) { + + controller_on = true; + + arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPosition())); + } + else if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOff){ + + controller_on = false; + } + } + + if (!controller_on) + { + arm->cancelGoal(); + } + + // Send latest commands to the arm + arm->send(); + } + + // Clear MobileIO text + mobile_io->clearText(); + + return 0; +} + + + diff --git a/kits/arm/ex_mobile_io_control.cpp b/kits/arm/ex_mobile_io_control.cpp index 6eab57bd..d32f4f21 100644 --- a/kits/arm/ex_mobile_io_control.cpp +++ b/kits/arm/ex_mobile_io_control.cpp @@ -4,12 +4,19 @@ * to pre-programmed waypoints. */ +/* +CAUTION: +This example uses waypoints containing fixed joint angles, which is a bad idea if your actuators have large wind-up. +The correct way to store waypoints is by using se3 coordinates, and converting them to joint positions using our IK functions. +*/ + #include "group_command.hpp" #include "group_feedback.hpp" #include "robot_model.hpp" #include "arm/arm.hpp" #include "util/mobile_io.hpp" #include +#include "hebi_util.hpp" using namespace hebi; using namespace experimental; @@ -23,7 +30,7 @@ int main(int argc, char* argv[]) // Config file path const std::string example_config_file = "config/ex_mobile_io_control.cfg.yaml"; std::vector errors; - + // Load the config const auto example_config = RobotConfig::loadConfig(example_config_file, errors); for (const auto& error : errors) { @@ -34,13 +41,16 @@ int main(int argc, char* argv[]) return -1; } + // For this demo, we need the arm and mobile_io + std::unique_ptr arm; + std::unique_ptr mobile_io; ////////////////////////// ///// Arm Setup ////////// ////////////////////////// - // Create the arm object from the configuration, and retry if not found - std::unique_ptr arm = arm::Arm::create(*example_config); + // Create the arm object from the configuration (retry if not found) + arm = arm::Arm::create(*example_config); while (!arm) { std::cerr << "Failed to create arm, retrying..." << std::endl; arm = arm::Arm::create(*example_config); @@ -51,35 +61,38 @@ int main(int argc, char* argv[]) //// MobileIO Setup ////// ////////////////////////// - // Create the MobileIO object - std::unique_ptr 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); + + // 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; - std::string instructions; - instructions = ("B1 - Waypoint 1\nB2 - Waypoint 2\n" - "B3 - Waypoint 3\n" - "B6 - Grav comp mode\nB8 - Quit\n"); // Clear any garbage on screen - mobile_io->clearText(); - - // Display instructions on screen - mobile_io->appendText(instructions); + mobile_io->clearText(); - // Setup instructions + // Refresh mobile_io auto last_state = mobile_io->update(); ///////////////////////////// // Control Variables Setup // ///////////////////////////// - // Single Waypoint Vectors + // Waypoints auto num_joints = arm->robotModel().getDoFCount(); - Eigen::VectorXd positions(num_joints); - double single_time = 3; + std::vector waypoints; + waypoints.push_back(Eigen::Map(example_config->getUserData().getFloatList("waypoint_1").data(), example_config->getUserData().getFloatList("waypoint_1").size())); + waypoints.push_back(Eigen::Map(example_config->getUserData().getFloatList("waypoint_2").data(), example_config->getUserData().getFloatList("waypoint_2").size())); + waypoints.push_back(Eigen::Map(example_config->getUserData().getFloatList("waypoint_3").data(), example_config->getUserData().getFloatList("waypoint_3").size())); + + // Travel time + double travel_time = example_config->getUserData().getFloat("travel_time"); ////////////////////////// //// Main Control Loop /// @@ -89,41 +102,28 @@ int main(int argc, char* argv[]) { 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 Presses ///////////////// - // Buttton B1 - Home Position - if (mobile_io->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { - positions << 0, 0, 0, 0, 0, 0; - arm -> setGoal(arm::Goal::createFromPosition(single_time, positions)); - } - - // Button B2 - Waypoint 1 - if (mobile_io->getButtonDiff(2) == util::MobileIO::ButtonState::ToOn) { - positions << M_PI/4, M_PI/3, 2*M_PI/3, M_PI/3, M_PI/4, 0; - arm -> setGoal(arm::Goal::createFromPosition(single_time, positions)); - } - - // Button B3 - Waypoint 2 - if (mobile_io->getButtonDiff(3) == util::MobileIO::ButtonState::ToOn) { - positions << -M_PI/4, M_PI/3, 2*M_PI/3, M_PI/3, 3*M_PI/4, 0; - arm -> setGoal(arm::Goal::createFromPosition(single_time, positions)); - + // BN - Waypoint N (N = 1, 2 , 3) + for (int button = 1; button <= 3; button++) + { + if (mobile_io->getButtonDiff(button) == hebi::util::MobileIO::ButtonState::ToOn) { + arm -> setGoal(arm::Goal::createFromPosition(travel_time, waypoints.at(button-1))); + } } // Button B6 - Grav Comp Mode - if (mobile_io->getButtonDiff(6) == util::MobileIO::ButtonState::ToOn) { + if (mobile_io->getButtonDiff(6) == hebi::util::MobileIO::ButtonState::ToOn) { // cancel any goal that is set, returning arm into gravComp mode arm -> cancelGoal(); } // 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; diff --git a/kits/arm/ex_teach_repeat.cpp b/kits/arm/ex_teach_repeat.cpp index a359bf97..ae330179 100644 --- a/kits/arm/ex_teach_repeat.cpp +++ b/kits/arm/ex_teach_repeat.cpp @@ -13,7 +13,7 @@ #include "arm/arm.hpp" #include "util/mobile_io.hpp" #include -#include +#include "hebi_util.hpp" using namespace hebi; using namespace experimental; @@ -23,6 +23,7 @@ struct Waypoint Eigen::VectorXd positions; Eigen::VectorXd vels; Eigen::VectorXd accels; + double time; }; struct State @@ -31,36 +32,38 @@ struct State std::vector waypoints; }; -void addWaypoint (State& state, const GroupFeedback& feedback, bool stop) { +void addWaypoint (State& state, double wp_time, const GroupFeedback& feedback, bool stop) { printf("Adding a Waypoint.\n"); if (stop) { // stop waypoint state.waypoints.push_back(Waypoint {feedback.getPosition(), VectorXd::Constant(state.num_modules, 0), - VectorXd::Constant(state.num_modules, 0)}); + VectorXd::Constant(state.num_modules, 0), + wp_time}); } else { // through waypoint state.waypoints.push_back(Waypoint {feedback.getPosition(), VectorXd::Constant(state.num_modules, std::numeric_limits::quiet_NaN()), - VectorXd::Constant(state.num_modules, std::numeric_limits::quiet_NaN())}); + VectorXd::Constant(state.num_modules, std::numeric_limits::quiet_NaN()), + wp_time}); } } -arm::Goal playWaypoints (State& state, double wp_time) { +arm::Goal playWaypoints (State& state) { // Set up required variables - Eigen::VectorXd times(state.waypoints.size()); Eigen::MatrixXd target_pos(state.num_modules, state.waypoints.size()); Eigen::MatrixXd target_vels(state.num_modules, state.waypoints.size()); Eigen::MatrixXd target_accels(state.num_modules, state.waypoints.size()); + Eigen::VectorXd times(state.waypoints.size()); // Fill up matrices appropriately for (int i = 0; i < state.waypoints.size(); i++) { - times[i] = (i+1) * wp_time; target_pos.col(i) << state.waypoints[i].positions; target_vels.col(i) << state.waypoints[i].vels; target_accels.col(i) << state.waypoints[i].accels; + times[i] = state.waypoints[i].time + (i > 0 ? times[i-1] : 0.0); } return arm::Goal::createFromWaypoints(times, target_pos, target_vels, target_accels); } @@ -85,98 +88,136 @@ int main(int argc, char* argv[]) return -1; } + // For this demo, we need the arm and mobile_io + std::unique_ptr arm; + std::unique_ptr mobile_io; + ////////////////////////// ///// Arm Setup ////////// ////////////////////////// - // Create the arm object from the configuration, and retry if 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; - arm = arm::Arm::create(*example_config); + 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 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); + + // 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(); - // Setup instructions for display - std::string instructions; - instructions = ("B1 - Add stop WP\nB2 - Clear waypoints\n" - "B3 - Add through WP\nB5 - Playback mode\n" - "B6 - Grav comp mode\nB8 - Quit\n"); + // Refresh mobile_io + auto last_state = mobile_io->update(); + + ///////////////////////////// + // Control Variables Setup // + ///////////////////////////// - // Display instructions on screen - mobile_io->appendText(instructions); + // Teach Repeat Variables + State state; + state.num_modules = arm->robotModel().getDoFCount(); - // Setup state variable for mobile device - auto last_mobile_state = mobile_io->update(); + // Run mode is either "training" or "playback" + std::string run_mode = "training"; + // Variable to hold slider value for A3 + double slider3 = 0.0; + + // Load travel times from config + double base_travel_time = example_config->getUserData().getFloat("base_travel_time"); + double min_travel_time = example_config->getUserData().getFloat("min_travel_time"); ////////////////////////// //// Main Control Loop /// ////////////////////////// - // Teach Repeat Variables - State state; - state.num_modules = arm->robotModel().getDoFCount(); - while(arm->update()) { // Get latest mobile_state bool 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) { - // Buttton B1 - Add Stop Waypoint - if (mobile_io->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { - addWaypoint(state, arm -> lastFeedback(), true); - } + if (run_mode == "training") + { + // Axis A3 state + slider3 = mobile_io->getAxis(3); + + // Buttton B1 - Add Stop Waypoint + if (mobile_io->getButtonDiff(1) == hebi::util::MobileIO::ButtonState::ToOn) { + addWaypoint(state, + base_travel_time + slider3 * (base_travel_time - min_travel_time), + arm -> lastFeedback(), + true); + } - // Button B2 - Clear Waypoints - if (mobile_io->getButtonDiff(2) == util::MobileIO::ButtonState::ToOn) { - state.waypoints.clear(); - } + // Button B2 - Add Through Waypoint + if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOn) { + addWaypoint(state, + base_travel_time + slider3 * (base_travel_time - min_travel_time), + arm -> lastFeedback(), + false); + } - // Button B3 - Add Through Waypoint - if (mobile_io->getButtonDiff(3) == util::MobileIO::ButtonState::ToOn) { - addWaypoint(state, arm -> lastFeedback(), false); - } + // Button B3 - Toggle to Playback Waypoints + if (mobile_io->getButtonDiff(3) == hebi::util::MobileIO::ButtonState::ToOn) { + if (state.waypoints.size() <= 1){ + std::cout << "You have not added enough waypoints! You need at least two.\n" << std::endl; + } + else { + std::cout << "Entering playback mode.\n" << std::endl; + const arm::Goal playback = playWaypoints(state); + arm->setGoal(playback); + run_mode = "playback"; + } + } - // Button B5 - Playback Waypoints - if (mobile_io->getButtonDiff(5) == util::MobileIO::ButtonState::ToOn) { - if (state.waypoints.size() <= 1){ - printf("You have not added enough waypoints!\n"); - } - else { - const arm::Goal playback = playWaypoints(state, 2.5); - arm->setGoal(playback); + // Button B4 - Clear Waypoints + if (mobile_io->getButtonDiff(4) == hebi::util::MobileIO::ButtonState::ToOn) { + std::cout << "Discarding waypoints.\n" << std::endl; + state.waypoints.clear(); + } + } + else if (run_mode == "playback") + { + // Button B3 - Toggle to Training Mode + if (mobile_io->getButtonDiff(3) == hebi::util::MobileIO::ButtonState::ToOn) { + std::cout << "Entering training mode.\n" << std::endl; + arm->cancelGoal(); + run_mode = "training"; } - } - // Button B6 - Grav Comp Mode - if (mobile_io->getButtonDiff(6) == util::MobileIO::ButtonState::ToOn) { - // Cancel any goal that is set, returning arm into gravComp mode - arm->cancelGoal(); + // Replay goal + if (arm -> atGoal()) { + const arm::Goal playback = playWaypoints(state); + arm -> setGoal(playback); + } } // 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->clearText(); return 1; diff --git a/kits/arm/hebi_util.cpp b/kits/arm/hebi_util.cpp new file mode 100644 index 00000000..3145db5f --- /dev/null +++ b/kits/arm/hebi_util.cpp @@ -0,0 +1,80 @@ +// hebi_util.cpp + +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" +#include "util/file.hpp" + +#include +#include +#include + +#include "hebi_util.hpp" + +std::unique_ptr createMobileIOFromConfig(const hebi::RobotConfig& config) { + std::map mobile_io_dict; + std::vector errors; + + auto parent_dir_absolute = config.getParentDirectory(); + + // Lambda function to check file paths + auto check_file = [&errors, &parent_dir_absolute](const std::string& type, const std::string& relative_filename) { + // Ensure file is relative: + if (hebi::util::file::File(relative_filename).isAbsolute()) { + errors.push_back("'" + type + "' exists but provides an absolute path."); + return std::string(); + } + // Ensure file exists + hebi::util::file::File absolute_filename(parent_dir_absolute); + absolute_filename.append(relative_filename); + if (!absolute_filename.exists()) { + errors.push_back("'" + type + "' exists but does not point to a valid file."); + return std::string(); + } + // Return success! + return absolute_filename.getAbsolutePath(); + }; + + // Validate the mobile_io configuration + if (config.getUserData().strings_.count("mobile_io_family") && + config.getUserData().strings_.count("mobile_io_name") && + config.getUserData().strings_.count("mobile_io_layout")) { + + // Check that all required fields are present and are strings + if (config.getUserData().getString("mobile_io_family").empty() || + config.getUserData().getString("mobile_io_name").empty()) { + errors.push_back("HEBI config \"user_data\"'s \"mobile_io_...\" fields must be non-empty strings."); + } + else { + // Populate the dictionary + mobile_io_dict["family"] = config.getUserData().getString("mobile_io_family"); + mobile_io_dict["name"] = config.getUserData().getString("mobile_io_name"); + + // Use check_file to validate and convert layout to absolute path + mobile_io_dict["layout"] = check_file("mobile_io_layout", config.getUserData().getString("mobile_io_layout")); + if (mobile_io_dict["layout"].empty()) { + errors.push_back("HEBI config \"user_data\"'s \"mobile_io_layout\" file is invalid."); + } + } + } else { + errors.push_back("HEBI config \"user_data\" must contain the keys: \"mobile_family\", \"mobile_name\", and \"mobile_layout\"."); + } + + // If there are errors, print them and throw an exception + if (!errors.empty()) { + for (const auto& error : errors) { + std::cerr << "Error: " << error << std::endl; + } + throw std::runtime_error("Failed to create MobileIO due to configuration errors."); + } + + // Create Mobile IO based on validated config + std::unique_ptr mobile_io = hebi::util::MobileIO::create(mobile_io_dict["family"], mobile_io_dict["name"]); + + if(mobile_io != nullptr) + { + mobile_io->sendLayout(mobile_io_dict["layout"]); + } + return mobile_io; +} \ No newline at end of file diff --git a/kits/arm/hebi_util.hpp b/kits/arm/hebi_util.hpp new file mode 100644 index 00000000..34d93fb4 --- /dev/null +++ b/kits/arm/hebi_util.hpp @@ -0,0 +1,29 @@ +// hebi_util.hpp + +#ifndef HEBI_UTIL_HPP +#define HEBI_UTIL_HPP + +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" + +#include +#include +#include + +#include "robot_config.hpp" +#include "arm/arm.hpp" +#include "util/mobile_io.hpp" +#include +#include + +// hebi_util stores helper functions that may be rolled into the API under hebi::util + +// Function to create Mobile IO from the config +std::unique_ptr createMobileIOFromConfig(const hebi::RobotConfig& example_config); + +// TODO: Function to create gripper from the config +// std::unique_ptr createGripperFromConfig(const hebi::RobotConfig& example_config, const std::string& config_file_path); + +#endif // HEBI_UTIL_HPP diff --git a/projects/cmake/CMakeLists.txt b/projects/cmake/CMakeLists.txt index 55b7a00d..c87b6ce0 100644 --- a/projects/cmake/CMakeLists.txt +++ b/projects/cmake/CMakeLists.txt @@ -275,11 +275,18 @@ endforeach (EXAMPLE ${ADVANCED_SOURCES}) SET(ARM_SOURCES ${ROOT_DIR}/kits/arm/ex_gravity_compensation.cpp + ${ROOT_DIR}/kits/arm/ex_gravity_compensation_toggle.cpp ${ROOT_DIR}/kits/arm/ex_mobile_io_control.cpp ${ROOT_DIR}/kits/arm/ex_teach_repeat.cpp ${ROOT_DIR}/kits/arm/ex_AR_kit.cpp ${ROOT_DIR}/kits/arm/ex_teach_repeat_with_gripper.cpp - ${ROOT_DIR}/kits/arm/ex_double_arm_teach_repeat.cpp) + ${ROOT_DIR}/kits/arm/ex_double_arm_teach_repeat.cpp + ${ROOT_DIR}/kits/arm/ex_impedance_control_fixed.cpp + ${ROOT_DIR}/kits/arm/ex_impedance_control_cartesian.cpp + ${ROOT_DIR}/kits/arm/ex_impedance_control_gimbal.cpp + ${ROOT_DIR}/kits/arm/ex_impedance_control_floor.cpp + ${ROOT_DIR}/kits/arm/ex_impedance_control_damping.cpp + ) set(CMAKE_RUNTIME_OUTPUT_DIRECTORY kits/arm) foreach (EXAMPLE ${ARM_SOURCES}) @@ -288,9 +295,9 @@ foreach (EXAMPLE ${ARM_SOURCES}) get_filename_component(EX_NAME ${EXAMPLE} NAME_WE) if(WIN32) - add_executable(${EX_NAME} ${EXAMPLE} $) + add_executable(${EX_NAME} ${EXAMPLE} ${ROOT_DIR}/kits/arm/hebi_util.cpp $) else() - add_executable(${EX_NAME} ${EXAMPLE}) + add_executable(${EX_NAME} ${EXAMPLE} ${ROOT_DIR}/kits/arm/hebi_util.cpp) endif() add_dependencies(examples ${EX_NAME}) target_include_directories(${EX_NAME} PRIVATE ${ROOT_DIR}) @@ -317,5 +324,5 @@ foreach (EXAMPLE ${ARM_SOURCES}) endforeach (EXAMPLE ${KIT_SOURCES}) -# Copy the HRDF files for kits into the build directory +# Copy the config directory for kits into the build directory file(COPY ${ROOT_DIR}/kits/arm/config DESTINATION ${CMAKE_BINARY_DIR}/kits/arm)