Manifold optimization on cable robot kinodynamics problem

gtdynamics/cablerobot/CMakeLists.txt

@@ -1,5 +1,5 @@
 # add cablerobot subfolders to gtdynamics' SOURCE_SUBDIRS list
-list(APPEND SOURCE_SUBDIRS cablerobot/factors)
+list(APPEND SOURCE_SUBDIRS cablerobot/factors cablerobot/utils)
 set(SOURCE_SUBDIRS ${SOURCE_SUBDIRS} PARENT_SCOPE)
 
 # add wrapper interface file

gtdynamics/cablerobot/cablerobot.i

@@ -39,4 +39,44 @@ class PriorFactor : gtsam::NonlinearFactor {
              const gtsam::KeyFormatter &keyFormatter);
 };
 
+#include <gtdynamics/cablerobot/utils/CustomWrap.h>
+gtsam::GaussianBayesNet* EliminateSequential(gtsam::GaussianFactorGraph graph,
+                                             const gtsam::Ordering &ordering);
+gtsam::GaussianBayesNet* BlockEliminateSequential(
+    gtsam::GaussianFactorGraph graph, const gtdynamics::BlockOrdering &ordering);
+
+// Tension Key Stuff
+gtdynamics::DynamicsSymbol TensionKey(int j, int t = 0);
+
+void InsertTension(gtsam::Values @values, int j, int t, double value);
+
+void InsertTension(gtsam::Values @values, int j, double value);
+
+double Tension(const gtsam::Values &values, int j, int t = 0);
+
+#include <gtdynamics/cablerobot/factors/WinchFactor.h>
+class WinchParams {
+  double radius_;
+  double inertia_;
+  double staticFriction_;
+  double viscousFriction_;
+  WinchParams(double radius = 1, double inertia = 0, double staticFriction = 0,
+              double viscousFriction = 0);
+};
+class WinchFactor : gtsam::NonlinearFactor {
+  WinchFactor(gtsam::Key torque, gtsam::Key tension, gtsam::Key cableVelocity,
+              gtsam::Key cableAcceleration,
+              const gtsam::noiseModel::Base *cost_model, gtdynamics::WinchParams params);
+  void print(const string &s, const gtsam::KeyFormatter &keyFormatter);
+};
+
+#include <gtdynamics/cablerobot/factors/CableAccelerationFactor.h>
+class CableAccelerationFactor : gtsam::NonlinearFactor {
+  CableAccelerationFactor(gtsam::Key ldot_key, gtsam::Key wTee_key,
+                          gtsam::Key Vee_key, gtsam::Key VA_key,
+                          const gtsam::noiseModel::Base *cost_model,
+                          const gtsam::Point3 &wPb, const gtsam::Point3 &eePem);
+  void print(const string &s, const gtsam::KeyFormatter &keyFormatter);
+};
+
 }  // namespace gtdynamics

gtdynamics/cablerobot/factors/CableAccelerationFactor.cpp

