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;
@@ -131,29 +182,60 @@ class Kinematics : public Optimizer {
   EqualityConstraints pointGoalConstraints(
       const CONTEXT& context, const ContactGoals& contact_goals) const;
 
+  /**
+   * @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 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. The
+   * default is an empty Values, meaning that no means are specified.
    * @returns graph with prior factors on joint angles.
    */
   template <class CONTEXT>
-  gtsam::NonlinearFactorGraph jointAngleObjectives(const CONTEXT& context,
-                                                   const Robot& robot) const;
+  gtsam::NonlinearFactorGraph jointAngleObjectives(
+      const CONTEXT& context, const Robot& robot,
+      const gtsam::Values& joint_priors = gtsam::Values()) const;
+
+  /**
+   * @fn Factors that enforce joint angle limits.
+   * @param context Slice or Interval instance.
+   * @param robot Robot specification from URDF/SDF.
+   * @return graph with prior factors on joint angles.
+   */
+  template <class CONTEXT>
+  gtsam::NonlinearFactorGraph jointAngleLimits(const CONTEXT& context,
+                                               const Robot& robot) const;
 
   /**
    * @fn Initialize kinematics.
    *
-   * Use wTcom for poses and zero-mean noise for joint angles.
+   * If no values in initial_joints are given, use wTcom for poses and zero-mean
+   * noise for joint angles.
+   * If values are given, initialize joints with their given values, and
+   * zero-mean noise to the ones that without a given value. Poses are
+   * initialized with their fk values.
+   *
    *
    * @param context Slice or Interval instance.
    * @param robot Robot specification from URDF/SDF.
    * @param gaussian_noise time step to check (default 0.1).
+   * @param initial_joints initial values for joints
    * @returns values with identity poses and zero joint angles.
    */
   template <class CONTEXT>
-  gtsam::Values initialValues(const CONTEXT& context, const Robot& robot,
-                              double gaussian_noise = 0.1) const;
+  gtsam::Values initialValues(
+      const CONTEXT& context, const Robot& robot, double gaussian_noise = 0.1,
+      const gtsam::Values& initial_joints = gtsam::Values(),
+      bool use_fk = false) const;
 
   /**
    * @fn Inverse kinematics given a set of contact goals.
@@ -169,6 +251,21 @@ class Kinematics : public Optimizer {
                         const ContactGoals& contact_goals,
                         bool contact_goals_as_constraints = true) const;
 
+  /**
+   * @fn Inverse kinematics given a set of desired poses
+   * @fn This function does inverse kinematics separately on each slice
+   * @param context Slice or Interval instance
+   * @param robot Robot specification from URDF/SDF
+   * @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 inverse(
+      const CONTEXT& context, const Robot& robot, const PoseGoals& pose_goals,
+      const gtsam::Values& joint_priors = gtsam::Values()) const;
+
   /**
    * Interpolate using inverse kinematics: the goals are linearly interpolated.
    * @param context Interval instance

gtdynamics/kinematics/KinematicsInterval.cpp

@@ -44,6 +44,17 @@ EqualityConstraints Kinematics::constraints<Interval>(
   return constraints;
 }
 
+template <>
+NonlinearFactorGraph Kinematics::poseGoalObjectives<Interval>(
+    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), pose_goals));
+  }
+  return graph;
+}
+
 template <>
 NonlinearFactorGraph Kinematics::pointGoalObjectives<Interval>(
     const Interval& interval, const ContactGoals& contact_goals) const {
@@ -66,21 +77,31 @@ EqualityConstraints Kinematics::pointGoalConstraints<Interval>(
 
 template <>
 NonlinearFactorGraph Kinematics::jointAngleObjectives<Interval>(
+    const Interval& interval, const Robot& robot, const Values& mean) const {
+  NonlinearFactorGraph graph;
+  for (size_t k = interval.k_start; k <= interval.k_end; k++) {
+    graph.add(jointAngleObjectives(Slice(k), robot, mean));
+  }
+  return graph;
+}
+
+template <>
+NonlinearFactorGraph Kinematics::jointAngleLimits<Interval>(
     const Interval& interval, const Robot& robot) const {
   NonlinearFactorGraph graph;
   for (size_t k = interval.k_start; k <= interval.k_end; k++) {
-    graph.add(jointAngleObjectives(Slice(k), robot));
+    graph.add(jointAngleLimits(Slice(k), robot));
   }
   return graph;
 }
 
 template <>
-Values Kinematics::initialValues<Interval>(const Interval& interval,
-                                           const Robot& robot,
-                                           double gaussian_noise) const {
+Values Kinematics::initialValues<Interval>(
+    const Interval& interval, const Robot& robot, double gaussian_noise,
+    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));
+    values.insert(initialValues(Slice(k), robot, gaussian_noise, joint_priors, use_fk));
   }
   return values;
 }