Skip to content
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.

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

Cwbollinger/mobile api wip #69

Open
wants to merge 16 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
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
103 changes: 103 additions & 0 deletions kits/base/omni_mobile_io_control.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,103 @@

#include "util/mobile_io.hpp"
#include "util/omni_base.hpp"

using namespace hebi;
using namespace experimental;
using namespace mobile;


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

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

std::cout << "Creating MobileIO" << std::endl;

std::string family = "Rosie";

// Create the MobileIO object
std::unique_ptr<MobileIO> mobile = MobileIO::create(family, "mobileIO");
while(!mobile) {
std::cout << "Couldn't find mobileIO, trying again..." << std::endl;
mobile = MobileIO::create(family, "mobileIO");
}

std::string instructions("A1/A2 - Move Base\n"
"A7 - Turn Base\n"
"B8 - Quit\n");

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

// Display instructions on screen
mobile -> sendText(instructions);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think these have bool return values; I'm not sure about the balance between making the example more complex vs. more robust by handling them


//////////////////////////
//// OmniBase Setup //////
//////////////////////////

// Set module names for mobile base
OmniBase::Params p;
p.families_ = {family};
p.names_ = {"W1", "W2", "W3"};

std::cout << "Creating Omni Base" << std::endl;

auto base = OmniBase::create(p);

if (!base) {
std::cout << "Failed to create base, exiting!" << std::endl;
exit(EXIT_FAILURE);
}

auto last_state = mobile->getState();

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

while(base->update())
{
auto state = mobile->getState();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This reminds me...I think we still need to fix the MobileIO::getState call to not block in C++, or accept a timeout. I'll track this as a separate issue.

MobileIODiff diff(last_state, state);

/////////////////
// Input Handling
/////////////////

auto dy = -1 * state.getAxis(1);
auto dx = state.getAxis(2);
auto dtheta = -1 * state.getAxis(7);

// Button B8 - End Demo
if (diff.get(8) == MobileIODiff::ButtonState::ToOn)
{
// Clear MobileIO text
mobile->clearText();
return 1;
}

/////////////////
// Update & send
/////////////////

// create Vel Goal for 0.5 second, 0.25 second ramp down
auto goal = CartesianGoal::createFromVelocity(Vel{dx, dy, dtheta}, 0.1, 0.5, 0.25);

// send goal to base
base->setGoal(goal);

// Update to the new last_state for mobile device
last_state = state;

// Send latest commands to the base
base->send();
}

// Clear MobileIO text
mobile -> clearText();

return 0;
};
75 changes: 75 additions & 0 deletions kits/base/omni_waypoints.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,75 @@

#include "util/omni_base.hpp"

using namespace hebi;
using namespace experimental;
using namespace mobile;


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

//////////////////////////
//// OmniBase Setup //////
//////////////////////////

// Set module names for mobile base
OmniBase::Params p;
p.families_ = {"Rosie"};
p.names_ = {"W1", "W2", "W3"};

std::cout << "Creating Omni Base" << std::endl;

auto base = OmniBase::create(p);

if (!base) {
std::cout << "Failed to create base, exiting!" << std::endl;
exit(EXIT_FAILURE);
}

auto currentPose = base->getOdometry();

Waypoint wp1;
Waypoint wp2;
Waypoint wp3;
Waypoint wp4;

wp1.t = 1.0;
wp1.pos = currentPose;

wp2 = wp1;
wp2.t += 1.0;
wp2.pos.x += 0.5;

wp3 = wp2;
wp3.t += 1.0;
wp3.pos.y += 0.5;

wp4 = wp3;
wp4.t += 1.0;
wp4.pos.x -= 0.5;

auto goal = CartesianGoal::createFromWaypoints({wp1, wp2, wp3, wp4});

// send goal to base
base->setGoal(goal);

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

std::cout << "Executing Goal" << std::endl;
return -1;

while (base->update())
{
// Send updated command to the base
base->send();

// if the trajectory has been completed, start another square
if (base->goalComplete()) {
base->setGoal(goal);
}
}

return 0;
};
33 changes: 32 additions & 1 deletion projects/cmake/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -315,13 +315,44 @@ endforeach (EXAMPLE ${KIT_SOURCES})
file(COPY ${ROOT_DIR}/kits/arm/hrdf DESTINATION ${ROOT_DIR}/build/kits/arm)
file(COPY ${ROOT_DIR}/kits/arm/gains DESTINATION ${ROOT_DIR}/build/kits/arm)

SET(BASE_SOURCES

${ROOT_DIR}/kits/base/omni_waypoints.cpp
${ROOT_DIR}/kits/base/omni_mobile_io_control.cpp)

set(CMAKE_RUNTIME_OUTPUT_DIRECTORY kits/base)
foreach (EXAMPLE ${BASE_SOURCES})

# The target for the individual example is based on the filename
get_filename_component(EX_NAME ${EXAMPLE} NAME_WE)

if(WIN32)
add_executable(${EX_NAME} ${EXAMPLE} $<TARGET_OBJECTS:_hebic++-obj>)
else()
add_executable(${EX_NAME} ${EXAMPLE})
endif()
add_dependencies(examples ${EX_NAME})
target_include_directories(${EX_NAME} PRIVATE ${ROOT_DIR})

target_include_directories(${EX_NAME} PRIVATE ${PYTHON_INCLUDE_DIRS})
target_link_libraries(${EX_NAME} ${PYTHON_LIBRARIES})
if(WIN32)
target_link_libraries(${EX_NAME} hebi kernel32)
target_include_directories(${EX_NAME} PRIVATE ${HEBI_DIR}/src ${HEBI_DIR}/include ${HEBI_DIR}/Eigen)
# For Windows, we copy the .dll file into the binary directory so that we
# don't have to set the PATH variable.

