Feature: End-Effector Workspace Pose and Velocity (Linear and Angular) Factor

gpmp2.i

@@ -363,6 +363,14 @@ class PointRobotModel {
   double sphere_radius(size_t i) const;
 };
 
+// desired workspace pose and velocity for last link factor
+#include <gpmp2/kinematics/WorkspacePoseVelocityPrior.h>
+
+virtual class WorkspacePoseVelocityPrior : gtsam::NoiseModelFactor {
+  WorkspacePoseVelocityPrior(size_t poseKey, size_t velKey, const gtsam::noiseModel::Base* cost_model,
+                             const gpmp2::Arm& arm, const gtsam::Pose3& des_pose, const gtsam::Vector& des_vel);
+};
+
 // goal destination factor
 #include <gpmp2/kinematics/GoalFactorArm.h>
 

gpmp2/kinematics/ForwardKinematics-inl.h

@@ -35,7 +35,7 @@ gtsam::Matrix ForwardKinematics<POSE, VELOCITY>::forwardKinematicsPosition(
   forwardKinematics(jp, {}, jpx, nullptr);
 
   // convert vector in matrix
-  gtsam::Matrix jpx_mat(6, nr_links_);
+  gtsam::Matrix jpx_mat(3, nr_links_);
   for (size_t i = 0; i < nr_links_; i++) jpx_mat.col(i) = jpx[i].translation();
   return jpx_mat;
 }

gpmp2/kinematics/ForwardKinematics.h

