Feature/ik panda

gtdynamics/kinematics/Kinematics.h

@@ -131,29 +131,59 @@ class Kinematics : public Optimizer {
   EqualityConstraints pointGoalConstraints(
       const CONTEXT& context, const ContactGoals& contact_goals) const;
 
+  /**
+   * @fn Create pose goal objectives.
+   * @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;
+
   /**
    * @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
    * @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 +199,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 goal_poses 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 gtsam::Values& goal_poses,
+      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 Robot& robot,
+    const gtsam::Values& goal_poses) const {
+  NonlinearFactorGraph graph;
+  for (size_t k = interval.k_start; k <= interval.k_end; k++) {
+    graph.add(poseGoalObjectives(Slice(k), robot, goal_poses));
+  }
+  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;
 }

gtdynamics/kinematics/KinematicsSlice.cpp

@@ -11,13 +11,15 @@
  * @author: Frank Dellaert
  */
 
+#include <gtdynamics/factors/JointLimitFactor.h>
 #include <gtdynamics/factors/PointGoalFactor.h>
 #include <gtdynamics/factors/PoseFactor.h>
 #include <gtdynamics/kinematics/Kinematics.h>
 #include <gtdynamics/utils/Slice.h>
 #include <gtsam/linear/Sampler.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/slam/expressions.h>
 
 namespace gtdynamics {
 
@@ -63,6 +65,26 @@ EqualityConstraints Kinematics::constraints<Slice>(const Slice& slice,
                                                             tolerance);
   }
 
+  // Constrain fixed links
+  for (auto&& link : robot.links()) {
+    if (link->isFixed()) {
+      using gtsam::Pose3_;
+
+      // Get an expression for the unknown link pose.
+      Pose3_ bTcom(PoseKey(link->id(), slice.k));
+
+      // 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 constraint
+      constraints.emplace_shared<VectorExpressionEquality<6>>(constraint_expr,
+                                                              tolerance);
+    }
+  }
+
   return constraints;
 }
 
@@ -98,23 +120,67 @@ EqualityConstraints Kinematics::pointGoalConstraints<Slice>(
   return constraints;
 }
 