@@ -0,0 +1,69 @@
+/**
+ * @file  CableAccelerationFactor.cpp
+ * @brief Cable acceleration factor: relates cable length acceleration with end
+ * effector pose/twist/twist acceleration.  The cable mounting locations on the
+ * stationary frame and on the end-effector are passed as parameters.
+ * @author Gerry Chen
+ */
+
+#include "CableAccelerationFactor.h"
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Pose3.h>
+
+#include <boost/optional.hpp>
+#include <iostream>
+#include <string>
+
+using namespace gtsam;
+
+namespace gtdynamics {
+
+// Attention: This version currently ignores all rotational effects since
+// they're relatively insignificant and it's just a pain to deal with them.
+double CableAccelerationFactor::computeLddot(
+    const Pose3 &wTx, const Vector6 &Vx, const Vector6 &VAx,
+    boost::optional<Matrix &> H_wTx, boost::optional<Matrix &> H_Vx,
+    boost::optional<Matrix &> H_VAx) const {
+  Matrix33 wAb_H_wRx;
+  Matrix33 wAb_H_xAb;
+  Matrix36 wAb_H_wTx;
+  Matrix36 wAb_H_VAx;
+  Matrix36 wPb_H_wTx;
+  Matrix33 dir_H_wPb;
+  Matrix13 H_dir;
+  Matrix13 H_wAb;
+
+  // linear acceleration
+  Vector3 xAb = VAx.bottomRows<3>();  // acceleration of point b in x's frame
+  Vector3 wAb = wTx.rotation().rotate(xAb, H_wTx ? &wAb_H_wRx : 0,
+                                      H_VAx ? &wAb_H_xAb : 0);
+  if (H_wTx) wAb_H_wTx << wAb_H_wRx, Z_3x3;
+  if (H_VAx) wAb_H_VAx << Z_3x3, wAb_H_xAb;
+  // cable direction
+  Point3 wPb = wTx.transformFrom(xPb_, H_wTx ? &wPb_H_wTx : 0);
+  Vector3 dir = normalize(wPb - wPa_, H_wTx ? &dir_H_wPb : 0);
+  // lddot
+  double lddot = dot(dir, wAb,            //
+                     H_wTx ? &H_dir : 0,  //
+                     (H_wTx || H_VAx) ? &H_wAb : 0);
+  if (H_wTx) *H_wTx = H_dir * dir_H_wPb * wPb_H_wTx + H_wAb * wAb_H_wTx;
+  if (H_Vx) *H_Vx = Matrix16::Zero();
+  if (H_VAx) *H_VAx = H_wAb * wAb_H_VAx;
+  return lddot;
+}
+
+// below: pseudocode for full version
+// static Vector3 compute_linear_acceleration_b(Pose3 x, Point3 xPe, Vector6 Vx, Vector6 VAx) {
+//   Vector6 VAe1 = xPe.inverse().AdjointMap() * VAx;
+//   Vector3 ae1 = VAe.bottom<3>();
+
+//   Vector6 Ve = xPe.inverse().AdjointMap() * Vx;
+//   Vector6 VAe2 = -Pose3::adjointMap(Ve, Ve);
+//   Vector3 ae2 = VAe2.bottom<3>();
+
+//   return ae1 + ae2;
+// }
+
+}  // namespace gtdynamics

gtdynamics/cablerobot/factors/CableAccelerationFactor.h

