diff --git a/kits/base/omni_mobile_io_control.cpp b/kits/base/omni_mobile_io_control.cpp new file mode 100644 index 00000000..8345d266 --- /dev/null +++ b/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 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; +}; diff --git a/kits/base/omni_waypoints.cpp b/kits/base/omni_waypoints.cpp new file mode 100644 index 00000000..d721995d --- /dev/null +++ b/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; +}; diff --git a/projects/cmake/CMakeLists.txt b/projects/cmake/CMakeLists.txt index 2f3d2ac3..91f1b9b0 100644 --- a/projects/cmake/CMakeLists.txt +++ b/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} $) + 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" + $) + elseif(UNIX) + target_link_libraries(${EX_NAME} hebi hebic++ m pthread) + endif() - +endforeach (EXAMPLE ${BASE_SOURCES}) diff --git a/util/group_manager.hpp b/util/group_manager.hpp new file mode 100644 index 00000000..815a7b0f --- /dev/null +++ b/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 families_; + std::vector 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 get_current_time_s_ = []() { + using clock = std::chrono::steady_clock; + static const clock::time_point start_time = clock::now(); + return (std::chrono::duration(clock::now() - start_time)).count(); + }; + }; + + ////////////////////////////////////////////////////////////////////////////// + // Setup functions + ////////////////////////////////////////////////////////////////////////////// + + static std::unique_ptr 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 get_current_time_s_; + double last_time_; + double dt_{ std::numeric_limits::quiet_NaN() }; + std::shared_ptr group_; + + hebi::GroupFeedback feedback_; + hebi::GroupCommand command_; + + // Private constructor + GroupManager( + std::function get_current_time_s, + std::shared_ptr 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::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; + { + 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 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 diff --git a/util/group_trajectory_manager.hpp b/util/group_trajectory_manager.hpp new file mode 100644 index 00000000..648ef50d --- /dev/null +++ b/util/group_trajectory_manager.hpp @@ -0,0 +1,242 @@ +#pragma once + +// HEBI C++ API components +#include "group.hpp" +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "trajectory.hpp" +#include "lookup.hpp" + +#include "trajectory_manager.hpp" +#include "arm/goal.hpp" + +namespace hebi { +namespace experimental { + +using arm::Goal; + +class GroupTrajectoryManager: GroupManager { + +public: + ////////////////////////////////////////////////////////////////////////////// + // Setup functions + ////////////////////////////////////////////////////////////////////////////// + + // Creates an "Trajectory Follower" object, and puts it into a "weightless" no-goal control + // mode. + static std::unique_ptr create(const GroupManager::Params& params); + + ////////////////////////////////////////////////////////////////////////////// + // Accessors + ////////////////////////////////////////////////////////////////////////////// + + // Returns the currently active internal trajectory. Not necessary for most + // use cases. + // Returns 'nullptr' if there is no active trajectory. + const trajectory::Trajectory* trajectory() const { return trajectory_.get(); } + + ////////////////////////////////////////////////////////////////////////////// + // Goals + ////////////////////////////////////////////////////////////////////////////// + + // Set the current goal waypoint(s), immediately replanning to these + // location(s) and optionally end effector states. + // Goal is a commanded position / velocity. + void setGoal(const Goal& goal); + + // Returns the progress (from 0 to 1) of the current goal, per the last + // update call. + // + // If we have reached the goal, progress is "1". If there is no active goal, + // or we have just begun, progress is "0". + double GroupTrajectoryManager::goalProgress() const { + if (trajectory_) { + double t_traj = last_time_ - trajectory_start_time_; + t_traj = std::min(t_traj, trajectory_->getDuration()); + return t_traj / trajectory_->getDuration(); + } + // No current goal! + return 0.0; + } + + // Have we reached the goal? If there is no goal, returns 'false' + bool atGoal() const { return goalProgress() >= 1.0; } + + // Cancels any active goal, returning to a "weightless" state which does not + // actively command position or velocity. + void cancelGoal() { + trajectory_ = nullptr; + trajectory_start_time_ = std::numeric_limits::quiet_NaN(); + } + +protected: + // Private constructor + GroupTrajectoryManager( + std::function get_current_time_s, + std::shared_ptr group): + get_current_time_s_(get_current_time_s), + last_time_(get_current_time_s()), + group_(group), + pos_(Eigen::VectorXd::Zero(group->size())), + vel_(Eigen::VectorXd::Zero(group->size())), + accel_(Eigen::VectorXd::Zero(group->size())), + feedback_(group->size()), + command_(group->size()) {} + +private: + // The joint angle trajectory for reaching the current goal. + std::shared_ptr trajectory_; + double trajectory_start_time_{ std::numeric_limits::quiet_NaN() }; + // These are just temporary variables to cache output from + // Trajectory::getState. + Eigen::VectorXd pos_; + Eigen::VectorXd vel_; + Eigen::VectorXd accel_; +}; + + +std::unique_ptr GroupTrajectoryManager::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; + { + 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; + } + + // 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" + GroupFeedback feedback(group->size()); + while (!group->getNextFeedback(feedback)) { + if (num_attempts++ > 10) { + std::cout << "Could not communicate with robot; check network connection.\n"; + return nullptr; + } + } + + // Note: once ROS moves up to C++14, we can change this to "make_unique". + return std::unique_ptr(new GroupTrajectoryManager(params.get_current_time_s_, group)); +} + +bool GroupTrajectoryManager::update() { + GroupManager::update(); + + // Update command from trajectory + if (trajectory_) { + // If we have an active trajectory to our goal, use this. + // Note -- this applies even if we are past the end of it; + // we just stay with last state. + // (trajectory_start_time_ should not be nan here!) + double t_traj = t - trajectory_start_time_; + t_traj = std::min(t_traj, trajectory_->getDuration()); + trajectory_->getState(t_traj, &pos_, &vel_, &accel_); + } else { + pos_.setConstant(std::numeric_limits::quiet_NaN()); + vel_.setConstant(std::numeric_limits::quiet_NaN()); + accel_.setConstant(0.0); + } + command_.setPosition(pos_); + command_.setVelocity(vel_); + + return true; +} + +// TODO: think about adding customizability, or at least more intelligence for +// the default heuristic. +Eigen::VectorXd getWaypointTimes( + const Eigen::MatrixXd& positions, + const Eigen::MatrixXd& velocities, + const Eigen::MatrixXd& accelerations) { + + double rampTime = 1.2; + + size_t num_waypoints = positions.cols(); + + Eigen::VectorXd times(num_waypoints); + for (size_t i = 0; i < num_waypoints; ++i) + times[i] = rampTime * (double)i; + + return times; +} + +void GroupTrajectoryManager::setGoal(const Goal& goal) { + int num_joints = goal.positions().rows(); + + // If there is a current trajectory, use the commands as a starting point; + // if not, replan from current feedback. + Eigen::VectorXd curr_pos = Eigen::VectorXd::Zero(num_joints); + Eigen::VectorXd curr_vel = Eigen::VectorXd::Zero(num_joints); + Eigen::VectorXd curr_accel = Eigen::VectorXd::Zero(num_joints); + + // Replan if these is a current trajectory: + if (trajectory_) { + double t_traj = last_time_ - trajectory_start_time_; + t_traj = std::min(t_traj, trajectory_->getDuration()); + trajectory_->getState(t_traj, &curr_pos, &curr_vel, &curr_accel); + } else { + curr_pos = feedback_.getPosition(); + curr_vel = feedback_.getVelocity(); + // (accelerations remain zero) + } + + int num_waypoints = goal.positions().cols() + 1; + + Eigen::MatrixXd positions(num_joints, num_waypoints); + Eigen::MatrixXd velocities(num_joints, num_waypoints); + Eigen::MatrixXd accelerations(num_joints, num_waypoints); + + // Initial state + positions.col(0) = curr_pos; + velocities.col(0) = curr_vel; + accelerations.col(0) = curr_accel; + + // Copy new waypoints + positions.rightCols(num_waypoints - 1) = goal.positions(); + velocities.rightCols(num_waypoints - 1) = goal.velocities(); + accelerations.rightCols(num_waypoints - 1) = goal.accelerations(); + + // Get waypoint times + Eigen::VectorXd waypoint_times(num_waypoints); + // If time vector is empty, automatically determine times + if (goal.times().size() == 0) { + waypoint_times = getWaypointTimes(positions, velocities, accelerations); + } else { + waypoint_times(0) = 0; + waypoint_times.tail(num_waypoints - 1) = goal.times(); + } + + /* + std::cout << "Times:\n" << waypoint_times << std::endl; + std::cout << "Pos:\n" << positions << std::endl; + std::cout << "Vel:\n" << velocities << std::endl; + std::cout << "Acc:\n" << accelerations << std::endl; + */ + + // Create new trajectory + trajectory_ = hebi::trajectory::Trajectory::createUnconstrainedQp( + waypoint_times, positions, &velocities, &accelerations); + trajectory_start_time_ = last_time_; +} + + +} // namespace experimental +} // namespace hebi diff --git a/util/mobile_base.hpp b/util/mobile_base.hpp new file mode 100644 index 00000000..f2a8833b --- /dev/null +++ b/util/mobile_base.hpp @@ -0,0 +1,326 @@ +#pragma once + +#include + +#include "group.hpp" +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "trajectory.hpp" + +#include "Eigen/Dense" + +#include "group_manager.hpp" + + +namespace hebi { +namespace experimental { +namespace mobile { + +using hebi::trajectory::Trajectory; +using TrajectoryQueue = std::queue>; + +class SE2Point { + public: + SE2Point() {} + SE2Point(double x, double y, double theta) : x(x), y(y), theta(theta) {} + + double x{std::numeric_limits::quiet_NaN()}; + double y{std::numeric_limits::quiet_NaN()}; + double theta{std::numeric_limits::quiet_NaN()}; + + SE2Point operator+(const SE2Point& rhs) const { + SE2Point out; + out.x = this->x + rhs.x; + out.y = this->y + rhs.y; + out.theta = this->theta + rhs.theta; + return out; + } + + SE2Point operator-(const SE2Point& rhs) const { + SE2Point out; + out.x = this->x - rhs.x; + out.y = this->y - rhs.y; + out.theta = this->theta - rhs.theta; + return out; + } + + void operator+=(const SE2Point& rhs) { + this->x += rhs.x; + this->y += rhs.y; + this->theta += rhs.theta; + } + +}; + +using Pose = SE2Point; +using Vel = SE2Point; + +Pose operator*(const Vel& val, double t) { + SE2Point out{val.x*t, val.y*t, val.theta*t}; + return out; +} + +class Waypoint { + public: + double t{std::numeric_limits::quiet_NaN()}; + Pose pos{}; + Vel vel{}; + SE2Point acc{}; +}; + +struct CartesianGoal { + // Moves with the given relative body velocity for a certain + // number of seconds, then slows to a stop over a certain number + // of seconds. Can be used for continuous replanning, e.g. + // mobile IO control. + // \param v target velocity + // \param rampTime "ramp up" time + // \param cruiseTime time to drive at velocity v + // \param slowTime "ramp down" time + static CartesianGoal createFromVelocity(Vel v, double rampTime, double cruiseTime, double slowTime) { + Waypoint start; + start.t = rampTime; + start.vel = v; + + Waypoint slow; + slow.t = cruiseTime; + slow.vel = v; + + Waypoint stop; + stop.t = cruiseTime + slowTime; + stop.vel = {0, 0, 0}; + stop.acc = {0, 0, 0}; + + return createFromWaypoints({start, slow, stop}); + } + + // Goes to (x, y, theta) in a certain number of seconds. + static CartesianGoal createFromPosition(Pose p, double goalTime) { + Waypoint end; + + end.t = goalTime; + end.pos = p; + end.vel = {0, 0, 0}; + end.acc = {0, 0, 0}; + + return createFromWaypoints({end}); + } + + static CartesianGoal createFromWaypoints(std::vector waypoints) { + auto count = waypoints.size(); + + VectorXd times(count); + MatrixXd positions(3, count); + MatrixXd velocities(3, count); + MatrixXd accelerations(3, count); + + for (auto idx = 0; idx < count; ++idx) { + times(idx) = waypoints.at(idx).t; + positions(0, idx) = waypoints.at(idx).pos.x; + positions(1, idx) = waypoints.at(idx).pos.y; + positions(2, idx) = waypoints.at(idx).pos.theta; + + velocities(0, idx) = waypoints.at(idx).vel.x; + velocities(1, idx) = waypoints.at(idx).vel.y; + velocities(2, idx) = waypoints.at(idx).vel.theta; + + accelerations(0, idx) = waypoints.at(idx).acc.x; + accelerations(1, idx) = waypoints.at(idx).acc.y; + accelerations(2, idx) = waypoints.at(idx).acc.theta; + } + + return CartesianGoal(times, positions, velocities, accelerations); + } + + const Eigen::VectorXd& times() const { return times_; } + const Eigen::Matrix& positions() const { return positions_; } + const Eigen::Matrix& velocities() const { return velocities_; } + const Eigen::Matrix& accelerations() const { return accelerations_; } + +private: + CartesianGoal(const Eigen::VectorXd& times, + const Eigen::Matrix& positions, + const Eigen::Matrix& velocities, + const Eigen::Matrix& accelerations) + : times_(times), + positions_(positions), + velocities_(velocities), + accelerations_(accelerations) {} + + const Eigen::VectorXd times_{0}; + const Eigen::Matrix positions_{0, 0}; + const Eigen::Matrix velocities_{0, 0}; + const Eigen::Matrix accelerations_{0, 0}; +}; + +////////////////////////////// + +class MobileBase { + +public: + + // Parameters for creating a base + struct Params: public GroupManager::Params { + }; + + // This is a generic implementation that assumes the use of a wheel-space trajectory + bool update() { + + // updates dt and last feedback message + auto ret = group_manager_->update(); + if (!ret) { + std::cout << "Warning: No update from base modules, possible connection issue..." << std::endl; + return false; + } + + double t_now = group_manager_->lastTime(); + // now update odometry + auto vel = group_manager_->lastFeedback().getVelocity(); + updateOdometry(vel, group_manager_->dT()); + + auto& cmd = group_manager_->pendingCommand(); + + clearStaleTrajectories(t_now); + // go into compliance mode if no trajectory + if (base_trajectories_.empty()) { + auto size = group_manager_->size(); + + auto nan = std::numeric_limits::quiet_NaN(); + VectorXd compliantState = VectorXd::Constant(size, nan); + + cmd.setPosition(compliantState); + cmd.setVelocity(compliantState); + compliantState.setZero(); + cmd.setEffort(compliantState); + + return ret; + } else { + auto traj = base_trajectories_.front(); + + VectorXd pos(3), vel(3), accel(3); + + // Update command from trajectory + traj->getState(t_now - trajectory_start_time_, &pos, &vel, &accel); + + // set velocity to steer along cartesian trajectory + + // Convert from x/y/theta to wheel velocities + auto wheel_vels = SE2ToWheelVel({pos(0), pos(1), pos(2)}, {vel(0), vel(1), vel(2)}); + + // Integrate position using wheel velocities. + last_wheel_pos_ += wheel_vels * group_manager_->dT(); + cmd.setPosition(last_wheel_pos_); + + // Use velocity from trajectory, converted from x/y/theta into wheel velocities above. + cmd.setVelocity(wheel_vels); + + return ret; + } + } + + bool send() { + return group_manager_->send(); + } + + virtual SE2Point wheelsToSE2(const Pose& pos, const Eigen::VectorXd& wheel_state) const = 0; + virtual Eigen::VectorXd SE2ToWheelVel(const Pose& pos, const Vel& vel) const = 0; + + // Get the global odometry state + Pose getOdometry() const { return global_pose_ + relative_odom_offset_; }; + + // Reset the current odometry state + void setOdometry(Pose p) { + relative_odom_offset_ = p - getOdometry(); + } + + // Get the maximum velocity in each direction + // NOTE: consider whether this is guaranteed maximum velocity + // or maximum velocity if no other directions are used! Also + // is this true for any config of legged robots, or is this some + // "safe" max vel? + // TODO: potential make this depend on state? + virtual Vel getMaxVelocity() const = 0; + + // Set the goal. Goal will be actively commanded until cleared, + // maintaining the end state of the goal after finishing a smooth + // trajectory to the goal. + // Note -- this turns the `Goal` into a `Trajectory` based on the + // `getMaxVelocity` function implementation + // TODO: "best effort" -- or return "can't do this"? + bool setGoal(const CartesianGoal& g) { + auto baseTrajectories = buildTrajectory(g); + if (baseTrajectories) { + base_trajectories_.swap(*baseTrajectories); + double t_now = group_manager_->lastTime(); + trajectory_start_time_ = t_now; + return true; + } + return false; + } + + // When cleared, the base should be passive / compliant if possible. + void clearGoal() { + // Taken from https://stackoverflow.com/a/709161 + // clear the trajectory queue + TrajectoryQueue().swap(base_trajectories_); + } + + bool goalComplete() { + if (base_trajectories_.empty()) + return true; + if (base_trajectories_.back()->getEndTime() < group_manager_->lastTime()) + return true; + return false; + }; + +protected: + // protected constructor + MobileBase(std::unique_ptr gm, Params p) : + group_manager_(std::move(gm)) + { + last_wheel_pos_ = group_manager_->lastFeedback().getPosition(); + // why is this necessary? + clearGoal(); + } + + // A cartesian trajectory is the only thing an individual base + // implementation must handle. This should be smoothly moved + // to from the current state of the system. + virtual std::unique_ptr buildTrajectory(const CartesianGoal& g) = 0; + + virtual void updateOdometry(const Eigen::VectorXd& wheel_vel, double dt) = 0; + + std::unique_ptr group_manager_{nullptr}; + + // These variables should be updated when updateOdometry is called + Pose global_pose_{0, 0, 0}; + Vel global_vel_{0, 0, 0}; + VectorXd last_wheel_pos_{}; + + Vel local_vel_{0, 0, 0}; + + TrajectoryQueue base_trajectories_{}; + double trajectory_start_time_ = 0; + +private: + Pose relative_odom_offset_{0, 0, 0}; + + void clearStaleTrajectories(double t_now) { + while (!base_trajectories_.empty()) + { + double t_end = base_trajectories_.front()->getEndTime(); + if(t_now > trajectory_start_time_ + t_end) + { + trajectory_start_time_ += t_end; + base_trajectories_.pop(); + } + else + break; + } + } +}; + + +} // namespace mobile +} // namespace experimental +} // namespace hebi diff --git a/util/omni_base.hpp b/util/omni_base.hpp new file mode 100644 index 00000000..bf3d074d --- /dev/null +++ b/util/omni_base.hpp @@ -0,0 +1,208 @@ +#pragma once + +#include "mobile_base.hpp" + +namespace hebi { +namespace experimental { +namespace mobile { + + +class OmniBase : public MobileBase { +public: + // Parameters for creating a base + struct Params: public MobileBase::Params { + double wheel_radius = 0.0762; // m + double base_radius = 0.220; // m (center of omni to origin of base) + }; + + static std::unique_ptr create(Params p) { + auto gm = GroupManager::create(p); + if (!gm) + return nullptr; + + auto retval = std::unique_ptr(new OmniBase(move(gm), p)); + return retval; + } + + virtual SE2Point wheelsToSE2(const Pose& pos,const Eigen::VectorXd&) const override; + virtual Eigen::VectorXd SE2ToWheelVel(const Pose& pos, const Vel& vel) const override; + virtual Vel getMaxVelocity() const override; + +protected: + // protected constructor + OmniBase(std::unique_ptr gm, Params p) : MobileBase(move(gm), p), + wheel_radius_(p.wheel_radius), + base_radius_(p.base_radius) + {} + + // Updates local velocity based on wheel change in position since last time + virtual void updateOdometry(const Eigen::VectorXd& wheel_vel, double dt) override; + + // TODO: think about limitations on (x,y) vs. (x,y,theta) + // trajectories + std::unique_ptr buildTrajectory(const CartesianGoal& g) override; + +private: + + Eigen::Matrix3d thetaToTF(double theta) const { + Eigen::Matrix3d tf = Matrix3d::Identity(); + tf.block<2,2>(0,0) = Rotation2D(theta).toRotationMatrix(); + return tf; + } + + Eigen::Matrix3d createJacobian() { + double a = sqrt(3)/(2 * wheel_radius_); + double b = 1.0 / wheel_radius_; + double c = -base_radius_ / wheel_radius_; + Eigen::Matrix3d j; + j << -a, -b / 2.0, c, + a, -b / 2.0, c, + 0.0, b, c; + return j; + } + + Eigen::Matrix3d createJacobianInv() { + Eigen::Matrix3d j_inv; + double a = wheel_radius_ / sqrt(3); + double b = wheel_radius_ / 3.0; + double c = -b / base_radius_; + j_inv << -a, a, 0, + -b, -b, 2.0 * b, + c, c, c; + return j_inv; + } + + // Helper function to create unconstrained points along a motion. + static MatrixXd nan(size_t num_joints, size_t num_waypoints) { + double nan = std::numeric_limits::quiet_NaN(); + MatrixXd matrix(num_joints, num_waypoints); + matrix.setConstant(nan); + return matrix; + } + + // Helper function to create unconstrained points along a motion, with nan at the right side. + static MatrixXd nanWithZeroRight(size_t num_joints, size_t num_waypoints) { + double nan = std::numeric_limits::quiet_NaN(); + MatrixXd matrix(num_joints, num_waypoints); + matrix.setConstant(nan); + matrix.rightCols<1>().setZero(); + return matrix; + } + + /* Declare main kinematic variables */ + double wheel_radius_; // m + double base_radius_; // m (center of omni to origin of base) + + const Eigen::Matrix3d jacobian_ = createJacobian(); + + // Wheel velocities to local (x,y,theta) + const Eigen::Matrix3d jacobian_inv_ = createJacobianInv(); +}; + +SE2Point OmniBase::wheelsToSE2(const Pose& pos, const Eigen::VectorXd& wheel_vels) const { + auto local_base_vels = jacobian_inv_ * wheel_vels; + // if provided, rotate around theta + if (std::isnan(pos.theta)) + return {local_base_vels(0), local_base_vels(1), local_base_vels(2)}; + + auto base_vels = thetaToTF(pos.theta) * local_base_vels; + return {base_vels(0), base_vels(1), base_vels(2)}; +} + +Eigen::VectorXd OmniBase::SE2ToWheelVel(const Pose& pos, const Vel& vel) const { + Eigen::VectorXd local_vel(3); + local_vel(0) = vel.x; + local_vel(1) = vel.y; + local_vel(2) = vel.theta; + + // if position isn't provided, just use the local frame velocity + if(std::isnan(pos.theta)) { + return jacobian_ * local_vel; + } + + Eigen::Vector3d global_vel = thetaToTF(pos.theta) * local_vel; + + return jacobian_ * global_vel; +}; + +Vel OmniBase::getMaxVelocity() const { + // TODO: Do something smarter + return Vel{2, 2, 2}; +}; + +void OmniBase::updateOdometry(const Eigen::VectorXd& wheel_vel, double dt) { + // Get local velocities + auto local_vel = jacobian_inv_ * wheel_vel; + local_vel_ = {local_vel(0), local_vel(1), local_vel(2)}; + + // Get global velocity: + auto global_vel = thetaToTF(global_pose_.theta) * local_vel; + global_vel_ = {global_vel(0), global_vel(1), global_vel(2)}; + + global_pose_ += global_vel_ * dt; +}; + +std::unique_ptr OmniBase::buildTrajectory(const CartesianGoal& g) { + // construct first waypoint from current state + // so trajectory smoothly transitions + + // remove stale trajectories + auto t_now = group_manager_->lastTime(); + while (!base_trajectories_.empty() && base_trajectories_.front()->getEndTime() < t_now) { + base_trajectories_.pop(); + } + + Vector3d starting_vel; + + if (base_trajectories_.empty()) + { + // if no trajectory, use last feedback. + auto v = group_manager_->pendingCommand().getVelocity(); + auto base_vel = wheelsToSE2({}, v); + starting_vel = {base_vel.x, base_vel.y, base_vel.theta}; + } + else + { + auto traj = base_trajectories_.front(); + Eigen::VectorXd pos(3); + Eigen::VectorXd vel(3); + Eigen::VectorXd acc(3); + traj->getState(t_now, &pos, &vel, &acc); + starting_vel = thetaToTF(pos(2)) * vel; + } + + auto zeros = VectorXd::Constant(3, 0.0); + auto num_waypoints = g.times().size(); + VectorXd times(num_waypoints + 1); + times.tail(num_waypoints) = g.times(); + times(0) = 0.0; + + MatrixXd pos(3, num_waypoints + 1); + pos.col(0) = zeros; + pos.rightCols(num_waypoints) = g.positions(); + + MatrixXd vel(3, num_waypoints + 1); + vel.col(0) = starting_vel; + vel.rightCols(num_waypoints) = g.velocities(); + + MatrixXd acc(3, num_waypoints + 1); + acc.col(0) = zeros; + acc.rightCols(num_waypoints) = g.accelerations(); + + auto traj = Trajectory::createUnconstrainedQp(times, + pos, + &vel, + &acc); + + if (!traj) { + return nullptr; + } + + std::unique_ptr retval(new TrajectoryQueue()); + retval->push(traj); + return retval; +}; + +} // namespace mobile +} // namespace experimental +} // namespace hebi