Skip to content

Remaining changes from #86 #101

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
iamtesch wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

Remaining changes from #86 #101

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
81 changes: 42 additions & 39 deletions kits/arm/ex_AR_kit.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
#include "arm/arm.hpp"
#include "util/mobile_io.hpp"
#include <chrono>
#include <iostream>
#include "hebi_util.hpp"

using namespace hebi;
using namespace experimental; // For all things mobileIO
Expand Down Expand Up @@ -48,44 +48,48 @@ int main(int argc, char* argv[])
return -1;
}

// For this demo, we need the arm and mobile_io
std::unique_ptr<arm::Arm> arm;
std::unique_ptr<hebi::util::MobileIO> mobile_io;

//////////////////////////
///// Arm Setup //////////
//////////////////////////

// Create the arm object from the configuration, and keep retrying if arm not found
auto arm = arm::Arm::create(*example_config);
// Create the arm object from the configuration
arm = arm::Arm::create(*example_config);

// Keep retrying if arm not found
while (!arm) {
std::cerr << "Failed to create arm, retrying..." << std::endl;

// Retry
arm = arm::Arm::create(*example_config);
}
std::cout << "Arm connected." << std::endl;


//////////////////////////
//// MobileIO Setup //////
//////////////////////////

// Create the MobileIO object
std::unique_ptr<util::MobileIO> mobile_io = util::MobileIO::create("Arm", "mobileIO");
if (!mobile_io)
{
std::cout << "couldn't find mobile IO device!\n";
return 1;
}
// Create the mobile_io object from the configuration
std::cout << "Waiting for Mobile IO device to come online..." << std::endl;
mobile_io = createMobileIOFromConfig(*example_config);

// Clear any garbage on screen
mobile_io->resetUI();
// Keep retrying if Mobile IO not found
while (mobile_io == nullptr) {
std::cout << "Couldn't find Mobile IO. Check name, family, or device status..." << std::endl;

// Setup instructions for display
std::string instructions;
instructions = ("B1 - Home Position\nB3 - AR Control Mode\n"
"B6 - Grav Comp Mode\nB8 - Quit\n");
// Retry
mobile_io = createMobileIOFromConfig(*example_config);
}
std::cout << "Mobile IO connected." << std::endl;

// Display instructions on screen
mobile_io->appendText(instructions);
// Clear any garbage on screen
mobile_io->clearText();

// Setup state variable for mobile device
auto last_mobile_state = mobile_io->update();
// Refresh mobile_io
auto last_state = mobile_io->update();

//////////////////////////
//// Main Control Loop ///
Expand All @@ -95,13 +99,15 @@ int main(int argc, char* argv[])
bool softstart = true;
bool ar_mode = false;

// Make sure we softstart the arm first.
// Load user data from config
Eigen::VectorXd home_position(arm -> robotModel().getDoFCount());
home_position << 0, M_PI/3, 2*M_PI/3, 5*M_PI/6, -M_PI/2, 0; // Adjust depending on your DoFs
home_position = Eigen::Map<const Eigen::VectorXd>(example_config->getUserData().getFloatList("home_position").data(), example_config->getUserData().getFloatList("home_position").size());
double soft_start_time = example_config->getUserData().getFloat("homing_duration");
double xyz_scale = example_config->getUserData().getFloat("xyz_scale");

// Command the softstart to home position
arm -> update();
arm -> setGoal(arm::Goal::createFromPosition(4, home_position)); // take 4 seconds
arm -> setGoal(arm::Goal::createFromPosition(soft_start_time, home_position)); // take 4 seconds
arm -> send();

// Get the cartesian position and rotation matrix @ home position
Expand Down Expand Up @@ -130,39 +136,36 @@ int main(int argc, char* argv[])

// Stay in softstart, don't do any other behavior
continue;
}
}

// Get latest mobile_state
auto updated_mobile_io = mobile_io->update(0);

