diff --git a/.github/workflows/linux-ci.yml b/.github/workflows/linux-ci.yml index 49d78e83..49ec2c5f 100644 --- a/.github/workflows/linux-ci.yml +++ b/.github/workflows/linux-ci.yml @@ -17,7 +17,8 @@ jobs: matrix: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. - name: [ubuntu-18.04-gcc-9, ubuntu-18.04-clang-9] +# name: [ubuntu-18.04-gcc-9, ubuntu-18.04-clang-9] #issued with clang-9 on CI, commented out for now. + name: [ubuntu-18.04-gcc-9] build_type: [Debug, Release] build_unstable: [ON] @@ -27,10 +28,11 @@ jobs: compiler: gcc version: "9" - - name: ubuntu-18.04-clang-9 - os: ubuntu-18.04 - compiler: clang - version: "9" +############ issues with clang-9 on CI, commented out for now +# - name: ubuntu-18.04-clang-9 +# os: ubuntu-18.04 +# compiler: clang +# version: "9" steps: - name: Setup Compiler diff --git a/examples/example_spider_walking/main.cpp b/examples/example_spider_walking/main.cpp index 01013165..7023faed 100644 --- a/examples/example_spider_walking/main.cpp +++ b/examples/example_spider_walking/main.cpp @@ -50,14 +50,14 @@ Trajectory getTrajectory(const Robot& robot, size_t repeat) { links.insert(links.end(), even_links.begin(), even_links.end()); const Point3 contact_in_com(0, 0.19, 0); - Phase stationary(1, links, contact_in_com), odd(2, odd_links, contact_in_com), - even(2, even_links, contact_in_com); + auto stationary = boost::make_shared(links, contact_in_com); + auto odd = boost::make_shared(odd_links, contact_in_com); + auto even = boost::make_shared(even_links, contact_in_com); - WalkCycle walk_cycle; - walk_cycle.addPhase(stationary); - walk_cycle.addPhase(even); - walk_cycle.addPhase(stationary); - walk_cycle.addPhase(odd); + FootContactVector states = {stationary, even, stationary, odd}; + std::vector phase_lengths = {1,2,1,2}; + + WalkCycle walk_cycle(states, phase_lengths); return Trajectory(walk_cycle, repeat); } @@ -98,7 +98,7 @@ int main(int argc, char** argv) { double ground_height = 1.0; const Point3 step(0, 0.4, 0); gtsam::NonlinearFactorGraph objectives = - trajectory.contactPointObjectives(robot, Isotropic::Sigma(3, 1e-7), step); + trajectory.contactPointObjectives(robot, Isotropic::Sigma(3, 1e-7), step, ground_height); // Get final time step. int K = trajectory.getEndTimeStep(trajectory.numPhases() - 1); diff --git a/gtdynamics.i b/gtdynamics.i index b6bca487..4c0a2600 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -812,26 +812,10 @@ class Interval { // ContactPointGoals is defined in specializations class Phase { - Phase(size_t num_time_steps); - Phase(size_t num_time_steps, - const std::vector &point_on_links); - Phase(size_t num_time_steps, - const std::vector &links, - const gtsam::Point3 &contact_in_com); - const gtdynamics::PointOnLinks &contactPoints() const; - const gtsam::Point3 &contactPoint(const std::string &link_name) const; + Phase(size_t k_start, size_t k_end, + const boost::shared_ptr &constraints); int numTimeSteps() const; void print(const string &s = ""); - gtsam::NonlinearFactorGraph - contactPointObjectives(const gtdynamics::PointOnLinks &all_contact_points, - const gtsam::Point3 &step, - const gtsam::SharedNoiseModel &cost_model, - size_t k_start, - gtdynamics::ContactPointGoals &cp_goals) const; - std::map - updateContactPointGoals(const gtdynamics::PointOnLinks &all_contact_points, - const gtsam::Point3 &step, - gtdynamics::ContactPointGoals &cp_goals) const; gtsam::Matrix jointMatrix(const gtdynamics::Robot &robot, const gtsam::Values &results, size_t k = 0, double dt) const; @@ -840,21 +824,21 @@ class Phase { #include class WalkCycle { WalkCycle(); - WalkCycle(const std::vector& phases); - void addPhase(const gtdynamics::Phase& phase); + WalkCycle(const std::vector &phase_vector); + WalkCycle(const gtdynamics::FootContactVector &states, + const std::vector &phase_lengths); + const gtdynamics::PointOnLinks& contactPoints() const; + std::vector allPhasesContactPoints() const; + std::vector transitionContactPoints() const; const gtdynamics::Phase& phase(size_t p); const std::vector& phases() const; size_t numPhases() const; - const gtdynamics::PointOnLinks& contactPoints() const; + size_t numTimeSteps() const; + gtdynamics::ContactPointGoals initContactPointGoal( + const gtdynamics::Robot &robot, double ground_height) const; void print(const string& s = "") const; - gtdynamics::ContactPointGoals - initContactPointGoal(const gtdynamics::Robot& robot, - double ground_height) const; - std::vector swingLinks(size_t p) const; - gtsam::NonlinearFactorGraph - contactPointObjectives(const gtsam::Point3 &step, - const gtsam::SharedNoiseModel &cost_model, size_t k_start, - gtdynamics::ContactPointGoals @cp_goals) const; + std::vector getPhaseSwingLinks(size_t p) const; + const gtdynamics::PointOnLinks getPhaseContactPoints(size_t p) const; }; #include @@ -880,12 +864,9 @@ class Trajectory { gtsam::Values multiPhaseInitialValues(const gtdynamics::Robot& robot, double gaussian_noise, double dt) const; std::vector finalTimeSteps() const; - size_t phaseIndex(size_t p) const; const Phase &phase(size_t p) const; size_t getStartTimeStep(size_t p) const; size_t getEndTimeStep(size_t p) const; - const gtdynamics::PointOnLinks &getPhaseContactLinks(size_t p) const; - std::vector getPhaseSwingLinks(size_t p) const; PointGoalFactor pointGoalFactor(const gtdynamics::Robot &robot, const string &link_name, const gtdynamics::PointOnLink &cp, size_t k, diff --git a/gtdynamics/kinematics/Kinematics.h b/gtdynamics/kinematics/Kinematics.h index 25baa59f..3d948e4a 100644 --- a/gtdynamics/kinematics/Kinematics.h +++ b/gtdynamics/kinematics/Kinematics.h @@ -137,6 +137,7 @@ class Kinematics : public Optimizer { /** * @fn Inverse kinematics given a set of contact goals. + * @fn This fuction does inverse kinematics seperately on each slice. * @param context Slice or Interval instance. * @param robot Robot specification from URDF/SDF. * @param contact_goals goals for contact points @@ -148,13 +149,14 @@ class Kinematics : public Optimizer { /** * Interpolate using inverse kinematics: the goals are linearly interpolated. - * @param interval Interval instance + * @param context Interval instance * @param robot Robot specification from URDF/SDF. - * @param contact_goals1 goals for contact points for interval.k_start - * @param contact_goals1 goals for contact points for interval.k_end + * @param contact_goals1 goals for contact points for context.k_start + * @param contact_goals2 goals for contact points for context.k_end * All results are return in values. */ - gtsam::Values interpolate(const Interval& interval, const Robot& robot, + template + gtsam::Values interpolate(const CONTEXT& context, const Robot& robot, const ContactGoals& contact_goals1, const ContactGoals& contact_goals2) const; }; diff --git a/gtdynamics/kinematics/KinematicsInterval.cpp b/gtdynamics/kinematics/KinematicsInterval.cpp index 1cc5465e..1be9ec10 100644 --- a/gtdynamics/kinematics/KinematicsInterval.cpp +++ b/gtdynamics/kinematics/KinematicsInterval.cpp @@ -76,9 +76,11 @@ Values Kinematics::inverse(const Interval& interval, return results; } -Values Kinematics::interpolate(const Interval& interval, const Robot& robot, - const ContactGoals& contact_goals1, - const ContactGoals& contact_goals2) const { +template <> +Values Kinematics::interpolate( + const Interval& interval, const Robot& robot, + const ContactGoals& contact_goals1, + const ContactGoals& contact_goals2) const { Values result; const double dt = 1.0 / (interval.k_start - interval.k_end); // 5 6 7 8 9 [10 for (size_t k = interval.k_start; k <= interval.k_end; k++) { diff --git a/gtdynamics/kinematics/KinematicsPhase.cpp b/gtdynamics/kinematics/KinematicsPhase.cpp new file mode 100644 index 00000000..0be126e2 --- /dev/null +++ b/gtdynamics/kinematics/KinematicsPhase.cpp @@ -0,0 +1,28 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file KinematicsPhase.cpp + * @brief Kinematics for a Phase with fixed contacts. + * @author: Dan Barladeanu, Frank Dellaert + */ + +#include +#include +#include +#include + +namespace gtdynamics { + +using gtsam::NonlinearFactorGraph; +using gtsam::Values; +using std::string; +using std::vector; + + + +} // namespace gtdynamics diff --git a/gtdynamics/kinematics/KinematicsTrajectory.cpp b/gtdynamics/kinematics/KinematicsTrajectory.cpp new file mode 100644 index 00000000..001f345d --- /dev/null +++ b/gtdynamics/kinematics/KinematicsTrajectory.cpp @@ -0,0 +1,92 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file KinematicsInterval.cpp + * @brief Kinematics for an trajectory with fixed contacts. + * @author: Frank Dellaert + */ + +#include +#include +#include +#include +#include + +namespace gtdynamics { + +using gtsam::NonlinearFactorGraph; +using gtsam::Values; +using std::string; +using std::vector; + +template <> +NonlinearFactorGraph Kinematics::graph(const Trajectory& trajectory, + const Robot& robot) const { + NonlinearFactorGraph graph; + for (auto&& phase : trajectory.phases()) { + graph.add(this->graph(phase, robot)); + } + return graph; +} + +template <> +NonlinearFactorGraph Kinematics::pointGoalObjectives( + const Trajectory& trajectory, const ContactGoals& contact_goals) const { + NonlinearFactorGraph graph; + for (auto&& phase : trajectory.phases()) { + graph.add(pointGoalObjectives(phase, contact_goals)); + } + return graph; +} + +template <> +NonlinearFactorGraph Kinematics::jointAngleObjectives( + const Trajectory& trajectory, const Robot& robot) const { + NonlinearFactorGraph graph; + for (auto&& phase : trajectory.phases()) { + graph.add(jointAngleObjectives(phase, robot)); + } + return graph; +} + +template <> +Values Kinematics::initialValues(const Trajectory& trajectory, + const Robot& robot, + double gaussian_noise) const { + Values values; + for (auto&& phase : trajectory.phases()) { + values.insert(initialValues(phase, robot, gaussian_noise)); + } + return values; +} + +template <> +Values Kinematics::inverse( + const Trajectory& trajectory, const Robot& robot, + const ContactGoals& contact_goals) const { + Values results; + for (auto&& phase : trajectory.phases()) { + results.insert(inverse(phase, robot, contact_goals)); + } + return results; +} + +template <> +Values Kinematics::interpolate( + const Trajectory& trajectory, const Robot& robot, + const ContactGoals& contact_goals1, + const ContactGoals& contact_goals2) const { + Values results; + for (auto&& phase : trajectory.phases()) { + results.insert( + interpolate(phase, robot, contact_goals1, contact_goals2)); + } + return results; +} + +} // namespace gtdynamics diff --git a/gtdynamics/utils/ConstraintSpec.h b/gtdynamics/utils/ConstraintSpec.h new file mode 100644 index 00000000..a814ebca --- /dev/null +++ b/gtdynamics/utils/ConstraintSpec.h @@ -0,0 +1,35 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file ConstraintSpec.h + * @brief Base class for constraint specification + * @author: Dan Barladeanu, Frank Dellaert + */ + +#pragma once + +namespace gtdynamics { + +/** + * @class ConstraintSpec is a base class for constraint specification on phases. + * Derived Constraints could be walking, jumping, rocketry etc.. + * Derived Class currently implemented: FootContactConstraintSpec - specific for walking. + */ + +class ConstraintSpec { + public: + /// GTSAM-style print, pure virtual here + virtual void print(const std::string &s) const = 0; + + // destructor + virtual ~ConstraintSpec() = default; +}; + + + +} // namespace gtdynamics \ No newline at end of file diff --git a/gtdynamics/utils/FootContactConstraintSpec.cpp b/gtdynamics/utils/FootContactConstraintSpec.cpp new file mode 100644 index 00000000..9f78d3ca --- /dev/null +++ b/gtdynamics/utils/FootContactConstraintSpec.cpp @@ -0,0 +1,133 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file FootContactConstraintSpec.cpp + * @brief Utility methods for generating FootContactConstraintSpec objects. + * @author: Disha Das, Dan Barladeanu, Frank Dellaert + */ + +#include +#include + +#include + +namespace gtdynamics { + +using gtsam::Matrix; +using gtsam::NonlinearFactorGraph; +using gtsam::Point3; +using std::string; + +FootContactConstraintSpec::FootContactConstraintSpec( + const std::vector &points_on_links) { + for (auto &&point_on_link : points_on_links) { + contact_points_.push_back(point_on_link); + } +} + +FootContactConstraintSpec::FootContactConstraintSpec( + const std::vector &links, + const gtsam::Point3 &contact_in_com) { + for (auto &&link : links) { + contact_points_.emplace_back(link, contact_in_com); + } +} + +bool FootContactConstraintSpec::hasContact(const LinkSharedPtr &link) const { + int link_count = + std::count_if(contact_points_.begin(), contact_points_.end(), + [&](const PointOnLink &contact_point) { + return contact_point.link->name() == link->name(); + }); + return link_count > 0; +} + +const gtsam::Point3 &FootContactConstraintSpec::contactPoint(const std::string &link_name) const { + auto it = std::find_if(contact_points_.begin(), contact_points_.end(), + [&](const PointOnLink &contact_point) { + return contact_point.link->name() == link_name; + }); + if (it == contact_points_.end()) + throw std::runtime_error("Link " + link_name + " has no contact point!"); + else + return (*it).point; +} + +std::ostream &operator<<(std::ostream &os, const FootContactConstraintSpec &phase) { + os << "["; + for (auto &&cp : phase.contactPoints()) { + os << cp.link->name() << ": [" << cp.point.transpose() << "], "; + } + os << "]"; + return os; +} + +void FootContactConstraintSpec::print(const string &s) const { + std::cout << (s.empty() ? s : s + " ") << *this << std::endl; +} + +// Searches a contact_link from ContactGoals object and returns the +// corresponding goal_point +const gtsam::Point3 &pointGoal(const ContactGoals &cp_goals, + const PointOnLink &contact_point) { + for (auto it = cp_goals.begin(); it != cp_goals.end(); ++it) { + if ((*it).point_on_link.link->name() == contact_point.link->name() && + (*it).point_on_link.point == contact_point.point) + return (*it).goal_point; + } + throw std::runtime_error("Contact Point was not found."); +} + +NonlinearFactorGraph FootContactConstraintSpec::contactPointObjectives( + const PointOnLinks &all_contact_points, const Point3 &step, + const gtsam::SharedNoiseModel &cost_model, size_t k_start, + const ContactPointGoals &cp_goals, const size_t ts) const { + NonlinearFactorGraph factors; + + for (auto &&cp : all_contact_points) { + const string &name = cp.link->name(); + const Point3 &cp_goal = cp_goals.at(name); + const bool stance = hasContact(cp.link); + auto goal_trajectory = + stance ? StanceTrajectory(cp_goal, ts) + : SimpleSwingTrajectory(cp_goal, step, ts); + + factors.push_back(PointGoalFactors(cost_model, cp.point, goal_trajectory, + cp.link->id(), k_start)); + } + return factors; +} + +std::vector FootContactConstraintSpec::swingLinks() const { + std::vector phase_swing_links; + for (auto &&kv : contactPoints()) { + if (!hasContact(kv.link)) { + phase_swing_links.push_back(kv.link->name()); + } + } + return phase_swing_links; +} + +ContactPointGoals FootContactConstraintSpec::updateContactPointGoals( + const PointOnLinks &all_contact_points, const Point3 &step, + const ContactPointGoals &cp_goals) const { + ContactPointGoals new_goals; + + // For all "feet", update the goal point with step iff in swing. + for (auto &&cp : all_contact_points) { + const string &name = cp.link->name(); + const Point3 &cp_goal = cp_goals.at(name); + const bool stance = hasContact(cp.link); + // If a contact is not on a stance leg, it is on a swing leg and we advance + // the contact goal by adding the 3-vector `step`. + new_goals.emplace(name, stance ? cp_goal : cp_goal + step); + } + return new_goals; +} + +} // namespace gtdynamics diff --git a/gtdynamics/utils/FootContactConstraintSpec.h b/gtdynamics/utils/FootContactConstraintSpec.h new file mode 100644 index 00000000..97059e28 --- /dev/null +++ b/gtdynamics/utils/FootContactConstraintSpec.h @@ -0,0 +1,104 @@ +/* ---------------------------------------------------------------------------- + * GTDynamics Copyright 2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file FootContactConstraintSpec.h + * @brief Utility methods for generating FootContactConstraintSpec objects. + * @author: Disha Das, Dan Barladeanu, Frank Dellaert + */ + +#pragma once + +#include +#include +#include +#include + +#include + +namespace gtdynamics { +/** + * @class FootContactConstraintSpec class stores information about a robot stance + * and its duration. + */ +using ContactPointGoals = std::map; + +class FootContactConstraintSpec : public ConstraintSpec { + protected: + PointOnLinks contact_points_; ///< Contact Points + + public: + /// Constructor + FootContactConstraintSpec() {}; + + /** + * @fbrief Constructor with all contact points, takes list of PointOnLinks. + * + * @param[in] points_on_links List of link PointOnLinks. + */ + FootContactConstraintSpec(const std::vector &points_on_links); + + /** + * @fbrief Constructor with all contact points, takes a number of links and + * creates same contact points on all links. + * + * @param[in] links List of link pointers. + * @param[in] contact_in_com Point of contact on link. + */ + FootContactConstraintSpec(const std::vector &links, + const gtsam::Point3 &contact_in_com); + + /// Returns all the contact points in the stance + const PointOnLinks &contactPoints() const { return contact_points_; } + + /// Check if phase has a contact for given link. + bool hasContact(const LinkSharedPtr &link) const; + + /// Returns the contact point object of link. + const gtsam::Point3 &contactPoint(const std::string &link_name) const; + + /// Print to stream. + friend std::ostream &operator<<(std::ostream &os, + const FootContactConstraintSpec &phase); + + /// GTSAM-style print, works with wrapper. + void print(const std::string &s) const override; + + /** + * Return PointGoalFactors for all feet as given in cp_goals. + * @param[in] all_contact_points stance *and* swing feet. + * @param[in] step 3D vector to move by + * @param[in] cost_model noise model + * @param[in] k_start Factors are added at this time step + * @param[in] cp_goals either stance goal or start of swing (updated) + * @param[in] ts number of time steps + */ + gtsam::NonlinearFactorGraph contactPointObjectives( + const PointOnLinks &all_contact_points, const gtsam::Point3 &step, + const gtsam::SharedNoiseModel &cost_model, size_t k_start, + const ContactPointGoals &cp_goals, const size_t ts) const; + + /** + * @fn Returns the swing links during this FootContact + * @return Vector of swing links. + */ + std::vector swingLinks() const; + + /** + * Update goal points by `step` for all swing legs. + * @param[in] all_contact_points stance *and* swing feet. + * @param[in] step 3D vector to move by + * @param[in] cp_goals either stance goal or start of swing + */ + ContactPointGoals updateContactPointGoals( + const PointOnLinks &all_contact_points, const gtsam::Point3 &step, + const ContactPointGoals &cp_goals) const; +}; + +using FootContactVector = std::vector>; + +} // namespace gtdynamics diff --git a/gtdynamics/utils/Interval.h b/gtdynamics/utils/Interval.h index 1bf1dceb..00ffc4c8 100644 --- a/gtdynamics/utils/Interval.h +++ b/gtdynamics/utils/Interval.h @@ -22,6 +22,8 @@ struct Interval { /// Constructor. explicit Interval(size_t k_start = 0, size_t k_end = 1) : k_start(k_start), k_end(k_end) {} + + size_t numTimeSteps() const { return (k_end - k_start);} }; } // namespace gtdynamics diff --git a/gtdynamics/utils/Phase.cpp b/gtdynamics/utils/Phase.cpp index 250dc2b0..c05b81a6 100644 --- a/gtdynamics/utils/Phase.cpp +++ b/gtdynamics/utils/Phase.cpp @@ -23,24 +23,8 @@ using gtsam::NonlinearFactorGraph; using gtsam::Point3; using std::string; -Phase::Phase(size_t num_time_steps, - const std::vector &points_on_links) - : num_time_steps_(num_time_steps), contact_points_(points_on_links) {} - -Phase::Phase(size_t num_time_steps, const std::vector &links, - const gtsam::Point3 &contact_in_com) - : num_time_steps_(num_time_steps) { - for (auto &&link : links) { - contact_points_.emplace_back(link, contact_in_com); - } -} - std::ostream &operator<<(std::ostream &os, const Phase &phase) { - os << "["; - for (auto &&cp : phase.contactPoints()) { - os << cp.link->name() << ": [" << cp.point.transpose() << "], "; - } - os << "]"; + os << phase.constraint_spec_; return os; } @@ -48,48 +32,12 @@ void Phase::print(const string &s) const { std::cout << (s.empty() ? s : s + " ") << *this << std::endl; } -NonlinearFactorGraph Phase::contactPointObjectives( - const PointOnLinks &all_contact_points, const Point3 &step, - const gtsam::SharedNoiseModel &cost_model, size_t k_start, - const ContactPointGoals &cp_goals) const { - NonlinearFactorGraph factors; - - for (auto &&cp : all_contact_points) { - const string &name = cp.link->name(); - const Point3 &cp_goal = cp_goals.at(name); - const bool stance = hasContact(cp.link); - auto goal_trajectory = - stance ? StanceTrajectory(cp_goal, num_time_steps_) - : SimpleSwingTrajectory(cp_goal, step, num_time_steps_); - - factors.push_back(PointGoalFactors(cost_model, cp.point, goal_trajectory, - cp.link->id(), k_start)); - } - return factors; -} - -ContactPointGoals Phase::updateContactPointGoals( - const PointOnLinks &all_contact_points, const Point3 &step, - const ContactPointGoals &cp_goals) const { - ContactPointGoals new_goals; - - // For all "feet", update the goal point with step iff in swing. - for (auto &&cp : all_contact_points) { - const string &name = cp.link->name(); - const Point3 &cp_goal = cp_goals.at(name); - const bool stance = hasContact(cp.link); - // If a contact is not on a stance leg, it is on a swing leg and we advance - // the contact goal by adding the 3-vector `step`. - new_goals.emplace(name, stance ? cp_goal : cp_goal + step); - } - return new_goals; -} - Matrix Phase::jointMatrix(const Robot &robot, const gtsam::Values &results, size_t k, boost::optional dt) const { const auto &joints = robot.joints(); const size_t J = joints.size(); - const int m = numTimeSteps(), n = 4 * J + (dt ? 1 : 0); + const int m = numTimeSteps(); + const int n = 4 * J + (dt ? 1 : 0); Matrix table(m, n); for (int i = 0; i < m; i++) { size_t j = 0; diff --git a/gtdynamics/utils/Phase.h b/gtdynamics/utils/Phase.h index 16acd1ee..68784106 100644 --- a/gtdynamics/utils/Phase.h +++ b/gtdynamics/utils/Phase.h @@ -14,8 +14,10 @@ #pragma once #include -#include #include +#include +#include +#include #include @@ -24,93 +26,28 @@ namespace gtdynamics { * @class Phase class stores information about a robot stance * and its duration. */ -using ContactPointGoals = std::map; -class Phase { - public: +class Phase : public Interval { protected: - size_t num_time_steps_; ///< Number of time steps in this phase - PointOnLinks contact_points_; ///< Contact Points + boost::shared_ptr constraint_spec_; public: /// Constructor - Phase(size_t num_time_steps) : num_time_steps_(num_time_steps) {} - - /** - * @fbrief Constructor with all contact points, takes list of PointOnLinks. - * - * @param[in] num_time_steps Number of time steps in phase. - * @param[in] points_on_links List of link PointOnLinks. - */ - Phase(size_t num_time_steps, const std::vector &points_on_links); - - /** - * @fbrief Constructor with all contact points, takes a number of links and - * creates same contact points on all links. - * - * @param[in] num_time_steps Number of time steps in phase. - * @param[in] links List of link pointers. - * @param[in] contact_in_com Point of contact on link. - */ - Phase(size_t num_time_steps, const std::vector &links, - const gtsam::Point3 &contact_in_com); + Phase(size_t k_start, size_t k_end, + const boost::shared_ptr &constraint_spec) + : Interval(k_start, k_end), constraint_spec_(constraint_spec) {} - /// Returns all the contact points in the stance - const PointOnLinks &contactPoints() const { return contact_points_; } - - /// Check if phase has a contact for given link. - bool hasContact(const LinkSharedPtr &link) const { - int link_count = - std::count_if(contact_points_.begin(), contact_points_.end(), - [&](const PointOnLink &contact_point) { - return contact_point.link->name() == link->name(); - }); - return link_count > 0; + ///Return Constraint Spec pointer + const boost::shared_ptr constraintSpec() const { + return constraint_spec_; } - /// Returns the contact point object of link. - const gtsam::Point3 &contactPoint(const std::string &link_name) const { - auto it = std::find_if(contact_points_.begin(), contact_points_.end(), - [&](const PointOnLink &contact_point) { - return contact_point.link->name() == link_name; - }); - if (it == contact_points_.end()) - throw std::runtime_error("Link " + link_name + " has no contact point!"); - else - return (*it).point; - } - - /// Returns the number of time steps in this phase - int numTimeSteps() const { return num_time_steps_; } - /// Print to stream. friend std::ostream &operator<<(std::ostream &os, const Phase &phase); /// GTSAM-style print, works with wrapper. void print(const std::string &s) const; - /** - * Add PointGoalFactors for all feet as given in cp_goals. - * @param[in] all_contact_points stance *and* swing feet. - * @param[in] step 3D vector to move by - * @param[in] cost_model noise model - * @param[in] k_start Factors are added at this time step - * @param[in] cp_goals either stance goal or start of swing - */ - gtsam::NonlinearFactorGraph contactPointObjectives( - const PointOnLinks &all_contact_points, const gtsam::Point3 &step, - const gtsam::SharedNoiseModel &cost_model, size_t k_start, - const ContactPointGoals &cp_goals) const; - - /** - * Update goal points by `step` for all swing legs. - * @param[in] step 3D vector to move by - * @param[in] cp_goals either stance goal or start of swing - */ - ContactPointGoals updateContactPointGoals( - const PointOnLinks &all_contact_points, const gtsam::Point3 &step, - const ContactPointGoals &cp_goals) const; - /// Parse results into a matrix, in order: qs, qdots, qddots, taus, dt gtsam::Matrix jointMatrix(const Robot &robot, const gtsam::Values &results, size_t k = 0, diff --git a/gtdynamics/utils/Trajectory.cpp b/gtdynamics/utils/Trajectory.cpp index 58898c39..5344d0c6 100644 --- a/gtdynamics/utils/Trajectory.cpp +++ b/gtdynamics/utils/Trajectory.cpp @@ -81,20 +81,18 @@ Values Trajectory::multiPhaseInitialValues(const Robot &robot, } NonlinearFactorGraph Trajectory::contactPointObjectives( - const Robot &robot, const SharedNoiseModel &cost_model, const Point3 &step, - double ground_height) const { + const Robot &robot, const SharedNoiseModel &cost_model, const Point3 &step, double ground_height) const { NonlinearFactorGraph factors; + // Create a walk cycle using all phases of trajectory + WalkCycle walk_cycle = WalkCycle(phases_); + // Initialize contact point goals. - ContactPointGoals cp_goals = - walk_cycle_.initContactPointGoal(robot, ground_height); + ContactPointGoals cp_goals = walk_cycle.initContactPointGoal(robot, ground_height); size_t k_start = 0; - for (int w = 0; w < repeat_; w++) { - factors.add(walk_cycle_.contactPointObjectives(step, cost_model, k_start, - &cp_goals)); - k_start += walk_cycle_.numTimeSteps(); - } + factors = walk_cycle.contactPointObjectives(step, cost_model, k_start, &cp_goals); + return factors; } diff --git a/gtdynamics/utils/Trajectory.h b/gtdynamics/utils/Trajectory.h index 8095af41..490735cc 100644 --- a/gtdynamics/utils/Trajectory.h +++ b/gtdynamics/utils/Trajectory.h @@ -29,22 +29,7 @@ namespace gtdynamics { */ class Trajectory { protected: - size_t repeat_; ///< Number of repetitions of walk cycle - WalkCycle walk_cycle_; ///< Walk Cycle - - /// Gets the intersection between two PointOnLinks objects - static PointOnLinks getIntersection(const PointOnLinks &cps1, - const PointOnLinks &cps2) { - PointOnLinks intersection; - for (auto &&cp1 : cps1) { - for (auto &&cp2 : cps2) { - if (cp1 == cp2) { - intersection.push_back(cp1); - } - } - } - return intersection; - } + std::vector phases_; ///< All phases in the trajectory public: /// Default Constructor (for serialization) @@ -52,28 +37,36 @@ class Trajectory { /** * Construct trajectory from WalkCycle and specified number of gait - * repetitions. + * repetitions. phases from walk_cycle will be added to Trajectory phases_ + * for repeat times. contact points from all phases then will be added to + * contact_points_. * * @param walk_cycle The Walk Cycle for the robot. * @param repeat The number of repetitions for each phase of the gait. */ - Trajectory(const WalkCycle &walk_cycle, size_t repeat) - : repeat_(repeat), walk_cycle_(walk_cycle) {} + Trajectory(const WalkCycle &walk_cycle, size_t repeat) { + // Get phases of walk_cycle. + auto phases_i = walk_cycle.phases(); + // Loop over `repeat` walk cycles W_i + for (size_t i = 0; i < repeat; i++) { + // Append phases_i of walk_cycle to phases_ vector member. + phases_.insert(phases_.end(), phases_i.begin(), phases_i.end()); + } + } + + /// Returns vector of phases in the trajectory + const std::vector& phases() const { return phases_; } /** * @fn Returns a vector of PointOnLinks objects for all phases after - * applying repetition on the walk cycle. + * applying repetition on the walk cycle. + * This function returns all contact points from all phases + * and may have repetitions, as opposed to contact_points_. * @return Phase CPs. */ std::vector phaseContactPoints() const { - std::vector phase_cps; - const auto &phases = walk_cycle_.phases(); - for (size_t i = 0; i < repeat_; i++) { - for (auto &&phase : phases) { - phase_cps.push_back(phase.contactPoints()); - } - } - return phase_cps; + WalkCycle wc = WalkCycle(phases_); + return wc.allPhasesContactPoints(); } /** @@ -82,34 +75,8 @@ class Trajectory { * @return Transition CPs. */ std::vector transitionContactPoints() const { - std::vector trans_cps_orig; - - auto phases = walk_cycle_.phases(); - PointOnLinks phase_1_cps; - PointOnLinks phase_2_cps; - - for (size_t p = 0; p < walk_cycle_.numPhases(); p++) { - phase_1_cps = phases[p].contactPoints(); - if (p == walk_cycle_.numPhases() - 1) { - phase_2_cps = phases[0].contactPoints(); - } else { - phase_2_cps = phases[p + 1].contactPoints(); - } - - PointOnLinks intersection = getIntersection(phase_1_cps, phase_2_cps); - trans_cps_orig.push_back(intersection); - } - - // Copy the original transition contact point sequence - // `repeat_` number of times. - std::vector trans_cps(trans_cps_orig); - for (size_t i = 0; i < repeat_ - 1; i++) { - trans_cps.insert(trans_cps.end(), trans_cps_orig.begin(), - trans_cps_orig.end()); - } - trans_cps.pop_back(); - - return trans_cps; + WalkCycle wc = WalkCycle(phases_); + return wc.transitionContactPoints(); } /** @@ -119,9 +86,7 @@ class Trajectory { */ std::vector phaseDurations() const { std::vector phase_durations; - const auto &phases = walk_cycle_.phases(); - for (size_t i = 0; i < repeat_; i++) { - for (auto &&phase : phases) + for (auto &&phase : phases_) { phase_durations.push_back(phase.numTimeSteps()); } return phase_durations; @@ -131,7 +96,7 @@ class Trajectory { * @fn Returns the number of phases. * @return Number of phases. */ - size_t numPhases() const { return walk_cycle_.numPhases() * repeat_; } + size_t numPhases() const { return phases_.size(); } /** * @fn Builds vector of Transition Graphs. @@ -181,34 +146,21 @@ class Trajectory { std::vector finalTimeSteps() const { int final_timestep = 0; std::vector final_timesteps; - auto phases = walk_cycle_.phases(); - for (size_t i = 0; i < numPhases(); i++) { - int phase_timestep = phases[i % walk_cycle_.numPhases()].numTimeSteps(); + for (size_t i = 0; i < phases_.size(); i++) { + int phase_timestep = phases_[i].numTimeSteps(); final_timestep += phase_timestep; final_timesteps.push_back(final_timestep); } return final_timesteps; } - /** - * @fn Return phase index for given phase number p. - * @param[in] p Phase number \in [0..repeat * numPhases()[. - * @return Phase instance. - */ - size_t phaseIndex(size_t p) const { - if (p >= numPhases()) { - throw std::invalid_argument("Trajectory:phase: no such phase"); - } - return p % walk_cycle_.numPhases(); - } - /** * @fn Return phase for given phase number p. * @param[in] p Phase number \in [0..repeat * numPhases()[. * @return Phase instance. */ const Phase &phase(size_t p) const { - return walk_cycle_.phase(phaseIndex(p)); + return phases_[p]; } /** @@ -230,24 +182,6 @@ class Trajectory { */ int getEndTimeStep(size_t p) const { return finalTimeSteps()[p]; } - /** - * @fn Returns the contact links for a given phase. - * @param[in] p Phase number. - * @return Vector of contact links. - */ - const PointOnLinks &getPhaseContactLinks(size_t p) const { - return phase(p).contactPoints(); - } - - /** - * @fn Returns the swing links for a given phase. - * @param[in] p Phase number. - * @return Vector of swing links. - */ - std::vector getPhaseSwingLinks(size_t p) const { - return walk_cycle_.swingLinks(phaseIndex(p)); - } - /** * @fn Generates a PointGoalFactor object * @param[in] robot Robot specification from URDF/SDF. @@ -269,6 +203,8 @@ class Trajectory { /** * @fn Create desired stance and swing trajectories for all contact links. + * @fn This function creates a WalkCycle object from all phases in the trajectory + * @fn and uses WalkCycle functionality * @param[in] robot Robot specification from URDF/SDF. * @param[in] cost_model Noise model * @param[in] step The 3D vector the foot moves in a step. @@ -277,7 +213,7 @@ class Trajectory { */ gtsam::NonlinearFactorGraph contactPointObjectives( const Robot &robot, const gtsam::SharedNoiseModel &cost_model, - const gtsam::Point3 &step, double ground_height = 0) const; + const gtsam::Point3 &step, double ground_height = {}) const; /** * @fn Add minimum torque objectives. diff --git a/gtdynamics/utils/WalkCycle.cpp b/gtdynamics/utils/WalkCycle.cpp index 10e0a41b..60bfae38 100644 --- a/gtdynamics/utils/WalkCycle.cpp +++ b/gtdynamics/utils/WalkCycle.cpp @@ -12,26 +12,37 @@ */ #include -#include +#include namespace gtdynamics { -using gtsam::NonlinearFactorGraph; using gtsam::Point3; -using std::string; +using gtsam::NonlinearFactorGraph; +using gtsam::SharedNoiseModel; + +/// cast ConstraintSpec to FootContactConstraintSpec +const boost::shared_ptr +castFootContactConstraintSpec( + const boost::shared_ptr &constraint_spec) { + return boost::dynamic_pointer_cast( + constraint_spec); +} -void WalkCycle::addPhase(const Phase &phase) { +void WalkCycle::addPhaseContactPoints(const Phase &phase) { // Add unique PointOnLink objects to contact_points_ - for (auto &&kv : phase.contactPoints()) { - int link_count = - std::count_if(contact_points_.begin(), contact_points_.end(), - [&](const PointOnLink &contact_point) { - return contact_point.point == kv.point && - contact_point.link == kv.link; - }); - if (link_count == 0) contact_points_.push_back(kv); + auto foot_contact_spec = + castFootContactConstraintSpec(phase.constraintSpec()); + if (foot_contact_spec) { + for (auto &&kv : foot_contact_spec->contactPoints()) { + int link_count = + std::count_if(contact_points_.begin(), contact_points_.end(), + [&](const PointOnLink &contact_point) { + return contact_point.point == kv.point && + contact_point.link == kv.link; + }); + if (link_count == 0) contact_points_.push_back(kv); + } } - phases_.push_back(phase); } const Phase &WalkCycle::phase(size_t p) const { @@ -47,19 +58,6 @@ size_t WalkCycle::numTimeSteps() const { return num_time_steps; } -std::ostream &operator<<(std::ostream &os, const WalkCycle &walk_cycle) { - os << "[\n"; - for (auto &&phase : walk_cycle.phases()) { - os << phase << ",\n"; - } - os << "]"; - return os; -} - -void WalkCycle::print(const string &s) const { - std::cout << (s.empty() ? s : s + " ") << *this << std::endl; -} - ContactPointGoals WalkCycle::initContactPointGoal(const Robot &robot, double ground_height) const { ContactPointGoals cp_goals; @@ -67,13 +65,17 @@ ContactPointGoals WalkCycle::initContactPointGoal(const Robot &robot, // Go over all phases, and all contact points for (auto &&phase : phases_) { - for (auto &&cp : phase.contactPoints()) { - auto link_name = cp.link->name(); - // If no goal set yet, add it here - if (cp_goals.count(link_name) == 0) { - LinkSharedPtr link = robot.link(link_name); - const Point3 foot_w = link->bMcom() * cp.point + adjust; - cp_goals.emplace(link_name, foot_w); + auto foot_contact_spec = + castFootContactConstraintSpec(phase.constraintSpec()); + if (foot_contact_spec) { + for (auto &&cp : foot_contact_spec->contactPoints()) { + auto link_name = cp.link->name(); + // If no goal set yet, add it here + if (cp_goals.count(link_name) == 0) { + LinkSharedPtr link = robot.link(link_name); + const Point3 foot_w = link->bMcom() * cp.point + adjust; + cp_goals.emplace(link_name, foot_w); + } } } } @@ -81,34 +83,106 @@ ContactPointGoals WalkCycle::initContactPointGoal(const Robot &robot, return cp_goals; } -std::vector WalkCycle::swingLinks(size_t p) const { - std::vector phase_swing_links; - const Phase &phase = this->phase(p); - for (auto &&kv : contact_points_) { - if (!phase.hasContact(kv.link)) { - phase_swing_links.push_back(kv.link->name()); +NonlinearFactorGraph WalkCycle::contactPointObjectives( + const Point3 &step, const SharedNoiseModel &cost_model, size_t k_start, + ContactPointGoals *cp_goals) const { + NonlinearFactorGraph factors; + + for (const Phase &phase : phases_) { + auto foot_contact_spec = + castFootContactConstraintSpec(phase.constraintSpec()); + if (foot_contact_spec) { + // Ask the Phase instance to anchor the stance legs + factors.add(foot_contact_spec->contactPointObjectives( + contact_points_, step, cost_model, k_start, *cp_goals, + phase.numTimeSteps())); + + // Update goals for swing legs + *cp_goals = foot_contact_spec->updateContactPointGoals(contact_points_, + step, *cp_goals); + + // update the start time step for the next phase + k_start += phase.numTimeSteps(); + } + } + + return factors; +} + +std::ostream &operator<<(std::ostream &os, const WalkCycle &walk_cycle) { + os << "[\n"; + for (auto &&phase : walk_cycle.phases()) { + os << phase << ",\n"; + } + os << "]"; + return os; +} + +std::vector WalkCycle::getPhaseSwingLinks(size_t p) const { + std::vector phase_swing_links; + const Phase &phase = phases_[p]; + auto foot_contact_spec = + castFootContactConstraintSpec(phase.constraintSpec()); + if (foot_contact_spec) { + for (auto &&kv : contact_points_) { + if (!foot_contact_spec->hasContact(kv.link)) { + phase_swing_links.push_back(kv.link->name()); + } } } return phase_swing_links; } -NonlinearFactorGraph WalkCycle::contactPointObjectives( - const Point3 &step, const gtsam::SharedNoiseModel &cost_model, - size_t k_start, ContactPointGoals *cp_goals) const { - NonlinearFactorGraph factors; +const PointOnLinks WalkCycle::getPhaseContactPoints(size_t p) const { + PointOnLinks cp; + auto foot_contact_spec = + castFootContactConstraintSpec(phases_[p].constraintSpec()); + if (foot_contact_spec) { + cp = foot_contact_spec->contactPoints(); + } + return cp; +} - for (const Phase &phase : phases_) { - // Ask the Phase instance to anchor the stance legs - factors.add(phase.contactPointObjectives(contact_points_, step, cost_model, - k_start, *cp_goals)); - // Update goals for swing legs - *cp_goals = phase.updateContactPointGoals(contact_points_, step, *cp_goals); - - // update the start time step for the next phase - k_start += phase.numTimeSteps(); +std::vector WalkCycle::allPhasesContactPoints() const { + std::vector phase_cps; + for (auto &&phase : phases_) { + auto foot_contact_spec = + castFootContactConstraintSpec(phase.constraintSpec()); + if (foot_contact_spec) { + phase_cps.push_back(foot_contact_spec->contactPoints()); + } } + return phase_cps; +} - return factors; +std::vector WalkCycle::transitionContactPoints() const { + std::vector trans_cps_orig; + + PointOnLinks phase_1_cps; + PointOnLinks phase_2_cps; + + for (size_t p = 0; p < phases_.size() - 1; p++) { + auto foot_contact_spec1 = + castFootContactConstraintSpec(phases_[p].constraintSpec()); + auto foot_contact_spec2 = + castFootContactConstraintSpec(phases_[p + 1].constraintSpec()); + + if (foot_contact_spec1) { + phase_1_cps = foot_contact_spec1->contactPoints(); + } + if (foot_contact_spec2) { + phase_2_cps = foot_contact_spec2->contactPoints(); + } + + PointOnLinks intersection = getIntersection(phase_1_cps, phase_2_cps); + trans_cps_orig.push_back(intersection); + } + + return trans_cps_orig; +} + +void WalkCycle::print(const std::string &s) const { + std::cout << (s.empty() ? s : s + " ") << *this << std::endl; } } // namespace gtdynamics diff --git a/gtdynamics/utils/WalkCycle.h b/gtdynamics/utils/WalkCycle.h index 345eaf39..5aaa3f5b 100644 --- a/gtdynamics/utils/WalkCycle.h +++ b/gtdynamics/utils/WalkCycle.h @@ -16,7 +16,7 @@ #include #include #include -#include +#include #include #include @@ -26,29 +26,97 @@ namespace gtdynamics { /** * @class WalkCycle class stores the sequence of phases - * in a walk cycle. + * in a walk cycle. A WalkCycle is built from FootContactConstraintSpecs + * and phase lengths, and can spawn phases. */ class WalkCycle { protected: std::vector phases_; ///< Phases in walk cycle - PointOnLinks contact_points_; ///< All contact points + PointOnLinks contact_points_; ///< All unique contact points in the walk cycle + + /// Gets the intersection between two PointOnLinks objects + static PointOnLinks getIntersection(const PointOnLinks &cps1, + const PointOnLinks &cps2) { + PointOnLinks intersection; + for (auto &&cp1 : cps1) { + for (auto &&cp2 : cps2) { + if (cp1 == cp2) { + intersection.push_back(cp1); + } + } + } + return intersection; + } public: /// Default Constructor WalkCycle() {} - /// Constructor with phases - explicit WalkCycle(const std::vector& phases) { - for (auto&& phase : phases) { - addPhase(phase); + /** + * @fn Construct a new Walk Cycle object from a vector of phases. + * + * @param states ........... a vector of phases + */ + WalkCycle(const std::vector &phase_vector) { + for (auto&& phase : phase_vector) { + phases_.push_back(phase); + } + + // After all phases in the walk cycle are in phases_, + // add unique contact from each phase in the walk cycle to contact_points_ + for (auto&& phase : phases_) { + addPhaseContactPoints(phase); } } /** - * @fn Adds phase in walk cycle - * @param[in] phase Swing or stance phase in the walk cycle. + * @fn Construct a new Walk Cycle object from a vector of + * FootContactConstraintSpec pointers and phase lengths. + * for each element in the vectors, a Phase will be generated with a + * corresponding FootContactConstraintSpec and phase length. + * + * @param states ........... a vector of FootContactConstraintSpec shared pointers. + * @param phase_lengths .... a vector of phase lengths corresponding to the states vector. */ - void addPhase(const Phase& phase); + WalkCycle(const FootContactVector &states, + const std::vector &phase_lengths) { + if (states.size() != phase_lengths.size()) { + throw std::runtime_error( + "states vector and phase_lengths vector have different sizes"); + } + size_t k = 0; + for (size_t i = 0; i < states.size(); i++) { + Phase ph = Phase(k, k + phase_lengths[i], states[i]); + phases_.push_back(ph); + k += phase_lengths[i]; + } + + // After all phases in the trajectory are in phases_, + // add unique contact from each phase in the trajectory to contact_points_ + for (auto &&phase : phases_) { + addPhaseContactPoints(phase); + } + } + + /** + * @fn adds unique contact points from phase to contact_points_ of trajectory + * + * @param[in] phase.... phase from which to add contact points + */ + void addPhaseContactPoints(const Phase &phase); + + /// Return all unique contact points. + const PointOnLinks& contactPoints() const { return contact_points_; } + + /// Return all contact points from all phases, not only unique + std::vector allPhasesContactPoints() const; + + /** + * @fn Returns a vector of PointOnLinks objects for all transitions between + * phases after applying repetition on the original sequence. + * @return Transition CPs. + */ + std::vector transitionContactPoints() const; /** * @fn Return phase for given phase number p. @@ -66,41 +134,47 @@ class WalkCycle { /// Returns the number of time steps, summing over all phases. size_t numTimeSteps() const; - /// Return all the contact points. - const PointOnLinks& contactPoints() const { return contact_points_; } - - /// Print to stream. - friend std::ostream& operator<<(std::ostream& os, - const WalkCycle& walk_cycle); - - /// GTSAM-style print, works with wrapper. - void print(const std::string& s = "") const; - - /** + /** * @fn Returns the initial contact point goal for every contact link. * @param[in] robot Robot specification from URDF/SDF. * @param[in] ground_height z-coordinate of ground in URDF/SDF rest config. * @return Map from link name to goal points. */ - ContactPointGoals initContactPointGoal(const Robot& robot, - double ground_height = 0.0) const; + ContactPointGoals initContactPointGoal(const Robot &robot, + double ground_height) const; + + /** + * @fn Create desired stance and swing trajectories for all contact links. + * @param[in] robot Robot specification from URDF/SDF. + * @param[in] cost_model Noise model + * @param[in] step The 3D vector the foot moves in a step. + * @param[in] ground_height z-coordinate of ground in URDF/SDF rest config. + * @return All objective factors as a NonlinearFactorGraph + */ + gtsam::NonlinearFactorGraph contactPointObjectives( + const gtsam::Point3 &step, const gtsam::SharedNoiseModel &cost_model, + size_t k_start, ContactPointGoals *cp_goals) const; /** * @fn Returns the swing links for a given phase. - * @param[in]p Phase number. + * @param[in] p Phase number. * @return Vector of swing links. */ - std::vector swingLinks(size_t p) const; + std::vector getPhaseSwingLinks(size_t p) const; /** - * Add PointGoalFactors for all feet as given in cp_goals. - * @param[in] step 3D vector to move by - * @param[in] cost_model noise model - * @param[in] k_start Factors are added at this time step - * @param[inout] cp_goals either stance goal or start of swing (updated) - */ - gtsam::NonlinearFactorGraph contactPointObjectives( - const gtsam::Point3& step, const gtsam::SharedNoiseModel& cost_model, - size_t k_start, ContactPointGoals* cp_goals) const; + * @fn Returns the contact points for a given phase. + * @param[in] p Phase number. + * @return Vector of contact links. + */ + const PointOnLinks getPhaseContactPoints(size_t p) const; + + /// Print to stream. + friend std::ostream& operator<<(std::ostream& os, + const WalkCycle& walk_cycle); + + /// GTSAM-style print, works with wrapper. + void print(const std::string& s = "") const; + }; } // namespace gtdynamics diff --git a/tests/testKinematicsPhase.cpp b/tests/testKinematicsPhase.cpp index befed5f5..2ed8755e 100644 --- a/tests/testKinematicsPhase.cpp +++ b/tests/testKinematicsPhase.cpp @@ -29,7 +29,11 @@ TEST(Phase, InverseKinematics) { using namespace contact_goals_example; constexpr size_t num_time_steps = 5; - Phase phase0(num_time_steps, {LH,RF}, contact_in_com); + const std::vector link_vec = {LH, RF}; + + auto constraint = boost::make_shared(link_vec , contact_in_com); + + Phase phase0(0, num_time_steps, constraint); // TODO(frank): test methods producing constraints. } diff --git a/tests/testPhase.cpp b/tests/testPhase.cpp index e29e56aa..568675c4 100644 --- a/tests/testPhase.cpp +++ b/tests/testPhase.cpp @@ -19,6 +19,7 @@ #include "gtdynamics/universal_robot/Robot.h" #include "gtdynamics/universal_robot/sdf.h" #include "gtdynamics/utils/Phase.h" +#include "gtdynamics/utils/Trajectory.h" #include "walkCycleExample.h" using namespace gtdynamics; @@ -30,26 +31,30 @@ TEST(Phase, All) { CreateRobotFromFile(kSdfPath + std::string("spider.sdf"), "spider"); using namespace walk_cycle_example; - Point3 cp = phase_1.contactPoint("tarsus_3_L3"); + Phase phase1(0, 2, phase_1); + + auto phase1_foot_constraint = boost::static_pointer_cast(phase1.constraintSpec()); + + Point3 cp = phase1_foot_constraint->contactPoint("tarsus_3_L3"); EXPECT(assert_equal(contact_in_com, cp)); - PointOnLinks cps = phase_1.contactPoints(); + PointOnLinks cps = phase1_foot_constraint->contactPoints(); EXPECT_LONGS_EQUAL(3, cps.size()); EXPECT(assert_equal(contact_in_com, cps[0].point)); - EXPECT_LONGS_EQUAL(2, phase_1.numTimeSteps()); + EXPECT_LONGS_EQUAL(2, phase1.numTimeSteps()); // Check printing std::stringstream ss; - ss << phase_1; + ss << *phase_1; EXPECT(std::string("[tarsus_1_L1: [ 0 0.19 0], tarsus_2_L2: [ 0 0.19 " " 0], tarsus_3_L3: [ 0 0.19 0], ]") == ss.str()); // Test hasContact. - EXPECT(phase_1.hasContact(robot.link("tarsus_1_L1"))); - EXPECT(phase_1.hasContact(robot.link("tarsus_2_L2"))); - EXPECT(phase_1.hasContact(robot.link("tarsus_3_L3"))); - EXPECT(!phase_1.hasContact(robot.link("tarsus_4_L4"))); - EXPECT(!phase_1.hasContact(robot.link("tarsus_5_R4"))); + EXPECT(phase1_foot_constraint->hasContact(robot.link("tarsus_1_L1"))); + EXPECT(phase1_foot_constraint->hasContact(robot.link("tarsus_2_L2"))); + EXPECT(phase1_foot_constraint->hasContact(robot.link("tarsus_3_L3"))); + EXPECT(!phase1_foot_constraint->hasContact(robot.link("tarsus_4_L4"))); + EXPECT(!phase1_foot_constraint->hasContact(robot.link("tarsus_5_R4"))); // contactPointObjectives const Point3 step(0, 0.4, 0); @@ -61,10 +66,10 @@ TEST(Phase, All) { {"tarsus_3_L3", goal}, {"tarsus_4_L4", goal}, {"tarsus_5_R4", goal}}; - gtsam::NonlinearFactorGraph factors = phase_1.contactPointObjectives( - walk_cycle.contactPoints(), step, cost_model, k_start, cp_goals); - auto new_goals = phase_1.updateContactPointGoals(walk_cycle.contactPoints(), - step, cp_goals); + gtsam::NonlinearFactorGraph factors = phase1_foot_constraint->contactPointObjectives( + walk_cycle.contactPoints(), step, cost_model, k_start, cp_goals, 2); + auto new_goals = phase1_foot_constraint->updateContactPointGoals(walk_cycle.contactPoints(), step, cp_goals); + EXPECT_LONGS_EQUAL(num_time_steps * 5, factors.size()); EXPECT(assert_equal(goal, new_goals["tarsus_2_L2"])); EXPECT(assert_equal(goal, new_goals["tarsus_1_L1"])); diff --git a/tests/testSpiderWalking.cpp b/tests/testSpiderWalking.cpp index 7f38477b..6c1ae880 100644 --- a/tests/testSpiderWalking.cpp +++ b/tests/testSpiderWalking.cpp @@ -37,26 +37,31 @@ using namespace gtdynamics; EXPECT_DOUBLES_EQUAL(expected, actual, rtol* expected) // Returns a Trajectory object for a single robot walk cycle. -Trajectory getTrajectory(const Robot& robot, size_t repeat) { - vector odd_links = { +Trajectory getTrajectory(const Robot &robot, size_t repeat) { + // Label which feet are what... + vector odd_feet = { robot.link("tarsus_1_L1"), robot.link("tarsus_3_L3"), robot.link("tarsus_5_R4"), robot.link("tarsus_7_R2")}; - vector even_links = { + vector even_feet = { robot.link("tarsus_2_L2"), robot.link("tarsus_4_L4"), robot.link("tarsus_6_R3"), robot.link("tarsus_8_R1")}; - auto links = odd_links; - links.insert(links.end(), even_links.begin(), even_links.end()); + auto all_feet = odd_feet; + all_feet.insert(all_feet.end(), even_feet.begin(), even_feet.end()); + // Create three different FootContactConstraintSpecs, one for all the feet on the + // ground, one with even feet on the ground, one with odd feet in contact.. const Point3 contact_in_com(0, 0.19, 0); - Phase stationary(1, links, contact_in_com), odd(2, odd_links, contact_in_com), - even(2, even_links, contact_in_com); + auto stationary = boost::make_shared(all_feet, contact_in_com); + auto odd = boost::make_shared(odd_feet, contact_in_com); + auto even = boost::make_shared(even_feet, contact_in_com); + + FootContactVector states = {stationary, even, stationary, odd}; + std::vector phase_lengths = {1,2,1,2}; - WalkCycle walk_cycle; - walk_cycle.addPhase(stationary); - walk_cycle.addPhase(even); - walk_cycle.addPhase(stationary); - walk_cycle.addPhase(odd); + WalkCycle walk_cycle(states, phase_lengths); + // TODO(issue #257): Trajectory should *be* a vector of phases, so rather that + // store the walkcycle and repeat, it should store the phases. return Trajectory(walk_cycle, repeat); } @@ -95,7 +100,7 @@ TEST(testSpiderWalking, WholeEnchilada) { // Build the objective factors. const Point3 step(0, 0.4, 0); NonlinearFactorGraph objectives = - trajectory.contactPointObjectives(robot, Isotropic::Sigma(3, 1e-7), step); + trajectory.contactPointObjectives(robot, Isotropic::Sigma(3, 1e-7), step, 0); // per walk cycle: 1*8 + 2*8 + 1*8 + 2*8 = 48 // 2 repeats, hence: EXPECT_LONGS_EQUAL(48 * 2, objectives.size()); diff --git a/tests/testTrajectory.cpp b/tests/testTrajectory.cpp index 70401f46..fa3ee0e9 100644 --- a/tests/testTrajectory.cpp +++ b/tests/testTrajectory.cpp @@ -30,30 +30,6 @@ auto kModel6 = gtsam::noiseModel::Unit::Create(6); using namespace gtdynamics; -// Class to test protected method -class TrajectoryTest : public Trajectory { - public: - TrajectoryTest() : Trajectory(){}; - using Trajectory::getIntersection; -}; - -TEST(Trajectory, Intersection) { - Robot robot = - CreateRobotFromFile(kSdfPath + std::string("spider.sdf"), "spider"); - - using namespace walk_cycle_example; - TrajectoryTest traj; - PointOnLinks intersection = - traj.getIntersection(phase_1.contactPoints(), phase_2.contactPoints()); - - PointOnLinks expected = {{robot.link("tarsus_2_L2"), contact_in_com}, - {robot.link("tarsus_3_L3"), contact_in_com}}; - - for (size_t i = 0; i < 2; i++) { - EXPECT(assert_equal(expected[i], intersection[i])); - } -} - TEST(Trajectory, error) { using namespace walk_cycle_example; Robot robot = @@ -87,12 +63,10 @@ TEST(Trajectory, error) { EXPECT_LONGS_EQUAL(7, final_timesteps[2]); EXPECT_LONGS_EQUAL(6, trajectory.getStartTimeStep(2)); EXPECT_LONGS_EQUAL(7, trajectory.getEndTimeStep(2)); - EXPECT_LONGS_EQUAL(4, trajectory.getPhaseContactLinks(3).size()); - EXPECT_LONGS_EQUAL(1, trajectory.getPhaseSwingLinks(3).size()); - auto cp_goals = walk_cycle.initContactPointGoal(robot); + auto cp_goals = walk_cycle.initContactPointGoal(robot, 0); EXPECT_LONGS_EQUAL(5, cp_goals.size()); - // regression + //regression EXPECT(gtsam::assert_equal(gtsam::Point3(-0.926417, 1.19512, 0.000151302), cp_goals["tarsus_2_L2"], 1e-5)); @@ -127,7 +101,7 @@ TEST(Trajectory, error) { // Test objectives for contact links. const Point3 step(0, 0.4, 0); auto contact_link_objectives = trajectory.contactPointObjectives( - robot, noiseModel::Isotropic::Sigma(3, 1e-7), step); + robot, noiseModel::Isotropic::Sigma(3, 1e-7), step, 0); // steps = 2+3 per walk cycle, 5 legs involved const size_t expected = repeat * ((2 + 3) * 5); EXPECT_LONGS_EQUAL(expected, contact_link_objectives.size()); diff --git a/tests/testWalkCycle.cpp b/tests/testWalkCycle.cpp index 03e1d26f..622cb1d1 100644 --- a/tests/testWalkCycle.cpp +++ b/tests/testWalkCycle.cpp @@ -18,19 +18,44 @@ #include "gtdynamics/universal_robot/sdf.h" #include "gtdynamics/utils/Phase.h" #include "gtdynamics/utils/WalkCycle.h" +#include "gtdynamics/utils/Trajectory.h" #include "walkCycleExample.h" using namespace gtdynamics; using gtsam::Point3; +// Class to test protected method +class WalkCycleTest : public WalkCycle { + public: + WalkCycleTest() : WalkCycle(){}; + using WalkCycle::getIntersection; +}; + +TEST(WalkCycle, Intersection) { + Robot robot = + CreateRobotFromFile(kSdfPath + std::string("spider.sdf"), "spider"); + + using namespace walk_cycle_example; + WalkCycleTest wc; + PointOnLinks intersection = + wc.getIntersection(phase_1->contactPoints(), phase_2->contactPoints()); + + PointOnLinks expected = {{robot.link("tarsus_2_L2"), contact_in_com}, + {robot.link("tarsus_3_L3"), contact_in_com}}; + + for (size_t i = 0; i < 2; i++) { + EXPECT(gtsam::assert_equal(expected[i], intersection[i])); + } +} + TEST(WalkCycle, contactPoints) { Robot robot = CreateRobotFromFile(kSdfPath + std::string("spider.sdf"), "spider"); using namespace walk_cycle_example; auto walk_cycle_phases = walk_cycle.phases(); - EXPECT_LONGS_EQUAL(3, walk_cycle_phases[0].contactPoints().size()); - EXPECT_LONGS_EQUAL(4, walk_cycle_phases[1].contactPoints().size()); + EXPECT_LONGS_EQUAL(3, walk_cycle.getPhaseContactPoints(0).size()); + EXPECT_LONGS_EQUAL(4, walk_cycle.getPhaseContactPoints(1).size()); EXPECT_LONGS_EQUAL(2, walk_cycle.numPhases()); EXPECT_LONGS_EQUAL(num_time_steps + num_time_steps_2, walk_cycle.numTimeSteps()); @@ -44,11 +69,23 @@ TEST(WalkCycle, objectives) { constexpr size_t num_time_steps = 5; const Point3 contact_in_com(0.14, 0, 0); - Phase phase0(num_time_steps, {robot.link("lower1"), robot.link("lower2")}, - contact_in_com), - phase1(num_time_steps, {robot.link("lower0"), robot.link("lower3")}, - contact_in_com); - auto walk_cycle = WalkCycle({phase0, phase1}); + + std::vector phase_0_links = {robot.link("lower1"), robot.link("lower2")}; + std::vector phase_1_links = {robot.link("lower0"), robot.link("lower3")}; + + auto phase0 = boost::make_shared(phase_0_links, contact_in_com); + auto phase1 = boost::make_shared(phase_1_links, contact_in_com); + + FootContactVector states = {phase0, phase1}; + std::vector phase_lengths = {num_time_steps, num_time_steps}; + + auto walk_cycle = WalkCycle({phase0, phase1}, {num_time_steps, num_time_steps}); + + //check Phase swing links function + auto swing_links0 = walk_cycle.getPhaseSwingLinks(0); + auto swing_links1 = walk_cycle.getPhaseSwingLinks(1); + EXPECT_LONGS_EQUAL(swing_links0.size(), 2); + EXPECT_LONGS_EQUAL(swing_links1.size(), 2); // Expected contact goal points. Point3 goal_LH(-0.371306, 0.1575, 0); // LH @@ -66,11 +103,10 @@ TEST(WalkCycle, objectives) { const Point3 step(0, 0.4, 0); const gtsam::SharedNoiseModel cost_model = nullptr; - const size_t k = 777; // Check creation of PointGoalFactors. gtsam::NonlinearFactorGraph factors = - walk_cycle.contactPointObjectives(step, cost_model, k, &cp_goals); + walk_cycle.contactPointObjectives(step, cost_model, 0, &cp_goals); EXPECT_LONGS_EQUAL(num_time_steps * 2 * 4, factors.size()); // Check goals have been updated diff --git a/tests/walkCycleExample.h b/tests/walkCycleExample.h index fae2fb8a..c460f8c5 100644 --- a/tests/walkCycleExample.h +++ b/tests/walkCycleExample.h @@ -27,7 +27,8 @@ const gtsam::Point3 contact_in_com(0, 0.19, 0); const std::vector links_1 = {robot.link("tarsus_1_L1"), robot.link("tarsus_2_L2"), robot.link("tarsus_3_L3")}; -const Phase phase_1(num_time_steps, links_1, contact_in_com); + +const auto phase_1 = boost::make_shared(links_1, contact_in_com); // Second phase constexpr size_t num_time_steps_2 = 3; @@ -35,10 +36,11 @@ const std::vector links_2 = {robot.link("tarsus_2_L2"), robot.link("tarsus_3_L3"), robot.link("tarsus_4_L4"), robot.link("tarsus_5_R4")}; -const Phase phase_2(num_time_steps_2, links_2, contact_in_com); + +const auto phase_2 = boost::make_shared(links_2, contact_in_com); // Initialize walk cycle -const WalkCycle walk_cycle({phase_1, phase_2}); +const WalkCycle walk_cycle({phase_1, phase_2}, {num_time_steps, num_time_steps_2}); } // namespace walk_cycle_example } // namespace gtdynamics \ No newline at end of file