Feature/ik panda

gtdynamics/kinematics/Kinematics.h

@@ -17,6 +17,7 @@
 #include <gtdynamics/universal_robot/Robot.h>
 #include <gtdynamics/utils/Interval.h>
 #include <gtdynamics/utils/PointOnLink.h>
+#include <gtdynamics/utils/PoseOnLink.h>
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/nonlinear/LevenbergMarquardtParams.h>
@@ -65,6 +66,56 @@ struct ContactGoal {
 ///< Map of link name to ContactGoal
 using ContactGoals = std::vector<ContactGoal>;
 
+/**
+ * Similar to the previous struct ContactGoal but with poses instead of
+ * points.
+ * Desired world pose for a end-effector pose.
+ *
+ * This simple struct stores the robot link that holds the end-effector, the
+ * end-effector's  pose in the final link's CoM frame, and a `goal_pose` in
+ * world coordinate frames. The goal is satisfied iff
+ * `pose_on_link.predict(values, k) == goal_pose`.
+ */
+struct PoseGoal {
+  LinkSharedPtr ee_link;      ///< Link that hold end-effector
+  gtsam::Pose3 comTee;  ///< In link's CoM frame.
+  gtsam::Pose3 goal_pose;  ///< In world frame.
+
+  /// Constructor
+  PoseGoal(const LinkSharedPtr& ee_link, const gtsam::Pose3& comTee,
+           const gtsam::Pose3& goal_pose)
+      : ee_link(ee_link), comTee(comTee), goal_pose(goal_pose) {}
+
+  /// Return link associated with contact pose.
+  const LinkSharedPtr& link() const { return ee_link; }
+
+  /// Return goal pose in ee_link COM frame.
+  const gtsam::Pose3& poseInCoM() const { return comTee; }
+
+  /// Return CoM pose needed to achieve goal pose.
+  const gtsam::Pose3 sTcom() const {
+    return goal_pose.compose(poseInCoM().inverse());
+  }
+
+  /// Print to stream.
+  friend std::ostream& operator<<(std::ostream& os, const PoseGoal& cg);
+
+  /// GTSAM-style print, works with wrapper.
+  void print(const std::string& s) const;
+
+  /**
+   * @fn Check that the contact goal has been achieved for given values.
+   * @param values a GTSAM Values instance that should contain link pose.
+   * @param k time step to check (default 0).
+   * @param tol tolerance in 3D (default 1e-9).
+   */
+  bool satisfied(const gtsam::Values& values, size_t k = 0,
+                 double tol = 1e-9) const;
+};
+
+///< Map of time to PoseGoal
+using PoseGoals = std::map<size_t, PoseGoal>;
+
 /// Noise models etc specific to Kinematics class
 struct KinematicsParameters : public OptimizationParameters {
   using Isotropic = gtsam::noiseModel::Isotropic;
@@ -132,21 +183,21 @@ class Kinematics : public Optimizer {
       const CONTEXT& context, const ContactGoals& contact_goals) const;
 
   /**
-   * @fn Create pose goal objectives.
+   * @fn Create a pose prior for a given link for each given pose.
    * @param context Slice or Interval instance.
    * @param pose_goals goals for poses
    * @returns graph with pose goal factors.
    */
   template <class CONTEXT>
   gtsam::NonlinearFactorGraph poseGoalObjectives(
-      const CONTEXT& context, const Robot& robot,
-      const gtsam::Values& pose_goals) const;
+      const CONTEXT& context, const PoseGoals& pose_goals) const;
 
   /**
    * @fn Factors that minimize joint angles.
    * @param context Slice or Interval instance.
    * @param robot Robot specification from URDF/SDF.
-   * @param joint_priors Values where the mean of the priors is specified
+   * @param joint_priors Values where the mean of the priors is specified. The
+   * default is an empty Values, meaning that no means are specified.
    * @returns graph with prior factors on joint angles.
    */
   template <class CONTEXT>
@@ -183,7 +234,8 @@ class Kinematics : public Optimizer {
   template <class CONTEXT>
   gtsam::Values initialValues(
       const CONTEXT& context, const Robot& robot, double gaussian_noise = 0.1,
-      const gtsam::Values& initial_joints = gtsam::Values()) const;
+      const gtsam::Values& initial_joints = gtsam::Values(),
+      bool use_fk = false) const;
 
   /**
    * @fn Inverse kinematics given a set of contact goals.
@@ -204,15 +256,14 @@ class Kinematics : public Optimizer {
    * @fn This function does inverse kinematics separately on each slice
    * @param context Slice or Interval instance
    * @param robot Robot specification from URDF/SDF
-   * @param goal_poses goals for EE poses
+   * @param pose_goals goals for EE poses
    * @param joint_priors optional argument to put priors centered on given
    * values. If empty, the priors will be centered on the origin
    * @return values with poses and joint angles
    */
   template <class CONTEXT>
-  gtsam::Values inverseWithPose(
-      const CONTEXT& context, const Robot& robot,
-      const gtsam::Values& goal_poses,
+  gtsam::Values inverse(
+      const CONTEXT& context, const Robot& robot, const PoseGoals& pose_goals,
       const gtsam::Values& joint_priors = gtsam::Values()) const;
 
   /**

gtdynamics/kinematics/KinematicsInterval.cpp

@@ -46,11 +46,11 @@ EqualityConstraints Kinematics::constraints<Interval>(
 
 template <>
 NonlinearFactorGraph Kinematics::poseGoalObjectives<Interval>(
-    const Interval& interval, const Robot& robot,
-    const gtsam::Values& goal_poses) const {
+    const Interval& interval, 
+    const PoseGoals& pose_goals) const {
   NonlinearFactorGraph graph;
   for (size_t k = interval.k_start; k <= interval.k_end; k++) {
-    graph.add(poseGoalObjectives(Slice(k), robot, goal_poses));
+    graph.add(poseGoalObjectives(Slice(k), pose_goals));
   }
   return graph;
 }
@@ -98,10 +98,10 @@ NonlinearFactorGraph Kinematics::jointAngleLimits<Interval>(
 template <>
 Values Kinematics::initialValues<Interval>(
     const Interval& interval, const Robot& robot, double gaussian_noise,
-    const gtsam::Values& joint_priors) const {
+    const gtsam::Values& joint_priors, bool use_fk) const {
   Values values;
   for (size_t k = interval.k_start; k <= interval.k_end; k++) {
-    values.insert(initialValues(Slice(k), robot, gaussian_noise, joint_priors));
+    values.insert(initialValues(Slice(k), robot, gaussian_noise, joint_priors, use_fk));
   }
   return values;
 }

gtdynamics/kinematics/KinematicsSlice.cpp

@@ -36,6 +36,21 @@ void ContactGoal::print(const std::string& s) const {
   std::cout << (s.empty() ? s : s + " ") << *this;
 }
 
+std::ostream& operator<<(std::ostream& os, const PoseGoal& pg) {
+  os << "{" << pg.link()->name() << ", [" << pg.comTee << "], [" << pg.goal_pose << "]}";
+  return os;
+}
+
+void PoseGoal::print(const std::string& s) const {
+  std::cout << (s.empty() ? s : s + " ") << *this;
+}
+
+bool PoseGoal::satisfied(const gtsam::Values& values, size_t k,
+                         double tol) const {
+  return gtsam::equal<gtsam::Pose3>(
+      Pose(values, link()->id(), k).compose(poseInCoM()), goal_pose, tol);
+}
+
 template <>
 NonlinearFactorGraph Kinematics::graph<Slice>(const Slice& slice,
                                               const Robot& robot) const {
@@ -73,13 +88,13 @@ EqualityConstraints Kinematics::constraints<Slice>(const Slice& slice,
       // Get an expression for the unknown link pose.
       Pose3_ bTcom(PoseKey(link->id(), slice.k));
 
-      // Kust make sure it does not move from its original rest pose
+      // Just make sure it does not move from its original rest pose
       Pose3_ bMcom(link->bMcom());
 
       // Create expression to calculate the error in tangent space
       auto constraint_expr = gtsam::logmap(bTcom, bMcom);
 
-      // Add constriant
+      // Add constraint
       constraints.emplace_shared<VectorExpressionEquality<6>>(constraint_expr,
                                                               tolerance);
     }
@@ -122,19 +137,15 @@ EqualityConstraints Kinematics::pointGoalConstraints<Slice>(
 
 template <>
 NonlinearFactorGraph Kinematics::poseGoalObjectives<Slice>(
-    const Slice& slice, const Robot& robot,
-    const gtsam::Values& goal_poses) const {
+    const Slice& slice, const PoseGoals& pose_goals) const {
   gtsam::NonlinearFactorGraph graph;
 
-  // Add priors on link poses with desired poses from argument
-  for (auto&& link : robot.links()) {
-    auto pose_key = PoseKey(link->id(), slice.k);
-    if (goal_poses.exists(pose_key)) {
-      const gtsam::Pose3& desired_pose = goal_poses.at<gtsam::Pose3>(pose_key);
-      // TODO: use poseprior from unstable gtsam slam or create new factors, to
-      // add pose from link7
-      graph.addPrior<gtsam::Pose3>(pose_key, desired_pose, p_.p_cost_model);
-    }
+  if (pose_goals.find(slice.k) != pose_goals.end()) {
+    auto pose_key = PoseKey(pose_goals.at(slice.k).link()->id(), slice.k);
+    const gtsam::Pose3& desired_pose = pose_goals.at(slice.k).sTcom();
+    // TODO(toni): use poseprior from unstable gtsam slam or create new
+    // factors, to add pose from link7
+    graph.addPrior<gtsam::Pose3>(pose_key, desired_pose, p_.p_cost_model);
   }
 
   return graph;
@@ -148,9 +159,8 @@ NonlinearFactorGraph Kinematics::jointAngleObjectives<Slice>(
   // Minimize the joint angles.
   for (auto&& joint : robot.joints()) {
     const gtsam::Key key = JointAngleKey(joint->id(), slice.k);
-    double joint_mean = 0.0;
-    if (mean.exists(key)) joint_mean = mean.at<double>(key);
-    graph.addPrior<double>(key, joint_mean, p_.prior_q_cost_model);
+    graph.addPrior<double>(key, (mean.exists(key) ? mean.at<double>(key) : 0.0),
+                           p_.prior_q_cost_model);
   }
 
   return graph;
@@ -171,31 +181,27 @@ NonlinearFactorGraph Kinematics::jointAngleLimits<Slice>(
 }
 
 template <>
-Values Kinematics::initialValues<Slice>(
-    const Slice& slice, const Robot& robot, double gaussian_noise,
-    const gtsam::Values& initial_joints) const {
+Values Kinematics::initialValues<Slice>(const Slice& slice, const Robot& robot,
+                                        double gaussian_noise,
+                                        const gtsam::Values& initial_joints,
+                                        bool use_fk) const {
   Values values;
 
   auto sampler_noise_model =
       gtsam::noiseModel::Isotropic::Sigma(6, gaussian_noise);
   gtsam::Sampler sampler(sampler_noise_model);
 
   // Initialize all joint angles.
-  bool any_value = false;
   for (auto&& joint : robot.joints()) {
     auto key = JointAngleKey(joint->id(), slice.k);
-    double value;
-    if (initial_joints.exists(key)) {
-      value = initial_joints.at<double>(key);
-      any_value = true;
-    } else
-      value = sampler.sample()[0];
-    InsertJointAngle(&values, joint->id(), slice.k, value);
+    double angle = initial_joints.exists(key) ? initial_joints.at<double>(key)
+                                              : sampler.sample()[0];
+    InsertJointAngle(&values, joint->id(), slice.k, angle);
   }
 
   // Maybe fk takes a long time, so only compute it if there was a given initial
   // joint value in this slice
-  if (any_value) {
+  if (use_fk) {
     // Initialize poses with fk of initialized values
     auto fk = robot.forwardKinematics(values, slice.k);
     for (auto&& link : robot.links()) {
@@ -228,7 +234,7 @@ Values Kinematics::inverse<Slice>(const Slice& slice, const Robot& robot,
     graph.add(pointGoalObjectives(slice, contact_goals));
   }
 
-  // Traget joint angles.
+  // Target joint angles.
   graph.add(jointAngleObjectives(slice, robot));
 
   // TODO(frank): allo pose prior as well.
@@ -241,24 +247,25 @@ Values Kinematics::inverse<Slice>(const Slice& slice, const Robot& robot,
 }
 
 template <>
-gtsam::Values Kinematics::inverseWithPose<Slice>(
-    const Slice& slice, const Robot& robot, const gtsam::Values& goal_poses,
+gtsam::Values Kinematics::inverse<Slice>(
+    const Slice& slice, const Robot& robot, const PoseGoals& pose_goals,
     const gtsam::Values& joint_priors) const {
   auto graph = this->graph(slice, robot);
 
-  // Add prior on joint angles to constrain the solution
+  // Add prior on joint angles to prefer solution close to our initial estimates
   graph.add(this->jointAngleObjectives(slice, robot, joint_priors));
 
   // Add priors on link poses with desired poses from argument
-  graph.add(this->poseGoalObjectives(slice, robot, goal_poses));
+  graph.add(this->poseGoalObjectives(slice, pose_goals));
 
   // Add joint angle limits factors
   graph.add(this->jointAngleLimits(slice, robot));
 
   // Robot kinematics constraints
   auto constraints = this->constraints(slice, robot);
 
-  auto initial_values = this->initialValues(slice, robot, 0.1, joint_priors);
+  auto initial_values =
+      this->initialValues(slice, robot, 0.1, joint_priors, true);
 
   return this->optimize(graph, constraints, initial_values);
 }

gtdynamics/kinematics/KinematicsTrajectory.cpp

@@ -78,11 +78,11 @@ NonlinearFactorGraph Kinematics::jointAngleObjectives<Trajectory>(
 template <>
 Values Kinematics::initialValues<Trajectory>(
     const Trajectory& trajectory, const Robot& robot, double gaussian_noise,
-    const gtsam::Values& initial_joints) const {
+    const gtsam::Values& initial_joints, bool use_fk) const {
   Values values;
   for (auto&& phase : trajectory.phases()) {
     values.insert(
-        initialValues<Interval>(phase, robot, gaussian_noise, initial_joints));
+        initialValues<Interval>(phase, robot, gaussian_noise, initial_joints, use_fk));
   }
   return values;
 }