From 6a343292d625c95ddb0756b70e8b567bfb44fd8c Mon Sep 17 00:00:00 2001 From: Chris Bollinger Date: Tue, 30 Mar 2021 17:41:16 -0400 Subject: [PATCH 01/15] WIP fleshing out mobile api prototype --- util/base.hpp | 234 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 234 insertions(+) create mode 100644 util/base.hpp diff --git a/util/base.hpp b/util/base.hpp new file mode 100644 index 00000000..ff7da9a0 --- /dev/null +++ b/util/base.hpp @@ -0,0 +1,234 @@ +#pragma once + +#include + +#include "group.hpp" +#include "group_command.hpp" +#include "group_feedback.hpp" + +#include "Eigen/Dense" + +namespace hebi { +namespace experimental { +namespace mobile { + +class SE2Point { + public: + float x{}; + float y{}; + float theta{}; + + SE2Point operator+(const SE2Point& rhs) { + SE2Point out; + out.x = this->x + rhs.x; + out.y = this->y + rhs.y; + out.theta = this->theta + rhs.theta; + } + + SE2Point operator-(const SE2Point& rhs) { + SE2Point out; + out.x = this->x - rhs.x; + out.y = this->y - rhs.y; + out.theta = this->theta - rhs.theta; + } +}; + +typedef SE2Point Pose; +typedef SE2Point Vel; + +class Waypoint { + public: + double t{}; + Pose pos{}; + Vel vel{}; + SE2Point eff{}; +}; + +// Potentially HEBI trajectory? Maybe piecewise combination? +// (for velocity, can this be used ) +class Trajectory; + +struct Goal { + // 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 cruiseTime time to drive at velocity v + // \param slowTime "ramp down" time + static Goal createFromVelocity(Vel v, double cruiseTime, double slowTime) { + Waypoint start; + start.t = 0; + start.vel = v; + + Waypoint slow; + slow.t = cruiseTime; + slow.vel = v; + + Waypoint stop; + stop.t = cruiseTime + slowTime; + stop.vel.x = 0; + stop.vel.y = 0; + stop.vel.theta = 0; + + return createFromWaypoints({start, slow, stop}, true); + } + + // Goes to (x, y, theta) in a certain number of seconds. + // \param isBaseFrame is "true" if this is relative to current body frame, + // and "false" for using odometry coordinates. + static Goal createFromPosition(Pose p, double goalTime, bool isBaseFrame) { + Waypoint start; + Waypoint end; + start.t = 0; + start.pos.x = 0; + start.pos.y = 0; + start.pos.theta = 0; + + end.t = goalTime; + end.pos = p; + + createFromWaypoints({start, end}, isBaseFrame); + } + + // Creates from cartesian trajectory. May be (x,y) or (x,y,theta), but + // (x,y,theta) less likely to be realizable depending on system. + // If "theta" is not defined, is tangent to path. + // (x, y path here could be used for planning path through obstacles, for example) + // \param isBaseFrame is "true" if this is relative to current body frame, + // and "false" for using odometry coordinates. + static Goal createFromTrajectory(Trajectory t, bool isBaseFrame); + static Goal createFromWaypoints(std::vector waypoints, bool isBaseFrame); + + 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: + Goal(const Eigen::VectorXd& times, + const Eigen::Matrix& positions, + const Eigen::Matrix& velocities, + const Eigen::Matrix& accelerations) + : times_(times), + positions_(positions), + velocities_(velocities), + accelerations_(accelerations) {} + + // TODO: some info here capturing above states/options + const Eigen::VectorXd times_{0}; + const Eigen::Matrix positions_{0}; + const Eigen::Matrix velocities_{0}; + const Eigen::Matrix accelerations_{0}; + + bool local_trajectory_{}; +}; + +////////////////////////////// + +class Base { + +public: + + // Parameters for creating an arm + 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.f; + + // 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(); + }; + }; + + + void update(float some_timestamp); + // TODO: add a "send" and "getFeedback" and "getCommand" here? + + // Get the global odometry state + virtual Pose getOdometry() const = 0; + + // 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 + // NOTE/TODO: alternatively, could pass a "Base" into the + // Goal factory methods to convert to trajectory goal there. + // Cleaner from the organization of the "Goal" class, but + // messier otherwise -- e.g., user passing base into a function + // that they are about to pass into base: + // base.setGoal(Goal.createFrom(blah, base)); + // TODO: "best effort" -- or return "can't do this"? + void setGoal(const Goal& g); + + // When cleared, the base should be passive / compliant if possible. + void clearGoal() { setTrajectory(std::nullopt); } + + // How far through the goal trajectory are we? Returns "nullopt" + // if there is no active goal. Returns "1" if goal is complete + // (base continues to command end state of trajectory if goal is + // as "1", until goal is explicitly cleared) + // TODO: might be able to move this to the parent class if additional + // trajectory state was here, but I would think that might be + // premature and limit the implementation of various Base types + // for the purpose of saving a couple lines of boilerplate code. + virtual std::optional getGoalProgress() const = 0; + +protected: + // 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. + // TODO: maybe a std::vector? + // When "nullopt" is set, this clears the active trajectory + // goal. + // TODO: think about how to handle starting state to initial + // state of trajectory -- should we just assume first waypoint + // of trajectory is not at time 0, and add first point based on + // current command (or feedback if not active) at time 0?) + virtual void setTrajectory(std::optional t) = 0; + +private: + std::function get_current_time_s_; + Pose _relative_odom_offset{}; +}; + +////////////////////////////// + +class MyBase : Base { +public: + Pose getOdometry() const override; + std::optional getGoalProgress() const override; + Vel getMaxVelocity() const override; +protected: + // TODO: think about limitations on (x,y) vs. (x,y,theta) + // trajectories + void setTrajectory(std::optional t) override; +}; + +} // namespace mobile +} // namespace experimental +} // namespace hebi From e107d82103332d961b355b125e9ffa7313e8b6b6 Mon Sep 17 00:00:00 2001 From: Chris Bollinger Date: Fri, 2 Apr 2021 10:49:47 -0400 Subject: [PATCH 02/15] More WIP --- util/base.hpp | 228 +++++++++++++++++++++-------- util/group_trajectory_follower.cpp | 183 +++++++++++++++++++++++ util/group_trajectory_follower.hpp | 185 +++++++++++++++++++++++ 3 files changed, 537 insertions(+), 59 deletions(-) create mode 100644 util/group_trajectory_follower.cpp create mode 100644 util/group_trajectory_follower.hpp diff --git a/util/base.hpp b/util/base.hpp index ff7da9a0..443e4ae7 100644 --- a/util/base.hpp +++ b/util/base.hpp @@ -5,9 +5,12 @@ #include "group.hpp" #include "group_command.hpp" #include "group_feedback.hpp" +#include "trajectory.hpp" #include "Eigen/Dense" +#include "group_trajectory_follower.hpp" + namespace hebi { namespace experimental { namespace mobile { @@ -23,6 +26,7 @@ class SE2Point { 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) { @@ -30,33 +34,49 @@ class SE2Point { 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; } + }; typedef SE2Point Pose; typedef SE2Point Vel; +Pose operator*(const Vel& val, double t) { + SE2Point out{val.x*t, val.y*t, val.theta*t}; + return out; +} + +Pose operator+(const Pose& v1, const Pose& v2) { + SE2Point out{ v1.x + v2.x, v1.y + v2.y, v1.theta + v2.theta }; + return out; +} + class Waypoint { public: double t{}; Pose pos{}; Vel vel{}; - SE2Point eff{}; + SE2Point acc{}; }; -// Potentially HEBI trajectory? Maybe piecewise combination? -// (for velocity, can this be used ) -class Trajectory; +class CartesianTrajectory; -struct Goal { +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. + // mobile IO con#include "kinematics_helper.hpp"trol. // \param v target velocity // \param cruiseTime time to drive at velocity v // \param slowTime "ramp down" time - static Goal createFromVelocity(Vel v, double cruiseTime, double slowTime) { + static CartesianGoal createFromVelocity(Vel v, double cruiseTime, double slowTime) { Waypoint start; start.t = 0; start.vel = v; @@ -77,7 +97,7 @@ struct Goal { // Goes to (x, y, theta) in a certain number of seconds. // \param isBaseFrame is "true" if this is relative to current body frame, // and "false" for using odometry coordinates. - static Goal createFromPosition(Pose p, double goalTime, bool isBaseFrame) { + static CartesianGoal createFromPosition(Pose p, double goalTime, bool isBaseFrame) { Waypoint start; Waypoint end; start.t = 0; @@ -91,14 +111,36 @@ struct Goal { createFromWaypoints({start, end}, isBaseFrame); } + static CartesianGoal createFromWaypoints(std::vector waypoints, bool isBaseFrame) { + 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; + } + } + // Creates from cartesian trajectory. May be (x,y) or (x,y,theta), but // (x,y,theta) less likely to be realizable depending on system. // If "theta" is not defined, is tangent to path. // (x, y path here could be used for planning path through obstacles, for example) // \param isBaseFrame is "true" if this is relative to current body frame, // and "false" for using odometry coordinates. - static Goal createFromTrajectory(Trajectory t, bool isBaseFrame); - static Goal createFromWaypoints(std::vector waypoints, bool isBaseFrame); + // TODO + static CartesianGoal createFromTrajectory(CartesianTrajectory t, bool isBaseFrame); const Eigen::VectorXd& times() const { return times_; } const Eigen::Matrix& positions() const { return positions_; } @@ -106,7 +148,7 @@ struct Goal { const Eigen::Matrix& accelerations() const { return accelerations_; } private: - Goal(const Eigen::VectorXd& times, + CartesianGoal(const Eigen::VectorXd& times, const Eigen::Matrix& positions, const Eigen::Matrix& velocities, const Eigen::Matrix& accelerations) @@ -126,42 +168,36 @@ struct Goal { ////////////////////////////// -class Base { +class Base: public GroupTrajectoryFollower { public: - // Parameters for creating an arm - 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.f; - - // 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(); - }; + // Parameters for creating a base + struct Params: public GroupTrajectoryFollower::Params { }; + static std::unique_ptr create(const Params& params); + + Base(std::function get_current_time_s, + std::shared_ptr group) { + + } + + bool update() { + auto ret = GroupTrajectoryFollower::update(); + // now update odometry + auto vel = feedback_.getVelocity(); + updateOdometry(vel, dt_); + } - void update(float some_timestamp); - // TODO: add a "send" and "getFeedback" and "getCommand" here? + //virtual SE2Point wheelsToSE2(Eigen::VectorXd wheel_state) const = 0; + virtual Eigen::VectorXd SE2ToWheels(SE2Point base_state) const = 0; // Get the global odometry state - virtual Pose getOdometry() const = 0; + Pose getOdometry() const { return global_pose_ + relative_odom_offset_; }; // Reset the current odometry state - void setOdometry(Pose p) { _relative_odom_offset = p - getOdometry(); } + void setOdometry(Pose p) { relative_odom_offset_ = p - getOdometry(); } // Get the maximum velocity in each direction // NOTE: consider whether this is guaranteed maximum velocity @@ -183,52 +219,126 @@ class Base { // that they are about to pass into base: // base.setGoal(Goal.createFrom(blah, base)); // TODO: "best effort" -- or return "can't do this"? - void setGoal(const Goal& g); + bool setGoal(const CartesianGoal& g) { + auto jointsGoal = buildTrajectory(g); + if (jointsGoal.has_value()) { + GroupTrajectoryFollower::setGoal(jointsGoal.value()); + return true; + } + return false; + } // When cleared, the base should be passive / compliant if possible. - void clearGoal() { setTrajectory(std::nullopt); } - - // How far through the goal trajectory are we? Returns "nullopt" - // if there is no active goal. Returns "1" if goal is complete - // (base continues to command end state of trajectory if goal is - // as "1", until goal is explicitly cleared) - // TODO: might be able to move this to the parent class if additional - // trajectory state was here, but I would think that might be - // premature and limit the implementation of various Base types - // for the purpose of saving a couple lines of boilerplate code. - virtual std::optional getGoalProgress() const = 0; + void clearGoal(); protected: // 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. // TODO: maybe a std::vector? - // When "nullopt" is set, this clears the active trajectory + // When "nullopt" is set, this clears t // Creates from cartesian trajectory. May be (x,y) or (x,y,theta), but + // (x,y,theta) less likely to be realizable depending on system. + // If "theta" is not defined, is tangent to path. + // (x, y path here could be used for planning path through obstacles, for example) + // \param isBaseFrame is "true" if this is relative to current body frame, + // and "false" for using odometry coordinates.he active trajectory // goal. // TODO: think about how to handle starting state to initial // state of trajectory -- should we just assume first waypoint // of trajectory is not at time 0, and add first point based on // current command (or feedback if not active) at time 0?) - virtual void setTrajectory(std::optional t) = 0; + virtual std::optional buildTrajectory(const CartesianGoal& g) = 0; + + virtual void updateOdometry(const Eigen::VectorXd& wheel_vel, double dt) = 0; + + // These variables should be updated when updateOdometry is called + Pose global_pose_{0, 0, 0}; + Vel global_vel_{0, 0, 0}; + + Vel local_vel_{0, 0, 0}; private: - std::function get_current_time_s_; - Pose _relative_odom_offset{}; + Pose relative_odom_offset_{}; }; ////////////////////////////// -class MyBase : Base { +class OmniBase : Base { public: - Pose getOdometry() const override; - std::optional getGoalProgress() const override; + virtual Eigen::VectorXd SE2ToWheelVel(Waypoint base_state) const final { + double theta = base_state.pos.theta; + double dtheta = base_state.vel.theta; + + double offset = 1.0; + double ctheta = std::cos(-theta); + double stheta = std::sin(-theta); + double dx = base_state.vel.x * ctheta - base_state.vel.y * stheta; + double dy = base_state.vel.x * stheta + base_state.vel.y * ctheta; + + Eigen::Vector3d local_vel; + local_vel << dx, dy, dtheta; + + return jacobian_ * local_vel; + }; + Vel getMaxVelocity() const override; -protected: + +private: + // Updates local velocity based on wheel change in position since last time + void OmniBase::updateOdometry(const Eigen::Vector3d& wheel_vel, double dt) { + // Get local velocities + auto local_vel = jacobian_inv_ * wheel_vel; + local_vel_.x = local_vel[0]; + local_vel_.y = local_vel[1]; + local_vel_.theta = local_vel[2]; + + // Get global velocity: + auto c = std::cos(global_pose_.theta); + auto s = std::sin(global_pose_.theta); + global_vel_.x = c * local_vel_.x - s * local_vel_.y; + global_vel_.y = s * local_vel_.x + c * local_vel_.y; + // Theta transforms directly + global_vel_.theta = local_vel_.theta; + + global_pose_ += global_vel_ * dt; + } + + Eigen::Matrix3d OmniBase::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 OmniBase::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; + } + // TODO: think about limitations on (x,y) vs. (x,y,theta) // trajectories - void setTrajectory(std::optional t) override; + std::optional buildTrajectory(const CartesianGoal& g) override; + + /* Declare main kinematic variables */ + static constexpr double wheel_radius_ = 0.0762; // m + static constexpr double base_radius_ = 0.220; // m (center of omni to origin of base) + + const Eigen::Matrix3d OmniBase::jacobian_ = createJacobian(); + + // Wheel velocities to local (x,y,theta) + const Eigen::Matrix3d OmniBase::jacobian_inv_ = createJacobianInv(); }; } // namespace mobile } // namespace experimental -} // namespace hebi +} // namespace hebi \ No newline at end of file diff --git a/util/group_trajectory_follower.cpp b/util/group_trajectory_follower.cpp new file mode 100644 index 00000000..4b7372db --- /dev/null +++ b/util/group_trajectory_follower.cpp @@ -0,0 +1,183 @@ +#include "group_trajectory_follower.hpp" +#include "lookup.hpp" + +namespace hebi { +namespace experimental { + +std::unique_ptr GroupTrajectoryFollower::create(const GroupTrajectoryFollower::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 GroupTrajectoryFollower(params.get_current_time_s_, group)); +} + +bool GroupTrajectoryFollower::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 GroupTrajectoryFollower::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; + + // Define aux state so end effector can be updated + Eigen::VectorXd aux(0); + + // 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; +} + +bool GroupTrajectoryFollower::send() { + return group_->sendCommand(command_); +} + +// 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 GroupTrajectoryFollower::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(); + } + + // Create new trajectory + trajectory_ = hebi::trajectory::Trajectory::createUnconstrainedQp( + waypoint_times, positions, &velocities, &accelerations); + trajectory_start_time_ = last_time_; +} + +double GroupTrajectoryFollower::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; +} + +void GroupTrajectoryFollower::cancelGoal() { + trajectory_ = nullptr; + trajectory_start_time_ = std::numeric_limits::quiet_NaN(); +} + +} // namespace experimental +} // namespace hebi \ No newline at end of file diff --git a/util/group_trajectory_follower.hpp b/util/group_trajectory_follower.hpp new file mode 100644 index 00000000..056558e7 --- /dev/null +++ b/util/group_trajectory_follower.hpp @@ -0,0 +1,185 @@ +#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 "arm/goal.hpp" + +namespace hebi { +namespace experimental { + +using arm::Goal; + +class GroupTrajectoryFollower { + +public: + + ////////////////////////////////////////////////////////////////////////////// + // Setup functions + ////////////////////////////////////////////////////////////////////////////// + + // Parameters for creating an arm + 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.f; + + // 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(); + }; + }; + + // Creates an "Trajectory Follower" object, and puts it into a "weightless" no-goal control + // mode. + 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_; } + + // 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(); } + + // Returns the command last computed by update, or an empty command object + // if "update" has never successfully run. The returned command can be + // modified as desired before it is sent to the robot with the send function. + 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_; } + + ////////////////////////////////////////////////////////////////////////////// + // Main loop functions + // + // Typical usage: + // + // while(true) { + // arm->update(); + // arm->send(); + // } + ////////////////////////////////////////////////////////////////////////////// + + // Updates feedback and generates the basic command for this timestep. + // 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 command last computed by "update" to the robot arm. Any user + // modifications to the command are included. + bool send(); + + ////////////////////////////////////////////////////////////////////////////// + // Goals + // + // A goal is a desired (joint angle) position that the arm should reach, and + // optionally information about the time it should reach that goal at and the + // path (position, velocity, and acceleration waypoints) it should take to + // get there. + // + // The default behavior when a goal is set is for the arm to plan and begin + // executing a smooth motion from its current state to this goal, with an + // internal heuristic that defines the time at which it will reach the goal. + // This immediately overrides any previous goal that was set. + // + // If there is no "active" goal the arm is set into a mode where it is + // actively controlled to be approximately weightless, and can be moved around + // by hand easily. This is the default state when the arm is created. + // + // After reaching the goal, the arm continues to be commanded with the final + // joint state of the set goal, and is _not_ implicitly returned to a + // "weightless" mode. + // + // A goal may also define "aux" states to be sent to an end effector + // associated with the arm. In this case, the end effector states are + // treated as "step functions", immediately being commanded at the timestamp + // of the waypoint they are associated with. An empty "aux" goal or "NaN" + // defines a "no transition" at the given waypoint. + ////////////////////////////////////////////////////////////////////////////// + + // 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 goalProgress() const; + + // 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(); + +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 arm constructor + GroupTrajectoryFollower( + 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_; +}; + +} +} \ No newline at end of file From 02b4c54fb98b31fcc3dd3f185bd1eef60e281364 Mon Sep 17 00:00:00 2001 From: Chris Bollinger Date: Tue, 13 Apr 2021 15:14:58 -0400 Subject: [PATCH 03/15] Commit of current work --- kits/base/omni_mobile_io_control.cpp | 90 +++++++++++++++++++++ util/base.hpp | 113 +++++---------------------- util/group_trajectory_follower.hpp | 1 + util/omni_base.cpp | 64 +++++++++++++++ util/omni_base.hpp | 85 ++++++++++++++++++++ 5 files changed, 260 insertions(+), 93 deletions(-) create mode 100644 kits/base/omni_mobile_io_control.cpp create mode 100644 util/omni_base.cpp create mode 100644 util/omni_base.hpp diff --git a/kits/base/omni_mobile_io_control.cpp b/kits/base/omni_mobile_io_control.cpp new file mode 100644 index 00000000..1a84548d --- /dev/null +++ b/kits/base/omni_mobile_io_control.cpp @@ -0,0 +1,90 @@ + +#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 ////// + ////////////////////////// + + // Create the MobileIO object + std::unique_ptr mobile = MobileIO::create("HEBI", "mobileIO"); + + 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(); + + // Display instructions on screen + mobile -> sendText(instructions); + + // Setup instructions + auto last_state = mobile->getState(); + + ////////////////////////// + //// OmniBase Setup ////// + ////////////////////////// + + // Set module names for mobile base + OmniBase::Params p; + p.families_ = {"HEBI"}; + p.names_ = {"W1", "W2", "W3"}; + + OmniBase base(p); + + ////////////////////////// + //// Main Control Loop /// + ////////////////////////// + + while(base.update()) + { + + auto state = mobile->getState(); + MobileIODiff diff(last_state, state); + + ///////////////// + // Button Presses + ///////////////// + + auto dy = state.getAxis(1); + auto dx = state.getAxis(2); + auto dtheta = state.getAxis(7); + + // Button B8 - End Demo + if (diff.get(8) == MobileIODiff::ButtonState::ToOn) + { + // Clear MobileIO text + mobile->clearText(); + return 1; + } + + ///////////////// + // Update & send + ///////////////// + + // create Base Goal (theta = 1), for 1 second, 0.5 second ramp down + auto goal = CartesianGoal::createFromVelocity(Vel{dx, dy, dtheta}, 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; +}; \ No newline at end of file diff --git a/util/base.hpp b/util/base.hpp index 443e4ae7..d521a4f6 100644 --- a/util/base.hpp +++ b/util/base.hpp @@ -17,9 +17,9 @@ namespace mobile { class SE2Point { public: - float x{}; - float y{}; - float theta{}; + double x{}; + double y{}; + double theta{}; SE2Point operator+(const SE2Point& rhs) { SE2Point out; @@ -168,7 +168,7 @@ struct CartesianGoal { ////////////////////////////// -class Base: public GroupTrajectoryFollower { +class MobileBase { public: @@ -176,22 +176,24 @@ class Base: public GroupTrajectoryFollower { struct Params: public GroupTrajectoryFollower::Params { }; - static std::unique_ptr create(const Params& params); - - Base(std::function get_current_time_s, - std::shared_ptr group) { - - } + MobileBase(Params p) : + trajectory_follower_(GroupTrajectoryFollower::create(p)) + {} bool update() { - auto ret = GroupTrajectoryFollower::update(); + auto ret = trajectory_follower_->update(); // now update odometry - auto vel = feedback_.getVelocity(); - updateOdometry(vel, dt_); + auto vel = trajectory_follower_->lastFeedback().getVelocity(); + updateOdometry(vel, trajectory_follower_->dT()); + return ret; + } + + bool send() { + return trajectory_follower_->send(); } //virtual SE2Point wheelsToSE2(Eigen::VectorXd wheel_state) const = 0; - virtual Eigen::VectorXd SE2ToWheels(SE2Point base_state) const = 0; + virtual Eigen::VectorXd SE2ToWheelVel(Pose pos, Vel vel) const = 0; // Get the global odometry state Pose getOdometry() const { return global_pose_ + relative_odom_offset_; }; @@ -222,7 +224,7 @@ class Base: public GroupTrajectoryFollower { bool setGoal(const CartesianGoal& g) { auto jointsGoal = buildTrajectory(g); if (jointsGoal.has_value()) { - GroupTrajectoryFollower::setGoal(jointsGoal.value()); + trajectory_follower_->setGoal(jointsGoal.value()); return true; } return false; @@ -251,6 +253,8 @@ class Base: public GroupTrajectoryFollower { virtual void updateOdometry(const Eigen::VectorXd& wheel_vel, double dt) = 0; + std::unique_ptr trajectory_follower_={nullptr}; + // These variables should be updated when updateOdometry is called Pose global_pose_{0, 0, 0}; Vel global_vel_{0, 0, 0}; @@ -258,86 +262,9 @@ class Base: public GroupTrajectoryFollower { Vel local_vel_{0, 0, 0}; private: - Pose relative_odom_offset_{}; + Pose relative_odom_offset_{0, 0, 0}; }; -////////////////////////////// - -class OmniBase : Base { -public: - virtual Eigen::VectorXd SE2ToWheelVel(Waypoint base_state) const final { - double theta = base_state.pos.theta; - double dtheta = base_state.vel.theta; - - double offset = 1.0; - double ctheta = std::cos(-theta); - double stheta = std::sin(-theta); - double dx = base_state.vel.x * ctheta - base_state.vel.y * stheta; - double dy = base_state.vel.x * stheta + base_state.vel.y * ctheta; - - Eigen::Vector3d local_vel; - local_vel << dx, dy, dtheta; - - return jacobian_ * local_vel; - }; - - Vel getMaxVelocity() const override; - -private: - // Updates local velocity based on wheel change in position since last time - void OmniBase::updateOdometry(const Eigen::Vector3d& wheel_vel, double dt) { - // Get local velocities - auto local_vel = jacobian_inv_ * wheel_vel; - local_vel_.x = local_vel[0]; - local_vel_.y = local_vel[1]; - local_vel_.theta = local_vel[2]; - - // Get global velocity: - auto c = std::cos(global_pose_.theta); - auto s = std::sin(global_pose_.theta); - global_vel_.x = c * local_vel_.x - s * local_vel_.y; - global_vel_.y = s * local_vel_.x + c * local_vel_.y; - // Theta transforms directly - global_vel_.theta = local_vel_.theta; - - global_pose_ += global_vel_ * dt; - } - - Eigen::Matrix3d OmniBase::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 OmniBase::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; - } - - // TODO: think about limitations on (x,y) vs. (x,y,theta) - // trajectories - std::optional buildTrajectory(const CartesianGoal& g) override; - - /* Declare main kinematic variables */ - static constexpr double wheel_radius_ = 0.0762; // m - static constexpr double base_radius_ = 0.220; // m (center of omni to origin of base) - - const Eigen::Matrix3d OmniBase::jacobian_ = createJacobian(); - - // Wheel velocities to local (x,y,theta) - const Eigen::Matrix3d OmniBase::jacobian_inv_ = createJacobianInv(); -}; } // namespace mobile } // namespace experimental diff --git a/util/group_trajectory_follower.hpp b/util/group_trajectory_follower.hpp index 056558e7..f931fecb 100644 --- a/util/group_trajectory_follower.hpp +++ b/util/group_trajectory_follower.hpp @@ -78,6 +78,7 @@ class GroupTrajectoryFollower { // 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_; } ////////////////////////////////////////////////////////////////////////////// // Main loop functions diff --git a/util/omni_base.cpp b/util/omni_base.cpp new file mode 100644 index 00000000..554881ef --- /dev/null +++ b/util/omni_base.cpp @@ -0,0 +1,64 @@ +#include "omni_base.hpp" + +namespace hebi { +namespace experimental { +namespace mobile { + +Eigen::VectorXd OmniBase::SE2ToWheelVel(Pose pos, Vel vel) const { + double theta = pos.theta; + double dtheta = vel.theta; + + double offset = 1.0; + double ctheta = std::cos(-theta); + double stheta = std::sin(-theta); + double dx = vel.x * ctheta - vel.y * stheta; + double dy = vel.x * stheta + vel.y * ctheta; + + Eigen::Vector3d local_vel; + local_vel << dx, dy, dtheta; + + return jacobian_ * local_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_.x = local_vel[0]; + local_vel_.y = local_vel[1]; + local_vel_.theta = local_vel[2]; + + // Get global velocity: + auto c = std::cos(global_pose_.theta); + auto s = std::sin(global_pose_.theta); + global_vel_.x = c * local_vel_.x - s * local_vel_.y; + global_vel_.y = s * local_vel_.x + c * local_vel_.y; + // Theta transforms directly + global_vel_.theta = local_vel_.theta; + + global_pose_ += global_vel_ * dt; +}; + +std::optional OmniBase::buildTrajectory(const CartesianGoal& g) { + auto num_waypoints = g.times().size(); + MatrixXd wheel_vels(3, num_waypoints); + + for(auto i = 0; i < num_waypoints; ++i) { + auto p = g.positions().col(i); + auto v = g.velocities().col(i); + wheel_vels.col(i) = SE2ToWheelVel(Pose{p(0), p(1), p(2)}, Vel{v(0), v(1), v(2)}); + } + + return Goal::createFromWaypoints(g.times(), + nan(3, num_waypoints), + wheel_vels, + nanWithZeroRight(3, num_waypoints)); +}; + +} // namespace mobile +} // namespace experimental +} // namespace hebi \ No newline at end of file diff --git a/util/omni_base.hpp b/util/omni_base.hpp new file mode 100644 index 00000000..3a679193 --- /dev/null +++ b/util/omni_base.hpp @@ -0,0 +1,85 @@ +#include "base.hpp" + +namespace hebi { +namespace experimental { +namespace mobile { + +class OmniBase : public MobileBase { +public: + // Parameters for creating a base + struct Params: public MobileBase::Params { + Params() : wheel_radius(0.0762), base_radius(0.220) {} + double wheel_radius; // m + double base_radius; // m (center of omni to origin of base) + }; + + OmniBase(Params p) : MobileBase(p), + wheel_radius_(p.wheel_radius), + base_radius_(p.base_radius) + {} + + virtual Eigen::VectorXd SE2ToWheelVel(Pose pos, Vel vel) const override; + virtual Vel getMaxVelocity() const override; + +protected: + // Updates local velocity based on wheel change in position since last time + virtual void updateOdometry(const Eigen::VectorXd& wheel_vel, double dt) override; + +private: + + 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; + } + + // TODO: think about limitations on (x,y) vs. (x,y,theta) + // trajectories + virtual std::optional buildTrajectory(const CartesianGoal& g) override; + + // 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(); +}; + +} +} +} \ No newline at end of file From 9c2042a943e390b62df5873a940b93352a45627a Mon Sep 17 00:00:00 2001 From: Chris Bollinger Date: Wed, 14 Apr 2021 00:33:19 -0400 Subject: [PATCH 04/15] Working teleop, waypoints not quite there Specifically, behavior is bad when waypoint velocities are not constrained. Need to fix `buildTrajectory` --- kits/base/omni_mobile_io_control.cpp | 34 +++-- kits/base/omni_waypoints.cpp | 70 ++++++++++ projects/cmake/CMakeLists.txt | 35 ++++- util/.omni_base.hpp.swp | Bin 0 -> 16384 bytes util/base.hpp | 42 ++++-- util/group_trajectory_follower.cpp | 183 -------------------------- util/group_trajectory_follower.hpp | 186 ++++++++++++++++++++++++++- util/omni_base.cpp | 64 --------- util/omni_base.hpp | 97 +++++++++++++- 9 files changed, 435 insertions(+), 276 deletions(-) create mode 100644 kits/base/omni_waypoints.cpp create mode 100644 util/.omni_base.hpp.swp delete mode 100644 util/group_trajectory_follower.cpp delete mode 100644 util/omni_base.cpp diff --git a/kits/base/omni_mobile_io_control.cpp b/kits/base/omni_mobile_io_control.cpp index 1a84548d..b89f175d 100644 --- a/kits/base/omni_mobile_io_control.cpp +++ b/kits/base/omni_mobile_io_control.cpp @@ -12,14 +12,23 @@ 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("HEBI", "mobileIO"); + 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; - instructions = ("B1 - Waypoint 1\nB2 - Waypoint 2\n" - "B3 - Waypoint 3\n" - "B6 - Grav comp mode\nB8 - Quit\n"); + instructions = ("A1/A2 - Move Base\n" + "A7 - Turn Base\n" + "B8 - Quit\n"); + // Clear any garbage on screen mobile -> clearText(); @@ -35,9 +44,11 @@ int main(int argc, char* argv[]) { // Set module names for mobile base OmniBase::Params p; - p.families_ = {"HEBI"}; + p.families_ = {family}; p.names_ = {"W1", "W2", "W3"}; + std::cout << "Creating Omni Base" << std::endl; + OmniBase base(p); ////////////////////////// @@ -46,17 +57,16 @@ int main(int argc, char* argv[]) { while(base.update()) { - auto state = mobile->getState(); MobileIODiff diff(last_state, state); ///////////////// - // Button Presses + // Input Handling ///////////////// - auto dy = state.getAxis(1); - auto dx = state.getAxis(2); - auto dtheta = state.getAxis(7); + auto dy = -1 * state.getAxis(7) / 2.0; + auto dx = state.getAxis(8) / 2.0; + auto dtheta = -1 * state.getAxis(1); // Button B8 - End Demo if (diff.get(8) == MobileIODiff::ButtonState::ToOn) @@ -70,7 +80,7 @@ int main(int argc, char* argv[]) { // Update & send ///////////////// - // create Base Goal (theta = 1), for 1 second, 0.5 second ramp down + // create Vel Goal for 0.5 second, 0.25 second ramp down auto goal = CartesianGoal::createFromVelocity(Vel{dx, dy, dtheta}, 0.5, 0.25); // send goal to base @@ -87,4 +97,4 @@ int main(int argc, char* argv[]) { mobile -> clearText(); return 0; -}; \ No newline at end of file +}; diff --git a/kits/base/omni_waypoints.cpp b/kits/base/omni_waypoints.cpp new file mode 100644 index 00000000..31d973b3 --- /dev/null +++ b/kits/base/omni_waypoints.cpp @@ -0,0 +1,70 @@ + +#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; + + OmniBase base(p); + + 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}, false); + + // 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.goalProgress() > 0.99) { + base.setGoal(goal); + } + } + + return 0; +}; diff --git a/projects/cmake/CMakeLists.txt b/projects/cmake/CMakeLists.txt index 2f3d2ac3..368ef9ae 100644 --- a/projects/cmake/CMakeLists.txt +++ b/projects/cmake/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.0) project(hebi_cpp_examples) -SET (CMAKE_CXX_STANDARD 11) +SET (CMAKE_CXX_STANDARD 17) SET (CMAKE_CXX_STANDARD_REQUIRED ON) if("${CMAKE_BUILD_TYPE}" STREQUAL "") @@ -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/.omni_base.hpp.swp b/util/.omni_base.hpp.swp new file mode 100644 index 0000000000000000000000000000000000000000..c2fef7210402d758f3569aac5473e19bdbb111e1 GIT binary patch literal 16384 zcmeHNU5wmT6?Q`@6ljB}Dix66WUCfWcK&u&DP^)7K{iXWw405RO;Q@l)MMY7@y262 zwr6&xn-sxID^&n?yS}|eBP*OzCXNw>$;-nn^2>ar zUgd5hvZH||D$9HJ%*p2R)gF|JwM{QL9r}hT!dg$X?TYF96)`gUz5~f1vYndOckNc& z2t~E$`}tOi=YoNPfvaX<5Oped-k{Al)ZDyvW*6P`rsG%LOi`g=pkSb2pkSb2pkSb2 zpkSb2pkUztmx0LNp?wJE?}%O4iodVPeZMMx-V=YH&6UTG#aF>V!9c-4!9c-4!9c-4 z!9c-4!9c-4!9c-4!N4n!0mIU?*TAlwoB+W4|K$FEVW+0O1pEg0HSj~=8^C$sN#Fq> z01gAYfIr-*X+HoS0zLwK7&ryo2E25Gru_~0EAUw$0L}m%;1=NfZ_u>w0Z#+p20j6F zfhFK(;N{nA+6%z5z_)-e1D^*z2b={A;9bDYz=hXo+OL4`0$&0?4Lk~b95@av0T-@^ zO~6w?A7}tOfEQn@X+Hyg4EzY_18)aj072gbJ_7_m3pfSb3)F#|fak75UEm4eali-W zfjUqFs=!}f4ZDH+fcF3u;LX4_z>C*v+HZm9foFg(0$%_=2Al>C0=EEvMk3}U>EQl#`bC85x@aff!#m};BmhZh(AQN8XXi7bsev5I5ZTF zXWG%YscA&`({Nf4wW?81L#)Rbr?uLT%{T7OmkYB>V9Tl4f(@TSg?-EUz!3V62;-8%DHj zcZJ(*o)Bi_1@~CDPg*XM6!wiE5`hN&DOBOzXI;Iz1M91rNLWJQW%~k8t^~4-yureucYf|QWp^8vh^Bk;8$8VZj z(4CI0-YCU>rj5!v`dlI(Pju^vY@}ov-tVzER`hx4vE9)n5q9VruFiIiN_6(DVrL>8 zUrZ}zsHO&uA=8&wB$MA!rVT+R7JqwD*{i^L};8Vuyl#nEW?d-1d-&BXjK*> z)|8g+4*IPP&vqjW8~?p&jD7hS>&KL!0gxs`5?Ug#&6Z>L?I>JO3j3PPGXq;ht;5D) zWd}ND*V6vb!bv;ov7txVd_A-u5Uogdniw!8n%I%V@gd>(7=g~fHDxSg2-F}FBLtS!6A?88#N+^HjvH8CxP?9vXgJ^)=UB-du zGqHxVaL_ZHebgSWVF3OL^9G4O&DatBF1yQM8mL}pY@q(v%!16nl+ev>SLCP|K+5Wc9|HBY8@L{ zyR5OYtj@b-#V8a0)s;A&4kji;rA;+C_0y$7R!r6qXk^v_D9`hj#Er)~d=Sd(glB?e z4@mcAHHUbmOni!$`Uq}TQNH|y@8R+G$)sNPdZ$w2BkGBlaVtbRSw!ix1MtjGF}8Rc+eobeuLrwSYc|lnn@;W+viv%%FGP z(I{aSO*)3K(ZDHB(uEyqU`BEMCi%D>@P?SW(QL--#2xoA?!SEhPu}nU0(bhSfKA}t0N?TdiM#wifS&_D z1%3iN477kdfIYyqz%#hhp9h`&d0iOgO1loY~FS+Fw-vt8&0|f&G z0|f&G0|f*B5d-J=tT#D}@sUfr0@?JLEw8VRcxlR+Ppy~uY|F&V=Tkp%?&lJ1aFIGM z<8~9OtKwzU(XGC%C$1UiA2~zNY^p2Udjqe3GRZ{g-6~&(LS3EuW%9w~yZ~LdL5SmABkW7 zzfn3T^PpQpDIQAkki{{%S&L32)(l}q#&S}Qqz)fRH2s{tOn-7qOTLM?OW11(Co=G7 z&5Bdktj$KHWpnv5nXFdz>ZS!^Nu;qEi8L{lAZKpV;$k+9iLOykHJnYV^T6cO=gi15 zFO4ikhg{FC^bGU#m>SSX@^oVgJjcQG::quiet_NaN()), + y(std::numeric_limits::quiet_NaN()), + theta(std::numeric_limits::quiet_NaN()) {} + SE2Point(double x, double y, double theta) : x(x), y(y), theta(theta) {} + + double x; + double y; + double theta; SE2Point operator+(const SE2Point& rhs) { SE2Point out; @@ -60,7 +66,7 @@ Pose operator+(const Pose& v1, const Pose& v2) { class Waypoint { public: - double t{}; + double t{std::numeric_limits::quiet_NaN()}; Pose pos{}; Vel vel{}; SE2Point acc{}; @@ -72,13 +78,13 @@ 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 con#include "kinematics_helper.hpp"trol. + // mobile IO control. // \param v target velocity // \param cruiseTime time to drive at velocity v // \param slowTime "ramp down" time static CartesianGoal createFromVelocity(Vel v, double cruiseTime, double slowTime) { Waypoint start; - start.t = 0; + start.t = 0.1; start.vel = v; Waypoint slow; @@ -100,7 +106,7 @@ struct CartesianGoal { static CartesianGoal createFromPosition(Pose p, double goalTime, bool isBaseFrame) { Waypoint start; Waypoint end; - start.t = 0; + start.t = 0.1; start.pos.x = 0; start.pos.y = 0; start.pos.theta = 0; @@ -113,10 +119,12 @@ struct CartesianGoal { static CartesianGoal createFromWaypoints(std::vector waypoints, bool isBaseFrame) { 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; @@ -131,6 +139,8 @@ struct CartesianGoal { accelerations(1, idx) = waypoints.at(idx).acc.y; accelerations(2, idx) = waypoints.at(idx).acc.theta; } + + return CartesianGoal(times, positions, velocities, accelerations); } // Creates from cartesian trajectory. May be (x,y) or (x,y,theta), but @@ -159,9 +169,9 @@ struct CartesianGoal { // TODO: some info here capturing above states/options const Eigen::VectorXd times_{0}; - const Eigen::Matrix positions_{0}; - const Eigen::Matrix velocities_{0}; - const Eigen::Matrix accelerations_{0}; + const Eigen::Matrix positions_{0, 0}; + const Eigen::Matrix velocities_{0, 0}; + const Eigen::Matrix accelerations_{0, 0}; bool local_trajectory_{}; }; @@ -222,6 +232,12 @@ class MobileBase { // base.setGoal(Goal.createFrom(blah, base)); // TODO: "best effort" -- or return "can't do this"? bool setGoal(const CartesianGoal& g) { + if (!trajectory_follower_) { + std::cout << "WARNING: Actuator Controller was not created successfully," + << " cannot track goal!" << std::endl; + return false; + } + auto jointsGoal = buildTrajectory(g); if (jointsGoal.has_value()) { trajectory_follower_->setGoal(jointsGoal.value()); @@ -233,6 +249,10 @@ class MobileBase { // When cleared, the base should be passive / compliant if possible. void clearGoal(); + double goalProgress() const { + return trajectory_follower_->goalProgress(); + } + protected: // A cartesian trajectory is the only thing an individual base // implementation must handle. This should be smoothly moved @@ -268,4 +288,4 @@ class MobileBase { } // namespace mobile } // namespace experimental -} // namespace hebi \ No newline at end of file +} // namespace hebi diff --git a/util/group_trajectory_follower.cpp b/util/group_trajectory_follower.cpp deleted file mode 100644 index 4b7372db..00000000 --- a/util/group_trajectory_follower.cpp +++ /dev/null @@ -1,183 +0,0 @@ -#include "group_trajectory_follower.hpp" -#include "lookup.hpp" - -namespace hebi { -namespace experimental { - -std::unique_ptr GroupTrajectoryFollower::create(const GroupTrajectoryFollower::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 GroupTrajectoryFollower(params.get_current_time_s_, group)); -} - -bool GroupTrajectoryFollower::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 GroupTrajectoryFollower::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; - - // Define aux state so end effector can be updated - Eigen::VectorXd aux(0); - - // 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; -} - -bool GroupTrajectoryFollower::send() { - return group_->sendCommand(command_); -} - -// 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 GroupTrajectoryFollower::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(); - } - - // Create new trajectory - trajectory_ = hebi::trajectory::Trajectory::createUnconstrainedQp( - waypoint_times, positions, &velocities, &accelerations); - trajectory_start_time_ = last_time_; -} - -double GroupTrajectoryFollower::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; -} - -void GroupTrajectoryFollower::cancelGoal() { - trajectory_ = nullptr; - trajectory_start_time_ = std::numeric_limits::quiet_NaN(); -} - -} // namespace experimental -} // namespace hebi \ No newline at end of file diff --git a/util/group_trajectory_follower.hpp b/util/group_trajectory_follower.hpp index f931fecb..0dddb708 100644 --- a/util/group_trajectory_follower.hpp +++ b/util/group_trajectory_follower.hpp @@ -6,6 +6,7 @@ #include "group_feedback.hpp" #include "robot_model.hpp" #include "trajectory.hpp" +#include "lookup.hpp" #include "arm/goal.hpp" @@ -182,5 +183,188 @@ class GroupTrajectoryFollower { Eigen::VectorXd accel_; }; + +std::unique_ptr GroupTrajectoryFollower::create(const GroupTrajectoryFollower::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 GroupTrajectoryFollower(params.get_current_time_s_, group)); +} + +bool GroupTrajectoryFollower::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 GroupTrajectoryFollower::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; + + // Define aux state so end effector can be updated + Eigen::VectorXd aux(0); + + // 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; +} + +bool GroupTrajectoryFollower::send() { + return group_->sendCommand(command_); +} + +// 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 GroupTrajectoryFollower::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_; } -} \ No newline at end of file + +double GroupTrajectoryFollower::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; +} + +void GroupTrajectoryFollower::cancelGoal() { + trajectory_ = nullptr; + trajectory_start_time_ = std::numeric_limits::quiet_NaN(); +} + +} // namespace experimental +} // namespace hebi diff --git a/util/omni_base.cpp b/util/omni_base.cpp deleted file mode 100644 index 554881ef..00000000 --- a/util/omni_base.cpp +++ /dev/null @@ -1,64 +0,0 @@ -#include "omni_base.hpp" - -namespace hebi { -namespace experimental { -namespace mobile { - -Eigen::VectorXd OmniBase::SE2ToWheelVel(Pose pos, Vel vel) const { - double theta = pos.theta; - double dtheta = vel.theta; - - double offset = 1.0; - double ctheta = std::cos(-theta); - double stheta = std::sin(-theta); - double dx = vel.x * ctheta - vel.y * stheta; - double dy = vel.x * stheta + vel.y * ctheta; - - Eigen::Vector3d local_vel; - local_vel << dx, dy, dtheta; - - return jacobian_ * local_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_.x = local_vel[0]; - local_vel_.y = local_vel[1]; - local_vel_.theta = local_vel[2]; - - // Get global velocity: - auto c = std::cos(global_pose_.theta); - auto s = std::sin(global_pose_.theta); - global_vel_.x = c * local_vel_.x - s * local_vel_.y; - global_vel_.y = s * local_vel_.x + c * local_vel_.y; - // Theta transforms directly - global_vel_.theta = local_vel_.theta; - - global_pose_ += global_vel_ * dt; -}; - -std::optional OmniBase::buildTrajectory(const CartesianGoal& g) { - auto num_waypoints = g.times().size(); - MatrixXd wheel_vels(3, num_waypoints); - - for(auto i = 0; i < num_waypoints; ++i) { - auto p = g.positions().col(i); - auto v = g.velocities().col(i); - wheel_vels.col(i) = SE2ToWheelVel(Pose{p(0), p(1), p(2)}, Vel{v(0), v(1), v(2)}); - } - - return Goal::createFromWaypoints(g.times(), - nan(3, num_waypoints), - wheel_vels, - nanWithZeroRight(3, num_waypoints)); -}; - -} // namespace mobile -} // namespace experimental -} // namespace hebi \ No newline at end of file diff --git a/util/omni_base.hpp b/util/omni_base.hpp index 3a679193..a2056a2c 100644 --- a/util/omni_base.hpp +++ b/util/omni_base.hpp @@ -1,3 +1,5 @@ +#pragma once + #include "base.hpp" namespace hebi { @@ -80,6 +82,95 @@ class OmniBase : public MobileBase { const Eigen::Matrix3d jacobian_inv_ = createJacobianInv(); }; -} -} -} \ No newline at end of file + +Eigen::VectorXd OmniBase::SE2ToWheelVel(Pose pos, Vel vel) const { + // if position isn't provided, just return the local frame velocity + if(std::isnan(pos.x) || std::isnan(pos.y) || std::isnan(pos.theta)) { + Eigen::VectorXd ret(3); + ret(0) = vel.x; + ret(1) = vel.y; + ret(2) = vel.theta; + return jacobian_ * ret; + } + + double theta = pos.theta; + double dtheta = vel.theta; + + double offset = 1.0; + double ctheta = std::cos(-theta); + double stheta = std::sin(-theta); + double dx = vel.x * ctheta - vel.y * stheta; + double dy = vel.x * stheta + vel.y * ctheta; + + Eigen::Vector3d local_vel; + local_vel << dx, dy, dtheta; + + return jacobian_ * local_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_.x = local_vel[0]; + local_vel_.y = local_vel[1]; + local_vel_.theta = local_vel[2]; + + // Get global velocity: + auto c = std::cos(global_pose_.theta); + auto s = std::sin(global_pose_.theta); + global_vel_.x = c * local_vel_.x - s * local_vel_.y; + global_vel_.y = s * local_vel_.x + c * local_vel_.y; + // Theta transforms directly + global_vel_.theta = local_vel_.theta; + + global_pose_ += global_vel_ * dt; +}; + +std::optional OmniBase::buildTrajectory(const CartesianGoal& g) { + auto num_waypoints = g.times().size(); + MatrixXd wheel_vels(3, num_waypoints); + + // TODO: REDO THIS PROPERLY IT SUCKS/IS BROKEN + // need to have some kind of non-hacky 2d trajectory rollout, + // or drop this approach entirely + + std::cout << "Build Trajectory" << std::endl; + + for(auto i = 0; i < num_waypoints; ++i) { + Vector3d p = g.positions().col(i); + Vector3d v = g.velocities().col(i); + if(std::isnan(v(0)) || std::isnan(v(1)) || std::isnan(v(2))) { + if(i == num_waypoints - 1) { + v(0) = 0; + v(1) = 0; + v(2) = 0; + } else { + auto delta = p - g.positions().col(i+1); + auto dt = g.times()(i+1) - g.times()(i); + v(0) = delta(0)/dt; + v(1) = delta(1)/dt; + v(2) = delta(2)/dt; + } + } + std::cout << "P:\n" << p << std::endl; + std::cout << "V:\n" << v < Date: Thu, 15 Apr 2021 12:18:07 -0400 Subject: [PATCH 05/15] Use unconstrainedQp to smooth cartesian waypoints --- util/base.hpp | 8 +-- ...jectory_follower.hpp => group_manager.hpp} | 25 ++++---- util/omni_base.hpp | 64 ++++++++++--------- 3 files changed, 50 insertions(+), 47 deletions(-) rename util/{group_trajectory_follower.hpp => group_manager.hpp} (94%) diff --git a/util/base.hpp b/util/base.hpp index 5e3dc280..bccb2a19 100644 --- a/util/base.hpp +++ b/util/base.hpp @@ -9,7 +9,7 @@ #include "Eigen/Dense" -#include "group_trajectory_follower.hpp" +#include "group_manager.hpp" namespace hebi { @@ -183,11 +183,11 @@ class MobileBase { public: // Parameters for creating a base - struct Params: public GroupTrajectoryFollower::Params { + struct Params: public GroupManager::Params { }; MobileBase(Params p) : - trajectory_follower_(GroupTrajectoryFollower::create(p)) + trajectory_follower_(GroupManager::create(p)) {} bool update() { @@ -273,7 +273,7 @@ class MobileBase { virtual void updateOdometry(const Eigen::VectorXd& wheel_vel, double dt) = 0; - std::unique_ptr trajectory_follower_={nullptr}; + std::unique_ptr trajectory_follower_={nullptr}; // These variables should be updated when updateOdometry is called Pose global_pose_{0, 0, 0}; diff --git a/util/group_trajectory_follower.hpp b/util/group_manager.hpp similarity index 94% rename from util/group_trajectory_follower.hpp rename to util/group_manager.hpp index 0dddb708..5493affa 100644 --- a/util/group_trajectory_follower.hpp +++ b/util/group_manager.hpp @@ -15,7 +15,7 @@ namespace experimental { using arm::Goal; -class GroupTrajectoryFollower { +class GroupManager { public: @@ -48,7 +48,7 @@ class GroupTrajectoryFollower { // Creates an "Trajectory Follower" object, and puts it into a "weightless" no-goal control // mode. - static std::unique_ptr create(const Params& params); + 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 @@ -160,7 +160,7 @@ class GroupTrajectoryFollower { hebi::GroupCommand command_; // Private arm constructor - GroupTrajectoryFollower( + GroupManager( std::function get_current_time_s, std::shared_ptr group): get_current_time_s_(get_current_time_s), @@ -184,7 +184,7 @@ class GroupTrajectoryFollower { }; -std::unique_ptr GroupTrajectoryFollower::create(const GroupTrajectoryFollower::Params& params) { +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) @@ -222,10 +222,10 @@ std::unique_ptr GroupTrajectoryFollower::create(const G } // Note: once ROS moves up to C++14, we can change this to "make_unique". - return std::unique_ptr(new GroupTrajectoryFollower(params.get_current_time_s_, group)); + return std::unique_ptr(new GroupManager(params.get_current_time_s_, group)); } -bool GroupTrajectoryFollower::loadGains(const std::string& gains_file) +bool GroupManager::loadGains(const std::string& gains_file) { hebi::GroupCommand gains_cmd(group_->size()); if (!gains_cmd.readGains(gains_file)) @@ -234,7 +234,7 @@ bool GroupTrajectoryFollower::loadGains(const std::string& gains_file) return group_->sendCommandWithAcknowledgement(gains_cmd); } -bool GroupTrajectoryFollower::update() { +bool GroupManager::update() { double t = get_current_time_s_(); // Time must be monotonically increasing! @@ -247,9 +247,6 @@ bool GroupTrajectoryFollower::update() { if (!group_->getNextFeedback(feedback_)) return false; - // Define aux state so end effector can be updated - Eigen::VectorXd aux(0); - // Update command from trajectory if (trajectory_) { // If we have an active trajectory to our goal, use this. @@ -270,7 +267,7 @@ bool GroupTrajectoryFollower::update() { return true; } -bool GroupTrajectoryFollower::send() { +bool GroupManager::send() { return group_->sendCommand(command_); } @@ -292,7 +289,7 @@ Eigen::VectorXd getWaypointTimes( return times; } -void GroupTrajectoryFollower::setGoal(const Goal& goal) { +void GroupManager::setGoal(const Goal& goal) { int num_joints = goal.positions().rows(); // If there is a current trajectory, use the commands as a starting point; @@ -351,7 +348,7 @@ void GroupTrajectoryFollower::setGoal(const Goal& goal) { trajectory_start_time_ = last_time_; } -double GroupTrajectoryFollower::goalProgress() const { +double GroupManager::goalProgress() const { if (trajectory_) { double t_traj = last_time_ - trajectory_start_time_; t_traj = std::min(t_traj, trajectory_->getDuration()); @@ -361,7 +358,7 @@ double GroupTrajectoryFollower::goalProgress() const { return 0.0; } -void GroupTrajectoryFollower::cancelGoal() { +void GroupManager::cancelGoal() { trajectory_ = nullptr; trajectory_start_time_ = std::numeric_limits::quiet_NaN(); } diff --git a/util/omni_base.hpp b/util/omni_base.hpp index a2056a2c..4b20999b 100644 --- a/util/omni_base.hpp +++ b/util/omni_base.hpp @@ -2,6 +2,8 @@ #include "base.hpp" +#include "trajectory.hpp" + namespace hebi { namespace experimental { namespace mobile { @@ -10,14 +12,16 @@ class OmniBase : public MobileBase { public: // Parameters for creating a base struct Params: public MobileBase::Params { - Params() : wheel_radius(0.0762), base_radius(0.220) {} + Params() : wheel_radius(0.0762), base_radius(0.220), sample_density(0.1) {} double wheel_radius; // m double base_radius; // m (center of omni to origin of base) + double sample_density; // time between sampled waypoints on cartesian trajectory }; OmniBase(Params p) : MobileBase(p), wheel_radius_(p.wheel_radius), - base_radius_(p.base_radius) + base_radius_(p.base_radius), + sample_density_(p.sample_density) {} virtual Eigen::VectorXd SE2ToWheelVel(Pose pos, Vel vel) const override; @@ -76,6 +80,8 @@ class OmniBase : public MobileBase { double wheel_radius_; // m double base_radius_; // m (center of omni to origin of base) + double sample_density_; // sec + const Eigen::Matrix3d jacobian_ = createJacobian(); // Wheel velocities to local (x,y,theta) @@ -133,42 +139,42 @@ void OmniBase::updateOdometry(const Eigen::VectorXd& wheel_vel, double dt) { std::optional OmniBase::buildTrajectory(const CartesianGoal& g) { auto num_waypoints = g.times().size(); - MatrixXd wheel_vels(3, num_waypoints); + auto num_wheels = g.positions().row(0).size(); - // TODO: REDO THIS PROPERLY IT SUCKS/IS BROKEN - // need to have some kind of non-hacky 2d trajectory rollout, - // or drop this approach entirely + MatrixXd wheel_pos(num_wheels, num_waypoints); + MatrixXd wheel_vel(num_wheels, num_waypoints); + MatrixXd wheel_acc(num_wheels, num_waypoints); std::cout << "Build Trajectory" << std::endl; - for(auto i = 0; i < num_waypoints; ++i) { - Vector3d p = g.positions().col(i); - Vector3d v = g.velocities().col(i); - if(std::isnan(v(0)) || std::isnan(v(1)) || std::isnan(v(2))) { - if(i == num_waypoints - 1) { - v(0) = 0; - v(1) = 0; - v(2) = 0; - } else { - auto delta = p - g.positions().col(i+1); - auto dt = g.times()(i+1) - g.times()(i); - v(0) = delta(0)/dt; - v(1) = delta(1)/dt; - v(2) = delta(2)/dt; - } - } - std::cout << "P:\n" << p << std::endl; - std::cout << "V:\n" << v <getDuration() / sample_density_); + VectorXd p, v, a; + + for(auto i = 0; i < num_samples; ++i) { + double t = i * sample_density_; + + traj->getState(t, &p, &v, &a); + + wheel_pos.col(i) = p; + wheel_vel.col(i) = SE2ToWheelVel(Pose{p(0), p(1), p(2)}, Vel{v(0), v(1), v(2)}); + // this feels hacky but works I think? + wheel_acc.col(i) = SE2ToWheelVel(Pose{p(0), p(1), p(2)}, Vel{a(0), a(1), a(2)}); } std::cout << "Times:\n" << g.times() << std::endl; - std::cout << "Vels:\n" << wheel_vels << std::endl; + std::cout << "Vels:\n" << wheel_vel << std::endl; return Goal::createFromWaypoints(g.times(), - nan(3, num_waypoints), - wheel_vels, - nanWithZeroRight(3, num_waypoints)); + wheel_pos, + wheel_vel, + wheel_acc); }; } // namespace mobile From 05e84bc8921c9bf2e4a06a7e263c7e2e3ee8f3cd Mon Sep 17 00:00:00 2001 From: cwbollinger Date: Thu, 15 Apr 2021 12:50:30 -0400 Subject: [PATCH 06/15] Delete .omni_base.hpp.swp --- util/.omni_base.hpp.swp | Bin 16384 -> 0 bytes 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 util/.omni_base.hpp.swp diff --git a/util/.omni_base.hpp.swp b/util/.omni_base.hpp.swp deleted file mode 100644 index c2fef7210402d758f3569aac5473e19bdbb111e1..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 16384 zcmeHNU5wmT6?Q`@6ljB}Dix66WUCfWcK&u&DP^)7K{iXWw405RO;Q@l)MMY7@y262 zwr6&xn-sxID^&n?yS}|eBP*OzCXNw>$;-nn^2>ar zUgd5hvZH||D$9HJ%*p2R)gF|JwM{QL9r}hT!dg$X?TYF96)`gUz5~f1vYndOckNc& z2t~E$`}tOi=YoNPfvaX<5Oped-k{Al)ZDyvW*6P`rsG%LOi`g=pkSb2pkSb2pkSb2 zpkSb2pkUztmx0LNp?wJE?}%O4iodVPeZMMx-V=YH&6UTG#aF>V!9c-4!9c-4!9c-4 z!9c-4!9c-4!9c-4!N4n!0mIU?*TAlwoB+W4|K$FEVW+0O1pEg0HSj~=8^C$sN#Fq> z01gAYfIr-*X+HoS0zLwK7&ryo2E25Gru_~0EAUw$0L}m%;1=NfZ_u>w0Z#+p20j6F zfhFK(;N{nA+6%z5z_)-e1D^*z2b={A;9bDYz=hXo+OL4`0$&0?4Lk~b95@av0T-@^ zO~6w?A7}tOfEQn@X+Hyg4EzY_18)aj072gbJ_7_m3pfSb3)F#|fak75UEm4eali-W zfjUqFs=!}f4ZDH+fcF3u;LX4_z>C*v+HZm9foFg(0$%_=2Al>C0=EEvMk3}U>EQl#`bC85x@aff!#m};BmhZh(AQN8XXi7bsev5I5ZTF zXWG%YscA&`({Nf4wW?81L#)Rbr?uLT%{T7OmkYB>V9Tl4f(@TSg?-EUz!3V62;-8%DHj zcZJ(*o)Bi_1@~CDPg*XM6!wiE5`hN&DOBOzXI;Iz1M91rNLWJQW%~k8t^~4-yureucYf|QWp^8vh^Bk;8$8VZj z(4CI0-YCU>rj5!v`dlI(Pju^vY@}ov-tVzER`hx4vE9)n5q9VruFiIiN_6(DVrL>8 zUrZ}zsHO&uA=8&wB$MA!rVT+R7JqwD*{i^L};8Vuyl#nEW?d-1d-&BXjK*> z)|8g+4*IPP&vqjW8~?p&jD7hS>&KL!0gxs`5?Ug#&6Z>L?I>JO3j3PPGXq;ht;5D) zWd}ND*V6vb!bv;ov7txVd_A-u5Uogdniw!8n%I%V@gd>(7=g~fHDxSg2-F}FBLtS!6A?88#N+^HjvH8CxP?9vXgJ^)=UB-du zGqHxVaL_ZHebgSWVF3OL^9G4O&DatBF1yQM8mL}pY@q(v%!16nl+ev>SLCP|K+5Wc9|HBY8@L{ zyR5OYtj@b-#V8a0)s;A&4kji;rA;+C_0y$7R!r6qXk^v_D9`hj#Er)~d=Sd(glB?e z4@mcAHHUbmOni!$`Uq}TQNH|y@8R+G$)sNPdZ$w2BkGBlaVtbRSw!ix1MtjGF}8Rc+eobeuLrwSYc|lnn@;W+viv%%FGP z(I{aSO*)3K(ZDHB(uEyqU`BEMCi%D>@P?SW(QL--#2xoA?!SEhPu}nU0(bhSfKA}t0N?TdiM#wifS&_D z1%3iN477kdfIYyqz%#hhp9h`&d0iOgO1loY~FS+Fw-vt8&0|f&G z0|f&G0|f*B5d-J=tT#D}@sUfr0@?JLEw8VRcxlR+Ppy~uY|F&V=Tkp%?&lJ1aFIGM z<8~9OtKwzU(XGC%C$1UiA2~zNY^p2Udjqe3GRZ{g-6~&(LS3EuW%9w~yZ~LdL5SmABkW7 zzfn3T^PpQpDIQAkki{{%S&L32)(l}q#&S}Qqz)fRH2s{tOn-7qOTLM?OW11(Co=G7 z&5Bdktj$KHWpnv5nXFdz>ZS!^Nu;qEi8L{lAZKpV;$k+9iLOykHJnYV^T6cO=gi15 zFO4ikhg{FC^bGU#m>SSX@^oVgJjcQG Date: Mon, 26 Apr 2021 20:17:04 -0400 Subject: [PATCH 07/15] Giving up on trying to use joint space trajectories Now uses piecewise cartesian trajectories and maps to joint space as you go. More similar to old approach --- util/base.hpp | 87 +++++++++++++++++++++++++++++++++++------- util/group_manager.hpp | 1 + util/omni_base.hpp | 36 ++++++----------- 3 files changed, 85 insertions(+), 39 deletions(-) diff --git a/util/base.hpp b/util/base.hpp index bccb2a19..3cd49517 100644 --- a/util/base.hpp +++ b/util/base.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include "group.hpp" #include "group_command.hpp" @@ -16,6 +17,11 @@ namespace hebi { namespace experimental { namespace mobile { +using std::optional; +using std::queue; +using std::shared_ptr; +using hebi::trajectory::Trajectory; + class SE2Point { public: SE2Point() : x(std::numeric_limits::quiet_NaN()), @@ -187,19 +193,66 @@ class MobileBase { }; MobileBase(Params p) : - trajectory_follower_(GroupManager::create(p)) + group_manager_(GroupManager::create(p)) {} + // This is a generic implementation that assumes the use of a wheel-space trajectory bool update() { - auto ret = trajectory_follower_->update(); + // updates dt and last feedback message + auto ret = group_manager_->update(); // now update odometry - auto vel = trajectory_follower_->lastFeedback().getVelocity(); - updateOdometry(vel, trajectory_follower_->dT()); - return ret; + auto vel = group_manager_->lastFeedback().getVelocity(); + updateOdometry(vel, group_manager_->dT()); + + auto cmd = group_manager_->pendingCommand(); + + while (base_trajectories_.front() && base_trajectories_.front()->getEndTime() < group_manager_->lastTime()) { + + } + + // go into compliance mode if no trajectory + if (base_trajectories_.size() == 0) { + auto size = group_manager_->size(); + + VectorXd compliantState; + compliantState.resize(size); + double nan = std::numeric_limits::quiet_NaN(); + for (auto i=0; igetState(group_manager_->lastTime(), &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 trajectory_follower_->send(); + return group_manager_->send(); } //virtual SE2Point wheelsToSE2(Eigen::VectorXd wheel_state) const = 0; @@ -209,7 +262,10 @@ class MobileBase { Pose getOdometry() const { return global_pose_ + relative_odom_offset_; }; // Reset the current odometry state - void setOdometry(Pose p) { relative_odom_offset_ = p - getOdometry(); } + void setOdometry(Pose p) { + last_wheel_pos_ = group_manager_->lastFeedback().getPosition(); + relative_odom_offset_ = p - getOdometry(); + } // Get the maximum velocity in each direction // NOTE: consider whether this is guaranteed maximum velocity @@ -232,15 +288,15 @@ class MobileBase { // base.setGoal(Goal.createFrom(blah, base)); // TODO: "best effort" -- or return "can't do this"? bool setGoal(const CartesianGoal& g) { - if (!trajectory_follower_) { + if (!group_manager_) { std::cout << "WARNING: Actuator Controller was not created successfully," << " cannot track goal!" << std::endl; return false; } - auto jointsGoal = buildTrajectory(g); - if (jointsGoal.has_value()) { - trajectory_follower_->setGoal(jointsGoal.value()); + auto baseTrajectory = buildTrajectory(g); + if (baseTrajectory.has_value()) { + base_trajectories_ = baseTrajectory.value(); return true; } return false; @@ -250,7 +306,7 @@ class MobileBase { void clearGoal(); double goalProgress() const { - return trajectory_follower_->goalProgress(); + return group_manager_->goalProgress(); } protected: @@ -269,18 +325,21 @@ class MobileBase { // state of trajectory -- should we just assume first waypoint // of trajectory is not at time 0, and add first point based on // current command (or feedback if not active) at time 0?) - virtual std::optional buildTrajectory(const CartesianGoal& g) = 0; + virtual optional>> buildTrajectory(const CartesianGoal& g) = 0; virtual void updateOdometry(const Eigen::VectorXd& wheel_vel, double dt) = 0; - std::unique_ptr trajectory_follower_={nullptr}; + 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}; + queue> base_trajectories_{}; + private: Pose relative_odom_offset_{0, 0, 0}; }; diff --git a/util/group_manager.hpp b/util/group_manager.hpp index 5493affa..ea94ed14 100644 --- a/util/group_manager.hpp +++ b/util/group_manager.hpp @@ -80,6 +80,7 @@ class GroupManager { // 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_; } ////////////////////////////////////////////////////////////////////////////// // Main loop functions diff --git a/util/omni_base.hpp b/util/omni_base.hpp index 4b20999b..eb4c6964 100644 --- a/util/omni_base.hpp +++ b/util/omni_base.hpp @@ -57,7 +57,7 @@ class OmniBase : public MobileBase { // TODO: think about limitations on (x,y) vs. (x,y,theta) // trajectories - virtual std::optional buildTrajectory(const CartesianGoal& g) override; + virtual optional>> buildTrajectory(const CartesianGoal& g) override; // Helper function to create unconstrained points along a motion. static MatrixXd nan(size_t num_joints, size_t num_waypoints) { @@ -137,7 +137,7 @@ void OmniBase::updateOdometry(const Eigen::VectorXd& wheel_vel, double dt) { global_pose_ += global_vel_ * dt; }; -std::optional OmniBase::buildTrajectory(const CartesianGoal& g) { +optional>> OmniBase::buildTrajectory(const CartesianGoal& g) { auto num_waypoints = g.times().size(); auto num_wheels = g.positions().row(0).size(); @@ -149,32 +149,18 @@ std::optional OmniBase::buildTrajectory(const CartesianGoal& g) { MatrixXd vel = g.velocities(); MatrixXd acc = g.accelerations(); - auto traj = hebi::trajectory::Trajectory::createUnconstrainedQp(g.times(), - g.positions(), - &vel, - &acc); + auto traj = Trajectory::createUnconstrainedQp(g.times(), + g.positions(), + &vel, + &acc); - int num_samples = (int) (traj->getDuration() / sample_density_); - VectorXd p, v, a; - - for(auto i = 0; i < num_samples; ++i) { - double t = i * sample_density_; - - traj->getState(t, &p, &v, &a); - - wheel_pos.col(i) = p; - wheel_vel.col(i) = SE2ToWheelVel(Pose{p(0), p(1), p(2)}, Vel{v(0), v(1), v(2)}); - // this feels hacky but works I think? - wheel_acc.col(i) = SE2ToWheelVel(Pose{p(0), p(1), p(2)}, Vel{a(0), a(1), a(2)}); + if (!traj) { + return std::nullopt; } - std::cout << "Times:\n" << g.times() << std::endl; - std::cout << "Vels:\n" << wheel_vel << std::endl; - - return Goal::createFromWaypoints(g.times(), - wheel_pos, - wheel_vel, - wheel_acc); + queue> retval; + retval.push(traj); + return {retval}; }; } // namespace mobile From 03c9ef41338cc7a6008f73ce62e0edeaf683d91d Mon Sep 17 00:00:00 2001 From: Chris Bollinger Date: Tue, 13 Jul 2021 13:57:40 -0400 Subject: [PATCH 08/15] Commit changes from a few weeks ago --- kits/base/omni_mobile_io_control.cpp | 12 +- util/group_manager.hpp | 198 +-------------- util/group_trajectory_manager.hpp | 346 +++++++++++++++++++++++++++ util/{base.hpp => mobile_base.hpp} | 33 +-- util/omni_base.hpp | 12 +- 5 files changed, 367 insertions(+), 234 deletions(-) create mode 100644 util/group_trajectory_manager.hpp rename util/{base.hpp => mobile_base.hpp} (91%) diff --git a/kits/base/omni_mobile_io_control.cpp b/kits/base/omni_mobile_io_control.cpp index b89f175d..eab1cf5c 100644 --- a/kits/base/omni_mobile_io_control.cpp +++ b/kits/base/omni_mobile_io_control.cpp @@ -24,10 +24,9 @@ int main(int argc, char* argv[]) { mobile = MobileIO::create(family, "mobileIO"); } - std::string instructions; - instructions = ("A1/A2 - Move Base\n" - "A7 - Turn Base\n" - "B8 - Quit\n"); + std::string instructions("A1/A2 - Move Base\n" + "A7 - Turn Base\n" + "B8 - Quit\n"); // Clear any garbage on screen mobile -> clearText(); @@ -35,9 +34,6 @@ int main(int argc, char* argv[]) { // Display instructions on screen mobile -> sendText(instructions); - // Setup instructions - auto last_state = mobile->getState(); - ////////////////////////// //// OmniBase Setup ////// ////////////////////////// @@ -51,6 +47,8 @@ int main(int argc, char* argv[]) { OmniBase base(p); + auto last_state = mobile->getState(); + ////////////////////////// //// Main Control Loop /// ////////////////////////// diff --git a/util/group_manager.hpp b/util/group_manager.hpp index ea94ed14..0ced4994 100644 --- a/util/group_manager.hpp +++ b/util/group_manager.hpp @@ -4,17 +4,11 @@ #include "group.hpp" #include "group_command.hpp" #include "group_feedback.hpp" -#include "robot_model.hpp" -#include "trajectory.hpp" #include "lookup.hpp" -#include "arm/goal.hpp" - namespace hebi { namespace experimental { -using arm::Goal; - class GroupManager { public: @@ -23,7 +17,7 @@ class GroupManager { // Setup functions ////////////////////////////////////////////////////////////////////////////// - // Parameters for creating an arm + // Parameters for creating a group manager struct Params { // The family and names passed to the "lookup" function to find modules // Both are required. @@ -65,11 +59,6 @@ class GroupManager { // Returns the internal group. Not necessary for most use cases. const Group& group() const { return *group_; } - // 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(); } - // Returns the command last computed by update, or an empty command object // if "update" has never successfully run. The returned command can be // modified as desired before it is sent to the robot with the send function. @@ -82,75 +71,16 @@ class GroupManager { const double dT() const { return dt_; } const double lastTime() const { return last_time_; } - ////////////////////////////////////////////////////////////////////////////// - // Main loop functions - // - // Typical usage: - // - // while(true) { - // arm->update(); - // arm->send(); - // } - ////////////////////////////////////////////////////////////////////////////// - - // Updates feedback and generates the basic command for this timestep. + // 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 command last computed by "update" to the robot arm. Any user - // modifications to the command are included. + // Sends the user set command to the group manager. bool send(); - ////////////////////////////////////////////////////////////////////////////// - // Goals - // - // A goal is a desired (joint angle) position that the arm should reach, and - // optionally information about the time it should reach that goal at and the - // path (position, velocity, and acceleration waypoints) it should take to - // get there. - // - // The default behavior when a goal is set is for the arm to plan and begin - // executing a smooth motion from its current state to this goal, with an - // internal heuristic that defines the time at which it will reach the goal. - // This immediately overrides any previous goal that was set. - // - // If there is no "active" goal the arm is set into a mode where it is - // actively controlled to be approximately weightless, and can be moved around - // by hand easily. This is the default state when the arm is created. - // - // After reaching the goal, the arm continues to be commanded with the final - // joint state of the set goal, and is _not_ implicitly returned to a - // "weightless" mode. - // - // A goal may also define "aux" states to be sent to an end effector - // associated with the arm. In this case, the end effector states are - // treated as "step functions", immediately being commanded at the timestamp - // of the waypoint they are associated with. An empty "aux" goal or "NaN" - // defines a "no transition" at the given waypoint. - ////////////////////////////////////////////////////////////////////////////// - - // 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 goalProgress() const; - - // 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(); - protected: std::function get_current_time_s_; double last_time_; @@ -167,21 +97,8 @@ class GroupManager { 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_; }; @@ -248,23 +165,6 @@ bool GroupManager::update() { if (!group_->getNextFeedback(feedback_)) return false; - // 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; } @@ -272,97 +172,5 @@ bool GroupManager::send() { return group_->sendCommand(command_); } -// 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 GroupManager::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_; -} - -double GroupManager::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; -} - -void GroupManager::cancelGoal() { - trajectory_ = nullptr; - trajectory_start_time_ = std::numeric_limits::quiet_NaN(); -} - } // namespace experimental } // namespace hebi diff --git a/util/group_trajectory_manager.hpp b/util/group_trajectory_manager.hpp new file mode 100644 index 00000000..2abc007e --- /dev/null +++ b/util/group_trajectory_manager.hpp @@ -0,0 +1,346 @@ +#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 { + +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); + + // 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_; } + + // 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(); } + + // Returns the command last computed by update, or an empty command object + // if "update" has never successfully run. The returned command can be + // modified as desired before it is sent to the robot with the send function. + 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_; } + + ////////////////////////////////////////////////////////////////////////////// + // Main loop functions + // + // Typical usage: + // + // while(true) { + // arm->update(); + // arm->send(); + // } + ////////////////////////////////////////////////////////////////////////////// + + // Updates feedback and generates the basic command for this timestep. + // 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 command last computed by "update" to the robot arm. Any user + // modifications to the command are included. + bool send(); + + ////////////////////////////////////////////////////////////////////////////// + // Goals + // + // A goal is a desired (joint angle) position that the arm should reach, and + // optionally information about the time it should reach that goal at and the + // path (position, velocity, and acceleration waypoints) it should take to + // get there. + // + // The default behavior when a goal is set is for the arm to plan and begin + // executing a smooth motion from its current state to this goal, with an + // internal heuristic that defines the time at which it will reach the goal. + // This immediately overrides any previous goal that was set. + // + // If there is no "active" goal the arm is set into a mode where it is + // actively controlled to be approximately weightless, and can be moved around + // by hand easily. This is the default state when the arm is created. + // + // After reaching the goal, the arm continues to be commanded with the final + // joint state of the set goal, and is _not_ implicitly returned to a + // "weightless" mode. + // + // A goal may also define "aux" states to be sent to an end effector + // associated with the arm. In this case, the end effector states are + // treated as "step functions", immediately being commanded at the timestamp + // of the waypoint they are associated with. An empty "aux" goal or "NaN" + // defines a "no transition" at the given waypoint. + ////////////////////////////////////////////////////////////////////////////// + + // 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 goalProgress() const; + + // 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(); + +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 arm 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::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 GroupTrajectoryManager::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; + + // 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; +} + +bool GroupTrajectoryManager::send() { + return group_->sendCommand(command_); +} + +// 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_; +} + +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; +} + +void GroupTrajectoryManager::cancelGoal() { + trajectory_ = nullptr; + trajectory_start_time_ = std::numeric_limits::quiet_NaN(); +} + +} // namespace experimental +} // namespace hebi diff --git a/util/base.hpp b/util/mobile_base.hpp similarity index 91% rename from util/base.hpp rename to util/mobile_base.hpp index 3cd49517..2ec60c4e 100644 --- a/util/base.hpp +++ b/util/mobile_base.hpp @@ -24,16 +24,14 @@ using hebi::trajectory::Trajectory; class SE2Point { public: - SE2Point() : x(std::numeric_limits::quiet_NaN()), - y(std::numeric_limits::quiet_NaN()), - theta(std::numeric_limits::quiet_NaN()) {} + SE2Point() {} SE2Point(double x, double y, double theta) : x(x), y(y), theta(theta) {} - double x; - double y; - double 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) { + SE2Point operator+(const SE2Point& rhs) const { SE2Point out; out.x = this->x + rhs.x; out.y = this->y + rhs.y; @@ -41,7 +39,7 @@ class SE2Point { return out; } - SE2Point operator-(const SE2Point& rhs) { + SE2Point operator-(const SE2Point& rhs) const { SE2Point out; out.x = this->x - rhs.x; out.y = this->y - rhs.y; @@ -65,11 +63,6 @@ Pose operator*(const Vel& val, double t) { return out; } -Pose operator+(const Pose& v1, const Pose& v2) { - SE2Point out{ v1.x + v2.x, v1.y + v2.y, v1.theta + v2.theta }; - return out; -} - class Waypoint { public: double t{std::numeric_limits::quiet_NaN()}; @@ -207,7 +200,7 @@ class MobileBase { auto cmd = group_manager_->pendingCommand(); while (base_trajectories_.front() && base_trajectories_.front()->getEndTime() < group_manager_->lastTime()) { - + base_trajectories_.pop(); } // go into compliance mode if no trajectory @@ -223,7 +216,7 @@ class MobileBase { cmd.setPosition(compliantState); cmd.setVelocity(compliantState); - compliantState.setZero(); // TODO: Double check w/ Matt this is supposed to be zero not NaN + compliantState.setZero(); cmd.setEffort(compliantState); return ret; @@ -280,12 +273,6 @@ class MobileBase { // trajectory to the goal. // Note -- this turns the `Goal` into a `Trajectory` based on the // `getMaxVelocity` function implementation - // NOTE/TODO: alternatively, could pass a "Base" into the - // Goal factory methods to convert to trajectory goal there. - // Cleaner from the organization of the "Goal" class, but - // messier otherwise -- e.g., user passing base into a function - // that they are about to pass into base: - // base.setGoal(Goal.createFrom(blah, base)); // TODO: "best effort" -- or return "can't do this"? bool setGoal(const CartesianGoal& g) { if (!group_manager_) { @@ -305,10 +292,6 @@ class MobileBase { // When cleared, the base should be passive / compliant if possible. void clearGoal(); - double goalProgress() const { - return group_manager_->goalProgress(); - } - protected: // A cartesian trajectory is the only thing an individual base // implementation must handle. This should be smoothly moved diff --git a/util/omni_base.hpp b/util/omni_base.hpp index eb4c6964..872cece3 100644 --- a/util/omni_base.hpp +++ b/util/omni_base.hpp @@ -1,8 +1,6 @@ #pragma once -#include "base.hpp" - -#include "trajectory.hpp" +#include "mobile_base.hpp" namespace hebi { namespace experimental { @@ -31,6 +29,10 @@ class OmniBase : public MobileBase { // 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 + optional>> buildTrajectory(const CartesianGoal& g) override; + private: Eigen::Matrix3d createJacobian() { @@ -55,10 +57,6 @@ class OmniBase : public MobileBase { return j_inv; } - // TODO: think about limitations on (x,y) vs. (x,y,theta) - // trajectories - virtual optional>> buildTrajectory(const CartesianGoal& g) override; - // 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(); From 55e03181854219494024cf56bf5160f021ac9ee4 Mon Sep 17 00:00:00 2001 From: Chris Bollinger Date: Tue, 13 Jul 2021 15:57:55 -0400 Subject: [PATCH 09/15] Strange build issue --- kits/base/omni_waypoints.cpp | 2 +- projects/cmake/CMakeLists.txt | 2 +- util/group_trajectory_manager.hpp | 71 +------------------------------ util/mobile_base.hpp | 32 ++++++++------ util/omni_base.hpp | 12 +++--- 5 files changed, 29 insertions(+), 90 deletions(-) diff --git a/kits/base/omni_waypoints.cpp b/kits/base/omni_waypoints.cpp index 31d973b3..7677e30a 100644 --- a/kits/base/omni_waypoints.cpp +++ b/kits/base/omni_waypoints.cpp @@ -61,7 +61,7 @@ int main(int argc, char* argv[]) { base.send(); // if the trajectory has been completed, start another square - if (base.goalProgress() > 0.99) { + if (base.goalComplete()) { base.setGoal(goal); } } diff --git a/projects/cmake/CMakeLists.txt b/projects/cmake/CMakeLists.txt index 368ef9ae..b651647f 100644 --- a/projects/cmake/CMakeLists.txt +++ b/projects/cmake/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.0) project(hebi_cpp_examples) -SET (CMAKE_CXX_STANDARD 17) +SET (CMAKE_CXX_STANDARD 14) SET (CMAKE_CXX_STANDARD_REQUIRED ON) if("${CMAKE_BUILD_TYPE}" STREQUAL "") diff --git a/util/group_trajectory_manager.hpp b/util/group_trajectory_manager.hpp index 2abc007e..db37b0bb 100644 --- a/util/group_trajectory_manager.hpp +++ b/util/group_trajectory_manager.hpp @@ -16,7 +16,7 @@ namespace experimental { using arm::Goal; -class GroupTrajectoryManager { +class GroupTrajectoryManager: GroupMananger { public: @@ -28,86 +28,17 @@ class GroupTrajectoryManager { // mode. static std::unique_ptr create(const GroupManager::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_; } - // 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(); } - // Returns the command last computed by update, or an empty command object - // if "update" has never successfully run. The returned command can be - // modified as desired before it is sent to the robot with the send function. - 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_; } - - ////////////////////////////////////////////////////////////////////////////// - // Main loop functions - // - // Typical usage: - // - // while(true) { - // arm->update(); - // arm->send(); - // } - ////////////////////////////////////////////////////////////////////////////// - - // Updates feedback and generates the basic command for this timestep. - // 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 command last computed by "update" to the robot arm. Any user - // modifications to the command are included. - bool send(); - ////////////////////////////////////////////////////////////////////////////// // Goals - // - // A goal is a desired (joint angle) position that the arm should reach, and - // optionally information about the time it should reach that goal at and the - // path (position, velocity, and acceleration waypoints) it should take to - // get there. - // - // The default behavior when a goal is set is for the arm to plan and begin - // executing a smooth motion from its current state to this goal, with an - // internal heuristic that defines the time at which it will reach the goal. - // This immediately overrides any previous goal that was set. - // - // If there is no "active" goal the arm is set into a mode where it is - // actively controlled to be approximately weightless, and can be moved around - // by hand easily. This is the default state when the arm is created. - // - // After reaching the goal, the arm continues to be commanded with the final - // joint state of the set goal, and is _not_ implicitly returned to a - // "weightless" mode. - // - // A goal may also define "aux" states to be sent to an end effector - // associated with the arm. In this case, the end effector states are - // treated as "step functions", immediately being commanded at the timestamp - // of the waypoint they are associated with. An empty "aux" goal or "NaN" - // defines a "no transition" at the given waypoint. ////////////////////////////////////////////////////////////////////////////// // Set the current goal waypoint(s), immediately replanning to these diff --git a/util/mobile_base.hpp b/util/mobile_base.hpp index 2ec60c4e..93aef9d0 100644 --- a/util/mobile_base.hpp +++ b/util/mobile_base.hpp @@ -1,6 +1,5 @@ #pragma once -#include #include #include "group.hpp" @@ -17,7 +16,6 @@ namespace hebi { namespace experimental { namespace mobile { -using std::optional; using std::queue; using std::shared_ptr; using hebi::trajectory::Trajectory; @@ -55,8 +53,8 @@ class SE2Point { }; -typedef SE2Point Pose; -typedef SE2Point Vel; +using Pose = SE2Point; +using Vel = SE2Point; Pose operator*(const Vel& val, double t) { SE2Point out{val.x*t, val.y*t, val.theta*t}; @@ -113,7 +111,7 @@ struct CartesianGoal { end.t = goalTime; end.pos = p; - createFromWaypoints({start, end}, isBaseFrame); + return createFromWaypoints({start, end}, isBaseFrame); } static CartesianGoal createFromWaypoints(std::vector waypoints, bool isBaseFrame) { @@ -171,8 +169,6 @@ struct CartesianGoal { const Eigen::Matrix positions_{0, 0}; const Eigen::Matrix velocities_{0, 0}; const Eigen::Matrix accelerations_{0, 0}; - - bool local_trajectory_{}; }; ////////////////////////////// @@ -281,16 +277,28 @@ class MobileBase { return false; } - auto baseTrajectory = buildTrajectory(g); - if (baseTrajectory.has_value()) { - base_trajectories_ = baseTrajectory.value(); + auto baseTrajectories = buildTrajectory(g); + if (baseTrajectories) { + base_trajectories_ = *baseTrajectories; return true; } return false; } // When cleared, the base should be passive / compliant if possible. - void clearGoal(); + void clearGoal() { + // Taken from https://stackoverflow.com/a/709161 + // clear the trajectory queue + std::queue>().swap(base_trajectories_); + } + + bool goalComplete() { + if (base_trajectories_.empty()) + return true; + if (base_trajectories_.back()->getEndTime() < group_manager_->lastTime()) + return true; + return false; + }; protected: // A cartesian trajectory is the only thing an individual base @@ -308,7 +316,7 @@ class MobileBase { // state of trajectory -- should we just assume first waypoint // of trajectory is not at time 0, and add first point based on // current command (or feedback if not active) at time 0?) - virtual optional>> buildTrajectory(const CartesianGoal& g) = 0; + virtual std::unique_ptr>> buildTrajectory(const CartesianGoal& g) = 0; virtual void updateOdometry(const Eigen::VectorXd& wheel_vel, double dt) = 0; diff --git a/util/omni_base.hpp b/util/omni_base.hpp index 872cece3..5bb522b1 100644 --- a/util/omni_base.hpp +++ b/util/omni_base.hpp @@ -31,7 +31,7 @@ class OmniBase : public MobileBase { // TODO: think about limitations on (x,y) vs. (x,y,theta) // trajectories - optional>> buildTrajectory(const CartesianGoal& g) override; + std::unique_ptr>> buildTrajectory(const CartesianGoal& g) override; private: @@ -135,7 +135,7 @@ void OmniBase::updateOdometry(const Eigen::VectorXd& wheel_vel, double dt) { global_pose_ += global_vel_ * dt; }; -optional>> OmniBase::buildTrajectory(const CartesianGoal& g) { +std::unique_ptr>> OmniBase::buildTrajectory(const CartesianGoal& g) { auto num_waypoints = g.times().size(); auto num_wheels = g.positions().row(0).size(); @@ -153,12 +153,12 @@ optional>> OmniBase::buildTrajectory(const Cartesia &acc); if (!traj) { - return std::nullopt; + return nullptr; } - queue> retval; - retval.push(traj); - return {retval}; + auto retval = std::make_unique>>(); + retval->push(traj); + return retval; }; } // namespace mobile From 58b1356bfa343f3a04e3a4566f181c57796cb5ad Mon Sep 17 00:00:00 2001 From: Chris Bollinger Date: Wed, 14 Jul 2021 13:48:58 -0400 Subject: [PATCH 10/15] auto doesn't work for references :\ --- util/mobile_base.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util/mobile_base.hpp b/util/mobile_base.hpp index 93aef9d0..6bb1872b 100644 --- a/util/mobile_base.hpp +++ b/util/mobile_base.hpp @@ -193,7 +193,7 @@ class MobileBase { auto vel = group_manager_->lastFeedback().getVelocity(); updateOdometry(vel, group_manager_->dT()); - auto cmd = group_manager_->pendingCommand(); + auto& cmd = group_manager_->pendingCommand(); while (base_trajectories_.front() && base_trajectories_.front()->getEndTime() < group_manager_->lastTime()) { base_trajectories_.pop(); From a7e2b4bd4bd755e27fb6a9c863f382127a098504 Mon Sep 17 00:00:00 2001 From: Chris Bollinger Date: Wed, 14 Jul 2021 14:18:09 -0400 Subject: [PATCH 11/15] GroupTrajectoryManager inherits from GroupManager --- util/group_manager.hpp | 12 +++---- util/group_trajectory_manager.hpp | 53 ++++++++----------------------- 2 files changed, 19 insertions(+), 46 deletions(-) diff --git a/util/group_manager.hpp b/util/group_manager.hpp index 0ced4994..96841530 100644 --- a/util/group_manager.hpp +++ b/util/group_manager.hpp @@ -12,11 +12,6 @@ namespace experimental { class GroupManager { public: - - ////////////////////////////////////////////////////////////////////////////// - // Setup functions - ////////////////////////////////////////////////////////////////////////////// - // Parameters for creating a group manager struct Params { // The family and names passed to the "lookup" function to find modules @@ -40,6 +35,10 @@ class GroupManager { }; }; + ////////////////////////////////////////////////////////////////////////////// + // Setup functions + ////////////////////////////////////////////////////////////////////////////// + // Creates an "Trajectory Follower" object, and puts it into a "weightless" no-goal control // mode. static std::unique_ptr create(const Params& params); @@ -90,7 +89,7 @@ class GroupManager { hebi::GroupFeedback feedback_; hebi::GroupCommand command_; - // Private arm constructor + // Private constructor GroupManager( std::function get_current_time_s, std::shared_ptr group): @@ -101,7 +100,6 @@ class GroupManager { command_(group->size()) {} }; - std::unique_ptr GroupManager::create(const GroupManager::Params& params) { // Get the group (scope the lookup object so it is destroyed diff --git a/util/group_trajectory_manager.hpp b/util/group_trajectory_manager.hpp index db37b0bb..8811dfe1 100644 --- a/util/group_trajectory_manager.hpp +++ b/util/group_trajectory_manager.hpp @@ -19,7 +19,6 @@ using arm::Goal; class GroupTrajectoryManager: GroupMananger { public: - ////////////////////////////////////////////////////////////////////////////// // Setup functions ////////////////////////////////////////////////////////////////////////////// @@ -51,25 +50,28 @@ class GroupTrajectoryManager: GroupMananger { // // If we have reached the goal, progress is "1". If there is no active goal, // or we have just begun, progress is "0". - double goalProgress() const; + 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(); + void cancelGoal() { + trajectory_ = nullptr; + trajectory_start_time_ = std::numeric_limits::quiet_NaN(); + } 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 arm constructor + // Private constructor GroupTrajectoryManager( std::function get_current_time_s, std::shared_ptr group): @@ -134,15 +136,6 @@ std::unique_ptr GroupTrajectoryManager::create(const Gro // 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::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 GroupTrajectoryManager::update() { double t = get_current_time_s_(); @@ -177,10 +170,6 @@ bool GroupTrajectoryManager::update() { return true; } -bool GroupTrajectoryManager::send() { - return group_->sendCommand(command_); -} - // TODO: think about adding customizability, or at least more intelligence for // the default heuristic. Eigen::VectorXd getWaypointTimes( @@ -258,20 +247,6 @@ void GroupTrajectoryManager::setGoal(const Goal& goal) { trajectory_start_time_ = last_time_; } -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; -} - -void GroupTrajectoryManager::cancelGoal() { - trajectory_ = nullptr; - trajectory_start_time_ = std::numeric_limits::quiet_NaN(); -} } // namespace experimental } // namespace hebi From ac722bfe641c09bb59bd8a504ceb70e8b2ac232a Mon Sep 17 00:00:00 2001 From: Chris Bollinger Date: Wed, 14 Jul 2021 22:00:05 -0400 Subject: [PATCH 12/15] Move back to cpp11 compatible --- kits/base/omni_mobile_io_control.cpp | 13 +++++++++---- kits/base/omni_waypoints.cpp | 19 +++++++++++------- projects/cmake/CMakeLists.txt | 2 +- util/mobile_base.hpp | 9 +++++---- util/omni_base.hpp | 29 ++++++++++++++++------------ 5 files changed, 44 insertions(+), 28 deletions(-) diff --git a/kits/base/omni_mobile_io_control.cpp b/kits/base/omni_mobile_io_control.cpp index eab1cf5c..f34a92b1 100644 --- a/kits/base/omni_mobile_io_control.cpp +++ b/kits/base/omni_mobile_io_control.cpp @@ -45,7 +45,12 @@ int main(int argc, char* argv[]) { std::cout << "Creating Omni Base" << std::endl; - OmniBase base(p); + auto base = OmniBase::create(p); + + if (!base) { + std::cout << "Failed to create base, exiting!" << std::endl; + exit(EXIT_FAILURE); + } auto last_state = mobile->getState(); @@ -53,7 +58,7 @@ int main(int argc, char* argv[]) { //// Main Control Loop /// ////////////////////////// - while(base.update()) + while(base->update()) { auto state = mobile->getState(); MobileIODiff diff(last_state, state); @@ -82,13 +87,13 @@ int main(int argc, char* argv[]) { auto goal = CartesianGoal::createFromVelocity(Vel{dx, dy, dtheta}, 0.5, 0.25); // send goal to base - base.setGoal(goal); + base->setGoal(goal); // Update to the new last_state for mobile device last_state = state; // Send latest commands to the base - base.send(); + base->send(); } // Clear MobileIO text diff --git a/kits/base/omni_waypoints.cpp b/kits/base/omni_waypoints.cpp index 7677e30a..e120530b 100644 --- a/kits/base/omni_waypoints.cpp +++ b/kits/base/omni_waypoints.cpp @@ -19,9 +19,14 @@ int main(int argc, char* argv[]) { std::cout << "Creating Omni Base" << std::endl; - OmniBase base(p); + auto base = OmniBase::create(p); - auto currentPose = base.getOdometry(); + if (!base) { + std::cout << "Failed to create base, exiting!" << std::endl; + exit(EXIT_FAILURE); + } + + auto currentPose = base->getOdometry(); Waypoint wp1; Waypoint wp2; @@ -46,7 +51,7 @@ int main(int argc, char* argv[]) { auto goal = CartesianGoal::createFromWaypoints({wp1, wp2, wp3, wp4}, false); // send goal to base - base.setGoal(goal); + base->setGoal(goal); ////////////////////////// //// Main Control Loop /// @@ -55,14 +60,14 @@ int main(int argc, char* argv[]) { std::cout << "Executing Goal" << std::endl; return -1; - while (base.update()) + while (base->update()) { // Send updated command to the base - base.send(); + base->send(); // if the trajectory has been completed, start another square - if (base.goalComplete()) { - base.setGoal(goal); + if (base->goalComplete()) { + base->setGoal(goal); } } diff --git a/projects/cmake/CMakeLists.txt b/projects/cmake/CMakeLists.txt index b651647f..91f1b9b0 100644 --- a/projects/cmake/CMakeLists.txt +++ b/projects/cmake/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.0) project(hebi_cpp_examples) -SET (CMAKE_CXX_STANDARD 14) +SET (CMAKE_CXX_STANDARD 11) SET (CMAKE_CXX_STANDARD_REQUIRED ON) if("${CMAKE_BUILD_TYPE}" STREQUAL "") diff --git a/util/mobile_base.hpp b/util/mobile_base.hpp index 6bb1872b..76172ea7 100644 --- a/util/mobile_base.hpp +++ b/util/mobile_base.hpp @@ -181,10 +181,6 @@ class MobileBase { struct Params: public GroupManager::Params { }; - MobileBase(Params p) : - group_manager_(GroupManager::create(p)) - {} - // This is a generic implementation that assumes the use of a wheel-space trajectory bool update() { // updates dt and last feedback message @@ -301,6 +297,11 @@ class MobileBase { }; protected: + // protected constructor + MobileBase(std::unique_ptr gm, Params p) : + group_manager_(move(gm)) + {} + // 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. diff --git a/util/omni_base.hpp b/util/omni_base.hpp index 5bb522b1..a45d4243 100644 --- a/util/omni_base.hpp +++ b/util/omni_base.hpp @@ -10,22 +10,29 @@ class OmniBase : public MobileBase { public: // Parameters for creating a base struct Params: public MobileBase::Params { - Params() : wheel_radius(0.0762), base_radius(0.220), sample_density(0.1) {} - double wheel_radius; // m - double base_radius; // m (center of omni to origin of base) - double sample_density; // time between sampled waypoints on cartesian trajectory + double wheel_radius = 0.0762; // m + double base_radius = 0.220; // m (center of omni to origin of base) }; - OmniBase(Params p) : MobileBase(p), - wheel_radius_(p.wheel_radius), - base_radius_(p.base_radius), - sample_density_(p.sample_density) - {} + 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 Eigen::VectorXd SE2ToWheelVel(Pose pos, 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; @@ -78,8 +85,6 @@ class OmniBase : public MobileBase { double wheel_radius_; // m double base_radius_; // m (center of omni to origin of base) - double sample_density_; // sec - const Eigen::Matrix3d jacobian_ = createJacobian(); // Wheel velocities to local (x,y,theta) @@ -156,7 +161,7 @@ std::unique_ptr>> OmniBase::buildTrajectory(const C return nullptr; } - auto retval = std::make_unique>>(); + auto retval = std::unique_ptr>>(new queue>()); retval->push(traj); return retval; }; From 76348e14b61f17106a4bbf92185d81798cef7a59 Mon Sep 17 00:00:00 2001 From: Chris Bollinger Date: Wed, 14 Jul 2021 22:23:40 -0400 Subject: [PATCH 13/15] Address more comments --- util/mobile_base.hpp | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/util/mobile_base.hpp b/util/mobile_base.hpp index 76172ea7..f8c49744 100644 --- a/util/mobile_base.hpp +++ b/util/mobile_base.hpp @@ -69,8 +69,6 @@ class Waypoint { SE2Point acc{}; }; -class CartesianTrajectory; - struct CartesianGoal { // Moves with the given relative body velocity for a certain // number of seconds, then slows to a stop over a certain number @@ -140,15 +138,6 @@ struct CartesianGoal { return CartesianGoal(times, positions, velocities, accelerations); } - // Creates from cartesian trajectory. May be (x,y) or (x,y,theta), but - // (x,y,theta) less likely to be realizable depending on system. - // If "theta" is not defined, is tangent to path. - // (x, y path here could be used for planning path through obstacles, for example) - // \param isBaseFrame is "true" if this is relative to current body frame, - // and "false" for using odometry coordinates. - // TODO - static CartesianGoal createFromTrajectory(CartesianTrajectory t, bool isBaseFrame); - const Eigen::VectorXd& times() const { return times_; } const Eigen::Matrix& positions() const { return positions_; } const Eigen::Matrix& velocities() const { return velocities_; } @@ -267,12 +256,6 @@ class MobileBase { // `getMaxVelocity` function implementation // TODO: "best effort" -- or return "can't do this"? bool setGoal(const CartesianGoal& g) { - if (!group_manager_) { - std::cout << "WARNING: Actuator Controller was not created successfully," - << " cannot track goal!" << std::endl; - return false; - } - auto baseTrajectories = buildTrajectory(g); if (baseTrajectories) { base_trajectories_ = *baseTrajectories; From 2127df2adba280d4548a5ed2c69919a7cd47884d Mon Sep 17 00:00:00 2001 From: Chris Bollinger Date: Wed, 21 Jul 2021 15:33:59 -0400 Subject: [PATCH 14/15] Implement Matt's latest round of comments Minor refactoring to remove duplicated transform math --- kits/base/omni_mobile_io_control.cpp | 2 +- kits/base/omni_waypoints.cpp | 2 +- util/group_manager.hpp | 16 ++--- util/mobile_base.hpp | 70 +++++++----------- util/omni_base.hpp | 104 ++++++++++++++++----------- 5 files changed, 96 insertions(+), 98 deletions(-) diff --git a/kits/base/omni_mobile_io_control.cpp b/kits/base/omni_mobile_io_control.cpp index f34a92b1..107cbe3a 100644 --- a/kits/base/omni_mobile_io_control.cpp +++ b/kits/base/omni_mobile_io_control.cpp @@ -84,7 +84,7 @@ int main(int argc, char* argv[]) { ///////////////// // create Vel Goal for 0.5 second, 0.25 second ramp down - auto goal = CartesianGoal::createFromVelocity(Vel{dx, dy, dtheta}, 0.5, 0.25); + auto goal = CartesianGoal::createFromVelocity(Vel{dx, dy, dtheta}, 0.1, 0.5, 0.25); // send goal to base base->setGoal(goal); diff --git a/kits/base/omni_waypoints.cpp b/kits/base/omni_waypoints.cpp index e120530b..d721995d 100644 --- a/kits/base/omni_waypoints.cpp +++ b/kits/base/omni_waypoints.cpp @@ -48,7 +48,7 @@ int main(int argc, char* argv[]) { wp4.t += 1.0; wp4.pos.x -= 0.5; - auto goal = CartesianGoal::createFromWaypoints({wp1, wp2, wp3, wp4}, false); + auto goal = CartesianGoal::createFromWaypoints({wp1, wp2, wp3, wp4}); // send goal to base base->setGoal(goal); diff --git a/util/group_manager.hpp b/util/group_manager.hpp index 96841530..815a7b0f 100644 --- a/util/group_manager.hpp +++ b/util/group_manager.hpp @@ -22,7 +22,7 @@ class GroupManager { int command_lifetime_ = 100; // Loop rate, in Hz. This is how fast the arm update loop will nominally // run. - double control_frequency_ = 200.f; + 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) @@ -39,8 +39,6 @@ class GroupManager { // Setup functions ////////////////////////////////////////////////////////////////////////////// - // Creates an "Trajectory Follower" object, and puts it into a "weightless" no-goal control - // mode. static std::unique_ptr create(const Params& params); // Loads gains from the given .xml file, and sends them to the module. Returns @@ -58,9 +56,6 @@ class GroupManager { // Returns the internal group. Not necessary for most use cases. const Group& group() const { return *group_; } - // Returns the command last computed by update, or an empty command object - // if "update" has never successfully run. The returned command can be - // modified as desired before it is sent to the robot with the send function. GroupCommand& pendingCommand() { return command_; } const GroupCommand& pendingCommand() const { return command_; } @@ -124,21 +119,22 @@ std::unique_ptr GroupManager::create(const GroupManager::Params& p 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" - GroupFeedback feedback(group->size()); - while (!group->getNextFeedback(feedback)) { + while (!gm->update()) { 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 GroupManager(params.get_current_time_s_, group)); + return gm; } bool GroupManager::loadGains(const std::string& gains_file) diff --git a/util/mobile_base.hpp b/util/mobile_base.hpp index f8c49744..99400fab 100644 --- a/util/mobile_base.hpp +++ b/util/mobile_base.hpp @@ -16,9 +16,8 @@ namespace hebi { namespace experimental { namespace mobile { -using std::queue; -using std::shared_ptr; using hebi::trajectory::Trajectory; +using TrajectoryQueue = std::queue>; class SE2Point { public: @@ -75,11 +74,12 @@ struct CartesianGoal { // 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 cruiseTime, double slowTime) { + static CartesianGoal createFromVelocity(Vel v, double rampTime, double cruiseTime, double slowTime) { Waypoint start; - start.t = 0.1; + start.t = rampTime; start.vel = v; Waypoint slow; @@ -88,31 +88,25 @@ struct CartesianGoal { Waypoint stop; stop.t = cruiseTime + slowTime; - stop.vel.x = 0; - stop.vel.y = 0; - stop.vel.theta = 0; + stop.vel = {0, 0, 0}; + stop.acc = {0, 0, 0}; - return createFromWaypoints({start, slow, stop}, true); + return createFromWaypoints({start, slow, stop}); } // Goes to (x, y, theta) in a certain number of seconds. - // \param isBaseFrame is "true" if this is relative to current body frame, - // and "false" for using odometry coordinates. - static CartesianGoal createFromPosition(Pose p, double goalTime, bool isBaseFrame) { - Waypoint start; + static CartesianGoal createFromPosition(Pose p, double goalTime) { Waypoint end; - start.t = 0.1; - start.pos.x = 0; - start.pos.y = 0; - start.pos.theta = 0; end.t = goalTime; end.pos = p; + end.vel = {0, 0, 0}; + end.acc = {0, 0, 0}; - return createFromWaypoints({start, end}, isBaseFrame); + return createFromWaypoints({end}); } - static CartesianGoal createFromWaypoints(std::vector waypoints, bool isBaseFrame) { + static CartesianGoal createFromWaypoints(std::vector waypoints) { auto count = waypoints.size(); VectorXd times(count); @@ -153,7 +147,6 @@ struct CartesianGoal { velocities_(velocities), accelerations_(accelerations) {} - // TODO: some info here capturing above states/options const Eigen::VectorXd times_{0}; const Eigen::Matrix positions_{0, 0}; const Eigen::Matrix velocities_{0, 0}; @@ -188,12 +181,8 @@ class MobileBase { if (base_trajectories_.size() == 0) { auto size = group_manager_->size(); - VectorXd compliantState; - compliantState.resize(size); double nan = std::numeric_limits::quiet_NaN(); - for (auto i=0; isend(); } - //virtual SE2Point wheelsToSE2(Eigen::VectorXd wheel_state) const = 0; - virtual Eigen::VectorXd SE2ToWheelVel(Pose pos, Vel vel) const = 0; + 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) { - last_wheel_pos_ = group_manager_->lastFeedback().getPosition(); relative_odom_offset_ = p - getOdometry(); } @@ -258,7 +246,7 @@ class MobileBase { bool setGoal(const CartesianGoal& g) { auto baseTrajectories = buildTrajectory(g); if (baseTrajectories) { - base_trajectories_ = *baseTrajectories; + base_trajectories_.swap(*baseTrajectories); return true; } return false; @@ -268,7 +256,7 @@ class MobileBase { void clearGoal() { // Taken from https://stackoverflow.com/a/709161 // clear the trajectory queue - std::queue>().swap(base_trajectories_); + TrajectoryQueue().swap(base_trajectories_); } bool goalComplete() { @@ -282,29 +270,19 @@ class MobileBase { protected: // protected constructor MobileBase(std::unique_ptr gm, Params p) : - group_manager_(move(gm)) - {} + group_manager_(std::move(gm)) + { + last_wheel_pos_ = group_manager_->lastFeedback().getPosition(); + } // 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. - // TODO: maybe a std::vector? - // When "nullopt" is set, this clears t // Creates from cartesian trajectory. May be (x,y) or (x,y,theta), but - // (x,y,theta) less likely to be realizable depending on system. - // If "theta" is not defined, is tangent to path. - // (x, y path here could be used for planning path through obstacles, for example) - // \param isBaseFrame is "true" if this is relative to current body frame, - // and "false" for using odometry coordinates.he active trajectory - // goal. - // TODO: think about how to handle starting state to initial - // state of trajectory -- should we just assume first waypoint - // of trajectory is not at time 0, and add first point based on - // current command (or feedback if not active) at time 0?) - virtual std::unique_ptr>> buildTrajectory(const CartesianGoal& g) = 0; + 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}; + std::unique_ptr group_manager_{nullptr}; // These variables should be updated when updateOdometry is called Pose global_pose_{0, 0, 0}; @@ -313,7 +291,7 @@ class MobileBase { Vel local_vel_{0, 0, 0}; - queue> base_trajectories_{}; + TrajectoryQueue base_trajectories_{}; private: Pose relative_odom_offset_{0, 0, 0}; diff --git a/util/omni_base.hpp b/util/omni_base.hpp index a45d4243..d4354944 100644 --- a/util/omni_base.hpp +++ b/util/omni_base.hpp @@ -6,6 +6,7 @@ namespace hebi { namespace experimental { namespace mobile { + class OmniBase : public MobileBase { public: // Parameters for creating a base @@ -23,7 +24,8 @@ class OmniBase : public MobileBase { return retval; } - virtual Eigen::VectorXd SE2ToWheelVel(Pose pos, Vel vel) const override; + 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: @@ -38,10 +40,20 @@ class OmniBase : public MobileBase { // TODO: think about limitations on (x,y) vs. (x,y,theta) // trajectories - std::unique_ptr>> buildTrajectory(const CartesianGoal& g) override; + std::unique_ptr buildTrajectory(const CartesianGoal& g) override; private: + Eigen::Matrix3d thetaToTF(double theta) const { + Eigen::Matrix3d tf; + tf(0, 0) = std::cos(theta); + tf(0, 1) = -std::sin(theta); + tf(1, 0) = std::cos(theta); + tf(1, 1) = std::sin(theta); + tf(2, 2) = theta; + return tf; + } + Eigen::Matrix3d createJacobian() { double a = sqrt(3)/(2 * wheel_radius_); double b = 1.0 / wheel_radius_; @@ -91,30 +103,30 @@ class OmniBase : public MobileBase { const Eigen::Matrix3d jacobian_inv_ = createJacobianInv(); }; - -Eigen::VectorXd OmniBase::SE2ToWheelVel(Pose pos, Vel vel) const { - // if position isn't provided, just return the local frame velocity - if(std::isnan(pos.x) || std::isnan(pos.y) || std::isnan(pos.theta)) { - Eigen::VectorXd ret(3); - ret(0) = vel.x; - ret(1) = vel.y; - ret(2) = vel.theta; - return jacobian_ * ret; +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; } - double theta = pos.theta; - double dtheta = vel.theta; - - double offset = 1.0; - double ctheta = std::cos(-theta); - double stheta = std::sin(-theta); - double dx = vel.x * ctheta - vel.y * stheta; - double dy = vel.x * stheta + vel.y * ctheta; + Eigen::Vector3d global_vel = thetaToTF(pos.theta) * local_vel; - Eigen::Vector3d local_vel; - local_vel << dx, dy, dtheta; - - return jacobian_ * local_vel; + return jacobian_ * global_vel; }; Vel OmniBase::getMaxVelocity() const { @@ -125,30 +137,42 @@ Vel OmniBase::getMaxVelocity() const { void OmniBase::updateOdometry(const Eigen::VectorXd& wheel_vel, double dt) { // Get local velocities auto local_vel = jacobian_inv_ * wheel_vel; - local_vel_.x = local_vel[0]; - local_vel_.y = local_vel[1]; - local_vel_.theta = local_vel[2]; + local_vel_ = {local_vel(0), local_vel(1), local_vel(2)}; // Get global velocity: - auto c = std::cos(global_pose_.theta); - auto s = std::sin(global_pose_.theta); - global_vel_.x = c * local_vel_.x - s * local_vel_.y; - global_vel_.y = s * local_vel_.x + c * local_vel_.y; - // Theta transforms directly - global_vel_.theta = local_vel_.theta; + 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) { - auto num_waypoints = g.times().size(); - auto num_wheels = g.positions().row(0).size(); +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_.front() && base_trajectories_.front()->getEndTime() < t_now) { + base_trajectories_.pop(); + } - MatrixXd wheel_pos(num_wheels, num_waypoints); - MatrixXd wheel_vel(num_wheels, num_waypoints); - MatrixXd wheel_acc(num_wheels, num_waypoints); + Vel starting_vel; - std::cout << "Build Trajectory" << std::endl; + if (base_trajectories_.empty()) + { + // if no trajectory, use last feedback. + auto v = group_manager_->pendingCommand().getVelocity(); + starting_vel = wheelsToSE2({}, v); + } + else + { + auto traj = base_trajectories_.front(); + Eigen::VectorXd pos; + Eigen::VectorXd vel; + Eigen::VectorXd acc; + traj->getState(t_now, &pos, &vel, &acc); + auto starting_vel = thetaToTF(pos(2)) * vel; + } MatrixXd vel = g.velocities(); MatrixXd acc = g.accelerations(); @@ -161,7 +185,7 @@ std::unique_ptr>> OmniBase::buildTrajectory(const C return nullptr; } - auto retval = std::unique_ptr>>(new queue>()); + std::unique_ptr retval(new TrajectoryQueue()); retval->push(traj); return retval; }; From 6685146c147fdd8b107c906632ff1206749d536a Mon Sep 17 00:00:00 2001 From: Chris Bollinger Date: Fri, 6 Aug 2021 15:17:33 -0400 Subject: [PATCH 15/15] Fixed transform matrix bug --- kits/base/omni_mobile_io_control.cpp | 6 ++-- util/group_trajectory_manager.hpp | 14 ++------- util/mobile_base.hpp | 41 ++++++++++++++++++------ util/omni_base.hpp | 47 ++++++++++++++++++---------- 4 files changed, 67 insertions(+), 41 deletions(-) diff --git a/kits/base/omni_mobile_io_control.cpp b/kits/base/omni_mobile_io_control.cpp index 107cbe3a..8345d266 100644 --- a/kits/base/omni_mobile_io_control.cpp +++ b/kits/base/omni_mobile_io_control.cpp @@ -67,9 +67,9 @@ int main(int argc, char* argv[]) { // Input Handling ///////////////// - auto dy = -1 * state.getAxis(7) / 2.0; - auto dx = state.getAxis(8) / 2.0; - auto dtheta = -1 * state.getAxis(1); + 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) diff --git a/util/group_trajectory_manager.hpp b/util/group_trajectory_manager.hpp index 8811dfe1..648ef50d 100644 --- a/util/group_trajectory_manager.hpp +++ b/util/group_trajectory_manager.hpp @@ -16,7 +16,7 @@ namespace experimental { using arm::Goal; -class GroupTrajectoryManager: GroupMananger { +class GroupTrajectoryManager: GroupManager { public: ////////////////////////////////////////////////////////////////////////////// @@ -138,17 +138,7 @@ std::unique_ptr GroupTrajectoryManager::create(const Gro } bool GroupTrajectoryManager::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; + GroupManager::update(); // Update command from trajectory if (trajectory_) { diff --git a/util/mobile_base.hpp b/util/mobile_base.hpp index 99400fab..f2a8833b 100644 --- a/util/mobile_base.hpp +++ b/util/mobile_base.hpp @@ -165,24 +165,28 @@ class MobileBase { // 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(); - while (base_trajectories_.front() && base_trajectories_.front()->getEndTime() < group_manager_->lastTime()) { - base_trajectories_.pop(); - } - + clearStaleTrajectories(t_now); // go into compliance mode if no trajectory - if (base_trajectories_.size() == 0) { + if (base_trajectories_.empty()) { auto size = group_manager_->size(); - double nan = std::numeric_limits::quiet_NaN(); - VectorXd compliantState = VectorXd::Constant(1, size, nan); + auto nan = std::numeric_limits::quiet_NaN(); + VectorXd compliantState = VectorXd::Constant(size, nan); cmd.setPosition(compliantState); cmd.setVelocity(compliantState); @@ -193,10 +197,10 @@ class MobileBase { } else { auto traj = base_trajectories_.front(); - VectorXd pos, vel, accel; + VectorXd pos(3), vel(3), accel(3); // Update command from trajectory - traj->getState(group_manager_->lastTime(), &pos, &vel, &accel); + traj->getState(t_now - trajectory_start_time_, &pos, &vel, &accel); // set velocity to steer along cartesian trajectory @@ -247,6 +251,8 @@ class MobileBase { 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; @@ -273,6 +279,8 @@ class MobileBase { 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 @@ -292,9 +300,24 @@ class MobileBase { 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; + } + } }; diff --git a/util/omni_base.hpp b/util/omni_base.hpp index d4354944..bf3d074d 100644 --- a/util/omni_base.hpp +++ b/util/omni_base.hpp @@ -45,12 +45,8 @@ class OmniBase : public MobileBase { private: Eigen::Matrix3d thetaToTF(double theta) const { - Eigen::Matrix3d tf; - tf(0, 0) = std::cos(theta); - tf(0, 1) = -std::sin(theta); - tf(1, 0) = std::cos(theta); - tf(1, 1) = std::sin(theta); - tf(2, 2) = theta; + Eigen::Matrix3d tf = Matrix3d::Identity(); + tf.block<2,2>(0,0) = Rotation2D(theta).toRotationMatrix(); return tf; } @@ -152,32 +148,49 @@ std::unique_ptr OmniBase::buildTrajectory(const CartesianGoal& // remove stale trajectories auto t_now = group_manager_->lastTime(); - while (base_trajectories_.front() && base_trajectories_.front()->getEndTime() < t_now) { + while (!base_trajectories_.empty() && base_trajectories_.front()->getEndTime() < t_now) { base_trajectories_.pop(); } - Vel starting_vel; + Vector3d starting_vel; if (base_trajectories_.empty()) { // if no trajectory, use last feedback. auto v = group_manager_->pendingCommand().getVelocity(); - starting_vel = wheelsToSE2({}, v); + 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; - Eigen::VectorXd vel; - Eigen::VectorXd acc; + Eigen::VectorXd pos(3); + Eigen::VectorXd vel(3); + Eigen::VectorXd acc(3); traj->getState(t_now, &pos, &vel, &acc); - auto starting_vel = thetaToTF(pos(2)) * vel; + starting_vel = thetaToTF(pos(2)) * vel; } - MatrixXd vel = g.velocities(); - MatrixXd acc = g.accelerations(); - auto traj = Trajectory::createUnconstrainedQp(g.times(), - g.positions(), + 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);