@@ -0,0 +1,122 @@
+/**
+ * @file  CableAccelerationFactor.h
+ * @brief Cable acceleration factor: relates cable length acceleration with end
+ * effector pose/twist/twist acceleration.  The cable mounting locations on the
+ * stationary frame and on the end-effector are passed as parameters.
+ * @author Gerry Chen
+ */
+
+#pragma once
+
+#include <gtdynamics/utils/DynamicsSymbol.h>
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+#include <boost/optional.hpp>
+
+#include <iostream>
+#include <string>
+
+namespace gtdynamics {
+
+/** CableAccelerationFactor is a 4-way nonlinear factor which enforces relation
+ * amongst cable acceleration, end-effector pose, end-effector twist, and
+ * end-effector twist acceleration
+ */
+class CableAccelerationFactor
+    : public gtsam::NoiseModelFactor4<double, gtsam::Pose3, gtsam::Vector6,
+                                      gtsam::Vector6> {
+ private:
+  using Point3 = gtsam::Point3;
+  using Vector3 = gtsam::Vector3;
+  using Pose3 = gtsam::Pose3;
+  using Vector6 = gtsam::Vector6;
+  using This = CableAccelerationFactor;
+  using Base = gtsam::NoiseModelFactor4<double, Pose3, Vector6, Vector6>;
+
+  Point3 wPa_, xPb_;
+
+ public:
+  /** Cable factor
+   * @param lddot_key -- key for cable speed
+   * @param wTx -- key for end effector pose
+   * @param Vx -- key for end effector twist
+   * @param VAx -- key for end effector twist acceleration
+   * @param cost_model -- noise model (1 dimensional)
+   * @param wPa -- cable mounting location on the fixed frame, in world coords
+   * @param xPb -- cable mounting location on the end effector, in the
+   * end-effector frame (wPb = wTx * xPb)
+   */
+  CableAccelerationFactor(gtsam::Key lddot_key, gtsam::Key wTx_key,
+                          gtsam::Key Vx_key, gtsam::Key VAx_key,
+                          const gtsam::noiseModel::Base::shared_ptr &cost_model,
+                          const Point3 &wPa, const Point3 &xPb)
+      : Base(cost_model, lddot_key, wTx_key, Vx_key, VAx_key),
+        wPa_(wPa),
+        xPb_(xPb) {}
+  virtual ~CableAccelerationFactor() {}
+
+ private:
+  /** Computes the cable acceleration that will result from some
+   * twist/twistaccel
+   * @param wTx the pose of the end effector
+   * @param Vx the twist of the end effector in the end effector's frame
+   * @param VAx the twist of the end effector in the end effector's frame
+   * @return double: calculated length acceleration change
+   */
+  double computeLddot(
+      const Pose3 &wTx, const Vector6 &Vx, const Vector6 &VAx,
+      boost::optional<gtsam::Matrix &> H_wTx = boost::none,
+      boost::optional<gtsam::Matrix &> H_Vx = boost::none,
+      boost::optional<gtsam::Matrix &> H_VAx = boost::none) const;
+
+ public:
+  /** Cable acceleration factor
+   * @param lddot -- cable speed (lddot)
+   * @param wTx -- end effector pose
+   * @param Vx -- end effector twist
+   * @param VAx -- end effector twist
+   * @return given lddot minus expected/calculated cable speed
+   */
+  gtsam::Vector evaluateError(
+      const double &lddot, const Pose3 &wTx, const Vector6 &Vx,
+      const Vector6 &VAx, boost::optional<gtsam::Matrix &> H_lddot = boost::none,
+      boost::optional<gtsam::Matrix &> H_wTx = boost::none,
+      boost::optional<gtsam::Matrix &> H_Vx = boost::none,
+      boost::optional<gtsam::Matrix &> H_VAx = boost::none) const override {
+    double expected_lddot = computeLddot(wTx, Vx, VAx, H_wTx, H_Vx, H_VAx);
+    if (H_lddot) *H_lddot = gtsam::I_1x1;
+    if (H_wTx) *H_wTx = -(*H_wTx);
+    if (H_Vx) *H_Vx = -(*H_Vx);
+    if (H_VAx) *H_VAx = -(*H_VAx);
+    return gtsam::Vector1(lddot - expected_lddot);
+  }
+
+  // @return a deep copy of this factor
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
+    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
+
+  /** print contents */
+  void print(const std::string &s = "",
+             const gtsam::KeyFormatter &keyFormatter =
+                 GTDKeyFormatter) const override {
+    std::cout << s << "cable accel factor" << std::endl;
+    Base::print("", keyFormatter);
+  }
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int version) {
+    ar &boost::serialization::make_nvp(
+        "NoiseModelFactor4", boost::serialization::base_object<Base>(*this));
+  }
+};
+
+}  // namespace gtdynamics

gtdynamics/cablerobot/factors/WinchFactor.h

