WIP: Refactor trajectory

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<FootContactConstraintSpec>(links, contact_in_com);
+  auto odd = boost::make_shared<FootContactConstraintSpec>(odd_links, contact_in_com);
+  auto even = boost::make_shared<FootContactConstraintSpec>(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<size_t> 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);

gtdynamics.i

@@ -835,26 +835,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<gtdynamics::PointOnLink> &point_on_links);
-  Phase(size_t num_time_steps, 
-        const std::vector<gtdynamics::LinkSharedPtr> &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<gtdynamics::ConstraintSpec> &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<string, gtsam::Point3>
-  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;
@@ -863,21 +847,11 @@ class Phase {
 #include <gtdynamics/utils/WalkCycle.h>
 class WalkCycle {
   WalkCycle();
-  WalkCycle(const std::vector<gtdynamics::Phase>& phases);
-  void addPhase(const gtdynamics::Phase& phase);
   const gtdynamics::Phase& phase(size_t p);
   const std::vector<gtdynamics::Phase>& phases() const;
   size_t numPhases() const;
-  const gtdynamics::PointOnLinks& contactPoints() const;
   void print(const string& s = "") const;
-  gtdynamics::ContactPointGoals
-  initContactPointGoal(const gtdynamics::Robot& robot, 
-                       double ground_height) const;
-  std::vector<string> 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<string> getPhaseSwingLinks(size_t p) const;
 };
 
 #include <gtdynamics/utils/Trajectory.h>
@@ -903,12 +877,10 @@ class Trajectory {
   gtsam::Values multiPhaseInitialValues(const gtdynamics::Robot& robot, 
                                         double gaussian_noise, double dt) const;
   std::vector<int> 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<string> getPhaseSwingLinks(size_t p) const;
   PointGoalFactor pointGoalFactor(const gtdynamics::Robot &robot,
                                   const string &link_name,
                                   const gtdynamics::PointOnLink &cp, size_t k,

gtdynamics/kinematics/Kinematics.h

@@ -154,7 +154,8 @@ class Kinematics : public Optimizer {
    * @param contact_goals1 goals for contact points for interval.k_end
    * All results are return in values.
    */
-  gtsam::Values interpolate(const Interval& interval, const Robot& robot,
+  template <class CONTEXT>
+  gtsam::Values interpolate(const CONTEXT& context, const Robot& robot,
                             const ContactGoals& contact_goals1,
                             const ContactGoals& contact_goals2) const;
 };

gtdynamics/kinematics/KinematicsInterval.cpp

@@ -76,9 +76,10 @@ Values Kinematics::inverse<Interval>(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<Interval>(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++) {

gtdynamics/kinematics/KinematicsPhase.cpp

@@ -0,0 +1,83 @@
+/* ----------------------------------------------------------------------------
+ * 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 <gtdynamics/kinematics/Kinematics.h>
+#include <gtdynamics/utils/Phase.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+
+namespace gtdynamics {
+
+using gtsam::NonlinearFactorGraph;
+using gtsam::Values;
+using std::string;
+using std::vector;
+
+template <>
+NonlinearFactorGraph Kinematics::graph<Phase>(const Phase& phase,
+                                                 const Robot& robot) const {
+  NonlinearFactorGraph graph;
+  Interval interval = static_cast<Interval>(phase);
+  graph.add(this->graph(interval, robot));
+  return graph;
+}
+
+template <>
+NonlinearFactorGraph Kinematics::pointGoalObjectives<Phase>(
+    const Phase& phase, const ContactGoals& contact_goals) const {
+  NonlinearFactorGraph graph;
+  Interval interval = static_cast<Interval>(phase);
+  graph.add(pointGoalObjectives(interval, contact_goals));
+  return graph;
+}
+
+template <>
+NonlinearFactorGraph Kinematics::jointAngleObjectives<Phase>(
+    const Phase& phase, const Robot& robot) const {
+  NonlinearFactorGraph graph;
+  Interval interval = static_cast<Interval>(phase);
+  graph.add(jointAngleObjectives(interval, robot));
+  return graph;
+}
+
+template <>
+Values Kinematics::initialValues<Phase>(const Phase& phase,
+                                           const Robot& robot,
+                                           double gaussian_noise) const {
+  Values values;
+  Interval interval = static_cast<Interval>(phase);
+  values.insert(initialValues(interval, robot, gaussian_noise));
+  return values;
+}
+
+template <>
+Values Kinematics::inverse<Phase>(const Phase& phase,
+                                     const Robot& robot,
+                                     const ContactGoals& contact_goals) const {
+  Values results;
+  Interval interval = static_cast<Interval>(phase);
+  results.insert(inverse(interval, robot, contact_goals));
+  return results;
+}
+
+template <>
+Values Kinematics::interpolate<Phase>(const Phase& phase, const Robot& robot,
+                                         const ContactGoals& contact_goals1,
+                                         const ContactGoals& contact_goals2) const {
+  Values result;
+  Interval interval = static_cast<Interval>(phase);
+  result = interpolate(interval, robot, contact_goals1, contact_goals2);
+  return result;
+}
+
+}  // namespace gtdynamics

gtdynamics/kinematics/KinematicsTrajectory.cpp

@@ -0,0 +1,90 @@
+/* ----------------------------------------------------------------------------
+ * 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 <gtdynamics/kinematics/Kinematics.h>
+#include <gtdynamics/utils/Slice.h>
+#include <gtdynamics/utils/Trajectory.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+
+namespace gtdynamics {
+
+using gtsam::NonlinearFactorGraph;
+using gtsam::Values;
+using std::string;
+using std::vector;
+
+template <>
+NonlinearFactorGraph Kinematics::graph<Trajectory>(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<Trajectory>(
+    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<Trajectory>(
+    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<Trajectory>(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<Trajectory>(
+    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<Trajectory>(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

gtdynamics/utils/ConstraintSpec.h

@@ -0,0 +1,32 @@
+/* ----------------------------------------------------------------------------
+ * 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;
+};
+
+
+
+}  // namespace gtdynamics