@@ -49,7 +49,7 @@ class ForwardKinematics {
    *  @param jp robot pose in config space
    *  @param jv robot velocity in config space
    *  @param jpx link poses in 3D work space
-   *  @param jvx link velocities in 3D work space, no angular rate
+   *  @param jvx link velocities in 3D work space
    *  @param J_jpx_jp et al. optional Jacobians
    **/
   virtual void forwardKinematics(
@@ -62,7 +62,7 @@ class ForwardKinematics {
 
   /**
    * Matrix wrapper for forwardKinematics, mainly used by matlab
-   * each column is a single point / velocity of the joint, size 6xN, 3xN, 3xN
+   * each column is a single point / velocity of the joint, size 6xN, 3xN, 6xN
    * No Jacobians provided by this version
    */
   gtsam::Matrix forwardKinematicsPose(const Pose& jp) const;

gpmp2/kinematics/WorkspacePoseVelocityPrior.h

@@ -0,0 +1,125 @@
+/**
+ *  @file   WorkspacePoseVelocityPrior.h
+ *  @brief  Prior defined on the workspace pose and velocity of final joint
+ *(end-effector) of a robot given its state in configuration space
+ *  @author Matthew King-Smith
+ *  @date   May 16, 2023
+ **/
+
+#pragma once
+
+#include <gpmp2/kinematics/Arm.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+#include <iostream>
+#include <vector>
+
+namespace gpmp2 {
+
+/**
+ * binary factor Gaussian prior defined on the workspace pose and velocities
+ */
+class WorkspacePoseVelocityPrior
+    : public gtsam::NoiseModelFactor2<gtsam::Vector, gtsam::Vector> {
+ private:
+  // typedefs
+  typedef WorkspacePoseVelocityPrior This;
+  typedef gtsam::NoiseModelFactor2<gtsam::Vector, gtsam::Vector> Base;
+
+  // arm
+  Arm arm_;
+
+  // desired workspace pose
+  gtsam::Pose3 des_pose_;
+
+  // desired velocity (linear and angular)
+  gtsam::Vector6 des_vel_;
+
+ public:
+  /// shorthand for a smart pointer to a factor
+  typedef std::shared_ptr<This> shared_ptr;
+
+  /// Default constructor
+  WorkspacePoseVelocityPrior() {}
+
+  /**
+   * Constructor
+   * @param cost_model cost function covariance
+   * @param des_vel desired velocity (linear and angular)
+   */
+  WorkspacePoseVelocityPrior(gtsam::Key poseKey, gtsam::Key velKey,
+                             const gtsam::SharedNoiseModel& cost_model,
+                             const Arm& arm, const gtsam::Pose3& des_pose,
+                             const gtsam::Vector6& des_vel)
+      : Base(cost_model, poseKey, velKey),
+        arm_(arm),
+        des_pose_(des_pose),
+        des_vel_(des_vel) {}
+
+  ~WorkspacePoseVelocityPrior() {}
+
+  /// error function
+  gtsam::Vector evaluateError(
+      const gtsam::Vector& joint_conf, const gtsam::Vector& joint_rates,
+      gtsam::OptionalMatrixType H1 = nullptr,
+      gtsam::OptionalMatrixType H2 = nullptr) const override {
+    using namespace gtsam;
+
+    // fk
+    std::vector<Pose3> joint_pose;
+    std::vector<Vector6> joint_vel;
+    std::vector<Matrix> J_jpx_jp, J_jvx_jp, J_jvx_jv;
+    arm_.forwardKinematics(joint_conf, joint_rates, joint_pose, &joint_vel,
+                           &J_jpx_jp, &J_jvx_jp, &J_jvx_jv);
+
+    Matrix66 H_ep;
+    Vector pose_error = des_pose_.logmap(joint_pose[arm_.dof() - 1], {}, H_ep);
+    Vector vel_error = des_vel_ - joint_vel[arm_.dof() - 1];
+    if (H1) {
+      // Jacobian for the joint positions
+      *H1 = (Matrix(12, arm_.dof()) << H_ep * J_jpx_jp[arm_.dof() - 1],//
+                              - J_jvx_jp[arm_.dof() - 1]).finished();
+    }
+    if (H2) {
+      // Jacobian for the joint rates
+      *H2 = (Matrix(12, arm_.dof()) << Matrix::Zero(6, arm_.dof()), //
+                              -J_jvx_jv[arm_.dof() - 1]).finished();
+    }
+    Vector12 tot_result;
+    tot_result << pose_error, vel_error;
+    return tot_result;
+  }
+
+  /// @return a deep copy of this factor
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
+    return std::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
+
+  /** print contents */
+  void print(const std::string& s = "",
+             const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const {
+    std::cout << s << "WorkspacePoseVelocityPrior :" << std::endl;
+    Base::print("", keyFormatter);
+    std::cout << "desired end-effector pose : ";
+    des_pose_.print();  // << std::endl;
+    std::cout << "desired end-effector velocity : " << des_vel_ << std::endl;
+  }
+
+ private:
+#ifdef GPMP2_ENABLE_BOOST_SERIALIZATION
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int version) {
+    ar& boost::serialization::make_nvp(
+        "NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
+  }
+#endif
+};
+
+}  // namespace gpmp2

gpmp2/kinematics/tests/testWorkspacePoseVelocityPrior.cpp

@@ -0,0 +1,104 @@
+/**
+ *  @file    testWorkspacePoseVelocityPrior.cpp
+ *  @author  Matthew King-Smith
+ **/
+
+#include <CppUnitLite/TestHarness.h>
+#include <gpmp2/kinematics/WorkspacePoseVelocityPrior.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+using namespace gpmp2;
+
+/* ************************************************************************** */
+TEST(WorkspacePoseVelocityPrior, error) {
+  Vector2 a(1, 1), alpha(M_PI / 2, 0), d(0, 0);
+  Arm arm = Arm(2, a, alpha, d);
+  Vector2 q;
+  Vector2 qdot;
+  Pose3 des_pose;
+  // Vector6 des_vel;
+  // WorkspacePoseVelocityPrior factor;
+  Vector actual, expect;
+  Matrix H_pose_exp, H_pose_act, H_vel_exp, H_vel_act;
+  noiseModel::Gaussian::shared_ptr cost_model =
+      noiseModel::Isotropic::Sigma(12, 1.0);
+
+  q = Vector2(M_PI / 4.0, -M_PI / 2);
+  qdot = (Vector2() << 0.1, 0.1).finished();
+  des_pose = Pose3();
+  Vector6 des_vel = (Vector6() << 0, 0, 0, 0, 0, 0).finished();
+  WorkspacePoseVelocityPrior factor(0, 0, cost_model, arm, des_pose, des_vel);
+  actual = factor.evaluateError(q, qdot, &H_pose_act, &H_vel_act);
+  expect = (Vector(12) << 00.613943126, 1.48218982, -0.613943126, 1.1609828,
+            0.706727485, -0.547039678, 0, -0.141421356237309, 0,
+            -0.070710678118655, 0.070710678118655, -0.1000)
+               .finished();
+  H_pose_exp = numericalDerivative11(
+      std::function<Vector(const Vector2&)>(
+          std::bind(&WorkspacePoseVelocityPrior::evaluateError, factor,
+                    std::placeholders::_1, qdot, nullptr, nullptr)),
+      q, 1e-6);
+
+  H_vel_exp = numericalDerivative11(
+      std::function<Vector(const Vector2&)>(
+          std::bind(&WorkspacePoseVelocityPrior::evaluateError, factor, q,
+                    std::placeholders::_1, nullptr, nullptr)),
+      qdot, 1e-6);
+  EXPECT(assert_equal(expect, actual, 1e-6));
+  EXPECT(assert_equal(H_pose_exp, H_pose_act, 1e-6));
+  EXPECT(assert_equal(H_vel_exp, H_vel_act, 1e-6));
+}
+
+/* ************************************************************************** */
+TEST(WorkspacePoseVelocityPrior, optimization) {
+  noiseModel::Gaussian::shared_ptr cost_model =
+      noiseModel::Isotropic::Sigma(12, 0.1);
+
+  Vector a = (Vector(2) << 1, 1).finished();
+  Vector alpha = (Vector(2) << 0, 0).finished();
+  Vector d = (Vector(2) << 0, 0).finished();
+  Arm arm = Arm(2, a, alpha, d);
+  Pose3 des_pose = Pose3(Rot3(), Point3(2, 0, 0));
+  Vector6 des_vel = Vector6(0, 0, 0, 0, 0, 0);
+  Key qkey = Symbol('x', 0);
+  Key qdotkey = Symbol('v', 0);
+  Vector q = (Vector(2) << 0, 0).finished();
+  Vector qdot = (Vector(2) << 0, 0).finished();
+  Vector qinit = (Vector(2) << M_PI / 2, M_PI / 2).finished();
+  Vector qdotinit = (Vector(2) << 0.1, 0.1).finished();
+
+  NonlinearFactorGraph graph;
+  graph.add(WorkspacePoseVelocityPrior(qkey, qdotkey, cost_model, arm, des_pose,
+                                       des_vel));
+  Values init_values;
+  init_values.insert(qkey, qinit);
+  init_values.insert(qdotkey, qdotinit);
+
+  LevenbergMarquardtParams parameters;
+  parameters.setVerbosity("ERROR");
+  parameters.setAbsoluteErrorTol(1e-12);
+  LevenbergMarquardtOptimizer optimizer(graph, init_values, parameters);
+  optimizer.optimize();
+  Values results = optimizer.values();
+
+  EXPECT_DOUBLES_EQUAL(0, graph.error(results), 1e-3);
+  EXPECT(assert_equal(q, results.at<Vector>(qkey), 1e-3));
+  EXPECT(assert_equal(qdot, results.at<Vector>(qdotkey), 1e-3));
+}
+
+/* ************************************************************************** */
+/* main function */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}