@@ -0,0 +1,148 @@
+/**
+ * @file WinchFactor.h
+ * @brief Maps motor input torque to cable tension
+ * @author Gerry Chen
+ * 
+ * Applies a mapping from input torque to cable tension based on:
+ *  - winch radius
+ *  - motor inertia
+ *  - static friction
+ *  - viscous friction
+ * 
+ * Input torque is given by (current) * (motor torque constant)
+ * External torques are given by
+ *    (tension) * (radius) + 
+ *    (static friction) +
+ *    (viscous friction)
+ * These together cause:
+ *    (motor inertia) * (angular acceleration)
+ **/
+#pragma once
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Unit3.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+#include <boost/optional.hpp>
+#include <iostream>
+#include <string>
+
+namespace gtdynamics {
+
+/**
+ * WinchParams holds parameters relevant to winches
+ */
+struct WinchParams {
+  double radius_;           // winch radius in m
+  double inertia_;          // motor inertia in kg.m^2
+  double staticFriction_;   // static friction of the winch in N.m
+  double viscousFriction_;  // viscous friction of the winch in N.m / rad
+  WinchParams(double radius, double inertia, double staticFriction,
+              double viscousFriction)
+      : radius_(radius),
+        inertia_(inertia),
+        staticFriction_(staticFriction),
+        viscousFriction_(viscousFriction) {}
+};
+
+/**
+ * WinchFactor is a 4-way factor which converts motor input torque to cable
+ * tension, taking into account winch radius, inertia, and friction.
+ * 
+ * The relationship is given by
+ *  $$ tau = tension * radius - inertia * acceleration / radius + friction * radius $$
+ * where
+ *  $$ friction = - (static * sign(v) + viscous * v) $$
+ * 
+ * This is just rewriting $\sum{torques} = I * alpha$ and adjusting for sign
+ * convention (torque is backwards...)
+ * 
+ * Note that static friction causes a major discontinuity, so instead it is
+ * approximated as tanh(v/eps) as is done in Gouttefarde15TRO, with eps=1/50.
+ */
+class WinchFactor
+    : public gtsam::NoiseModelFactor4<double, double, double, double> {
+ private:
+  typedef WinchFactor This;
+  typedef gtsam::NoiseModelFactor4<double, double, double, double> Base;
+  const WinchParams params_;
+
+ public:
+  /** Winch factor
+   * @param torque -- key for motor torque input
+   * @param tenison -- key for cable tension
+   * @param cableVelocity -- key for cable velocity
+   * @param cableAcceleration -- key for cable acceleration
+   * @param cost_model -- noise model (1 dimensional)
+   * @param params -- winch parameters
+   */
+  WinchFactor(gtsam::Key torque, gtsam::Key tension, gtsam::Key cableVelocity,
+              gtsam::Key cableAcceleration,
+              const gtsam::noiseModel::Base::shared_ptr &cost_model,
+              const WinchParams &params)
+      : Base(cost_model, torque, tension, cableVelocity, cableAcceleration),
+        params_(params) {}
+  virtual ~WinchFactor() {}
+
+ public:
+  /** Winch factor
+   * @param torque -- motor torque input
+   * @param tension -- cable tension
+   * @param vel -- cable velocity
+   * @param acc -- cable acceleration
+   * @return cable torque minus computed cable torque
+   */
+  gtsam::Vector evaluateError(
+      const double &torque, const double &tension, const double &vel,
+      const double &acc,
+      boost::optional<gtsam::Matrix &> H_torque = boost::none,
+      boost::optional<gtsam::Matrix &> H_tension = boost::none,
+      boost::optional<gtsam::Matrix &> H_vel = boost::none,
+      boost::optional<gtsam::Matrix &> H_acc = boost::none) const override {
+    const double &r = params_.radius_;
+    const double &I = params_.inertia_;
+    const double &mu = params_.staticFriction_;
+    const double &b = params_.viscousFriction_;
+
+    double sign_vel = tanh(50 * vel);  // tanh approximation to sign(vel)
+
+    if (H_torque) *H_torque = gtsam::Vector1(1);
+    if (H_tension) *H_tension = gtsam::Vector1(-r);
+    if (H_vel)
+      *H_vel = gtsam::Vector1(mu * 50 * (1 - sign_vel * sign_vel) + b / r);
+    if (H_acc) *H_acc = gtsam::Vector1(I / r);
+
+    return gtsam::Vector1(torque -            // tau =
+                          tension * r +       //  F * r -
+                          I * acc / r -       //  I * alpha +
+                          (-mu * sign_vel) -  //  static friction +
+                          (-b * vel / r));    //  viscous friction
+  }
+
+  // @return a deep copy of this factor
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
+    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
+
+  /** print contents */
+  void print(const std::string &s = "",
+             const gtsam::KeyFormatter &keyFormatter =
+                 GTDKeyFormatter) const override {
+    std::cout << s << "winch factor" << std::endl;
+    Base::print("", keyFormatter);
+  }
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int version) {
+    ar &boost::serialization::make_nvp(
+        "NoiseModelFactor4", boost::serialization::base_object<Base>(*this));
+    ar &BOOST_SERIALIZATION_NVP(params_);
+  }
+};
+
+}  // namespace gtdynamics