+template <>
+NonlinearFactorGraph Kinematics::poseGoalObjectives<Slice>(
+    const Slice& slice, const Robot& robot,
+    const gtsam::Values& goal_poses) const {
+  gtsam::NonlinearFactorGraph graph;
+
+  for (const gtsam::Key& key : goal_poses.keys()) {
+    const gtsam::Pose3& desired_pose = goal_poses.at<gtsam::Pose3>(key);
+    // TODO(toni): use poseprior from unstable gtsam slam or create new
+    // factors, to add pose from link7
+    graph.addPrior<gtsam::Pose3>(key, desired_pose, p_.p_cost_model);
+  }
+
+  // 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(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;
+}
+
 template <>
 NonlinearFactorGraph Kinematics::jointAngleObjectives<Slice>(
-    const Slice& slice, const Robot& robot) const {
+    const Slice& slice, const Robot& robot, const Values& mean) const {
   NonlinearFactorGraph graph;
 
   // Minimize the joint angles.
   for (auto&& joint : robot.joints()) {
     const gtsam::Key key = JointAngleKey(joint->id(), slice.k);
-    graph.addPrior<double>(key, 0.0, 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;
 }
 
+template <>
+NonlinearFactorGraph Kinematics::jointAngleLimits<Slice>(
+    const Slice& slice, const Robot& robot) const {
+  NonlinearFactorGraph graph;
+  for (auto&& joint : robot.joints()) {
+    graph.add(JointLimitFactor(
+        JointAngleKey(joint->id(), slice.k), p_.prior_q_cost_model,
+        joint->parameters().scalar_limits.value_lower_limit,
+        joint->parameters().scalar_limits.value_upper_limit,
+        joint->parameters().scalar_limits.value_limit_threshold));
+  }
+  return graph;
+}
+
 template <>
 Values Kinematics::initialValues<Slice>(const Slice& slice, const Robot& robot,
-                                        double gaussian_noise) const {
+                                        double gaussian_noise,
+                                        const gtsam::Values& initial_joints,
+                                        bool use_fk) const {
   Values values;
 
   auto sampler_noise_model =
@@ -123,13 +189,27 @@ Values Kinematics::initialValues<Slice>(const Slice& slice, const Robot& robot,
 
   // Initialize all joint angles.
   for (auto&& joint : robot.joints()) {
-    InsertJointAngle(&values, joint->id(), slice.k, sampler.sample()[0]);
+    auto key = JointAngleKey(joint->id(), slice.k);
+    double angle = initial_joints.exists(key) ? initial_joints.at<double>(key)
+                                              : sampler.sample()[0];
+    InsertJointAngle(&values, joint->id(), slice.k, angle);
   }
 
-  // Initialize all poses.
-  for (auto&& link : robot.links()) {
-    const gtsam::Vector6 xi = sampler.sample();
-    InsertPose(&values, link->id(), slice.k, link->bMcom().expmap(xi));
+  // Maybe fk takes a long time, so only compute it if there was a given initial
+  // joint value in this slice
+  if (use_fk) {
+    // Initialize poses with fk of initialized values
+    auto fk = robot.forwardKinematics(values, slice.k);
+    for (auto&& link : robot.links()) {
+      InsertPose(&values, link->id(), slice.k,
+                 fk.at<gtsam::Pose3>(PoseKey(link->id(), slice.k)));
+    }
+  } else {
+    // Initialize all poses.
+    for (auto&& link : robot.links()) {
+      const gtsam::Vector6 xi = sampler.sample();
+      InsertPose(&values, link->id(), slice.k, link->bMcom().expmap(xi));
+    }
   }
 
   return values;
@@ -161,4 +241,27 @@ Values Kinematics::inverse<Slice>(const Slice& slice, const Robot& robot,
 
   return optimize(graph, constraints, initial_values);
 }
+
+template <>
+gtsam::Values Kinematics::inverse<Slice>(
+    const Slice& slice, const Robot& robot, const gtsam::Values& goal_poses,
+    const gtsam::Values& joint_priors) const {
+  auto graph = this->graph(slice, robot);
+
+  // 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));
+
+  // 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, true);
+
+  return this->optimize(graph, constraints, initial_values);
+}
 }  // namespace gtdynamics

gtdynamics/kinematics/KinematicsTrajectory.cpp

@@ -35,8 +35,8 @@ NonlinearFactorGraph Kinematics::graph<Trajectory>(const Trajectory& trajectory,
 }
 
 template <>
-EqualityConstraints Kinematics::constraints<Trajectory>(const Trajectory& trajectory,
-                                                   const Robot& robot) const {
+EqualityConstraints Kinematics::constraints<Trajectory>(
+    const Trajectory& trajectory, const Robot& robot) const {
   EqualityConstraints constraints;
   for (auto&& phase : trajectory.phases()) {
     constraints.add(this->constraints<Interval>(phase, robot));
@@ -66,21 +66,23 @@ EqualityConstraints Kinematics::pointGoalConstraints<Trajectory>(
 
 template <>
 NonlinearFactorGraph Kinematics::jointAngleObjectives<Trajectory>(
-    const Trajectory& trajectory, const Robot& robot) const {
+    const Trajectory& trajectory, const Robot& robot,
+    const Values& mean) const {
   NonlinearFactorGraph graph;
   for (auto&& phase : trajectory.phases()) {
-    graph.add(jointAngleObjectives<Interval>(phase, robot));
+    graph.add(jointAngleObjectives<Interval>(phase, robot, mean));
   }
   return graph;
 }
 
 template <>
-Values Kinematics::initialValues<Trajectory>(const Trajectory& trajectory,
-                                             const Robot& robot,
-                                             double gaussian_noise) const {
+Values Kinematics::initialValues<Trajectory>(
+    const Trajectory& trajectory, const Robot& robot, double gaussian_noise,
+    const gtsam::Values& initial_joints, bool use_fk) const {
   Values values;
   for (auto&& phase : trajectory.phases()) {
-    values.insert(initialValues<Interval>(phase, robot, gaussian_noise));
+    values.insert(
+        initialValues<Interval>(phase, robot, gaussian_noise, initial_joints, use_fk));
   }
   return values;
 }

gtdynamics/universal_robot/Joint.cpp

@@ -266,15 +266,6 @@ std::ostream &operator<<(std::ostream &os, const JointSharedPtr &j) {
   return j->to_stream(os);
 }
 
-/* ************************************************************************* */
-// TODO(yetong): Remove the logmap and use the one in gtsam/slam/expressions.h.
-template <typename T>
-gtsam::Expression<typename gtsam::traits<T>::TangentVector> logmap(
-    const gtsam::Expression<T> &x1, const gtsam::Expression<T> &x2) {
-  return gtsam::Expression<typename gtsam::traits<T>::TangentVector>(
-      gtsam::traits<T>::Logmap, between(x1, x2));
-}
-
 /* ************************************************************************* */
 gtsam::Vector6_ Joint::poseConstraint(uint64_t t) const {
   using gtsam::Pose3_;
@@ -291,7 +282,7 @@ gtsam::Vector6_ Joint::poseConstraint(uint64_t t) const {
   Pose3_ wTc_hat = wTp * pTc;
 
   // Return the error in tangent space
-  return gtdynamics::logmap(wTc, wTc_hat);
+  return gtsam::logmap(wTc, wTc_hat);
 }
 
 /* ************************************************************************* */