Cwbollinger/mobile api wip

kits/base/omni_mobile_io_control.cpp

@@ -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); 
+
+  //////////////////////////
+  //// 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();
+    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;
+};

kits/base/omni_waypoints.cpp

@@ -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;
+};

projects/cmake/CMakeLists.txt

@@ -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})
 
 

util/group_manager.hpp

@@ -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) {
+
+  // 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