Added new functions in kinematics

gtdynamics/kinematics/Kinematics.h

@@ -131,29 +131,49 @@ 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 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()) const;
 
   /**
    * @fn Inverse kinematics given a set of contact goals.
@@ -169,6 +189,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 inverseWithPose(
+      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,21 @@ EqualityConstraints Kinematics::pointGoalConstraints<Interval>(
 
 template <>
 NonlinearFactorGraph Kinematics::jointAngleObjectives<Interval>(
-    const Interval& interval, const Robot& robot) const {
+    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));
+    graph.add(jointAngleObjectives(Slice(k), robot, mean));
   }
   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) 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));
   }
   return values;
 }

gtdynamics/kinematics/KinematicsSlice.cpp

@@ -119,38 +119,80 @@ 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;
+
+  // 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);
+    }
+  }
+
+  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);
+    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);
   }
 
   return graph;
 }
 
 template <>
-Values Kinematics::initialValues<Slice>(const Slice& slice, const Robot& robot,
-                                        double gaussian_noise) const {
+Values Kinematics::initialValues<Slice>(
+    const Slice& slice, const Robot& robot, double gaussian_noise,
+    const gtsam::Values& initial_joints) 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()) {
-    InsertJointAngle(&values, joint->id(), slice.k, sampler.sample()[0]);
+    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);
   }
 
-  // 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 (any_value) {
+    // 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;
@@ -182,4 +224,24 @@ Values Kinematics::inverse<Slice>(const Slice& slice, const Robot& robot,
 
   return optimize(graph, constraints, initial_values);
 }
+
+template <>
+gtsam::Values Kinematics::inverseWithPose<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 constrain the solution
+  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));
+
+  // Robot kinematics constraints
+  auto constraints = this->constraints(slice, robot);
+
+  auto initial_values = this->initialValues(slice, robot, 0.1, joint_priors);
+
+  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) 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));
   }
   return values;
 }