set(LIBHEBI_LOCATION "lib/win_${LIBHEBI_TARGET_ARCHITECTURE}")
set(HEBI_CPP_LIB_DIRECTORY ${HEBI_DIR}/hebi/${LIBHEBI_LOCATION}/)

add_custom_command(TARGET ${EX_NAME} POST_BUILD
COMMAND ${CMAKE_COMMAND} -E copy_if_different
"${HEBI_CPP_LIB_DIRECTORY}/hebi.dll"
$<TARGET_FILE_DIR:${EX_NAME}>)
elseif(UNIX)
target_link_libraries(${EX_NAME} hebi hebic++ m pthread)
endif()


endforeach (EXAMPLE ${BASE_SOURCES})
Comment on lines +318 to +356
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I really should add a CMake function here to help clean this and the other content up. I'm adding a issue for this post-merge.



170 changes: 170 additions & 0 deletions util/group_manager.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,170 @@
#pragma once

// HEBI C++ API components
#include "group.hpp"
#include "group_command.hpp"
#include "group_feedback.hpp"
#include "lookup.hpp"

namespace hebi {
namespace experimental {

class GroupManager {

public:
// Parameters for creating a group manager
struct Params {
// The family and names passed to the "lookup" function to find modules
// Both are required.
std::vector<std::string> families_;
std::vector<std::string> names_;
// How long a command takes effect for on the robot before expiring.
int command_lifetime_ = 100;
// Loop rate, in Hz. This is how fast the arm update loop will nominally
// run.
double control_frequency_ = 200;

// A function pointer that returns a double representing the current time in
// seconds. (Can be overloaded to use, e.g., simulator time)
//
// The default value uses the steady clock wall time.
std::function<double()> get_current_time_s_ = []() {
using clock = std::chrono::steady_clock;
static const clock::time_point start_time = clock::now();
return (std::chrono::duration<double>(clock::now() - start_time)).count();
};
};

//////////////////////////////////////////////////////////////////////////////
// Setup functions
//////////////////////////////////////////////////////////////////////////////

static std::unique_ptr<GroupManager> create(const Params& params);

// Loads gains from the given .xml file, and sends them to the module. Returns
// false if the gains file could not be found, if these is a mismatch in
// number of modules, or the modules do not acknowledge receipt of the gains.
bool loadGains(const std::string& gains_file);

//////////////////////////////////////////////////////////////////////////////
// Accessors
//////////////////////////////////////////////////////////////////////////////

// Returns the number of modules / DoF in the arm
size_t size() const { return group_->size(); }

// Returns the internal group. Not necessary for most use cases.
const Group& group() const { return *group_; }

GroupCommand& pendingCommand() { return command_; }
const GroupCommand& pendingCommand() const { return command_; }

// Returns the last feedback obtained by update, or an empty feedback object
// if "update" has never successfully run.
const GroupFeedback& lastFeedback() const { return feedback_; }
const double dT() const { return dt_; }
const double lastTime() const { return last_time_; }

// Updates feedback.
// To retrieve the feedback, call `getLastFeedback()` after this call.
// You can modify the command object after calling this.
//
// Returns 'false' on a connection problem; true on success.
bool update();

// Sends the user set command to the group manager.
bool send();

protected:
std::function<double()> get_current_time_s_;
double last_time_;
double dt_{ std::numeric_limits<double>::quiet_NaN() };
std::shared_ptr<Group> group_;

hebi::GroupFeedback feedback_;
hebi::GroupCommand command_;

// Private constructor
GroupManager(
std::function<double()> get_current_time_s,
std::shared_ptr<Group> group):
get_current_time_s_(get_current_time_s),
last_time_(get_current_time_s()),
group_(group),
feedback_(group->size()),
command_(group->size()) {}
};

std::unique_ptr<GroupManager> GroupManager::create(const GroupManager::Params& params) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We should split things into .hpp and .cpp at some point :D


// Get the group (scope the lookup object so it is destroyed
// immediately after the lookup operation)
std::shared_ptr<Group> group;
{
Lookup lookup;
group = lookup.getGroupFromNames(params.families_, params.names_);
}
if (!group) {
std::cout << "Could not create arm! Check that family and names match actuators on the network.\n";
return nullptr;
}

// Set parameters
if (!group->setCommandLifetimeMs(params.command_lifetime_)) {
std::cout << "Could not set command lifetime on group; check that it is valid.\n";
return nullptr;
}
if (!group->setFeedbackFrequencyHz(params.control_frequency_)) {
std::cout << "Could not set feedback frequency on group; check that it is valid.\n";
return nullptr;
}

// TODO: once we move up to C++14, we can change this to "make_unique".
std::unique_ptr<GroupManager> gm(new GroupManager(params.get_current_time_s_, group));

// Try to get feedback -- if we don't get a packet in the first N times,
// something is wrong
int num_attempts = 0;

// We need feedback, so we can plan trajectories if that gets called before the first "update"
while (!gm->update()) {
if (num_attempts++ > 10) {
std::cout << "Could not communicate with robot; check network connection.\n";
return nullptr;
}
}

return gm;
}

bool GroupManager::loadGains(const std::string& gains_file)
{
hebi::GroupCommand gains_cmd(group_->size());
if (!gains_cmd.readGains(gains_file))
return false;

return group_->sendCommandWithAcknowledgement(gains_cmd);
}

bool GroupManager::update() {
double t = get_current_time_s_();

// Time must be monotonically increasing!
if (t < last_time_)
return false;

dt_ = t - last_time_;
last_time_ = t;

if (!group_->getNextFeedback(feedback_))
return false;

return true;
}

bool GroupManager::send() {
return group_->sendCommand(command_);
}

} // namespace experimental
} // namespace hebi
Loading