From 510709e8eef9b9255971dbf9b42cfdc9d641fe32 Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Fri, 19 Jul 2024 10:35:44 -0400 Subject: [PATCH 01/31] Complete impedance control example with documentation --- kits/arm/example_impedance_control_fixed.cpp | 180 +++++++++++++++++++ kits/arm/gains/T-arm.xml | 58 ++++++ kits/arm/hrdf/T-arm.hrdf | 21 +++ projects/cmake/CMakeLists.txt | 4 +- 4 files changed, 262 insertions(+), 1 deletion(-) create mode 100644 kits/arm/example_impedance_control_fixed.cpp create mode 100644 kits/arm/gains/T-arm.xml create mode 100644 kits/arm/hrdf/T-arm.hrdf diff --git a/kits/arm/example_impedance_control_fixed.cpp b/kits/arm/example_impedance_control_fixed.cpp new file mode 100644 index 00000000..68cae3f2 --- /dev/null +++ b/kits/arm/example_impedance_control_fixed.cpp @@ -0,0 +1,180 @@ +/** +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. + +The following example is for the "Fixed" demo: +*/ + +// #include "lookup.hpp" +// #include "group.hpp" +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" +#include "util/mobile_io.hpp" +#include + +using namespace hebi; +using namespace experimental; + +int main(int argc, char* argv[]) +{ + ////////////////////////// + ///// Arm Setup ////////// + ////////////////////////// + + arm::Arm::Params params; + + // Setup Module Family and Module Names + params.families_ = {"HEBIArm-T"}; + params.names_ = {"J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"}; + + // Read HRDF file to seutp a RobotModel object for the 6-DoF Arm + // Make sure you are running this from the correct directory! + params.hrdf_file_ = "kits/arm/hrdf/T-arm.hrdf"; + + // Create the Arm Object + auto arm = arm::Arm::create(params); + while (!arm) { + arm = arm::Arm::create(params); + } + + // Load the gains file that is approriate to the arm + arm -> loadGains("kits/arm/gains/T-arm.xml"); + + // Create and configure the ImpedanceController plugin + hebi::experimental::arm::PluginConfig impedance_config("ImpedanceController", "ImpedanceController"); + impedance_config.float_lists_["kp"] = {300.0, 300.0, 300.0, 5.0, 5.0, 1.0}; + impedance_config.float_lists_["kd"] = {5.0, 5.0, 5.0, 0.0, 0.0, 0.0}; + impedance_config.float_lists_["ki"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + impedance_config.float_lists_["i_clamp"] = {10.0, 10.0, 10.0, 1.0, 1.0, 1.0}; + impedance_config.bools_["gains_in_end_effector_frame"] = true; + + auto impedance_plugin = hebi::experimental::arm::plugin::ImpedanceController::create(impedance_config); + if (!impedance_plugin) { + std::cerr << "Failed to create ImpedanceController plugin." << std::endl; + return -1; + } + + // Initialize variables used to clear the commanded position and velocity in every cycle + hebi::GroupCommand& command = arm->pendingCommand(); + + auto num_joints = arm->robotModel().getDoFCount(); + Eigen::VectorXd pos_nan(num_joints), vel_nan(num_joints); + pos_nan.fill(std::numeric_limits::quiet_NaN()); + vel_nan.fill(std::numeric_limits::quiet_NaN()); + + // Add the plugin to the arm + if (!arm->addPlugin(std::move(impedance_plugin))) { + std::cerr << "Failed to add ImpedanceController plugin to arm." << std::endl; + return -1; + } + + ////////////////////////// + //// MobileIO Setup ////// + ////////////////////////// + + // Set up MobileIO + std::unique_ptr mobile = util::MobileIO::create(params.families_[0], "mobileIO"); + if (!mobile) + { + std::cout << "couldn't find mobile IO device!\n"; + return 1; + } + mobile->setButtonMode(1, util::MobileIO::ButtonMode::Momentary); + mobile->setButtonLabel(1, "❌"); + mobile->setButtonMode(2, util::MobileIO::ButtonMode::Toggle); + mobile->setButtonLabel(2, "💪"); + + std::string instructions; + instructions = " Fixed demo"; + + // Clear any garbage on screen + mobile->clearText(); + + // Display instructions on screen + mobile->appendText(instructions); + + // Setup instructions + auto last_state = mobile->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 // + ///////////////////////////// + + // Flag to indicate when impedance controller is on + bool controller_on = false; + + ////////////////////////// + //// Main Control Loop /// + ////////////////////////// + + while(arm->update()) + { + auto updated_mobile = mobile->update(0); + + if (updated_mobile) + { + ///////////////// + // Button Presses + ///////////////// + + // Buttton B1 - End demo + if (mobile->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { + // Clear MobileIO text + mobile->resetUI(); + return 1; + } + + // Button B2 - Set and unset impedance mode when button is pressed and released, respectively + if (mobile->getButtonDiff(2) == util::MobileIO::ButtonState::ToOn) { + + controller_on = true; + + arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPositionCommand())); + } + else if (mobile->getButtonDiff(2) == util::MobileIO::ButtonState::ToOff){ + + controller_on = false; + } + } + + if (!controller_on) + { + arm->cancelGoal(); + } + + // Clear all position and velocity commands + command.setPosition(pos_nan); + command.setVelocity(vel_nan); + + // Send latest commands to the arm + arm->send(); + } + + // Clear MobileIO text + mobile->clearText(); + + return 0; +} + + + diff --git a/kits/arm/gains/T-arm.xml b/kits/arm/gains/T-arm.xml new file mode 100644 index 00000000..49b3338b --- /dev/null +++ b/kits/arm/gains/T-arm.xml @@ -0,0 +1,58 @@ + + + + + 4 4 4 4 4 4 + + + 75 100 50 20 10 10 + 0 0 0 0 0 0 + 0 0 0 0 0 0 + 0 0 0 0 0 0 + 0 0 0 0 0 0 + 1 1 1 1 1 1 + 0 0 0 0 0 0 + -inf -inf -inf -inf -inf -inf + inf inf inf inf inf inf + 1 1 1 1 1 1 + -40 -40 -40 -10 -10 -10 + 40 40 40 10 10 10 + 1 1 1 1 1 1 + 1 1 1 1 1 1 + + + + 0.1 0.2 0.1 0.1 0.05 0.05 + 0 0 0 0 0 0 + 0 0 0 0 0 0 + 1 1 1 1 1 1 + 0 0 0 0 0 0 + 0.25 0.25 0.25 0.25 0.25 0.25 + 0 0 0 0 0 0 + -1.790422 -1.790422 -1.790422 -3.434687 -9.617128 -9.617128 + 1.790422 1.790422 1.790422 3.434687 9.617128 9.617128 + 1 1 1 1 1 1 + -1 -1 -1 -1 -1 -1 + 1 1 1 1 1 1 + 0.75 0.75 0.75 0.75 0.75 0.75 + 1 1 1 1 1 1 + + + + 0.05 0.05 0.05 0.1 0.20 0.20 + 0 0 0 0 0 0 + 0.0001 0.0001 0.0001 0.001 0.001 0.001 + 1 1 1 1 1 1 + 0.1 0.1 0.1 0.05 0.05 0.05 + 0.25 0.25 0.25 0.25 0.25 0.25 + 0 0 0 0 0 0 + -40 -40 -40 -25 -5 -5 + 40 40 40 25 5 5 + 1 1 1 1 1 1 + -1 -1 -1 -1 -1 -1 + 1 1 1 1 1 1 + 0.9 0.9 0.9 0.9 0.9 0.9 + 0 0 0 0 0 0 + + + diff --git a/kits/arm/hrdf/T-arm.hrdf b/kits/arm/hrdf/T-arm.hrdf new file mode 100644 index 00000000..fd7a1dca --- /dev/null +++ b/kits/arm/hrdf/T-arm.hrdf @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/projects/cmake/CMakeLists.txt b/projects/cmake/CMakeLists.txt index 82e3e516..f13b848a 100644 --- a/projects/cmake/CMakeLists.txt +++ b/projects/cmake/CMakeLists.txt @@ -279,7 +279,9 @@ SET(ARM_SOURCES ${ROOT_DIR}/kits/arm/example_teach_repeat.cpp ${ROOT_DIR}/kits/arm/example_AR_kit.cpp ${ROOT_DIR}/kits/arm/example_teach_repeat_with_gripper.cpp - ${ROOT_DIR}/kits/arm/example_double_arm_teach_repeat.cpp) + ${ROOT_DIR}/kits/arm/example_double_arm_teach_repeat.cpp + ${ROOT_DIR}/kits/arm/example_impedance_control_fixed.cpp + ) set(CMAKE_RUNTIME_OUTPUT_DIRECTORY kits/arm) foreach (EXAMPLE ${ARM_SOURCES}) From 4fe2ad7b9da85f16cd1b374677ec161fa19cfdc4 Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Fri, 19 Jul 2024 10:53:08 -0400 Subject: [PATCH 02/31] Complete gimbal example --- kits/arm/example_impedance_control_gimbal.cpp | 180 ++++++++++++++++++ projects/cmake/CMakeLists.txt | 1 + 2 files changed, 181 insertions(+) create mode 100644 kits/arm/example_impedance_control_gimbal.cpp diff --git a/kits/arm/example_impedance_control_gimbal.cpp b/kits/arm/example_impedance_control_gimbal.cpp new file mode 100644 index 00000000..63dc6b38 --- /dev/null +++ b/kits/arm/example_impedance_control_gimbal.cpp @@ -0,0 +1,180 @@ +/** +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. + +The following example is for the "Gimbal" demo: +*/ + +// #include "lookup.hpp" +// #include "group.hpp" +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" +#include "util/mobile_io.hpp" +#include + +using namespace hebi; +using namespace experimental; + +int main(int argc, char* argv[]) +{ + ////////////////////////// + ///// Arm Setup ////////// + ////////////////////////// + + arm::Arm::Params params; + + // Setup Module Family and Module Names + params.families_ = {"HEBIArm-T"}; + params.names_ = {"J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"}; + + // Read HRDF file to seutp a RobotModel object for the 6-DoF Arm + // Make sure you are running this from the correct directory! + params.hrdf_file_ = "kits/arm/hrdf/T-arm.hrdf"; + + // Create the Arm Object + auto arm = arm::Arm::create(params); + while (!arm) { + arm = arm::Arm::create(params); + } + + // Load the gains file that is approriate to the arm + arm -> loadGains("kits/arm/gains/T-arm.xml"); + + // Create and configure the ImpedanceController plugin + hebi::experimental::arm::PluginConfig impedance_config("ImpedanceController", "ImpedanceController"); + impedance_config.float_lists_["kp"] = {0.0, 0.0, 0.0, 5.0, 5.0, 1.0}; + impedance_config.float_lists_["kd"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + impedance_config.float_lists_["ki"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + impedance_config.float_lists_["i_clamp"] = {10.0, 10.0, 10.0, 1.0, 1.0, 1.0}; + impedance_config.bools_["gains_in_end_effector_frame"] = true; + + auto impedance_plugin = hebi::experimental::arm::plugin::ImpedanceController::create(impedance_config); + if (!impedance_plugin) { + std::cerr << "Failed to create ImpedanceController plugin." << std::endl; + return -1; + } + + // Initialize variables used to clear the commanded position and velocity in every cycle + hebi::GroupCommand& command = arm->pendingCommand(); + + auto num_joints = arm->robotModel().getDoFCount(); + Eigen::VectorXd pos_nan(num_joints), vel_nan(num_joints); + pos_nan.fill(std::numeric_limits::quiet_NaN()); + vel_nan.fill(std::numeric_limits::quiet_NaN()); + + // Add the plugin to the arm + if (!arm->addPlugin(std::move(impedance_plugin))) { + std::cerr << "Failed to add ImpedanceController plugin to arm." << std::endl; + return -1; + } + + ////////////////////////// + //// MobileIO Setup ////// + ////////////////////////// + + // Set up MobileIO + std::unique_ptr mobile = util::MobileIO::create(params.families_[0], "mobileIO"); + if (!mobile) + { + std::cout << "couldn't find mobile IO device!\n"; + return 1; + } + mobile->setButtonMode(1, util::MobileIO::ButtonMode::Momentary); + mobile->setButtonLabel(1, "❌"); + mobile->setButtonMode(2, util::MobileIO::ButtonMode::Toggle); + mobile->setButtonLabel(2, "💪"); + + std::string instructions; + instructions = " Gimbal demo"; + + // Clear any garbage on screen + mobile->clearText(); + + // Display instructions on screen + mobile->appendText(instructions); + + // Setup instructions + auto last_state = mobile->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 // + ///////////////////////////// + + // Flag to indicate when impedance controller is on + bool controller_on = false; + + ////////////////////////// + //// Main Control Loop /// + ////////////////////////// + + while(arm->update()) + { + auto updated_mobile = mobile->update(0); + + if (updated_mobile) + { + ///////////////// + // Button Presses + ///////////////// + + // Buttton B1 - End demo + if (mobile->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { + // Clear MobileIO text + mobile->resetUI(); + return 1; + } + + // Button B2 - Set and unset impedance mode when button is pressed and released, respectively + if (mobile->getButtonDiff(2) == util::MobileIO::ButtonState::ToOn) { + + controller_on = true; + + arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPositionCommand())); + } + else if (mobile->getButtonDiff(2) == util::MobileIO::ButtonState::ToOff){ + + controller_on = false; + } + } + + if (!controller_on) + { + arm->cancelGoal(); + } + + // Clear all position and velocity commands + command.setPosition(pos_nan); + command.setVelocity(vel_nan); + + // Send latest commands to the arm + arm->send(); + } + + // Clear MobileIO text + mobile->clearText(); + + return 0; +} + + + diff --git a/projects/cmake/CMakeLists.txt b/projects/cmake/CMakeLists.txt index f13b848a..46e56f24 100644 --- a/projects/cmake/CMakeLists.txt +++ b/projects/cmake/CMakeLists.txt @@ -281,6 +281,7 @@ SET(ARM_SOURCES ${ROOT_DIR}/kits/arm/example_teach_repeat_with_gripper.cpp ${ROOT_DIR}/kits/arm/example_double_arm_teach_repeat.cpp ${ROOT_DIR}/kits/arm/example_impedance_control_fixed.cpp + ${ROOT_DIR}/kits/arm/example_impedance_control_gimbal.cpp ) set(CMAKE_RUNTIME_OUTPUT_DIRECTORY kits/arm) From 8cce890d54c57fb05f5ca4312c4054ae5af3d97e Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Fri, 19 Jul 2024 11:05:16 -0400 Subject: [PATCH 03/31] Complete cartesian impedance controller example Added wraparound note to all impedance controller examples Fixed alignment of gimbal instructions --- .../example_impedance_control_cartesian.cpp | 186 ++++++++++++++++++ kits/arm/example_impedance_control_fixed.cpp | 6 + kits/arm/example_impedance_control_gimbal.cpp | 8 +- projects/cmake/CMakeLists.txt | 1 + 4 files changed, 200 insertions(+), 1 deletion(-) create mode 100644 kits/arm/example_impedance_control_cartesian.cpp diff --git a/kits/arm/example_impedance_control_cartesian.cpp b/kits/arm/example_impedance_control_cartesian.cpp new file mode 100644 index 00000000..a40d5e11 --- /dev/null +++ b/kits/arm/example_impedance_control_cartesian.cpp @@ -0,0 +1,186 @@ +/** +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. + +The following example is for the "Cartesian" demo: +*/ + +// #include "lookup.hpp" +// #include "group.hpp" +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" +#include "util/mobile_io.hpp" +#include + +using namespace hebi; +using namespace experimental; + +int main(int argc, char* argv[]) +{ + ////////////////////////// + ///// Arm Setup ////////// + ////////////////////////// + + arm::Arm::Params params; + + // Setup Module Family and Module Names + params.families_ = {"HEBIArm-T"}; + params.names_ = {"J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"}; + + // Read HRDF file to seutp a RobotModel object for the 6-DoF Arm + // Make sure you are running this from the correct directory! + params.hrdf_file_ = "kits/arm/hrdf/T-arm.hrdf"; + + // Create the Arm Object + auto arm = arm::Arm::create(params); + while (!arm) { + arm = arm::Arm::create(params); + } + + // Load the gains file that is approriate to the arm + arm -> loadGains("kits/arm/gains/T-arm.xml"); + + // Create and configure the ImpedanceController plugin + + // 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 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. + + hebi::experimental::arm::PluginConfig impedance_config("ImpedanceController", "ImpedanceController"); + impedance_config.float_lists_["kp"] = {300.0, 300.0, 300.0, 0.0, 0.0, 0.0}; + impedance_config.float_lists_["kd"] = {5.0, 5.0, 5.0, 0.0, 0.0, 0.0}; + impedance_config.float_lists_["ki"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + impedance_config.float_lists_["i_clamp"] = {10.0, 10.0, 10.0, 1.0, 1.0, 1.0}; + impedance_config.bools_["gains_in_end_effector_frame"] = true; + + auto impedance_plugin = hebi::experimental::arm::plugin::ImpedanceController::create(impedance_config); + if (!impedance_plugin) { + std::cerr << "Failed to create ImpedanceController plugin." << std::endl; + return -1; + } + + // Initialize variables used to clear the commanded position and velocity in every cycle + hebi::GroupCommand& command = arm->pendingCommand(); + + auto num_joints = arm->robotModel().getDoFCount(); + Eigen::VectorXd pos_nan(num_joints), vel_nan(num_joints); + pos_nan.fill(std::numeric_limits::quiet_NaN()); + vel_nan.fill(std::numeric_limits::quiet_NaN()); + + // Add the plugin to the arm + if (!arm->addPlugin(std::move(impedance_plugin))) { + std::cerr << "Failed to add ImpedanceController plugin to arm." << std::endl; + return -1; + } + + ////////////////////////// + //// MobileIO Setup ////// + ////////////////////////// + + // Set up MobileIO + std::unique_ptr mobile = util::MobileIO::create(params.families_[0], "mobileIO"); + if (!mobile) + { + std::cout << "couldn't find mobile IO device!\n"; + return 1; + } + mobile->setButtonMode(1, util::MobileIO::ButtonMode::Momentary); + mobile->setButtonLabel(1, "❌"); + mobile->setButtonMode(2, util::MobileIO::ButtonMode::Toggle); + mobile->setButtonLabel(2, "💪"); + + std::string instructions; + instructions = " Cartesian demo"; + + // Clear any garbage on screen + mobile->clearText(); + + // Display instructions on screen + mobile->appendText(instructions); + + // Setup instructions + auto last_state = mobile->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 // + ///////////////////////////// + + // Flag to indicate when impedance controller is on + bool controller_on = false; + + ////////////////////////// + //// Main Control Loop /// + ////////////////////////// + + while(arm->update()) + { + auto updated_mobile = mobile->update(0); + + if (updated_mobile) + { + ///////////////// + // Button Presses + ///////////////// + + // Buttton B1 - End demo + if (mobile->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { + // Clear MobileIO text + mobile->resetUI(); + return 1; + } + + // Button B2 - Set and unset impedance mode when button is pressed and released, respectively + if (mobile->getButtonDiff(2) == util::MobileIO::ButtonState::ToOn) { + + controller_on = true; + + arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPositionCommand())); + } + else if (mobile->getButtonDiff(2) == util::MobileIO::ButtonState::ToOff){ + + controller_on = false; + } + } + + if (!controller_on) + { + arm->cancelGoal(); + } + + // Clear all position and velocity commands + command.setPosition(pos_nan); + command.setVelocity(vel_nan); + + // Send latest commands to the arm + arm->send(); + } + + // Clear MobileIO text + mobile->clearText(); + + return 0; +} + + + diff --git a/kits/arm/example_impedance_control_fixed.cpp b/kits/arm/example_impedance_control_fixed.cpp index 68cae3f2..68718c38 100644 --- a/kits/arm/example_impedance_control_fixed.cpp +++ b/kits/arm/example_impedance_control_fixed.cpp @@ -55,6 +55,12 @@ int main(int argc, char* argv[]) arm -> loadGains("kits/arm/gains/T-arm.xml"); // Create and configure the ImpedanceController plugin + + // 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 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. + hebi::experimental::arm::PluginConfig impedance_config("ImpedanceController", "ImpedanceController"); impedance_config.float_lists_["kp"] = {300.0, 300.0, 300.0, 5.0, 5.0, 1.0}; impedance_config.float_lists_["kd"] = {5.0, 5.0, 5.0, 0.0, 0.0, 0.0}; diff --git a/kits/arm/example_impedance_control_gimbal.cpp b/kits/arm/example_impedance_control_gimbal.cpp index 63dc6b38..a2220213 100644 --- a/kits/arm/example_impedance_control_gimbal.cpp +++ b/kits/arm/example_impedance_control_gimbal.cpp @@ -55,6 +55,12 @@ int main(int argc, char* argv[]) arm -> loadGains("kits/arm/gains/T-arm.xml"); // Create and configure the ImpedanceController plugin + + // 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 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. + hebi::experimental::arm::PluginConfig impedance_config("ImpedanceController", "ImpedanceController"); impedance_config.float_lists_["kp"] = {0.0, 0.0, 0.0, 5.0, 5.0, 1.0}; impedance_config.float_lists_["kd"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; @@ -99,7 +105,7 @@ int main(int argc, char* argv[]) mobile->setButtonLabel(2, "💪"); std::string instructions; - instructions = " Gimbal demo"; + instructions = " Gimbal demo"; // Clear any garbage on screen mobile->clearText(); diff --git a/projects/cmake/CMakeLists.txt b/projects/cmake/CMakeLists.txt index 46e56f24..1c4d6968 100644 --- a/projects/cmake/CMakeLists.txt +++ b/projects/cmake/CMakeLists.txt @@ -281,6 +281,7 @@ SET(ARM_SOURCES ${ROOT_DIR}/kits/arm/example_teach_repeat_with_gripper.cpp ${ROOT_DIR}/kits/arm/example_double_arm_teach_repeat.cpp ${ROOT_DIR}/kits/arm/example_impedance_control_fixed.cpp + ${ROOT_DIR}/kits/arm/example_impedance_control_cartesian.cpp ${ROOT_DIR}/kits/arm/example_impedance_control_gimbal.cpp ) From cdd7e9a10e4527df617a5a4e8a39257bd03943bd Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Wed, 24 Jul 2024 16:10:33 -0400 Subject: [PATCH 04/31] Created floor and variable damping examples. Damping examples is mostly complete, floor needs serious reworking --- .../example_impedance_control_cartesian.cpp | 3 +- .../arm/example_impedance_control_damping.cpp | 234 +++++++++++ kits/arm/example_impedance_control_fixed.cpp | 5 +- kits/arm/example_impedance_control_floor.cpp | 372 ++++++++++++++++++ kits/arm/example_impedance_control_gimbal.cpp | 5 +- projects/cmake/CMakeLists.txt | 2 + 6 files changed, 616 insertions(+), 5 deletions(-) create mode 100644 kits/arm/example_impedance_control_damping.cpp create mode 100644 kits/arm/example_impedance_control_floor.cpp diff --git a/kits/arm/example_impedance_control_cartesian.cpp b/kits/arm/example_impedance_control_cartesian.cpp index a40d5e11..4accc6b9 100644 --- a/kits/arm/example_impedance_control_cartesian.cpp +++ b/kits/arm/example_impedance_control_cartesian.cpp @@ -13,6 +13,7 @@ This comprises the following demos: - 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: */ @@ -155,7 +156,7 @@ int main(int argc, char* argv[]) controller_on = true; - arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPositionCommand())); + arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPosition())); } else if (mobile->getButtonDiff(2) == util::MobileIO::ButtonState::ToOff){ diff --git a/kits/arm/example_impedance_control_damping.cpp b/kits/arm/example_impedance_control_damping.cpp new file mode 100644 index 00000000..6a726fa5 --- /dev/null +++ b/kits/arm/example_impedance_control_damping.cpp @@ -0,0 +1,234 @@ +/** +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 "lookup.hpp" +// #include "group.hpp" +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" +#include "util/mobile_io.hpp" +#include + +using namespace hebi; +using namespace experimental; + +int main(int argc, char* argv[]) +{ + ////////////////////////// + ///// Arm Setup ////////// + ////////////////////////// + + arm::Arm::Params params; + + // Setup Module Family and Module Names + params.families_ = {"HEBIArm-T"}; + params.names_ = {"J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"}; + + // Read HRDF file to seutp a RobotModel object for the 6-DoF Arm + // Make sure you are running this from the correct directory! + params.hrdf_file_ = "kits/arm/hrdf/T-arm.hrdf"; + + // Create the Arm Object + auto arm = arm::Arm::create(params); + while (!arm) { + arm = arm::Arm::create(params); + } + + // Load the gains file that is approriate to the arm + arm -> loadGains("kits/arm/gains/T-arm.xml"); + + // Create and configure the ImpedanceController plugin + + // 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 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. + + hebi::experimental::arm::PluginConfig impedance_config("ImpedanceController", "ImpedanceController"); + impedance_config.float_lists_["kp"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + impedance_config.float_lists_["kd"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + impedance_config.bools_["gains_in_end_effector_frame"] = false; + + auto impedance_plugin = hebi::experimental::arm::plugin::ImpedanceController::create(impedance_config); + if (!impedance_plugin) { + std::cerr << "Failed to create ImpedanceController plugin." << std::endl; + return -1; + } + + // Initialize variables used to clear the commanded position and velocity in every cycle + hebi::GroupCommand& command = arm->pendingCommand(); + + auto num_joints = arm->robotModel().getDoFCount(); + Eigen::VectorXd pos_nan(num_joints), vel_nan(num_joints); + pos_nan.fill(std::numeric_limits::quiet_NaN()); + vel_nan.fill(std::numeric_limits::quiet_NaN()); + + // Keep a reference to the ImpedanceController plugin + // You will have to do this if you intend to reconfigure the impedance control after adding it. + auto impedance_plugin_ptr = impedance_plugin.get(); + + // Add the plugin to the arm + if (!arm->addPlugin(std::move(impedance_plugin))) { + std::cerr << "Failed to add ImpedanceController plugin to arm." << std::endl; + return -1; + } + + ////////////////////////// + //// MobileIO Setup ////// + ////////////////////////// + + // Set up MobileIO + std::unique_ptr mobile = util::MobileIO::create(params.families_[0], "mobileIO"); + if (!mobile) + { + std::cout << "couldn't find mobile IO device!\n"; + return 1; + } + mobile->setButtonMode(1, util::MobileIO::ButtonMode::Momentary); + mobile->setButtonLabel(1, "❌"); + mobile->setButtonMode(2, util::MobileIO::ButtonMode::Toggle); + mobile->setButtonLabel(2, "💪"); + + std::string instructions; + instructions = " Damping demo"; + + // Clear any garbage on screen + mobile->clearText(); + + // Display instructions on screen + mobile->appendText(instructions); + + // Setup instructions + auto last_state = mobile->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 = {0.0, 0.15, 0.3}; + // State variable for current mode: 0 for overdamped, 1 for crtically damped, 2 for underdamped, -1 for + int mode = -1; + int prevmode = -1; + std::vector damping_kp, damping_kd; + + // 0: Overdamped + damping_kp.push_back((Eigen::VectorXd(6) << 100.0f, 100.0f, 0.0f, 5.0f, 5.0f, 1.0f).finished()); + damping_kd.push_back((Eigen::VectorXd(6) << 15.0f, 15.0f, 15.0f, 0.0f, 0.0f, 0.0f).finished()); + + // 1: Critically damped + damping_kp.push_back((Eigen::VectorXd(6) << 100.0f, 100.0f, 0.0f, 5.0f, 5.0f, 1.0f).finished()); + damping_kd.push_back((Eigen::VectorXd(6) << 5.0f, 5.0f, 5.0f, 0.0f, 0.0f, 0.0f).finished()); + + // 2: Underdamped + damping_kp.push_back((Eigen::VectorXd(6) << 100.0f, 100.0f, 0.0f, 5.0f, 5.0f, 1.0f).finished()); + damping_kd.push_back((Eigen::VectorXd(6) << 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f).finished()); + + + // Flag to indicate when impedance controller is on + bool controller_on = false; + + ////////////////////////// + //// Main Control Loop /// + ////////////////////////// + + while(arm->update()) + { + auto updated_mobile = mobile->update(0); + + if (updated_mobile) + { + ///////////////// + // Button Presses + ///////////////// + + // Buttton B1 - End demo + if (mobile->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { + // Clear MobileIO text + mobile->resetUI(); + return 1; + } + + // Button B2 - Set and unset impedance mode when button is pressed and released, respectively + if (mobile->getButtonDiff(2) == util::MobileIO::ButtonState::ToOn) { + + controller_on = true; + + arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPosition())); + } + else if (mobile->getButtonDiff(2) == util::MobileIO::ButtonState::ToOff){ + + controller_on = false; + } + } + + if (!controller_on) + { + arm->cancelGoal(); + mode = -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); + + // + for (int i = 0; i < static_cast(lower_limits.size()); i++) + { + if (ee_position_curr(2) > lower_limits[i]) + { + mode = i; + } + } + + if (mode != prevmode) + { + impedance_plugin_ptr->setKp(damping_kp.at(mode)); + impedance_plugin_ptr->setKd(damping_kd.at(mode)); + std::cout << "Mode: " << mode << std::endl; + } + prevmode = mode; + } + + // Clear all position and velocity commands + command.setPosition(pos_nan); + command.setVelocity(vel_nan); + + // Send latest commands to the arm + arm->send(); + } + + // Clear MobileIO text + mobile->clearText(); + + return 0; +} + + + diff --git a/kits/arm/example_impedance_control_fixed.cpp b/kits/arm/example_impedance_control_fixed.cpp index 68718c38..daf221dc 100644 --- a/kits/arm/example_impedance_control_fixed.cpp +++ b/kits/arm/example_impedance_control_fixed.cpp @@ -13,6 +13,7 @@ This comprises the following demos: - 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: */ @@ -60,7 +61,7 @@ int main(int argc, char* argv[]) // Make sure that the rotational gains 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. - + hebi::experimental::arm::PluginConfig impedance_config("ImpedanceController", "ImpedanceController"); impedance_config.float_lists_["kp"] = {300.0, 300.0, 300.0, 5.0, 5.0, 1.0}; impedance_config.float_lists_["kd"] = {5.0, 5.0, 5.0, 0.0, 0.0, 0.0}; @@ -155,7 +156,7 @@ int main(int argc, char* argv[]) controller_on = true; - arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPositionCommand())); + arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPosition())); } else if (mobile->getButtonDiff(2) == util::MobileIO::ButtonState::ToOff){ diff --git a/kits/arm/example_impedance_control_floor.cpp b/kits/arm/example_impedance_control_floor.cpp new file mode 100644 index 00000000..462f2143 --- /dev/null +++ b/kits/arm/example_impedance_control_floor.cpp @@ -0,0 +1,372 @@ +/** +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 "lookup.hpp" +// #include "group.hpp" +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" +#include "util/mobile_io.hpp" +#include + +using namespace hebi; +using namespace experimental; + +int main(int argc, char* argv[]) +{ + ////////////////////////// + ///// Arm Setup ////////// + ////////////////////////// + + arm::Arm::Params params; + + // Setup Module Family and Module Names + params.families_ = {"HEBIArm-T"}; + params.names_ = {"J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"}; + + // Read HRDF file to seutp a RobotModel object for the 6-DoF Arm + // Make sure you are running this from the correct directory! + params.hrdf_file_ = "kits/arm/hrdf/T-arm.hrdf"; + + // Create the Arm Object + auto arm = arm::Arm::create(params); + while (!arm) { + arm = arm::Arm::create(params); + } + + // Load the gains file that is approriate to the arm + arm -> loadGains("kits/arm/gains/T-arm.xml"); + + // Create and configure the ImpedanceController plugin + + // 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 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. + + hebi::experimental::arm::PluginConfig impedance_config("ImpedanceController", "ImpedanceController"); + // impedance_config.float_lists_["kp"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + // impedance_config.float_lists_["kd"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + // impedance_config.float_lists_["ki"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + // impedance_config.float_lists_["i_clamp"] = {10.0, 10.0, 10.0, 1.0, 1.0, 1.0}; + impedance_config.bools_["gains_in_end_effector_frame"] = true; + + impedance_config.float_lists_["kp"] = {300.0, 300.0, 300.0, 5.0, 5.0, 1.0}; + impedance_config.float_lists_["kd"] = {5.0, 5.0, 5.0, 0.0, 0.0, 0.0}; + impedance_config.float_lists_["ki"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + impedance_config.float_lists_["i_clamp"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + + auto impedance_plugin = hebi::experimental::arm::plugin::ImpedanceController::create(impedance_config); + if (!impedance_plugin) { + std::cerr << "Failed to create ImpedanceController plugin." << std::endl; + return -1; + } + + // Initialize variables used to clear the commanded position and velocity in every cycle + hebi::GroupCommand& command = arm->pendingCommand(); + + auto num_joints = arm->robotModel().getDoFCount(); + Eigen::VectorXd pos_nan(num_joints), vel_nan(num_joints); + pos_nan.fill(std::numeric_limits::quiet_NaN()); + vel_nan.fill(std::numeric_limits::quiet_NaN()); + + // Keep a reference to the ImpedanceController plugin + // You will have to do this if you intend to reconfigure the impedance control after adding it. + auto impedance_plugin_ptr = impedance_plugin.get(); + + // Add the plugin to the arm + if (!arm->addPlugin(std::move(impedance_plugin))) { + std::cerr << "Failed to add ImpedanceController plugin to arm." << std::endl; + return -1; + } + + ////////////////////////// + //// MobileIO Setup ////// + ////////////////////////// + + // Set up MobileIO + std::unique_ptr mobile = util::MobileIO::create(params.families_[0], "mobileIO"); + if (!mobile) + { + std::cout << "couldn't find mobile IO device!\n"; + return 1; + } + mobile->setButtonMode(1, util::MobileIO::ButtonMode::Momentary); + mobile->setButtonLabel(1, "❌"); + mobile->setButtonMode(2, util::MobileIO::ButtonMode::Toggle); + mobile->setButtonLabel(2, "💪"); + + std::string instructions; + instructions = " Floor demo"; + + // Clear any garbage on screen + mobile->clearText(); + + // Display instructions on screen + mobile->appendText(instructions); + + // Setup instructions + auto last_state = mobile->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 = mobile->update(0); + + if (updated_mobile) + { + ///////////////// + // Button Presses + ///////////////// + + // Buttton B1 - End demo + if (mobile->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { + // Clear MobileIO text + mobile->resetUI(); + return 1; + } + + // Button B2 - Set and unset impedance mode when button is pressed and released, respectively + if (mobile->getButtonDiff(2) == 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->getButtonDiff(2) == 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) + { + // Set surface stiffness of floor + // impedance_config.float_lists_["kp"] = {300.0, 300.0, 300.0, 5.0, 5.0, 1.0}; + // impedance_config.float_lists_["kd"] = {5.0, 5.0, 5.0, 0.0, 0.0, 0.0}; + // impedance_config.float_lists_["ki"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + // impedance_config.float_lists_["i_clamp"] = {10.0, 10.0, 10.0, 1.0, 1.0, 1.0}; + + // std::vector new_kp = {300.0, 300.0, 300.0, 5.0, 5.0, 5.0}; + // std::vector new_kd = {5.0, 5.0, 5.0, 5.0, 5.0, 1.0}; + // std::vector new_ki = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + // std::vector new_i_clamp = {10.0, 10.0, 10.0, 1.0, 1.0, 1.0}; + + std::cout << "1" << std::endl; + std::cout << impedance_plugin_ptr->kp()(2) << std::endl; + std::cout << impedance_plugin_ptr->kd()(2) << std::endl; + std::cout << impedance_plugin_ptr->ki()(2) << std::endl; + std::cout << impedance_plugin_ptr->iClamp().size() << std::endl; + std::cout << impedance_plugin_ptr->gainsInEndEffectorFrame() << std::endl; + + Eigen::VectorXd new_kp(6), new_kd(6), new_ki(6), new_i_clamp(6); + new_kp << 2.0, 2.0, 2.0, 2.0, 2.0, 2.0; + new_kd << 2.0, 2.0, 2.0, 2.0, 2.0, 2.0; + new_ki << 2.0, 2.0, 2.0, 2.0, 2.0, 2.0; + new_i_clamp << 2.0, 2.0, 2.0, 2.0, 2.0, 2.0; + bool new_gains = false; + + std::vector vals; + vals.push_back(impedance_plugin_ptr->setKp(new_kp)); + vals.push_back(impedance_plugin_ptr->setKd(new_kd)); + vals.push_back(impedance_plugin_ptr->setKi(new_ki)); + vals.push_back(impedance_plugin_ptr->setIClamp(new_i_clamp)); + impedance_plugin_ptr->setGainsInEndEffectorFrame(new_gains); + std::cout << vals.at(0) << vals.at(1) << vals.at(2) << vals.at(3) << std::endl; + + std::cout << "2" << std::endl; + std::cout << impedance_plugin_ptr->kp()(2) << std::endl; + std::cout << impedance_plugin_ptr->kd()(2) << std::endl; + std::cout << impedance_plugin_ptr->ki()(2) << std::endl; + std::cout << impedance_plugin_ptr->iClamp().size() << std::endl; + std::cout << impedance_plugin_ptr->gainsInEndEffectorFrame() << std::endl; + + Eigen::VectorXd new_kp2, new_kd2, new_ki2, new_i_clamp2; + bool new_gains2 = true; + + std::vector vals2; + vals2.push_back(impedance_plugin_ptr->setKp(new_kp2)); + vals2.push_back(impedance_plugin_ptr->setKd(new_kd2)); + vals2.push_back(impedance_plugin_ptr->setKi(new_ki2)); + vals2.push_back(impedance_plugin_ptr->setIClamp(new_i_clamp2)); + impedance_plugin_ptr->setGainsInEndEffectorFrame(new_gains2); + std::cout << vals2.at(0) << vals2.at(1) << vals2.at(2) << vals2.at(3) << std::endl; + + std::cout << "3" << std::endl; + std::cout << impedance_plugin_ptr->kp()(2) << std::endl; + std::cout << impedance_plugin_ptr->kd()(2) << std::endl; + std::cout << impedance_plugin_ptr->ki()(2) << std::endl; + std::cout << impedance_plugin_ptr->iClamp().size() << std::endl; + std::cout << impedance_plugin_ptr->gainsInEndEffectorFrame() << std::endl; + + Eigen::VectorXd new_kp3(6), new_kd3(7), new_ki3(5), new_i_clamp3(6); + new_kp3 << 2.0, 2.0, -2.0, 2.0, 2.0, 2.0; + new_kd3 << 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0; + new_ki3 << 2.0, 2.0, 2.0, 2.0, 2.0; + new_i_clamp3 << 2.0, -2.0, 2.0, 2.0, 2.0, 2.0; + bool new_gains3 = false; + + std::vector vals3; + vals3.push_back(impedance_plugin_ptr->setKp(new_kp3)); + vals3.push_back(impedance_plugin_ptr->setKd(new_kd3)); + vals3.push_back(impedance_plugin_ptr->setKi(new_ki3)); + vals3.push_back(impedance_plugin_ptr->setIClamp(new_i_clamp3)); + impedance_plugin_ptr->setGainsInEndEffectorFrame(new_gains3); + std::cout << vals3.at(0) << vals3.at(1) << vals3.at(2) << vals3.at(3) << std::endl; + + std::cout << "4" << std::endl; + std::cout << impedance_plugin_ptr->kp()(2) << std::endl; + std::cout << impedance_plugin_ptr->kd()(2) << std::endl; + std::cout << impedance_plugin_ptr->ki()(2) << std::endl; + std::cout << impedance_plugin_ptr->iClamp().size() << std::endl; + std::cout << impedance_plugin_ptr->gainsInEndEffectorFrame() << std::endl; + + Eigen::VectorXd new_kp4(6), new_kd4(6), new_ki4(6), new_i_clamp4(6); + new_kp4 << 2.0, 2.0, 2.0, 2.0, 2.0, 2.0; + new_kd4 << 2.0, 2.0, 2.0, 2.0, 2.0, 2.0; + new_ki4 << 2.0, 2.0, 2.0, 2.0, 2.0, 2.0; + new_i_clamp4 << 2.0, 2.0, 2.0, 2.0, 2.0, 2.0; + bool new_gains4 = false; + + std::vector vals4; + vals4.push_back(impedance_plugin_ptr->setKp(new_kp4)); + vals4.push_back(impedance_plugin_ptr->setKd(new_kd4)); + vals4.push_back(impedance_plugin_ptr->setKi(new_ki4)); + vals4.push_back(impedance_plugin_ptr->setIClamp(new_i_clamp4)); + impedance_plugin_ptr->setGainsInEndEffectorFrame(new_gains4); + std::cout << vals4.at(0) << vals4.at(1) << vals4.at(2) << vals4.at(3) << std::endl; + + std::cout << "5" << std::endl; + std::cout << impedance_plugin_ptr->kp()(2) << std::endl; + std::cout << impedance_plugin_ptr->kd()(2) << std::endl; + std::cout << impedance_plugin_ptr->ki()(2) << std::endl; + std::cout << impedance_plugin_ptr->iClamp().size() << std::endl; + std::cout << impedance_plugin_ptr->gainsInEndEffectorFrame() << std::endl; + + // std::cout << impedance_plugin->kp()(2); + // impedance_plugin_ptr->applyParameter("kp", new_kp); + // impedance_plugin_ptr->applyParameter("kd", new_kd); + // impedance_plugin_ptr->applyParameter("ki", new_ki); + // impedance_plugin_ptr->applyParameter("i_clamp", new_i_clamp); + + // impedance_plugin_ptr->applyParameters(impedance_config, {"gains_in_end_effector_frame", "kp", "kd", "ki", "i_clamp"}); + // impedance_plugin_ptr->applyParameterImpl("kp", new_kp); + + // 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 << "goal set\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) + { + // Set stiffness in air + impedance_config.float_lists_["kp"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + impedance_config.float_lists_["kd"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + impedance_config.float_lists_["ki"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + impedance_config.float_lists_["i_clamp"] = {10.0, 10.0, 10.0, 1.0, 1.0, 1.0}; + + // Cancel goal to move freely + arm->cancelGoal(); + + // Update flags to indicate having left the floor + cancel_command_flag = false; + floor_command_flag = true; + } + } + + // Clear all position and velocity commands + command.setPosition(pos_nan); + command.setVelocity(vel_nan); + + // Send latest commands to the arm + arm->send(); + } + + // Clear MobileIO text + mobile->clearText(); + + return 0; +} + + + diff --git a/kits/arm/example_impedance_control_gimbal.cpp b/kits/arm/example_impedance_control_gimbal.cpp index a2220213..ffce15e9 100644 --- a/kits/arm/example_impedance_control_gimbal.cpp +++ b/kits/arm/example_impedance_control_gimbal.cpp @@ -13,6 +13,7 @@ This comprises the following demos: - 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: */ @@ -60,7 +61,7 @@ int main(int argc, char* argv[]) // Make sure that the rotational gains 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. - + hebi::experimental::arm::PluginConfig impedance_config("ImpedanceController", "ImpedanceController"); impedance_config.float_lists_["kp"] = {0.0, 0.0, 0.0, 5.0, 5.0, 1.0}; impedance_config.float_lists_["kd"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; @@ -155,7 +156,7 @@ int main(int argc, char* argv[]) controller_on = true; - arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPositionCommand())); + arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPosition())); } else if (mobile->getButtonDiff(2) == util::MobileIO::ButtonState::ToOff){ diff --git a/projects/cmake/CMakeLists.txt b/projects/cmake/CMakeLists.txt index 1c4d6968..184af91b 100644 --- a/projects/cmake/CMakeLists.txt +++ b/projects/cmake/CMakeLists.txt @@ -283,6 +283,8 @@ SET(ARM_SOURCES ${ROOT_DIR}/kits/arm/example_impedance_control_fixed.cpp ${ROOT_DIR}/kits/arm/example_impedance_control_cartesian.cpp ${ROOT_DIR}/kits/arm/example_impedance_control_gimbal.cpp + ${ROOT_DIR}/kits/arm/example_impedance_control_floor.cpp + ${ROOT_DIR}/kits/arm/example_impedance_control_damping.cpp ) set(CMAKE_RUNTIME_OUTPUT_DIRECTORY kits/arm) From f72675e9ccabc7cd7a4bc88099d13c0f800140e0 Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Wed, 24 Jul 2024 16:40:07 -0400 Subject: [PATCH 05/31] All examples are complete now --- .../arm/example_impedance_control_damping.cpp | 2 +- kits/arm/example_impedance_control_floor.cpp | 125 +----------------- 2 files changed, 4 insertions(+), 123 deletions(-) diff --git a/kits/arm/example_impedance_control_damping.cpp b/kits/arm/example_impedance_control_damping.cpp index 6a726fa5..56ce3cef 100644 --- a/kits/arm/example_impedance_control_damping.cpp +++ b/kits/arm/example_impedance_control_damping.cpp @@ -108,7 +108,7 @@ int main(int argc, char* argv[]) mobile->setButtonLabel(2, "💪"); std::string instructions; - instructions = " Damping demo"; + instructions = " Damping demo"; // Clear any garbage on screen mobile->clearText(); diff --git a/kits/arm/example_impedance_control_floor.cpp b/kits/arm/example_impedance_control_floor.cpp index 462f2143..89aca557 100644 --- a/kits/arm/example_impedance_control_floor.cpp +++ b/kits/arm/example_impedance_control_floor.cpp @@ -88,10 +88,6 @@ int main(int argc, char* argv[]) pos_nan.fill(std::numeric_limits::quiet_NaN()); vel_nan.fill(std::numeric_limits::quiet_NaN()); - // Keep a reference to the ImpedanceController plugin - // You will have to do this if you intend to reconfigure the impedance control after adding it. - auto impedance_plugin_ptr = impedance_plugin.get(); - // Add the plugin to the arm if (!arm->addPlugin(std::move(impedance_plugin))) { std::cerr << "Failed to add ImpedanceController plugin to arm." << std::endl; @@ -209,117 +205,6 @@ int main(int argc, char* argv[]) // 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) { - // Set surface stiffness of floor - // impedance_config.float_lists_["kp"] = {300.0, 300.0, 300.0, 5.0, 5.0, 1.0}; - // impedance_config.float_lists_["kd"] = {5.0, 5.0, 5.0, 0.0, 0.0, 0.0}; - // impedance_config.float_lists_["ki"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - // impedance_config.float_lists_["i_clamp"] = {10.0, 10.0, 10.0, 1.0, 1.0, 1.0}; - - // std::vector new_kp = {300.0, 300.0, 300.0, 5.0, 5.0, 5.0}; - // std::vector new_kd = {5.0, 5.0, 5.0, 5.0, 5.0, 1.0}; - // std::vector new_ki = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - // std::vector new_i_clamp = {10.0, 10.0, 10.0, 1.0, 1.0, 1.0}; - - std::cout << "1" << std::endl; - std::cout << impedance_plugin_ptr->kp()(2) << std::endl; - std::cout << impedance_plugin_ptr->kd()(2) << std::endl; - std::cout << impedance_plugin_ptr->ki()(2) << std::endl; - std::cout << impedance_plugin_ptr->iClamp().size() << std::endl; - std::cout << impedance_plugin_ptr->gainsInEndEffectorFrame() << std::endl; - - Eigen::VectorXd new_kp(6), new_kd(6), new_ki(6), new_i_clamp(6); - new_kp << 2.0, 2.0, 2.0, 2.0, 2.0, 2.0; - new_kd << 2.0, 2.0, 2.0, 2.0, 2.0, 2.0; - new_ki << 2.0, 2.0, 2.0, 2.0, 2.0, 2.0; - new_i_clamp << 2.0, 2.0, 2.0, 2.0, 2.0, 2.0; - bool new_gains = false; - - std::vector vals; - vals.push_back(impedance_plugin_ptr->setKp(new_kp)); - vals.push_back(impedance_plugin_ptr->setKd(new_kd)); - vals.push_back(impedance_plugin_ptr->setKi(new_ki)); - vals.push_back(impedance_plugin_ptr->setIClamp(new_i_clamp)); - impedance_plugin_ptr->setGainsInEndEffectorFrame(new_gains); - std::cout << vals.at(0) << vals.at(1) << vals.at(2) << vals.at(3) << std::endl; - - std::cout << "2" << std::endl; - std::cout << impedance_plugin_ptr->kp()(2) << std::endl; - std::cout << impedance_plugin_ptr->kd()(2) << std::endl; - std::cout << impedance_plugin_ptr->ki()(2) << std::endl; - std::cout << impedance_plugin_ptr->iClamp().size() << std::endl; - std::cout << impedance_plugin_ptr->gainsInEndEffectorFrame() << std::endl; - - Eigen::VectorXd new_kp2, new_kd2, new_ki2, new_i_clamp2; - bool new_gains2 = true; - - std::vector vals2; - vals2.push_back(impedance_plugin_ptr->setKp(new_kp2)); - vals2.push_back(impedance_plugin_ptr->setKd(new_kd2)); - vals2.push_back(impedance_plugin_ptr->setKi(new_ki2)); - vals2.push_back(impedance_plugin_ptr->setIClamp(new_i_clamp2)); - impedance_plugin_ptr->setGainsInEndEffectorFrame(new_gains2); - std::cout << vals2.at(0) << vals2.at(1) << vals2.at(2) << vals2.at(3) << std::endl; - - std::cout << "3" << std::endl; - std::cout << impedance_plugin_ptr->kp()(2) << std::endl; - std::cout << impedance_plugin_ptr->kd()(2) << std::endl; - std::cout << impedance_plugin_ptr->ki()(2) << std::endl; - std::cout << impedance_plugin_ptr->iClamp().size() << std::endl; - std::cout << impedance_plugin_ptr->gainsInEndEffectorFrame() << std::endl; - - Eigen::VectorXd new_kp3(6), new_kd3(7), new_ki3(5), new_i_clamp3(6); - new_kp3 << 2.0, 2.0, -2.0, 2.0, 2.0, 2.0; - new_kd3 << 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0; - new_ki3 << 2.0, 2.0, 2.0, 2.0, 2.0; - new_i_clamp3 << 2.0, -2.0, 2.0, 2.0, 2.0, 2.0; - bool new_gains3 = false; - - std::vector vals3; - vals3.push_back(impedance_plugin_ptr->setKp(new_kp3)); - vals3.push_back(impedance_plugin_ptr->setKd(new_kd3)); - vals3.push_back(impedance_plugin_ptr->setKi(new_ki3)); - vals3.push_back(impedance_plugin_ptr->setIClamp(new_i_clamp3)); - impedance_plugin_ptr->setGainsInEndEffectorFrame(new_gains3); - std::cout << vals3.at(0) << vals3.at(1) << vals3.at(2) << vals3.at(3) << std::endl; - - std::cout << "4" << std::endl; - std::cout << impedance_plugin_ptr->kp()(2) << std::endl; - std::cout << impedance_plugin_ptr->kd()(2) << std::endl; - std::cout << impedance_plugin_ptr->ki()(2) << std::endl; - std::cout << impedance_plugin_ptr->iClamp().size() << std::endl; - std::cout << impedance_plugin_ptr->gainsInEndEffectorFrame() << std::endl; - - Eigen::VectorXd new_kp4(6), new_kd4(6), new_ki4(6), new_i_clamp4(6); - new_kp4 << 2.0, 2.0, 2.0, 2.0, 2.0, 2.0; - new_kd4 << 2.0, 2.0, 2.0, 2.0, 2.0, 2.0; - new_ki4 << 2.0, 2.0, 2.0, 2.0, 2.0, 2.0; - new_i_clamp4 << 2.0, 2.0, 2.0, 2.0, 2.0, 2.0; - bool new_gains4 = false; - - std::vector vals4; - vals4.push_back(impedance_plugin_ptr->setKp(new_kp4)); - vals4.push_back(impedance_plugin_ptr->setKd(new_kd4)); - vals4.push_back(impedance_plugin_ptr->setKi(new_ki4)); - vals4.push_back(impedance_plugin_ptr->setIClamp(new_i_clamp4)); - impedance_plugin_ptr->setGainsInEndEffectorFrame(new_gains4); - std::cout << vals4.at(0) << vals4.at(1) << vals4.at(2) << vals4.at(3) << std::endl; - - std::cout << "5" << std::endl; - std::cout << impedance_plugin_ptr->kp()(2) << std::endl; - std::cout << impedance_plugin_ptr->kd()(2) << std::endl; - std::cout << impedance_plugin_ptr->ki()(2) << std::endl; - std::cout << impedance_plugin_ptr->iClamp().size() << std::endl; - std::cout << impedance_plugin_ptr->gainsInEndEffectorFrame() << std::endl; - - // std::cout << impedance_plugin->kp()(2); - // impedance_plugin_ptr->applyParameter("kp", new_kp); - // impedance_plugin_ptr->applyParameter("kd", new_kd); - // impedance_plugin_ptr->applyParameter("ki", new_ki); - // impedance_plugin_ptr->applyParameter("i_clamp", new_i_clamp); - - // impedance_plugin_ptr->applyParameters(impedance_config, {"gains_in_end_effector_frame", "kp", "kd", "ki", "i_clamp"}); - // impedance_plugin_ptr->applyParameterImpl("kp", new_kp); - // Snap current pose to floor ee_position_floor = ee_position_curr; ee_position_floor(2) = floor_level; @@ -330,7 +215,7 @@ int main(int argc, char* argv[]) // 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 << "goal set\n"; + std::cout << "Hit floor!\n"; // Update flags to indicate being in contact with the floor floor_command_flag = false; @@ -339,15 +224,11 @@ int main(int argc, char* argv[]) // 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) { - // Set stiffness in air - impedance_config.float_lists_["kp"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - impedance_config.float_lists_["kd"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - impedance_config.float_lists_["ki"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - impedance_config.float_lists_["i_clamp"] = {10.0, 10.0, 10.0, 1.0, 1.0, 1.0}; - // 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; From f015b5fe718757a6733a6459fdc32de18fc26bc9 Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Thu, 25 Jul 2024 09:36:11 -0400 Subject: [PATCH 06/31] Properly used ki and i_clamp terms for fixed example --- kits/arm/example_impedance_control_fixed.cpp | 4 ++-- kits/arm/example_impedance_control_floor.cpp | 6 +----- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/kits/arm/example_impedance_control_fixed.cpp b/kits/arm/example_impedance_control_fixed.cpp index daf221dc..f0f241f6 100644 --- a/kits/arm/example_impedance_control_fixed.cpp +++ b/kits/arm/example_impedance_control_fixed.cpp @@ -65,8 +65,8 @@ int main(int argc, char* argv[]) hebi::experimental::arm::PluginConfig impedance_config("ImpedanceController", "ImpedanceController"); impedance_config.float_lists_["kp"] = {300.0, 300.0, 300.0, 5.0, 5.0, 1.0}; impedance_config.float_lists_["kd"] = {5.0, 5.0, 5.0, 0.0, 0.0, 0.0}; - impedance_config.float_lists_["ki"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - impedance_config.float_lists_["i_clamp"] = {10.0, 10.0, 10.0, 1.0, 1.0, 1.0}; + impedance_config.float_lists_["ki"] = {20.0, 20.0, 20.0, 0.5, 0.5, 0.5}; + impedance_config.float_lists_["i_clamp"] = {10.0, 10.0, 10.0, 1.0, 1.0, 1.0}; // Clamp on the end-effector wrench and NOT on the integral error impedance_config.bools_["gains_in_end_effector_frame"] = true; auto impedance_plugin = hebi::experimental::arm::plugin::ImpedanceController::create(impedance_config); diff --git a/kits/arm/example_impedance_control_floor.cpp b/kits/arm/example_impedance_control_floor.cpp index 89aca557..493f52c2 100644 --- a/kits/arm/example_impedance_control_floor.cpp +++ b/kits/arm/example_impedance_control_floor.cpp @@ -63,16 +63,12 @@ int main(int argc, char* argv[]) // However, ensure that nothing prevents the wrist's actuators from moving, and DO NOT place your fingers between them. hebi::experimental::arm::PluginConfig impedance_config("ImpedanceController", "ImpedanceController"); - // impedance_config.float_lists_["kp"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - // impedance_config.float_lists_["kd"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - // impedance_config.float_lists_["ki"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - // impedance_config.float_lists_["i_clamp"] = {10.0, 10.0, 10.0, 1.0, 1.0, 1.0}; - impedance_config.bools_["gains_in_end_effector_frame"] = true; impedance_config.float_lists_["kp"] = {300.0, 300.0, 300.0, 5.0, 5.0, 1.0}; impedance_config.float_lists_["kd"] = {5.0, 5.0, 5.0, 0.0, 0.0, 0.0}; impedance_config.float_lists_["ki"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; impedance_config.float_lists_["i_clamp"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + impedance_config.bools_["gains_in_end_effector_frame"] = true; auto impedance_plugin = hebi::experimental::arm::plugin::ImpedanceController::create(impedance_config); if (!impedance_plugin) { From b586b2f3933448c13a1076fc3cdb1b0ca293ed16 Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Thu, 25 Jul 2024 09:50:28 -0400 Subject: [PATCH 07/31] Correctly demonstrated how to use i_terms across all examples --- kits/arm/example_impedance_control_cartesian.cpp | 4 ++-- kits/arm/example_impedance_control_damping.cpp | 7 ++++--- kits/arm/example_impedance_control_floor.cpp | 4 ++-- kits/arm/example_impedance_control_gimbal.cpp | 4 ++-- 4 files changed, 10 insertions(+), 9 deletions(-) diff --git a/kits/arm/example_impedance_control_cartesian.cpp b/kits/arm/example_impedance_control_cartesian.cpp index 4accc6b9..ecaaff13 100644 --- a/kits/arm/example_impedance_control_cartesian.cpp +++ b/kits/arm/example_impedance_control_cartesian.cpp @@ -65,8 +65,8 @@ int main(int argc, char* argv[]) hebi::experimental::arm::PluginConfig impedance_config("ImpedanceController", "ImpedanceController"); impedance_config.float_lists_["kp"] = {300.0, 300.0, 300.0, 0.0, 0.0, 0.0}; impedance_config.float_lists_["kd"] = {5.0, 5.0, 5.0, 0.0, 0.0, 0.0}; - impedance_config.float_lists_["ki"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - impedance_config.float_lists_["i_clamp"] = {10.0, 10.0, 10.0, 1.0, 1.0, 1.0}; + impedance_config.float_lists_["ki"] = {20.0, 20.0, 20.0, 0.0, 0.0, 0.0}; + impedance_config.float_lists_["i_clamp"] = {10.0, 10.0, 10.0, 0.0, 0.0, 0.0}; // Clamp on the end-effector wrench and NOT on the integral error impedance_config.bools_["gains_in_end_effector_frame"] = true; auto impedance_plugin = hebi::experimental::arm::plugin::ImpedanceController::create(impedance_config); diff --git a/kits/arm/example_impedance_control_damping.cpp b/kits/arm/example_impedance_control_damping.cpp index 56ce3cef..8ceceea4 100644 --- a/kits/arm/example_impedance_control_damping.cpp +++ b/kits/arm/example_impedance_control_damping.cpp @@ -131,7 +131,7 @@ int main(int argc, char* argv[]) // Meters above the base for overdamped, critically damped, and underdamped cases respectively std::vector lower_limits = {0.0, 0.15, 0.3}; - // State variable for current mode: 0 for overdamped, 1 for crtically damped, 2 for underdamped, -1 for + // State variable for current mode: 0 for overdamped, 1 for crtically damped, 2 for underdamped, -1 for free int mode = -1; int prevmode = -1; std::vector damping_kp, damping_kd; @@ -189,7 +189,7 @@ int main(int argc, char* argv[]) if (!controller_on) { arm->cancelGoal(); - mode = -1; + mode = -1; // Free } else { @@ -198,7 +198,7 @@ int main(int argc, char* argv[]) 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]) @@ -207,6 +207,7 @@ int main(int argc, char* argv[]) } } + // Change gains only upon mode switches if (mode != prevmode) { impedance_plugin_ptr->setKp(damping_kp.at(mode)); diff --git a/kits/arm/example_impedance_control_floor.cpp b/kits/arm/example_impedance_control_floor.cpp index 493f52c2..64c03bdb 100644 --- a/kits/arm/example_impedance_control_floor.cpp +++ b/kits/arm/example_impedance_control_floor.cpp @@ -66,8 +66,8 @@ int main(int argc, char* argv[]) impedance_config.float_lists_["kp"] = {300.0, 300.0, 300.0, 5.0, 5.0, 1.0}; impedance_config.float_lists_["kd"] = {5.0, 5.0, 5.0, 0.0, 0.0, 0.0}; - impedance_config.float_lists_["ki"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - impedance_config.float_lists_["i_clamp"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + impedance_config.float_lists_["ki"] = {20.0, 20.0, 20.0, 0.5, 0.5, 0.5}; + impedance_config.float_lists_["i_clamp"] = {10.0, 10.0, 10.0, 1.0, 1.0, 1.0}; impedance_config.bools_["gains_in_end_effector_frame"] = true; auto impedance_plugin = hebi::experimental::arm::plugin::ImpedanceController::create(impedance_config); diff --git a/kits/arm/example_impedance_control_gimbal.cpp b/kits/arm/example_impedance_control_gimbal.cpp index ffce15e9..48fd6157 100644 --- a/kits/arm/example_impedance_control_gimbal.cpp +++ b/kits/arm/example_impedance_control_gimbal.cpp @@ -65,8 +65,8 @@ int main(int argc, char* argv[]) hebi::experimental::arm::PluginConfig impedance_config("ImpedanceController", "ImpedanceController"); impedance_config.float_lists_["kp"] = {0.0, 0.0, 0.0, 5.0, 5.0, 1.0}; impedance_config.float_lists_["kd"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - impedance_config.float_lists_["ki"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - impedance_config.float_lists_["i_clamp"] = {10.0, 10.0, 10.0, 1.0, 1.0, 1.0}; + impedance_config.float_lists_["ki"] = {0.0, 0.0, 0.0, 0.5, 0.5, 0.5}; + impedance_config.float_lists_["i_clamp"] = {0.0, 0.0, 0.0, 1.0, 1.0, 1.0}; // Clamp on the end-effector wrench and NOT on the integral error impedance_config.bools_["gains_in_end_effector_frame"] = true; auto impedance_plugin = hebi::experimental::arm::plugin::ImpedanceController::create(impedance_config); From d63877f787ee91688056c4b6595cf12f02718fd2 Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Thu, 25 Jul 2024 12:21:20 -0400 Subject: [PATCH 08/31] Changed prevmode back to -1 also, for consistency --- kits/arm/example_impedance_control_damping.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/kits/arm/example_impedance_control_damping.cpp b/kits/arm/example_impedance_control_damping.cpp index 8ceceea4..ac628878 100644 --- a/kits/arm/example_impedance_control_damping.cpp +++ b/kits/arm/example_impedance_control_damping.cpp @@ -190,6 +190,7 @@ int main(int argc, char* argv[]) { arm->cancelGoal(); mode = -1; // Free + prevmode = -1; } else { From 88cece2e59b1d74130b648cec709bdb908062451 Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Thu, 25 Jul 2024 12:27:48 -0400 Subject: [PATCH 09/31] Added check for mode validity --- kits/arm/example_impedance_control_damping.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kits/arm/example_impedance_control_damping.cpp b/kits/arm/example_impedance_control_damping.cpp index ac628878..c02d011e 100644 --- a/kits/arm/example_impedance_control_damping.cpp +++ b/kits/arm/example_impedance_control_damping.cpp @@ -209,7 +209,7 @@ int main(int argc, char* argv[]) } // Change gains only upon mode switches - if (mode != prevmode) + if (mode != prevmode && mode >= 0) { impedance_plugin_ptr->setKp(damping_kp.at(mode)); impedance_plugin_ptr->setKd(damping_kd.at(mode)); From cf5717e9185d7fd7c145d8f83ccc06eaa8aab380 Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Fri, 9 Aug 2024 14:58:54 -0400 Subject: [PATCH 10/31] - CPP examples now follow config file convention - examples renamed to match python - hebi_util added for easier mobile_io creation from config file, and testing features that might be rolled in - cartesian impedance example fully integrated with config files - minor doc fixes --- kits/arm/config/examples/ex_AR_kit.cfg.yaml | 35 ++++++ .../examples/ex_AR_kit_w_gripper.cfg.yaml | 41 +++++++ .../examples/ex_gravity_compensation.cfg.yaml | 26 ++++ .../ex_impedance_control_cartesian.cfg.yaml | 42 +++++++ .../ex_impedance_control_damping.cfg.yaml | 53 ++++++++ .../ex_impedance_control_fixed.cfg.yaml | 42 +++++++ .../ex_impedance_control_floor.cfg.yaml | 42 +++++++ .../ex_impedance_control_gimbal.cfg.yaml | 42 +++++++ .../examples/ex_mobile_io_control.cfg.yaml | 34 ++++++ .../config/examples/ex_teach_repeat.cfg.yaml | 36 ++++++ .../ex_teach_repeat_w_gripper.cfg.yaml | 42 +++++++ kits/arm/{ => config}/gains/A-2080-01.xml | 0 kits/arm/{ => config}/gains/A-2084-01.xml | 0 kits/arm/{ => config}/gains/A-2085-03.xml | 0 kits/arm/{ => config}/gains/A-2085-04.xml | 0 kits/arm/{ => config}/gains/A-2085-05.xml | 0 kits/arm/{ => config}/gains/A-2085-06.xml | 0 kits/arm/{ => config}/gains/A-2240-04.xml | 0 kits/arm/{ => config}/gains/A-2240-05.xml | 0 kits/arm/{ => config}/gains/A-2240-06.xml | 0 kits/arm/{ => config}/gains/A-2255-01.xml | 0 kits/arm/{ => config}/gains/A-2302-01.xml | 0 kits/arm/{ => config}/gains/T-arm.xml | 0 .../gains/gripper_spool_gains.xml | 0 kits/arm/{ => config}/hrdf/A-2084-01.hrdf | 0 kits/arm/{ => config}/hrdf/A-2084-01G.hrdf | 0 kits/arm/{ => config}/hrdf/A-2085-03.hrdf | 0 kits/arm/{ => config}/hrdf/A-2085-04.hrdf | 0 kits/arm/{ => config}/hrdf/A-2085-05.hrdf | 0 kits/arm/{ => config}/hrdf/A-2085-05G.hrdf | 0 kits/arm/{ => config}/hrdf/A-2085-06.hrdf | 0 kits/arm/{ => config}/hrdf/A-2085-06G.hrdf | 0 kits/arm/{ => config}/hrdf/A-2240-04.hrdf | 0 kits/arm/{ => config}/hrdf/A-2240-05.hrdf | 0 kits/arm/{ => config}/hrdf/A-2240-05G.hrdf | 0 kits/arm/{ => config}/hrdf/A-2240-06.hrdf | 0 kits/arm/{ => config}/hrdf/A-2240-06G.hrdf | 0 kits/arm/{ => config}/hrdf/A-2302-01.hrdf | 0 kits/arm/{ => config}/hrdf/A-2302-01G.hrdf | 0 kits/arm/{ => config}/hrdf/T-arm.hrdf | 0 kits/arm/config/layouts/ar_control_sm.json | 49 ++++++++ kits/arm/config/layouts/ex_AR_kit.json | 61 ++++++++++ .../config/layouts/ex_AR_kit_w_gripper.json | 76 ++++++++++++ .../layouts/ex_gravity_compensation.json | 37 ++++++ .../ex_impedance_control_cartesian.json | 37 ++++++ .../layouts/ex_impedance_control_damping.json | 37 ++++++ .../layouts/ex_impedance_control_fixed.json | 37 ++++++ .../layouts/ex_impedance_control_floor.json | 37 ++++++ .../layouts/ex_impedance_control_gimbal.json | 37 ++++++ .../config/layouts/ex_mobile_io_control.json | 73 +++++++++++ kits/arm/config/layouts/ex_teach_repeat.json | 88 ++++++++++++++ .../layouts/ex_teach_repeat_w_gripper.json | 100 +++++++++++++++ .../config/layouts/joystick_control_sm.json | 112 +++++++++++++++++ .../arm/{example_AR_kit.cpp => ex_AR_kit.cpp} | 0 ...eat.cpp => ex_double_arm_teach_repeat.cpp} | 0 ...sation.cpp => ex_gravity_compensation.cpp} | 0 ...cpp => ex_impedance_control_cartesian.cpp} | 115 ++++++++---------- ...g.cpp => ex_impedance_control_damping.cpp} | 2 +- ...xed.cpp => ex_impedance_control_fixed.cpp} | 2 +- ...oor.cpp => ex_impedance_control_floor.cpp} | 2 +- ...al.cpp => ex_impedance_control_gimbal.cpp} | 2 +- ...o_control.cpp => ex_mobile_io_control.cpp} | 0 ...e_teach_repeat.cpp => ex_teach_repeat.cpp} | 0 ...pper.cpp => ex_teach_repeat_w_gripper.cpp} | 0 kits/arm/hebi_util.cpp | 78 ++++++++++++ kits/arm/hebi_util.hpp | 29 +++++ projects/cmake/CMakeLists.txt | 31 +++-- 67 files changed, 1390 insertions(+), 87 deletions(-) create mode 100644 kits/arm/config/examples/ex_AR_kit.cfg.yaml create mode 100644 kits/arm/config/examples/ex_AR_kit_w_gripper.cfg.yaml create mode 100644 kits/arm/config/examples/ex_gravity_compensation.cfg.yaml create mode 100644 kits/arm/config/examples/ex_impedance_control_cartesian.cfg.yaml create mode 100644 kits/arm/config/examples/ex_impedance_control_damping.cfg.yaml create mode 100644 kits/arm/config/examples/ex_impedance_control_fixed.cfg.yaml create mode 100644 kits/arm/config/examples/ex_impedance_control_floor.cfg.yaml create mode 100644 kits/arm/config/examples/ex_impedance_control_gimbal.cfg.yaml create mode 100644 kits/arm/config/examples/ex_mobile_io_control.cfg.yaml create mode 100644 kits/arm/config/examples/ex_teach_repeat.cfg.yaml create mode 100644 kits/arm/config/examples/ex_teach_repeat_w_gripper.cfg.yaml rename kits/arm/{ => config}/gains/A-2080-01.xml (100%) rename kits/arm/{ => config}/gains/A-2084-01.xml (100%) rename kits/arm/{ => config}/gains/A-2085-03.xml (100%) rename kits/arm/{ => config}/gains/A-2085-04.xml (100%) rename kits/arm/{ => config}/gains/A-2085-05.xml (100%) rename kits/arm/{ => config}/gains/A-2085-06.xml (100%) rename kits/arm/{ => config}/gains/A-2240-04.xml (100%) rename kits/arm/{ => config}/gains/A-2240-05.xml (100%) rename kits/arm/{ => config}/gains/A-2240-06.xml (100%) rename kits/arm/{ => config}/gains/A-2255-01.xml (100%) rename kits/arm/{ => config}/gains/A-2302-01.xml (100%) rename kits/arm/{ => config}/gains/T-arm.xml (100%) rename kits/arm/{ => config}/gains/gripper_spool_gains.xml (100%) rename kits/arm/{ => config}/hrdf/A-2084-01.hrdf (100%) rename kits/arm/{ => config}/hrdf/A-2084-01G.hrdf (100%) rename kits/arm/{ => config}/hrdf/A-2085-03.hrdf (100%) rename kits/arm/{ => config}/hrdf/A-2085-04.hrdf (100%) rename kits/arm/{ => config}/hrdf/A-2085-05.hrdf (100%) rename kits/arm/{ => config}/hrdf/A-2085-05G.hrdf (100%) rename kits/arm/{ => config}/hrdf/A-2085-06.hrdf (100%) rename kits/arm/{ => config}/hrdf/A-2085-06G.hrdf (100%) rename kits/arm/{ => config}/hrdf/A-2240-04.hrdf (100%) rename kits/arm/{ => config}/hrdf/A-2240-05.hrdf (100%) rename kits/arm/{ => config}/hrdf/A-2240-05G.hrdf (100%) rename kits/arm/{ => config}/hrdf/A-2240-06.hrdf (100%) rename kits/arm/{ => config}/hrdf/A-2240-06G.hrdf (100%) rename kits/arm/{ => config}/hrdf/A-2302-01.hrdf (100%) rename kits/arm/{ => config}/hrdf/A-2302-01G.hrdf (100%) rename kits/arm/{ => config}/hrdf/T-arm.hrdf (100%) create mode 100644 kits/arm/config/layouts/ar_control_sm.json create mode 100755 kits/arm/config/layouts/ex_AR_kit.json create mode 100755 kits/arm/config/layouts/ex_AR_kit_w_gripper.json create mode 100755 kits/arm/config/layouts/ex_gravity_compensation.json create mode 100755 kits/arm/config/layouts/ex_impedance_control_cartesian.json create mode 100755 kits/arm/config/layouts/ex_impedance_control_damping.json create mode 100755 kits/arm/config/layouts/ex_impedance_control_fixed.json create mode 100755 kits/arm/config/layouts/ex_impedance_control_floor.json create mode 100755 kits/arm/config/layouts/ex_impedance_control_gimbal.json create mode 100644 kits/arm/config/layouts/ex_mobile_io_control.json create mode 100755 kits/arm/config/layouts/ex_teach_repeat.json create mode 100644 kits/arm/config/layouts/ex_teach_repeat_w_gripper.json create mode 100644 kits/arm/config/layouts/joystick_control_sm.json rename kits/arm/{example_AR_kit.cpp => ex_AR_kit.cpp} (100%) rename kits/arm/{example_double_arm_teach_repeat.cpp => ex_double_arm_teach_repeat.cpp} (100%) rename kits/arm/{example_gravity_compensation.cpp => ex_gravity_compensation.cpp} (100%) rename kits/arm/{example_impedance_control_cartesian.cpp => ex_impedance_control_cartesian.cpp} (59%) rename kits/arm/{example_impedance_control_damping.cpp => ex_impedance_control_damping.cpp} (97%) rename kits/arm/{example_impedance_control_fixed.cpp => ex_impedance_control_fixed.cpp} (97%) rename kits/arm/{example_impedance_control_floor.cpp => ex_impedance_control_floor.cpp} (97%) rename kits/arm/{example_impedance_control_gimbal.cpp => ex_impedance_control_gimbal.cpp} (97%) rename kits/arm/{example_mobile_io_control.cpp => ex_mobile_io_control.cpp} (100%) rename kits/arm/{example_teach_repeat.cpp => ex_teach_repeat.cpp} (100%) rename kits/arm/{example_teach_repeat_with_gripper.cpp => ex_teach_repeat_w_gripper.cpp} (100%) create mode 100644 kits/arm/hebi_util.cpp create mode 100644 kits/arm/hebi_util.hpp diff --git a/kits/arm/config/examples/ex_AR_kit.cfg.yaml b/kits/arm/config/examples/ex_AR_kit.cfg.yaml new file mode 100644 index 00000000..2fff73c2 --- /dev/null +++ b/kits/arm/config/examples/ex_AR_kit.cfg.yaml @@ -0,0 +1,35 @@ +# 6-DoF Arm +version: 1.0 +families: ["Arm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "../hrdf/A-2085-06.hrdf" + +gains: + default: "../gains/A-2085-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: true + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + +user_data: + + # Joint angles at home for AR demo: [0, pi/3, 2*pi/3, 5*pi/6, -pi/2, 0] + home_position: [0, 1.0471975511965976, 2.0943951023931953, 2.6179938779914944, -1.5707963267948966, 0] # radians + + # Time taken for a steady motion to the home position + soft_start_time: 4 # seconds + + # Displacements of the phone are scaled by this value to give displacement of the end-effector + xyz_scale: 0.75 + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "../layouts/ex_AR_kit.json" \ No newline at end of file diff --git a/kits/arm/config/examples/ex_AR_kit_w_gripper.cfg.yaml b/kits/arm/config/examples/ex_AR_kit_w_gripper.cfg.yaml new file mode 100644 index 00000000..813c4cb5 --- /dev/null +++ b/kits/arm/config/examples/ex_AR_kit_w_gripper.cfg.yaml @@ -0,0 +1,41 @@ +# 6-DoF Arm +version: 1.0 +families: ["Arm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "../hrdf/A-2240-06G.hrdf" + +gains: + default: "../gains/A-2240-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: true + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + +user_data: + + # Joint angles at home for AR demo: [0, pi/3, pi/2, 2*pi/3, -pi/2, 0] + home_position: [0, 1.0471975511965976, 1.5707963267948966, 2.0943951023931953, -1.5707963267948966, 0] # radians + + # Time taken for a steady motion to the home position + soft_start_time: 4 # seconds + + # Displacements of the phone are scaled by this value to give displacement of the end-effector + xyz_scale: 0.75 + + gripper_family: "Arm" + gripper_name: "gripperSpool" + gripper_gains: "../gains/gripper_spool_gains.xml" + gripper_close_effort: -5.0 # (Nm) Effort applied to close the gripper. More negative effort will pinch the gripper harder. + gripper_open_effort: 1.0 # (Nm) Effort applied to open the gripper. More positive effort will NOT make the gripper harder to close. + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "../layouts/ex_AR_kit_w_gripper.json" \ No newline at end of file diff --git a/kits/arm/config/examples/ex_gravity_compensation.cfg.yaml b/kits/arm/config/examples/ex_gravity_compensation.cfg.yaml new file mode 100644 index 00000000..9426affe --- /dev/null +++ b/kits/arm/config/examples/ex_gravity_compensation.cfg.yaml @@ -0,0 +1,26 @@ +# 6-DoF Arm +version: 1.0 +families: ["Arm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "../hrdf/A-2085-06.hrdf" + +gains: + default: "../gains/A-2085-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: true + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + +user_data: + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "../layouts/ex_gravity_compensation.json" \ No newline at end of file diff --git a/kits/arm/config/examples/ex_impedance_control_cartesian.cfg.yaml b/kits/arm/config/examples/ex_impedance_control_cartesian.cfg.yaml new file mode 100644 index 00000000..7dc399d0 --- /dev/null +++ b/kits/arm/config/examples/ex_impedance_control_cartesian.cfg.yaml @@ -0,0 +1,42 @@ +# 6-DoF Arm +version: 1.0 +families: ["HEBIArm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "../hrdf/A-2085-06.hrdf" + +gains: + default: "../gains/A-2085-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: true + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + + # An impedance controller adds a virtual spring to the + # end-effector and can improve tracking. It can be enabled + # by setting 'enabled' to true. The gains are in the form of + # [x, y, z, rx, ry, rz]. Setting gains of zero deactivates + # the corresponding degree of translation or rotation. + # These gains correspond to a translational spring which maintains position but not orientation + - type: ImpedanceController + name: impedanceController + enabled: true + ramp_time: 0 + gains_in_end_effector_frame: true + kp: [300, 300, 300, 0, 0, 0] # (N/m) or (Nm/rad) + kd: [5, 5, 5, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) + ki: [20, 20, 20, 0, 0, 0] + i_clamp: [10, 10, 10, 0, 0, 0] # max value + +user_data: + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "../layouts/ex_impedance_control_cartesian.json" \ No newline at end of file diff --git a/kits/arm/config/examples/ex_impedance_control_damping.cfg.yaml b/kits/arm/config/examples/ex_impedance_control_damping.cfg.yaml new file mode 100644 index 00000000..ab29825f --- /dev/null +++ b/kits/arm/config/examples/ex_impedance_control_damping.cfg.yaml @@ -0,0 +1,53 @@ +# 6-DoF Arm +version: 1.0 +families: ["Arm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "../hrdf/A-2085-06.hrdf" + +gains: + default: "../gains/A-2085-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: true + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + + # An impedance controller adds a virtual spring to the + # end-effector and can improve tracking. It can be enabled + # by setting 'enabled' to true. The gains are in the form of + # [x, y, z, rx, ry, rz]. Setting gains of zero deactivates + # the corresponding degree of translation or rotation. + # These gains are initialized to zero as the will be changed over the course of the damping example + - type: ImpedanceController + name: impedanceController + enabled: true + ramp_time: 0 + gains_in_end_effector_frame: false + kp: [0, 0, 0, 0, 0, 0] # (N/m) or (Nm/rad) + kd: [0, 0, 0, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) + +# Any extra configuration data is stored here +user_data: + + # Distance above the base for overdamped, critically damped, and underdamped cases respectively + lower_limits: [0.0, 0.15, 0.3] # (m) + + overdamped_kp: [100, 100, 0, 5, 5, 1] # (N/m) or (Nm/rad) + overdamped_kd: [15, 15, 1, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) + + critically_damped_kp: [100, 100, 0, 5, 5, 1] # (N/m) or (Nm/rad) + critically_damped_kd: [5, 5, 1, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) + + underdamped_kp: [100, 100, 0, 5, 5, 1] # (N/m) or (Nm/rad) + under_damped_kd: [0, 0, 0, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "../layouts/ex_impedance_control_damping.json" \ No newline at end of file diff --git a/kits/arm/config/examples/ex_impedance_control_fixed.cfg.yaml b/kits/arm/config/examples/ex_impedance_control_fixed.cfg.yaml new file mode 100644 index 00000000..9e6bce4a --- /dev/null +++ b/kits/arm/config/examples/ex_impedance_control_fixed.cfg.yaml @@ -0,0 +1,42 @@ +# 6-DoF Arm +version: 1.0 +families: ["Arm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "../hrdf/A-2085-06.hrdf" + +gains: + default: "../gains/A-2085-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: true + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + + # An impedance controller adds a virtual spring to the + # end-effector and can improve tracking. It can be enabled + # by setting 'enabled' to true. The gains are in the form of + # [x, y, z, rx, ry, rz]. Setting gains of zero deactivates + # the corresponding degree of translation or rotation. + # These gains correspond to a controller that rigidly maintains a fixed pose + - type: ImpedanceController + name: impedanceController + enabled: true + ramp_time: 0 + gains_in_end_effector_frame: true + kp: [300, 300, 300, 5, 5, 1] # (N/m) or (Nm/rad) + kd: [5, 5, 5, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) + ki: [20, 20, 20, 0.5, 0.5, 0.5] + i_clamp: [10, 10, 10, 1, 1, 1] # max value + +user_data: + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "../layouts/ex_impedance_control_fixed.json" \ No newline at end of file diff --git a/kits/arm/config/examples/ex_impedance_control_floor.cfg.yaml b/kits/arm/config/examples/ex_impedance_control_floor.cfg.yaml new file mode 100644 index 00000000..5480b666 --- /dev/null +++ b/kits/arm/config/examples/ex_impedance_control_floor.cfg.yaml @@ -0,0 +1,42 @@ +# 6-DoF Arm +version: 1.0 +families: ["Arm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "../hrdf/A-2085-06.hrdf" + +gains: + default: "../gains/A-2085-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: true + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + + # An impedance controller adds a virtual spring to the + # end-effector and can improve tracking. It can be enabled + # by setting 'enabled' to true. The gains are in the form of + # [x, y, z, rx, ry, rz]. Setting gains of zero deactivates + # the corresponding degree of translation or rotation. + # These gains correspond to the stiffness of the virtual floor + - type: ImpedanceController + name: impedanceController + enabled: true + ramp_time: 0 + gains_in_end_effector_frame: true + kp: [500, 500, 500, 5, 5, 1] # (N/m) or (Nm/rad) + kd: [5, 5, 5, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) + ki: [20, 20, 20, 0.5, 0.5, 0.5] + i_clamp: [10, 10, 10, 1, 1, 1] # max value + +user_data: + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "../layouts/ex_impedance_control_floor.json" \ No newline at end of file diff --git a/kits/arm/config/examples/ex_impedance_control_gimbal.cfg.yaml b/kits/arm/config/examples/ex_impedance_control_gimbal.cfg.yaml new file mode 100644 index 00000000..9a772fc4 --- /dev/null +++ b/kits/arm/config/examples/ex_impedance_control_gimbal.cfg.yaml @@ -0,0 +1,42 @@ +# 6-DoF Arm +version: 1.0 +families: ["Arm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "../hrdf/A-2085-06.hrdf" + +gains: + default: "../gains/A-2085-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: true + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + + # An impedance controller adds a virtual spring to the + # end-effector and can improve tracking. It can be enabled + # by setting 'enabled' to true. The gains are in the form of + # [x, y, z, rx, ry, rz]. Setting gains of zero deactivates + # the corresponding degree of translation or rotation. + # These gains correspond to a rotational spring emulating a gimbal + - type: ImpedanceController + name: impedanceController + enabled: true + ramp_time: 0 + gains_in_end_effector_frame: true + kp: [0, 0, 0, 8, 8, 1] # (N/m) or (Nm/rad) + kd: [0, 0, 0, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) + ki: [0, 0, 0, 0.5, 0.5, 0.5] + i_clamp: [0, 0, 0, 1, 1, 1] # max value + +user_data: + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "../layouts/ex_impedance_control_gimbal.json" \ No newline at end of file diff --git a/kits/arm/config/examples/ex_mobile_io_control.cfg.yaml b/kits/arm/config/examples/ex_mobile_io_control.cfg.yaml new file mode 100644 index 00000000..ae452cda --- /dev/null +++ b/kits/arm/config/examples/ex_mobile_io_control.cfg.yaml @@ -0,0 +1,34 @@ +# 6-DoF Arm +version: 1.0 +families: ["Arm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "../hrdf/A-2085-06.hrdf" + +gains: + default: "../gains/A-2085-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: true + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + +user_data: + + # Three waypoints for Buttons - B1, B2, B3 + waypoint_1: [0, 0, 0, 0, 0, 0] + waypoint_2: [0.7853981633974483, 1.0471975511965976, 2.0943951023931953, 1.0471975511965976, 0.7853981633974483, 0] # [pi/4, pi/3, 2*pi/3, pi/3, pi/4, 0] + waypoint_3: [-0.7853981633974483, 1.0471975511965976, 2.0943951023931953, 1.0471975511965976, 2.356194490192345, 0] # [-pi/4, pi/3, 2*pi/3, pi/3, 3*pi/4, 0] + + # Time taken to travel to a waypoint + travel_time: 3 # seconds + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "../layouts/ex_mobile_io_control.json" \ No newline at end of file diff --git a/kits/arm/config/examples/ex_teach_repeat.cfg.yaml b/kits/arm/config/examples/ex_teach_repeat.cfg.yaml new file mode 100644 index 00000000..1cf332d9 --- /dev/null +++ b/kits/arm/config/examples/ex_teach_repeat.cfg.yaml @@ -0,0 +1,36 @@ +# 6-DoF Arm +version: 1.0 +families: ["Arm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "../hrdf/A-2085-06.hrdf" + +gains: + default: "../gains/A-2085-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: true + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + +user_data: + + # Travel time is calculated using + # base_travel_time + slider * (base_travel_time - min_travel_time) + # Since slider goes from -1 to 1, travel time goes from min_travel_time to 2 * base_travel_time - min_travel_time + + # Minimum travel time value + min_travel_time: 0.5 + + # The default travel time value + base_travel_time: 3 # seconds + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "../layouts/ex_teach_repeat.json" \ No newline at end of file diff --git a/kits/arm/config/examples/ex_teach_repeat_w_gripper.cfg.yaml b/kits/arm/config/examples/ex_teach_repeat_w_gripper.cfg.yaml new file mode 100644 index 00000000..49a70588 --- /dev/null +++ b/kits/arm/config/examples/ex_teach_repeat_w_gripper.cfg.yaml @@ -0,0 +1,42 @@ +# 6-DoF Arm +version: 1.0 +families: ["Arm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "../hrdf/A-2240-06G.hrdf" + +gains: + default: "../gains/A-2240-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: true + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + +user_data: + + # Travel time is calculated using + # base_travel_time + slider * (base_travel_time - min_travel_time) + # Since slider goes from -1 to 1, travel time goes from min_travel_time to 2 * base_travel_time - min_travel_time + + # Minimum travel time value + min_travel_time: 0.5 + + # The default travel time value + base_travel_time: 3 # seconds + + gripper_family: "Arm" + gripper_name: "gripperSpool" + gripper_gains: "../gains/gripper_spool_gains.xml" + gripper_close_effort: -5.0 # (Nm) Effort applied to close the gripper. More negative effort will pinch the gripper harder. + gripper_open_effort: 1.0 # (Nm) Effort applied to open the gripper. More positive effort will NOT make the gripper harder to close. + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "../layouts/ex_AR_kit_w_gripper.json" \ No newline at end of file diff --git a/kits/arm/gains/A-2080-01.xml b/kits/arm/config/gains/A-2080-01.xml similarity index 100% rename from kits/arm/gains/A-2080-01.xml rename to kits/arm/config/gains/A-2080-01.xml diff --git a/kits/arm/gains/A-2084-01.xml b/kits/arm/config/gains/A-2084-01.xml similarity index 100% rename from kits/arm/gains/A-2084-01.xml rename to kits/arm/config/gains/A-2084-01.xml diff --git a/kits/arm/gains/A-2085-03.xml b/kits/arm/config/gains/A-2085-03.xml similarity index 100% rename from kits/arm/gains/A-2085-03.xml rename to kits/arm/config/gains/A-2085-03.xml diff --git a/kits/arm/gains/A-2085-04.xml b/kits/arm/config/gains/A-2085-04.xml similarity index 100% rename from kits/arm/gains/A-2085-04.xml rename to kits/arm/config/gains/A-2085-04.xml diff --git a/kits/arm/gains/A-2085-05.xml b/kits/arm/config/gains/A-2085-05.xml similarity index 100% rename from kits/arm/gains/A-2085-05.xml rename to kits/arm/config/gains/A-2085-05.xml diff --git a/kits/arm/gains/A-2085-06.xml b/kits/arm/config/gains/A-2085-06.xml similarity index 100% rename from kits/arm/gains/A-2085-06.xml rename to kits/arm/config/gains/A-2085-06.xml diff --git a/kits/arm/gains/A-2240-04.xml b/kits/arm/config/gains/A-2240-04.xml similarity index 100% rename from kits/arm/gains/A-2240-04.xml rename to kits/arm/config/gains/A-2240-04.xml diff --git a/kits/arm/gains/A-2240-05.xml b/kits/arm/config/gains/A-2240-05.xml similarity index 100% rename from kits/arm/gains/A-2240-05.xml rename to kits/arm/config/gains/A-2240-05.xml diff --git a/kits/arm/gains/A-2240-06.xml b/kits/arm/config/gains/A-2240-06.xml similarity index 100% rename from kits/arm/gains/A-2240-06.xml rename to kits/arm/config/gains/A-2240-06.xml diff --git a/kits/arm/gains/A-2255-01.xml b/kits/arm/config/gains/A-2255-01.xml similarity index 100% rename from kits/arm/gains/A-2255-01.xml rename to kits/arm/config/gains/A-2255-01.xml diff --git a/kits/arm/gains/A-2302-01.xml b/kits/arm/config/gains/A-2302-01.xml similarity index 100% rename from kits/arm/gains/A-2302-01.xml rename to kits/arm/config/gains/A-2302-01.xml diff --git a/kits/arm/gains/T-arm.xml b/kits/arm/config/gains/T-arm.xml similarity index 100% rename from kits/arm/gains/T-arm.xml rename to kits/arm/config/gains/T-arm.xml diff --git a/kits/arm/gains/gripper_spool_gains.xml b/kits/arm/config/gains/gripper_spool_gains.xml similarity index 100% rename from kits/arm/gains/gripper_spool_gains.xml rename to kits/arm/config/gains/gripper_spool_gains.xml diff --git a/kits/arm/hrdf/A-2084-01.hrdf b/kits/arm/config/hrdf/A-2084-01.hrdf similarity index 100% rename from kits/arm/hrdf/A-2084-01.hrdf rename to kits/arm/config/hrdf/A-2084-01.hrdf diff --git a/kits/arm/hrdf/A-2084-01G.hrdf b/kits/arm/config/hrdf/A-2084-01G.hrdf similarity index 100% rename from kits/arm/hrdf/A-2084-01G.hrdf rename to kits/arm/config/hrdf/A-2084-01G.hrdf diff --git a/kits/arm/hrdf/A-2085-03.hrdf b/kits/arm/config/hrdf/A-2085-03.hrdf similarity index 100% rename from kits/arm/hrdf/A-2085-03.hrdf rename to kits/arm/config/hrdf/A-2085-03.hrdf diff --git a/kits/arm/hrdf/A-2085-04.hrdf b/kits/arm/config/hrdf/A-2085-04.hrdf similarity index 100% rename from kits/arm/hrdf/A-2085-04.hrdf rename to kits/arm/config/hrdf/A-2085-04.hrdf diff --git a/kits/arm/hrdf/A-2085-05.hrdf b/kits/arm/config/hrdf/A-2085-05.hrdf similarity index 100% rename from kits/arm/hrdf/A-2085-05.hrdf rename to kits/arm/config/hrdf/A-2085-05.hrdf diff --git a/kits/arm/hrdf/A-2085-05G.hrdf b/kits/arm/config/hrdf/A-2085-05G.hrdf similarity index 100% rename from kits/arm/hrdf/A-2085-05G.hrdf rename to kits/arm/config/hrdf/A-2085-05G.hrdf diff --git a/kits/arm/hrdf/A-2085-06.hrdf b/kits/arm/config/hrdf/A-2085-06.hrdf similarity index 100% rename from kits/arm/hrdf/A-2085-06.hrdf rename to kits/arm/config/hrdf/A-2085-06.hrdf diff --git a/kits/arm/hrdf/A-2085-06G.hrdf b/kits/arm/config/hrdf/A-2085-06G.hrdf similarity index 100% rename from kits/arm/hrdf/A-2085-06G.hrdf rename to kits/arm/config/hrdf/A-2085-06G.hrdf diff --git a/kits/arm/hrdf/A-2240-04.hrdf b/kits/arm/config/hrdf/A-2240-04.hrdf similarity index 100% rename from kits/arm/hrdf/A-2240-04.hrdf rename to kits/arm/config/hrdf/A-2240-04.hrdf diff --git a/kits/arm/hrdf/A-2240-05.hrdf b/kits/arm/config/hrdf/A-2240-05.hrdf similarity index 100% rename from kits/arm/hrdf/A-2240-05.hrdf rename to kits/arm/config/hrdf/A-2240-05.hrdf diff --git a/kits/arm/hrdf/A-2240-05G.hrdf b/kits/arm/config/hrdf/A-2240-05G.hrdf similarity index 100% rename from kits/arm/hrdf/A-2240-05G.hrdf rename to kits/arm/config/hrdf/A-2240-05G.hrdf diff --git a/kits/arm/hrdf/A-2240-06.hrdf b/kits/arm/config/hrdf/A-2240-06.hrdf similarity index 100% rename from kits/arm/hrdf/A-2240-06.hrdf rename to kits/arm/config/hrdf/A-2240-06.hrdf diff --git a/kits/arm/hrdf/A-2240-06G.hrdf b/kits/arm/config/hrdf/A-2240-06G.hrdf similarity index 100% rename from kits/arm/hrdf/A-2240-06G.hrdf rename to kits/arm/config/hrdf/A-2240-06G.hrdf diff --git a/kits/arm/hrdf/A-2302-01.hrdf b/kits/arm/config/hrdf/A-2302-01.hrdf similarity index 100% rename from kits/arm/hrdf/A-2302-01.hrdf rename to kits/arm/config/hrdf/A-2302-01.hrdf diff --git a/kits/arm/hrdf/A-2302-01G.hrdf b/kits/arm/config/hrdf/A-2302-01G.hrdf similarity index 100% rename from kits/arm/hrdf/A-2302-01G.hrdf rename to kits/arm/config/hrdf/A-2302-01G.hrdf diff --git a/kits/arm/hrdf/T-arm.hrdf b/kits/arm/config/hrdf/T-arm.hrdf similarity index 100% rename from kits/arm/hrdf/T-arm.hrdf rename to kits/arm/config/hrdf/T-arm.hrdf diff --git a/kits/arm/config/layouts/ar_control_sm.json b/kits/arm/config/layouts/ar_control_sm.json new file mode 100644 index 00000000..41401bbc --- /dev/null +++ b/kits/arm/config/layouts/ar_control_sm.json @@ -0,0 +1,49 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.5, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "Home", + "mode": 0 + } + }, + { + "id": "b2", + "type": "button", + "x": 0, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "Arm Control", + "mode": 0 + } + }, + { + "id": "b3", + "type": "button", + "x": 0.5, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "Gripper", + "mode": 0 + } + }, + { + "id": "led", + "type": "led", + "x": 0, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] \ No newline at end of file diff --git a/kits/arm/config/layouts/ex_AR_kit.json b/kits/arm/config/layouts/ex_AR_kit.json new file mode 100755 index 00000000..1c03a393 --- /dev/null +++ b/kits/arm/config/layouts/ex_AR_kit.json @@ -0,0 +1,61 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.75, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🏠", + "mode": "momentary" + } + }, + { + "id": "b3", + "type": "button", + "x": -0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "📲", + "mode": "momentary" + } + }, + { + "id": "b6", + "type": "button", + "x": 0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🌍", + "mode": "momentary" + } + }, + { + "id": "b8", + "type": "button", + "x": 0.75, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "❌", + "mode": "momentary" + } + }, + { + "id": "led", + "type": "led", + "x": 0, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] diff --git a/kits/arm/config/layouts/ex_AR_kit_w_gripper.json b/kits/arm/config/layouts/ex_AR_kit_w_gripper.json new file mode 100755 index 00000000..19b03251 --- /dev/null +++ b/kits/arm/config/layouts/ex_AR_kit_w_gripper.json @@ -0,0 +1,76 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.75, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🏠", + "mode": "momentary" + } + }, + { + "id": "b3", + "type": "button", + "x": -0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "📳", + "mode": "momentary" + } + }, + { + "id": "b6", + "type": "button", + "x": 0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🌍", + "mode": "momentary" + } + }, + { + "id": "b8", + "type": "button", + "x": 0.75, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "❌", + "mode": "momentary" + } + }, + { + "id": "a3", + "type": "rotslider", + "x": -0.25, + "y": 0.6, + "width": 0.05, + "height": 0.5, + "parameters": { + "text": "🤌", + "steps": 10, + "min": -1, + "max": 1, + "rest": 0 + } + }, + { + "id": "led", + "type": "led", + "x": 0.25, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] \ No newline at end of file diff --git a/kits/arm/config/layouts/ex_gravity_compensation.json b/kits/arm/config/layouts/ex_gravity_compensation.json new file mode 100755 index 00000000..9057c8c4 --- /dev/null +++ b/kits/arm/config/layouts/ex_gravity_compensation.json @@ -0,0 +1,37 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "❌", + "mode": "momentary" + } + }, + { + "id": "b2", + "type": "button", + "x": 0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🌍", + "mode": "toggle" + } + }, + { + "id": "led", + "type": "led", + "x": 0, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] \ No newline at end of file diff --git a/kits/arm/config/layouts/ex_impedance_control_cartesian.json b/kits/arm/config/layouts/ex_impedance_control_cartesian.json new file mode 100755 index 00000000..acb02d8c --- /dev/null +++ b/kits/arm/config/layouts/ex_impedance_control_cartesian.json @@ -0,0 +1,37 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "📈", + "mode": "momentary" + } + }, + { + "id": "b2", + "type": "button", + "x": 0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "📌", + "mode": "toggle" + } + }, + { + "id": "led", + "type": "led", + "x": 0, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] \ No newline at end of file diff --git a/kits/arm/config/layouts/ex_impedance_control_damping.json b/kits/arm/config/layouts/ex_impedance_control_damping.json new file mode 100755 index 00000000..43f34114 --- /dev/null +++ b/kits/arm/config/layouts/ex_impedance_control_damping.json @@ -0,0 +1,37 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "📈", + "mode": "momentary" + } + }, + { + "id": "b2", + "type": "button", + "x": 0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "💪", + "mode": "toggle" + } + }, + { + "id": "led", + "type": "led", + "x": 0, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] \ No newline at end of file diff --git a/kits/arm/config/layouts/ex_impedance_control_fixed.json b/kits/arm/config/layouts/ex_impedance_control_fixed.json new file mode 100755 index 00000000..09b7e359 --- /dev/null +++ b/kits/arm/config/layouts/ex_impedance_control_fixed.json @@ -0,0 +1,37 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "📈", + "mode": "momentary" + } + }, + { + "id": "b2", + "type": "button", + "x": 0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🛑", + "mode": "toggle" + } + }, + { + "id": "led", + "type": "led", + "x": 0, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] \ No newline at end of file diff --git a/kits/arm/config/layouts/ex_impedance_control_floor.json b/kits/arm/config/layouts/ex_impedance_control_floor.json new file mode 100755 index 00000000..fc04783b --- /dev/null +++ b/kits/arm/config/layouts/ex_impedance_control_floor.json @@ -0,0 +1,37 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "📈", + "mode": "momentary" + } + }, + { + "id": "b2", + "type": "button", + "x": 0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🧱", + "mode": "toggle" + } + }, + { + "id": "led", + "type": "led", + "x": 0, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] \ No newline at end of file diff --git a/kits/arm/config/layouts/ex_impedance_control_gimbal.json b/kits/arm/config/layouts/ex_impedance_control_gimbal.json new file mode 100755 index 00000000..e066913f --- /dev/null +++ b/kits/arm/config/layouts/ex_impedance_control_gimbal.json @@ -0,0 +1,37 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "📈", + "mode": "momentary" + } + }, + { + "id": "b2", + "type": "button", + "x": 0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🤳", + "mode": "toggle" + } + }, + { + "id": "led", + "type": "led", + "x": 0, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] \ No newline at end of file diff --git a/kits/arm/config/layouts/ex_mobile_io_control.json b/kits/arm/config/layouts/ex_mobile_io_control.json new file mode 100644 index 00000000..c531668a --- /dev/null +++ b/kits/arm/config/layouts/ex_mobile_io_control.json @@ -0,0 +1,73 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.8, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "1", + "mode": "momentary" + } + }, + { + "id": "b2", + "type": "button", + "x": -0.4, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "2", + "mode": "momentary" + } + }, + { + "id": "b3", + "type": "button", + "x": -0, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "3", + "mode": "momentary" + } + }, + { + "id": "b6", + "type": "button", + "x": 0.4, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🌍", + "mode": "momentary" + } + }, + { + "id": "b8", + "type": "button", + "x": 0.8, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "❌", + "mode": "momentary" + } + }, + { + "id": "led", + "type": "led", + "x": -0, + "y": -0.15, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] \ No newline at end of file diff --git a/kits/arm/config/layouts/ex_teach_repeat.json b/kits/arm/config/layouts/ex_teach_repeat.json new file mode 100755 index 00000000..063c3ff4 --- /dev/null +++ b/kits/arm/config/layouts/ex_teach_repeat.json @@ -0,0 +1,88 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.8, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "📌", + "mode": "momentary" + } + }, + { + "id": "b2", + "type": "button", + "x": -0.4, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🚏", + "mode": "momentary" + } + }, + { + "id": "b3", + "type": "button", + "x": -0, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🔄", + "mode": "momentary" + } + }, + { + "id": "b4", + "type": "button", + "x": 0.4, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🗑️", + "mode": "momentary" + } + }, + { + "id": "b8", + "type": "button", + "x": 0.8, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "❌", + "mode": "momentary" + } + }, + { + "id": "a3", + "type": "rotslider", + "x": -0.25, + "y": 0.6, + "width": 0.05, + "height": 0.5, + "parameters": { + "text": "⏱️", + "steps": 10, + "min": -1, + "max": 1, + "rest": 0 + } + }, + { + "id": "led", + "type": "led", + "x": 0.25, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] \ No newline at end of file diff --git a/kits/arm/config/layouts/ex_teach_repeat_w_gripper.json b/kits/arm/config/layouts/ex_teach_repeat_w_gripper.json new file mode 100644 index 00000000..20660c98 --- /dev/null +++ b/kits/arm/config/layouts/ex_teach_repeat_w_gripper.json @@ -0,0 +1,100 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.75, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "📌", + "mode": "momentary" + } + }, + { + "id": "b2", + "type": "button", + "x": -0.45, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🤌", + "mode": "momentary" + } + }, + { + "id": "b3", + "type": "button", + "x": -0.15, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🚏", + "mode": "momentary" + } + }, + { + "id": "b5", + "type": "button", + "x": 0.15, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🔄", + "mode": "momentary" + } + }, + { + "id": "b6", + "type": "button", + "x": 0.45, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🗑️", + "mode": "momentary" + } + }, + { + "id": "b8", + "type": "button", + "x": 0.75, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "❌", + "mode": "momentary" + } + }, + { + "id": "a3", + "type": "rotslider", + "x": -0.25, + "y": 0.6, + "width": 0.05, + "height": 0.5, + "parameters": { + "text": "Time to Waypoint", + "steps": 10, + "min": -1, + "max": 1, + "rest": 0 + } + }, + { + "id": "led", + "type": "led", + "x": 0.25, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] \ No newline at end of file diff --git a/kits/arm/config/layouts/joystick_control_sm.json b/kits/arm/config/layouts/joystick_control_sm.json new file mode 100644 index 00000000..d95b7bcb --- /dev/null +++ b/kits/arm/config/layouts/joystick_control_sm.json @@ -0,0 +1,112 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.8, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "Home", + "mode": 0 + } + }, + { + "id": "b4", + "type": "button", + "x": -0.4, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "Quit", + "mode": 0 + } + }, + { + "id": "b6", + "type": "button", + "x": -0, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "\u21E7", + "mode": 0 + } + }, + { + "id": "b7", + "type": "button", + "x": 0.4, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "Grip", + "mode": 1 + } + }, + { + "id": "b8", + "type": "button", + "x": 0.8, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "\u21E9", + "mode": 0 + } + }, + { + "id": "a1,a2", + "type": "joystick", + "x": -0.9, + "y": 0.88, + "width": 0.18, + "height": 0.22, + "parameters": { + "text1": "", + "text2": "Rotate" + } + }, + { + "id": "a3", + "type": "rotslider", + "x": -0.25, + "y": 0.6, + "width": 0.05, + "height": 0.5, + "parameters": { + "text": "Wrist", + "steps": 10, + "min": -1, + "max": 1, + "rest": 0 + } + }, + { + "id": "led", + "type": "led", + "x": 0.25, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + }, + { + "id": "a7,a8", + "type": "joystick", + "x": 0.9, + "y": 0.86, + "width": 0.18, + "height": 0.22, + "parameters": { + "text1": "", + "text2": "Translate" + } + } +] \ No newline at end of file diff --git a/kits/arm/example_AR_kit.cpp b/kits/arm/ex_AR_kit.cpp similarity index 100% rename from kits/arm/example_AR_kit.cpp rename to kits/arm/ex_AR_kit.cpp diff --git a/kits/arm/example_double_arm_teach_repeat.cpp b/kits/arm/ex_double_arm_teach_repeat.cpp similarity index 100% rename from kits/arm/example_double_arm_teach_repeat.cpp rename to kits/arm/ex_double_arm_teach_repeat.cpp diff --git a/kits/arm/example_gravity_compensation.cpp b/kits/arm/ex_gravity_compensation.cpp similarity index 100% rename from kits/arm/example_gravity_compensation.cpp rename to kits/arm/ex_gravity_compensation.cpp diff --git a/kits/arm/example_impedance_control_cartesian.cpp b/kits/arm/ex_impedance_control_cartesian.cpp similarity index 59% rename from kits/arm/example_impedance_control_cartesian.cpp rename to kits/arm/ex_impedance_control_cartesian.cpp index ecaaff13..a37a3cde 100644 --- a/kits/arm/example_impedance_control_cartesian.cpp +++ b/kits/arm/ex_impedance_control_cartesian.cpp @@ -18,13 +18,8 @@ This comprises the following demos: The following example is for the "Cartesian" demo: */ -// #include "lookup.hpp" -// #include "group.hpp" -#include "group_command.hpp" -#include "group_feedback.hpp" -#include "robot_model.hpp" -#include "arm/arm.hpp" -#include "util/mobile_io.hpp" +#include "hebi_util.hpp" +#include #include using namespace hebi; @@ -33,47 +28,37 @@ using namespace experimental; int main(int argc, char* argv[]) { ////////////////////////// - ///// Arm Setup ////////// + ///// Config Setup /////// ////////////////////////// - arm::Arm::Params params; - - // Setup Module Family and Module Names - params.families_ = {"HEBIArm-T"}; - params.names_ = {"J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"}; - - // Read HRDF file to seutp a RobotModel object for the 6-DoF Arm - // Make sure you are running this from the correct directory! - params.hrdf_file_ = "kits/arm/hrdf/T-arm.hrdf"; - - // Create the Arm Object - auto arm = arm::Arm::create(params); - while (!arm) { - arm = arm::Arm::create(params); + const std::string example_config_file = "config/examples/ex_impedance_control_cartesian.cfg.yaml"; + std::vector errors; + + 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; } - // Load the gains file that is approriate to the arm - arm -> loadGains("kits/arm/gains/T-arm.xml"); - - // Create and configure the ImpedanceController plugin - - // 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 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. + // For this demo, we need the arm and mobile_io + std::unique_ptr arm; + std::unique_ptr mobile_io; - hebi::experimental::arm::PluginConfig impedance_config("ImpedanceController", "ImpedanceController"); - impedance_config.float_lists_["kp"] = {300.0, 300.0, 300.0, 0.0, 0.0, 0.0}; - impedance_config.float_lists_["kd"] = {5.0, 5.0, 5.0, 0.0, 0.0, 0.0}; - impedance_config.float_lists_["ki"] = {20.0, 20.0, 20.0, 0.0, 0.0, 0.0}; - impedance_config.float_lists_["i_clamp"] = {10.0, 10.0, 10.0, 0.0, 0.0, 0.0}; // Clamp on the end-effector wrench and NOT on the integral error - impedance_config.bools_["gains_in_end_effector_frame"] = true; + ////////////////////////// + ///// Arm Setup ////////// + ////////////////////////// - auto impedance_plugin = hebi::experimental::arm::plugin::ImpedanceController::create(impedance_config); - if (!impedance_plugin) { - std::cerr << "Failed to create ImpedanceController plugin." << std::endl; - return -1; + // Create the arm object from the configuration + arm = hebi::experimental::arm::Arm::create(*example_config); + while (!arm) { + std::cerr << "Failed to create arm, retrying..." << std::endl; + std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait for 1 second before retrying + arm = hebi::experimental::arm::Arm::create(*example_config); } + std::cout << "Arm connected." << std::endl; // Initialize variables used to clear the commanded position and velocity in every cycle hebi::GroupCommand& command = arm->pendingCommand(); @@ -83,39 +68,30 @@ int main(int argc, char* argv[]) pos_nan.fill(std::numeric_limits::quiet_NaN()); vel_nan.fill(std::numeric_limits::quiet_NaN()); - // Add the plugin to the arm - if (!arm->addPlugin(std::move(impedance_plugin))) { - std::cerr << "Failed to add ImpedanceController plugin to arm." << std::endl; - return -1; - } - ////////////////////////// //// MobileIO Setup ////// ////////////////////////// - // Set up MobileIO - std::unique_ptr mobile = util::MobileIO::create(params.families_[0], "mobileIO"); - if (!mobile) - { - std::cout << "couldn't find mobile IO device!\n"; - return 1; + // Create the mobile_io object from the configuration + mobile_io = createMobileIOFromConfig(*example_config, example_config_file); + while (!mobile_io) { + std::cout << "Waiting for Mobile IO device to come online..." << std::endl; + std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait for 1 second before retrying + mobile_io = createMobileIOFromConfig(*example_config, example_config_file); } - mobile->setButtonMode(1, util::MobileIO::ButtonMode::Momentary); - mobile->setButtonLabel(1, "❌"); - mobile->setButtonMode(2, util::MobileIO::ButtonMode::Toggle); - mobile->setButtonLabel(2, "💪"); + std::cout << "MobileIO connected." << std::endl; std::string instructions; instructions = " Cartesian demo"; // Clear any garbage on screen - mobile->clearText(); + mobile_io->clearText(); // Display instructions on screen - mobile->appendText(instructions); + mobile_io->appendText(instructions); // Setup instructions - auto last_state = mobile->update(); + 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" @@ -123,6 +99,11 @@ int main(int argc, char* argv[]) << " 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 // ///////////////////////////// @@ -136,29 +117,29 @@ int main(int argc, char* argv[]) while(arm->update()) { - auto updated_mobile = mobile->update(0); + auto updated_mobile_io = mobile_io->update(0); - if (updated_mobile) + if (updated_mobile_io) { ///////////////// // Button Presses ///////////////// // Buttton B1 - End demo - if (mobile->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { + if (mobile_io->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { // Clear MobileIO text - mobile->resetUI(); + mobile_io->resetUI(); return 1; } // Button B2 - Set and unset impedance mode when button is pressed and released, respectively - if (mobile->getButtonDiff(2) == util::MobileIO::ButtonState::ToOn) { + if (mobile_io->getButtonDiff(2) == util::MobileIO::ButtonState::ToOn) { controller_on = true; arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPosition())); } - else if (mobile->getButtonDiff(2) == util::MobileIO::ButtonState::ToOff){ + else if (mobile_io->getButtonDiff(2) == util::MobileIO::ButtonState::ToOff){ controller_on = false; } @@ -178,7 +159,7 @@ int main(int argc, char* argv[]) } // Clear MobileIO text - mobile->clearText(); + mobile_io->clearText(); return 0; } diff --git a/kits/arm/example_impedance_control_damping.cpp b/kits/arm/ex_impedance_control_damping.cpp similarity index 97% rename from kits/arm/example_impedance_control_damping.cpp rename to kits/arm/ex_impedance_control_damping.cpp index c02d011e..1680e4e0 100644 --- a/kits/arm/example_impedance_control_damping.cpp +++ b/kits/arm/ex_impedance_control_damping.cpp @@ -58,7 +58,7 @@ int main(int argc, char* argv[]) // Create and configure the ImpedanceController plugin // 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 high enough to prevent large angular errors (greater than pi/2). The gains provided in these examples are well behaved. + // 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. diff --git a/kits/arm/example_impedance_control_fixed.cpp b/kits/arm/ex_impedance_control_fixed.cpp similarity index 97% rename from kits/arm/example_impedance_control_fixed.cpp rename to kits/arm/ex_impedance_control_fixed.cpp index f0f241f6..33fa0066 100644 --- a/kits/arm/example_impedance_control_fixed.cpp +++ b/kits/arm/ex_impedance_control_fixed.cpp @@ -58,7 +58,7 @@ int main(int argc, char* argv[]) // Create and configure the ImpedanceController plugin // 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 high enough to prevent large angular errors (greater than pi/2). The gains provided in these examples are well behaved. + // 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. diff --git a/kits/arm/example_impedance_control_floor.cpp b/kits/arm/ex_impedance_control_floor.cpp similarity index 97% rename from kits/arm/example_impedance_control_floor.cpp rename to kits/arm/ex_impedance_control_floor.cpp index 64c03bdb..360207b7 100644 --- a/kits/arm/example_impedance_control_floor.cpp +++ b/kits/arm/ex_impedance_control_floor.cpp @@ -58,7 +58,7 @@ int main(int argc, char* argv[]) // Create and configure the ImpedanceController plugin // 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 high enough to prevent large angular errors (greater than pi/2). The gains provided in these examples are well behaved. + // 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. diff --git a/kits/arm/example_impedance_control_gimbal.cpp b/kits/arm/ex_impedance_control_gimbal.cpp similarity index 97% rename from kits/arm/example_impedance_control_gimbal.cpp rename to kits/arm/ex_impedance_control_gimbal.cpp index 48fd6157..16a42f6e 100644 --- a/kits/arm/example_impedance_control_gimbal.cpp +++ b/kits/arm/ex_impedance_control_gimbal.cpp @@ -58,7 +58,7 @@ int main(int argc, char* argv[]) // Create and configure the ImpedanceController plugin // 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 high enough to prevent large angular errors (greater than pi/2). The gains provided in these examples are well behaved. + // 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. diff --git a/kits/arm/example_mobile_io_control.cpp b/kits/arm/ex_mobile_io_control.cpp similarity index 100% rename from kits/arm/example_mobile_io_control.cpp rename to kits/arm/ex_mobile_io_control.cpp diff --git a/kits/arm/example_teach_repeat.cpp b/kits/arm/ex_teach_repeat.cpp similarity index 100% rename from kits/arm/example_teach_repeat.cpp rename to kits/arm/ex_teach_repeat.cpp diff --git a/kits/arm/example_teach_repeat_with_gripper.cpp b/kits/arm/ex_teach_repeat_w_gripper.cpp similarity index 100% rename from kits/arm/example_teach_repeat_with_gripper.cpp rename to kits/arm/ex_teach_repeat_w_gripper.cpp diff --git a/kits/arm/hebi_util.cpp b/kits/arm/hebi_util.cpp new file mode 100644 index 00000000..ce976672 --- /dev/null +++ b/kits/arm/hebi_util.cpp @@ -0,0 +1,78 @@ +// 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, const std::string& config_file_path) { + std::map mobile_io_dict; + std::vector errors; + + hebi::util::file::File cfg_file(config_file_path); + auto parent_dir_absolute = cfg_file.getParentDirectory().getAbsolutePath(); + + // 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().count("mobile_io_family") && + config.getUserData().count("mobile_io_name") && + config.getUserData().count("mobile_io_layout")) { + + // Check that all required fields are present and are strings + if (config.getUserData().at("mobile_io_family").empty() || + config.getUserData().at("mobile_io_name").empty()) { + errors.push_back("HEBI config \"user_data\"'s \"mobile_io_...\" fields must be non-empty strings."); + } + + // Populate the dictionary + mobile_io_dict["family"] = config.getUserData().at("mobile_io_family"); + mobile_io_dict["name"] = config.getUserData().at("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().at("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"]); + + mobile_io->sendLayoutFile(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..843b894e --- /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, const std::string& config_file_path); + +// 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 184af91b..615739c5 100644 --- a/projects/cmake/CMakeLists.txt +++ b/projects/cmake/CMakeLists.txt @@ -274,17 +274,17 @@ endforeach (EXAMPLE ${ADVANCED_SOURCES}) # Kit Examples SET(ARM_SOURCES - ${ROOT_DIR}/kits/arm/example_gravity_compensation.cpp - ${ROOT_DIR}/kits/arm/example_mobile_io_control.cpp - ${ROOT_DIR}/kits/arm/example_teach_repeat.cpp - ${ROOT_DIR}/kits/arm/example_AR_kit.cpp - ${ROOT_DIR}/kits/arm/example_teach_repeat_with_gripper.cpp - ${ROOT_DIR}/kits/arm/example_double_arm_teach_repeat.cpp - ${ROOT_DIR}/kits/arm/example_impedance_control_fixed.cpp - ${ROOT_DIR}/kits/arm/example_impedance_control_cartesian.cpp - ${ROOT_DIR}/kits/arm/example_impedance_control_gimbal.cpp - ${ROOT_DIR}/kits/arm/example_impedance_control_floor.cpp - ${ROOT_DIR}/kits/arm/example_impedance_control_damping.cpp + ${ROOT_DIR}/kits/arm/ex_gravity_compensation.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_w_gripper.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) @@ -294,9 +294,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}) @@ -323,9 +323,8 @@ foreach (EXAMPLE ${ARM_SOURCES}) endforeach (EXAMPLE ${KIT_SOURCES}) -# Copy the HRDF files for kits into the build directory -file(COPY ${ROOT_DIR}/kits/arm/hrdf DESTINATION ${CMAKE_BINARY_DIR}/kits/arm) -file(COPY ${ROOT_DIR}/kits/arm/gains DESTINATION ${CMAKE_BINARY_DIR}/kits/arm) +# Copy the config directory for kits into the build directory +file(COPY ${ROOT_DIR}/kits/arm/config DESTINATION ${CMAKE_BINARY_DIR}/kits/arm) From 6443fa5361dc52e1b42d7e48b8086f0fb4f0bae5 Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Tue, 13 Aug 2024 10:20:58 -0400 Subject: [PATCH 11/31] All impedance control examples work Refactored hebi_util to use the updated for of user_data --- .../ex_impedance_control_damping.cfg.yaml | 4 +- .../ex_impedance_control_fixed.cfg.yaml | 2 +- .../ex_impedance_control_floor.cfg.yaml | 2 +- .../ex_impedance_control_gimbal.cfg.yaml | 2 +- kits/arm/ex_impedance_control_cartesian.cpp | 38 ++++- kits/arm/ex_impedance_control_damping.cpp | 151 ++++++++++-------- kits/arm/ex_impedance_control_fixed.cpp | 127 ++++++++------- kits/arm/ex_impedance_control_floor.cpp | 125 +++++++-------- kits/arm/ex_impedance_control_gimbal.cpp | 127 ++++++++------- kits/arm/hebi_util.cpp | 23 +-- 10 files changed, 325 insertions(+), 276 deletions(-) diff --git a/kits/arm/config/examples/ex_impedance_control_damping.cfg.yaml b/kits/arm/config/examples/ex_impedance_control_damping.cfg.yaml index ab29825f..31f2b961 100644 --- a/kits/arm/config/examples/ex_impedance_control_damping.cfg.yaml +++ b/kits/arm/config/examples/ex_impedance_control_damping.cfg.yaml @@ -1,6 +1,6 @@ # 6-DoF Arm version: 1.0 -families: ["Arm"] +families: ["HEBIArm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] hrdf: "../hrdf/A-2085-06.hrdf" @@ -46,7 +46,7 @@ user_data: critically_damped_kd: [5, 5, 1, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) underdamped_kp: [100, 100, 0, 5, 5, 1] # (N/m) or (Nm/rad) - under_damped_kd: [0, 0, 0, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) + underdamped_kd: [0, 0, 0, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) mobile_io_family: "Arm" mobile_io_name: "mobileIO" diff --git a/kits/arm/config/examples/ex_impedance_control_fixed.cfg.yaml b/kits/arm/config/examples/ex_impedance_control_fixed.cfg.yaml index 9e6bce4a..403c3049 100644 --- a/kits/arm/config/examples/ex_impedance_control_fixed.cfg.yaml +++ b/kits/arm/config/examples/ex_impedance_control_fixed.cfg.yaml @@ -1,6 +1,6 @@ # 6-DoF Arm version: 1.0 -families: ["Arm"] +families: ["HEBIArm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] hrdf: "../hrdf/A-2085-06.hrdf" diff --git a/kits/arm/config/examples/ex_impedance_control_floor.cfg.yaml b/kits/arm/config/examples/ex_impedance_control_floor.cfg.yaml index 5480b666..5ef3b879 100644 --- a/kits/arm/config/examples/ex_impedance_control_floor.cfg.yaml +++ b/kits/arm/config/examples/ex_impedance_control_floor.cfg.yaml @@ -1,6 +1,6 @@ # 6-DoF Arm version: 1.0 -families: ["Arm"] +families: ["HEBIArm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] hrdf: "../hrdf/A-2085-06.hrdf" diff --git a/kits/arm/config/examples/ex_impedance_control_gimbal.cfg.yaml b/kits/arm/config/examples/ex_impedance_control_gimbal.cfg.yaml index 9a772fc4..9e2b2da1 100644 --- a/kits/arm/config/examples/ex_impedance_control_gimbal.cfg.yaml +++ b/kits/arm/config/examples/ex_impedance_control_gimbal.cfg.yaml @@ -1,6 +1,6 @@ # 6-DoF Arm version: 1.0 -families: ["Arm"] +families: ["HEBIArm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] hrdf: "../hrdf/A-2085-06.hrdf" diff --git a/kits/arm/ex_impedance_control_cartesian.cpp b/kits/arm/ex_impedance_control_cartesian.cpp index a37a3cde..71f26535 100644 --- a/kits/arm/ex_impedance_control_cartesian.cpp +++ b/kits/arm/ex_impedance_control_cartesian.cpp @@ -18,9 +18,14 @@ This comprises the following demos: The following example is for the "Cartesian" demo: */ -#include "hebi_util.hpp" -#include +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" +#include "util/mobile_io.hpp" #include +#include +#include "hebi_util.hpp" using namespace hebi; using namespace experimental; @@ -53,16 +58,26 @@ int main(int argc, char* argv[]) // Create the arm object from the configuration arm = hebi::experimental::arm::Arm::create(*example_config); + + // Keep retrying if arm not found while (!arm) { std::cerr << "Failed to create arm, retrying..." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait for 1 second before retrying + + // Wait for 1 second before retrying + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Retry arm = hebi::experimental::arm::Arm::create(*example_config); } std::cout << "Arm connected." << std::endl; + // Ideally, in the impedance control demos, positions and velocities must not be commanded + // Initialize variables used to clear the commanded position and velocity in every cycle + // Get the pending command pointer hebi::GroupCommand& command = arm->pendingCommand(); + // Create nan vectors for positions and velocities auto num_joints = arm->robotModel().getDoFCount(); Eigen::VectorXd pos_nan(num_joints), vel_nan(num_joints); pos_nan.fill(std::numeric_limits::quiet_NaN()); @@ -73,13 +88,20 @@ int main(int argc, char* argv[]) ////////////////////////// // 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, example_config_file); - while (!mobile_io) { - std::cout << "Waiting for Mobile IO device to come online..." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait for 1 second before retrying + + // 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; + + // Wait for 1 second before retrying + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Retry mobile_io = createMobileIOFromConfig(*example_config, example_config_file); } - std::cout << "MobileIO connected." << std::endl; + std::cout << "Mobile IO connected." << std::endl; std::string instructions; instructions = " Cartesian demo"; @@ -94,7 +116,7 @@ int main(int argc, char* argv[]) 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" + << " 📌 (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"; diff --git a/kits/arm/ex_impedance_control_damping.cpp b/kits/arm/ex_impedance_control_damping.cpp index 1680e4e0..c26e656c 100644 --- a/kits/arm/ex_impedance_control_damping.cpp +++ b/kits/arm/ex_impedance_control_damping.cpp @@ -26,6 +26,8 @@ The following example is for the "Damping" demo: #include "arm/arm.hpp" #include "util/mobile_io.hpp" #include +#include +#include "hebi_util.hpp" using namespace hebi; using namespace experimental; @@ -33,61 +35,74 @@ using namespace experimental; int main(int argc, char* argv[]) { ////////////////////////// - ///// Arm Setup ////////// + ///// Config Setup /////// ////////////////////////// - arm::Arm::Params params; + const std::string example_config_file = "config/examples/ex_impedance_control_damping.cfg.yaml"; + std::vector errors; + + 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; - // Setup Module Family and Module Names - params.families_ = {"HEBIArm-T"}; - params.names_ = {"J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"}; + ////////////////////////// + ///// Arm Setup ////////// + ////////////////////////// - // Read HRDF file to seutp a RobotModel object for the 6-DoF Arm - // Make sure you are running this from the correct directory! - params.hrdf_file_ = "kits/arm/hrdf/T-arm.hrdf"; + // Create the arm object from the configuration + arm = hebi::experimental::arm::Arm::create(*example_config); - // Create the Arm Object - auto arm = arm::Arm::create(params); + // Keep retrying if arm not found while (!arm) { - arm = arm::Arm::create(params); - } - - // Load the gains file that is approriate to the arm - arm -> loadGains("kits/arm/gains/T-arm.xml"); + std::cerr << "Failed to create arm, retrying..." << std::endl; - // Create and configure the ImpedanceController plugin + // Wait for 1 second before retrying + std::this_thread::sleep_for(std::chrono::seconds(1)); - // 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. - - hebi::experimental::arm::PluginConfig impedance_config("ImpedanceController", "ImpedanceController"); - impedance_config.float_lists_["kp"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - impedance_config.float_lists_["kd"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - impedance_config.bools_["gains_in_end_effector_frame"] = false; - - auto impedance_plugin = hebi::experimental::arm::plugin::ImpedanceController::create(impedance_config); - if (!impedance_plugin) { - std::cerr << "Failed to create ImpedanceController plugin." << std::endl; - return -1; + // Retry + arm = hebi::experimental::arm::Arm::create(*example_config); } + std::cout << "Arm connected." << std::endl; + + // Ideally, in the impedance control demos, positions and velocities must not be commanded // Initialize variables used to clear the commanded position and velocity in every cycle + // Get the pending command pointer hebi::GroupCommand& command = arm->pendingCommand(); + // Create nan vectors for positions and velocities auto num_joints = arm->robotModel().getDoFCount(); Eigen::VectorXd pos_nan(num_joints), vel_nan(num_joints); pos_nan.fill(std::numeric_limits::quiet_NaN()); vel_nan.fill(std::numeric_limits::quiet_NaN()); - // Keep a reference to the ImpedanceController plugin - // You will have to do this if you intend to reconfigure the impedance control after adding it. - auto impedance_plugin_ptr = impedance_plugin.get(); + // Retrieve the impedance controller plugin from the arm + + // Pointer magic + + // Lock the weak_ptr and get a shared_ptr + auto plugin_shared_ptr = arm->getPluginByName("impedanceController").lock(); - // Add the plugin to the arm - if (!arm->addPlugin(std::move(impedance_plugin))) { - std::cerr << "Failed to add ImpedanceController plugin to arm." << std::endl; + // Check if the shared_ptr is valid + if (!plugin_shared_ptr) { + std::cerr << "Failed to lock plugin shared_ptr. The plugin may have been destroyed." << std::endl; + return -1; + } + + // Downcast to ImpedanceController + 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; } @@ -95,29 +110,33 @@ int main(int argc, char* argv[]) //// MobileIO Setup ////// ////////////////////////// - // Set up MobileIO - std::unique_ptr mobile = util::MobileIO::create(params.families_[0], "mobileIO"); - if (!mobile) - { - 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, example_config_file); + + // 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; + + // Wait for 1 second before retrying + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Retry + mobile_io = createMobileIOFromConfig(*example_config, example_config_file); } - mobile->setButtonMode(1, util::MobileIO::ButtonMode::Momentary); - mobile->setButtonLabel(1, "❌"); - mobile->setButtonMode(2, util::MobileIO::ButtonMode::Toggle); - mobile->setButtonLabel(2, "💪"); + std::cout << "Mobile IO connected." << std::endl; std::string instructions; - instructions = " Damping demo"; + instructions = " Damping demo"; // Clear any garbage on screen - mobile->clearText(); + mobile_io->clearText(); // Display instructions on screen - mobile->appendText(instructions); + mobile_io->appendText(instructions); // Setup instructions - auto last_state = mobile->update(); + 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" @@ -130,24 +149,26 @@ int main(int argc, char* argv[]) ///////////////////////////// // Meters above the base for overdamped, critically damped, and underdamped cases respectively - std::vector lower_limits = {0.0, 0.15, 0.3}; + std::vector lower_limits = example_config->getUserData().float_lists_.at("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::VectorXd(6) << 100.0f, 100.0f, 0.0f, 5.0f, 5.0f, 1.0f).finished()); - damping_kd.push_back((Eigen::VectorXd(6) << 15.0f, 15.0f, 15.0f, 0.0f, 0.0f, 0.0f).finished()); + damping_kp.push_back(Eigen::Map(example_config->getUserData().float_lists_.at("overdamped_kp").data(), example_config->getUserData().float_lists_.at("overdamped_kp").size())); + damping_kd.push_back(Eigen::Map(example_config->getUserData().float_lists_.at("overdamped_kd").data(), example_config->getUserData().float_lists_.at("overdamped_kd").size())); // 1: Critically damped - damping_kp.push_back((Eigen::VectorXd(6) << 100.0f, 100.0f, 0.0f, 5.0f, 5.0f, 1.0f).finished()); - damping_kd.push_back((Eigen::VectorXd(6) << 5.0f, 5.0f, 5.0f, 0.0f, 0.0f, 0.0f).finished()); + damping_kp.push_back(Eigen::Map(example_config->getUserData().float_lists_.at("critically_damped_kp").data(), example_config->getUserData().float_lists_.at("critically_damped_kp").size())); + damping_kd.push_back(Eigen::Map(example_config->getUserData().float_lists_.at("critically_damped_kd").data(), example_config->getUserData().float_lists_.at("critically_damped_kd").size())); // 2: Underdamped - damping_kp.push_back((Eigen::VectorXd(6) << 100.0f, 100.0f, 0.0f, 5.0f, 5.0f, 1.0f).finished()); - damping_kd.push_back((Eigen::VectorXd(6) << 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f).finished()); - + damping_kp.push_back(Eigen::Map(example_config->getUserData().float_lists_.at("underdamped_kp").data(), example_config->getUserData().float_lists_.at("underdamped_kp").size())); + damping_kd.push_back(Eigen::Map(example_config->getUserData().float_lists_.at("underdamped_kd").data(), example_config->getUserData().float_lists_.at("underdamped_kd").size())); // Flag to indicate when impedance controller is on bool controller_on = false; @@ -158,29 +179,29 @@ int main(int argc, char* argv[]) while(arm->update()) { - auto updated_mobile = mobile->update(0); + auto updated_mobile_io = mobile_io->update(0); - if (updated_mobile) + if (updated_mobile_io) { ///////////////// // Button Presses ///////////////// // Buttton B1 - End demo - if (mobile->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { + if (mobile_io->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { // Clear MobileIO text - mobile->resetUI(); + mobile_io->resetUI(); return 1; } // Button B2 - Set and unset impedance mode when button is pressed and released, respectively - if (mobile->getButtonDiff(2) == util::MobileIO::ButtonState::ToOn) { + if (mobile_io->getButtonDiff(2) == util::MobileIO::ButtonState::ToOn) { controller_on = true; arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPosition())); } - else if (mobile->getButtonDiff(2) == util::MobileIO::ButtonState::ToOff){ + else if (mobile_io->getButtonDiff(2) == util::MobileIO::ButtonState::ToOff){ controller_on = false; } @@ -227,7 +248,7 @@ int main(int argc, char* argv[]) } // Clear MobileIO text - mobile->clearText(); + mobile_io->clearText(); return 0; } diff --git a/kits/arm/ex_impedance_control_fixed.cpp b/kits/arm/ex_impedance_control_fixed.cpp index 33fa0066..aeb1f1e2 100644 --- a/kits/arm/ex_impedance_control_fixed.cpp +++ b/kits/arm/ex_impedance_control_fixed.cpp @@ -18,14 +18,14 @@ This comprises the following demos: The following example is for the "Fixed" demo: */ -// #include "lookup.hpp" -// #include "group.hpp" #include "group_command.hpp" #include "group_feedback.hpp" #include "robot_model.hpp" #include "arm/arm.hpp" #include "util/mobile_io.hpp" #include +#include +#include "hebi_util.hpp" using namespace hebi; using namespace experimental; @@ -33,96 +33,99 @@ using namespace experimental; int main(int argc, char* argv[]) { ////////////////////////// - ///// Arm Setup ////////// + ///// Config Setup /////// ////////////////////////// - arm::Arm::Params params; - - // Setup Module Family and Module Names - params.families_ = {"HEBIArm-T"}; - params.names_ = {"J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"}; - - // Read HRDF file to seutp a RobotModel object for the 6-DoF Arm - // Make sure you are running this from the correct directory! - params.hrdf_file_ = "kits/arm/hrdf/T-arm.hrdf"; - - // Create the Arm Object - auto arm = arm::Arm::create(params); - while (!arm) { - arm = arm::Arm::create(params); + const std::string example_config_file = "config/examples/ex_impedance_control_fixed.cfg.yaml"; + std::vector errors; + + 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; } - // Load the gains file that is approriate to the arm - arm -> loadGains("kits/arm/gains/T-arm.xml"); + // For this demo, we need the arm and mobile_io + std::unique_ptr arm; + std::unique_ptr mobile_io; - // Create and configure the ImpedanceController plugin + ////////////////////////// + ///// Arm Setup ////////// + ////////////////////////// - // 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. + // Create the arm object from the configuration + arm = hebi::experimental::arm::Arm::create(*example_config); - hebi::experimental::arm::PluginConfig impedance_config("ImpedanceController", "ImpedanceController"); - impedance_config.float_lists_["kp"] = {300.0, 300.0, 300.0, 5.0, 5.0, 1.0}; - impedance_config.float_lists_["kd"] = {5.0, 5.0, 5.0, 0.0, 0.0, 0.0}; - impedance_config.float_lists_["ki"] = {20.0, 20.0, 20.0, 0.5, 0.5, 0.5}; - impedance_config.float_lists_["i_clamp"] = {10.0, 10.0, 10.0, 1.0, 1.0, 1.0}; // Clamp on the end-effector wrench and NOT on the integral error - impedance_config.bools_["gains_in_end_effector_frame"] = true; + // Keep retrying if arm not found + while (!arm) { + std::cerr << "Failed to create arm, retrying..." << std::endl; - auto impedance_plugin = hebi::experimental::arm::plugin::ImpedanceController::create(impedance_config); - if (!impedance_plugin) { - std::cerr << "Failed to create ImpedanceController plugin." << std::endl; - return -1; + // Wait for 1 second before retrying + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Retry + arm = hebi::experimental::arm::Arm::create(*example_config); } + std::cout << "Arm connected." << std::endl; + + // Ideally, in the impedance control demos, positions and velocities must not be commanded // Initialize variables used to clear the commanded position and velocity in every cycle + // Get the pending command pointer hebi::GroupCommand& command = arm->pendingCommand(); + // Create nan vectors for positions and velocities auto num_joints = arm->robotModel().getDoFCount(); Eigen::VectorXd pos_nan(num_joints), vel_nan(num_joints); pos_nan.fill(std::numeric_limits::quiet_NaN()); vel_nan.fill(std::numeric_limits::quiet_NaN()); - // Add the plugin to the arm - if (!arm->addPlugin(std::move(impedance_plugin))) { - std::cerr << "Failed to add ImpedanceController plugin to arm." << std::endl; - return -1; - } - ////////////////////////// //// MobileIO Setup ////// ////////////////////////// - // Set up MobileIO - std::unique_ptr mobile = util::MobileIO::create(params.families_[0], "mobileIO"); - if (!mobile) - { - 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, example_config_file); + + // 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; + + // Wait for 1 second before retrying + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Retry + mobile_io = createMobileIOFromConfig(*example_config, example_config_file); } - mobile->setButtonMode(1, util::MobileIO::ButtonMode::Momentary); - mobile->setButtonLabel(1, "❌"); - mobile->setButtonMode(2, util::MobileIO::ButtonMode::Toggle); - mobile->setButtonLabel(2, "💪"); + std::cout << "Mobile IO connected." << std::endl; std::string instructions; - instructions = " Fixed demo"; + instructions = " Fixed demo"; // Clear any garbage on screen - mobile->clearText(); + mobile_io->clearText(); // Display instructions on screen - mobile->appendText(instructions); + mobile_io->appendText(instructions); // Setup instructions - auto last_state = mobile->update(); + 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" + << " 🛑 (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 // ///////////////////////////// @@ -136,29 +139,29 @@ int main(int argc, char* argv[]) while(arm->update()) { - auto updated_mobile = mobile->update(0); + auto updated_mobile_io = mobile_io->update(0); - if (updated_mobile) + if (updated_mobile_io) { ///////////////// // Button Presses ///////////////// // Buttton B1 - End demo - if (mobile->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { + if (mobile_io->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { // Clear MobileIO text - mobile->resetUI(); + mobile_io->resetUI(); return 1; } // Button B2 - Set and unset impedance mode when button is pressed and released, respectively - if (mobile->getButtonDiff(2) == util::MobileIO::ButtonState::ToOn) { + if (mobile_io->getButtonDiff(2) == util::MobileIO::ButtonState::ToOn) { controller_on = true; arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPosition())); } - else if (mobile->getButtonDiff(2) == util::MobileIO::ButtonState::ToOff){ + else if (mobile_io->getButtonDiff(2) == util::MobileIO::ButtonState::ToOff){ controller_on = false; } @@ -178,7 +181,7 @@ int main(int argc, char* argv[]) } // Clear MobileIO text - mobile->clearText(); + mobile_io->clearText(); return 0; } diff --git a/kits/arm/ex_impedance_control_floor.cpp b/kits/arm/ex_impedance_control_floor.cpp index 360207b7..a605231b 100644 --- a/kits/arm/ex_impedance_control_floor.cpp +++ b/kits/arm/ex_impedance_control_floor.cpp @@ -18,14 +18,14 @@ This comprises the following demos: The following example is for the "Floor" demo: */ -// #include "lookup.hpp" -// #include "group.hpp" #include "group_command.hpp" #include "group_feedback.hpp" #include "robot_model.hpp" #include "arm/arm.hpp" #include "util/mobile_io.hpp" #include +#include +#include "hebi_util.hpp" using namespace hebi; using namespace experimental; @@ -33,93 +33,90 @@ using namespace experimental; int main(int argc, char* argv[]) { ////////////////////////// - ///// Arm Setup ////////// + ///// Config Setup /////// ////////////////////////// - arm::Arm::Params params; - - // Setup Module Family and Module Names - params.families_ = {"HEBIArm-T"}; - params.names_ = {"J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"}; - - // Read HRDF file to seutp a RobotModel object for the 6-DoF Arm - // Make sure you are running this from the correct directory! - params.hrdf_file_ = "kits/arm/hrdf/T-arm.hrdf"; - - // Create the Arm Object - auto arm = arm::Arm::create(params); - while (!arm) { - arm = arm::Arm::create(params); + const std::string example_config_file = "config/examples/ex_impedance_control_floor.cfg.yaml"; + std::vector errors; + + 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; } - // Load the gains file that is approriate to the arm - arm -> loadGains("kits/arm/gains/T-arm.xml"); + // For this demo, we need the arm and mobile_io + std::unique_ptr arm; + std::unique_ptr mobile_io; - // Create and configure the ImpedanceController plugin + ////////////////////////// + ///// Arm Setup ////////// + ////////////////////////// - // 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. - - hebi::experimental::arm::PluginConfig impedance_config("ImpedanceController", "ImpedanceController"); + // Create the arm object from the configuration + arm = hebi::experimental::arm::Arm::create(*example_config); - impedance_config.float_lists_["kp"] = {300.0, 300.0, 300.0, 5.0, 5.0, 1.0}; - impedance_config.float_lists_["kd"] = {5.0, 5.0, 5.0, 0.0, 0.0, 0.0}; - impedance_config.float_lists_["ki"] = {20.0, 20.0, 20.0, 0.5, 0.5, 0.5}; - impedance_config.float_lists_["i_clamp"] = {10.0, 10.0, 10.0, 1.0, 1.0, 1.0}; - impedance_config.bools_["gains_in_end_effector_frame"] = true; + // Keep retrying if arm not found + while (!arm) { + std::cerr << "Failed to create arm, retrying..." << std::endl; - auto impedance_plugin = hebi::experimental::arm::plugin::ImpedanceController::create(impedance_config); - if (!impedance_plugin) { - std::cerr << "Failed to create ImpedanceController plugin." << std::endl; - return -1; + // Wait for 1 second before retrying + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Retry + arm = hebi::experimental::arm::Arm::create(*example_config); } + std::cout << "Arm connected." << std::endl; + + // Ideally, in the impedance control demos, positions and velocities must not be commanded // Initialize variables used to clear the commanded position and velocity in every cycle + // Get the pending command pointer hebi::GroupCommand& command = arm->pendingCommand(); + // Create nan vectors for positions and velocities auto num_joints = arm->robotModel().getDoFCount(); Eigen::VectorXd pos_nan(num_joints), vel_nan(num_joints); pos_nan.fill(std::numeric_limits::quiet_NaN()); vel_nan.fill(std::numeric_limits::quiet_NaN()); - // Add the plugin to the arm - if (!arm->addPlugin(std::move(impedance_plugin))) { - std::cerr << "Failed to add ImpedanceController plugin to arm." << std::endl; - return -1; - } - ////////////////////////// //// MobileIO Setup ////// ////////////////////////// - // Set up MobileIO - std::unique_ptr mobile = util::MobileIO::create(params.families_[0], "mobileIO"); - if (!mobile) - { - 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, example_config_file); + + // 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; + + // Wait for 1 second before retrying + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Retry + mobile_io = createMobileIOFromConfig(*example_config, example_config_file); } - mobile->setButtonMode(1, util::MobileIO::ButtonMode::Momentary); - mobile->setButtonLabel(1, "❌"); - mobile->setButtonMode(2, util::MobileIO::ButtonMode::Toggle); - mobile->setButtonLabel(2, "💪"); + std::cout << "Mobile IO connected." << std::endl; std::string instructions; - instructions = " Floor demo"; + instructions = " Fixed demo"; // Clear any garbage on screen - mobile->clearText(); + mobile_io->clearText(); // Display instructions on screen - mobile->appendText(instructions); + mobile_io->appendText(instructions); // Setup instructions - auto last_state = mobile->update(); - + 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" + << " 🧱 (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"; @@ -145,23 +142,23 @@ int main(int argc, char* argv[]) while(arm->update()) { - auto updated_mobile = mobile->update(0); + auto updated_mobile_io = mobile_io->update(0); - if (updated_mobile) + if (updated_mobile_io) { ///////////////// // Button Presses ///////////////// // Buttton B1 - End demo - if (mobile->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { + if (mobile_io->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { // Clear MobileIO text - mobile->resetUI(); + mobile_io->resetUI(); return 1; } // Button B2 - Set and unset impedance mode when button is pressed and released, respectively - if (mobile->getButtonDiff(2) == util::MobileIO::ButtonState::ToOn) { + if (mobile_io->getButtonDiff(2) == util::MobileIO::ButtonState::ToOn) { controller_on = true; @@ -180,7 +177,7 @@ int main(int argc, char* argv[]) // Update flags to indicate having left the floor cancel_command_flag = true; } - else if (mobile->getButtonDiff(2) == util::MobileIO::ButtonState::ToOff){ + else if (mobile_io->getButtonDiff(2) == util::MobileIO::ButtonState::ToOff){ controller_on = false; } @@ -240,7 +237,7 @@ int main(int argc, char* argv[]) } // Clear MobileIO text - mobile->clearText(); + mobile_io->clearText(); return 0; } diff --git a/kits/arm/ex_impedance_control_gimbal.cpp b/kits/arm/ex_impedance_control_gimbal.cpp index 16a42f6e..24876beb 100644 --- a/kits/arm/ex_impedance_control_gimbal.cpp +++ b/kits/arm/ex_impedance_control_gimbal.cpp @@ -18,14 +18,14 @@ This comprises the following demos: The following example is for the "Gimbal" demo: */ -// #include "lookup.hpp" -// #include "group.hpp" #include "group_command.hpp" #include "group_feedback.hpp" #include "robot_model.hpp" #include "arm/arm.hpp" #include "util/mobile_io.hpp" #include +#include +#include "hebi_util.hpp" using namespace hebi; using namespace experimental; @@ -33,96 +33,99 @@ using namespace experimental; int main(int argc, char* argv[]) { ////////////////////////// - ///// Arm Setup ////////// + ///// Config Setup /////// ////////////////////////// - arm::Arm::Params params; - - // Setup Module Family and Module Names - params.families_ = {"HEBIArm-T"}; - params.names_ = {"J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"}; - - // Read HRDF file to seutp a RobotModel object for the 6-DoF Arm - // Make sure you are running this from the correct directory! - params.hrdf_file_ = "kits/arm/hrdf/T-arm.hrdf"; - - // Create the Arm Object - auto arm = arm::Arm::create(params); - while (!arm) { - arm = arm::Arm::create(params); + const std::string example_config_file = "config/examples/ex_impedance_control_gimbal.cfg.yaml"; + std::vector errors; + + 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; } - // Load the gains file that is approriate to the arm - arm -> loadGains("kits/arm/gains/T-arm.xml"); + // For this demo, we need the arm and mobile_io + std::unique_ptr arm; + std::unique_ptr mobile_io; - // Create and configure the ImpedanceController plugin + ////////////////////////// + ///// Arm Setup ////////// + ////////////////////////// - // 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. + // Create the arm object from the configuration + arm = hebi::experimental::arm::Arm::create(*example_config); - hebi::experimental::arm::PluginConfig impedance_config("ImpedanceController", "ImpedanceController"); - impedance_config.float_lists_["kp"] = {0.0, 0.0, 0.0, 5.0, 5.0, 1.0}; - impedance_config.float_lists_["kd"] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - impedance_config.float_lists_["ki"] = {0.0, 0.0, 0.0, 0.5, 0.5, 0.5}; - impedance_config.float_lists_["i_clamp"] = {0.0, 0.0, 0.0, 1.0, 1.0, 1.0}; // Clamp on the end-effector wrench and NOT on the integral error - impedance_config.bools_["gains_in_end_effector_frame"] = true; + // Keep retrying if arm not found + while (!arm) { + std::cerr << "Failed to create arm, retrying..." << std::endl; - auto impedance_plugin = hebi::experimental::arm::plugin::ImpedanceController::create(impedance_config); - if (!impedance_plugin) { - std::cerr << "Failed to create ImpedanceController plugin." << std::endl; - return -1; + // Wait for 1 second before retrying + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Retry + arm = hebi::experimental::arm::Arm::create(*example_config); } + std::cout << "Arm connected." << std::endl; + + // Ideally, in the impedance control demos, positions and velocities must not be commanded // Initialize variables used to clear the commanded position and velocity in every cycle + // Get the pending command pointer hebi::GroupCommand& command = arm->pendingCommand(); + // Create nan vectors for positions and velocities auto num_joints = arm->robotModel().getDoFCount(); Eigen::VectorXd pos_nan(num_joints), vel_nan(num_joints); pos_nan.fill(std::numeric_limits::quiet_NaN()); vel_nan.fill(std::numeric_limits::quiet_NaN()); - // Add the plugin to the arm - if (!arm->addPlugin(std::move(impedance_plugin))) { - std::cerr << "Failed to add ImpedanceController plugin to arm." << std::endl; - return -1; - } - ////////////////////////// //// MobileIO Setup ////// ////////////////////////// - // Set up MobileIO - std::unique_ptr mobile = util::MobileIO::create(params.families_[0], "mobileIO"); - if (!mobile) - { - 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, example_config_file); + + // 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; + + // Wait for 1 second before retrying + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Retry + mobile_io = createMobileIOFromConfig(*example_config, example_config_file); } - mobile->setButtonMode(1, util::MobileIO::ButtonMode::Momentary); - mobile->setButtonLabel(1, "❌"); - mobile->setButtonMode(2, util::MobileIO::ButtonMode::Toggle); - mobile->setButtonLabel(2, "💪"); + std::cout << "Mobile IO connected." << std::endl; std::string instructions; - instructions = " Gimbal demo"; + instructions = " Gimbal demo"; // Clear any garbage on screen - mobile->clearText(); + mobile_io->clearText(); // Display instructions on screen - mobile->appendText(instructions); + mobile_io->appendText(instructions); // Setup instructions - auto last_state = mobile->update(); + 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" + << " 🤳 (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 // ///////////////////////////// @@ -136,29 +139,29 @@ int main(int argc, char* argv[]) while(arm->update()) { - auto updated_mobile = mobile->update(0); + auto updated_mobile_io = mobile_io->update(0); - if (updated_mobile) + if (updated_mobile_io) { ///////////////// // Button Presses ///////////////// // Buttton B1 - End demo - if (mobile->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { + if (mobile_io->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { // Clear MobileIO text - mobile->resetUI(); + mobile_io->resetUI(); return 1; } // Button B2 - Set and unset impedance mode when button is pressed and released, respectively - if (mobile->getButtonDiff(2) == util::MobileIO::ButtonState::ToOn) { + if (mobile_io->getButtonDiff(2) == util::MobileIO::ButtonState::ToOn) { controller_on = true; arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPosition())); } - else if (mobile->getButtonDiff(2) == util::MobileIO::ButtonState::ToOff){ + else if (mobile_io->getButtonDiff(2) == util::MobileIO::ButtonState::ToOff){ controller_on = false; } @@ -178,7 +181,7 @@ int main(int argc, char* argv[]) } // Clear MobileIO text - mobile->clearText(); + mobile_io->clearText(); return 0; } diff --git a/kits/arm/hebi_util.cpp b/kits/arm/hebi_util.cpp index ce976672..2588f33b 100644 --- a/kits/arm/hebi_util.cpp +++ b/kits/arm/hebi_util.cpp @@ -38,22 +38,22 @@ std::unique_ptr createMobileIOFromConfig(const hebi::Robot }; // Validate the mobile_io configuration - if (config.getUserData().count("mobile_io_family") && - config.getUserData().count("mobile_io_name") && - config.getUserData().count("mobile_io_layout")) { + 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().at("mobile_io_family").empty() || - config.getUserData().at("mobile_io_name").empty()) { + if (config.getUserData().strings_.at("mobile_io_family").empty() || + config.getUserData().strings_.at("mobile_io_name").empty()) { errors.push_back("HEBI config \"user_data\"'s \"mobile_io_...\" fields must be non-empty strings."); } // Populate the dictionary - mobile_io_dict["family"] = config.getUserData().at("mobile_io_family"); - mobile_io_dict["name"] = config.getUserData().at("mobile_io_name"); + mobile_io_dict["family"] = config.getUserData().strings_.at("mobile_io_family"); + mobile_io_dict["name"] = config.getUserData().strings_.at("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().at("mobile_io_layout")); + mobile_io_dict["layout"] = check_file("mobile_io_layout", config.getUserData().strings_.at("mobile_io_layout")); if (mobile_io_dict["layout"].empty()) { errors.push_back("HEBI config \"user_data\"'s \"mobile_io_layout\" file is invalid."); } @@ -63,6 +63,7 @@ std::unique_ptr createMobileIOFromConfig(const hebi::Robot // If there are errors, print them and throw an exception if (!errors.empty()) { + for (const auto& error : errors) { std::cerr << "Error: " << error << std::endl; } @@ -72,7 +73,9 @@ std::unique_ptr createMobileIOFromConfig(const hebi::Robot // Create Mobile IO based on validated config std::unique_ptr mobile_io = hebi::util::MobileIO::create(mobile_io_dict["family"], mobile_io_dict["name"]); - mobile_io->sendLayoutFile(mobile_io_dict["layout"]); - + if(mobile_io != nullptr) + { + mobile_io->sendLayoutFile(mobile_io_dict["layout"]); + } return mobile_io; } \ No newline at end of file From 89dc7793e7c7e224ae381751b06d9a2cf04418d6 Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Tue, 13 Aug 2024 12:35:44 -0400 Subject: [PATCH 12/31] mobile_io_control example integrated with config files --- .../examples/ex_mobile_io_control.cfg.yaml | 2 +- kits/arm/ex_mobile_io_control.cpp | 129 ++++++++++-------- 2 files changed, 76 insertions(+), 55 deletions(-) diff --git a/kits/arm/config/examples/ex_mobile_io_control.cfg.yaml b/kits/arm/config/examples/ex_mobile_io_control.cfg.yaml index ae452cda..1bbd1ccd 100644 --- a/kits/arm/config/examples/ex_mobile_io_control.cfg.yaml +++ b/kits/arm/config/examples/ex_mobile_io_control.cfg.yaml @@ -1,6 +1,6 @@ # 6-DoF Arm version: 1.0 -families: ["Arm"] +families: ["HEBIArm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] hrdf: "../hrdf/A-2085-06.hrdf" diff --git a/kits/arm/ex_mobile_io_control.cpp b/kits/arm/ex_mobile_io_control.cpp index b09a7590..b304d360 100644 --- a/kits/arm/ex_mobile_io_control.cpp +++ b/kits/arm/ex_mobile_io_control.cpp @@ -4,14 +4,20 @@ * to pre-programmed waypoints. */ -// #include "lookup.hpp" -// #include "group.hpp" +/* +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 +#include "hebi_util.hpp" using namespace hebi; using namespace experimental; @@ -19,62 +25,90 @@ using namespace experimental; int main(int argc, char* argv[]) { ////////////////////////// - ///// Arm Setup ////////// + ///// Config Setup /////// ////////////////////////// - arm::Arm::Params params; + const std::string example_config_file = "config/examples/ex_mobile_io_control.cfg.yaml"; + std::vector errors; + + 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; + } - // Setup Module Family and Module Names - params.families_ = {"Arm"}; - params.names_ = {"J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"}; + // For this demo, we need the arm and mobile_io + std::unique_ptr arm; + std::unique_ptr mobile_io; - // Read HRDF file to seutp a RobotModel object for the 6-DoF Arm - // Make sure you are running this from the correct directory! - params.hrdf_file_ = "kits/arm/hrdf/A-2085-06.hrdf"; + ////////////////////////// + ///// Arm Setup ////////// + ////////////////////////// + + // Create the arm object from the configuration + arm = hebi::experimental::arm::Arm::create(*example_config); - // Create the Arm Object - auto arm = arm::Arm::create(params); + // Keep retrying if arm not found while (!arm) { - arm = arm::Arm::create(params); - } + std::cerr << "Failed to create arm, retrying..." << std::endl; - // Load the gains file that is approriate to the arm - arm -> loadGains("kits/arm/gains/A-2085-06.xml"); + // Wait for 1 second before retrying + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Retry + arm = hebi::experimental::arm::Arm::create(*example_config); + } + std::cout << "Arm connected." << std::endl; ////////////////////////// //// MobileIO Setup ////// ////////////////////////// - // Create the MobileIO object - std::unique_ptr mobile = util::MobileIO::create(params.families_[0], "mobileIO"); - if (!mobile) - { - 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, example_config_file); + + // 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; + + // Wait for 1 second before retrying + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Retry + mobile_io = createMobileIOFromConfig(*example_config, example_config_file); } + 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->clearText(); + mobile_io->clearText(); // Display instructions on screen - mobile->appendText(instructions); + mobile_io->appendText(instructions); // Setup instructions - auto last_state = mobile->update(); + 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; - single_time = 3; + std::vector waypoints; + waypoints.push_back(Eigen::Map(example_config->getUserData().float_lists_.at("waypoint_1").data(), example_config->getUserData().float_lists_.at("waypoint_1").size())); + waypoints.push_back(Eigen::Map(example_config->getUserData().float_lists_.at("waypoint_2").data(), example_config->getUserData().float_lists_.at("waypoint_2").size())); + waypoints.push_back(Eigen::Map(example_config->getUserData().float_lists_.at("waypoint_3").data(), example_config->getUserData().float_lists_.at("waypoint_3").size())); + + // Travel time + double travel_time = example_config->getUserData().floats_.at("travel_time"); ////////////////////////// //// Main Control Loop /// @@ -82,45 +116,32 @@ int main(int argc, char* argv[]) while(arm->update()) { - auto updated_mobile = mobile->update(0); + auto updated_mobile_io = mobile_io->update(0); - if (!updated_mobile) - 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->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->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->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) == util::MobileIO::ButtonState::ToOn) { + arm -> setGoal(arm::Goal::createFromPosition(travel_time, waypoints.at(button-1))); + } } // Button B6 - Grav Comp Mode - if (mobile->getButtonDiff(6) == util::MobileIO::ButtonState::ToOn) { + if (mobile_io->getButtonDiff(6) == util::MobileIO::ButtonState::ToOn) { // cancel any goal that is set, returning arm into gravComp mode arm -> cancelGoal(); } // Button B8 - End Demo - if (mobile->getButtonDiff(8) == util::MobileIO::ButtonState::ToOn) { + if (mobile_io->getButtonDiff(8) == util::MobileIO::ButtonState::ToOn) { // Clear MobileIO text - mobile->resetUI(); + mobile_io->resetUI(); return 1; } } @@ -130,7 +151,7 @@ int main(int argc, char* argv[]) } // Clear MobileIO text - mobile->clearText(); + mobile_io->clearText(); return 0; } From 3afe1bfd7b6c2af9cfdb1c8c8d3d4285d14be01e Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Wed, 14 Aug 2024 11:08:14 -0400 Subject: [PATCH 13/31] Refactored repo according to new layout --- kits/arm/config/{examples => }/ex_AR_kit.cfg.yaml | 0 kits/arm/config/{examples => }/ex_AR_kit_w_gripper.cfg.yaml | 0 kits/arm/config/{examples => }/ex_gravity_compensation.cfg.yaml | 0 .../config/{examples => }/ex_impedance_control_cartesian.cfg.yaml | 0 .../config/{examples => }/ex_impedance_control_damping.cfg.yaml | 0 .../arm/config/{examples => }/ex_impedance_control_fixed.cfg.yaml | 0 .../arm/config/{examples => }/ex_impedance_control_floor.cfg.yaml | 0 .../config/{examples => }/ex_impedance_control_gimbal.cfg.yaml | 0 kits/arm/config/{examples => }/ex_mobile_io_control.cfg.yaml | 0 kits/arm/config/{examples => }/ex_teach_repeat.cfg.yaml | 0 kits/arm/config/{examples => }/ex_teach_repeat_w_gripper.cfg.yaml | 0 11 files changed, 0 insertions(+), 0 deletions(-) rename kits/arm/config/{examples => }/ex_AR_kit.cfg.yaml (100%) rename kits/arm/config/{examples => }/ex_AR_kit_w_gripper.cfg.yaml (100%) rename kits/arm/config/{examples => }/ex_gravity_compensation.cfg.yaml (100%) rename kits/arm/config/{examples => }/ex_impedance_control_cartesian.cfg.yaml (100%) rename kits/arm/config/{examples => }/ex_impedance_control_damping.cfg.yaml (100%) rename kits/arm/config/{examples => }/ex_impedance_control_fixed.cfg.yaml (100%) rename kits/arm/config/{examples => }/ex_impedance_control_floor.cfg.yaml (100%) rename kits/arm/config/{examples => }/ex_impedance_control_gimbal.cfg.yaml (100%) rename kits/arm/config/{examples => }/ex_mobile_io_control.cfg.yaml (100%) rename kits/arm/config/{examples => }/ex_teach_repeat.cfg.yaml (100%) rename kits/arm/config/{examples => }/ex_teach_repeat_w_gripper.cfg.yaml (100%) diff --git a/kits/arm/config/examples/ex_AR_kit.cfg.yaml b/kits/arm/config/ex_AR_kit.cfg.yaml similarity index 100% rename from kits/arm/config/examples/ex_AR_kit.cfg.yaml rename to kits/arm/config/ex_AR_kit.cfg.yaml diff --git a/kits/arm/config/examples/ex_AR_kit_w_gripper.cfg.yaml b/kits/arm/config/ex_AR_kit_w_gripper.cfg.yaml similarity index 100% rename from kits/arm/config/examples/ex_AR_kit_w_gripper.cfg.yaml rename to kits/arm/config/ex_AR_kit_w_gripper.cfg.yaml diff --git a/kits/arm/config/examples/ex_gravity_compensation.cfg.yaml b/kits/arm/config/ex_gravity_compensation.cfg.yaml similarity index 100% rename from kits/arm/config/examples/ex_gravity_compensation.cfg.yaml rename to kits/arm/config/ex_gravity_compensation.cfg.yaml diff --git a/kits/arm/config/examples/ex_impedance_control_cartesian.cfg.yaml b/kits/arm/config/ex_impedance_control_cartesian.cfg.yaml similarity index 100% rename from kits/arm/config/examples/ex_impedance_control_cartesian.cfg.yaml rename to kits/arm/config/ex_impedance_control_cartesian.cfg.yaml diff --git a/kits/arm/config/examples/ex_impedance_control_damping.cfg.yaml b/kits/arm/config/ex_impedance_control_damping.cfg.yaml similarity index 100% rename from kits/arm/config/examples/ex_impedance_control_damping.cfg.yaml rename to kits/arm/config/ex_impedance_control_damping.cfg.yaml diff --git a/kits/arm/config/examples/ex_impedance_control_fixed.cfg.yaml b/kits/arm/config/ex_impedance_control_fixed.cfg.yaml similarity index 100% rename from kits/arm/config/examples/ex_impedance_control_fixed.cfg.yaml rename to kits/arm/config/ex_impedance_control_fixed.cfg.yaml diff --git a/kits/arm/config/examples/ex_impedance_control_floor.cfg.yaml b/kits/arm/config/ex_impedance_control_floor.cfg.yaml similarity index 100% rename from kits/arm/config/examples/ex_impedance_control_floor.cfg.yaml rename to kits/arm/config/ex_impedance_control_floor.cfg.yaml diff --git a/kits/arm/config/examples/ex_impedance_control_gimbal.cfg.yaml b/kits/arm/config/ex_impedance_control_gimbal.cfg.yaml similarity index 100% rename from kits/arm/config/examples/ex_impedance_control_gimbal.cfg.yaml rename to kits/arm/config/ex_impedance_control_gimbal.cfg.yaml diff --git a/kits/arm/config/examples/ex_mobile_io_control.cfg.yaml b/kits/arm/config/ex_mobile_io_control.cfg.yaml similarity index 100% rename from kits/arm/config/examples/ex_mobile_io_control.cfg.yaml rename to kits/arm/config/ex_mobile_io_control.cfg.yaml diff --git a/kits/arm/config/examples/ex_teach_repeat.cfg.yaml b/kits/arm/config/ex_teach_repeat.cfg.yaml similarity index 100% rename from kits/arm/config/examples/ex_teach_repeat.cfg.yaml rename to kits/arm/config/ex_teach_repeat.cfg.yaml diff --git a/kits/arm/config/examples/ex_teach_repeat_w_gripper.cfg.yaml b/kits/arm/config/ex_teach_repeat_w_gripper.cfg.yaml similarity index 100% rename from kits/arm/config/examples/ex_teach_repeat_w_gripper.cfg.yaml rename to kits/arm/config/ex_teach_repeat_w_gripper.cfg.yaml From 6317ab3407caabe6bddaf6bf41aad7f828265ba1 Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Wed, 14 Aug 2024 11:23:53 -0400 Subject: [PATCH 14/31] Renamed file paths to acoomaodate new config file layout --- kits/arm/config/ex_AR_kit.cfg.yaml | 6 +++--- kits/arm/config/ex_AR_kit_w_gripper.cfg.yaml | 8 ++++---- kits/arm/config/ex_gravity_compensation.cfg.yaml | 6 +++--- kits/arm/config/ex_impedance_control_cartesian.cfg.yaml | 6 +++--- kits/arm/config/ex_impedance_control_damping.cfg.yaml | 6 +++--- kits/arm/config/ex_impedance_control_fixed.cfg.yaml | 6 +++--- kits/arm/config/ex_impedance_control_floor.cfg.yaml | 6 +++--- kits/arm/config/ex_impedance_control_gimbal.cfg.yaml | 6 +++--- kits/arm/config/ex_mobile_io_control.cfg.yaml | 6 +++--- kits/arm/config/ex_teach_repeat.cfg.yaml | 6 +++--- kits/arm/config/ex_teach_repeat_w_gripper.cfg.yaml | 8 ++++---- kits/arm/ex_impedance_control_cartesian.cpp | 2 +- kits/arm/ex_impedance_control_damping.cpp | 4 ++-- kits/arm/ex_impedance_control_fixed.cpp | 2 +- kits/arm/ex_impedance_control_floor.cpp | 8 ++++---- kits/arm/ex_impedance_control_gimbal.cpp | 2 +- kits/arm/ex_mobile_io_control.cpp | 2 +- 17 files changed, 45 insertions(+), 45 deletions(-) diff --git a/kits/arm/config/ex_AR_kit.cfg.yaml b/kits/arm/config/ex_AR_kit.cfg.yaml index 2fff73c2..faa723da 100644 --- a/kits/arm/config/ex_AR_kit.cfg.yaml +++ b/kits/arm/config/ex_AR_kit.cfg.yaml @@ -2,10 +2,10 @@ version: 1.0 families: ["Arm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] -hrdf: "../hrdf/A-2085-06.hrdf" +hrdf: "hrdf/A-2085-06.hrdf" gains: - default: "../gains/A-2085-06.xml" + default: "gains/A-2085-06.xml" plugins: @@ -32,4 +32,4 @@ user_data: mobile_io_family: "Arm" mobile_io_name: "mobileIO" - mobile_io_layout: "../layouts/ex_AR_kit.json" \ No newline at end of file + mobile_io_layout: "layouts/ex_AR_kit.json" \ No newline at end of file diff --git a/kits/arm/config/ex_AR_kit_w_gripper.cfg.yaml b/kits/arm/config/ex_AR_kit_w_gripper.cfg.yaml index 813c4cb5..3b036b39 100644 --- a/kits/arm/config/ex_AR_kit_w_gripper.cfg.yaml +++ b/kits/arm/config/ex_AR_kit_w_gripper.cfg.yaml @@ -2,10 +2,10 @@ version: 1.0 families: ["Arm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] -hrdf: "../hrdf/A-2240-06G.hrdf" +hrdf: "hrdf/A-2240-06G.hrdf" gains: - default: "../gains/A-2240-06.xml" + default: "gains/A-2240-06.xml" plugins: @@ -32,10 +32,10 @@ user_data: gripper_family: "Arm" gripper_name: "gripperSpool" - gripper_gains: "../gains/gripper_spool_gains.xml" + gripper_gains: "gains/gripper_spool_gains.xml" gripper_close_effort: -5.0 # (Nm) Effort applied to close the gripper. More negative effort will pinch the gripper harder. gripper_open_effort: 1.0 # (Nm) Effort applied to open the gripper. More positive effort will NOT make the gripper harder to close. mobile_io_family: "Arm" mobile_io_name: "mobileIO" - mobile_io_layout: "../layouts/ex_AR_kit_w_gripper.json" \ No newline at end of file + mobile_io_layout: "layouts/ex_AR_kit_w_gripper.json" \ No newline at end of file diff --git a/kits/arm/config/ex_gravity_compensation.cfg.yaml b/kits/arm/config/ex_gravity_compensation.cfg.yaml index 9426affe..24cbceac 100644 --- a/kits/arm/config/ex_gravity_compensation.cfg.yaml +++ b/kits/arm/config/ex_gravity_compensation.cfg.yaml @@ -2,10 +2,10 @@ version: 1.0 families: ["Arm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] -hrdf: "../hrdf/A-2085-06.hrdf" +hrdf: "hrdf/A-2085-06.hrdf" gains: - default: "../gains/A-2085-06.xml" + default: "gains/A-2085-06.xml" plugins: @@ -23,4 +23,4 @@ user_data: mobile_io_family: "Arm" mobile_io_name: "mobileIO" - mobile_io_layout: "../layouts/ex_gravity_compensation.json" \ No newline at end of file + mobile_io_layout: "layouts/ex_gravity_compensation.json" \ No newline at end of file diff --git a/kits/arm/config/ex_impedance_control_cartesian.cfg.yaml b/kits/arm/config/ex_impedance_control_cartesian.cfg.yaml index 7dc399d0..96f234a0 100644 --- a/kits/arm/config/ex_impedance_control_cartesian.cfg.yaml +++ b/kits/arm/config/ex_impedance_control_cartesian.cfg.yaml @@ -2,10 +2,10 @@ version: 1.0 families: ["HEBIArm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] -hrdf: "../hrdf/A-2085-06.hrdf" +hrdf: "hrdf/A-2085-06.hrdf" gains: - default: "../gains/A-2085-06.xml" + default: "gains/A-2085-06.xml" plugins: @@ -39,4 +39,4 @@ user_data: mobile_io_family: "Arm" mobile_io_name: "mobileIO" - mobile_io_layout: "../layouts/ex_impedance_control_cartesian.json" \ No newline at end of file + mobile_io_layout: "layouts/ex_impedance_control_cartesian.json" \ No newline at end of file diff --git a/kits/arm/config/ex_impedance_control_damping.cfg.yaml b/kits/arm/config/ex_impedance_control_damping.cfg.yaml index 31f2b961..d8a877c1 100644 --- a/kits/arm/config/ex_impedance_control_damping.cfg.yaml +++ b/kits/arm/config/ex_impedance_control_damping.cfg.yaml @@ -2,10 +2,10 @@ version: 1.0 families: ["HEBIArm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] -hrdf: "../hrdf/A-2085-06.hrdf" +hrdf: "hrdf/A-2085-06.hrdf" gains: - default: "../gains/A-2085-06.xml" + default: "gains/A-2085-06.xml" plugins: @@ -50,4 +50,4 @@ user_data: mobile_io_family: "Arm" mobile_io_name: "mobileIO" - mobile_io_layout: "../layouts/ex_impedance_control_damping.json" \ No newline at end of file + mobile_io_layout: "layouts/ex_impedance_control_damping.json" \ No newline at end of file diff --git a/kits/arm/config/ex_impedance_control_fixed.cfg.yaml b/kits/arm/config/ex_impedance_control_fixed.cfg.yaml index 403c3049..4764c6fa 100644 --- a/kits/arm/config/ex_impedance_control_fixed.cfg.yaml +++ b/kits/arm/config/ex_impedance_control_fixed.cfg.yaml @@ -2,10 +2,10 @@ version: 1.0 families: ["HEBIArm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] -hrdf: "../hrdf/A-2085-06.hrdf" +hrdf: "hrdf/A-2085-06.hrdf" gains: - default: "../gains/A-2085-06.xml" + default: "gains/A-2085-06.xml" plugins: @@ -39,4 +39,4 @@ user_data: mobile_io_family: "Arm" mobile_io_name: "mobileIO" - mobile_io_layout: "../layouts/ex_impedance_control_fixed.json" \ No newline at end of file + mobile_io_layout: "layouts/ex_impedance_control_fixed.json" \ No newline at end of file diff --git a/kits/arm/config/ex_impedance_control_floor.cfg.yaml b/kits/arm/config/ex_impedance_control_floor.cfg.yaml index 5ef3b879..5c04af6d 100644 --- a/kits/arm/config/ex_impedance_control_floor.cfg.yaml +++ b/kits/arm/config/ex_impedance_control_floor.cfg.yaml @@ -2,10 +2,10 @@ version: 1.0 families: ["HEBIArm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] -hrdf: "../hrdf/A-2085-06.hrdf" +hrdf: "hrdf/A-2085-06.hrdf" gains: - default: "../gains/A-2085-06.xml" + default: "gains/A-2085-06.xml" plugins: @@ -39,4 +39,4 @@ user_data: mobile_io_family: "Arm" mobile_io_name: "mobileIO" - mobile_io_layout: "../layouts/ex_impedance_control_floor.json" \ No newline at end of file + mobile_io_layout: "layouts/ex_impedance_control_floor.json" \ No newline at end of file diff --git a/kits/arm/config/ex_impedance_control_gimbal.cfg.yaml b/kits/arm/config/ex_impedance_control_gimbal.cfg.yaml index 9e2b2da1..b18a1540 100644 --- a/kits/arm/config/ex_impedance_control_gimbal.cfg.yaml +++ b/kits/arm/config/ex_impedance_control_gimbal.cfg.yaml @@ -2,10 +2,10 @@ version: 1.0 families: ["HEBIArm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] -hrdf: "../hrdf/A-2085-06.hrdf" +hrdf: "hrdf/A-2085-06.hrdf" gains: - default: "../gains/A-2085-06.xml" + default: "gains/A-2085-06.xml" plugins: @@ -39,4 +39,4 @@ user_data: mobile_io_family: "Arm" mobile_io_name: "mobileIO" - mobile_io_layout: "../layouts/ex_impedance_control_gimbal.json" \ No newline at end of file + mobile_io_layout: "layouts/ex_impedance_control_gimbal.json" \ No newline at end of file diff --git a/kits/arm/config/ex_mobile_io_control.cfg.yaml b/kits/arm/config/ex_mobile_io_control.cfg.yaml index 1bbd1ccd..826421f2 100644 --- a/kits/arm/config/ex_mobile_io_control.cfg.yaml +++ b/kits/arm/config/ex_mobile_io_control.cfg.yaml @@ -2,10 +2,10 @@ version: 1.0 families: ["HEBIArm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] -hrdf: "../hrdf/A-2085-06.hrdf" +hrdf: "hrdf/A-2085-06.hrdf" gains: - default: "../gains/A-2085-06.xml" + default: "gains/A-2085-06.xml" plugins: @@ -31,4 +31,4 @@ user_data: mobile_io_family: "Arm" mobile_io_name: "mobileIO" - mobile_io_layout: "../layouts/ex_mobile_io_control.json" \ No newline at end of file + mobile_io_layout: "layouts/ex_mobile_io_control.json" \ No newline at end of file diff --git a/kits/arm/config/ex_teach_repeat.cfg.yaml b/kits/arm/config/ex_teach_repeat.cfg.yaml index 1cf332d9..1eedfc45 100644 --- a/kits/arm/config/ex_teach_repeat.cfg.yaml +++ b/kits/arm/config/ex_teach_repeat.cfg.yaml @@ -2,10 +2,10 @@ version: 1.0 families: ["Arm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] -hrdf: "../hrdf/A-2085-06.hrdf" +hrdf: "hrdf/A-2085-06.hrdf" gains: - default: "../gains/A-2085-06.xml" + default: "gains/A-2085-06.xml" plugins: @@ -33,4 +33,4 @@ user_data: mobile_io_family: "Arm" mobile_io_name: "mobileIO" - mobile_io_layout: "../layouts/ex_teach_repeat.json" \ No newline at end of file + mobile_io_layout: "layouts/ex_teach_repeat.json" \ No newline at end of file diff --git a/kits/arm/config/ex_teach_repeat_w_gripper.cfg.yaml b/kits/arm/config/ex_teach_repeat_w_gripper.cfg.yaml index 49a70588..5e30eaad 100644 --- a/kits/arm/config/ex_teach_repeat_w_gripper.cfg.yaml +++ b/kits/arm/config/ex_teach_repeat_w_gripper.cfg.yaml @@ -2,10 +2,10 @@ version: 1.0 families: ["Arm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] -hrdf: "../hrdf/A-2240-06G.hrdf" +hrdf: "hrdf/A-2240-06G.hrdf" gains: - default: "../gains/A-2240-06.xml" + default: "gains/A-2240-06.xml" plugins: @@ -33,10 +33,10 @@ user_data: gripper_family: "Arm" gripper_name: "gripperSpool" - gripper_gains: "../gains/gripper_spool_gains.xml" + gripper_gains: "gains/gripper_spool_gains.xml" gripper_close_effort: -5.0 # (Nm) Effort applied to close the gripper. More negative effort will pinch the gripper harder. gripper_open_effort: 1.0 # (Nm) Effort applied to open the gripper. More positive effort will NOT make the gripper harder to close. mobile_io_family: "Arm" mobile_io_name: "mobileIO" - mobile_io_layout: "../layouts/ex_AR_kit_w_gripper.json" \ No newline at end of file + mobile_io_layout: "layouts/ex_AR_kit_w_gripper.json" \ No newline at end of file diff --git a/kits/arm/ex_impedance_control_cartesian.cpp b/kits/arm/ex_impedance_control_cartesian.cpp index 71f26535..2bd6ad35 100644 --- a/kits/arm/ex_impedance_control_cartesian.cpp +++ b/kits/arm/ex_impedance_control_cartesian.cpp @@ -36,7 +36,7 @@ int main(int argc, char* argv[]) ///// Config Setup /////// ////////////////////////// - const std::string example_config_file = "config/examples/ex_impedance_control_cartesian.cfg.yaml"; + const std::string example_config_file = "config/ex_impedance_control_cartesian.cfg.yaml"; std::vector errors; const auto example_config = RobotConfig::loadConfig(example_config_file, errors); diff --git a/kits/arm/ex_impedance_control_damping.cpp b/kits/arm/ex_impedance_control_damping.cpp index c26e656c..2f29f95d 100644 --- a/kits/arm/ex_impedance_control_damping.cpp +++ b/kits/arm/ex_impedance_control_damping.cpp @@ -38,7 +38,7 @@ int main(int argc, char* argv[]) ///// Config Setup /////// ////////////////////////// - const std::string example_config_file = "config/examples/ex_impedance_control_damping.cfg.yaml"; + const std::string example_config_file = "config/ex_impedance_control_damping.cfg.yaml"; std::vector errors; const auto example_config = RobotConfig::loadConfig(example_config_file, errors); @@ -86,7 +86,7 @@ int main(int argc, char* argv[]) vel_nan.fill(std::numeric_limits::quiet_NaN()); // Retrieve the impedance controller plugin from the arm - + // Pointer magic // Lock the weak_ptr and get a shared_ptr diff --git a/kits/arm/ex_impedance_control_fixed.cpp b/kits/arm/ex_impedance_control_fixed.cpp index aeb1f1e2..6b216349 100644 --- a/kits/arm/ex_impedance_control_fixed.cpp +++ b/kits/arm/ex_impedance_control_fixed.cpp @@ -36,7 +36,7 @@ int main(int argc, char* argv[]) ///// Config Setup /////// ////////////////////////// - const std::string example_config_file = "config/examples/ex_impedance_control_fixed.cfg.yaml"; + const std::string example_config_file = "config/ex_impedance_control_fixed.cfg.yaml"; std::vector errors; const auto example_config = RobotConfig::loadConfig(example_config_file, errors); diff --git a/kits/arm/ex_impedance_control_floor.cpp b/kits/arm/ex_impedance_control_floor.cpp index a605231b..18927641 100644 --- a/kits/arm/ex_impedance_control_floor.cpp +++ b/kits/arm/ex_impedance_control_floor.cpp @@ -36,7 +36,7 @@ int main(int argc, char* argv[]) ///// Config Setup /////// ////////////////////////// - const std::string example_config_file = "config/examples/ex_impedance_control_floor.cfg.yaml"; + const std::string example_config_file = "config/ex_impedance_control_floor.cfg.yaml"; std::vector errors; const auto example_config = RobotConfig::loadConfig(example_config_file, errors); @@ -104,7 +104,7 @@ int main(int argc, char* argv[]) std::cout << "Mobile IO connected." << std::endl; std::string instructions; - instructions = " Fixed demo"; + instructions = " Floor demo"; // Clear any garbage on screen mobile_io->clearText(); @@ -114,7 +114,7 @@ int main(int argc, char* argv[]) // Setup instructions 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" @@ -237,7 +237,7 @@ int main(int argc, char* argv[]) } // Clear MobileIO text - mobile_io->clearText(); + mobile->clearText(); return 0; } diff --git a/kits/arm/ex_impedance_control_gimbal.cpp b/kits/arm/ex_impedance_control_gimbal.cpp index 24876beb..7162feb5 100644 --- a/kits/arm/ex_impedance_control_gimbal.cpp +++ b/kits/arm/ex_impedance_control_gimbal.cpp @@ -36,7 +36,7 @@ int main(int argc, char* argv[]) ///// Config Setup /////// ////////////////////////// - const std::string example_config_file = "config/examples/ex_impedance_control_gimbal.cfg.yaml"; + const std::string example_config_file = "config/ex_impedance_control_gimbal.cfg.yaml"; std::vector errors; const auto example_config = RobotConfig::loadConfig(example_config_file, errors); diff --git a/kits/arm/ex_mobile_io_control.cpp b/kits/arm/ex_mobile_io_control.cpp index b304d360..b553f1ef 100644 --- a/kits/arm/ex_mobile_io_control.cpp +++ b/kits/arm/ex_mobile_io_control.cpp @@ -28,7 +28,7 @@ int main(int argc, char* argv[]) ///// Config Setup /////// ////////////////////////// - const std::string example_config_file = "config/examples/ex_mobile_io_control.cfg.yaml"; + const std::string example_config_file = "config/ex_mobile_io_control.cfg.yaml"; std::vector errors; const auto example_config = RobotConfig::loadConfig(example_config_file, errors); From c35092ce9bdecfaa2ea7b0f3b7dc42841e247d42 Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Wed, 14 Aug 2024 12:08:04 -0400 Subject: [PATCH 15/31] gravcomp and new gravcomp_toggle examples also integrated with config --- .../config/ex_gravity_compensation.cfg.yaml | 10 +- .../ex_gravity_compensation_toggle.cfg.yaml | 26 +++ ...on => ex_gravity_compensation_toggle.json} | 0 kits/arm/ex_gravity_compensation.cpp | 49 ++++-- kits/arm/ex_gravity_compensation_toggle.cpp | 166 ++++++++++++++++++ kits/arm/ex_impedance_control_cartesian.cpp | 2 + kits/arm/ex_impedance_control_damping.cpp | 2 + kits/arm/ex_impedance_control_fixed.cpp | 2 + kits/arm/ex_impedance_control_floor.cpp | 4 +- kits/arm/ex_impedance_control_gimbal.cpp | 2 + kits/arm/ex_mobile_io_control.cpp | 2 + projects/cmake/CMakeLists.txt | 1 + 12 files changed, 242 insertions(+), 24 deletions(-) create mode 100644 kits/arm/config/ex_gravity_compensation_toggle.cfg.yaml rename kits/arm/config/layouts/{ex_gravity_compensation.json => ex_gravity_compensation_toggle.json} (100%) create mode 100644 kits/arm/ex_gravity_compensation_toggle.cpp diff --git a/kits/arm/config/ex_gravity_compensation.cfg.yaml b/kits/arm/config/ex_gravity_compensation.cfg.yaml index 24cbceac..80704f52 100644 --- a/kits/arm/config/ex_gravity_compensation.cfg.yaml +++ b/kits/arm/config/ex_gravity_compensation.cfg.yaml @@ -1,6 +1,6 @@ # 6-DoF Arm version: 1.0 -families: ["Arm"] +families: ["HEBIArm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] hrdf: "hrdf/A-2085-06.hrdf" @@ -17,10 +17,4 @@ plugins: - type: DynamicsCompensationEffort name: dynamicsComp enabled: true - ramp_time: 0 - -user_data: - - mobile_io_family: "Arm" - mobile_io_name: "mobileIO" - mobile_io_layout: "layouts/ex_gravity_compensation.json" \ No newline at end of file + ramp_time: 0 \ No newline at end of file diff --git a/kits/arm/config/ex_gravity_compensation_toggle.cfg.yaml b/kits/arm/config/ex_gravity_compensation_toggle.cfg.yaml new file mode 100644 index 00000000..0eaebf72 --- /dev/null +++ b/kits/arm/config/ex_gravity_compensation_toggle.cfg.yaml @@ -0,0 +1,26 @@ +# 6-DoF Arm +version: 1.0 +families: ["HEBIArm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "hrdf/A-2085-06.hrdf" + +gains: + default: "gains/A-2085-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: false + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + +user_data: + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "layouts/ex_gravity_compensation_toggle.json" \ No newline at end of file diff --git a/kits/arm/config/layouts/ex_gravity_compensation.json b/kits/arm/config/layouts/ex_gravity_compensation_toggle.json similarity index 100% rename from kits/arm/config/layouts/ex_gravity_compensation.json rename to kits/arm/config/layouts/ex_gravity_compensation_toggle.json diff --git a/kits/arm/ex_gravity_compensation.cpp b/kits/arm/ex_gravity_compensation.cpp index be1d0103..35adb010 100644 --- a/kits/arm/ex_gravity_compensation.cpp +++ b/kits/arm/ex_gravity_compensation.cpp @@ -8,13 +8,14 @@ * demo. */ -// #include "lookup.hpp" -// #include "group.hpp" #include "group_command.hpp" #include "group_feedback.hpp" #include "robot_model.hpp" #include "arm/arm.hpp" +#include "util/mobile_io.hpp" #include +#include +#include "hebi_util.hpp" using namespace hebi; using namespace experimental; @@ -22,26 +23,44 @@ using namespace experimental; int main(int argc, char* argv[]) { ////////////////////////// - ///// Arm Setup ////////// + ///// Config Setup /////// ////////////////////////// - arm::Arm::Params params; + // Config file path + const std::string example_config_file = "config/ex_gravity_compensation.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; + } - // Setup Module Family and Module Names - params.families_ = {"Arm"}; //[change back] - params.names_ = {"J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"}; + // For this demo, we need just the arm + std::unique_ptr arm; + + ////////////////////////// + ///// Arm Setup ////////// + ////////////////////////// - // Read HRDF file to seutp a RobotModel object for the 6-DoF Arm - params.hrdf_file_ = "kits/arm/hrdf/A-2085-06.hrdf"; + // Create the arm object from the configuration + arm = hebi::experimental::arm::Arm::create(*example_config); - // Create the Arm Object - auto arm = arm::Arm::create(params); + // Keep retrying if arm not found while (!arm) { - arm = arm::Arm::create(params); - } + std::cerr << "Failed to create arm, retrying..." << std::endl; - // Load the gains file that is approriate to the arm - arm -> loadGains("kits/arm/gains/A-2085-06.xml"); + // Wait for 1 second before retrying + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Retry + arm = hebi::experimental::arm::Arm::create(*example_config); + } + std::cout << "Arm connected." << std::endl; ////////////////////////// //// Main Control Loop /// diff --git a/kits/arm/ex_gravity_compensation_toggle.cpp b/kits/arm/ex_gravity_compensation_toggle.cpp new file mode 100644 index 00000000..c62f4967 --- /dev/null +++ b/kits/arm/ex_gravity_compensation_toggle.cpp @@ -0,0 +1,166 @@ +/** + * 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 +#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 = hebi::experimental::arm::Arm::create(*example_config); + + // Keep retrying if arm not found + while (!arm) { + std::cerr << "Failed to create arm, retrying..." << std::endl; + + // Wait for 1 second before retrying + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Retry + arm = hebi::experimental::arm::Arm::create(*example_config); + } + std::cout << "Arm connected." << std::endl; + + // Retrieve the gravcomp plugin from the arm + + // Pointer magic + + // Lock the weak_ptr and get a shared_ptr + auto plugin_shared_ptr = arm->getPluginByName("gravComp").lock(); + + // Check if the shared_ptr is valid + if (!plugin_shared_ptr) { + std::cerr << "Failed to lock plugin shared_ptr. The plugin may have been destroyed." << std::endl; + return -1; + } + + // Downcast to ImpedanceController + 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, example_config_file); + + // 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; + + // Wait for 1 second before retrying + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Retry + mobile_io = createMobileIOFromConfig(*example_config, example_config_file); + } + std::cout << "Mobile IO connected." << std::endl; + + std::string instructions; + instructions = " Gravcomp demo"; + + // Clear any garbage on screen + mobile_io->clearText(); + + // Display instructions on screen + mobile_io->appendText(instructions); + + // Setup instructions + 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) == 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) == util::MobileIO::ButtonState::ToOn) { + + // Enable gravcomp + gravcomp_plugin_ptr->setEnabled(true); + } + else if (mobile_io->getButtonDiff(2) == 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 index 2bd6ad35..92ee28ce 100644 --- a/kits/arm/ex_impedance_control_cartesian.cpp +++ b/kits/arm/ex_impedance_control_cartesian.cpp @@ -36,9 +36,11 @@ 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; diff --git a/kits/arm/ex_impedance_control_damping.cpp b/kits/arm/ex_impedance_control_damping.cpp index 2f29f95d..44ca7689 100644 --- a/kits/arm/ex_impedance_control_damping.cpp +++ b/kits/arm/ex_impedance_control_damping.cpp @@ -38,9 +38,11 @@ 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; diff --git a/kits/arm/ex_impedance_control_fixed.cpp b/kits/arm/ex_impedance_control_fixed.cpp index 6b216349..c3d6cda4 100644 --- a/kits/arm/ex_impedance_control_fixed.cpp +++ b/kits/arm/ex_impedance_control_fixed.cpp @@ -36,9 +36,11 @@ 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; diff --git a/kits/arm/ex_impedance_control_floor.cpp b/kits/arm/ex_impedance_control_floor.cpp index 18927641..bdde1fea 100644 --- a/kits/arm/ex_impedance_control_floor.cpp +++ b/kits/arm/ex_impedance_control_floor.cpp @@ -36,9 +36,11 @@ 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; @@ -237,7 +239,7 @@ int main(int argc, char* argv[]) } // Clear MobileIO text - mobile->clearText(); + mobile_io->clearText(); return 0; } diff --git a/kits/arm/ex_impedance_control_gimbal.cpp b/kits/arm/ex_impedance_control_gimbal.cpp index 7162feb5..7f715318 100644 --- a/kits/arm/ex_impedance_control_gimbal.cpp +++ b/kits/arm/ex_impedance_control_gimbal.cpp @@ -36,9 +36,11 @@ 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; diff --git a/kits/arm/ex_mobile_io_control.cpp b/kits/arm/ex_mobile_io_control.cpp index b553f1ef..1e7e6fa6 100644 --- a/kits/arm/ex_mobile_io_control.cpp +++ b/kits/arm/ex_mobile_io_control.cpp @@ -28,9 +28,11 @@ int main(int argc, char* argv[]) ///// Config Setup /////// ////////////////////////// + // 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) { std::cerr << error << std::endl; diff --git a/projects/cmake/CMakeLists.txt b/projects/cmake/CMakeLists.txt index 615739c5..f3864993 100644 --- a/projects/cmake/CMakeLists.txt +++ b/projects/cmake/CMakeLists.txt @@ -275,6 +275,7 @@ 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 From c6ec868e83390c4297267d0b2230146b731eb7c3 Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Wed, 14 Aug 2024 12:29:14 -0400 Subject: [PATCH 16/31] ex_AR_kit integrated --- kits/arm/config/ex_AR_kit.cfg.yaml | 2 +- kits/arm/ex_AR_kit.cpp | 176 ++++++++++++++++------------- 2 files changed, 101 insertions(+), 77 deletions(-) diff --git a/kits/arm/config/ex_AR_kit.cfg.yaml b/kits/arm/config/ex_AR_kit.cfg.yaml index faa723da..66cbafa8 100644 --- a/kits/arm/config/ex_AR_kit.cfg.yaml +++ b/kits/arm/config/ex_AR_kit.cfg.yaml @@ -1,6 +1,6 @@ # 6-DoF Arm version: 1.0 -families: ["Arm"] +families: ["HEBIArm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] hrdf: "hrdf/A-2085-06.hrdf" diff --git a/kits/arm/ex_AR_kit.cpp b/kits/arm/ex_AR_kit.cpp index 4ef76df0..2bf0aca2 100644 --- a/kits/arm/ex_AR_kit.cpp +++ b/kits/arm/ex_AR_kit.cpp @@ -11,7 +11,8 @@ #include "arm/arm.hpp" #include "util/mobile_io.hpp" #include -#include +#include +#include "hebi_util.hpp" using namespace hebi; using namespace experimental; // For all things mobileIO @@ -31,56 +32,79 @@ Eigen::Matrix3d makeRotationMatrix (hebi::Quaternionf phone_orientation) { int main(int argc, char* argv[]) { ////////////////////////// - ///// Arm Setup ////////// + ///// Config Setup /////// ////////////////////////// - arm::Arm::Params params; - - // Setup Module Family and Module Names - params.families_ = {"Arm"}; - params.names_ = {"J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"}; + // Config file path + const std::string example_config_file = "config/ex_AR_kit.cfg.yaml"; + std::vector errors; - // Read HRDF file to setup a RobotModel object for the 6-DoF Arm - params.hrdf_file_ = "kits/arm/hrdf/A-2085-06.hrdf"; + // 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 ////////// + ////////////////////////// - // Setup Gripper - // std::shared_ptr> gripper(arm::EffortEndEffector<1>::create(params.families_[0], "gripperSpool").release()); - // params.end_effector_ = gripper; + // Create the arm object from the configuration + arm = hebi::experimental::arm::Arm::create(*example_config); - // Create the Arm Object - auto arm = arm::Arm::create(params); + // Keep retrying if arm not found while (!arm) { - arm = arm::Arm::create(params); + std::cerr << "Failed to create arm, retrying..." << std::endl; + + // Wait for 1 second before retrying + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Retry + arm = hebi::experimental::arm::Arm::create(*example_config); } + std::cout << "Arm connected." << std::endl; - // Load the gains file that is approriate to the arm - arm -> loadGains("kits/arm/gains/A-2085-06.xml"); + ////////////////////////// + //// MobileIO Setup ////// + ////////////////////////// - ///////////////////////// - //// 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, example_config_file); - // Create the MobileIO object - std::unique_ptr mobile = util::MobileIO::create(params.families_[0], "mobileIO"); - if (!mobile) - { - std::cout << "couldn't find mobile IO device!\n"; - return 1; - } + // 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; - // Clear any garbage on screen - mobile->resetUI(); + // Wait for 1 second before retrying + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Retry + mobile_io = createMobileIOFromConfig(*example_config, example_config_file); + } + std::cout << "Mobile IO connected." << 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"); + instructions = ("🏠 - Home Position\n📲 - AR Control Mode\n" + "🌍 - Grav Comp Mode\n❌ - Quit\n"); + + // Clear any garbage on screen + mobile_io->clearText(); // Display instructions on screen - mobile->appendText(instructions); + mobile_io->appendText(instructions); // Setup state variable for mobile device - auto last_mobile_state = mobile->update(); + auto last_mobile_state = mobile_io->update(); ////////////////////////// //// Main Control Loop /// @@ -90,13 +114,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().float_lists_.at("home_position").data(), example_config->getUserData().float_lists_.at("home_position").size()); + double soft_start_time = example_config->getUserData().floats_.at("soft_start_time"); + double xyz_scale = example_config->getUserData().floats_.at("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 @@ -117,7 +143,7 @@ int main(int argc, char* argv[]) if (softstart) { // End softstart when arm reaches its homePosition if (arm -> atGoal()){ - mobile->appendText("Softstart Complete!"); + mobile_io->appendText("Softstart Complete!"); softstart = false; continue; } @@ -128,54 +154,52 @@ int main(int argc, char* argv[]) } // Get latest mobile_state - auto updated_mobile = mobile->update(0); + auto updated_mobile_io = mobile_io->update(0); - if (!updated_mobile) - 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->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { + // 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)); + } + + // Button B3 - Start AR Control + if (mobile_io->getButtonDiff(3) == 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) { + arm -> cancelGoal(); ar_mode = false; - arm -> setGoal(arm::Goal::createFromPosition(4, home_position)); - } - - // Button B3 - Start AR Control - if (mobile->getButtonDiff(3) == util::MobileIO::ButtonState::ToOn) { - xyz_phone_init << mobile -> getLastFeedback().mobile().arPosition().get().getX(), - mobile -> getLastFeedback().mobile().arPosition().get().getY(), - mobile -> getLastFeedback().mobile().arPosition().get().getZ(); - std::cout << xyz_phone_init << std::endl; - rot_phone_init = makeRotationMatrix(mobile -> getLastFeedback().mobile().arOrientation().get()); - ar_mode = true; - } - - // Button B6 - Grav Comp Mode - if (mobile->getButtonDiff(6) == util::MobileIO::ButtonState::ToOn) { - arm -> cancelGoal(); - ar_mode = false; - } - - // Button B8 - End Demo - if (mobile->getButtonDiff(8) == util::MobileIO::ButtonState::ToOn) { - // Clear MobileIO text - mobile->resetUI(); - return 1; - } + } + + // Button B8 - End Demo + if (mobile_io->getButtonDiff(8) == util::MobileIO::ButtonState::ToOn) { + // Clear MobileIO text + mobile_io->resetUI(); + return 1; + } } if (ar_mode) { // Get the latest mobile position and orientation Eigen::Vector3d xyz_phone; - xyz_phone << mobile->getLastFeedback().mobile().arPosition().get().getX(), - mobile->getLastFeedback().mobile().arPosition().get().getY(), - mobile->getLastFeedback().mobile().arPosition().get().getZ(); - auto rot_phone = makeRotationMatrix(mobile->getLastFeedback().mobile().arOrientation().get()); + xyz_phone << mobile_io->getLastFeedback().mobile().arPosition().get().getX(), + mobile_io->getLastFeedback().mobile().arPosition().get().getY(), + mobile_io->getLastFeedback().mobile().arPosition().get().getZ(); + 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; @@ -193,7 +217,7 @@ int main(int argc, char* argv[]) } // Clear MobileIO text - mobile->resetUI(); + mobile_io->resetUI(); return 0; } From b988f085927e71396da30ad58e6ad0ab5666ea93 Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Wed, 14 Aug 2024 13:48:29 -0400 Subject: [PATCH 17/31] teach repeat fully integrated --- kits/arm/config/ex_teach_repeat.cfg.yaml | 2 +- kits/arm/ex_teach_repeat.cpp | 213 +++++++++++++++-------- 2 files changed, 140 insertions(+), 75 deletions(-) diff --git a/kits/arm/config/ex_teach_repeat.cfg.yaml b/kits/arm/config/ex_teach_repeat.cfg.yaml index 1eedfc45..d4d4747f 100644 --- a/kits/arm/config/ex_teach_repeat.cfg.yaml +++ b/kits/arm/config/ex_teach_repeat.cfg.yaml @@ -1,6 +1,6 @@ # 6-DoF Arm version: 1.0 -families: ["Arm"] +families: ["HEBIArm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] hrdf: "hrdf/A-2085-06.hrdf" diff --git a/kits/arm/ex_teach_repeat.cpp b/kits/arm/ex_teach_repeat.cpp index 82d9b530..b526e0e9 100644 --- a/kits/arm/ex_teach_repeat.cpp +++ b/kits/arm/ex_teach_repeat.cpp @@ -13,7 +13,8 @@ #include "arm/arm.hpp" #include "util/mobile_io.hpp" #include -#include +#include +#include "hebi_util.hpp" using namespace hebi; using namespace experimental; @@ -23,6 +24,7 @@ struct Waypoint Eigen::VectorXd positions; Eigen::VectorXd vels; Eigen::VectorXd accels; + double time; }; struct State @@ -31,148 +33,211 @@ 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; } return arm::Goal::createFromWaypoints(times, target_pos, target_vels, target_accels); } - int main(int argc, char* argv[]) { ////////////////////////// - ///// Arm Setup ////////// + ///// Config Setup /////// ////////////////////////// - arm::Arm::Params params; - - // Setup Module Family and Module Names - std::string family = "Arm"; - params.families_ = {family}; - params.names_ = {"J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"}; + // Config file path + const std::string example_config_file = "config/ex_teach_repeat.cfg.yaml"; + std::vector errors; - // Read HRDF file to setup a RobotModel object for the 6-DoF Arm - // Make sure you are running this from the correct directory! - params.hrdf_file_ = "kits/arm/hrdf/A-2085-06.hrdf"; + // 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 - auto arm = arm::Arm::create(params); + // Create the arm object from the configuration + arm = hebi::experimental::arm::Arm::create(*example_config); + + // Keep retrying if arm not found while (!arm) { - arm = arm::Arm::create(params); + std::cerr << "Failed to create arm, retrying..." << std::endl; + + // Wait for 1 second before retrying + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Retry + arm = hebi::experimental::arm::Arm::create(*example_config); } + std::cout << "Arm connected." << std::endl; - // Load the gains file that is approriate to the arm - arm -> loadGains("kits/arm/gains/A-2085-06.xml"); + ////////////////////////// + //// MobileIO Setup ////// + ////////////////////////// - ///////////////////////// - //// 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, example_config_file); - // Create the MobileIO object - std::unique_ptr mobile = util::MobileIO::create(family, "mobileIO"); - if (!mobile) - { - std::cout << "couldn't find mobile IO device!\n"; - return 1; + // 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; + + // Wait for 1 second before retrying + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Retry + mobile_io = createMobileIOFromConfig(*example_config, example_config_file); } + std::cout << "Mobile IO connected." << std::endl; // Clear any garbage on screen - mobile -> clearText(); + 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"); + + instructions = ("📌 - Add stop WP\n🗑️- Clear waypoints\n" + "🚏 - Add flow WP\n🔄 - Toggle training/playback\n" + "⏱️ - Up/down for longer/shorter time to waypoint\n" + "❌ - Quit\n"); // Display instructions on screen - mobile->appendText(instructions); + mobile_io->appendText(instructions); // Setup state variable for mobile device - auto last_mobile_state = mobile->update(); - + auto last_mobile_state = mobile_io->update(); - ////////////////////////// - //// Main Control Loop /// - ////////////////////////// + ///////////////////////////// + // Control Variables Setup // + ///////////////////////////// // Teach Repeat Variables State state; state.num_modules = arm->robotModel().getDoFCount(); + // 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().floats_.at("base_travel_time"); + double min_travel_time = example_config->getUserData().floats_.at("min_travel_time"); + + ////////////////////////// + //// Main Control Loop /// + ////////////////////////// + while(arm->update()) { // Get latest mobile_state - bool updated_mobile = mobile->update(0); + bool updated_mobile_io = mobile_io->update(0); - if (!updated_mobile) - 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->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) == 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->getButtonDiff(2) == util::MobileIO::ButtonState::ToOn) { - state.waypoints.clear(); - } + // Button B2 - Add Through Waypoint + if (mobile_io->getButtonDiff(2) == 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->getButtonDiff(3) == util::MobileIO::ButtonState::ToOn) { - addWaypoint(state, arm -> lastFeedback(), false); - } + // Button B3 - Toggle to Playback Waypoints + if (mobile_io->getButtonDiff(3) == 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->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) == 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) == util::MobileIO::ButtonState::ToOn) { + std::cout << "Entering training mode.\n" << std::endl; + arm->cancelGoal(); + run_mode = "training"; } - } - // Button B6 - Grav Comp Mode - if (mobile->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->getButtonDiff(8) == util::MobileIO::ButtonState::ToOn) { + if (mobile_io->getButtonDiff(8) == util::MobileIO::ButtonState::ToOn) { // Clear MobileIO text - mobile->clearText(); + mobile_io->clearText(); return 1; } } @@ -182,7 +247,7 @@ int main(int argc, char* argv[]) } // Clear MobileIO text - mobile->clearText(); + mobile_io->clearText(); return 0; } From 551e6cea9410cc1bb5485ca9c72dd4e1bbe1e85a Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Wed, 14 Aug 2024 14:25:02 -0400 Subject: [PATCH 18/31] Ironed a few things out --- kits/arm/config/ex_AR_kit.cfg.yaml | 2 +- .../config/ex_gravity_compensation.cfg.yaml | 2 +- .../ex_gravity_compensation_toggle.cfg.yaml | 2 +- .../ex_impedance_control_cartesian.cfg.yaml | 2 +- .../ex_impedance_control_damping.cfg.yaml | 2 +- .../ex_impedance_control_fixed.cfg.yaml | 2 +- .../ex_impedance_control_floor.cfg.yaml | 2 +- .../ex_impedance_control_gimbal.cfg.yaml | 2 +- kits/arm/config/ex_mobile_io_control.cfg.yaml | 2 +- kits/arm/config/ex_teach_repeat.cfg.yaml | 2 +- kits/arm/config/gains/T-arm.xml | 58 ------------------- kits/arm/config/hrdf/T-arm.hrdf | 21 ------- 12 files changed, 10 insertions(+), 89 deletions(-) delete mode 100644 kits/arm/config/gains/T-arm.xml delete mode 100644 kits/arm/config/hrdf/T-arm.hrdf diff --git a/kits/arm/config/ex_AR_kit.cfg.yaml b/kits/arm/config/ex_AR_kit.cfg.yaml index 66cbafa8..faa723da 100644 --- a/kits/arm/config/ex_AR_kit.cfg.yaml +++ b/kits/arm/config/ex_AR_kit.cfg.yaml @@ -1,6 +1,6 @@ # 6-DoF Arm version: 1.0 -families: ["HEBIArm"] +families: ["Arm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] hrdf: "hrdf/A-2085-06.hrdf" diff --git a/kits/arm/config/ex_gravity_compensation.cfg.yaml b/kits/arm/config/ex_gravity_compensation.cfg.yaml index 80704f52..504ce76e 100644 --- a/kits/arm/config/ex_gravity_compensation.cfg.yaml +++ b/kits/arm/config/ex_gravity_compensation.cfg.yaml @@ -1,6 +1,6 @@ # 6-DoF Arm version: 1.0 -families: ["HEBIArm"] +families: ["Arm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] hrdf: "hrdf/A-2085-06.hrdf" diff --git a/kits/arm/config/ex_gravity_compensation_toggle.cfg.yaml b/kits/arm/config/ex_gravity_compensation_toggle.cfg.yaml index 0eaebf72..cc92451a 100644 --- a/kits/arm/config/ex_gravity_compensation_toggle.cfg.yaml +++ b/kits/arm/config/ex_gravity_compensation_toggle.cfg.yaml @@ -1,6 +1,6 @@ # 6-DoF Arm version: 1.0 -families: ["HEBIArm"] +families: ["Arm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] hrdf: "hrdf/A-2085-06.hrdf" diff --git a/kits/arm/config/ex_impedance_control_cartesian.cfg.yaml b/kits/arm/config/ex_impedance_control_cartesian.cfg.yaml index 96f234a0..5dd000a2 100644 --- a/kits/arm/config/ex_impedance_control_cartesian.cfg.yaml +++ b/kits/arm/config/ex_impedance_control_cartesian.cfg.yaml @@ -1,6 +1,6 @@ # 6-DoF Arm version: 1.0 -families: ["HEBIArm"] +families: ["Arm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] hrdf: "hrdf/A-2085-06.hrdf" diff --git a/kits/arm/config/ex_impedance_control_damping.cfg.yaml b/kits/arm/config/ex_impedance_control_damping.cfg.yaml index d8a877c1..3e54d2a2 100644 --- a/kits/arm/config/ex_impedance_control_damping.cfg.yaml +++ b/kits/arm/config/ex_impedance_control_damping.cfg.yaml @@ -1,6 +1,6 @@ # 6-DoF Arm version: 1.0 -families: ["HEBIArm"] +families: ["Arm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] hrdf: "hrdf/A-2085-06.hrdf" diff --git a/kits/arm/config/ex_impedance_control_fixed.cfg.yaml b/kits/arm/config/ex_impedance_control_fixed.cfg.yaml index 4764c6fa..2b4ea4ab 100644 --- a/kits/arm/config/ex_impedance_control_fixed.cfg.yaml +++ b/kits/arm/config/ex_impedance_control_fixed.cfg.yaml @@ -1,6 +1,6 @@ # 6-DoF Arm version: 1.0 -families: ["HEBIArm"] +families: ["Arm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] hrdf: "hrdf/A-2085-06.hrdf" diff --git a/kits/arm/config/ex_impedance_control_floor.cfg.yaml b/kits/arm/config/ex_impedance_control_floor.cfg.yaml index 5c04af6d..9e5a89d5 100644 --- a/kits/arm/config/ex_impedance_control_floor.cfg.yaml +++ b/kits/arm/config/ex_impedance_control_floor.cfg.yaml @@ -1,6 +1,6 @@ # 6-DoF Arm version: 1.0 -families: ["HEBIArm"] +families: ["Arm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] hrdf: "hrdf/A-2085-06.hrdf" diff --git a/kits/arm/config/ex_impedance_control_gimbal.cfg.yaml b/kits/arm/config/ex_impedance_control_gimbal.cfg.yaml index b18a1540..210ab69a 100644 --- a/kits/arm/config/ex_impedance_control_gimbal.cfg.yaml +++ b/kits/arm/config/ex_impedance_control_gimbal.cfg.yaml @@ -1,6 +1,6 @@ # 6-DoF Arm version: 1.0 -families: ["HEBIArm"] +families: ["Arm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] hrdf: "hrdf/A-2085-06.hrdf" diff --git a/kits/arm/config/ex_mobile_io_control.cfg.yaml b/kits/arm/config/ex_mobile_io_control.cfg.yaml index 826421f2..401f7ee2 100644 --- a/kits/arm/config/ex_mobile_io_control.cfg.yaml +++ b/kits/arm/config/ex_mobile_io_control.cfg.yaml @@ -1,6 +1,6 @@ # 6-DoF Arm version: 1.0 -families: ["HEBIArm"] +families: ["Arm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] hrdf: "hrdf/A-2085-06.hrdf" diff --git a/kits/arm/config/ex_teach_repeat.cfg.yaml b/kits/arm/config/ex_teach_repeat.cfg.yaml index d4d4747f..1eedfc45 100644 --- a/kits/arm/config/ex_teach_repeat.cfg.yaml +++ b/kits/arm/config/ex_teach_repeat.cfg.yaml @@ -1,6 +1,6 @@ # 6-DoF Arm version: 1.0 -families: ["HEBIArm"] +families: ["Arm"] names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] hrdf: "hrdf/A-2085-06.hrdf" diff --git a/kits/arm/config/gains/T-arm.xml b/kits/arm/config/gains/T-arm.xml deleted file mode 100644 index 49b3338b..00000000 --- a/kits/arm/config/gains/T-arm.xml +++ /dev/null @@ -1,58 +0,0 @@ - - - - - 4 4 4 4 4 4 - - - 75 100 50 20 10 10 - 0 0 0 0 0 0 - 0 0 0 0 0 0 - 0 0 0 0 0 0 - 0 0 0 0 0 0 - 1 1 1 1 1 1 - 0 0 0 0 0 0 - -inf -inf -inf -inf -inf -inf - inf inf inf inf inf inf - 1 1 1 1 1 1 - -40 -40 -40 -10 -10 -10 - 40 40 40 10 10 10 - 1 1 1 1 1 1 - 1 1 1 1 1 1 - - - - 0.1 0.2 0.1 0.1 0.05 0.05 - 0 0 0 0 0 0 - 0 0 0 0 0 0 - 1 1 1 1 1 1 - 0 0 0 0 0 0 - 0.25 0.25 0.25 0.25 0.25 0.25 - 0 0 0 0 0 0 - -1.790422 -1.790422 -1.790422 -3.434687 -9.617128 -9.617128 - 1.790422 1.790422 1.790422 3.434687 9.617128 9.617128 - 1 1 1 1 1 1 - -1 -1 -1 -1 -1 -1 - 1 1 1 1 1 1 - 0.75 0.75 0.75 0.75 0.75 0.75 - 1 1 1 1 1 1 - - - - 0.05 0.05 0.05 0.1 0.20 0.20 - 0 0 0 0 0 0 - 0.0001 0.0001 0.0001 0.001 0.001 0.001 - 1 1 1 1 1 1 - 0.1 0.1 0.1 0.05 0.05 0.05 - 0.25 0.25 0.25 0.25 0.25 0.25 - 0 0 0 0 0 0 - -40 -40 -40 -25 -5 -5 - 40 40 40 25 5 5 - 1 1 1 1 1 1 - -1 -1 -1 -1 -1 -1 - 1 1 1 1 1 1 - 0.9 0.9 0.9 0.9 0.9 0.9 - 0 0 0 0 0 0 - - - diff --git a/kits/arm/config/hrdf/T-arm.hrdf b/kits/arm/config/hrdf/T-arm.hrdf deleted file mode 100644 index fd7a1dca..00000000 --- a/kits/arm/config/hrdf/T-arm.hrdf +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file From dd264b1fb7c7ac7c8c4a52f9f7dd4b7820af71c6 Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Tue, 20 Aug 2024 10:05:23 -0400 Subject: [PATCH 19/31] removed instructions from all examples tested out additional imrpovements in cartesian example --- kits/arm/ex_AR_kit.cpp | 11 ----------- kits/arm/ex_gravity_compensation_toggle.cpp | 9 --------- kits/arm/ex_impedance_control_cartesian.cpp | 15 +++------------ kits/arm/ex_impedance_control_damping.cpp | 9 --------- kits/arm/ex_impedance_control_fixed.cpp | 9 --------- kits/arm/ex_impedance_control_floor.cpp | 9 --------- kits/arm/ex_impedance_control_gimbal.cpp | 9 --------- kits/arm/ex_mobile_io_control.cpp | 10 ---------- kits/arm/ex_teach_repeat.cpp | 14 -------------- kits/arm/hebi_util.hpp | 2 +- 10 files changed, 4 insertions(+), 93 deletions(-) diff --git a/kits/arm/ex_AR_kit.cpp b/kits/arm/ex_AR_kit.cpp index 2bf0aca2..e4cc2c8a 100644 --- a/kits/arm/ex_AR_kit.cpp +++ b/kits/arm/ex_AR_kit.cpp @@ -92,20 +92,9 @@ int main(int argc, char* argv[]) } std::cout << "Mobile IO connected." << std::endl; - // Setup instructions for display - std::string instructions; - instructions = ("🏠 - Home Position\n📲 - AR Control Mode\n" - "🌍 - Grav Comp Mode\n❌ - Quit\n"); - // Clear any garbage on screen mobile_io->clearText(); - // Display instructions on screen - mobile_io->appendText(instructions); - - // Setup state variable for mobile device - auto last_mobile_state = mobile_io->update(); - ////////////////////////// //// Main Control Loop /// ////////////////////////// diff --git a/kits/arm/ex_gravity_compensation_toggle.cpp b/kits/arm/ex_gravity_compensation_toggle.cpp index c62f4967..bf3552d9 100644 --- a/kits/arm/ex_gravity_compensation_toggle.cpp +++ b/kits/arm/ex_gravity_compensation_toggle.cpp @@ -104,18 +104,9 @@ int main(int argc, char* argv[]) } std::cout << "Mobile IO connected." << std::endl; - std::string instructions; - instructions = " Gravcomp demo"; - // Clear any garbage on screen mobile_io->clearText(); - // Display instructions on screen - mobile_io->appendText(instructions); - - // Setup instructions - 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" diff --git a/kits/arm/ex_impedance_control_cartesian.cpp b/kits/arm/ex_impedance_control_cartesian.cpp index 92ee28ce..7e75b357 100644 --- a/kits/arm/ex_impedance_control_cartesian.cpp +++ b/kits/arm/ex_impedance_control_cartesian.cpp @@ -51,7 +51,7 @@ int main(int argc, char* argv[]) } // For this demo, we need the arm and mobile_io - std::unique_ptr arm; + std::unique_ptr arm; std::unique_ptr mobile_io; ////////////////////////// @@ -66,7 +66,7 @@ int main(int argc, char* argv[]) std::cerr << "Failed to create arm, retrying..." << std::endl; // Wait for 1 second before retrying - std::this_thread::sleep_for(std::chrono::seconds(1)); + // std::this_thread::sleep_for(std::chrono::seconds(1)); // Retry arm = hebi::experimental::arm::Arm::create(*example_config); @@ -98,25 +98,16 @@ int main(int argc, char* argv[]) std::cout << "Couldn't find Mobile IO. Check name, family, or device status..." << std::endl; // Wait for 1 second before retrying - std::this_thread::sleep_for(std::chrono::seconds(1)); + // std::this_thread::sleep_for(std::chrono::seconds(1)); // Retry mobile_io = createMobileIOFromConfig(*example_config, example_config_file); } std::cout << "Mobile IO connected." << std::endl; - - std::string instructions; - instructions = " Cartesian demo"; // Clear any garbage on screen mobile_io->clearText(); - // Display instructions on screen - mobile_io->appendText(instructions); - - // Setup instructions - 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" diff --git a/kits/arm/ex_impedance_control_damping.cpp b/kits/arm/ex_impedance_control_damping.cpp index 44ca7689..362e68d3 100644 --- a/kits/arm/ex_impedance_control_damping.cpp +++ b/kits/arm/ex_impedance_control_damping.cpp @@ -127,19 +127,10 @@ int main(int argc, char* argv[]) mobile_io = createMobileIOFromConfig(*example_config, example_config_file); } std::cout << "Mobile IO connected." << std::endl; - - std::string instructions; - instructions = " Damping demo"; // Clear any garbage on screen mobile_io->clearText(); - // Display instructions on screen - mobile_io->appendText(instructions); - - // Setup instructions - 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" diff --git a/kits/arm/ex_impedance_control_fixed.cpp b/kits/arm/ex_impedance_control_fixed.cpp index c3d6cda4..2d734ff1 100644 --- a/kits/arm/ex_impedance_control_fixed.cpp +++ b/kits/arm/ex_impedance_control_fixed.cpp @@ -104,19 +104,10 @@ int main(int argc, char* argv[]) mobile_io = createMobileIOFromConfig(*example_config, example_config_file); } std::cout << "Mobile IO connected." << std::endl; - - std::string instructions; - instructions = " Fixed demo"; // Clear any garbage on screen mobile_io->clearText(); - // Display instructions on screen - mobile_io->appendText(instructions); - - // Setup instructions - 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" diff --git a/kits/arm/ex_impedance_control_floor.cpp b/kits/arm/ex_impedance_control_floor.cpp index bdde1fea..89c19128 100644 --- a/kits/arm/ex_impedance_control_floor.cpp +++ b/kits/arm/ex_impedance_control_floor.cpp @@ -105,18 +105,9 @@ int main(int argc, char* argv[]) } std::cout << "Mobile IO connected." << std::endl; - std::string instructions; - instructions = " Floor demo"; - // Clear any garbage on screen mobile_io->clearText(); - // Display instructions on screen - mobile_io->appendText(instructions); - - // Setup instructions - 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" diff --git a/kits/arm/ex_impedance_control_gimbal.cpp b/kits/arm/ex_impedance_control_gimbal.cpp index 7f715318..8e82cd47 100644 --- a/kits/arm/ex_impedance_control_gimbal.cpp +++ b/kits/arm/ex_impedance_control_gimbal.cpp @@ -104,19 +104,10 @@ int main(int argc, char* argv[]) mobile_io = createMobileIOFromConfig(*example_config, example_config_file); } std::cout << "Mobile IO connected." << std::endl; - - std::string instructions; - instructions = " Gimbal demo"; // Clear any garbage on screen mobile_io->clearText(); - // Display instructions on screen - mobile_io->appendText(instructions); - - // Setup instructions - 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" diff --git a/kits/arm/ex_mobile_io_control.cpp b/kits/arm/ex_mobile_io_control.cpp index 1e7e6fa6..bd28cf63 100644 --- a/kits/arm/ex_mobile_io_control.cpp +++ b/kits/arm/ex_mobile_io_control.cpp @@ -85,19 +85,9 @@ int main(int argc, char* argv[]) } 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); - - // Setup instructions - auto last_state = mobile_io->update(); - ///////////////////////////// // Control Variables Setup // ///////////////////////////// diff --git a/kits/arm/ex_teach_repeat.cpp b/kits/arm/ex_teach_repeat.cpp index b526e0e9..51a29a5f 100644 --- a/kits/arm/ex_teach_repeat.cpp +++ b/kits/arm/ex_teach_repeat.cpp @@ -135,20 +135,6 @@ int main(int argc, char* argv[]) // Clear any garbage on screen mobile_io -> clearText(); - // Setup instructions for display - std::string instructions; - - instructions = ("📌 - Add stop WP\n🗑️- Clear waypoints\n" - "🚏 - Add flow WP\n🔄 - Toggle training/playback\n" - "⏱️ - Up/down for longer/shorter time to waypoint\n" - "❌ - Quit\n"); - - // Display instructions on screen - mobile_io->appendText(instructions); - - // Setup state variable for mobile device - auto last_mobile_state = mobile_io->update(); - ///////////////////////////// // Control Variables Setup // ///////////////////////////// diff --git a/kits/arm/hebi_util.hpp b/kits/arm/hebi_util.hpp index 843b894e..a931e46a 100644 --- a/kits/arm/hebi_util.hpp +++ b/kits/arm/hebi_util.hpp @@ -23,7 +23,7 @@ // Function to create Mobile IO from the config std::unique_ptr createMobileIOFromConfig(const hebi::RobotConfig& example_config, const std::string& config_file_path); -// Function to create gripper from the 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 From 2110d4fa947b112e121a6cc87f2a28f6298eb2e4 Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Tue, 20 Aug 2024 10:11:58 -0400 Subject: [PATCH 20/31] Re-added initial update for reset --- kits/arm/ex_AR_kit.cpp | 3 +++ kits/arm/ex_gravity_compensation_toggle.cpp | 3 +++ kits/arm/ex_impedance_control_cartesian.cpp | 7 +++++-- kits/arm/ex_impedance_control_damping.cpp | 3 +++ kits/arm/ex_impedance_control_fixed.cpp | 3 +++ kits/arm/ex_impedance_control_floor.cpp | 3 +++ kits/arm/ex_impedance_control_gimbal.cpp | 3 +++ kits/arm/ex_mobile_io_control.cpp | 3 +++ kits/arm/ex_teach_repeat.cpp | 3 +++ 9 files changed, 29 insertions(+), 2 deletions(-) diff --git a/kits/arm/ex_AR_kit.cpp b/kits/arm/ex_AR_kit.cpp index e4cc2c8a..9fd5eb4d 100644 --- a/kits/arm/ex_AR_kit.cpp +++ b/kits/arm/ex_AR_kit.cpp @@ -95,6 +95,9 @@ int main(int argc, char* argv[]) // Clear any garbage on screen mobile_io->clearText(); + // Refresh mobile_io + auto last_state = mobile_io->update(); + ////////////////////////// //// Main Control Loop /// ////////////////////////// diff --git a/kits/arm/ex_gravity_compensation_toggle.cpp b/kits/arm/ex_gravity_compensation_toggle.cpp index bf3552d9..9e013281 100644 --- a/kits/arm/ex_gravity_compensation_toggle.cpp +++ b/kits/arm/ex_gravity_compensation_toggle.cpp @@ -107,6 +107,9 @@ int main(int argc, char* argv[]) // 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" diff --git a/kits/arm/ex_impedance_control_cartesian.cpp b/kits/arm/ex_impedance_control_cartesian.cpp index 7e75b357..a9bb0a15 100644 --- a/kits/arm/ex_impedance_control_cartesian.cpp +++ b/kits/arm/ex_impedance_control_cartesian.cpp @@ -59,7 +59,7 @@ int main(int argc, char* argv[]) ////////////////////////// // Create the arm object from the configuration - arm = hebi::experimental::arm::Arm::create(*example_config); + arm = arm::Arm::create(*example_config); // Keep retrying if arm not found while (!arm) { @@ -69,7 +69,7 @@ int main(int argc, char* argv[]) // std::this_thread::sleep_for(std::chrono::seconds(1)); // Retry - arm = hebi::experimental::arm::Arm::create(*example_config); + arm = arm::Arm::create(*example_config); } std::cout << "Arm connected." << std::endl; @@ -108,6 +108,9 @@ int main(int argc, char* argv[]) // 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" diff --git a/kits/arm/ex_impedance_control_damping.cpp b/kits/arm/ex_impedance_control_damping.cpp index 362e68d3..ca75a251 100644 --- a/kits/arm/ex_impedance_control_damping.cpp +++ b/kits/arm/ex_impedance_control_damping.cpp @@ -131,6 +131,9 @@ int main(int argc, char* argv[]) // 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" diff --git a/kits/arm/ex_impedance_control_fixed.cpp b/kits/arm/ex_impedance_control_fixed.cpp index 2d734ff1..a282e8c1 100644 --- a/kits/arm/ex_impedance_control_fixed.cpp +++ b/kits/arm/ex_impedance_control_fixed.cpp @@ -108,6 +108,9 @@ int main(int argc, char* argv[]) // 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" diff --git a/kits/arm/ex_impedance_control_floor.cpp b/kits/arm/ex_impedance_control_floor.cpp index 89c19128..f1cb83dc 100644 --- a/kits/arm/ex_impedance_control_floor.cpp +++ b/kits/arm/ex_impedance_control_floor.cpp @@ -108,6 +108,9 @@ int main(int argc, char* argv[]) // 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" diff --git a/kits/arm/ex_impedance_control_gimbal.cpp b/kits/arm/ex_impedance_control_gimbal.cpp index 8e82cd47..c820b27b 100644 --- a/kits/arm/ex_impedance_control_gimbal.cpp +++ b/kits/arm/ex_impedance_control_gimbal.cpp @@ -108,6 +108,9 @@ int main(int argc, char* argv[]) // 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" diff --git a/kits/arm/ex_mobile_io_control.cpp b/kits/arm/ex_mobile_io_control.cpp index bd28cf63..6dcd5288 100644 --- a/kits/arm/ex_mobile_io_control.cpp +++ b/kits/arm/ex_mobile_io_control.cpp @@ -88,6 +88,9 @@ int main(int argc, char* argv[]) // Clear any garbage on screen mobile_io->clearText(); + // Refresh mobile_io + auto last_state = mobile_io->update(); + ///////////////////////////// // Control Variables Setup // ///////////////////////////// diff --git a/kits/arm/ex_teach_repeat.cpp b/kits/arm/ex_teach_repeat.cpp index 51a29a5f..2d0aa0dc 100644 --- a/kits/arm/ex_teach_repeat.cpp +++ b/kits/arm/ex_teach_repeat.cpp @@ -135,6 +135,9 @@ int main(int argc, char* argv[]) // Clear any garbage on screen mobile_io -> clearText(); + // Refresh mobile_io + auto last_state = mobile_io->update(); + ///////////////////////////// // Control Variables Setup // ///////////////////////////// From 75a1cd327911a3fe6698c7fa0b8f4a3aed3c1197 Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Tue, 20 Aug 2024 11:14:49 -0400 Subject: [PATCH 21/31] standardized namespace declarations fixed teach repeat bug --- kits/arm/ex_AR_kit.cpp | 14 +++++++------- kits/arm/ex_gravity_compensation.cpp | 6 +++--- kits/arm/ex_gravity_compensation_toggle.cpp | 14 +++++++------- kits/arm/ex_impedance_control_cartesian.cpp | 6 +++--- kits/arm/ex_impedance_control_damping.cpp | 16 +++++++--------- kits/arm/ex_impedance_control_fixed.cpp | 14 +++++++------- kits/arm/ex_impedance_control_floor.cpp | 12 ++++++------ kits/arm/ex_impedance_control_gimbal.cpp | 12 ++++++------ kits/arm/ex_mobile_io_control.cpp | 12 ++++++------ kits/arm/ex_teach_repeat.cpp | 20 ++++++++++---------- 10 files changed, 62 insertions(+), 64 deletions(-) diff --git a/kits/arm/ex_AR_kit.cpp b/kits/arm/ex_AR_kit.cpp index 9fd5eb4d..c85b8a4f 100644 --- a/kits/arm/ex_AR_kit.cpp +++ b/kits/arm/ex_AR_kit.cpp @@ -50,7 +50,7 @@ int main(int argc, char* argv[]) } // For this demo, we need the arm and mobile_io - std::unique_ptr arm; + std::unique_ptr arm; std::unique_ptr mobile_io; ////////////////////////// @@ -58,7 +58,7 @@ int main(int argc, char* argv[]) ////////////////////////// // Create the arm object from the configuration - arm = hebi::experimental::arm::Arm::create(*example_config); + arm = arm::Arm::create(*example_config); // Keep retrying if arm not found while (!arm) { @@ -68,7 +68,7 @@ int main(int argc, char* argv[]) std::this_thread::sleep_for(std::chrono::seconds(1)); // Retry - arm = hebi::experimental::arm::Arm::create(*example_config); + arm = arm::Arm::create(*example_config); } std::cout << "Arm connected." << std::endl; @@ -151,13 +151,13 @@ int main(int argc, char* argv[]) if (updated_mobile_io) { // Button B1 - Return to home position - if (mobile_io->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { + if (mobile_io->getButtonDiff(1) == hebi::util::MobileIO::ButtonState::ToOn) { ar_mode = false; arm -> setGoal(arm::Goal::createFromPosition(4, 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(); @@ -167,13 +167,13 @@ int main(int argc, char* argv[]) } // 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; diff --git a/kits/arm/ex_gravity_compensation.cpp b/kits/arm/ex_gravity_compensation.cpp index 35adb010..785f38cd 100644 --- a/kits/arm/ex_gravity_compensation.cpp +++ b/kits/arm/ex_gravity_compensation.cpp @@ -41,14 +41,14 @@ int main(int argc, char* argv[]) } // For this demo, we need just the arm - std::unique_ptr arm; + std::unique_ptr arm; ////////////////////////// ///// Arm Setup ////////// ////////////////////////// // Create the arm object from the configuration - arm = hebi::experimental::arm::Arm::create(*example_config); + arm = arm::Arm::create(*example_config); // Keep retrying if arm not found while (!arm) { @@ -58,7 +58,7 @@ int main(int argc, char* argv[]) std::this_thread::sleep_for(std::chrono::seconds(1)); // Retry - arm = hebi::experimental::arm::Arm::create(*example_config); + arm = arm::Arm::create(*example_config); } std::cout << "Arm connected." << std::endl; diff --git a/kits/arm/ex_gravity_compensation_toggle.cpp b/kits/arm/ex_gravity_compensation_toggle.cpp index 9e013281..30afdc01 100644 --- a/kits/arm/ex_gravity_compensation_toggle.cpp +++ b/kits/arm/ex_gravity_compensation_toggle.cpp @@ -41,7 +41,7 @@ int main(int argc, char* argv[]) } // For this demo, we need the arm and mobile_io - std::unique_ptr arm; + std::unique_ptr arm; std::unique_ptr mobile_io; ////////////////////////// @@ -49,7 +49,7 @@ int main(int argc, char* argv[]) ////////////////////////// // Create the arm object from the configuration - arm = hebi::experimental::arm::Arm::create(*example_config); + arm = arm::Arm::create(*example_config); // Keep retrying if arm not found while (!arm) { @@ -59,7 +59,7 @@ int main(int argc, char* argv[]) std::this_thread::sleep_for(std::chrono::seconds(1)); // Retry - arm = hebi::experimental::arm::Arm::create(*example_config); + arm = arm::Arm::create(*example_config); } std::cout << "Arm connected." << std::endl; @@ -77,7 +77,7 @@ int main(int argc, char* argv[]) } // Downcast to ImpedanceController - auto gravcomp_plugin_ptr = std::dynamic_pointer_cast(plugin_shared_ptr); + 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; @@ -131,19 +131,19 @@ int main(int argc, char* argv[]) ///////////////// // Buttton B1 - End demo - if (mobile_io->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { + 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) == util::MobileIO::ButtonState::ToOn) { + if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOn) { // Enable gravcomp gravcomp_plugin_ptr->setEnabled(true); } - else if (mobile_io->getButtonDiff(2) == util::MobileIO::ButtonState::ToOff){ + else if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOff){ // Disable gravcomp gravcomp_plugin_ptr->setEnabled(false); diff --git a/kits/arm/ex_impedance_control_cartesian.cpp b/kits/arm/ex_impedance_control_cartesian.cpp index a9bb0a15..71195096 100644 --- a/kits/arm/ex_impedance_control_cartesian.cpp +++ b/kits/arm/ex_impedance_control_cartesian.cpp @@ -144,20 +144,20 @@ int main(int argc, char* argv[]) ///////////////// // Buttton B1 - End demo - if (mobile_io->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { + 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) == util::MobileIO::ButtonState::ToOn) { + 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) == util::MobileIO::ButtonState::ToOff){ + else if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOff){ controller_on = false; } diff --git a/kits/arm/ex_impedance_control_damping.cpp b/kits/arm/ex_impedance_control_damping.cpp index ca75a251..7567e6be 100644 --- a/kits/arm/ex_impedance_control_damping.cpp +++ b/kits/arm/ex_impedance_control_damping.cpp @@ -18,8 +18,6 @@ This comprises the following demos: The following example is for the "Damping" demo: */ -// #include "lookup.hpp" -// #include "group.hpp" #include "group_command.hpp" #include "group_feedback.hpp" #include "robot_model.hpp" @@ -53,7 +51,7 @@ int main(int argc, char* argv[]) } // For this demo, we need the arm and mobile_io - std::unique_ptr arm; + std::unique_ptr arm; std::unique_ptr mobile_io; ////////////////////////// @@ -61,7 +59,7 @@ int main(int argc, char* argv[]) ////////////////////////// // Create the arm object from the configuration - arm = hebi::experimental::arm::Arm::create(*example_config); + arm = arm::Arm::create(*example_config); // Keep retrying if arm not found while (!arm) { @@ -71,7 +69,7 @@ int main(int argc, char* argv[]) std::this_thread::sleep_for(std::chrono::seconds(1)); // Retry - arm = hebi::experimental::arm::Arm::create(*example_config); + arm = arm::Arm::create(*example_config); } std::cout << "Arm connected." << std::endl; @@ -101,7 +99,7 @@ int main(int argc, char* argv[]) } // Downcast to ImpedanceController - auto impedance_plugin_ptr = std::dynamic_pointer_cast(plugin_shared_ptr); + 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; @@ -184,20 +182,20 @@ int main(int argc, char* argv[]) ///////////////// // Buttton B1 - End demo - if (mobile_io->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { + 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) == util::MobileIO::ButtonState::ToOn) { + 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) == util::MobileIO::ButtonState::ToOff){ + else if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOff){ controller_on = false; } diff --git a/kits/arm/ex_impedance_control_fixed.cpp b/kits/arm/ex_impedance_control_fixed.cpp index a282e8c1..4a9a634d 100644 --- a/kits/arm/ex_impedance_control_fixed.cpp +++ b/kits/arm/ex_impedance_control_fixed.cpp @@ -51,15 +51,15 @@ int main(int argc, char* argv[]) } // For this demo, we need the arm and mobile_io - std::unique_ptr arm; - std::unique_ptr mobile_io; + std::unique_ptr arm; + std::unique_ptr mobile_io; ////////////////////////// ///// Arm Setup ////////// ////////////////////////// // Create the arm object from the configuration - arm = hebi::experimental::arm::Arm::create(*example_config); + arm = arm::Arm::create(*example_config); // Keep retrying if arm not found while (!arm) { @@ -69,7 +69,7 @@ int main(int argc, char* argv[]) std::this_thread::sleep_for(std::chrono::seconds(1)); // Retry - arm = hebi::experimental::arm::Arm::create(*example_config); + arm = arm::Arm::create(*example_config); } std::cout << "Arm connected." << std::endl; @@ -144,20 +144,20 @@ int main(int argc, char* argv[]) ///////////////// // Buttton B1 - End demo - if (mobile_io->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { + 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) == util::MobileIO::ButtonState::ToOn) { + 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) == util::MobileIO::ButtonState::ToOff){ + else if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOff){ controller_on = false; } diff --git a/kits/arm/ex_impedance_control_floor.cpp b/kits/arm/ex_impedance_control_floor.cpp index f1cb83dc..28c79cec 100644 --- a/kits/arm/ex_impedance_control_floor.cpp +++ b/kits/arm/ex_impedance_control_floor.cpp @@ -51,7 +51,7 @@ int main(int argc, char* argv[]) } // For this demo, we need the arm and mobile_io - std::unique_ptr arm; + std::unique_ptr arm; std::unique_ptr mobile_io; ////////////////////////// @@ -59,7 +59,7 @@ int main(int argc, char* argv[]) ////////////////////////// // Create the arm object from the configuration - arm = hebi::experimental::arm::Arm::create(*example_config); + arm = arm::Arm::create(*example_config); // Keep retrying if arm not found while (!arm) { @@ -69,7 +69,7 @@ int main(int argc, char* argv[]) std::this_thread::sleep_for(std::chrono::seconds(1)); // Retry - arm = hebi::experimental::arm::Arm::create(*example_config); + arm = arm::Arm::create(*example_config); } std::cout << "Arm connected." << std::endl; @@ -147,14 +147,14 @@ int main(int argc, char* argv[]) ///////////////// // Buttton B1 - End demo - if (mobile_io->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { + 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) == util::MobileIO::ButtonState::ToOn) { + if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOn) { controller_on = true; @@ -173,7 +173,7 @@ int main(int argc, char* argv[]) // Update flags to indicate having left the floor cancel_command_flag = true; } - else if (mobile_io->getButtonDiff(2) == util::MobileIO::ButtonState::ToOff){ + else if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOff){ controller_on = false; } diff --git a/kits/arm/ex_impedance_control_gimbal.cpp b/kits/arm/ex_impedance_control_gimbal.cpp index c820b27b..74a886b1 100644 --- a/kits/arm/ex_impedance_control_gimbal.cpp +++ b/kits/arm/ex_impedance_control_gimbal.cpp @@ -51,7 +51,7 @@ int main(int argc, char* argv[]) } // For this demo, we need the arm and mobile_io - std::unique_ptr arm; + std::unique_ptr arm; std::unique_ptr mobile_io; ////////////////////////// @@ -59,7 +59,7 @@ int main(int argc, char* argv[]) ////////////////////////// // Create the arm object from the configuration - arm = hebi::experimental::arm::Arm::create(*example_config); + arm = arm::Arm::create(*example_config); // Keep retrying if arm not found while (!arm) { @@ -69,7 +69,7 @@ int main(int argc, char* argv[]) std::this_thread::sleep_for(std::chrono::seconds(1)); // Retry - arm = hebi::experimental::arm::Arm::create(*example_config); + arm = arm::Arm::create(*example_config); } std::cout << "Arm connected." << std::endl; @@ -144,20 +144,20 @@ int main(int argc, char* argv[]) ///////////////// // Buttton B1 - End demo - if (mobile_io->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { + 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) == util::MobileIO::ButtonState::ToOn) { + 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) == util::MobileIO::ButtonState::ToOff){ + else if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOff){ controller_on = false; } diff --git a/kits/arm/ex_mobile_io_control.cpp b/kits/arm/ex_mobile_io_control.cpp index 6dcd5288..8c23b2b8 100644 --- a/kits/arm/ex_mobile_io_control.cpp +++ b/kits/arm/ex_mobile_io_control.cpp @@ -43,7 +43,7 @@ int main(int argc, char* argv[]) } // For this demo, we need the arm and mobile_io - std::unique_ptr arm; + std::unique_ptr arm; std::unique_ptr mobile_io; ////////////////////////// @@ -51,7 +51,7 @@ int main(int argc, char* argv[]) ////////////////////////// // Create the arm object from the configuration - arm = hebi::experimental::arm::Arm::create(*example_config); + arm = arm::Arm::create(*example_config); // Keep retrying if arm not found while (!arm) { @@ -61,7 +61,7 @@ int main(int argc, char* argv[]) std::this_thread::sleep_for(std::chrono::seconds(1)); // Retry - arm = hebi::experimental::arm::Arm::create(*example_config); + arm = arm::Arm::create(*example_config); } std::cout << "Arm connected." << std::endl; @@ -122,19 +122,19 @@ int main(int argc, char* argv[]) // BN - Waypoint N (N = 1, 2 , 3) for (int button = 1; button <= 3; button++) { - if (mobile_io->getButtonDiff(button) == util::MobileIO::ButtonState::ToOn) { + 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 2d0aa0dc..14840fe8 100644 --- a/kits/arm/ex_teach_repeat.cpp +++ b/kits/arm/ex_teach_repeat.cpp @@ -64,7 +64,7 @@ arm::Goal playWaypoints (State& state) { 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; + times[i] = state.waypoints[i].time + (i > 0 ? times[i-1] : 0.0); } return arm::Goal::createFromWaypoints(times, target_pos, target_vels, target_accels); } @@ -90,7 +90,7 @@ int main(int argc, char* argv[]) } // For this demo, we need the arm and mobile_io - std::unique_ptr arm; + std::unique_ptr arm; std::unique_ptr mobile_io; ////////////////////////// @@ -98,7 +98,7 @@ int main(int argc, char* argv[]) ////////////////////////// // Create the arm object from the configuration - arm = hebi::experimental::arm::Arm::create(*example_config); + arm = arm::Arm::create(*example_config); // Keep retrying if arm not found while (!arm) { @@ -108,7 +108,7 @@ int main(int argc, char* argv[]) std::this_thread::sleep_for(std::chrono::seconds(1)); // Retry - arm = hebi::experimental::arm::Arm::create(*example_config); + arm = arm::Arm::create(*example_config); } std::cout << "Arm connected." << std::endl; @@ -173,7 +173,7 @@ int main(int argc, char* argv[]) slider3 = mobile_io->getAxis(3); // Buttton B1 - Add Stop Waypoint - if (mobile_io->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { + if (mobile_io->getButtonDiff(1) == hebi::util::MobileIO::ButtonState::ToOn) { addWaypoint(state, base_travel_time + slider3 * (base_travel_time - min_travel_time), arm -> lastFeedback(), @@ -181,7 +181,7 @@ int main(int argc, char* argv[]) } // Button B2 - Add Through Waypoint - if (mobile_io->getButtonDiff(2) == util::MobileIO::ButtonState::ToOn) { + if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOn) { addWaypoint(state, base_travel_time + slider3 * (base_travel_time - min_travel_time), arm -> lastFeedback(), @@ -189,7 +189,7 @@ int main(int argc, char* argv[]) } // Button B3 - Toggle to Playback Waypoints - if (mobile_io->getButtonDiff(3) == util::MobileIO::ButtonState::ToOn) { + 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; } @@ -202,7 +202,7 @@ int main(int argc, char* argv[]) } // Button B4 - Clear Waypoints - if (mobile_io->getButtonDiff(4) == util::MobileIO::ButtonState::ToOn) { + if (mobile_io->getButtonDiff(4) == hebi::util::MobileIO::ButtonState::ToOn) { std::cout << "Discarding waypoints.\n" << std::endl; state.waypoints.clear(); } @@ -210,7 +210,7 @@ int main(int argc, char* argv[]) else if (run_mode == "playback") { // Button B3 - Toggle to Training Mode - if (mobile_io->getButtonDiff(3) == util::MobileIO::ButtonState::ToOn) { + if (mobile_io->getButtonDiff(3) == hebi::util::MobileIO::ButtonState::ToOn) { std::cout << "Entering training mode.\n" << std::endl; arm->cancelGoal(); run_mode = "training"; @@ -224,7 +224,7 @@ int main(int argc, char* argv[]) } // 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; From e5dd48e82e2f09303158a727bdcc88f09a537c5c Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Tue, 20 Aug 2024 11:22:25 -0400 Subject: [PATCH 22/31] removed all sleeps --- kits/arm/ex_AR_kit.cpp | 7 ------- kits/arm/ex_gravity_compensation.cpp | 4 ---- kits/arm/ex_gravity_compensation_toggle.cpp | 7 ------- kits/arm/ex_impedance_control_cartesian.cpp | 7 ------- kits/arm/ex_impedance_control_damping.cpp | 7 ------- kits/arm/ex_impedance_control_fixed.cpp | 7 ------- kits/arm/ex_impedance_control_floor.cpp | 7 ------- kits/arm/ex_impedance_control_gimbal.cpp | 7 ------- kits/arm/ex_mobile_io_control.cpp | 7 ------- kits/arm/ex_teach_repeat.cpp | 7 ------- 10 files changed, 67 deletions(-) diff --git a/kits/arm/ex_AR_kit.cpp b/kits/arm/ex_AR_kit.cpp index c85b8a4f..17c01060 100644 --- a/kits/arm/ex_AR_kit.cpp +++ b/kits/arm/ex_AR_kit.cpp @@ -11,7 +11,6 @@ #include "arm/arm.hpp" #include "util/mobile_io.hpp" #include -#include #include "hebi_util.hpp" using namespace hebi; @@ -64,9 +63,6 @@ int main(int argc, char* argv[]) while (!arm) { std::cerr << "Failed to create arm, retrying..." << std::endl; - // Wait for 1 second before retrying - std::this_thread::sleep_for(std::chrono::seconds(1)); - // Retry arm = arm::Arm::create(*example_config); } @@ -84,9 +80,6 @@ int main(int argc, char* argv[]) while (mobile_io == nullptr) { std::cout << "Couldn't find Mobile IO. Check name, family, or device status..." << std::endl; - // Wait for 1 second before retrying - std::this_thread::sleep_for(std::chrono::seconds(1)); - // Retry mobile_io = createMobileIOFromConfig(*example_config, example_config_file); } diff --git a/kits/arm/ex_gravity_compensation.cpp b/kits/arm/ex_gravity_compensation.cpp index 785f38cd..c6121fad 100644 --- a/kits/arm/ex_gravity_compensation.cpp +++ b/kits/arm/ex_gravity_compensation.cpp @@ -14,7 +14,6 @@ #include "arm/arm.hpp" #include "util/mobile_io.hpp" #include -#include #include "hebi_util.hpp" using namespace hebi; @@ -54,9 +53,6 @@ int main(int argc, char* argv[]) while (!arm) { std::cerr << "Failed to create arm, retrying..." << std::endl; - // Wait for 1 second before retrying - std::this_thread::sleep_for(std::chrono::seconds(1)); - // Retry arm = arm::Arm::create(*example_config); } diff --git a/kits/arm/ex_gravity_compensation_toggle.cpp b/kits/arm/ex_gravity_compensation_toggle.cpp index 30afdc01..42df456d 100644 --- a/kits/arm/ex_gravity_compensation_toggle.cpp +++ b/kits/arm/ex_gravity_compensation_toggle.cpp @@ -14,7 +14,6 @@ #include "arm/arm.hpp" #include "util/mobile_io.hpp" #include -#include #include "hebi_util.hpp" using namespace hebi; @@ -55,9 +54,6 @@ int main(int argc, char* argv[]) while (!arm) { std::cerr << "Failed to create arm, retrying..." << std::endl; - // Wait for 1 second before retrying - std::this_thread::sleep_for(std::chrono::seconds(1)); - // Retry arm = arm::Arm::create(*example_config); } @@ -96,9 +92,6 @@ int main(int argc, char* argv[]) while (mobile_io == nullptr) { std::cout << "Couldn't find Mobile IO. Check name, family, or device status..." << std::endl; - // Wait for 1 second before retrying - std::this_thread::sleep_for(std::chrono::seconds(1)); - // Retry mobile_io = createMobileIOFromConfig(*example_config, example_config_file); } diff --git a/kits/arm/ex_impedance_control_cartesian.cpp b/kits/arm/ex_impedance_control_cartesian.cpp index 71195096..8f8359e6 100644 --- a/kits/arm/ex_impedance_control_cartesian.cpp +++ b/kits/arm/ex_impedance_control_cartesian.cpp @@ -24,7 +24,6 @@ The following example is for the "Cartesian" demo: #include "arm/arm.hpp" #include "util/mobile_io.hpp" #include -#include #include "hebi_util.hpp" using namespace hebi; @@ -65,9 +64,6 @@ int main(int argc, char* argv[]) while (!arm) { std::cerr << "Failed to create arm, retrying..." << std::endl; - // Wait for 1 second before retrying - // std::this_thread::sleep_for(std::chrono::seconds(1)); - // Retry arm = arm::Arm::create(*example_config); } @@ -97,9 +93,6 @@ int main(int argc, char* argv[]) while (mobile_io == nullptr) { std::cout << "Couldn't find Mobile IO. Check name, family, or device status..." << std::endl; - // Wait for 1 second before retrying - // std::this_thread::sleep_for(std::chrono::seconds(1)); - // Retry mobile_io = createMobileIOFromConfig(*example_config, example_config_file); } diff --git a/kits/arm/ex_impedance_control_damping.cpp b/kits/arm/ex_impedance_control_damping.cpp index 7567e6be..aff952db 100644 --- a/kits/arm/ex_impedance_control_damping.cpp +++ b/kits/arm/ex_impedance_control_damping.cpp @@ -24,7 +24,6 @@ The following example is for the "Damping" demo: #include "arm/arm.hpp" #include "util/mobile_io.hpp" #include -#include #include "hebi_util.hpp" using namespace hebi; @@ -65,9 +64,6 @@ int main(int argc, char* argv[]) while (!arm) { std::cerr << "Failed to create arm, retrying..." << std::endl; - // Wait for 1 second before retrying - std::this_thread::sleep_for(std::chrono::seconds(1)); - // Retry arm = arm::Arm::create(*example_config); } @@ -118,9 +114,6 @@ int main(int argc, char* argv[]) while (mobile_io == nullptr) { std::cout << "Couldn't find Mobile IO. Check name, family, or device status..." << std::endl; - // Wait for 1 second before retrying - std::this_thread::sleep_for(std::chrono::seconds(1)); - // Retry mobile_io = createMobileIOFromConfig(*example_config, example_config_file); } diff --git a/kits/arm/ex_impedance_control_fixed.cpp b/kits/arm/ex_impedance_control_fixed.cpp index 4a9a634d..01b51093 100644 --- a/kits/arm/ex_impedance_control_fixed.cpp +++ b/kits/arm/ex_impedance_control_fixed.cpp @@ -24,7 +24,6 @@ The following example is for the "Fixed" demo: #include "arm/arm.hpp" #include "util/mobile_io.hpp" #include -#include #include "hebi_util.hpp" using namespace hebi; @@ -65,9 +64,6 @@ int main(int argc, char* argv[]) while (!arm) { std::cerr << "Failed to create arm, retrying..." << std::endl; - // Wait for 1 second before retrying - std::this_thread::sleep_for(std::chrono::seconds(1)); - // Retry arm = arm::Arm::create(*example_config); } @@ -97,9 +93,6 @@ int main(int argc, char* argv[]) while (mobile_io == nullptr) { std::cout << "Couldn't find Mobile IO. Check name, family, or device status..." << std::endl; - // Wait for 1 second before retrying - std::this_thread::sleep_for(std::chrono::seconds(1)); - // Retry mobile_io = createMobileIOFromConfig(*example_config, example_config_file); } diff --git a/kits/arm/ex_impedance_control_floor.cpp b/kits/arm/ex_impedance_control_floor.cpp index 28c79cec..6076b09c 100644 --- a/kits/arm/ex_impedance_control_floor.cpp +++ b/kits/arm/ex_impedance_control_floor.cpp @@ -24,7 +24,6 @@ The following example is for the "Floor" demo: #include "arm/arm.hpp" #include "util/mobile_io.hpp" #include -#include #include "hebi_util.hpp" using namespace hebi; @@ -65,9 +64,6 @@ int main(int argc, char* argv[]) while (!arm) { std::cerr << "Failed to create arm, retrying..." << std::endl; - // Wait for 1 second before retrying - std::this_thread::sleep_for(std::chrono::seconds(1)); - // Retry arm = arm::Arm::create(*example_config); } @@ -97,9 +93,6 @@ int main(int argc, char* argv[]) while (mobile_io == nullptr) { std::cout << "Couldn't find Mobile IO. Check name, family, or device status..." << std::endl; - // Wait for 1 second before retrying - std::this_thread::sleep_for(std::chrono::seconds(1)); - // Retry mobile_io = createMobileIOFromConfig(*example_config, example_config_file); } diff --git a/kits/arm/ex_impedance_control_gimbal.cpp b/kits/arm/ex_impedance_control_gimbal.cpp index 74a886b1..77acd80f 100644 --- a/kits/arm/ex_impedance_control_gimbal.cpp +++ b/kits/arm/ex_impedance_control_gimbal.cpp @@ -24,7 +24,6 @@ The following example is for the "Gimbal" demo: #include "arm/arm.hpp" #include "util/mobile_io.hpp" #include -#include #include "hebi_util.hpp" using namespace hebi; @@ -65,9 +64,6 @@ int main(int argc, char* argv[]) while (!arm) { std::cerr << "Failed to create arm, retrying..." << std::endl; - // Wait for 1 second before retrying - std::this_thread::sleep_for(std::chrono::seconds(1)); - // Retry arm = arm::Arm::create(*example_config); } @@ -97,9 +93,6 @@ int main(int argc, char* argv[]) while (mobile_io == nullptr) { std::cout << "Couldn't find Mobile IO. Check name, family, or device status..." << std::endl; - // Wait for 1 second before retrying - std::this_thread::sleep_for(std::chrono::seconds(1)); - // Retry mobile_io = createMobileIOFromConfig(*example_config, example_config_file); } diff --git a/kits/arm/ex_mobile_io_control.cpp b/kits/arm/ex_mobile_io_control.cpp index 8c23b2b8..8b83ede3 100644 --- a/kits/arm/ex_mobile_io_control.cpp +++ b/kits/arm/ex_mobile_io_control.cpp @@ -16,7 +16,6 @@ The correct way to store waypoints is by using se3 coordinates, and converting t #include "arm/arm.hpp" #include "util/mobile_io.hpp" #include -#include #include "hebi_util.hpp" using namespace hebi; @@ -57,9 +56,6 @@ int main(int argc, char* argv[]) while (!arm) { std::cerr << "Failed to create arm, retrying..." << std::endl; - // Wait for 1 second before retrying - std::this_thread::sleep_for(std::chrono::seconds(1)); - // Retry arm = arm::Arm::create(*example_config); } @@ -77,9 +73,6 @@ int main(int argc, char* argv[]) while (mobile_io == nullptr) { std::cout << "Couldn't find Mobile IO. Check name, family, or device status..." << std::endl; - // Wait for 1 second before retrying - std::this_thread::sleep_for(std::chrono::seconds(1)); - // Retry mobile_io = createMobileIOFromConfig(*example_config, example_config_file); } diff --git a/kits/arm/ex_teach_repeat.cpp b/kits/arm/ex_teach_repeat.cpp index 14840fe8..d2485987 100644 --- a/kits/arm/ex_teach_repeat.cpp +++ b/kits/arm/ex_teach_repeat.cpp @@ -13,7 +13,6 @@ #include "arm/arm.hpp" #include "util/mobile_io.hpp" #include -#include #include "hebi_util.hpp" using namespace hebi; @@ -104,9 +103,6 @@ int main(int argc, char* argv[]) while (!arm) { std::cerr << "Failed to create arm, retrying..." << std::endl; - // Wait for 1 second before retrying - std::this_thread::sleep_for(std::chrono::seconds(1)); - // Retry arm = arm::Arm::create(*example_config); } @@ -124,9 +120,6 @@ int main(int argc, char* argv[]) while (mobile_io == nullptr) { std::cout << "Couldn't find Mobile IO. Check name, family, or device status..." << std::endl; - // Wait for 1 second before retrying - std::this_thread::sleep_for(std::chrono::seconds(1)); - // Retry mobile_io = createMobileIOFromConfig(*example_config, example_config_file); } From 046a22146ebc70fff9f3eb884aaa86202c16923a Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Tue, 20 Aug 2024 12:14:50 -0400 Subject: [PATCH 23/31] changed getpluginbyname to getpluginbytype --- kits/arm/ex_gravity_compensation_toggle.cpp | 11 ++--------- kits/arm/ex_impedance_control_damping.cpp | 11 ++--------- 2 files changed, 4 insertions(+), 18 deletions(-) diff --git a/kits/arm/ex_gravity_compensation_toggle.cpp b/kits/arm/ex_gravity_compensation_toggle.cpp index 42df456d..f73d6ba3 100644 --- a/kits/arm/ex_gravity_compensation_toggle.cpp +++ b/kits/arm/ex_gravity_compensation_toggle.cpp @@ -61,20 +61,13 @@ int main(int argc, char* argv[]) // Retrieve the gravcomp plugin from the arm - // Pointer magic - - // Lock the weak_ptr and get a shared_ptr - auto plugin_shared_ptr = arm->getPluginByName("gravComp").lock(); - - // Check if the shared_ptr is valid + // 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; } - - // Downcast to ImpedanceController 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; diff --git a/kits/arm/ex_impedance_control_damping.cpp b/kits/arm/ex_impedance_control_damping.cpp index aff952db..c016bd0f 100644 --- a/kits/arm/ex_impedance_control_damping.cpp +++ b/kits/arm/ex_impedance_control_damping.cpp @@ -83,20 +83,13 @@ int main(int argc, char* argv[]) // Retrieve the impedance controller plugin from the arm - // Pointer magic - - // Lock the weak_ptr and get a shared_ptr - auto plugin_shared_ptr = arm->getPluginByName("impedanceController").lock(); - - // Check if the shared_ptr is valid + // 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; } - - // Downcast to ImpedanceController 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; From 6c75cd887596c9c4f3769a654afa51dcef1af0e0 Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Tue, 20 Aug 2024 14:22:14 -0400 Subject: [PATCH 24/31] UserData's getters have been incorporated --- kits/arm/ex_AR_kit.cpp | 6 +++--- kits/arm/ex_impedance_control_damping.cpp | 14 +++++++------- kits/arm/ex_mobile_io_control.cpp | 8 ++++---- kits/arm/ex_teach_repeat.cpp | 4 ++-- kits/arm/hebi_util.cpp | 10 +++++----- 5 files changed, 21 insertions(+), 21 deletions(-) diff --git a/kits/arm/ex_AR_kit.cpp b/kits/arm/ex_AR_kit.cpp index 17c01060..4dee08c0 100644 --- a/kits/arm/ex_AR_kit.cpp +++ b/kits/arm/ex_AR_kit.cpp @@ -101,9 +101,9 @@ int main(int argc, char* argv[]) // Load user data from config Eigen::VectorXd home_position(arm -> robotModel().getDoFCount()); - home_position = Eigen::Map(example_config->getUserData().float_lists_.at("home_position").data(), example_config->getUserData().float_lists_.at("home_position").size()); - double soft_start_time = example_config->getUserData().floats_.at("soft_start_time"); - double xyz_scale = example_config->getUserData().floats_.at("xyz_scale"); + 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("soft_start_time"); + double xyz_scale = example_config->getUserData().getFloat("xyz_scale"); // Command the softstart to home position arm -> update(); diff --git a/kits/arm/ex_impedance_control_damping.cpp b/kits/arm/ex_impedance_control_damping.cpp index c016bd0f..8c839c13 100644 --- a/kits/arm/ex_impedance_control_damping.cpp +++ b/kits/arm/ex_impedance_control_damping.cpp @@ -129,7 +129,7 @@ int main(int argc, char* argv[]) ///////////////////////////// // Meters above the base for overdamped, critically damped, and underdamped cases respectively - std::vector lower_limits = example_config->getUserData().float_lists_.at("lower_limits"); + 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; @@ -139,16 +139,16 @@ int main(int argc, char* argv[]) std::vector damping_kp, damping_kd; // 0: Overdamped - damping_kp.push_back(Eigen::Map(example_config->getUserData().float_lists_.at("overdamped_kp").data(), example_config->getUserData().float_lists_.at("overdamped_kp").size())); - damping_kd.push_back(Eigen::Map(example_config->getUserData().float_lists_.at("overdamped_kd").data(), example_config->getUserData().float_lists_.at("overdamped_kd").size())); + 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().float_lists_.at("critically_damped_kp").data(), example_config->getUserData().float_lists_.at("critically_damped_kp").size())); - damping_kd.push_back(Eigen::Map(example_config->getUserData().float_lists_.at("critically_damped_kd").data(), example_config->getUserData().float_lists_.at("critically_damped_kd").size())); + 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().float_lists_.at("underdamped_kp").data(), example_config->getUserData().float_lists_.at("underdamped_kp").size())); - damping_kd.push_back(Eigen::Map(example_config->getUserData().float_lists_.at("underdamped_kd").data(), example_config->getUserData().float_lists_.at("underdamped_kd").size())); + 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; diff --git a/kits/arm/ex_mobile_io_control.cpp b/kits/arm/ex_mobile_io_control.cpp index 8b83ede3..e7be7d43 100644 --- a/kits/arm/ex_mobile_io_control.cpp +++ b/kits/arm/ex_mobile_io_control.cpp @@ -91,12 +91,12 @@ int main(int argc, char* argv[]) // Waypoints auto num_joints = arm->robotModel().getDoFCount(); std::vector waypoints; - waypoints.push_back(Eigen::Map(example_config->getUserData().float_lists_.at("waypoint_1").data(), example_config->getUserData().float_lists_.at("waypoint_1").size())); - waypoints.push_back(Eigen::Map(example_config->getUserData().float_lists_.at("waypoint_2").data(), example_config->getUserData().float_lists_.at("waypoint_2").size())); - waypoints.push_back(Eigen::Map(example_config->getUserData().float_lists_.at("waypoint_3").data(), example_config->getUserData().float_lists_.at("waypoint_3").size())); + 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().floats_.at("travel_time"); + double travel_time = example_config->getUserData().getFloat("travel_time"); ////////////////////////// //// Main Control Loop /// diff --git a/kits/arm/ex_teach_repeat.cpp b/kits/arm/ex_teach_repeat.cpp index d2485987..0efccdf0 100644 --- a/kits/arm/ex_teach_repeat.cpp +++ b/kits/arm/ex_teach_repeat.cpp @@ -146,8 +146,8 @@ int main(int argc, char* argv[]) double slider3 = 0.0; // Load travel times from config - double base_travel_time = example_config->getUserData().floats_.at("base_travel_time"); - double min_travel_time = example_config->getUserData().floats_.at("min_travel_time"); + 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 /// diff --git a/kits/arm/hebi_util.cpp b/kits/arm/hebi_util.cpp index 2588f33b..b9bc977f 100644 --- a/kits/arm/hebi_util.cpp +++ b/kits/arm/hebi_util.cpp @@ -43,17 +43,17 @@ std::unique_ptr createMobileIOFromConfig(const hebi::Robot config.getUserData().strings_.count("mobile_io_layout")) { // Check that all required fields are present and are strings - if (config.getUserData().strings_.at("mobile_io_family").empty() || - config.getUserData().strings_.at("mobile_io_name").empty()) { + 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."); } // Populate the dictionary - mobile_io_dict["family"] = config.getUserData().strings_.at("mobile_io_family"); - mobile_io_dict["name"] = config.getUserData().strings_.at("mobile_io_name"); + 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().strings_.at("mobile_io_layout")); + 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."); } From 0e699561379df467430537dbcd4665731aa52338 Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Tue, 20 Aug 2024 14:34:26 -0400 Subject: [PATCH 25/31] Checked every single example one last time --- kits/arm/ex_AR_kit.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/kits/arm/ex_AR_kit.cpp b/kits/arm/ex_AR_kit.cpp index 4dee08c0..dabc0c7d 100644 --- a/kits/arm/ex_AR_kit.cpp +++ b/kits/arm/ex_AR_kit.cpp @@ -154,7 +154,6 @@ int main(int argc, char* argv[]) 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; } From 153d9d5d588cae78caaba3fe8e59aa14b021ea8a Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Thu, 22 Aug 2024 11:00:19 -0400 Subject: [PATCH 26/31] Compatible with config.location_ --- kits/arm/ex_AR_kit.cpp | 4 ++-- kits/arm/ex_gravity_compensation_toggle.cpp | 4 ++-- kits/arm/ex_impedance_control_cartesian.cpp | 4 ++-- kits/arm/ex_impedance_control_damping.cpp | 4 ++-- kits/arm/ex_impedance_control_fixed.cpp | 4 ++-- kits/arm/ex_impedance_control_floor.cpp | 4 ++-- kits/arm/ex_impedance_control_gimbal.cpp | 4 ++-- kits/arm/ex_mobile_io_control.cpp | 4 ++-- kits/arm/ex_teach_repeat.cpp | 4 ++-- kits/arm/hebi_util.cpp | 5 ++--- kits/arm/hebi_util.hpp | 2 +- 11 files changed, 21 insertions(+), 22 deletions(-) diff --git a/kits/arm/ex_AR_kit.cpp b/kits/arm/ex_AR_kit.cpp index dabc0c7d..7a78a232 100644 --- a/kits/arm/ex_AR_kit.cpp +++ b/kits/arm/ex_AR_kit.cpp @@ -74,14 +74,14 @@ int main(int argc, char* argv[]) // 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, example_config_file); + 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, example_config_file); + mobile_io = createMobileIOFromConfig(*example_config); } std::cout << "Mobile IO connected." << std::endl; diff --git a/kits/arm/ex_gravity_compensation_toggle.cpp b/kits/arm/ex_gravity_compensation_toggle.cpp index f73d6ba3..cc2656c1 100644 --- a/kits/arm/ex_gravity_compensation_toggle.cpp +++ b/kits/arm/ex_gravity_compensation_toggle.cpp @@ -79,14 +79,14 @@ int main(int argc, char* argv[]) // 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, example_config_file); + 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, example_config_file); + mobile_io = createMobileIOFromConfig(*example_config); } std::cout << "Mobile IO connected." << std::endl; diff --git a/kits/arm/ex_impedance_control_cartesian.cpp b/kits/arm/ex_impedance_control_cartesian.cpp index 8f8359e6..210674f4 100644 --- a/kits/arm/ex_impedance_control_cartesian.cpp +++ b/kits/arm/ex_impedance_control_cartesian.cpp @@ -87,14 +87,14 @@ int main(int argc, char* argv[]) // 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, example_config_file); + 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, example_config_file); + mobile_io = createMobileIOFromConfig(*example_config); } std::cout << "Mobile IO connected." << std::endl; diff --git a/kits/arm/ex_impedance_control_damping.cpp b/kits/arm/ex_impedance_control_damping.cpp index 8c839c13..4a026c77 100644 --- a/kits/arm/ex_impedance_control_damping.cpp +++ b/kits/arm/ex_impedance_control_damping.cpp @@ -101,14 +101,14 @@ int main(int argc, char* argv[]) // 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, example_config_file); + 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, example_config_file); + mobile_io = createMobileIOFromConfig(*example_config); } std::cout << "Mobile IO connected." << std::endl; diff --git a/kits/arm/ex_impedance_control_fixed.cpp b/kits/arm/ex_impedance_control_fixed.cpp index 01b51093..08b9cd0d 100644 --- a/kits/arm/ex_impedance_control_fixed.cpp +++ b/kits/arm/ex_impedance_control_fixed.cpp @@ -87,14 +87,14 @@ int main(int argc, char* argv[]) // 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, example_config_file); + 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, example_config_file); + mobile_io = createMobileIOFromConfig(*example_config); } std::cout << "Mobile IO connected." << std::endl; diff --git a/kits/arm/ex_impedance_control_floor.cpp b/kits/arm/ex_impedance_control_floor.cpp index 6076b09c..431eca87 100644 --- a/kits/arm/ex_impedance_control_floor.cpp +++ b/kits/arm/ex_impedance_control_floor.cpp @@ -87,14 +87,14 @@ int main(int argc, char* argv[]) // 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, example_config_file); + 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, example_config_file); + mobile_io = createMobileIOFromConfig(*example_config); } std::cout << "Mobile IO connected." << std::endl; diff --git a/kits/arm/ex_impedance_control_gimbal.cpp b/kits/arm/ex_impedance_control_gimbal.cpp index 77acd80f..1b086704 100644 --- a/kits/arm/ex_impedance_control_gimbal.cpp +++ b/kits/arm/ex_impedance_control_gimbal.cpp @@ -87,14 +87,14 @@ int main(int argc, char* argv[]) // 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, example_config_file); + 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, example_config_file); + mobile_io = createMobileIOFromConfig(*example_config); } std::cout << "Mobile IO connected." << std::endl; diff --git a/kits/arm/ex_mobile_io_control.cpp b/kits/arm/ex_mobile_io_control.cpp index e7be7d43..82ea83d3 100644 --- a/kits/arm/ex_mobile_io_control.cpp +++ b/kits/arm/ex_mobile_io_control.cpp @@ -67,14 +67,14 @@ int main(int argc, char* argv[]) // 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, example_config_file); + 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, example_config_file); + mobile_io = createMobileIOFromConfig(*example_config); } std::cout << "Mobile IO connected." << std::endl; diff --git a/kits/arm/ex_teach_repeat.cpp b/kits/arm/ex_teach_repeat.cpp index 0efccdf0..ae330179 100644 --- a/kits/arm/ex_teach_repeat.cpp +++ b/kits/arm/ex_teach_repeat.cpp @@ -114,14 +114,14 @@ int main(int argc, char* argv[]) // 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, example_config_file); + 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, example_config_file); + mobile_io = createMobileIOFromConfig(*example_config); } std::cout << "Mobile IO connected." << std::endl; diff --git a/kits/arm/hebi_util.cpp b/kits/arm/hebi_util.cpp index b9bc977f..b3743e9a 100644 --- a/kits/arm/hebi_util.cpp +++ b/kits/arm/hebi_util.cpp @@ -12,12 +12,11 @@ #include "hebi_util.hpp" -std::unique_ptr createMobileIOFromConfig(const hebi::RobotConfig& config, const std::string& config_file_path) { +std::unique_ptr createMobileIOFromConfig(const hebi::RobotConfig& config) { std::map mobile_io_dict; std::vector errors; - hebi::util::file::File cfg_file(config_file_path); - auto parent_dir_absolute = cfg_file.getParentDirectory().getAbsolutePath(); + auto parent_dir_absolute = config.getLocation(); // Lambda function to check file paths auto check_file = [&errors, &parent_dir_absolute](const std::string& type, const std::string& relative_filename) { diff --git a/kits/arm/hebi_util.hpp b/kits/arm/hebi_util.hpp index a931e46a..34d93fb4 100644 --- a/kits/arm/hebi_util.hpp +++ b/kits/arm/hebi_util.hpp @@ -21,7 +21,7 @@ // 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, const std::string& config_file_path); +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); From ba70958f4fd28c3c8e9d069c1172558a4e1b85cb Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Wed, 11 Sep 2024 11:35:10 -0400 Subject: [PATCH 27/31] examples repo compatible with newest API --- kits/arm/config/ex_wine_glass.cfg.yaml | 48 +++++++++++++++++++++ kits/arm/ex_impedance_control_cartesian.cpp | 20 +++++---- kits/arm/ex_impedance_control_damping.cpp | 20 +++++---- kits/arm/ex_impedance_control_fixed.cpp | 31 +++++++++---- kits/arm/ex_impedance_control_floor.cpp | 20 +++++---- kits/arm/ex_impedance_control_gimbal.cpp | 20 +++++---- kits/arm/hebi_util.cpp | 26 +++++------ projects/cmake/DownloadHebiCpp.cmake | 4 +- 8 files changed, 129 insertions(+), 60 deletions(-) create mode 100644 kits/arm/config/ex_wine_glass.cfg.yaml diff --git a/kits/arm/config/ex_wine_glass.cfg.yaml b/kits/arm/config/ex_wine_glass.cfg.yaml new file mode 100644 index 00000000..ab14b975 --- /dev/null +++ b/kits/arm/config/ex_wine_glass.cfg.yaml @@ -0,0 +1,48 @@ +# 6-DoF Arm +version: 1.0 +families: ["Arm2"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "hrdf/A-2085-06.hrdf" + +gains: + default: "gains/A-2085-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: true + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + + # An impedance controller adds a virtual spring to the + # end-effector and can improve tracking. It can be enabled + # by setting 'enabled' to true. The gains are in the form of + # [x, y, z, rx, ry, rz]. Setting gains of zero deactivates + # the corresponding degree of translation or rotation. + # These gains correspond to a rotational spring emulating a gimbal + - type: ImpedanceController + name: impedanceController + enabled: true + ramp_time: 0 + gains_in_end_effector_frame: true + kp: [0, 0, 0, 8, 8, 1] # (N/m) or (Nm/rad) + kd: [0, 0, 0, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) + ki: [0, 0, 0, 0.5, 0.5, 0.5] + i_clamp: [0, 0, 0, 1, 1, 1] # max value + +user_data: + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "layouts/ex_impedance_control_gimbal.json" + + gripper_family: "Arm2" + gripper_name: "gripperSpool" + gripper_gains: "gains/gripper_spool_gains.xml" + gripper_close_effort: -5.0 # (Nm) Effort applied to close the gripper. More negative effort will pinch the gripper harder. + gripper_open_effort: 1.0 # (Nm) Effort applied to open the gripper. More positive effort will NOT make the gripper harder to close. \ No newline at end of file diff --git a/kits/arm/ex_impedance_control_cartesian.cpp b/kits/arm/ex_impedance_control_cartesian.cpp index 210674f4..a8940e09 100644 --- a/kits/arm/ex_impedance_control_cartesian.cpp +++ b/kits/arm/ex_impedance_control_cartesian.cpp @@ -71,15 +71,21 @@ int main(int argc, char* argv[]) // Ideally, in the impedance control demos, positions and velocities must not be commanded - // Initialize variables used to clear the commanded position and velocity in every cycle // Get the pending command pointer hebi::GroupCommand& command = arm->pendingCommand(); - // Create nan vectors for positions and velocities + // Clear gains for all position and velocity commands auto num_joints = arm->robotModel().getDoFCount(); - Eigen::VectorXd pos_nan(num_joints), vel_nan(num_joints); - pos_nan.fill(std::numeric_limits::quiet_NaN()); - vel_nan.fill(std::numeric_limits::quiet_NaN()); + 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 ////// @@ -161,10 +167,6 @@ int main(int argc, char* argv[]) arm->cancelGoal(); } - // Clear all position and velocity commands - command.setPosition(pos_nan); - command.setVelocity(vel_nan); - // Send latest commands to the arm arm->send(); } diff --git a/kits/arm/ex_impedance_control_damping.cpp b/kits/arm/ex_impedance_control_damping.cpp index 4a026c77..6acdf0be 100644 --- a/kits/arm/ex_impedance_control_damping.cpp +++ b/kits/arm/ex_impedance_control_damping.cpp @@ -71,15 +71,21 @@ int main(int argc, char* argv[]) // Ideally, in the impedance control demos, positions and velocities must not be commanded - // Initialize variables used to clear the commanded position and velocity in every cycle // Get the pending command pointer hebi::GroupCommand& command = arm->pendingCommand(); - // Create nan vectors for positions and velocities + // Clear gains for all position and velocity commands auto num_joints = arm->robotModel().getDoFCount(); - Eigen::VectorXd pos_nan(num_joints), vel_nan(num_joints); - pos_nan.fill(std::numeric_limits::quiet_NaN()); - vel_nan.fill(std::numeric_limits::quiet_NaN()); + 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 @@ -219,10 +225,6 @@ int main(int argc, char* argv[]) prevmode = mode; } - // Clear all position and velocity commands - command.setPosition(pos_nan); - command.setVelocity(vel_nan); - // Send latest commands to the arm arm->send(); } diff --git a/kits/arm/ex_impedance_control_fixed.cpp b/kits/arm/ex_impedance_control_fixed.cpp index 08b9cd0d..0408b4ee 100644 --- a/kits/arm/ex_impedance_control_fixed.cpp +++ b/kits/arm/ex_impedance_control_fixed.cpp @@ -71,15 +71,21 @@ int main(int argc, char* argv[]) // Ideally, in the impedance control demos, positions and velocities must not be commanded - // Initialize variables used to clear the commanded position and velocity in every cycle // Get the pending command pointer hebi::GroupCommand& command = arm->pendingCommand(); - // Create nan vectors for positions and velocities + // Clear gains for all position and velocity commands auto num_joints = arm->robotModel().getDoFCount(); - Eigen::VectorXd pos_nan(num_joints), vel_nan(num_joints); - pos_nan.fill(std::numeric_limits::quiet_NaN()); - vel_nan.fill(std::numeric_limits::quiet_NaN()); + 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 ////// @@ -122,6 +128,17 @@ int main(int argc, char* argv[]) // 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 /// ////////////////////////// @@ -161,10 +178,6 @@ int main(int argc, char* argv[]) arm->cancelGoal(); } - // Clear all position and velocity commands - command.setPosition(pos_nan); - command.setVelocity(vel_nan); - // Send latest commands to the arm arm->send(); } diff --git a/kits/arm/ex_impedance_control_floor.cpp b/kits/arm/ex_impedance_control_floor.cpp index 431eca87..ef567ce8 100644 --- a/kits/arm/ex_impedance_control_floor.cpp +++ b/kits/arm/ex_impedance_control_floor.cpp @@ -71,15 +71,21 @@ int main(int argc, char* argv[]) // Ideally, in the impedance control demos, positions and velocities must not be commanded - // Initialize variables used to clear the commanded position and velocity in every cycle // Get the pending command pointer hebi::GroupCommand& command = arm->pendingCommand(); - // Create nan vectors for positions and velocities + // Clear gains for all position and velocity commands auto num_joints = arm->robotModel().getDoFCount(); - Eigen::VectorXd pos_nan(num_joints), vel_nan(num_joints); - pos_nan.fill(std::numeric_limits::quiet_NaN()); - vel_nan.fill(std::numeric_limits::quiet_NaN()); + 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 ////// @@ -217,10 +223,6 @@ int main(int argc, char* argv[]) } } - // Clear all position and velocity commands - command.setPosition(pos_nan); - command.setVelocity(vel_nan); - // Send latest commands to the arm arm->send(); } diff --git a/kits/arm/ex_impedance_control_gimbal.cpp b/kits/arm/ex_impedance_control_gimbal.cpp index 1b086704..2a304995 100644 --- a/kits/arm/ex_impedance_control_gimbal.cpp +++ b/kits/arm/ex_impedance_control_gimbal.cpp @@ -71,15 +71,21 @@ int main(int argc, char* argv[]) // Ideally, in the impedance control demos, positions and velocities must not be commanded - // Initialize variables used to clear the commanded position and velocity in every cycle // Get the pending command pointer hebi::GroupCommand& command = arm->pendingCommand(); - // Create nan vectors for positions and velocities + // Clear gains for all position and velocity commands auto num_joints = arm->robotModel().getDoFCount(); - Eigen::VectorXd pos_nan(num_joints), vel_nan(num_joints); - pos_nan.fill(std::numeric_limits::quiet_NaN()); - vel_nan.fill(std::numeric_limits::quiet_NaN()); + 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 ////// @@ -161,10 +167,6 @@ int main(int argc, char* argv[]) arm->cancelGoal(); } - // Clear all position and velocity commands - command.setPosition(pos_nan); - command.setVelocity(vel_nan); - // Send latest commands to the arm arm->send(); } diff --git a/kits/arm/hebi_util.cpp b/kits/arm/hebi_util.cpp index b3743e9a..3145db5f 100644 --- a/kits/arm/hebi_util.cpp +++ b/kits/arm/hebi_util.cpp @@ -16,7 +16,7 @@ std::unique_ptr createMobileIOFromConfig(const hebi::Robot std::map mobile_io_dict; std::vector errors; - auto parent_dir_absolute = config.getLocation(); + 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) { @@ -43,18 +43,19 @@ std::unique_ptr createMobileIOFromConfig(const hebi::Robot // 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."); + 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"); - // 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."); + // 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\"."); @@ -62,7 +63,6 @@ std::unique_ptr createMobileIOFromConfig(const hebi::Robot // If there are errors, print them and throw an exception if (!errors.empty()) { - for (const auto& error : errors) { std::cerr << "Error: " << error << std::endl; } @@ -74,7 +74,7 @@ std::unique_ptr createMobileIOFromConfig(const hebi::Robot if(mobile_io != nullptr) { - mobile_io->sendLayoutFile(mobile_io_dict["layout"]); + mobile_io->sendLayout(mobile_io_dict["layout"]); } return mobile_io; } \ No newline at end of file diff --git a/projects/cmake/DownloadHebiCpp.cmake b/projects/cmake/DownloadHebiCpp.cmake index 5f1ee048..54451ebe 100644 --- a/projects/cmake/DownloadHebiCpp.cmake +++ b/projects/cmake/DownloadHebiCpp.cmake @@ -1,9 +1,9 @@ # Used to download the C++ API - this should not be used directly. cmake_minimum_required(VERSION 3.0) -set(HEBI_CPP_VERSION "3.9.0") +set(HEBI_CPP_VERSION "3.10.0") set(HEBI_CPP_FILE_NAME "hebi-cpp-${HEBI_CPP_VERSION}.tar.gz") -set(HEBI_CPP_LIB_SHA256 "9ad5752448117fb4f5e0f0822c0c596205e4eb33a97aa94e47e81d4d9bc77695") +set(HEBI_CPP_LIB_SHA256 "df04b75f75a407164cb7edf6d5d2a162ff52998d7c493f8ed346543f648ded38") set(HEBI_CPP_URL "https://files.hebi.us/download/cpp/${HEBI_CPP_FILE_NAME}") # If the CMakeLists.txt is not found, then redownload the C++ API From f843fbd8c8591383c131adaf08cf8d5122681da0 Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Wed, 11 Sep 2024 16:33:11 -0400 Subject: [PATCH 28/31] removed unnecessary file --- kits/arm/config/ex_wine_glass.cfg.yaml | 48 -------------------------- 1 file changed, 48 deletions(-) delete mode 100644 kits/arm/config/ex_wine_glass.cfg.yaml diff --git a/kits/arm/config/ex_wine_glass.cfg.yaml b/kits/arm/config/ex_wine_glass.cfg.yaml deleted file mode 100644 index ab14b975..00000000 --- a/kits/arm/config/ex_wine_glass.cfg.yaml +++ /dev/null @@ -1,48 +0,0 @@ -# 6-DoF Arm -version: 1.0 -families: ["Arm2"] -names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] -hrdf: "hrdf/A-2085-06.hrdf" - -gains: - default: "gains/A-2085-06.xml" - -plugins: - - - type: GravityCompensationEffort - name: gravComp - enabled: true - ramp_time: 0 - - - type: DynamicsCompensationEffort - name: dynamicsComp - enabled: true - ramp_time: 0 - - # An impedance controller adds a virtual spring to the - # end-effector and can improve tracking. It can be enabled - # by setting 'enabled' to true. The gains are in the form of - # [x, y, z, rx, ry, rz]. Setting gains of zero deactivates - # the corresponding degree of translation or rotation. - # These gains correspond to a rotational spring emulating a gimbal - - type: ImpedanceController - name: impedanceController - enabled: true - ramp_time: 0 - gains_in_end_effector_frame: true - kp: [0, 0, 0, 8, 8, 1] # (N/m) or (Nm/rad) - kd: [0, 0, 0, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) - ki: [0, 0, 0, 0.5, 0.5, 0.5] - i_clamp: [0, 0, 0, 1, 1, 1] # max value - -user_data: - - mobile_io_family: "Arm" - mobile_io_name: "mobileIO" - mobile_io_layout: "layouts/ex_impedance_control_gimbal.json" - - gripper_family: "Arm2" - gripper_name: "gripperSpool" - gripper_gains: "gains/gripper_spool_gains.xml" - gripper_close_effort: -5.0 # (Nm) Effort applied to close the gripper. More negative effort will pinch the gripper harder. - gripper_open_effort: 1.0 # (Nm) Effort applied to open the gripper. More positive effort will NOT make the gripper harder to close. \ No newline at end of file From e3c6c74eb386f2dace2b5d3a79f0df27f2961dd4 Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Wed, 11 Sep 2024 16:35:21 -0400 Subject: [PATCH 29/31] reflects changes that will be made into config directory --- kits/arm/ex_AR_kit.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kits/arm/ex_AR_kit.cpp b/kits/arm/ex_AR_kit.cpp index 7a78a232..2a66ee99 100644 --- a/kits/arm/ex_AR_kit.cpp +++ b/kits/arm/ex_AR_kit.cpp @@ -102,7 +102,7 @@ int main(int argc, char* argv[]) // Load user data from config Eigen::VectorXd home_position(arm -> robotModel().getDoFCount()); 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("soft_start_time"); + 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 From fe4a2305357e6eb1f29ab2af6e21b14cc112c22e Mon Sep 17 00:00:00 2001 From: Aditya Nair Date: Wed, 11 Sep 2024 16:58:20 -0400 Subject: [PATCH 30/31] Introduced latency in AR kit, python also has this now --- kits/arm/config/ex_AR_kit.cfg.yaml | 5 ++++- kits/arm/config/ex_AR_kit_w_gripper.cfg.yaml | 5 ++++- kits/arm/ex_AR_kit.cpp | 4 ++-- 3 files changed, 10 insertions(+), 4 deletions(-) diff --git a/kits/arm/config/ex_AR_kit.cfg.yaml b/kits/arm/config/ex_AR_kit.cfg.yaml index faa723da..a9a7ab37 100644 --- a/kits/arm/config/ex_AR_kit.cfg.yaml +++ b/kits/arm/config/ex_AR_kit.cfg.yaml @@ -25,7 +25,10 @@ user_data: home_position: [0, 1.0471975511965976, 2.0943951023931953, 2.6179938779914944, -1.5707963267948966, 0] # radians # Time taken for a steady motion to the home position - soft_start_time: 4 # seconds + homing_duration: 4 # seconds + + # Latency in tracking the AR pose. Increase this to avoid jerks + latency: 0.3 # seconds # Displacements of the phone are scaled by this value to give displacement of the end-effector xyz_scale: 0.75 diff --git a/kits/arm/config/ex_AR_kit_w_gripper.cfg.yaml b/kits/arm/config/ex_AR_kit_w_gripper.cfg.yaml index 3b036b39..fd065071 100644 --- a/kits/arm/config/ex_AR_kit_w_gripper.cfg.yaml +++ b/kits/arm/config/ex_AR_kit_w_gripper.cfg.yaml @@ -25,7 +25,10 @@ user_data: home_position: [0, 1.0471975511965976, 1.5707963267948966, 2.0943951023931953, -1.5707963267948966, 0] # radians # Time taken for a steady motion to the home position - soft_start_time: 4 # seconds + homing_duration: 4 # seconds + + # Latency in tracking the AR pose. Increase this to avoid jerks + latency: 0.3 # seconds # Displacements of the phone are scaled by this value to give displacement of the end-effector xyz_scale: 0.75 diff --git a/kits/arm/ex_AR_kit.cpp b/kits/arm/ex_AR_kit.cpp index 2a66ee99..da3cb4d6 100644 --- a/kits/arm/ex_AR_kit.cpp +++ b/kits/arm/ex_AR_kit.cpp @@ -146,7 +146,7 @@ int main(int argc, char* argv[]) // Button B1 - Return to home position if (mobile_io->getButtonDiff(1) == hebi::util::MobileIO::ButtonState::ToOn) { ar_mode = false; - arm -> setGoal(arm::Goal::createFromPosition(4, home_position)); + arm -> setGoal(arm::Goal::createFromPosition(soft_start_time, home_position)); } // Button B3 - Start AR Control @@ -193,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 From e0782fc029430c42c47e163a0529db3d86595ab7 Mon Sep 17 00:00:00 2001 From: Matthew Tesch Date: Thu, 17 Oct 2024 12:30:05 -0400 Subject: [PATCH 31/31] Updated the C++ library reference --- projects/cmake/DownloadHebiCpp.cmake | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/projects/cmake/DownloadHebiCpp.cmake b/projects/cmake/DownloadHebiCpp.cmake index 54451ebe..c426cfae 100644 --- a/projects/cmake/DownloadHebiCpp.cmake +++ b/projects/cmake/DownloadHebiCpp.cmake @@ -1,9 +1,9 @@ # Used to download the C++ API - this should not be used directly. cmake_minimum_required(VERSION 3.0) -set(HEBI_CPP_VERSION "3.10.0") +set(HEBI_CPP_VERSION "3.11.1") set(HEBI_CPP_FILE_NAME "hebi-cpp-${HEBI_CPP_VERSION}.tar.gz") -set(HEBI_CPP_LIB_SHA256 "df04b75f75a407164cb7edf6d5d2a162ff52998d7c493f8ed346543f648ded38") +set(HEBI_CPP_LIB_SHA256 "26454863b4f4d9dd7902c8d7530fc70f766353b0aed97c33781f897874c3d7dd") set(HEBI_CPP_URL "https://files.hebi.us/download/cpp/${HEBI_CPP_FILE_NAME}") # If the CMakeLists.txt is not found, then redownload the C++ API