if (!updated_mobile_io)
std::cout << "Failed to get feedback from mobile I/O; check connection!\n";
else
if (updated_mobile_io)
{
// Button B1 - Return to home position
if (mobile_io->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) {
ar_mode = false;
arm -> setGoal(arm::Goal::createFromPosition(4, home_position));
if (mobile_io->getButtonDiff(1) == hebi::util::MobileIO::ButtonState::ToOn) {
ar_mode = false;
arm -> setGoal(arm::Goal::createFromPosition(soft_start_time, home_position));
}

// Button B3 - Start AR Control
if (mobile_io->getButtonDiff(3) == util::MobileIO::ButtonState::ToOn) {
if (mobile_io->getButtonDiff(3) == hebi::util::MobileIO::ButtonState::ToOn) {
xyz_phone_init << mobile_io -> getLastFeedback().mobile().arPosition().get().getX(),
mobile_io -> getLastFeedback().mobile().arPosition().get().getY(),
mobile_io -> getLastFeedback().mobile().arPosition().get().getZ();
std::cout << xyz_phone_init << std::endl;
rot_phone_init = makeRotationMatrix(mobile_io -> getLastFeedback().mobile().arOrientation().get());
ar_mode = true;
}

// Button B6 - Grav Comp Mode
if (mobile_io->getButtonDiff(6) == util::MobileIO::ButtonState::ToOn) {
if (mobile_io->getButtonDiff(6) == hebi::util::MobileIO::ButtonState::ToOn) {
arm -> cancelGoal();
ar_mode = false;
}

// Button B8 - End Demo
if (mobile_io->getButtonDiff(8) == util::MobileIO::ButtonState::ToOn) {
if (mobile_io->getButtonDiff(8) == hebi::util::MobileIO::ButtonState::ToOn) {
// Clear MobileIO text
mobile_io->resetUI();
return 1;
Expand All @@ -178,9 +181,9 @@ int main(int argc, char* argv[])
auto rot_phone = makeRotationMatrix(mobile_io->getLastFeedback().mobile().arOrientation().get());

// Calculate new targets
Eigen::Vector3d xyz_scale;
xyz_scale << 1, 1, 2;
Eigen::Vector3d xyz_target = xyz_home + (0.75 * xyz_scale.array() *
Eigen::Vector3d xyz_scale_vec;
xyz_scale_vec << 1, 1, 2;
Eigen::Vector3d xyz_target = xyz_home + (xyz_scale * xyz_scale_vec.array() *
(rot_phone_init.transpose() * (xyz_phone - xyz_phone_init)).array()).matrix();
Eigen::Matrix3d rot_target = rot_phone_init.transpose() * rot_phone * rot_home;

Expand All @@ -190,7 +193,7 @@ int main(int argc, char* argv[])
rot_target);

// Create and send new goal to the arm
arm -> setGoal(arm::Goal::createFromPosition(target_joints));
arm -> setGoal(arm::Goal::createFromPosition(example_config->getUserData().getFloat("latency"), target_joints));
}

// Send latest commands to the arm
Expand Down
146 changes: 146 additions & 0 deletions kits/arm/ex_gravity_compensation_toggle.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,146 @@
/**
* This file is a barebones skeleton of how to setup an arm for use.
* It demonstrates gravity compensation behavior by commanding torques
* equal to the force from gravity on the links and joints of an arm.
* Note that this only approximately balances out gravity, as imperfections in
* the torque sensing and modeled system can lead to "drift". Also, the
* particular choice of PID control gains can affect the performance of this
* demo.
*/

#include "group_command.hpp"
#include "group_feedback.hpp"
#include "robot_model.hpp"
#include "arm/arm.hpp"
#include "util/mobile_io.hpp"
#include <chrono>
#include "hebi_util.hpp"

using namespace hebi;
using namespace experimental;

int main(int argc, char* argv[])
{
//////////////////////////
///// Config Setup ///////
//////////////////////////

// Config file path
const std::string example_config_file = "config/ex_gravity_compensation_toggle.cfg.yaml";
std::vector<std::string> errors;

// Load the config
const auto example_config = RobotConfig::loadConfig(example_config_file, errors);
for (const auto& error : errors) {
std::cerr << error << std::endl;
}
if (!example_config) {
std::cerr << "Failed to load configuration from: " << example_config_file << std::endl;
return -1;
}

// For this demo, we need the arm and mobile_io
std::unique_ptr<arm::Arm> arm;
std::unique_ptr<hebi::util::MobileIO> mobile_io;

//////////////////////////
///// Arm Setup //////////
//////////////////////////

// Create the arm object from the configuration
arm = arm::Arm::create(*example_config);

// Keep retrying if arm not found
while (!arm) {
std::cerr << "Failed to create arm, retrying..." << std::endl;

// Retry
arm = arm::Arm::create(*example_config);
}
std::cout << "Arm connected." << std::endl;

// Retrieve the gravcomp plugin from the arm

// Get a weak_ptr from the arm API, lock it as a shared_ptr, and then downcast it from a general plugin pointer to a specific plugin pointer
auto plugin_shared_ptr = arm->getPluginByType<arm::plugin::GravityCompensationEffort>().lock();
if (!plugin_shared_ptr) {
std::cerr << "Failed to lock plugin shared_ptr. The plugin may have been destroyed." << std::endl;
return -1;
}
auto gravcomp_plugin_ptr = std::dynamic_pointer_cast<arm::plugin::GravityCompensationEffort>(plugin_shared_ptr);
if (!gravcomp_plugin_ptr) {
std::cerr << "Failed to cast plugin to GravityCompensationEffort." << std::endl;
return -1;
}

//////////////////////////
//// MobileIO Setup //////
//////////////////////////

// Create the mobile_io object from the configuration
std::cout << "Waiting for Mobile IO device to come online..." << std::endl;
mobile_io = createMobileIOFromConfig(*example_config);

// Keep retrying if Mobile IO not found
while (mobile_io == nullptr) {
std::cout << "Couldn't find Mobile IO. Check name, family, or device status..." << std::endl;

// Retry
mobile_io = createMobileIOFromConfig(*example_config);
}
std::cout << "Mobile IO connected." << std::endl;

// Clear any garbage on screen
mobile_io->clearText();

// Refresh mobile_io
auto last_state = mobile_io->update();

std::cout << "Commanded gravity-compensated zero force to the arm.\n"
<< " 🌍 (B2) - Toggles the gravity compensation on/off:\n"
<< " ON - Apply controller \n"
<< " OFF - Disable controller\n"
<< " ❌ (B1) - Exits the demo.\n";

//////////////////////////
//// Main Control Loop ///
//////////////////////////

while(arm->update())
{
auto updated_mobile_io = mobile_io->update(0);

if (updated_mobile_io)
{
/////////////////
// Button Presses
/////////////////

// Buttton B1 - End demo
if (mobile_io->getButtonDiff(1) == hebi::util::MobileIO::ButtonState::ToOn) {
// Clear MobileIO text
mobile_io->resetUI();
return 1;
}

// Button B2 - Set and unset gravcomp mode when button is pressed and released, respectively
if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOn) {

// Enable gravcomp
gravcomp_plugin_ptr->setEnabled(true);
}
else if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOff){

// Disable gravcomp
gravcomp_plugin_ptr->setEnabled(false);
}
}
// Send latest commands to the arm
arm->send();
}

return 0;
}



Loading