Feature/ik panda

.gitignore

@@ -121,3 +121,4 @@ scripts/gerry00_rpr_viz.gif
 traj.csv
 
 results/
+_codeql_detected_source_root

gtdynamics/kinematics/Kinematics.h

@@ -65,6 +65,53 @@ 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 comTgoal;  ///< Goal pose in link's CoM frame.
+  gtsam::Pose3 wTgoal;    ///< Goal pose in world frame.
+
+  /// Constructor
+  PoseGoal(const LinkSharedPtr& ee_link, const gtsam::Pose3& comTgoal,
+           const gtsam::Pose3& wTgoal)
+      : ee_link(ee_link), comTgoal(comTgoal), wTgoal(wTgoal) {}
+
+  /// Return link associated with contact pose.
+  const LinkSharedPtr& link() const { return ee_link; }
+
+  /// Return CoM pose needed to achieve goal pose.
+  const gtsam::Pose3 wTcom() const {
+    return wTgoal.compose(comTgoal.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 step k to PoseGoal for that time step
+using PoseGoals = std::map<size_t, PoseGoal>;
+
 /// Noise models etc specific to Kinematics class
 struct KinematicsParameters : public OptimizationParameters {
   using Isotropic = gtsam::noiseModel::Isotropic;
@@ -73,10 +120,12 @@ struct KinematicsParameters : public OptimizationParameters {
       prior_q_cost_model;                      // joint angle prior factor
 
   // TODO(yetong): replace noise model with tolerance.
-  KinematicsParameters()
-      : p_cost_model(Isotropic::Sigma(6, 1e-4)),
-        g_cost_model(Isotropic::Sigma(3, 0.01)),
-        prior_q_cost_model(Isotropic::Sigma(1, 0.5)) {}
+  KinematicsParameters(double p_cost_model_sigma = 1e-4,
+                       double g_cost_model_sigma = 1e-2,
+                       double prior_q_cost_model_sigma = 0.5)
+      : p_cost_model(Isotropic::Sigma(6, p_cost_model_sigma)),
+        g_cost_model(Isotropic::Sigma(3, g_cost_model_sigma)),
+        prior_q_cost_model(Isotropic::Sigma(1, prior_q_cost_model_sigma)) {}
 };
 
 /// All things kinematics, zero velocities/twists, and no forces.
@@ -131,29 +180,67 @@ class Kinematics : public Optimizer {
   gtsam::NonlinearEqualityConstraints 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 an object of PoseGoals: map of time step k to desired pose
+   * at that time step, which will be used as mean of the prior
+   * @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. Uses
+   * gtsam::Values to support different prior means for different time steps in
+   * intervals/trajectories. The default is an empty Values, meaning that the
+   * means will default to 0.
    * @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 noise (stddev) added to initial values (default 0.0).
+   * @param gaussian_noise Gaussian noise standard deviation for initialization
+   * (default 0.0).
+   * @param initial_joints Initial values for joints.
+   * @param use_fk If true, use forward kinematics to initialize poses based on
+   * joint angles. If false, initialize poses near rest configuration with
+   * Gaussian noise (default false).
    * @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.0) const;
+  gtsam::Values initialValues(
+      const CONTEXT& context, const Robot& robot, double gaussian_noise = 0.0,
+      const gtsam::Values& initial_joints = gtsam::Values(),
+      bool use_fk = false) const;
 
   /**
    * @fn Inverse kinematics given a set of contact goals.
@@ -169,6 +256,22 @@ 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. Uses gtsam::Values to support different prior means for different
+   * time steps. 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

@@ -47,6 +47,16 @@ gtsam::NonlinearEqualityConstraints 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 {
@@ -72,21 +82,34 @@ gtsam::NonlinearEqualityConstraints 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 {
+                                           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;
 }

gtdynamics/kinematics/KinematicsPhase.cpp

@@ -23,6 +23,4 @@ using gtsam::Values;
 using std::string;
 using std::vector;
 
-
-
 }  // namespace gtdynamics