diff --git a/gtdynamics/cablerobot/CMakeLists.txt b/gtdynamics/cablerobot/CMakeLists.txt
index 6c2c74dd..9b57efba 100644
--- a/gtdynamics/cablerobot/CMakeLists.txt
+++ b/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
diff --git a/gtdynamics/cablerobot/cablerobot.i b/gtdynamics/cablerobot/cablerobot.i
index 7de4255e..4a6ed386 100644
--- a/gtdynamics/cablerobot/cablerobot.i
+++ b/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
diff --git a/gtdynamics/cablerobot/factors/CableAccelerationFactor.cpp b/gtdynamics/cablerobot/factors/CableAccelerationFactor.cpp
new file mode 100644
index 00000000..baafa659
--- /dev/null
+++ b/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
diff --git a/gtdynamics/cablerobot/factors/CableAccelerationFactor.h b/gtdynamics/cablerobot/factors/CableAccelerationFactor.h
new file mode 100644
index 00000000..72937e5e
--- /dev/null
+++ b/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
diff --git a/gtdynamics/cablerobot/factors/WinchFactor.h b/gtdynamics/cablerobot/factors/WinchFactor.h
new file mode 100644
index 00000000..2a2e74e2
--- /dev/null
+++ b/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
diff --git a/gtdynamics/cablerobot/src/cdpr_controller.py b/gtdynamics/cablerobot/src/cdpr_controller.py
new file mode 100644
index 00000000..b0d61b5d
--- /dev/null
+++ b/gtdynamics/cablerobot/src/cdpr_controller.py
@@ -0,0 +1,36 @@
+"""
+GTDynamics Copyright 2021, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+See LICENSE for the license information
+
+@file  cdpr_controller.py
+@brief Base class for a controller for a cable robot.
+@author Frank Dellaert
+@author Gerry Chen
+"""
+
+import gtsam
+import gtdynamics as gtd
+import numpy as np
+import utils
+
+class CdprControllerBase:
+    """Interface for cable robot controllers
+    """
+    @property
+    def update(self, values, t):
+        """gives the new control input given current measurements
+
+        Args:
+            values (gtsam.Values): values object will contain at least the current Pose and Twist,
+            but should often also include the current joint angles and velocities
+            t (int): The current time index (discrete time index)
+
+        Returns:
+            gtsam.Values: A values object which contains the joint torques for this time step.
+
+        Raises:
+            NotImplementedError: Derived classes must override this function
+        """
+        raise NotImplementedError("CdprControllers need to implement the `update` function")
diff --git a/gtdynamics/cablerobot/src/cdpr_controller_ilqr.py b/gtdynamics/cablerobot/src/cdpr_controller_ilqr.py
new file mode 100644
index 00000000..e17521bf
--- /dev/null
+++ b/gtdynamics/cablerobot/src/cdpr_controller_ilqr.py
@@ -0,0 +1,271 @@
+"""
+GTDynamics Copyright 2021, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+See LICENSE for the license information
+
+@file  cdpr_controller_ilqr.py
+@brief Optimal controller for a cable robot.  Solved by creating a factor graph and adding state
+objectives and control costs, then optimizing
+@author Frank Dellaert
+@author Gerry Chen
+"""
+
+import gtsam
+import gtdynamics as gtd
+import numpy as np
+import utils
+from cdpr_controller import CdprControllerBase
+from scipy.linalg import solve_triangular
+import time
+
+
+class CdprControllerIlqr(CdprControllerBase):
+    """Precomputes the open-loop trajectory then just calls on that for each update.
+    """
+    def __init__(self,
+                 cdpr,
+                 x0,
+                 pdes=[],
+                 dt=0.01,
+                 Q=None,
+                 R=np.array([1.]),
+                 x_guess=None,
+                 debug=False,
+                 progress=None):
+        """constructor
+
+        Args:
+            cdpr (Cdpr): cable robot object
+            x0 (gtsam.Values): initial state
+            pdes (list, optional): list of desired poses. Defaults to [].
+            dt (float, optional): time step duration. Defaults to 0.01.
+            Q (np.ndarray, optional): State objective cost (as a vector). Defaults to None, which
+            denotes a constrained noise model.
+            R (np.ndarray, optional): Control cost (as a 1-vector). Defaults to np.array([1.]).
+        """
+        self.cdpr = cdpr
+        self.pdes = pdes
+        self.dt = dt
+
+        # create iLQR graph
+        fg = self.create_ilqr_fg(cdpr, x0, pdes, dt, Q, R)
+
+        # initial guess
+        if x_guess is None:
+            x_guess = gtsam.Values()
+            for k, pdes_ in enumerate(pdes):
+                gtd.InsertPose(x_guess, cdpr.ee_id(), k, pdes_)
+                for ji in range(4):
+                    l = np.linalg.norm(cdpr.params.a_locs[ji] - pdes_.translation())
+                    gtd.InsertJointAngle(x_guess, ji, k, l)
+        x_guess = utils.InitValues(fg, x_guess, dt=dt)
+
+        # optimize
+        params = utils.MyLMParams(None)
+        # params.setRelativeErrorTol(1e-10)
+        # params.setAbsoluteErrorTol(1e-10)
+        # params.setVerbosityLM('TRYLAMBDA')
+        if progress is not None:
+            def hook(iter, errorBefore, errorAfter):
+                progress.update()
+                progress.set_postfix(iter=iter, errorBefore=errorBefore, errorAfter=errorAfter)
+            params.iterationHook = hook
+        self.optimizer = gtsam.LevenbergMarquardtOptimizer(fg, x_guess, params)
+        def optimize(optimizer): # so this shows up in the profiler
+            return optimizer.optimize()
+        tstart = time.time()
+        self.result = optimize(self.optimizer)
+        tend = time.time()
+
+        # gains
+        self.gains_ff = self.extract_gains_ff(cdpr, fg, self.result, len(self.pdes))
+
+        # print debug info
+        if debug:
+            print("FG size: ", fg.size())
+            print("\tNumber of variables: ", len(x_guess.keys()))
+            print("\tNumber of timesteps: ", len(pdes))
+            print("\tTime to optimize: ", tend - tstart)
+
+    def update(self, values, t):
+        """New control: returns the entire results vector, which contains the optimal open-loop
+        control from the optimal trajectory.
+        """
+        K, uff, Vff, Tff = self.gains_ff[t]
+        # compute dx
+        Vhat = gtd.Twist(values, self.cdpr.ee_id(), t)
+        That = gtd.Pose(values, self.cdpr.ee_id(), t)
+        dx = np.hstack((Vhat - Vff, Tff.localCoordinates(That)))
+        #compute u
+        u = K @ dx + uff
+        # populate into values object
+        result = gtsam.Values()
+        for ji in range(4):
+            gtd.InsertTorque(result, ji, t, u[ji])
+        return result
+
+    @staticmethod
+    def create_ilqr_fg(cdpr, x0, pdes, dt, Q, R):
+        """Creates the factor graph for the iLQR problem.  This essentially consists of creating a
+        factor graph that describes the CDPR dynamics over all time steps, then adding state
+        objective and control cost factors.
+
+        Args:
+            cdpr (Cdpr): cable robot object
+            x0 (gtsam.Values): initial Pose/Twist of the cable robot
+            pdes (List[gtsam.Pose3]): desired poses of the cdpr
+            dt (float): time step duration
+            Q (Union[np.ndarray, None]): The state objective cost (None for hard constraint)
+            R (np.ndarray): The control cost
+
+        Returns:
+            gtsam.NonlinearFactorGraph: The factor graph corresponding to the iLQR problem.
+        """
+        N = len(pdes)
+        # initial conditions
+        fg = cdpr.priors_ik(ks=[0],
+                            Ts=[gtd.Pose(x0, cdpr.ee_id(), 0)],
+                            Vs=[gtd.Twist(x0, cdpr.ee_id(), 0)])
+        # dynamics
+        fg.push_back(cdpr.all_factors(N, dt))
+        # control costs
+        tmid = (cdpr.params.tmin + cdpr.params.tmax) / 2
+        for k in range(N):
+            for ji in range(4):
+                fg.push_back(
+                    gtd.PriorFactorDouble(gtd.TorqueKey(ji, k).key(), tmid,
+                                          gtsam.noiseModel.Diagonal.Precisions(R)))
+        # state objective costs
+        cost_x = gtsam.noiseModel.Isotropic.Sigma(6, 0.001) if Q is None else \
+            gtsam.noiseModel.Diagonal.Precisions(Q)
+        for k in range(N):
+            fg.push_back(
+                gtsam.PriorFactorPose3(
+                    gtd.PoseKey(cdpr.ee_id(), k).key(), pdes[k], cost_x))
+        return fg
+
+    @staticmethod
+    def extract_bayesnets(cdpr, fg, openloop_results, N):
+        lid = cdpr.ee_id()
+        gfg = fg.linearize(openloop_results)
+
+        # ordering
+        ordering = []
+        # ordering.append(gtsam.Ordering())
+        for t in range(N - 1, -1, -1):
+            # stuff we don't care about
+            ordering.append(gtsam.Ordering())
+            for ji in range(4):
+                ordering[-1].push_back(gtd.JointAngleKey(ji, t).key())
+            for ji in range(4):
+                ordering[-1].push_back(gtd.JointVelKey(ji, t).key())
+            for ji in range(4):
+                ordering[-1].push_back(gtd.JointAccelKey(ji, t).key())
+            ordering[-1].push_back(gtd.TwistAccelKey(lid, t).key())
+            for ji in range(4):
+                ordering[-1].push_back(gtd.WrenchKey(lid, ji, t).key())
+            for ji in range(4):
+                ordering[-1].push_back(gtd.TensionKey(ji, t).key())
+            # control variables
+            ordering.append(gtsam.Ordering())
+            for ji in range(4):
+                ordering[-1].push_back(gtd.TorqueKey(ji, t).key())
+            # measurement inputs
+            ordering.append(gtsam.Ordering())
+            ordering[-1].push_back(gtd.TwistKey(lid, t).key())
+            ordering[-1].push_back(gtd.PoseKey(lid, t).key())
+        ordering.append(gtsam.Ordering())
+        ordering[-1].push_back(0)
+
+        # eliminate
+        return gtd.BlockEliminateSequential(gfg, ordering), reversed(range(3, 4*N, 4))
+
+    @staticmethod
+    def extract_gains(cdpr, fg, openloop_results, N):
+        """Extracts the locally linear optimal feedback control gains
+
+        Args:
+            cdpr (Cdpr): cable robot object
+            fg (gtsam.NonlinearFactorGraph): The iLQR factor graph
+            openloop_results (gtsam.Values): The open-loop optimal trajectory and controls
+            N (int): The total number of timesteps in the iLQR factor graph
+        Returns:
+            gains (List[np.ndarray]): The feedback gains in the form u = K*(x-xff) + uff
+        """
+        lid = cdpr.ee_id()
+        net, u_inds = CdprControllerIlqr.extract_bayesnets(cdpr, fg, openloop_results, N)
+
+        # extract_gains
+        gains = [None for t in range(N)]
+        # for t, neti in enumerate(u_inds):
+        # 0 mod 3: ("stuff we do care about") -> ("stuff we don't care about")
+        # 1 mod 3: (twist, pose) -> torques
+        # 2 mod 3: (prev state) -> (twist, pose)   aka collocation update
+        for t, (neti, netu, netx) in enumerate(
+                zip(
+                    reversed(range(0, 3 * N, 3)),  #
+                    reversed(range(1, 3 * N, 3)),
+                    reversed(range(2, 3 * N, 3)))):
+            ucond = net.at(netu)
+            u_K_x = solve_triangular(ucond.R(), -ucond.S()[:, -6:])
+            u_K_v = solve_triangular(ucond.R(), -ucond.S()[:, -12:-6])
+            gains[t] = np.hstack((u_K_v, u_K_x))
+        return gains
+
+    @staticmethod
+    def extract_uff(results, N):
+        """Extracts the feedforward control terms in a more python-friendly format
+
+        Args:
+            results (gtsam.Values): contains the result of optimizing an iLQR factor graph
+            N (int): The total number of timesteps in the iLQR factor graph
+
+        Returns:
+            List[np.ndarray]: Feedforward control terms.
+        """
+        uff = []
+        for t in range(N):
+            uff.append(np.array([gtd.Torque(results, ji, t) for ji in range(4)]))
+        return uff
+
+    @staticmethod
+    def extract_xff(cdpr, results, N):
+        """Extracts the feedforward trajectory terms in a more python-friendly format
+
+        Args:
+            cdpr (Cdpr): cable robot
+            results (gtsam.Values): contains the result of optimizing an iLQR factor graph
+            N (int): The total number of timesteps in the iLQR factor graph
+
+        Returns:
+            List[Tuple[np.ndarray, gtsam.Pose3]]: Feedforward twist and pose terms, where each
+            element of the list contains a tuple of (Twist, Pose).
+        """
+        xff = []
+        for t in range(N):
+            xff.append((
+                gtd.Twist(results, cdpr.ee_id(), t),  #
+                gtd.Pose(results, cdpr.ee_id(), t)))
+        return xff
+
+    @staticmethod
+    def extract_gains_ff(cdpr, fg, openloop_results, N):
+        """Extracts both the gains and the feedforward controls/trajectory terms of the form
+        u = K*(x-xff) + uff
+
+        Args:
+            cdpr (Cdpr): cable robot object
+            fg (gtsam.NonlinearFactorGraph): The iLQR factor graph
+            openloop_results (gtsam.Values): The open-loop optimal trajectory and controls
+            N (int): The total number of timesteps in the iLQR factor graph
+        
+        Returns:
+            List[Tuple[np.ndarray, np.ndarray, np.ndarray, gtsam.Pose3]]: Gains and feedforward
+            terms as a list of tuples: (K, tension_ff, twist_ff, pose_ff) where u = K*(x-xff) + uff
+            and x stacks twist on top of pose.
+        """
+        return list(
+            zip(CdprControllerIlqr.extract_gains(cdpr, fg, openloop_results, N),
+                CdprControllerIlqr.extract_uff(openloop_results, N),
+                *zip(*CdprControllerIlqr.extract_xff(cdpr, openloop_results, N))))
diff --git a/gtdynamics/cablerobot/src/cdpr_controller_tension_dist.py b/gtdynamics/cablerobot/src/cdpr_controller_tension_dist.py
new file mode 100644
index 00000000..b803bce6
--- /dev/null
+++ b/gtdynamics/cablerobot/src/cdpr_controller_tension_dist.py
@@ -0,0 +1,159 @@
+"""
+GTDynamics Copyright 2021, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+See LICENSE for the license information
+
+@file  cdpr_controller_tension_dist.py
+@brief Controller for a cable robot based on computing the wrench necessary to track the desired
+trajectory exactly followed by applying a tension distribution algorithm to compute the necessary
+torque to achieve said wrench.
+@author Frank Dellaert
+@author Gerry Chen
+"""
+
+import gtsam
+import gtdynamics as gtd
+import numpy as np
+import utils
+from cdpr_controller import CdprControllerBase
+from cdpr_planar import Cdpr
+
+class CdprControllerTensionDist(CdprControllerBase):
+    """Does whatever wrench is needed to follow the desired trajectory exactly.  Uses a simple
+    least-squares minimum effort tension distribution algorithm (using GTSAM) to find the tensions
+    necessary to enact such a wrench.
+    """
+    def __init__(self, cdpr, x0, pdes=[], dt=0.01, Q=None, R=np.array([1.])):
+        """constructor
+
+        Args:
+            cdpr (Cdpr): cable robot object
+            x0 (gtsam.Values): initial state
+            pdes (list, optional): list of desired poses. Defaults to [].
+            dt (float, optional): time step duration. Defaults to 0.01.
+            R (np.ndarray, optional): Control cost (as a 1-vector). Defaults to np.array([1.]).
+        """
+        self.cdpr = cdpr
+        self.pdes = pdes
+        self.dt = dt
+        self.R = R
+        N = len(pdes)
+
+    def update(self, values, t):
+        """New control: returns the entire results vector, which contains the optimal open-loop
+        control from the optimal trajectory.
+        """
+        return self.solve_one_step(self.cdpr,
+                                   self.cdpr.ee_id(),
+                                   self.pdes[t+1],
+                                   t,
+                                   TVnow=values,
+                                   dt=self.dt,
+                                   R=self.R)
+
+    @staticmethod
+    def solve_one_step(cdpr, lid, Tgoal, k, TVnow=None, lldotnow=None, dt=0.01, R=np.ones(1), debug=False):
+        """Creates the factor graph for the tension distribution problem.  This essentially consists
+        of creating a factor graph that describes the CDPR dynamics for this one timestep, then
+        adding control cost factors.  Either the current pose/twist may be specified, or the current
+        cable lengths/velocities may be specified (in which case FK will be used to calculate the
+        pose/twist).
+
+        Args:
+            cdpr (Cdpr): cable robot object
+            Tgoal (gtsam.Pose3): goal pose for the next time step
+            TVnow (gtsam.Values, optional): The current pose and twist
+            lldotnow (gtsam.Values, optional): The current cable lengths / speeds
+            dt (float): time step duration
+            R (np.ndarray): The control cost
+
+        Returns:
+            gtsam.Values: a Values object containing the control torques
+        """
+
+        # First solve for required TwistAccel
+        VAres = CdprControllerTensionDist.solve_twist_accel(cdpr, lid, Tgoal, k, TVnow, lldotnow,
+                                                            dt)
+
+        # Now solve for required torques
+        result = CdprControllerTensionDist.solve_torques(cdpr, lid, k, VAres, VAres, dt, R)
+
+        # Debug
+        if debug:
+            print("My goal for k = {:d} is:".format(k), Tgoal.translation())
+            print("\cur pose: ", gtd.Pose(TVnow, cdpr.ee_id(), k).translation())
+            print("\tcur Twist: ", gtd.Twist(TVnow, cdpr.ee_id(), k)[3:])
+            print("\tcur pose VA: ", gtd.Pose(VAres, cdpr.ee_id(), k).translation())
+            print("\tnext pose VA: ", gtd.Pose(VAres, cdpr.ee_id(), k + 1).translation())
+            print("\tcur Twist VA: ", gtd.Twist(VAres, cdpr.ee_id(), k)[3:])
+            print("\tnext Twist VA: ", gtd.Twist(VAres, cdpr.ee_id(), k + 1)[3:])
+            print("\tcur TwistAccel VA: ", gtd.TwistAccel(VAres, cdpr.ee_id(), k)[3:])
+            print("\tcur TwistAccel res: ", gtd.TwistAccel(result, cdpr.ee_id(), k)[3:])
+            print("\tcur Torques res: ", [gtd.TorqueDouble(result, ji, k) for ji in range(4)])
+
+        return result
+
+    @staticmethod
+    def solve_twist_accel(cdpr, lid, Tgoal, k, TVnow=None, lldotnow=None, dt=0.01):
+        fg = gtsam.NonlinearFactorGraph()
+
+        # IK: either solve for current pose T given measurements, or use open-loop solution
+        if lldotnow is not None:
+            fg.push_back(cdpr.kinematics_factors(ks=[k]))
+            fg.push_back(cdpr.priors_fk(ks=[k], values=lldotnow))
+        else:
+            fg.push_back(cdpr.priors_ik(ks=[k], values=TVnow))
+        # pose constraints: must reach next pose T
+        fg.push_back(gtsam.PriorFactorPose3(gtd.PoseKey(lid, k+1).key(),
+                                            Tgoal, cdpr.costmodel_prior_pose))
+        # collocation: given current+next Ts, solve for current+next Vs and current VAs
+        fg.push_back(cdpr.collocation_factors(ks=[k], dt=dt))
+
+        xk = gtsam.Values()
+        xk.insert(0, dt)
+        if lldotnow is not None:
+            utils.InsertJointAngles(xk, k, lldotnow)
+            utils.InsertJointVels(xk, k, lldotnow)
+            gtd.InsertPose(xk, lid, k, gtsam.Pose3())
+            gtd.InsertTwist(xk, lid, k, np.zeros(6))
+        else:
+            utils.InsertPose(xk, lid, k, TVnow)
+            utils.InsertTwist(xk, lid, k, TVnow)
+        gtd.InsertPose(xk, lid, k+1, Tgoal)
+        gtd.InsertTwist(xk, lid, k+1, np.zeros(6))
+        gtd.InsertTwistAccel(xk, lid, k, np.zeros(6))
+
+        return gtsam.LevenbergMarquardtOptimizer(fg, xk, utils.MyLMParams()).optimize()
+
+    @staticmethod
+    def solve_torques(cdpr: Cdpr, lid, k, VAnow, TVnow, dt, R):
+        fg = gtsam.NonlinearFactorGraph()
+        # priors
+        fg.push_back(cdpr.priors_ik(ks=[k], values=TVnow))
+        fg.push_back(cdpr.priors_id_va(ks=[k], values=VAnow))
+
+        # dynamics: given VA, solve for torque/wrenches
+        fg.push_back(cdpr.dynamics_factors(ks=[k]))
+        # redundancy resolution: control costs
+        for ji in range(4):
+            fg.push_back(
+                gtd.PriorFactorDouble(
+                    gtd.TorqueKey(ji, k).key(),  #
+                    0.0,
+                    gtsam.noiseModel.Diagonal.Precisions(R)))
+
+        # tmp initial guess
+        xk = gtsam.Values()
+        utils.InsertPose(xk, lid, k, TVnow)
+        utils.InsertTwist(xk, lid, k, TVnow)
+        gtd.InsertTwistAccel(xk, lid, k, np.zeros(6))
+        for ji in range(4):
+            gtd.InsertJointVel(xk, ji, k, 0)
+            gtd.InsertJointAccel(xk, ji, k, 1)
+            gtd.InsertTorque(xk, ji, k, 1)
+            gtd.InsertTension(xk, ji, k, 50)
+            gtd.InsertWrench(xk, lid, ji, k, np.zeros(6))
+
+        # optimize
+        return gtsam.LevenbergMarquardtOptimizer(fg, xk, utils.MyLMParams()).optimize()
diff --git a/gtdynamics/cablerobot/src/cdpr_planar.py b/gtdynamics/cablerobot/src/cdpr_planar.py
index a3c8fe93..c00ba035 100644
--- a/gtdynamics/cablerobot/src/cdpr_planar.py
+++ b/gtdynamics/cablerobot/src/cdpr_planar.py
@@ -28,6 +28,14 @@ def __init__(self):
         self.mass = 1.0
         self.inertia = np.eye(3)
         self.gravity = np.zeros((3, 1))
+        self.tmin = 0.1  # minimum torque in N.m
+        self.tmax = 1.2
+        self.winch_params = gtd.WinchParams(inertia=0,
+                                            radius=1,
+                                            staticFriction=0,
+                                            viscousFriction=0)
+        self.collocation_mode = 0
+
 
 
 class Cdpr:
@@ -37,24 +45,25 @@ class Cdpr:
     def __init__(self, params=CdprParams()):
         self.params = params
         ee = gtd.Link(1, "ee", params.mass, params.inertia, Pose3(), Pose3())
-        self.robot = gtd.Robot({'ee': ee}, {})
-        self.costmodel_l = gtsam.noiseModel.Isotropic.Sigma(1, 0.001)
-        self.costmodel_ldot = gtsam.noiseModel.Isotropic.Sigma(1, 0.001)
-        self.costmodel_wrench = gtsam.noiseModel.Isotropic.Sigma(6, 0.001)
-        self.costmodel_torque = gtsam.noiseModel.Isotropic.Sigma(6, 0.001)
-        self.costmodel_twistcollo = gtsam.noiseModel.Isotropic.Sigma(6, 0.001)
-        self.costmodel_posecollo = gtsam.noiseModel.Isotropic.Sigma(6, 0.001)
-        self.costmodel_prior_l = gtsam.noiseModel.Isotropic.Sigma(1, 0.001)
-        self.costmodel_prior_ldot = gtsam.noiseModel.Isotropic.Sigma(1, 0.001)
-        self.costmodel_prior_tau = gtsam.noiseModel.Isotropic.Sigma(1, 0.001)
-        self.costmodel_prior_pose = gtsam.noiseModel.Isotropic.Sigma(6, 0.001)
-        self.costmodel_prior_twist = gtsam.noiseModel.Isotropic.Sigma(6, 0.001)
-        self.costmodel_prior_twistaccel = gtsam.noiseModel.Isotropic.Sigma(
-            6, 0.001)
-        self.costmodel_planar_pose = gtsam.noiseModel.Isotropic.Sigma(3, 0.001)
-        self.costmodel_planar_twist = gtsam.noiseModel.Isotropic.Sigma(
-            3, 0.001)
-        self.costmodel_dt = gtsam.noiseModel.Isotropic.Sigma(1, 0.001)
+        self.robot = gtd.Robot({'ee' : ee}, {})
+        self.costmodel_l = gtsam.noiseModel.Constrained.All(1)
+        self.costmodel_ldot = gtsam.noiseModel.Constrained.All(1)
+        self.costmodel_lddot = gtsam.noiseModel.Constrained.All(1)
+        self.costmodel_winch = gtsam.noiseModel.Constrained.All(1)
+        self.costmodel_torque = gtsam.noiseModel.Constrained.All(6)
+        self.costmodel_wrench = gtsam.noiseModel.Constrained.All(6)
+        self.costmodel_twistcollo = gtsam.noiseModel.Constrained.All(6)
+        self.costmodel_posecollo = gtsam.noiseModel.Constrained.All(6)
+        self.costmodel_prior_l = gtsam.noiseModel.Constrained.All(1)
+        self.costmodel_prior_ldot = gtsam.noiseModel.Constrained.All(1)
+        self.costmodel_prior_lddot = gtsam.noiseModel.Constrained.All(1)
+        self.costmodel_prior_tau = gtsam.noiseModel.Constrained.All(1)
+        self.costmodel_prior_pose = gtsam.noiseModel.Constrained.All(6)
+        self.costmodel_prior_twist = gtsam.noiseModel.Constrained.All(6)
+        self.costmodel_prior_twistaccel = gtsam.noiseModel.Constrained.All(6)
+        self.costmodel_planar_pose = gtsam.noiseModel.Constrained.All(3)
+        self.costmodel_planar_twist = gtsam.noiseModel.Constrained.All(3)
+        self.costmodel_dt = gtsam.noiseModel.Constrained.All(1)
 
     def eelink(self):
         """Link object for the end-effector
@@ -147,9 +156,9 @@ def kinematics_factors(self, ks=[]):
     def dynamics_factors(self, ks=[]):
         """Creates factors necessary for dynamics calculations.  Specifically, consists of the
         generalized version of F=ma and calculates wrenches from cable tensions.
-        Primary variables:          Torque <--> TwistAccel
-        Intermediate variables:     Wrenches
-        Prerequisite variables:     Pose, Twist
+        Primary variables:          Torque <--> lengthddot
+        Intermediate variables:     Wrenches, TwistAccel
+        Prerequisite variables:     Pose, Twist, lengthdot, tension
 
 
         Args:
@@ -171,11 +180,26 @@ def dynamics_factors(self, ks=[]):
             for ji in range(4):
                 dfg.push_back(
                     gtd.CableTensionFactor(
-                        gtd.TorqueKey(ji, k).key(),
+                        gtd.TensionKey(ji, k).key(),
                         gtd.PoseKey(self.ee_id(), k).key(),
-                        gtd.WrenchKey(self.ee_id(), ji,
-                                      k).key(), self.costmodel_torque,
-                        self.params.a_locs[ji], self.params.b_locs[ji]))
+                        gtd.WrenchKey(self.ee_id(), ji, k).key(),
+                        self.costmodel_torque, self.params.a_locs[ji], self.params.b_locs[ji]))
+                dfg.push_back(
+                    gtd.CableAccelerationFactor(
+                        gtd.JointAccelKey(ji, k).key(),
+                        gtd.PoseKey(self.ee_id(), k).key(),
+                        gtd.TwistKey(self.ee_id(), k).key(),
+                        gtd.TwistAccelKey(self.ee_id(), k).key(),
+                        self.costmodel_lddot, self.params.a_locs[ji], self.params.b_locs[ji]))
+                dfg.push_back(
+                    gtd.WinchFactor(
+                        gtd.TorqueKey(ji, k).key(),
+                        gtd.TensionKey(ji, k).key(),
+                        gtd.JointVelKey(ji, k).key(),
+                        gtd.JointAccelKey(ji, k).key(),
+                        self.costmodel_winch, self.params.winch_params
+                    )
+                )
         return dfg
 
     def collocation_factors(self, ks=[], dt=0.01):
@@ -192,36 +216,61 @@ def collocation_factors(self, ks=[], dt=0.01):
             gtsam.NonlinearFactorGraph: the collocation factors
         """
         dfg = gtsam.NonlinearFactorGraph()
-        for k in ks:
-            dfg.push_back(
-                gtd.EulerPoseCollocationFactor(
-                    gtd.PoseKey(self.ee_id(), k).key(),
-                    gtd.PoseKey(self.ee_id(), k + 1).key(),
-                    gtd.TwistKey(self.ee_id(), k).key(), 0,
-                    self.costmodel_posecollo))
-            dfg.push_back(
-                gtd.EulerTwistCollocationFactor(
-                    gtd.TwistKey(self.ee_id(), k).key(),
-                    gtd.TwistKey(self.ee_id(), k + 1).key(),
-                    gtd.TwistAccelKey(self.ee_id(), k).key(), 0,
-                    self.costmodel_twistcollo))
+        if self.params.collocation_mode == 1:
+            for k in ks:
+                dfg.push_back(
+                    gtd.TrapezoidalPoseCollocationFactor(
+                        gtd.PoseKey(self.ee_id(), k).key(),
+                        gtd.PoseKey(self.ee_id(), k + 1).key(),
+                        gtd.TwistKey(self.ee_id(), k).key(),  #
+                        gtd.TwistKey(self.ee_id(), k + 1).key(),  #
+                        0,
+                        self.costmodel_posecollo))
+                dfg.push_back(
+                    gtd.EulerTwistCollocationFactor(
+                        gtd.TwistKey(self.ee_id(), k).key(),
+                        gtd.TwistKey(self.ee_id(), k + 1).key(),
+                        gtd.TwistAccelKey(self.ee_id(), k).key(),  #
+                        0,
+                        self.costmodel_twistcollo))
+        else:
+            for k in ks:
+                dfg.push_back(
+                    gtd.EulerPoseCollocationFactor(
+                        gtd.PoseKey(self.ee_id(), k).key(),
+                        gtd.PoseKey(self.ee_id(), k + 1).key(),
+                        gtd.TwistKey(self.ee_id(), k).key(), #
+                        0,
+                        self.costmodel_posecollo))
+                dfg.push_back(
+                    gtd.EulerTwistCollocationFactor(
+                        gtd.TwistKey(self.ee_id(), k).key(),
+                        gtd.TwistKey(self.ee_id(), k + 1).key(),
+                        gtd.TwistAccelKey(self.ee_id(), k).key(),  #
+                        0,
+                        self.costmodel_twistcollo))
         dfg.push_back(gtd.PriorFactorDouble(0, dt, self.costmodel_dt))
         return dfg
 
-    def priors_fk(self, ks=[], ls=[[]], ldots=[[]]):
+    def priors_fk(self, ks=[], ls=[[]], ldots=[[]], values=None):
         """Creates prior factors which correspond to solving the forward kinematics problem by
         specifying the joint angles and velocities.  To be used with kinematics_factors to optimize
-        for the Pose and Twist.
+        for the Pose and Twist.  Either supply ks/ls/ldots, or ks/values.  If values is supplied, ls
+        and ldots will be ignored.
 
         Args:
             ks (list, optional): Time step indices. Defaults to [].
             ls (list, optional): List of list joint angles for each time step. Defaults to [[]].
             ldots (list, optional): List of list of joint velocities for each time step. Defaults to
             [[]].
+            values (gtsam.Values, optional): Values object containing all the needed key/values.
 
         Returns:
             gtsam.NonlinearFactorGraph: The forward kinematics prior factors
         """
+        if values is not None:
+            ls = [[gtd.JointAngle(values, ji, k) for ji in range(4)] for k in ks]
+            ldots = [[gtd.JointVel(values, ji, k) for ji in range(4)] for k in ks]
         graph = gtsam.NonlinearFactorGraph()
         for k, l, ldot in zip(ks, ls, ldots):
             for ji, (lval, ldotval) in enumerate(zip(l, ldot)):
@@ -235,19 +284,24 @@ def priors_fk(self, ks=[], ls=[[]], ldots=[[]]):
                         self.costmodel_prior_ldot))
         return graph
 
-    def priors_ik(self, ks=[], Ts=[], Vs=[]):
+    def priors_ik(self, ks=[], Ts=[], Vs=[], values=None):
         """Creates prior factors which correspond to solving the inverse kinematics problem by
         specifying the Pose/Twist of the end effector.  To be used with kinematics_factors to
-        optimize for the joint angles and velocities.
+        optimize for the joint angles and velocities.  Either supply ks/Ts/Vs, or ks/values.  If
+        values is supplied, Ts and Vs will be ignored.
 
         Args:
             ks (list, optional): Time step indices. Defaults to [].
             Ts (list, optional): List of Poses for each time step. Defaults to [[]].
             Vs (list, optional): List of Twists for each time step. Defaults to [[]].
+            values (gtsam.Values, optional): Values object containing all the needed key/values.
 
         Returns:
             gtsam.NonlinearFactorGraph: The inverve kinematics prior factors
         """
+        if values is not None:
+            Ts = [gtd.Pose(values, self.ee_id(), k) for k in ks]
+            Vs = [gtd.Twist(values, self.ee_id(), k) for k in ks]
         graph = gtsam.NonlinearFactorGraph()
         for k, T, V in zip(ks, Ts, Vs):
             graph.push_back(
@@ -260,23 +314,24 @@ def priors_ik(self, ks=[], Ts=[], Vs=[]):
                     self.costmodel_prior_twist))
         return graph
 
-    # note: I am not using the strict definitions for forward/inverse dynamics.
-    # priors_fd solves for torques given twistaccel (no joint accel)
-    # priors_id solves for twistaccel (no joint accel) given torques
-    def priors_id(self, ks=[], torquess=[[]]):
-        """Creates factors roughly corresponding to the inverse dynamics problem.  While strictly
-        inverse dynamics in Lynch & Park refers to the problem of calculating joint accelerations
-        given joint torques, temproarily this function is more convenient which directly relates
-        constrains joint torques (to obtain twist accelerations when used with dynamics_factors).
+    # priors_fd solves for joint accel given torques
+    # priors_id solves for torques given joint accel
+    # priors_id_va solves for torques given twist accel (since this may optimize easier)
+    def priors_fd(self, ks=[], torquess=[[]], values=None):
+        """Creates factors roughly corresponding to the forward dynamics problem.  Either supply
+        ks/torquess, or ks/values.  If values is supplied, torquess will be ignored.
 
         Args:
             ks (list, optional): Time step indices. Defaults to [].
             torquess (list, optional): List of list of joint torques for each time step. Defaults to
             [[]].
+            values (gtsam.Values, optional): Values object containing all the needed key/values.
 
         Returns:
-            gtsam.NonlinearFactorGraph: The inverse dynamics prior factors
+            gtsam.NonlinearFactorGraph: The forward dynamics prior factors
         """
+        if values is not None:
+            torquess = [[gtd.Torque(values, ji, k) for ji in range(4)] for k in ks]
         graph = gtsam.NonlinearFactorGraph()
         for k, torques in zip(ks, torquess):
             for ji, torque in enumerate(torques):
@@ -286,19 +341,47 @@ def priors_id(self, ks=[], torquess=[[]]):
                         self.costmodel_prior_tau))
         return graph
 
-    def priors_fd(self, ks=[], VAs=[]):
-        """Creates factors roughly corresponding to the forward dynamics problem.  While strictly
-        forward dynamics in Lynch & Park refers to the problem of calculating joint torques given
+    def priors_id(self, ks=[], lddotss=[[]], values=None):
+        """Creates factors roughly corresponding to the inverse dynamics problem.  Either supply
+        ks/lddotss, or ks/values.  If values is supplied, lddotss will be ignored.
+
+        Args:
+            ks (list, optional): Time step indices. Defaults to [].
+            lddotss (list, optional): List of list of joint accelerations for each time step.
+            Defaults to [[]].
+            values (gtsam.Values, optional): Values object containing all the needed key/values.
+
+        Returns:
+            gtsam.NonlinearFactorGraph: The inverse dynamics prior factors
+        """
+        if values is not None:
+            lddotss = [[gtd.JointAccel(values, ji, k) for ji in range(4)] for k in ks]
+        graph = gtsam.NonlinearFactorGraph()
+        for k, lddots in zip(ks, lddotss):
+            for ji, lddot in enumerate(lddots):
+                graph.push_back(
+                    gtd.PriorFactorDouble(
+                        gtd.JointAccelKey(ji, k).key(), lddot, self.costmodel_prior_lddot))
+        return graph
+
+    def priors_id_va(self, ks=[], VAs=[], values=None):
+        """Creates factors roughly corresponding to the inverse dynamics problem.  While strictly
+        inverse dynamics in Lynch & Park refers to the problem of calculating joint torques given
         joint accelerations, temproarily this function is more convenient which directly relates
         constraints TwistAccelerations (to obtain joint torques when used with dynamics_factors).
+        Either supply ks/twistaccels, or ks/values.  If values is supplied, twistaccels will be
+        ignored.
 
         Args:
             ks (list, optional): Time step indices. Defaults to [].
             VAs (list, optional): List of twist accelerations for each time step. Defaults to [[]].
+            values (gtsam.Values, optional): Values object containing all the needed key/values.
 
         Returns:
-            gtsam.NonlinearFactorGraph: The forward dynamics prior factors
+            gtsam.NonlinearFactorGraph: The inverse dynamics prior factors
         """
+        if values is not None:
+            VAs = [gtd.TwistAccel(values, self.ee_id(), k) for k in ks]
         graph = gtsam.NonlinearFactorGraph()
         for k, VA in zip(ks, VAs):
             graph.push_back(
diff --git a/gtdynamics/cablerobot/src/cdpr_planar_controller.py b/gtdynamics/cablerobot/src/cdpr_planar_controller.py
deleted file mode 100644
index 78e07a44..00000000
--- a/gtdynamics/cablerobot/src/cdpr_planar_controller.py
+++ /dev/null
@@ -1,116 +0,0 @@
-"""
-GTDynamics Copyright 2021, Georgia Tech Research Corporation,
-Atlanta, Georgia 30332-0415
-All Rights Reserved
-See LICENSE for the license information
-
-@file  cdpr_planar_controller.py
-@brief Optimal controller for a cable robot.  Solved by creating a factor graph and adding state
-objectives and control costs, then optimizing
-@author Frank Dellaert
-@author Gerry Chen
-"""
-
-import gtdynamics as gtd
-import gtsam
-import numpy as np
-
-import utils
-
-
-class CdprControllerBase:
-    """Interface for cable robot controllers
-    """
-    @property
-    def update(self, values, t):
-        """gives the new control input given current measurements
-
-        Args:
-            values (gtsam.Values): values object will contain at least the current Pose and Twist,
-            but should often also include the current joint angles and velocities
-            t (int): The current time index (discrete time index)
-
-        Returns:
-            gtsam.Values: A values object which contains the joint torques for this time step.
-
-        Raises:
-            NotImplementedError: Derived classes must override this function
-        """
-        raise NotImplementedError(
-            "CdprControllers need to implement the `update` function")
-
-
-class CdprController(CdprControllerBase):
-    """Precomputes the open-loop trajectory
-    then just calls on that for each update.
-    """
-    def __init__(self, cdpr, x0, pdes=[], dt=0.01, Q=None, R=np.array([1.])):
-        """constructor
-
-        Args:
-            cdpr (Cdpr): cable robot object
-            x0 (gtsam.Values): initial state
-            pdes (list, optional): list of desired poses. Defaults to [].
-            dt (float, optional): time step duration. Defaults to 0.01.
-            Q (np.ndarray, optional): State objective cost (as a vector). Defaults to None, which
-            denotes a constrained noise model.
-            R (np.ndarray, optional): Control cost (as a 1-vector). Defaults to np.array([1.]).
-        """
-        self.cdpr = cdpr
-        self.pdes = pdes
-        self.dt = dt
-
-        # create iLQR graph
-        fg = self.create_ilqr_fg(cdpr, x0, pdes, dt, Q, R)
-        # initial guess
-        init = utils.zerovalues(cdpr.ee_id(), range(len(pdes)), dt=dt)
-        # optimize
-        self.optimizer = gtsam.LevenbergMarquardtOptimizer(fg, init)
-        self.result = self.optimizer.optimize()
-        self.fg = fg
-
-    def update(self, values, t):
-        """New control: returns the entire results vector, which contains the optimal open-loop
-        control from the optimal trajectory.
-        """
-        return self.result
-
-    @staticmethod
-    def create_ilqr_fg(cdpr, x0, pdes, dt, Q, R):
-        """Creates the factor graph for the iLQR problem.  This essentially consists of creating a
-        factor graph that describes the CDPR dynamics over all time steps, then adding state
-        objective and control cost factors.
-
-        Args:
-            cdpr (Cdpr): cable robot object
-            x0 (gtsam.Values): initial Pose/Twist of the cable robot
-            pdes (List[gtsam.Pose3]): desired poses of the cdpr
-            dt (float): time step duration
-            Q (Union[np.ndarray, None]): The state objective cost (None for hard constraint)
-            R (np.ndarray): The control cost
-
-        Returns:
-            gtsam.NonlinearFactorGraph: The factor graph corresponding to the iLQR problem.
-        """
-        N = len(pdes)
-        # initial conditions
-        fg = cdpr.priors_ik(ks=[0],
-                            Ts=[gtd.Pose(x0, cdpr.ee_id(), 0)],
-                            Vs=[gtd.Twist(x0, cdpr.ee_id(), 0)])
-        # dynamics
-        fg.push_back(cdpr.all_factors(N, dt))
-        # control costs
-        for k in range(N):
-            for ji in range(4):
-                fg.push_back(
-                    gtd.PriorFactorDouble(
-                        gtd.TorqueKey(ji, k).key(), 0.0,
-                        gtsam.noiseModel.Diagonal.Precisions(R)))
-        # state objective costs
-        cost_x = gtsam.noiseModel.Isotropic.Sigma(6, 0.001) if Q is None else \
-            gtsam.noiseModel.Diagonal.Precisions(Q)
-        for k in range(N):
-            fg.push_back(
-                gtsam.PriorFactorPose3(
-                    gtd.PoseKey(cdpr.ee_id(), k).key(), pdes[k], cost_x))
-        return fg
diff --git a/gtdynamics/cablerobot/src/cdpr_planar_sim.py b/gtdynamics/cablerobot/src/cdpr_planar_sim.py
index ad6846f6..f6608748 100644
--- a/gtdynamics/cablerobot/src/cdpr_planar_sim.py
+++ b/gtdynamics/cablerobot/src/cdpr_planar_sim.py
@@ -13,6 +13,7 @@
 import gtsam
 import gtdynamics as gtd
 import numpy as np
+from utils import MyLMParams, InitValues
 
 class CdprSimulator:
     """Simulates a cable robot forward in time, given a robot, initial state, and controller.
@@ -56,71 +57,85 @@ def __init__(self, cdpr, x0, controller, dt=0.01):
         self.reset()
 
     @staticmethod
-    def update_kinematics(cdpr, fg, x, k):
-        """Runs IK to solve for the cable lengths and velocities at time step k
+    def update_kinematics(cdpr, x, k):
+        """Runs IK to solve for the cable lengths and velocities at time step k.  Modifies x
+        inplace!!!
 
         Args:
-            fg (gtsam.NonlineaFactorGraph): any previous factors, if applicable
-            x (gtsam.Values): Values object containing the current Pose and current Twist, plus any
-            other values that may be needed (as initial guesses) for the `fg` argument.
+            x (gtsam.Values): Values object containing at least the current Pose and current Twist
             k (int): current time step
 
         Returns:
-            tuple(gtsam.NonlinearFactorGraph, gtsam.Values): the updated factor graph and values
+            tuple(gtsam.NonlinearFactorGraph, gtsam.Values): the factor graph used to perform IK
+            and the solution values which includes the current Pose, Twist, JointAngles, and
+            JointVels.
         """
+        fg = gtsam.NonlinearFactorGraph()
+        lid = cdpr.ee_id()
+        # local copy of values
+        xk = gtsam.Values()
+        gtd.InsertPose(xk, lid, k, gtd.Pose(x, lid, k))
+        gtd.InsertTwist(xk, lid, k, gtd.Twist(x, lid, k))
         # IK for this time step, graph
         fg.push_back(cdpr.kinematics_factors(ks=[k]))
-        fg.push_back(
-            cdpr.priors_ik(ks=[k],
-                           Ts=[gtd.Pose(x, cdpr.ee_id(), k)],
-                           Vs=[gtd.Twist(x, cdpr.ee_id(), k)]))
+        fg.push_back(cdpr.priors_ik(ks=[k], values=xk))
         # IK initial estimate
         for j in range(4):
-            gtd.InsertJointAngle(x, j, k, 0)
-            gtd.InsertJointVel(x, j, k, 0)
+            gtd.InsertJointAngle(xk, j, k, 0)
+            gtd.InsertJointVel(xk, j, k, 0)
         # IK solve
-        result = gtsam.LevenbergMarquardtOptimizer(fg, x).optimize()
+        result = gtsam.LevenbergMarquardtOptimizer(fg, xk, MyLMParams()).optimize()
         assert abs(fg.error(result)) < 1e-20, "inverse kinematics didn't converge"
-        x.update(result)
-        return fg, x
+        xk.update(result)
+        return fg, xk
 
     @staticmethod
-    def update_dynamics(cdpr, fg, x, u, k, dt):
+    def update_dynamics(cdpr, x, u, k, dt):
         """Runs ID to solve for the twistAccel, and also runs collocation to get the next timestep
-        Pose/Twist
+        Pose/Twist.  Modifies x inplace!!!
 
         Args:
             cdpr (Cdpr): the cable robot
-            fg (gtsam.NonlinearFactorGraph): a factor graph containing any previous factors
-            x (gtsam.Values): Values object containing at least the current Pose and Twist, and any
-            other values that may be needed (as initial guesses) for the `fg` argument
+            x (gtsam.Values): Values object containing at least the current Pose and Twist
             u (gtsam.Values): The current joint torques
             k (int): The current time index
             dt (float): the time slice duration
 
         Returns:
-            tuple(gtsam.NonlinearFactorGraph, gtsam.Values): the factor graph with added factors,
-            and the solution Values which adds the TwistAccel, next Pose, and next Twist to the `x`
-            argument.
+            tuple(gtsam.NonlinearFactorGraph, gtsam.Values): the factor graph used to update the
+            dynamics, and the solution Values which consists of the Pose, Twist, torque, TwistAccel,
+            next Pose, and next Twist.
         """
-        # ID for this timestep + collocation to next time step
+        fg = gtsam.NonlinearFactorGraph()
+        lid = cdpr.ee_id()
+        # local copy of values
+        xd = gtsam.Values()
+        xd.insert(0, dt)
+        gtd.InsertPose(xd, lid, k, gtd.Pose(x, lid, k))
+        gtd.InsertTwist(xd, lid, k, gtd.Twist(x, lid, k))
+        # FD for this timestep + collocation to next time step
         fg.push_back(cdpr.dynamics_factors(ks=[k]))
         fg.push_back(cdpr.collocation_factors(ks=[k], dt=dt))
-        # ID priors (torque inputs)
-        fg.push_back(
-            cdpr.priors_id(ks=[k], torquess=[[gtd.Torque(u, ji, k) for ji in range(4)]]))
-        # ID initial guess
+        # priors (pose/twist and torque inputs)
+        fg.push_back(cdpr.priors_ik(ks=[k], values=xd))
+        fg.push_back(cdpr.priors_fd(ks=[k], values=u))
+        # FD initial guess
         for ji in range(4):
-            gtd.InsertTorque(x, ji, k, gtd.Torque(u, ji, k))
-            gtd.InsertWrench(x, cdpr.ee_id(), ji, k, np.zeros(6))
-        gtd.InsertPose(x, cdpr.ee_id(), k+1, gtsam.Pose3(gtsam.Rot3(), (1.5, 0, 1.5)))
-        gtd.InsertTwist(x, cdpr.ee_id(), k+1, np.zeros(6))
-        gtd.InsertTwistAccel(x, cdpr.ee_id(), k, np.zeros(6))
+            gtd.InsertJointVel(xd, ji, k, 0)
+            gtd.InsertJointAccel(xd, ji, k, 0)
+            gtd.InsertTorque(xd, ji, k, gtd.Torque(u, ji, k))
+            gtd.InsertTension(xd, ji, k,
+                                    gtd.Torque(u, ji, k) / cdpr.params.winch_params.radius_)
+            gtd.InsertWrench(xd, cdpr.ee_id(), ji, k, np.zeros(6))
+        gtd.InsertPose(xd, cdpr.ee_id(), k + 1, gtd.Pose(x, lid, k))
+        gtd.InsertTwist(xd, cdpr.ee_id(), k + 1, gtd.Twist(x, lid, k))
+        gtd.InsertTwistAccel(xd, cdpr.ee_id(), k, np.zeros(6))
         # optimize
-        result = gtsam.LevenbergMarquardtOptimizer(fg, x).optimize()
-        assert abs(fg.error(result)) < 1e-20, "dynamics simulation didn't converge"
-        x.update(result)
-        return fg, x
+        result = gtsam.LevenbergMarquardtOptimizer(fg, xd, MyLMParams()).optimize()
+        assert abs(fg.error(result)) < 1e-15, "dynamics simulation didn't converge: {:.5e}".format(
+            abs(fg.error(result)))
+        xd.update(result)
+        return fg, xd
 
     def step(self, verbose=False):
         """Performs one time step of the simulation, which consists of:
@@ -133,21 +148,38 @@ def step(self, verbose=False):
 
         Returns:
             gtsam.Values: The new values object containing the current state and next Pose+Twist.
-        """        
-        if verbose:
-            print('time step: {:4d}   --   EE position: ({:.2f}, {:.2f}, {:.2f})'.format(
-                self.k,
-                *gtd.Pose(self.x, self.cdpr.ee_id(), self.k).translation()), end='  --  ')
-        self.update_kinematics(self.cdpr, self.fg, self.x, self.k)
-        if self.k == 0:
-            self.x.insert(0, self.dt)
-        u = self.controller.update(self.x, self.k)
+        """
+        # setup
+        x, lid, k, dt = self.x, self.cdpr.ee_id(), self.k, self.dt
+
+        # kinematics
+        _, xk = self.update_kinematics(self.cdpr, x, k)
+        # controller
+        u = self.controller.update(xk, k)
+        # dynamics
+        _, xd = self.update_dynamics(self.cdpr, x, u, k, dt)
+
+        # update x
+        for ji in range(4):
+            gtd.InsertJointAngle(x, ji, k, gtd.JointAngle(xk, ji, k))
+            gtd.InsertJointVel(x, ji, k, gtd.JointVel(xk, ji, k))
+            gtd.InsertTorque(x, ji, k, gtd.Torque(u, ji, k))
+        gtd.InsertTwistAccel(x, lid, k, gtd.TwistAccel(xd, lid, k))
+        gtd.InsertPose(x, lid, k + 1, gtd.Pose(xd, lid, k + 1))
+        gtd.InsertTwist(x, lid, k + 1, gtd.Twist(xd, lid, k + 1))
+
+        # debug
         if verbose:
+            print('time step: {:4d}'.format(k), end='  --  ')
+            print('EE position: ({:.2f}, {:.2f}, {:.2f})'.format(
+                *gtd.Pose(x, lid, k).translation()), end='  --  ')
+            print('next: ({:.2f}, {:.2f}, {:.2f})'.format(
+                *gtd.Pose(x, lid, k+1).translation()), end='  --  ')
             print('control torques: {:.2e},   {:.2e},   {:.2e},   {:.2e}'.format(
-                *[gtd.Torque(u, ji, self.k) for ji in range(4)]))
-        self.update_dynamics(self.cdpr, self.fg, self.x, u, self.k, self.dt)
+                *[gtd.Torque(u, ji, k) for ji in range(4)]))
+
         self.k += 1
-        return self.x
+        return x
 
     def run(self, N=100, verbose=False):
         """Runs the simulation
@@ -158,8 +190,8 @@ def run(self, N=100, verbose=False):
 
         Returns:
             gtsam.Values: The values object containing all the data from the simulation.
-        """        
-        for k in range(N):
+        """
+        for _ in range(N):
             self.step(verbose=verbose)
         return self.x
 
diff --git a/gtdynamics/cablerobot/src/data/.gitattributes b/gtdynamics/cablerobot/src/data/.gitattributes
new file mode 100644
index 00000000..5b1a61bf
--- /dev/null
+++ b/gtdynamics/cablerobot/src/data/.gitattributes
@@ -0,0 +1,3 @@
+ATL_controller_1e2.h filter=lfs diff=lfs merge=lfs -text
+ATL_controller_1e4.h filter=lfs diff=lfs merge=lfs -text
+ATL_controller_1e6.h filter=lfs diff=lfs merge=lfs -text
diff --git a/gtdynamics/cablerobot/src/data/ATL.h b/gtdynamics/cablerobot/src/data/ATL.h
new file mode 100644
index 00000000..244f6867
--- /dev/null
+++ b/gtdynamics/cablerobot/src/data/ATL.h
@@ -0,0 +1,2929 @@
+bool painton[] = {
+	1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1
+};
+uint8_t colorinds[] = {
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+};
+uint8_t colorpalette[][3] = {
+	{4, 75, 148}
+};
+float traj[][2] = {
+	{ 0.243,  0.852},
+	{ 0.244,  0.857},
+	{ 0.246,  0.861},
+	{ 0.247,  0.866},
+	{ 0.248,  0.871},
+	{ 0.250,  0.876},
+	{ 0.251,  0.881},
+	{ 0.252,  0.885},
+	{ 0.253,  0.890},
+	{ 0.254,  0.895},
+	{ 0.256,  0.900},
+	{ 0.257,  0.905},
+	{ 0.258,  0.910},
+	{ 0.259,  0.915},
+	{ 0.260,  0.920},
+	{ 0.262,  0.924},
+	{ 0.263,  0.929},
+	{ 0.264,  0.934},
+	{ 0.265,  0.939},
+	{ 0.266,  0.944},
+	{ 0.268,  0.949},
+	{ 0.269,  0.954},
+	{ 0.270,  0.958},
+	{ 0.271,  0.963},
+	{ 0.272,  0.968},
+	{ 0.273,  0.973},
+	{ 0.275,  0.978},
+	{ 0.276,  0.983},
+	{ 0.277,  0.988},
+	{ 0.278,  0.993},
+	{ 0.279,  0.997},
+	{ 0.280,  1.002},
+	{ 0.281,  1.007},
+	{ 0.282,  1.012},
+	{ 0.284,  1.017},
+	{ 0.285,  1.022},
+	{ 0.286,  1.027},
+	{ 0.287,  1.032},
+	{ 0.288,  1.037},
+	{ 0.289,  1.041},
+	{ 0.290,  1.046},
+	{ 0.291,  1.051},
+	{ 0.292,  1.056},
+	{ 0.293,  1.061},
+	{ 0.294,  1.066},
+	{ 0.295,  1.071},
+	{ 0.296,  1.076},
+	{ 0.297,  1.081},
+	{ 0.298,  1.086},
+	{ 0.300,  1.090},
+	{ 0.301,  1.095},
+	{ 0.302,  1.100},
+	{ 0.303,  1.105},
+	{ 0.304,  1.110},
+	{ 0.305,  1.115},
+	{ 0.306,  1.120},
+	{ 0.307,  1.125},
+	{ 0.308,  1.130},
+	{ 0.309,  1.135},
+	{ 0.310,  1.139},
+	{ 0.311,  1.144},
+	{ 0.312,  1.149},
+	{ 0.313,  1.154},
+	{ 0.314,  1.159},
+	{ 0.315,  1.164},
+	{ 0.316,  1.169},
+	{ 0.317,  1.174},
+	{ 0.318,  1.179},
+	{ 0.319,  1.184},
+	{ 0.321,  1.188},
+	{ 0.322,  1.193},
+	{ 0.323,  1.198},
+	{ 0.324,  1.203},
+	{ 0.325,  1.208},
+	{ 0.326,  1.213},
+	{ 0.328,  1.218},
+	{ 0.329,  1.222},
+	{ 0.330,  1.227},
+	{ 0.331,  1.232},
+	{ 0.332,  1.237},
+	{ 0.334,  1.242},
+	{ 0.335,  1.247},
+	{ 0.336,  1.252},
+	{ 0.337,  1.257},
+	{ 0.338,  1.261},
+	{ 0.339,  1.266},
+	{ 0.340,  1.271},
+	{ 0.341,  1.276},
+	{ 0.342,  1.281},
+	{ 0.344,  1.286},
+	{ 0.345,  1.291},
+	{ 0.346,  1.296},
+	{ 0.347,  1.301},
+	{ 0.348,  1.306},
+	{ 0.348,  1.310},
+	{ 0.349,  1.315},
+	{ 0.350,  1.320},
+	{ 0.351,  1.325},
+	{ 0.352,  1.330},
+	{ 0.354,  1.335},
+	{ 0.355,  1.340},
+	{ 0.356,  1.345},
+	{ 0.357,  1.350},
+	{ 0.358,  1.355},
+	{ 0.359,  1.359},
+	{ 0.360,  1.364},
+	{ 0.361,  1.369},
+	{ 0.362,  1.374},
+	{ 0.363,  1.379},
+	{ 0.364,  1.384},
+	{ 0.365,  1.389},
+	{ 0.366,  1.394},
+	{ 0.367,  1.399},
+	{ 0.368,  1.404},
+	{ 0.369,  1.409},
+	{ 0.370,  1.414},
+	{ 0.370,  1.418},
+	{ 0.371,  1.423},
+	{ 0.372,  1.428},
+	{ 0.373,  1.433},
+	{ 0.374,  1.438},
+	{ 0.375,  1.443},
+	{ 0.376,  1.448},
+	{ 0.377,  1.453},
+	{ 0.378,  1.458},
+	{ 0.379,  1.463},
+	{ 0.380,  1.468},
+	{ 0.381,  1.473},
+	{ 0.381,  1.478},
+	{ 0.382,  1.482},
+	{ 0.383,  1.487},
+	{ 0.384,  1.492},
+	{ 0.385,  1.497},
+	{ 0.386,  1.502},
+	{ 0.387,  1.507},
+	{ 0.387,  1.512},
+	{ 0.388,  1.517},
+	{ 0.389,  1.522},
+	{ 0.390,  1.527},
+	{ 0.391,  1.532},
+	{ 0.391,  1.537},
+	{ 0.392,  1.542},
+	{ 0.393,  1.547},
+	{ 0.394,  1.552},
+	{ 0.395,  1.557},
+	{ 0.396,  1.561},
+	{ 0.397,  1.566},
+	{ 0.398,  1.571},
+	{ 0.399,  1.576},
+	{ 0.399,  1.581},
+	{ 0.400,  1.586},
+	{ 0.401,  1.591},
+	{ 0.402,  1.596},
+	{ 0.403,  1.601},
+	{ 0.404,  1.606},
+	{ 0.404,  1.611},
+	{ 0.405,  1.616},
+	{ 0.406,  1.621},
+	{ 0.407,  1.626},
+	{ 0.408,  1.631},
+	{ 0.408,  1.636},
+	{ 0.409,  1.641},
+	{ 0.410,  1.645},
+	{ 0.411,  1.650},
+	{ 0.412,  1.655},
+	{ 0.413,  1.660},
+	{ 0.414,  1.665},
+	{ 0.415,  1.670},
+	{ 0.416,  1.675},
+	{ 0.417,  1.680},
+	{ 0.418,  1.685},
+	{ 0.419,  1.690},
+	{ 0.419,  1.695},
+	{ 0.420,  1.700},
+	{ 0.421,  1.705},
+	{ 0.422,  1.709},
+	{ 0.423,  1.714},
+	{ 0.424,  1.719},
+	{ 0.425,  1.724},
+	{ 0.426,  1.729},
+	{ 0.427,  1.734},
+	{ 0.428,  1.739},
+	{ 0.429,  1.744},
+	{ 0.430,  1.749},
+	{ 0.431,  1.754},
+	{ 0.432,  1.759},
+	{ 0.433,  1.764},
+	{ 0.434,  1.768},
+	{ 0.435,  1.773},
+	{ 0.435,  1.778},
+	{ 0.436,  1.783},
+	{ 0.437,  1.788},
+	{ 0.438,  1.793},
+	{ 0.439,  1.798},
+	{ 0.440,  1.803},
+	{ 0.441,  1.808},
+	{ 0.442,  1.813},
+	{ 0.443,  1.818},
+	{ 0.444,  1.823},
+	{ 0.445,  1.827},
+	{ 0.446,  1.832},
+	{ 0.447,  1.837},
+	{ 0.448,  1.842},
+	{ 0.449,  1.847},
+	{ 0.451,  1.852},
+	{ 0.452,  1.857},
+	{ 0.453,  1.862},
+	{ 0.455,  1.866},
+	{ 0.457,  1.871},
+	{ 0.461,  1.873},
+	{ 0.466,  1.874},
+	{ 0.471,  1.873},
+	{ 0.476,  1.873},
+	{ 0.481,  1.873},
+	{ 0.486,  1.873},
+	{ 0.491,  1.873},
+	{ 0.496,  1.873},
+	{ 0.501,  1.873},
+	{ 0.506,  1.873},
+	{ 0.511,  1.873},
+	{ 0.516,  1.873},
+	{ 0.521,  1.874},
+	{ 0.526,  1.874},
+	{ 0.531,  1.875},
+	{ 0.536,  1.875},
+	{ 0.541,  1.876},
+	{ 0.546,  1.876},
+	{ 0.550,  1.876},
+	{ 0.555,  1.877},
+	{ 0.560,  1.877},
+	{ 0.565,  1.878},
+	{ 0.570,  1.878},
+	{ 0.575,  1.879},
+	{ 0.580,  1.879},
+	{ 0.585,  1.879},
+	{ 0.590,  1.880},
+	{ 0.595,  1.880},
+	{ 0.600,  1.881},
+	{ 0.605,  1.881},
+	{ 0.610,  1.881},
+	{ 0.615,  1.882},
+	{ 0.620,  1.882},
+	{ 0.625,  1.883},
+	{ 0.630,  1.883},
+	{ 0.635,  1.883},
+	{ 0.640,  1.884},
+	{ 0.645,  1.884},
+	{ 0.650,  1.885},
+	{ 0.655,  1.885},
+	{ 0.660,  1.886},
+	{ 0.665,  1.886},
+	{ 0.670,  1.887},
+	{ 0.675,  1.887},
+	{ 0.680,  1.887},
+	{ 0.685,  1.888},
+	{ 0.690,  1.888},
+	{ 0.695,  1.888},
+	{ 0.700,  1.889},
+	{ 0.705,  1.889},
+	{ 0.710,  1.889},
+	{ 0.715,  1.890},
+	{ 0.720,  1.890},
+	{ 0.725,  1.891},
+	{ 0.730,  1.891},
+	{ 0.735,  1.891},
+	{ 0.740,  1.892},
+	{ 0.745,  1.892},
+	{ 0.750,  1.893},
+	{ 0.755,  1.893},
+	{ 0.760,  1.894},
+	{ 0.765,  1.894},
+	{ 0.770,  1.895},
+	{ 0.775,  1.895},
+	{ 0.780,  1.896},
+	{ 0.785,  1.896},
+	{ 0.790,  1.897},
+	{ 0.795,  1.897},
+	{ 0.800,  1.898},
+	{ 0.805,  1.898},
+	{ 0.810,  1.899},
+	{ 0.815,  1.899},
+	{ 0.820,  1.900},
+	{ 0.825,  1.900},
+	{ 0.830,  1.901},
+	{ 0.835,  1.901},
+	{ 0.840,  1.902},
+	{ 0.845,  1.903},
+	{ 0.850,  1.903},
+	{ 0.855,  1.904},
+	{ 0.860,  1.905},
+	{ 0.865,  1.905},
+	{ 0.870,  1.906},
+	{ 0.875,  1.906},
+	{ 0.880,  1.907},
+	{ 0.885,  1.908},
+	{ 0.890,  1.908},
+	{ 0.895,  1.909},
+	{ 0.899,  1.910},
+	{ 0.904,  1.911},
+	{ 0.909,  1.911},
+	{ 0.914,  1.912},
+	{ 0.919,  1.913},
+	{ 0.924,  1.913},
+	{ 0.928,  1.912},
+	{ 0.924,  1.909},
+	{ 0.919,  1.911},
+	{ 0.920,  1.906},
+	{ 0.921,  1.901},
+	{ 0.921,  1.896},
+	{ 0.921,  1.891},
+	{ 0.921,  1.886},
+	{ 0.922,  1.881},
+	{ 0.922,  1.876},
+	{ 0.922,  1.871},
+	{ 0.922,  1.866},
+	{ 0.923,  1.861},
+	{ 0.924,  1.856},
+	{ 0.924,  1.851},
+	{ 0.925,  1.846},
+	{ 0.925,  1.841},
+	{ 0.926,  1.836},
+	{ 0.927,  1.832},
+	{ 0.927,  1.827},
+	{ 0.928,  1.822},
+	{ 0.929,  1.817},
+	{ 0.929,  1.812},
+	{ 0.930,  1.807},
+	{ 0.931,  1.802},
+	{ 0.931,  1.797},
+	{ 0.932,  1.792},
+	{ 0.933,  1.787},
+	{ 0.933,  1.782},
+	{ 0.934,  1.777},
+	{ 0.934,  1.772},
+	{ 0.935,  1.767},
+	{ 0.936,  1.762},
+	{ 0.936,  1.757},
+	{ 0.937,  1.752},
+	{ 0.938,  1.747},
+	{ 0.939,  1.742},
+	{ 0.939,  1.737},
+	{ 0.940,  1.732},
+	{ 0.941,  1.727},
+	{ 0.942,  1.722},
+	{ 0.943,  1.717},
+	{ 0.944,  1.713},
+	{ 0.945,  1.708},
+	{ 0.945,  1.703},
+	{ 0.946,  1.698},
+	{ 0.947,  1.693},
+	{ 0.948,  1.688},
+	{ 0.949,  1.683},
+	{ 0.950,  1.678},
+	{ 0.952,  1.673},
+	{ 0.953,  1.668},
+	{ 0.954,  1.664},
+	{ 0.955,  1.659},
+	{ 0.956,  1.654},
+	{ 0.957,  1.649},
+	{ 0.959,  1.644},
+	{ 0.960,  1.639},
+	{ 0.961,  1.634},
+	{ 0.962,  1.629},
+	{ 0.963,  1.624},
+	{ 0.964,  1.620},
+	{ 0.965,  1.615},
+	{ 0.967,  1.610},
+	{ 0.968,  1.605},
+	{ 0.969,  1.600},
+	{ 0.970,  1.595},
+	{ 0.971,  1.590},
+	{ 0.973,  1.586},
+	{ 0.974,  1.581},
+	{ 0.975,  1.576},
+	{ 0.976,  1.571},
+	{ 0.977,  1.566},
+	{ 0.979,  1.561},
+	{ 0.980,  1.556},
+	{ 0.981,  1.552},
+	{ 0.982,  1.547},
+	{ 0.984,  1.542},
+	{ 0.985,  1.537},
+	{ 0.986,  1.532},
+	{ 0.988,  1.527},
+	{ 0.989,  1.522},
+	{ 0.990,  1.518},
+	{ 0.991,  1.513},
+	{ 0.993,  1.508},
+	{ 0.994,  1.503},
+	{ 0.995,  1.498},
+	{ 0.997,  1.493},
+	{ 0.998,  1.489},
+	{ 0.999,  1.484},
+	{ 1.000,  1.479},
+	{ 1.002,  1.474},
+	{ 1.003,  1.469},
+	{ 1.004,  1.464},
+	{ 1.005,  1.459},
+	{ 1.006,  1.455},
+	{ 1.007,  1.450},
+	{ 1.008,  1.445},
+	{ 1.009,  1.440},
+	{ 1.010,  1.435},
+	{ 1.011,  1.430},
+	{ 1.013,  1.425},
+	{ 1.014,  1.420},
+	{ 1.015,  1.415},
+	{ 1.016,  1.411},
+	{ 1.017,  1.406},
+	{ 1.018,  1.401},
+	{ 1.019,  1.396},
+	{ 1.020,  1.391},
+	{ 1.021,  1.386},
+	{ 1.023,  1.381},
+	{ 1.024,  1.376},
+	{ 1.025,  1.371},
+	{ 1.026,  1.367},
+	{ 1.027,  1.362},
+	{ 1.028,  1.357},
+	{ 1.029,  1.352},
+	{ 1.030,  1.347},
+	{ 1.031,  1.342},
+	{ 1.033,  1.337},
+	{ 1.034,  1.332},
+	{ 1.035,  1.327},
+	{ 1.036,  1.323},
+	{ 1.037,  1.318},
+	{ 1.038,  1.313},
+	{ 1.039,  1.308},
+	{ 1.040,  1.303},
+	{ 1.041,  1.298},
+	{ 1.043,  1.293},
+	{ 1.044,  1.288},
+	{ 1.045,  1.284},
+	{ 1.046,  1.279},
+	{ 1.047,  1.274},
+	{ 1.048,  1.269},
+	{ 1.049,  1.264},
+	{ 1.050,  1.259},
+	{ 1.052,  1.254},
+	{ 1.053,  1.249},
+	{ 1.054,  1.244},
+	{ 1.055,  1.240},
+	{ 1.056,  1.235},
+	{ 1.057,  1.230},
+	{ 1.058,  1.225},
+	{ 1.059,  1.220},
+	{ 1.061,  1.215},
+	{ 1.062,  1.210},
+	{ 1.063,  1.205},
+	{ 1.064,  1.201},
+	{ 1.065,  1.196},
+	{ 1.066,  1.191},
+	{ 1.067,  1.186},
+	{ 1.069,  1.181},
+	{ 1.070,  1.176},
+	{ 1.071,  1.171},
+	{ 1.072,  1.166},
+	{ 1.073,  1.162},
+	{ 1.074,  1.157},
+	{ 1.075,  1.152},
+	{ 1.077,  1.147},
+	{ 1.078,  1.142},
+	{ 1.079,  1.137},
+	{ 1.080,  1.132},
+	{ 1.081,  1.127},
+	{ 1.082,  1.122},
+	{ 1.083,  1.118},
+	{ 1.084,  1.113},
+	{ 1.085,  1.108},
+	{ 1.086,  1.103},
+	{ 1.088,  1.098},
+	{ 1.089,  1.093},
+	{ 1.090,  1.088},
+	{ 1.091,  1.083},
+	{ 1.092,  1.079},
+	{ 1.093,  1.074},
+	{ 1.095,  1.069},
+	{ 1.096,  1.064},
+	{ 1.097,  1.059},
+	{ 1.098,  1.054},
+	{ 1.099,  1.049},
+	{ 1.101,  1.045},
+	{ 1.102,  1.040},
+	{ 1.104,  1.035},
+	{ 1.105,  1.030},
+	{ 1.106,  1.025},
+	{ 1.108,  1.020},
+	{ 1.109,  1.016},
+	{ 1.110,  1.011},
+	{ 1.112,  1.006},
+	{ 1.113,  1.001},
+	{ 1.115,  0.997},
+	{ 1.117,  0.992},
+	{ 1.118,  0.987},
+	{ 1.120,  0.982},
+	{ 1.121,  0.977},
+	{ 1.123,  0.973},
+	{ 1.125,  0.968},
+	{ 1.126,  0.963},
+	{ 1.128,  0.959},
+	{ 1.130,  0.954},
+	{ 1.132,  0.949},
+	{ 1.133,  0.945},
+	{ 1.135,  0.940},
+	{ 1.137,  0.935},
+	{ 1.139,  0.931},
+	{ 1.141,  0.926},
+	{ 1.143,  0.921},
+	{ 1.145,  0.917},
+	{ 1.147,  0.912},
+	{ 1.148,  0.907},
+	{ 1.150,  0.903},
+	{ 1.152,  0.898},
+	{ 1.154,  0.893},
+	{ 1.155,  0.889},
+	{ 1.157,  0.884},
+	{ 1.158,  0.879},
+	{ 1.159,  0.874},
+	{ 1.160,  0.869},
+	{ 1.159,  0.864},
+	{ 1.156,  0.860},
+	{ 1.152,  0.858},
+	{ 1.147,  0.856},
+	{ 1.143,  0.854},
+	{ 1.138,  0.853},
+	{ 1.133,  0.851},
+	{ 1.128,  0.850},
+	{ 1.123,  0.850},
+	{ 1.118,  0.849},
+	{ 1.113,  0.848},
+	{ 1.108,  0.847},
+	{ 1.103,  0.847},
+	{ 1.098,  0.846},
+	{ 1.093,  0.846},
+	{ 1.088,  0.845},
+	{ 1.083,  0.844},
+	{ 1.078,  0.844},
+	{ 1.074,  0.843},
+	{ 1.069,  0.843},
+	{ 1.064,  0.842},
+	{ 1.059,  0.841},
+	{ 1.054,  0.841},
+	{ 1.049,  0.840},
+	{ 1.044,  0.839},
+	{ 1.039,  0.839},
+	{ 1.034,  0.838},
+	{ 1.029,  0.837},
+	{ 1.024,  0.837},
+	{ 1.019,  0.836},
+	{ 1.014,  0.836},
+	{ 1.009,  0.835},
+	{ 1.004,  0.835},
+	{ 0.999,  0.834},
+	{ 0.994,  0.833},
+	{ 0.989,  0.833},
+	{ 0.984,  0.832},
+	{ 0.979,  0.832},
+	{ 0.974,  0.831},
+	{ 0.969,  0.831},
+	{ 0.964,  0.830},
+	{ 0.959,  0.829},
+	{ 0.954,  0.829},
+	{ 0.949,  0.828},
+	{ 0.944,  0.828},
+	{ 0.939,  0.827},
+	{ 0.934,  0.827},
+	{ 0.929,  0.826},
+	{ 0.924,  0.826},
+	{ 0.919,  0.825},
+	{ 0.914,  0.825},
+	{ 0.909,  0.824},
+	{ 0.904,  0.823},
+	{ 0.899,  0.823},
+	{ 0.894,  0.822},
+	{ 0.889,  0.822},
+	{ 0.884,  0.821},
+	{ 0.879,  0.821},
+	{ 0.874,  0.820},
+	{ 0.870,  0.819},
+	{ 0.865,  0.819},
+	{ 0.860,  0.818},
+	{ 0.855,  0.818},
+	{ 0.850,  0.818},
+	{ 0.845,  0.818},
+	{ 0.840,  0.819},
+	{ 0.835,  0.820},
+	{ 0.830,  0.822},
+	{ 0.826,  0.824},
+	{ 0.822,  0.828},
+	{ 0.819,  0.832},
+	{ 0.815,  0.835},
+	{ 0.813,  0.839},
+	{ 0.810,  0.844},
+	{ 0.808,  0.848},
+	{ 0.805,  0.852},
+	{ 0.803,  0.857},
+	{ 0.801,  0.862},
+	{ 0.799,  0.866},
+	{ 0.798,  0.871},
+	{ 0.796,  0.876},
+	{ 0.794,  0.880},
+	{ 0.792,  0.885},
+	{ 0.791,  0.890},
+	{ 0.789,  0.895},
+	{ 0.788,  0.899},
+	{ 0.787,  0.904},
+	{ 0.785,  0.909},
+	{ 0.784,  0.914},
+	{ 0.783,  0.919},
+	{ 0.781,  0.923},
+	{ 0.780,  0.928},
+	{ 0.779,  0.933},
+	{ 0.778,  0.938},
+	{ 0.776,  0.943},
+	{ 0.775,  0.948},
+	{ 0.774,  0.953},
+	{ 0.773,  0.958},
+	{ 0.772,  0.962},
+	{ 0.771,  0.967},
+	{ 0.770,  0.972},
+	{ 0.769,  0.977},
+	{ 0.767,  0.982},
+	{ 0.766,  0.987},
+	{ 0.765,  0.992},
+	{ 0.764,  0.997},
+	{ 0.763,  1.001},
+	{ 0.761,  1.006},
+	{ 0.760,  1.011},
+	{ 0.758,  1.016},
+	{ 0.757,  1.021},
+	{ 0.756,  1.025},
+	{ 0.754,  1.030},
+	{ 0.752,  1.035},
+	{ 0.751,  1.040},
+	{ 0.749,  1.044},
+	{ 0.747,  1.049},
+	{ 0.745,  1.053},
+	{ 0.741,  1.057},
+	{ 0.737,  1.060},
+	{ 0.732,  1.061},
+	{ 0.728,  1.062},
+	{ 0.723,  1.063},
+	{ 0.718,  1.064},
+	{ 0.713,  1.065},
+	{ 0.708,  1.066},
+	{ 0.703,  1.067},
+	{ 0.698,  1.068},
+	{ 0.693,  1.069},
+	{ 0.688,  1.070},
+	{ 0.683,  1.071},
+	{ 0.678,  1.071},
+	{ 0.673,  1.072},
+	{ 0.668,  1.073},
+	{ 0.663,  1.073},
+	{ 0.658,  1.074},
+	{ 0.653,  1.075},
+	{ 0.648,  1.075},
+	{ 0.643,  1.076},
+	{ 0.639,  1.076},
+	{ 0.634,  1.076},
+	{ 0.628,  1.077},
+	{ 0.623,  1.077},
+	{ 0.618,  1.077},
+	{ 0.613,  1.077},
+	{ 0.608,  1.078},
+	{ 0.603,  1.078},
+	{ 0.599,  1.077},
+	{ 0.594,  1.076},
+	{ 0.590,  1.073},
+	{ 0.586,  1.069},
+	{ 0.583,  1.065},
+	{ 0.580,  1.061},
+	{ 0.578,  1.057},
+	{ 0.575,  1.052},
+	{ 0.573,  1.048},
+	{ 0.571,  1.043},
+	{ 0.569,  1.039},
+	{ 0.568,  1.034},
+	{ 0.566,  1.029},
+	{ 0.565,  1.024},
+	{ 0.563,  1.019},
+	{ 0.562,  1.015},
+	{ 0.561,  1.010},
+	{ 0.559,  1.005},
+	{ 0.558,  1.000},
+	{ 0.557,  0.995},
+	{ 0.555,  0.990},
+	{ 0.554,  0.986},
+	{ 0.553,  0.981},
+	{ 0.552,  0.976},
+	{ 0.551,  0.971},
+	{ 0.550,  0.966},
+	{ 0.549,  0.961},
+	{ 0.548,  0.956},
+	{ 0.547,  0.951},
+	{ 0.547,  0.946},
+	{ 0.546,  0.941},
+	{ 0.545,  0.936},
+	{ 0.544,  0.931},
+	{ 0.543,  0.927},
+	{ 0.542,  0.922},
+	{ 0.540,  0.917},
+	{ 0.539,  0.912},
+	{ 0.538,  0.907},
+	{ 0.537,  0.902},
+	{ 0.536,  0.897},
+	{ 0.535,  0.892},
+	{ 0.534,  0.887},
+	{ 0.533,  0.882},
+	{ 0.532,  0.878},
+	{ 0.531,  0.873},
+	{ 0.530,  0.868},
+	{ 0.529,  0.863},
+	{ 0.527,  0.858},
+	{ 0.526,  0.853},
+	{ 0.524,  0.849},
+	{ 0.523,  0.844},
+	{ 0.521,  0.839},
+	{ 0.520,  0.834},
+	{ 0.518,  0.830},
+	{ 0.515,  0.826},
+	{ 0.511,  0.822},
+	{ 0.507,  0.819},
+	{ 0.503,  0.818},
+	{ 0.498,  0.817},
+	{ 0.493,  0.817},
+	{ 0.488,  0.818},
+	{ 0.483,  0.818},
+	{ 0.478,  0.819},
+	{ 0.473,  0.819},
+	{ 0.468,  0.820},
+	{ 0.463,  0.820},
+	{ 0.458,  0.821},
+	{ 0.453,  0.821},
+	{ 0.448,  0.821},
+	{ 0.443,  0.821},
+	{ 0.438,  0.822},
+	{ 0.433,  0.822},
+	{ 0.428,  0.822},
+	{ 0.423,  0.822},
+	{ 0.418,  0.822},
+	{ 0.413,  0.822},
+	{ 0.408,  0.822},
+	{ 0.403,  0.822},
+	{ 0.398,  0.822},
+	{ 0.393,  0.822},
+	{ 0.388,  0.822},
+	{ 0.383,  0.822},
+	{ 0.378,  0.822},
+	{ 0.373,  0.822},
+	{ 0.368,  0.823},
+	{ 0.363,  0.823},
+	{ 0.358,  0.823},
+	{ 0.353,  0.824},
+	{ 0.348,  0.824},
+	{ 0.343,  0.824},
+	{ 0.338,  0.825},
+	{ 0.333,  0.825},
+	{ 0.328,  0.826},
+	{ 0.323,  0.826},
+	{ 0.318,  0.827},
+	{ 0.313,  0.827},
+	{ 0.308,  0.828},
+	{ 0.303,  0.828},
+	{ 0.298,  0.829},
+	{ 0.293,  0.829},
+	{ 0.288,  0.830},
+	{ 0.283,  0.832},
+	{ 0.278,  0.833},
+	{ 0.273,  0.834},
+	{ 0.269,  0.836},
+	{ 0.264,  0.837},
+	{ 0.264,  0.837},
+	{ 0.268,  0.841},
+	{ 0.271,  0.844},
+	{ 0.275,  0.848},
+	{ 0.279,  0.851},
+	{ 0.282,  0.855},
+	{ 0.286,  0.858},
+	{ 0.290,  0.862},
+	{ 0.293,  0.865},
+	{ 0.297,  0.869},
+	{ 0.301,  0.872},
+	{ 0.305,  0.875},
+	{ 0.308,  0.879},
+	{ 0.312,  0.882},
+	{ 0.316,  0.886},
+	{ 0.319,  0.889},
+	{ 0.323,  0.893},
+	{ 0.327,  0.896},
+	{ 0.330,  0.900},
+	{ 0.334,  0.903},
+	{ 0.338,  0.907},
+	{ 0.341,  0.910},
+	{ 0.345,  0.914},
+	{ 0.349,  0.917},
+	{ 0.353,  0.920},
+	{ 0.356,  0.924},
+	{ 0.360,  0.927},
+	{ 0.364,  0.931},
+	{ 0.367,  0.934},
+	{ 0.371,  0.938},
+	{ 0.375,  0.941},
+	{ 0.378,  0.945},
+	{ 0.382,  0.948},
+	{ 0.386,  0.952},
+	{ 0.389,  0.955},
+	{ 0.393,  0.959},
+	{ 0.397,  0.962},
+	{ 0.400,  0.965},
+	{ 0.404,  0.969},
+	{ 0.408,  0.972},
+	{ 0.412,  0.976},
+	{ 0.415,  0.979},
+	{ 0.419,  0.983},
+	{ 0.423,  0.986},
+	{ 0.426,  0.990},
+	{ 0.430,  0.993},
+	{ 0.434,  0.997},
+	{ 0.437,  1.000},
+	{ 0.441,  1.003},
+	{ 0.445,  1.007},
+	{ 0.448,  1.010},
+	{ 0.452,  1.014},
+	{ 0.456,  1.017},
+	{ 0.460,  1.021},
+	{ 0.463,  1.024},
+	{ 0.467,  1.028},
+	{ 0.471,  1.031},
+	{ 0.474,  1.035},
+	{ 0.478,  1.038},
+	{ 0.482,  1.042},
+	{ 0.485,  1.045},
+	{ 0.489,  1.048},
+	{ 0.493,  1.052},
+	{ 0.496,  1.055},
+	{ 0.500,  1.059},
+	{ 0.504,  1.062},
+	{ 0.508,  1.066},
+	{ 0.511,  1.069},
+	{ 0.515,  1.073},
+	{ 0.519,  1.076},
+	{ 0.522,  1.080},
+	{ 0.526,  1.083},
+	{ 0.530,  1.087},
+	{ 0.533,  1.090},
+	{ 0.537,  1.093},
+	{ 0.541,  1.097},
+	{ 0.544,  1.100},
+	{ 0.548,  1.104},
+	{ 0.552,  1.107},
+	{ 0.555,  1.111},
+	{ 0.559,  1.114},
+	{ 0.563,  1.118},
+	{ 0.567,  1.121},
+	{ 0.570,  1.125},
+	{ 0.574,  1.128},
+	{ 0.578,  1.132},
+	{ 0.581,  1.135},
+	{ 0.585,  1.138},
+	{ 0.589,  1.142},
+	{ 0.592,  1.145},
+	{ 0.596,  1.149},
+	{ 0.600,  1.152},
+	{ 0.603,  1.156},
+	{ 0.607,  1.159},
+	{ 0.611,  1.163},
+	{ 0.615,  1.166},
+	{ 0.618,  1.170},
+	{ 0.622,  1.173},
+	{ 0.626,  1.177},
+	{ 0.629,  1.180},
+	{ 0.633,  1.183},
+	{ 0.637,  1.187},
+	{ 0.640,  1.190},
+	{ 0.644,  1.194},
+	{ 0.648,  1.197},
+	{ 0.651,  1.201},
+	{ 0.655,  1.204},
+	{ 0.659,  1.208},
+	{ 0.662,  1.211},
+	{ 0.666,  1.215},
+	{ 0.670,  1.218},
+	{ 0.674,  1.221},
+	{ 0.677,  1.225},
+	{ 0.681,  1.228},
+	{ 0.685,  1.232},
+	{ 0.688,  1.235},
+	{ 0.692,  1.239},
+	{ 0.696,  1.242},
+	{ 0.699,  1.246},
+	{ 0.703,  1.249},
+	{ 0.707,  1.253},
+	{ 0.710,  1.256},
+	{ 0.710,  1.256},
+	{ 0.715,  1.256},
+	{ 0.721,  1.256},
+	{ 0.726,  1.256},
+	{ 0.731,  1.256},
+	{ 0.736,  1.256},
+	{ 0.741,  1.257},
+	{ 0.746,  1.257},
+	{ 0.751,  1.258},
+	{ 0.755,  1.260},
+	{ 0.758,  1.263},
+	{ 0.759,  1.268},
+	{ 0.758,  1.273},
+	{ 0.758,  1.278},
+	{ 0.757,  1.283},
+	{ 0.756,  1.288},
+	{ 0.755,  1.293},
+	{ 0.754,  1.298},
+	{ 0.753,  1.303},
+	{ 0.752,  1.308},
+	{ 0.751,  1.313},
+	{ 0.750,  1.318},
+	{ 0.749,  1.323},
+	{ 0.748,  1.328},
+	{ 0.747,  1.332},
+	{ 0.746,  1.337},
+	{ 0.744,  1.342},
+	{ 0.743,  1.347},
+	{ 0.742,  1.352},
+	{ 0.741,  1.357},
+	{ 0.740,  1.362},
+	{ 0.739,  1.367},
+	{ 0.738,  1.372},
+	{ 0.737,  1.377},
+	{ 0.735,  1.382},
+	{ 0.734,  1.387},
+	{ 0.733,  1.391},
+	{ 0.731,  1.396},
+	{ 0.730,  1.401},
+	{ 0.728,  1.406},
+	{ 0.727,  1.411},
+	{ 0.726,  1.416},
+	{ 0.724,  1.421},
+	{ 0.723,  1.425},
+	{ 0.721,  1.430},
+	{ 0.720,  1.435},
+	{ 0.718,  1.440},
+	{ 0.717,  1.445},
+	{ 0.716,  1.450},
+	{ 0.714,  1.454},
+	{ 0.712,  1.459},
+	{ 0.711,  1.464},
+	{ 0.709,  1.469},
+	{ 0.708,  1.473},
+	{ 0.706,  1.478},
+	{ 0.704,  1.483},
+	{ 0.701,  1.487},
+	{ 0.699,  1.492},
+	{ 0.694,  1.493},
+	{ 0.690,  1.490},
+	{ 0.687,  1.486},
+	{ 0.686,  1.482},
+	{ 0.684,  1.477},
+	{ 0.682,  1.472},
+	{ 0.680,  1.468},
+	{ 0.678,  1.463},
+	{ 0.676,  1.458},
+	{ 0.674,  1.454},
+	{ 0.672,  1.449},
+	{ 0.671,  1.444},
+	{ 0.669,  1.439},
+	{ 0.667,  1.435},
+	{ 0.666,  1.430},
+	{ 0.664,  1.425},
+	{ 0.662,  1.420},
+	{ 0.661,  1.415},
+	{ 0.659,  1.411},
+	{ 0.658,  1.406},
+	{ 0.656,  1.401},
+	{ 0.655,  1.396},
+	{ 0.653,  1.391},
+	{ 0.652,  1.387},
+	{ 0.650,  1.382},
+	{ 0.649,  1.377},
+	{ 0.647,  1.372},
+	{ 0.646,  1.367},
+	{ 0.644,  1.362},
+	{ 0.643,  1.358},
+	{ 0.642,  1.353},
+	{ 0.640,  1.348},
+	{ 0.639,  1.343},
+	{ 0.637,  1.338},
+	{ 0.636,  1.333},
+	{ 0.635,  1.328},
+	{ 0.633,  1.324},
+	{ 0.632,  1.319},
+	{ 0.630,  1.314},
+	{ 0.629,  1.309},
+	{ 0.627,  1.304},
+	{ 0.626,  1.299},
+	{ 0.624,  1.295},
+	{ 0.623,  1.290},
+	{ 0.621,  1.285},
+	{ 0.619,  1.280},
+	{ 0.617,  1.276},
+	{ 0.615,  1.271},
+	{ 0.613,  1.267},
+	{ 0.612,  1.266},
+	{ 0.617,  1.266},
+	{ 0.622,  1.265},
+	{ 0.627,  1.263},
+	{ 0.631,  1.261},
+	{ 0.635,  1.258},
+	{ 0.638,  1.254},
+	{ 0.640,  1.250},
+	{ 0.641,  1.245},
+	{ 0.642,  1.240},
+	{ 0.642,  1.240},
+	{ 0.647,  1.241},
+	{ 0.652,  1.242},
+	{ 0.657,  1.243},
+	{ 0.662,  1.244},
+	{ 0.667,  1.245},
+	{ 0.672,  1.247},
+	{ 0.677,  1.248},
+	{ 0.682,  1.249},
+	{ 0.687,  1.250},
+	{ 0.691,  1.251},
+	{ 0.696,  1.252},
+	{ 0.701,  1.254},
+	{ 0.706,  1.255},
+	{ 0.711,  1.256},
+	{ 0.716,  1.257},
+	{ 0.721,  1.258},
+	{ 0.726,  1.259},
+	{ 0.731,  1.261},
+	{ 0.736,  1.262},
+	{ 0.741,  1.263},
+	{ 0.746,  1.264},
+	{ 0.751,  1.265},
+	{ 0.756,  1.266},
+	{ 0.761,  1.268},
+	{ 0.766,  1.269},
+	{ 0.770,  1.270},
+	{ 0.775,  1.271},
+	{ 0.780,  1.272},
+	{ 0.785,  1.273},
+	{ 0.790,  1.275},
+	{ 0.795,  1.276},
+	{ 0.800,  1.277},
+	{ 0.805,  1.278},
+	{ 0.810,  1.279},
+	{ 0.815,  1.280},
+	{ 0.820,  1.282},
+	{ 0.825,  1.283},
+	{ 0.830,  1.284},
+	{ 0.835,  1.285},
+	{ 0.840,  1.286},
+	{ 0.845,  1.287},
+	{ 0.849,  1.289},
+	{ 0.854,  1.290},
+	{ 0.859,  1.291},
+	{ 0.864,  1.292},
+	{ 0.869,  1.293},
+	{ 0.874,  1.294},
+	{ 0.879,  1.296},
+	{ 0.884,  1.297},
+	{ 0.889,  1.298},
+	{ 0.894,  1.299},
+	{ 0.899,  1.300},
+	{ 0.904,  1.301},
+	{ 0.909,  1.303},
+	{ 0.914,  1.304},
+	{ 0.919,  1.305},
+	{ 0.924,  1.306},
+	{ 0.928,  1.307},
+	{ 0.933,  1.308},
+	{ 0.938,  1.310},
+	{ 0.943,  1.311},
+	{ 0.948,  1.312},
+	{ 0.953,  1.313},
+	{ 0.958,  1.314},
+	{ 0.963,  1.315},
+	{ 0.968,  1.317},
+	{ 0.973,  1.318},
+	{ 0.978,  1.319},
+	{ 0.983,  1.320},
+	{ 0.988,  1.321},
+	{ 0.993,  1.322},
+	{ 0.998,  1.324},
+	{ 1.003,  1.325},
+	{ 1.007,  1.326},
+	{ 1.012,  1.327},
+	{ 1.017,  1.328},
+	{ 1.022,  1.329},
+	{ 1.027,  1.331},
+	{ 1.032,  1.332},
+	{ 1.037,  1.333},
+	{ 1.042,  1.334},
+	{ 1.047,  1.335},
+	{ 1.052,  1.336},
+	{ 1.057,  1.338},
+	{ 1.062,  1.339},
+	{ 1.067,  1.340},
+	{ 1.072,  1.341},
+	{ 1.077,  1.342},
+	{ 1.082,  1.343},
+	{ 1.086,  1.345},
+	{ 1.091,  1.346},
+	{ 1.091,  1.346},
+	{ 1.092,  1.351},
+	{ 1.092,  1.356},
+	{ 1.092,  1.361},
+	{ 1.092,  1.366},
+	{ 1.093,  1.371},
+	{ 1.093,  1.376},
+	{ 1.093,  1.381},
+	{ 1.093,  1.386},
+	{ 1.093,  1.391},
+	{ 1.093,  1.396},
+	{ 1.094,  1.401},
+	{ 1.094,  1.406},
+	{ 1.094,  1.411},
+	{ 1.095,  1.416},
+	{ 1.095,  1.421},
+	{ 1.095,  1.426},
+	{ 1.095,  1.431},
+	{ 1.095,  1.436},
+	{ 1.095,  1.441},
+	{ 1.095,  1.446},
+	{ 1.096,  1.451},
+	{ 1.096,  1.456},
+	{ 1.096,  1.461},
+	{ 1.096,  1.466},
+	{ 1.096,  1.471},
+	{ 1.097,  1.476},
+	{ 1.097,  1.481},
+	{ 1.097,  1.486},
+	{ 1.097,  1.491},
+	{ 1.098,  1.496},
+	{ 1.098,  1.501},
+	{ 1.098,  1.506},
+	{ 1.098,  1.511},
+	{ 1.098,  1.516},
+	{ 1.099,  1.521},
+	{ 1.099,  1.526},
+	{ 1.099,  1.531},
+	{ 1.099,  1.536},
+	{ 1.099,  1.541},
+	{ 1.100,  1.546},
+	{ 1.100,  1.551},
+	{ 1.100,  1.556},
+	{ 1.100,  1.561},
+	{ 1.100,  1.566},
+	{ 1.100,  1.571},
+	{ 1.100,  1.576},
+	{ 1.101,  1.581},
+	{ 1.101,  1.586},
+	{ 1.101,  1.591},
+	{ 1.101,  1.596},
+	{ 1.101,  1.601},
+	{ 1.101,  1.606},
+	{ 1.101,  1.611},
+	{ 1.101,  1.616},
+	{ 1.101,  1.621},
+	{ 1.101,  1.626},
+	{ 1.102,  1.631},
+	{ 1.102,  1.636},
+	{ 1.102,  1.641},
+	{ 1.102,  1.646},
+	{ 1.102,  1.651},
+	{ 1.102,  1.656},
+	{ 1.103,  1.661},
+	{ 1.103,  1.666},
+	{ 1.103,  1.671},
+	{ 1.103,  1.676},
+	{ 1.104,  1.681},
+	{ 1.104,  1.686},
+	{ 1.105,  1.691},
+	{ 1.105,  1.696},
+	{ 1.106,  1.701},
+	{ 1.107,  1.706},
+	{ 1.108,  1.711},
+	{ 1.109,  1.716},
+	{ 1.111,  1.720},
+	{ 1.112,  1.725},
+	{ 1.115,  1.729},
+	{ 1.118,  1.733},
+	{ 1.122,  1.736},
+	{ 1.126,  1.739},
+	{ 1.131,  1.740},
+	{ 1.136,  1.741},
+	{ 1.141,  1.742},
+	{ 1.146,  1.742},
+	{ 1.151,  1.741},
+	{ 1.156,  1.741},
+	{ 1.161,  1.741},
+	{ 1.166,  1.740},
+	{ 1.171,  1.740},
+	{ 1.176,  1.739},
+	{ 1.181,  1.739},
+	{ 1.186,  1.738},
+	{ 1.191,  1.738},
+	{ 1.196,  1.737},
+	{ 1.201,  1.737},
+	{ 1.206,  1.737},
+	{ 1.211,  1.736},
+	{ 1.216,  1.736},
+	{ 1.221,  1.735},
+	{ 1.226,  1.735},
+	{ 1.231,  1.734},
+	{ 1.236,  1.734},
+	{ 1.241,  1.734},
+	{ 1.246,  1.733},
+	{ 1.251,  1.733},
+	{ 1.256,  1.733},
+	{ 1.261,  1.733},
+	{ 1.266,  1.733},
+	{ 1.271,  1.732},
+	{ 1.276,  1.732},
+	{ 1.281,  1.732},
+	{ 1.286,  1.732},
+	{ 1.291,  1.731},
+	{ 1.296,  1.731},
+	{ 1.301,  1.731},
+	{ 1.306,  1.731},
+	{ 1.311,  1.731},
+	{ 1.316,  1.730},
+	{ 1.321,  1.730},
+	{ 1.326,  1.730},
+	{ 1.331,  1.730},
+	{ 1.336,  1.729},
+	{ 1.341,  1.729},
+	{ 1.346,  1.729},
+	{ 1.351,  1.729},
+	{ 1.356,  1.729},
+	{ 1.361,  1.728},
+	{ 1.366,  1.728},
+	{ 1.371,  1.728},
+	{ 1.376,  1.728},
+	{ 1.381,  1.728},
+	{ 1.386,  1.728},
+	{ 1.391,  1.728},
+	{ 1.396,  1.728},
+	{ 1.401,  1.728},
+	{ 1.406,  1.728},
+	{ 1.411,  1.728},
+	{ 1.416,  1.728},
+	{ 1.421,  1.727},
+	{ 1.426,  1.727},
+	{ 1.431,  1.727},
+	{ 1.436,  1.727},
+	{ 1.441,  1.727},
+	{ 1.446,  1.727},
+	{ 1.451,  1.727},
+	{ 1.456,  1.727},
+	{ 1.461,  1.727},
+	{ 1.466,  1.727},
+	{ 1.471,  1.727},
+	{ 1.476,  1.727},
+	{ 1.481,  1.727},
+	{ 1.486,  1.727},
+	{ 1.491,  1.727},
+	{ 1.496,  1.727},
+	{ 1.501,  1.727},
+	{ 1.506,  1.727},
+	{ 1.511,  1.727},
+	{ 1.516,  1.727},
+	{ 1.521,  1.727},
+	{ 1.526,  1.727},
+	{ 1.531,  1.727},
+	{ 1.536,  1.727},
+	{ 1.541,  1.727},
+	{ 1.546,  1.727},
+	{ 1.551,  1.728},
+	{ 1.556,  1.728},
+	{ 1.561,  1.728},
+	{ 1.566,  1.728},
+	{ 1.571,  1.728},
+	{ 1.576,  1.728},
+	{ 1.581,  1.728},
+	{ 1.586,  1.729},
+	{ 1.591,  1.729},
+	{ 1.596,  1.729},
+	{ 1.601,  1.729},
+	{ 1.606,  1.729},
+	{ 1.611,  1.729},
+	{ 1.616,  1.729},
+	{ 1.621,  1.729},
+	{ 1.626,  1.729},
+	{ 1.631,  1.729},
+	{ 1.636,  1.729},
+	{ 1.641,  1.729},
+	{ 1.646,  1.729},
+	{ 1.651,  1.729},
+	{ 1.656,  1.730},
+	{ 1.661,  1.730},
+	{ 1.666,  1.730},
+	{ 1.671,  1.730},
+	{ 1.676,  1.730},
+	{ 1.681,  1.730},
+	{ 1.686,  1.731},
+	{ 1.691,  1.731},
+	{ 1.696,  1.731},
+	{ 1.701,  1.731},
+	{ 1.706,  1.731},
+	{ 1.711,  1.732},
+	{ 1.716,  1.732},
+	{ 1.721,  1.732},
+	{ 1.726,  1.732},
+	{ 1.731,  1.732},
+	{ 1.736,  1.733},
+	{ 1.741,  1.733},
+	{ 1.746,  1.733},
+	{ 1.751,  1.733},
+	{ 1.756,  1.733},
+	{ 1.761,  1.734},
+	{ 1.766,  1.734},
+	{ 1.771,  1.734},
+	{ 1.776,  1.734},
+	{ 1.781,  1.735},
+	{ 1.786,  1.735},
+	{ 1.791,  1.735},
+	{ 1.796,  1.735},
+	{ 1.801,  1.736},
+	{ 1.806,  1.736},
+	{ 1.811,  1.736},
+	{ 1.816,  1.736},
+	{ 1.821,  1.737},
+	{ 1.826,  1.737},
+	{ 1.831,  1.737},
+	{ 1.836,  1.737},
+	{ 1.841,  1.737},
+	{ 1.846,  1.738},
+	{ 1.851,  1.738},
+	{ 1.856,  1.738},
+	{ 1.861,  1.738},
+	{ 1.866,  1.738},
+	{ 1.871,  1.739},
+	{ 1.876,  1.739},
+	{ 1.881,  1.739},
+	{ 1.886,  1.739},
+	{ 1.891,  1.739},
+	{ 1.896,  1.740},
+	{ 1.901,  1.740},
+	{ 1.906,  1.740},
+	{ 1.911,  1.740},
+	{ 1.916,  1.741},
+	{ 1.921,  1.741},
+	{ 1.926,  1.741},
+	{ 1.931,  1.741},
+	{ 1.936,  1.741},
+	{ 1.941,  1.741},
+	{ 1.946,  1.742},
+	{ 1.951,  1.742},
+	{ 1.956,  1.742},
+	{ 1.961,  1.742},
+	{ 1.966,  1.742},
+	{ 1.971,  1.741},
+	{ 1.976,  1.741},
+	{ 1.981,  1.740},
+	{ 1.986,  1.739},
+	{ 1.991,  1.738},
+	{ 1.996,  1.736},
+	{ 2.000,  1.735},
+	{ 2.005,  1.732},
+	{ 2.009,  1.729},
+	{ 2.012,  1.726},
+	{ 2.015,  1.722},
+	{ 2.017,  1.717},
+	{ 2.019,  1.713},
+	{ 2.021,  1.708},
+	{ 2.022,  1.703},
+	{ 2.023,  1.698},
+	{ 2.024,  1.693},
+	{ 2.025,  1.688},
+	{ 2.025,  1.683},
+	{ 2.026,  1.678},
+	{ 2.026,  1.673},
+	{ 2.026,  1.668},
+	{ 2.026,  1.663},
+	{ 2.027,  1.658},
+	{ 2.027,  1.653},
+	{ 2.027,  1.648},
+	{ 2.027,  1.643},
+	{ 2.026,  1.638},
+	{ 2.026,  1.633},
+	{ 2.026,  1.628},
+	{ 2.026,  1.623},
+	{ 2.026,  1.618},
+	{ 2.025,  1.613},
+	{ 2.025,  1.608},
+	{ 2.025,  1.603},
+	{ 2.024,  1.598},
+	{ 2.024,  1.593},
+	{ 2.024,  1.588},
+	{ 2.023,  1.583},
+	{ 2.023,  1.578},
+	{ 2.023,  1.573},
+	{ 2.023,  1.568},
+	{ 2.022,  1.563},
+	{ 2.022,  1.558},
+	{ 2.021,  1.553},
+	{ 2.021,  1.548},
+	{ 2.021,  1.543},
+	{ 2.020,  1.538},
+	{ 2.020,  1.533},
+	{ 2.019,  1.528},
+	{ 2.019,  1.523},
+	{ 2.019,  1.518},
+	{ 2.018,  1.514},
+	{ 2.018,  1.509},
+	{ 2.017,  1.504},
+	{ 2.017,  1.499},
+	{ 2.017,  1.494},
+	{ 2.016,  1.489},
+	{ 2.016,  1.484},
+	{ 2.015,  1.479},
+	{ 2.014,  1.474},
+	{ 2.014,  1.469},
+	{ 2.013,  1.464},
+	{ 2.013,  1.459},
+	{ 2.013,  1.454},
+	{ 2.012,  1.449},
+	{ 2.012,  1.444},
+	{ 2.011,  1.439},
+	{ 2.011,  1.434},
+	{ 2.010,  1.429},
+	{ 2.010,  1.424},
+	{ 2.009,  1.419},
+	{ 2.009,  1.414},
+	{ 2.009,  1.409},
+	{ 2.008,  1.404},
+	{ 2.008,  1.399},
+	{ 2.008,  1.394},
+	{ 2.007,  1.389},
+	{ 2.007,  1.384},
+	{ 2.007,  1.379},
+	{ 2.006,  1.374},
+	{ 2.006,  1.369},
+	{ 2.006,  1.364},
+	{ 2.005,  1.359},
+	{ 2.005,  1.354},
+	{ 2.005,  1.349},
+	{ 2.005,  1.344},
+	{ 2.004,  1.339},
+	{ 2.004,  1.334},
+	{ 2.004,  1.329},
+	{ 2.004,  1.324},
+	{ 2.004,  1.319},
+	{ 2.004,  1.314},
+	{ 2.004,  1.309},
+	{ 2.004,  1.304},
+	{ 2.004,  1.299},
+	{ 2.004,  1.294},
+	{ 2.003,  1.289},
+	{ 2.003,  1.284},
+	{ 2.003,  1.279},
+	{ 2.003,  1.274},
+	{ 2.003,  1.269},
+	{ 2.003,  1.264},
+	{ 2.002,  1.259},
+	{ 2.002,  1.254},
+	{ 2.001,  1.249},
+	{ 2.000,  1.244},
+	{ 1.997,  1.240},
+	{ 1.994,  1.236},
+	{ 1.990,  1.233},
+	{ 1.986,  1.230},
+	{ 1.981,  1.230},
+	{ 1.976,  1.229},
+	{ 1.971,  1.229},
+	{ 1.966,  1.229},
+	{ 1.961,  1.229},
+	{ 1.956,  1.230},
+	{ 1.951,  1.230},
+	{ 1.946,  1.231},
+	{ 1.941,  1.232},
+	{ 1.936,  1.233},
+	{ 1.931,  1.234},
+	{ 1.926,  1.235},
+	{ 1.921,  1.235},
+	{ 1.916,  1.236},
+	{ 1.911,  1.237},
+	{ 1.907,  1.238},
+	{ 1.902,  1.239},
+	{ 1.897,  1.240},
+	{ 1.892,  1.241},
+	{ 1.887,  1.242},
+	{ 1.882,  1.243},
+	{ 1.877,  1.244},
+	{ 1.872,  1.245},
+	{ 1.867,  1.246},
+	{ 1.862,  1.247},
+	{ 1.857,  1.248},
+	{ 1.852,  1.249},
+	{ 1.848,  1.250},
+	{ 1.843,  1.251},
+	{ 1.838,  1.251},
+	{ 1.833,  1.252},
+	{ 1.828,  1.253},
+	{ 1.823,  1.254},
+	{ 1.818,  1.255},
+	{ 1.813,  1.256},
+	{ 1.808,  1.257},
+	{ 1.803,  1.258},
+	{ 1.798,  1.258},
+	{ 1.793,  1.259},
+	{ 1.788,  1.260},
+	{ 1.783,  1.261},
+	{ 1.778,  1.262},
+	{ 1.774,  1.263},
+	{ 1.769,  1.263},
+	{ 1.764,  1.264},
+	{ 1.759,  1.265},
+	{ 1.754,  1.265},
+	{ 1.749,  1.266},
+	{ 1.744,  1.267},
+	{ 1.739,  1.268},
+	{ 1.734,  1.269},
+	{ 1.729,  1.269},
+	{ 1.724,  1.270},
+	{ 1.719,  1.271},
+	{ 1.714,  1.272},
+	{ 1.709,  1.273},
+	{ 1.704,  1.274},
+	{ 1.700,  1.275},
+	{ 1.695,  1.277},
+	{ 1.690,  1.278},
+	{ 1.685,  1.279},
+	{ 1.680,  1.278},
+	{ 1.676,  1.276},
+	{ 1.674,  1.272},
+	{ 1.674,  1.267},
+	{ 1.675,  1.262},
+	{ 1.676,  1.257},
+	{ 1.677,  1.252},
+	{ 1.679,  1.247},
+	{ 1.680,  1.243},
+	{ 1.681,  1.238},
+	{ 1.683,  1.233},
+	{ 1.684,  1.228},
+	{ 1.686,  1.223},
+	{ 1.687,  1.219},
+	{ 1.689,  1.214},
+	{ 1.690,  1.209},
+	{ 1.692,  1.204},
+	{ 1.693,  1.199},
+	{ 1.695,  1.195},
+	{ 1.696,  1.190},
+	{ 1.698,  1.185},
+	{ 1.699,  1.180},
+	{ 1.701,  1.175},
+	{ 1.702,  1.171},
+	{ 1.704,  1.166},
+	{ 1.705,  1.161},
+	{ 1.706,  1.156},
+	{ 1.708,  1.152},
+	{ 1.709,  1.147},
+	{ 1.710,  1.142},
+	{ 1.712,  1.137},
+	{ 1.713,  1.132},
+	{ 1.714,  1.127},
+	{ 1.715,  1.122},
+	{ 1.717,  1.118},
+	{ 1.718,  1.113},
+	{ 1.719,  1.108},
+	{ 1.720,  1.103},
+	{ 1.722,  1.098},
+	{ 1.723,  1.093},
+	{ 1.724,  1.088},
+	{ 1.725,  1.084},
+	{ 1.726,  1.079},
+	{ 1.728,  1.074},
+	{ 1.729,  1.069},
+	{ 1.730,  1.064},
+	{ 1.731,  1.059},
+	{ 1.732,  1.054},
+	{ 1.734,  1.050},
+	{ 1.735,  1.045},
+	{ 1.736,  1.040},
+	{ 1.737,  1.035},
+	{ 1.738,  1.030},
+	{ 1.740,  1.025},
+	{ 1.741,  1.020},
+	{ 1.742,  1.016},
+	{ 1.744,  1.011},
+	{ 1.745,  1.006},
+	{ 1.746,  1.001},
+	{ 1.747,  0.996},
+	{ 1.749,  0.991},
+	{ 1.750,  0.987},
+	{ 1.751,  0.982},
+	{ 1.752,  0.977},
+	{ 1.754,  0.972},
+	{ 1.755,  0.967},
+	{ 1.756,  0.962},
+	{ 1.757,  0.957},
+	{ 1.758,  0.953},
+	{ 1.759,  0.948},
+	{ 1.761,  0.943},
+	{ 1.762,  0.938},
+	{ 1.763,  0.933},
+	{ 1.764,  0.928},
+	{ 1.765,  0.923},
+	{ 1.766,  0.918},
+	{ 1.767,  0.913},
+	{ 1.768,  0.909},
+	{ 1.769,  0.904},
+	{ 1.771,  0.899},
+	{ 1.772,  0.894},
+	{ 1.773,  0.889},
+	{ 1.774,  0.884},
+	{ 1.775,  0.879},
+	{ 1.776,  0.874},
+	{ 1.777,  0.870},
+	{ 1.778,  0.865},
+	{ 1.779,  0.860},
+	{ 1.781,  0.855},
+	{ 1.782,  0.850},
+	{ 1.783,  0.845},
+	{ 1.784,  0.840},
+	{ 1.785,  0.835},
+	{ 1.786,  0.830},
+	{ 1.787,  0.826},
+	{ 1.788,  0.821},
+	{ 1.789,  0.816},
+	{ 1.791,  0.811},
+	{ 1.792,  0.806},
+	{ 1.793,  0.801},
+	{ 1.794,  0.796},
+	{ 1.795,  0.791},
+	{ 1.796,  0.786},
+	{ 1.797,  0.782},
+	{ 1.798,  0.777},
+	{ 1.799,  0.772},
+	{ 1.800,  0.767},
+	{ 1.801,  0.762},
+	{ 1.803,  0.757},
+	{ 1.804,  0.752},
+	{ 1.805,  0.747},
+	{ 1.806,  0.743},
+	{ 1.807,  0.738},
+	{ 1.808,  0.733},
+	{ 1.809,  0.728},
+	{ 1.810,  0.723},
+	{ 1.810,  0.718},
+	{ 1.811,  0.713},
+	{ 1.812,  0.708},
+	{ 1.813,  0.703},
+	{ 1.813,  0.698},
+	{ 1.813,  0.693},
+	{ 1.814,  0.688},
+	{ 1.814,  0.683},
+	{ 1.813,  0.678},
+	{ 1.812,  0.673},
+	{ 1.810,  0.669},
+	{ 1.806,  0.665},
+	{ 1.803,  0.662},
+	{ 1.798,  0.660},
+	{ 1.794,  0.658},
+	{ 1.789,  0.657},
+	{ 1.784,  0.656},
+	{ 1.779,  0.656},
+	{ 1.774,  0.655},
+	{ 1.769,  0.654},
+	{ 1.764,  0.654},
+	{ 1.759,  0.654},
+	{ 1.754,  0.654},
+	{ 1.749,  0.654},
+	{ 1.744,  0.654},
+	{ 1.739,  0.654},
+	{ 1.734,  0.654},
+	{ 1.729,  0.654},
+	{ 1.724,  0.654},
+	{ 1.719,  0.654},
+	{ 1.714,  0.655},
+	{ 1.709,  0.655},
+	{ 1.704,  0.655},
+	{ 1.699,  0.655},
+	{ 1.694,  0.655},
+	{ 1.689,  0.656},
+	{ 1.684,  0.656},
+	{ 1.679,  0.656},
+	{ 1.674,  0.656},
+	{ 1.669,  0.656},
+	{ 1.664,  0.656},
+	{ 1.659,  0.657},
+	{ 1.654,  0.657},
+	{ 1.649,  0.657},
+	{ 1.644,  0.657},
+	{ 1.639,  0.657},
+	{ 1.634,  0.658},
+	{ 1.629,  0.658},
+	{ 1.624,  0.658},
+	{ 1.619,  0.658},
+	{ 1.614,  0.658},
+	{ 1.609,  0.659},
+	{ 1.604,  0.659},
+	{ 1.599,  0.659},
+	{ 1.594,  0.659},
+	{ 1.589,  0.659},
+	{ 1.584,  0.659},
+	{ 1.579,  0.660},
+	{ 1.574,  0.660},
+	{ 1.569,  0.660},
+	{ 1.564,  0.660},
+	{ 1.559,  0.660},
+	{ 1.554,  0.660},
+	{ 1.549,  0.660},
+	{ 1.544,  0.660},
+	{ 1.539,  0.660},
+	{ 1.534,  0.660},
+	{ 1.529,  0.660},
+	{ 1.524,  0.660},
+	{ 1.519,  0.660},
+	{ 1.514,  0.660},
+	{ 1.509,  0.660},
+	{ 1.504,  0.660},
+	{ 1.499,  0.660},
+	{ 1.494,  0.659},
+	{ 1.489,  0.659},
+	{ 1.484,  0.659},
+	{ 1.479,  0.659},
+	{ 1.474,  0.659},
+	{ 1.469,  0.658},
+	{ 1.464,  0.658},
+	{ 1.459,  0.658},
+	{ 1.454,  0.658},
+	{ 1.449,  0.657},
+	{ 1.444,  0.657},
+	{ 1.439,  0.656},
+	{ 1.434,  0.656},
+	{ 1.429,  0.656},
+	{ 1.424,  0.655},
+	{ 1.419,  0.655},
+	{ 1.414,  0.655},
+	{ 1.409,  0.654},
+	{ 1.404,  0.654},
+	{ 1.399,  0.653},
+	{ 1.394,  0.653},
+	{ 1.389,  0.652},
+	{ 1.384,  0.652},
+	{ 1.379,  0.651},
+	{ 1.374,  0.651},
+	{ 1.369,  0.650},
+	{ 1.364,  0.650},
+	{ 1.359,  0.649},
+	{ 1.354,  0.649},
+	{ 1.349,  0.648},
+	{ 1.344,  0.648},
+	{ 1.339,  0.647},
+	{ 1.334,  0.647},
+	{ 1.329,  0.647},
+	{ 1.324,  0.647},
+	{ 1.319,  0.647},
+	{ 1.314,  0.647},
+	{ 1.309,  0.648},
+	{ 1.304,  0.649},
+	{ 1.300,  0.651},
+	{ 1.296,  0.655},
+	{ 1.293,  0.659},
+	{ 1.291,  0.663},
+	{ 1.290,  0.668},
+	{ 1.289,  0.673},
+	{ 1.289,  0.678},
+	{ 1.289,  0.683},
+	{ 1.289,  0.688},
+	{ 1.289,  0.693},
+	{ 1.289,  0.698},
+	{ 1.290,  0.703},
+	{ 1.290,  0.708},
+	{ 1.291,  0.713},
+	{ 1.291,  0.718},
+	{ 1.292,  0.723},
+	{ 1.293,  0.728},
+	{ 1.294,  0.733},
+	{ 1.295,  0.738},
+	{ 1.296,  0.742},
+	{ 1.297,  0.747},
+	{ 1.297,  0.752},
+	{ 1.298,  0.757},
+	{ 1.299,  0.762},
+	{ 1.300,  0.767},
+	{ 1.301,  0.772},
+	{ 1.302,  0.777},
+	{ 1.303,  0.782},
+	{ 1.304,  0.787},
+	{ 1.305,  0.792},
+	{ 1.306,  0.797},
+	{ 1.307,  0.801},
+	{ 1.308,  0.806},
+	{ 1.309,  0.811},
+	{ 1.310,  0.816},
+	{ 1.311,  0.821},
+	{ 1.312,  0.826},
+	{ 1.313,  0.831},
+	{ 1.314,  0.836},
+	{ 1.315,  0.841},
+	{ 1.316,  0.846},
+	{ 1.317,  0.850},
+	{ 1.318,  0.855},
+	{ 1.319,  0.860},
+	{ 1.320,  0.865},
+	{ 1.321,  0.870},
+	{ 1.322,  0.875},
+	{ 1.323,  0.880},
+	{ 1.324,  0.885},
+	{ 1.325,  0.890},
+	{ 1.326,  0.895},
+	{ 1.327,  0.900},
+	{ 1.328,  0.905},
+	{ 1.328,  0.910},
+	{ 1.329,  0.914},
+	{ 1.330,  0.919},
+	{ 1.331,  0.924},
+	{ 1.332,  0.929},
+	{ 1.333,  0.934},
+	{ 1.334,  0.939},
+	{ 1.335,  0.944},
+	{ 1.335,  0.949},
+	{ 1.336,  0.954},
+	{ 1.337,  0.959},
+	{ 1.338,  0.964},
+	{ 1.339,  0.969},
+	{ 1.340,  0.974},
+	{ 1.341,  0.979},
+	{ 1.341,  0.984},
+	{ 1.342,  0.988},
+	{ 1.343,  0.993},
+	{ 1.343,  0.998},
+	{ 1.344,  1.003},
+	{ 1.345,  1.008},
+	{ 1.346,  1.013},
+	{ 1.346,  1.018},
+	{ 1.347,  1.023},
+	{ 1.348,  1.028},
+	{ 1.349,  1.033},
+	{ 1.349,  1.038},
+	{ 1.350,  1.043},
+	{ 1.351,  1.048},
+	{ 1.352,  1.053},
+	{ 1.353,  1.058},
+	{ 1.353,  1.063},
+	{ 1.354,  1.068},
+	{ 1.355,  1.073},
+	{ 1.356,  1.078},
+	{ 1.357,  1.083},
+	{ 1.357,  1.087},
+	{ 1.358,  1.092},
+	{ 1.359,  1.097},
+	{ 1.360,  1.102},
+	{ 1.361,  1.107},
+	{ 1.361,  1.112},
+	{ 1.362,  1.117},
+	{ 1.363,  1.122},
+	{ 1.364,  1.127},
+	{ 1.365,  1.132},
+	{ 1.366,  1.137},
+	{ 1.367,  1.142},
+	{ 1.368,  1.147},
+	{ 1.369,  1.152},
+	{ 1.370,  1.156},
+	{ 1.371,  1.161},
+	{ 1.371,  1.166},
+	{ 1.372,  1.171},
+	{ 1.373,  1.176},
+	{ 1.374,  1.181},
+	{ 1.375,  1.186},
+	{ 1.376,  1.191},
+	{ 1.377,  1.196},
+	{ 1.378,  1.201},
+	{ 1.380,  1.206},
+	{ 1.381,  1.210},
+	{ 1.382,  1.215},
+	{ 1.383,  1.220},
+	{ 1.385,  1.225},
+	{ 1.386,  1.230},
+	{ 1.387,  1.235},
+	{ 1.388,  1.239},
+	{ 1.389,  1.244},
+	{ 1.391,  1.249},
+	{ 1.392,  1.254},
+	{ 1.393,  1.259},
+	{ 1.394,  1.264},
+	{ 1.395,  1.269},
+	{ 1.395,  1.274},
+	{ 1.394,  1.279},
+	{ 1.390,  1.280},
+	{ 1.385,  1.280},
+	{ 1.380,  1.279},
+	{ 1.375,  1.279},
+	{ 1.370,  1.278},
+	{ 1.365,  1.278},
+	{ 1.360,  1.277},
+	{ 1.355,  1.276},
+	{ 1.350,  1.276},
+	{ 1.345,  1.275},
+	{ 1.340,  1.274},
+	{ 1.336,  1.273},
+	{ 1.331,  1.273},
+	{ 1.326,  1.272},
+	{ 1.321,  1.272},
+	{ 1.316,  1.271},
+	{ 1.311,  1.271},
+	{ 1.306,  1.270},
+	{ 1.301,  1.270},
+	{ 1.296,  1.269},
+	{ 1.291,  1.269},
+	{ 1.286,  1.268},
+	{ 1.281,  1.268},
+	{ 1.276,  1.267},
+	{ 1.271,  1.267},
+	{ 1.266,  1.267},
+	{ 1.261,  1.266},
+	{ 1.256,  1.266},
+	{ 1.251,  1.266},
+	{ 1.246,  1.265},
+	{ 1.241,  1.265},
+	{ 1.236,  1.264},
+	{ 1.231,  1.264},
+	{ 1.226,  1.263},
+	{ 1.221,  1.263},
+	{ 1.216,  1.262},
+	{ 1.211,  1.262},
+	{ 1.206,  1.261},
+	{ 1.201,  1.261},
+	{ 1.196,  1.261},
+	{ 1.191,  1.260},
+	{ 1.186,  1.260},
+	{ 1.181,  1.259},
+	{ 1.176,  1.259},
+	{ 1.171,  1.258},
+	{ 1.166,  1.258},
+	{ 1.161,  1.257},
+	{ 1.156,  1.257},
+	{ 1.151,  1.256},
+	{ 1.146,  1.256},
+	{ 1.141,  1.255},
+	{ 1.136,  1.255},
+	{ 1.131,  1.254},
+	{ 1.126,  1.254},
+	{ 1.121,  1.254},
+	{ 1.116,  1.254},
+	{ 1.111,  1.253},
+	{ 1.106,  1.254},
+	{ 1.101,  1.255},
+	{ 1.097,  1.258},
+	{ 1.094,  1.262},
+	{ 1.093,  1.266},
+	{ 1.092,  1.271},
+	{ 1.091,  1.276},
+	{ 1.091,  1.281},
+	{ 1.091,  1.286},
+	{ 1.091,  1.291},
+	{ 1.091,  1.296},
+	{ 1.091,  1.301},
+	{ 1.091,  1.306},
+	{ 1.091,  1.311},
+	{ 1.091,  1.316},
+	{ 1.091,  1.321},
+	{ 1.091,  1.326},
+	{ 1.091,  1.326},
+	{ 1.096,  1.327},
+	{ 1.101,  1.328},
+	{ 1.106,  1.329},
+	{ 1.111,  1.329},
+	{ 1.116,  1.330},
+	{ 1.121,  1.331},
+	{ 1.126,  1.332},
+	{ 1.131,  1.333},
+	{ 1.136,  1.333},
+	{ 1.141,  1.334},
+	{ 1.146,  1.335},
+	{ 1.151,  1.336},
+	{ 1.156,  1.337},
+	{ 1.161,  1.337},
+	{ 1.166,  1.338},
+	{ 1.171,  1.339},
+	{ 1.176,  1.340},
+	{ 1.181,  1.341},
+	{ 1.186,  1.341},
+	{ 1.191,  1.342},
+	{ 1.196,  1.343},
+	{ 1.201,  1.344},
+	{ 1.206,  1.344},
+	{ 1.211,  1.345},
+	{ 1.216,  1.346},
+	{ 1.221,  1.347},
+	{ 1.226,  1.348},
+	{ 1.230,  1.348},
+	{ 1.235,  1.349},
+	{ 1.240,  1.350},
+	{ 1.245,  1.351},
+	{ 1.250,  1.352},
+	{ 1.255,  1.352},
+	{ 1.260,  1.353},
+	{ 1.265,  1.354},
+	{ 1.270,  1.355},
+	{ 1.275,  1.356},
+	{ 1.280,  1.356},
+	{ 1.285,  1.357},
+	{ 1.290,  1.358},
+	{ 1.295,  1.359},
+	{ 1.300,  1.360},
+	{ 1.305,  1.360},
+	{ 1.310,  1.361},
+	{ 1.315,  1.362},
+	{ 1.320,  1.363},
+	{ 1.325,  1.364},
+	{ 1.330,  1.364},
+	{ 1.335,  1.365},
+	{ 1.340,  1.366},
+	{ 1.345,  1.367},
+	{ 1.350,  1.367},
+	{ 1.355,  1.368},
+	{ 1.360,  1.369},
+	{ 1.365,  1.370},
+	{ 1.370,  1.371},
+	{ 1.375,  1.371},
+	{ 1.380,  1.372},
+	{ 1.385,  1.373},
+	{ 1.390,  1.374},
+	{ 1.395,  1.375},
+	{ 1.400,  1.375},
+	{ 1.405,  1.376},
+	{ 1.410,  1.377},
+	{ 1.415,  1.378},
+	{ 1.420,  1.379},
+	{ 1.425,  1.379},
+	{ 1.430,  1.380},
+	{ 1.435,  1.381},
+	{ 1.440,  1.382},
+	{ 1.445,  1.383},
+	{ 1.450,  1.383},
+	{ 1.455,  1.384},
+	{ 1.460,  1.385},
+	{ 1.464,  1.386},
+	{ 1.469,  1.387},
+	{ 1.474,  1.387},
+	{ 1.479,  1.388},
+	{ 1.484,  1.389},
+	{ 1.489,  1.390},
+	{ 1.494,  1.391},
+	{ 1.499,  1.391},
+	{ 1.504,  1.392},
+	{ 1.509,  1.393},
+	{ 1.514,  1.394},
+	{ 1.519,  1.394},
+	{ 1.524,  1.395},
+	{ 1.529,  1.396},
+	{ 1.534,  1.397},
+	{ 1.539,  1.398},
+	{ 1.544,  1.398},
+	{ 1.549,  1.399},
+	{ 1.554,  1.400},
+	{ 1.559,  1.401},
+	{ 1.564,  1.402},
+	{ 1.569,  1.402},
+	{ 1.574,  1.403},
+	{ 1.579,  1.404},
+	{ 1.584,  1.405},
+	{ 1.589,  1.406},
+	{ 1.594,  1.406},
+	{ 1.599,  1.407},
+	{ 1.604,  1.408},
+	{ 1.609,  1.409},
+	{ 1.614,  1.410},
+	{ 1.619,  1.410},
+	{ 1.624,  1.411},
+	{ 1.629,  1.412},
+	{ 1.634,  1.413},
+	{ 1.639,  1.414},
+	{ 1.644,  1.414},
+	{ 1.649,  1.415},
+	{ 1.654,  1.416},
+	{ 1.659,  1.417},
+	{ 1.664,  1.417},
+	{ 1.669,  1.418},
+	{ 1.674,  1.419},
+	{ 1.679,  1.420},
+	{ 1.684,  1.421},
+	{ 1.689,  1.421},
+	{ 1.694,  1.422},
+	{ 1.698,  1.423},
+	{ 1.703,  1.424},
+	{ 1.708,  1.425},
+	{ 1.713,  1.425},
+	{ 1.718,  1.426},
+	{ 1.723,  1.427},
+	{ 1.728,  1.428},
+	{ 1.733,  1.429},
+	{ 1.738,  1.429},
+	{ 1.743,  1.430},
+	{ 1.748,  1.431},
+	{ 1.753,  1.432},
+	{ 1.758,  1.433},
+	{ 1.763,  1.433},
+	{ 1.768,  1.434},
+	{ 1.773,  1.435},
+	{ 1.778,  1.436},
+	{ 1.783,  1.437},
+	{ 1.788,  1.437},
+	{ 1.793,  1.438},
+	{ 1.798,  1.439},
+	{ 1.803,  1.440},
+	{ 1.808,  1.440},
+	{ 1.813,  1.441},
+	{ 1.818,  1.442},
+	{ 1.823,  1.443},
+	{ 1.828,  1.444},
+	{ 1.833,  1.444},
+	{ 1.838,  1.445},
+	{ 1.843,  1.446},
+	{ 1.848,  1.447},
+	{ 1.853,  1.448},
+	{ 1.858,  1.448},
+	{ 1.863,  1.449},
+	{ 1.868,  1.450},
+	{ 1.873,  1.451},
+	{ 1.878,  1.452},
+	{ 1.883,  1.452},
+	{ 1.888,  1.453},
+	{ 1.893,  1.454},
+	{ 1.898,  1.455},
+	{ 1.903,  1.456},
+	{ 1.903,  1.456},
+	{ 1.903,  1.451},
+	{ 1.903,  1.446},
+	{ 1.903,  1.441},
+	{ 1.903,  1.436},
+	{ 1.903,  1.431},
+	{ 1.903,  1.426},
+	{ 1.903,  1.421},
+	{ 1.903,  1.415},
+	{ 1.903,  1.410},
+	{ 1.903,  1.405},
+	{ 1.903,  1.400},
+	{ 1.903,  1.395},
+	{ 1.903,  1.390},
+	{ 1.903,  1.385},
+	{ 1.903,  1.380},
+	{ 1.903,  1.375},
+	{ 1.903,  1.370},
+	{ 1.903,  1.365},
+	{ 1.903,  1.360},
+	{ 1.902,  1.355},
+	{ 1.902,  1.350},
+	{ 1.902,  1.345},
+	{ 1.902,  1.340},
+	{ 1.902,  1.335},
+	{ 1.902,  1.330},
+	{ 1.902,  1.325},
+	{ 1.902,  1.320},
+	{ 1.902,  1.315},
+	{ 1.902,  1.310},
+	{ 1.902,  1.305},
+	{ 1.902,  1.300},
+	{ 1.901,  1.295},
+	{ 1.901,  1.290},
+	{ 1.901,  1.285},
+	{ 1.901,  1.280},
+	{ 1.901,  1.275},
+	{ 1.901,  1.270},
+	{ 1.901,  1.265},
+	{ 1.900,  1.260},
+	{ 1.900,  1.255},
+	{ 1.900,  1.250},
+	{ 1.900,  1.245},
+	{ 1.900,  1.240},
+	{ 1.900,  1.235},
+	{ 1.899,  1.230},
+	{ 1.899,  1.225},
+	{ 1.899,  1.220},
+	{ 1.899,  1.215},
+	{ 1.898,  1.210},
+	{ 1.898,  1.205},
+	{ 1.898,  1.200},
+	{ 1.897,  1.195},
+	{ 1.897,  1.190},
+	{ 1.897,  1.185},
+	{ 1.896,  1.180},
+	{ 1.896,  1.175},
+	{ 1.896,  1.170},
+	{ 1.896,  1.165},
+	{ 1.895,  1.160},
+	{ 1.895,  1.155},
+	{ 1.894,  1.150},
+	{ 1.894,  1.145},
+	{ 1.894,  1.140},
+	{ 1.893,  1.135},
+	{ 1.893,  1.130},
+	{ 1.893,  1.125},
+	{ 1.892,  1.120},
+	{ 1.892,  1.115},
+	{ 1.892,  1.110},
+	{ 1.891,  1.105},
+	{ 1.891,  1.100},
+	{ 1.891,  1.095},
+	{ 1.890,  1.090},
+	{ 1.890,  1.085},
+	{ 1.890,  1.080},
+	{ 1.889,  1.075},
+	{ 1.889,  1.070},
+	{ 1.889,  1.065},
+	{ 1.888,  1.060},
+	{ 1.888,  1.055},
+	{ 1.888,  1.050},
+	{ 1.887,  1.045},
+	{ 1.887,  1.040},
+	{ 1.886,  1.035},
+	{ 1.886,  1.030},
+	{ 1.886,  1.025},
+	{ 1.885,  1.020},
+	{ 1.885,  1.015},
+	{ 1.884,  1.010},
+	{ 1.884,  1.005},
+	{ 1.884,  1.000},
+	{ 1.883,  0.995},
+	{ 1.883,  0.990},
+	{ 1.882,  0.985},
+	{ 1.882,  0.980},
+	{ 1.881,  0.975},
+	{ 1.881,  0.970},
+	{ 1.880,  0.965},
+	{ 1.880,  0.960},
+	{ 1.879,  0.955},
+	{ 1.879,  0.950},
+	{ 1.879,  0.945},
+	{ 1.878,  0.940},
+	{ 1.878,  0.935},
+	{ 1.877,  0.931},
+	{ 1.877,  0.926},
+	{ 1.876,  0.921},
+	{ 1.876,  0.916},
+	{ 1.875,  0.911},
+	{ 1.875,  0.906},
+	{ 1.875,  0.901},
+	{ 1.874,  0.896},
+	{ 1.874,  0.891},
+	{ 1.873,  0.886},
+	{ 1.873,  0.881},
+	{ 1.873,  0.876},
+	{ 1.872,  0.871},
+	{ 1.872,  0.866},
+	{ 1.871,  0.861},
+	{ 1.871,  0.856},
+	{ 1.871,  0.851},
+	{ 1.870,  0.846},
+	{ 1.870,  0.841},
+	{ 1.869,  0.836},
+	{ 1.869,  0.831},
+	{ 1.869,  0.826},
+	{ 1.868,  0.821},
+	{ 1.868,  0.816},
+	{ 1.867,  0.811},
+	{ 1.867,  0.806},
+	{ 1.867,  0.801},
+	{ 1.866,  0.796},
+	{ 1.866,  0.791},
+	{ 1.866,  0.786},
+	{ 1.865,  0.781},
+	{ 1.865,  0.776},
+	{ 1.865,  0.771},
+	{ 1.864,  0.766},
+	{ 1.864,  0.761},
+	{ 1.864,  0.756},
+	{ 1.864,  0.751},
+	{ 1.863,  0.746},
+	{ 1.863,  0.741},
+	{ 1.863,  0.736},
+	{ 1.863,  0.731},
+	{ 1.862,  0.726},
+	{ 1.862,  0.721},
+	{ 1.862,  0.716},
+	{ 1.861,  0.711},
+	{ 1.861,  0.706},
+	{ 1.860,  0.701},
+	{ 1.860,  0.696},
+	{ 1.859,  0.691},
+	{ 1.859,  0.686},
+	{ 1.859,  0.681},
+	{ 1.858,  0.676},
+	{ 1.858,  0.671},
+	{ 1.858,  0.666},
+	{ 1.857,  0.661},
+	{ 1.857,  0.656},
+	{ 1.857,  0.651},
+	{ 1.856,  0.646},
+	{ 1.856,  0.641},
+	{ 1.856,  0.636},
+	{ 1.855,  0.631},
+	{ 1.855,  0.626},
+	{ 1.855,  0.621},
+	{ 1.855,  0.616},
+	{ 1.854,  0.611},
+	{ 1.854,  0.606},
+	{ 1.854,  0.601},
+	{ 1.853,  0.596},
+	{ 1.853,  0.591},
+	{ 1.853,  0.586},
+	{ 1.852,  0.581},
+	{ 1.852,  0.576},
+	{ 1.851,  0.571},
+	{ 1.851,  0.566},
+	{ 1.850,  0.561},
+	{ 1.850,  0.556},
+	{ 1.850,  0.551},
+	{ 1.849,  0.546},
+	{ 1.849,  0.541},
+	{ 1.848,  0.536},
+	{ 1.848,  0.531},
+	{ 1.848,  0.526},
+	{ 1.847,  0.521},
+	{ 1.847,  0.516},
+	{ 1.846,  0.511},
+	{ 1.846,  0.506},
+	{ 1.846,  0.501},
+	{ 1.845,  0.496},
+	{ 1.845,  0.491},
+	{ 1.845,  0.486},
+	{ 1.844,  0.481},
+	{ 1.844,  0.476},
+	{ 1.843,  0.471},
+	{ 1.843,  0.466},
+	{ 1.842,  0.461},
+	{ 1.842,  0.456},
+	{ 1.842,  0.451},
+	{ 1.841,  0.446},
+	{ 1.841,  0.441},
+	{ 1.840,  0.436},
+	{ 1.840,  0.431},
+	{ 1.840,  0.426},
+	{ 1.840,  0.421},
+	{ 1.841,  0.416},
+	{ 1.842,  0.412},
+	{ 1.847,  0.409},
+	{ 1.852,  0.409},
+	{ 1.857,  0.409},
+	{ 1.862,  0.410},
+	{ 1.867,  0.410},
+	{ 1.872,  0.409},
+	{ 1.877,  0.409},
+	{ 1.882,  0.409},
+	{ 1.887,  0.409},
+	{ 1.892,  0.409},
+	{ 1.897,  0.409},
+	{ 1.902,  0.409},
+	{ 1.907,  0.409},
+	{ 1.912,  0.408},
+	{ 1.917,  0.408},
+	{ 1.922,  0.408},
+	{ 1.927,  0.408},
+	{ 1.932,  0.407},
+	{ 1.937,  0.407},
+	{ 1.942,  0.407},
+	{ 1.947,  0.407},
+	{ 1.952,  0.407},
+	{ 1.957,  0.407},
+	{ 1.962,  0.406},
+	{ 1.967,  0.406},
+	{ 1.972,  0.406},
+	{ 1.977,  0.406},
+	{ 1.982,  0.406},
+	{ 1.987,  0.406},
+	{ 1.992,  0.406},
+	{ 1.997,  0.406},
+	{ 2.002,  0.406},
+	{ 2.007,  0.406},
+	{ 2.012,  0.406},
+	{ 2.017,  0.406},
+	{ 2.022,  0.406},
+	{ 2.027,  0.406},
+	{ 2.032,  0.406},
+	{ 2.037,  0.406},
+	{ 2.042,  0.406},
+	{ 2.047,  0.406},
+	{ 2.052,  0.406},
+	{ 2.057,  0.406},
+	{ 2.062,  0.406},
+	{ 2.067,  0.406},
+	{ 2.072,  0.406},
+	{ 2.077,  0.406},
+	{ 2.082,  0.406},
+	{ 2.087,  0.406},
+	{ 2.092,  0.406},
+	{ 2.097,  0.406},
+	{ 2.102,  0.406},
+	{ 2.107,  0.406},
+	{ 2.112,  0.406},
+	{ 2.117,  0.406},
+	{ 2.122,  0.406},
+	{ 2.127,  0.406},
+	{ 2.132,  0.406},
+	{ 2.137,  0.406},
+	{ 2.142,  0.405},
+	{ 2.147,  0.405},
+	{ 2.152,  0.405},
+	{ 2.157,  0.405},
+	{ 2.162,  0.405},
+	{ 2.167,  0.405},
+	{ 2.172,  0.405},
+	{ 2.177,  0.405},
+	{ 2.182,  0.405},
+	{ 2.187,  0.405},
+	{ 2.192,  0.404},
+	{ 2.197,  0.404},
+	{ 2.202,  0.404},
+	{ 2.207,  0.404},
+	{ 2.212,  0.404},
+	{ 2.217,  0.404},
+	{ 2.222,  0.404},
+	{ 2.227,  0.404},
+	{ 2.232,  0.404},
+	{ 2.237,  0.404},
+	{ 2.242,  0.403},
+	{ 2.247,  0.403},
+	{ 2.252,  0.403},
+	{ 2.257,  0.403},
+	{ 2.262,  0.403},
+	{ 2.267,  0.403},
+	{ 2.273,  0.403},
+	{ 2.278,  0.403},
+	{ 2.283,  0.402},
+	{ 2.288,  0.402},
+	{ 2.293,  0.402},
+	{ 2.298,  0.402},
+	{ 2.303,  0.402},
+	{ 2.308,  0.402},
+	{ 2.313,  0.402},
+	{ 2.318,  0.401},
+	{ 2.323,  0.401},
+	{ 2.328,  0.401},
+	{ 2.333,  0.401},
+	{ 2.338,  0.401},
+	{ 2.343,  0.400},
+	{ 2.348,  0.400},
+	{ 2.353,  0.400},
+	{ 2.358,  0.400},
+	{ 2.363,  0.400},
+	{ 2.368,  0.399},
+	{ 2.373,  0.399},
+	{ 2.378,  0.399},
+	{ 2.383,  0.399},
+	{ 2.388,  0.399},
+	{ 2.393,  0.399},
+	{ 2.398,  0.398},
+	{ 2.403,  0.398},
+	{ 2.408,  0.398},
+	{ 2.413,  0.398},
+	{ 2.418,  0.397},
+	{ 2.423,  0.397},
+	{ 2.428,  0.397},
+	{ 2.433,  0.396},
+	{ 2.438,  0.396},
+	{ 2.443,  0.395},
+	{ 2.448,  0.395},
+	{ 2.453,  0.395},
+	{ 2.458,  0.394},
+	{ 2.463,  0.394},
+	{ 2.468,  0.393},
+	{ 2.473,  0.393},
+	{ 2.478,  0.393},
+	{ 2.483,  0.392},
+	{ 2.488,  0.392},
+	{ 2.493,  0.392},
+	{ 2.498,  0.391},
+	{ 2.503,  0.391},
+	{ 2.508,  0.391},
+	{ 2.513,  0.390},
+	{ 2.518,  0.390},
+	{ 2.523,  0.390},
+	{ 2.528,  0.390},
+	{ 2.533,  0.389},
+	{ 2.538,  0.389},
+	{ 2.543,  0.389},
+	{ 2.548,  0.389},
+	{ 2.553,  0.388},
+	{ 2.558,  0.388},
+	{ 2.563,  0.388},
+	{ 2.568,  0.388},
+	{ 2.573,  0.388},
+	{ 2.578,  0.388},
+	{ 2.583,  0.388},
+	{ 2.588,  0.388},
+	{ 2.593,  0.387},
+	{ 2.598,  0.387},
+	{ 2.603,  0.387},
+	{ 2.608,  0.387},
+	{ 2.613,  0.387},
+	{ 2.618,  0.387},
+	{ 2.623,  0.387},
+	{ 2.628,  0.387},
+	{ 2.633,  0.387},
+	{ 2.638,  0.387},
+	{ 2.643,  0.388},
+	{ 2.647,  0.390},
+	{ 2.651,  0.393},
+	{ 2.654,  0.397},
+	{ 2.656,  0.401},
+	{ 2.657,  0.406},
+	{ 2.657,  0.411},
+	{ 2.657,  0.416},
+	{ 2.657,  0.421},
+	{ 2.657,  0.426},
+	{ 2.656,  0.431},
+	{ 2.656,  0.436},
+	{ 2.655,  0.441},
+	{ 2.655,  0.446},
+	{ 2.654,  0.451},
+	{ 2.653,  0.456},
+	{ 2.652,  0.461},
+	{ 2.652,  0.466},
+	{ 2.651,  0.471},
+	{ 2.650,  0.476},
+	{ 2.649,  0.481},
+	{ 2.649,  0.486},
+	{ 2.648,  0.491},
+	{ 2.647,  0.496},
+	{ 2.647,  0.501},
+	{ 2.646,  0.506},
+	{ 2.645,  0.511},
+	{ 2.645,  0.516},
+	{ 2.644,  0.521},
+	{ 2.643,  0.525},
+	{ 2.642,  0.530},
+	{ 2.642,  0.535},
+	{ 2.641,  0.540},
+	{ 2.640,  0.545},
+	{ 2.640,  0.550},
+	{ 2.639,  0.555},
+	{ 2.638,  0.560},
+	{ 2.638,  0.565},
+	{ 2.637,  0.570},
+	{ 2.637,  0.575},
+	{ 2.636,  0.580},
+	{ 2.635,  0.585},
+	{ 2.635,  0.590},
+	{ 2.634,  0.595},
+	{ 2.634,  0.600},
+	{ 2.634,  0.605},
+	{ 2.633,  0.610},
+	{ 2.633,  0.615},
+	{ 2.632,  0.620},
+	{ 2.632,  0.625},
+	{ 2.631,  0.630},
+	{ 2.631,  0.635},
+	{ 2.631,  0.640},
+	{ 2.630,  0.645},
+	{ 2.630,  0.650},
+	{ 2.629,  0.655},
+	{ 2.629,  0.660},
+	{ 2.629,  0.665},
+	{ 2.628,  0.670},
+	{ 2.628,  0.675},
+	{ 2.627,  0.680},
+	{ 2.627,  0.685},
+	{ 2.627,  0.690},
+	{ 2.626,  0.695},
+	{ 2.626,  0.700},
+	{ 2.625,  0.705},
+	{ 2.625,  0.710},
+	{ 2.625,  0.715},
+	{ 2.624,  0.720},
+	{ 2.624,  0.725},
+	{ 2.624,  0.730},
+	{ 2.623,  0.735},
+	{ 2.623,  0.740},
+	{ 2.622,  0.745},
+	{ 2.622,  0.750},
+	{ 2.622,  0.755},
+	{ 2.621,  0.760},
+	{ 2.621,  0.765},
+	{ 2.620,  0.770},
+	{ 2.620,  0.775},
+	{ 2.620,  0.780},
+	{ 2.619,  0.785},
+	{ 2.619,  0.790},
+	{ 2.618,  0.795},
+	{ 2.618,  0.800},
+	{ 2.617,  0.805},
+	{ 2.617,  0.810},
+	{ 2.617,  0.815},
+	{ 2.616,  0.820},
+	{ 2.615,  0.825},
+	{ 2.614,  0.830},
+	{ 2.614,  0.835},
+	{ 2.613,  0.840},
+	{ 2.612,  0.844},
+	{ 2.611,  0.849},
+	{ 2.610,  0.854},
+	{ 2.609,  0.859},
+	{ 2.608,  0.864},
+	{ 2.607,  0.869},
+	{ 2.606,  0.874},
+	{ 2.605,  0.879},
+	{ 2.603,  0.884},
+	{ 2.602,  0.888},
+	{ 2.601,  0.893},
+	{ 2.599,  0.898},
+	{ 2.597,  0.903},
+	{ 2.595,  0.907},
+	{ 2.592,  0.911},
+	{ 2.587,  0.913},
+	{ 2.583,  0.912},
+	{ 2.578,  0.910},
+	{ 2.573,  0.908},
+	{ 2.569,  0.906},
+	{ 2.564,  0.904},
+	{ 2.560,  0.901},
+	{ 2.556,  0.899},
+	{ 2.551,  0.897},
+	{ 2.547,  0.894},
+	{ 2.542,  0.892},
+	{ 2.538,  0.890},
+	{ 2.533,  0.888},
+	{ 2.529,  0.885},
+	{ 2.524,  0.883},
+	{ 2.520,  0.881},
+	{ 2.515,  0.879},
+	{ 2.511,  0.877},
+	{ 2.506,  0.875},
+	{ 2.501,  0.873},
+	{ 2.497,  0.871},
+	{ 2.492,  0.869},
+	{ 2.488,  0.867},
+	{ 2.483,  0.865},
+	{ 2.478,  0.863},
+	{ 2.474,  0.861},
+	{ 2.469,  0.859},
+	{ 2.465,  0.857},
+	{ 2.460,  0.855},
+	{ 2.455,  0.853},
+	{ 2.451,  0.851},
+	{ 2.446,  0.849},
+	{ 2.442,  0.847},
+	{ 2.437,  0.845},
+	{ 2.432,  0.843},
+	{ 2.428,  0.841},
+	{ 2.423,  0.840},
+	{ 2.418,  0.838},
+	{ 2.414,  0.836},
+	{ 2.409,  0.834},
+	{ 2.405,  0.832},
+	{ 2.400,  0.830},
+	{ 2.395,  0.828},
+	{ 2.391,  0.826},
+	{ 2.386,  0.824},
+	{ 2.382,  0.822},
+	{ 2.377,  0.820},
+	{ 2.373,  0.817},
+	{ 2.368,  0.815},
+	{ 2.363,  0.813},
+	{ 2.359,  0.811},
+	{ 2.354,  0.809},
+	{ 2.350,  0.807},
+	{ 2.345,  0.805},
+	{ 2.341,  0.803},
+	{ 2.336,  0.800},
+	{ 2.332,  0.798},
+	{ 2.327,  0.796},
+	{ 2.323,  0.794},
+	{ 2.318,  0.792},
+	{ 2.314,  0.789},
+	{ 2.309,  0.787},
+	{ 2.305,  0.785},
+	{ 2.300,  0.783},
+	{ 2.296,  0.781},
+	{ 2.291,  0.779},
+	{ 2.286,  0.777},
+	{ 2.282,  0.775},
+	{ 2.277,  0.773},
+	{ 2.272,  0.772},
+	{ 2.268,  0.770},
+	{ 2.263,  0.769},
+	{ 2.258,  0.768},
+	{ 2.253,  0.769},
+	{ 2.248,  0.771},
+	{ 2.245,  0.775},
+	{ 2.243,  0.779},
+	{ 2.242,  0.784},
+	{ 2.242,  0.789},
+	{ 2.241,  0.794},
+	{ 2.241,  0.799},
+	{ 2.242,  0.804},
+	{ 2.242,  0.809},
+	{ 2.243,  0.814},
+	{ 2.244,  0.819},
+	{ 2.245,  0.824},
+	{ 2.246,  0.829},
+	{ 2.247,  0.834},
+	{ 2.248,  0.839},
+	{ 2.249,  0.843},
+	{ 2.250,  0.848},
+	{ 2.251,  0.853},
+	{ 2.252,  0.858},
+	{ 2.253,  0.863},
+	{ 2.254,  0.868},
+	{ 2.256,  0.873},
+	{ 2.257,  0.878},
+	{ 2.258,  0.882},
+	{ 2.259,  0.887},
+	{ 2.260,  0.892},
+	{ 2.262,  0.897},
+	{ 2.263,  0.902},
+	{ 2.264,  0.907},
+	{ 2.265,  0.912},
+	{ 2.267,  0.916},
+	{ 2.268,  0.921},
+	{ 2.269,  0.926},
+	{ 2.270,  0.931},
+	{ 2.272,  0.936},
+	{ 2.273,  0.941},
+	{ 2.274,  0.946},
+	{ 2.275,  0.950},
+	{ 2.276,  0.955},
+	{ 2.278,  0.960},
+	{ 2.279,  0.965},
+	{ 2.280,  0.970},
+	{ 2.281,  0.975},
+	{ 2.282,  0.980},
+	{ 2.284,  0.984},
+	{ 2.285,  0.989},
+	{ 2.286,  0.994},
+	{ 2.287,  0.999},
+	{ 2.289,  1.004},
+	{ 2.290,  1.009},
+	{ 2.291,  1.014},
+	{ 2.292,  1.018},
+	{ 2.293,  1.023},
+	{ 2.295,  1.028},
+	{ 2.296,  1.033},
+	{ 2.297,  1.038},
+	{ 2.298,  1.043},
+	{ 2.300,  1.048},
+	{ 2.301,  1.052},
+	{ 2.302,  1.057},
+	{ 2.303,  1.062},
+	{ 2.304,  1.067},
+	{ 2.306,  1.072},
+	{ 2.307,  1.077},
+	{ 2.308,  1.082},
+	{ 2.310,  1.086},
+	{ 2.311,  1.091},
+	{ 2.312,  1.096},
+	{ 2.313,  1.101},
+	{ 2.315,  1.106},
+	{ 2.316,  1.111},
+	{ 2.317,  1.115},
+	{ 2.318,  1.120},
+	{ 2.320,  1.125},
+	{ 2.321,  1.130},
+	{ 2.322,  1.135},
+	{ 2.324,  1.140},
+	{ 2.325,  1.144},
+	{ 2.326,  1.149},
+	{ 2.328,  1.154},
+	{ 2.329,  1.159},
+	{ 2.330,  1.164},
+	{ 2.332,  1.169},
+	{ 2.333,  1.173},
+	{ 2.335,  1.178},
+	{ 2.336,  1.183},
+	{ 2.338,  1.188},
+	{ 2.339,  1.193},
+	{ 2.340,  1.197},
+	{ 2.342,  1.202},
+	{ 2.343,  1.207},
+	{ 2.345,  1.212},
+	{ 2.347,  1.216},
+	{ 2.348,  1.221},
+	{ 2.350,  1.226},
+	{ 2.351,  1.231},
+	{ 2.353,  1.235},
+	{ 2.355,  1.240},
+	{ 2.356,  1.245},
+	{ 2.358,  1.250},
+	{ 2.360,  1.254},
+	{ 2.361,  1.259},
+	{ 2.363,  1.264},
+	{ 2.364,  1.269},
+	{ 2.366,  1.273},
+	{ 2.367,  1.278},
+	{ 2.369,  1.283},
+	{ 2.371,  1.288},
+	{ 2.372,  1.292},
+	{ 2.374,  1.297},
+	{ 2.376,  1.302},
+	{ 2.377,  1.307},
+	{ 2.379,  1.311},
+	{ 2.381,  1.316},
+	{ 2.382,  1.321},
+	{ 2.384,  1.325},
+	{ 2.386,  1.330},
+	{ 2.388,  1.335},
+	{ 2.390,  1.339},
+	{ 2.391,  1.344},
+	{ 2.393,  1.349},
+	{ 2.395,  1.353},
+	{ 2.397,  1.358},
+	{ 2.399,  1.363},
+	{ 2.401,  1.367},
+	{ 2.403,  1.372},
+	{ 2.404,  1.377},
+	{ 2.406,  1.381},
+	{ 2.408,  1.386},
+	{ 2.409,  1.391},
+	{ 2.411,  1.395},
+	{ 2.413,  1.400},
+	{ 2.415,  1.405},
+	{ 2.417,  1.409},
+	{ 2.419,  1.414},
+	{ 2.421,  1.419},
+	{ 2.423,  1.423},
+	{ 2.425,  1.428},
+	{ 2.427,  1.432},
+	{ 2.429,  1.437},
+	{ 2.431,  1.442},
+	{ 2.433,  1.446},
+	{ 2.434,  1.451},
+	{ 2.436,  1.456},
+	{ 2.437,  1.461},
+	{ 2.436,  1.465},
+	{ 2.433,  1.469},
+	{ 2.429,  1.472},
+	{ 2.424,  1.474},
+	{ 2.419,  1.475},
+	{ 2.414,  1.476},
+	{ 2.409,  1.477},
+	{ 2.404,  1.478},
+	{ 2.400,  1.478},
+	{ 2.395,  1.479},
+	{ 2.390,  1.479},
+	{ 2.385,  1.480},
+	{ 2.380,  1.480},
+	{ 2.375,  1.480},
+	{ 2.370,  1.480},
+	{ 2.365,  1.481},
+	{ 2.360,  1.481},
+	{ 2.355,  1.481},
+	{ 2.349,  1.481},
+	{ 2.344,  1.481},
+	{ 2.339,  1.481},
+	{ 2.334,  1.481},
+	{ 2.329,  1.481},
+	{ 2.324,  1.481},
+	{ 2.319,  1.481},
+	{ 2.314,  1.482},
+	{ 2.309,  1.482},
+	{ 2.304,  1.482},
+	{ 2.299,  1.482},
+	{ 2.294,  1.482},
+	{ 2.289,  1.482},
+	{ 2.284,  1.482},
+	{ 2.279,  1.482},
+	{ 2.274,  1.482},
+	{ 2.269,  1.483},
+	{ 2.264,  1.483},
+	{ 2.259,  1.483},
+	{ 2.254,  1.483},
+	{ 2.249,  1.483},
+	{ 2.244,  1.483},
+	{ 2.239,  1.484},
+	{ 2.234,  1.484},
+	{ 2.229,  1.484},
+	{ 2.224,  1.484},
+	{ 2.219,  1.485},
+	{ 2.214,  1.485},
+	{ 2.209,  1.485},
+	{ 2.204,  1.486},
+	{ 2.199,  1.486},
+	{ 2.194,  1.486},
+	{ 2.189,  1.486},
+	{ 2.184,  1.487},
+	{ 2.179,  1.487},
+	{ 2.174,  1.488},
+	{ 2.169,  1.488},
+	{ 2.164,  1.488},
+	{ 2.159,  1.489},
+	{ 2.154,  1.489},
+	{ 2.149,  1.489},
+	{ 2.144,  1.490},
+	{ 2.139,  1.490},
+	{ 2.134,  1.491},
+	{ 2.129,  1.491},
+	{ 2.124,  1.491},
+	{ 2.119,  1.492},
+	{ 2.114,  1.492},
+	{ 2.109,  1.492},
+	{ 2.104,  1.493},
+	{ 2.099,  1.493},
+	{ 2.094,  1.493},
+	{ 2.089,  1.494},
+	{ 2.084,  1.494},
+	{ 2.079,  1.494},
+	{ 2.074,  1.494},
+	{ 2.069,  1.495},
+	{ 2.064,  1.495},
+	{ 2.059,  1.495},
+	{ 2.054,  1.495},
+	{ 2.049,  1.496},
+	{ 2.044,  1.496},
+	{ 2.039,  1.496},
+	{ 2.034,  1.496},
+	{ 2.029,  1.496},
+	{ 2.024,  1.496},
+	{ 2.019,  1.496},
+	{ 2.014,  1.496},
+	{ 2.009,  1.496},
+	{ 2.004,  1.496},
+	{ 1.999,  1.496},
+	{ 1.994,  1.497},
+	{ 1.989,  1.497},
+	{ 1.984,  1.497},
+	{ 1.979,  1.497},
+	{ 1.974,  1.497},
+	{ 1.969,  1.497},
+	{ 1.964,  1.497},
+	{ 1.959,  1.497},
+	{ 1.954,  1.496},
+	{ 1.949,  1.495},
+	{ 1.945,  1.493},
+	{ 1.941,  1.491}
+};
diff --git a/gtdynamics/cablerobot/src/data/test_traj.h b/gtdynamics/cablerobot/src/data/test_traj.h
new file mode 100644
index 00000000..f9d434d4
--- /dev/null
+++ b/gtdynamics/cablerobot/src/data/test_traj.h
@@ -0,0 +1,20 @@
+bool painton[] = {
+	1, 1, 0, 1, 1, 1
+};
+uint8_t colorinds[] = {
+	0, 0, 1, 1, 2, 3
+};
+uint8_t colorpalette[][3] = {
+	{4, 49, 75},
+	{209, 4, 32},
+	{236, 237, 237},
+	{0, 0, 0}
+};
+float traj[][2] = {
+	{ 0.100,  1.100},
+	{ 0.200,  1.000},
+	{ 0.300,  0.900},
+	{ 0.400,  1.100},
+	{ 0.500,  1.200},
+	{ 0.100,  1.150}
+};
diff --git a/gtdynamics/cablerobot/src/draw_cdpr.py b/gtdynamics/cablerobot/src/draw_cdpr.py
new file mode 100644
index 00000000..58928d66
--- /dev/null
+++ b/gtdynamics/cablerobot/src/draw_cdpr.py
@@ -0,0 +1,134 @@
+"""
+GTDynamics Copyright 2021, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+See LICENSE for the license information
+
+@file  draw_cdpr.py
+@brief Utility functions for plotting a cable robot trajectory
+@author Frank Dellaert
+@author Gerry Chen
+"""
+
+import gtdynamics as gtd
+import gtsam
+import matplotlib.pyplot as plt
+import matplotlib.animation as animation
+import numpy as np
+from gtsam import Pose3, Rot3
+
+from cdpr_planar import Cdpr
+
+BOXINDS = np.array([0,1,2,3,0]).reshape(5,1)
+
+def pose32xy(x):
+    """Extracts just the planar translational component of a Pose3"""
+    return x.translation()[[0, 2]]
+def a_coords(cdpr):
+    """Returns a 2x5 array of the xy coordinates of the frame corners"""
+    return cdpr.params.a_locs[BOXINDS, [0,2]].T
+def b_coords(cdpr, x):
+    """Returns a 2x5 array of the xy coordinates of the end-effector mounting points"""
+    return (pose32xy(x) + cdpr.params.b_locs[BOXINDS, [0, 2]]).T
+def ab_coords(cdpr, x, ji):
+    """Returns a 2x2 arrays of the xy coordinates of the specified cable's start/end locations."""
+    b = b_coords(cdpr, x)
+    return np.array([[cdpr.params.a_locs[ji][0], b[0, ji]],
+                     [cdpr.params.a_locs[ji][2], b[1, ji]]])
+def draw_cdpr(ax, cdpr, x):
+    """Draws the CDPR in the specified axis.
+    Args:
+        ax (plt.Axes): matplotlib axis object
+        cdpr (Cdpr): cable robot object
+        x (gtsam.Pose3): current pose of the end effector
+    
+    Returns:
+        tuple(): matplotlib line objects:
+            l_a - the frame
+            l_b - the end effector
+            ls_ab - a 4-list containing the 4 cable lines
+    """
+    l_a, = ax.plot(*a_coords(cdpr), 'k-')
+    l_b, = ax.plot(*b_coords(cdpr, x), color='#caa472')
+    ls_ab = [ax.plot(*ab_coords(cdpr, x, ji))[0] for ji in range(4)]
+    ax.axis('equal')
+    ax.set_xlabel('x(m)');ax.set_ylabel('y(m)');ax.set_title('Trajectory');ax.grid()
+    return l_a, l_b, ls_ab
+def redraw_cdpr(l_a, l_b, ls_ab, cdpr, x):
+    """Updates the lines for the frame, end effector, and cables"""
+    l_a.set_data(*a_coords(cdpr))
+    l_b.set_data(*b_coords(cdpr, x))
+    [ls_ab[ci].set_data(*ab_coords(cdpr, x, ci)) for ci in range(4)]
+    return l_a, l_b, *ls_ab
+def draw_traj(ax, cdpr, des_xy, act_xy):
+    """Draws the desired and actual x/y trajectories"""
+    l_des, = plt.plot(*des_xy, 'r-')  # desired trajectory
+    l_act, = plt.plot(*act_xy, 'k-')  # actual trajectory
+    ax.axis('equal')
+    ax.set_xlabel('x(m)');ax.set_ylabel('y(m)');ax.set_title('Trajectory')
+    return l_des, l_act
+def redraw_traj(l_des, l_act, des_xy, act_xy, N=None):
+    """Updates the lines for the trajectory drawing"""
+    if N is None:
+        N = des_xy.shape[1]
+    l_des.set_data(*des_xy[:, :N])
+    l_act.set_data(*act_xy[:, :N])
+    return l_des, l_act
+def draw_ctrl(ax, cdpr, tensions, Tf, dt):
+    """Draws the control tension signals"""
+    ls_ctrl = ax.plot(np.arange(0,Tf,dt), tensions)
+    ax.plot([0, Tf], [cdpr.params.tmin,]*2, 'r--')
+    ax.plot([0, Tf], [cdpr.params.tmax,]*2, 'r--')
+    ax.set_xlabel('time (s)');ax.set_ylabel('Motor torque (N.m)');ax.set_title('Control Inputs')
+    ax.grid()
+    return ls_ctrl,
+def redraw_ctrl(ls_ctrl, tensions, Tf, dt, N=None):
+    """Updates the lines for the tensions plot"""
+    if N is None:
+        N = tensions.shape[0]
+    else:
+        ls_ctrl[0].axes.set_xlim(max(0, dt*N-10), max(10, dt*N))
+    for ji in range(4):
+        ls_ctrl[ji].set_data(np.arange(0,Tf,dt)[:N], tensions[:N, ji])
+    return *ls_ctrl,
+
+def plot_trajectory(cdpr, result, Tf, dt, N, x_des, step=1):
+    """Animates the cdpr and controls in side-by-side subplots.
+
+    Args:
+        cdpr (Cdpr): cable robot object
+        result (gtsam.Values): the data from the simulation including poses and tensions
+        Tf (float): final time
+        dt (float): time step interval
+        N (int): number of discrete samples
+        x_des (List[gtsam.Pose3]): the desired trajectory as a list of gtsam Pose3 objects
+        step (int, optional): number of time steps to update the animation by each time. 1 doesn't skip any frames, and e.g. 10 would skip 9 frames at a time and update at a period of 10*dt. Defaults to 1.
+
+    Returns:
+        matplotlib.animation.FuncAnimation: a matplotlib animation object
+    """    
+    # extract useful variables as lists
+    act_T = [gtd.Pose(result, cdpr.ee_id(), k) for k in range(N+1)]
+    act_xy = np.array([pose32xy(pose) for pose in act_T]).T
+    des_xy = np.array([pose32xy(pose) for pose in x_des]).T
+    tensions = np.array([[gtd.Torque(result, ji, k) for ji in range(4)] for k in range(N)])
+
+    # plot
+    fig = plt.figure(1, figsize=(12,4))
+    # xy plot
+    ax1 = plt.subplot(1,2,1)
+    cdpr_lines = draw_cdpr(ax1, cdpr, gtd.Pose(result, cdpr.ee_id(), 0))
+    traj_lines = draw_traj(ax1, cdpr, des_xy, act_xy)
+    # controls
+    ax2 = plt.subplot(1,2,2)
+    ctrl_lines = draw_ctrl(ax2, cdpr, tensions, Tf, dt)
+
+    # animate
+    plt.rcParams["savefig.dpi"] = 80
+    def update_line(num):
+        cdpr_to_update = redraw_cdpr(*cdpr_lines, cdpr, gtd.Pose(result, cdpr.ee_id(), num))
+        traj_to_update = redraw_traj(*traj_lines, des_xy, act_xy, num if num > 0 else None)
+        ctrl_to_update = redraw_ctrl(*ctrl_lines, tensions, Tf, dt, num if num > 0 else None)
+        return [*cdpr_to_update, *traj_to_update, *ctrl_to_update]
+    return animation.FuncAnimation(fig, update_line, frames=range(0, N, step),
+                                   interval=dt*step*1e3, blit=True)
diff --git a/gtdynamics/cablerobot/src/draw_controller.py b/gtdynamics/cablerobot/src/draw_controller.py
new file mode 100644
index 00000000..c3b434a2
--- /dev/null
+++ b/gtdynamics/cablerobot/src/draw_controller.py
@@ -0,0 +1,155 @@
+"""
+GTDynamics Copyright 2021, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+See LICENSE for the license information
+
+@file  draw_cdpr.py
+@brief Utility functions for drawing a cable robot's controller using opencv
+@author Frank Dellaert
+@author Gerry Chen
+"""
+
+import gtdynamics as gtd
+import gtsam
+import matplotlib.pyplot as plt
+import matplotlib.animation as animation
+import numpy as np
+import cv2
+from gtsam import Pose3, Rot3
+
+from cdpr_planar import Cdpr
+from cdpr_controller_ilqr import CdprControllerIlqr
+from draw_cdpr import pose32xy, a_coords, b_coords, ab_coords
+
+COLORS = [(255, 0, 0), (0, 255, 0), (0, 0, 255), (127, 127, 127)]
+
+def x2img(x):
+    return x * 130 + 5
+def img2x(img):
+    return (img - 5) / 130
+def condition(pts):
+    """Rescales and reshapes a 2xN numpy matrix to be passed into cv2.polylines
+    """
+    return x2img(pts.T.reshape((-1, 1, 2))).astype(np.int32)
+
+def draw_cdpr(img, cdpr, x):
+    """Draws the CDPR in the specified axis.
+    Args:
+        ax (plt.Axes): matplotlib axis object
+        cdpr (Cdpr): cable robot object
+        x (gtsam.Pose3): current pose of the end effector
+    
+    Returns:
+        tuple(): matplotlib line objects:
+            l_a - the frame
+            l_b - the end effector
+            ls_ab - a 4-list containing the 4 cable lines
+    """
+    cv2.polylines(img, [condition(a_coords(cdpr))], False, (0, 0, 0))
+    cv2.polylines(img, [condition(b_coords(cdpr, x))], False, (202, 164, 114))
+    for ji in range(4):
+        cv2.polylines(img, [condition(ab_coords(cdpr, x, ji))], False, COLORS[ji])
+    # ax.set_xlabel('x(m)');ax.set_ylabel('y(m)');ax.set_title('Trajectory');ax.grid()
+    return img
+
+def draw_traj(img, cdpr, controller, act_xy, N):
+    """Draws the desired and actual x/y trajectories"""
+    img = img.astype(np.float)
+    kw2 = 81
+    kw = 40
+    for k in range(N):
+        K, uff, Vff, Tff = controller.gains_ff[k]
+        K = np.abs(K) / 400 * 7
+        # y, x = act_xy[k, 0, :]
+        y, x = x2img(Tff.translation()[[0, 2]]).astype(np.int)
+        for ci in range(4):
+            patch = np.zeros((kw2, kw2, 1))
+            patch[kw, kw] = 1
+            patch[:, :, 0] = cv2.GaussianBlur(patch, (kw2, kw2), sigmaX=K[ci, 9], sigmaY=K[ci, 11]) * (4-ci)
+            alph_src = img[x - kw:x + kw + 1, y - kw:y + kw + 1, 3:]
+            img_src = img[x - kw:x + kw + 1, y - kw:y + kw + 1, :3]
+            alph_dst = alph_src * (1 - patch) + patch
+            tmp = (img_src * alph_src)
+            img[x - kw:x + kw + 1, y - kw:y + kw + 1, :3] = (
+                img_src * alph_src * (1 - patch) + COLORS[ci] * patch) / alph_dst
+            img[x - kw:x + kw + 1, y - kw:y + kw + 1, 3:] = alph_dst
+    img[:, :, 3] = img[:, :, 3]*255
+    img = img.astype(np.uint8)
+    imgtraj = cv2.polylines(np.zeros(img.shape[:2]), [act_xy], False, (1))
+    img[imgtraj!=0, :3] = 0
+    return img
+
+def draw_controller_one(img, cdpr, controller: CdprControllerIlqr, k, cablei):
+    """Draws the controller torque response for every point x in the space, at time step k"""
+    xvals = img2x(np.arange(0, img.shape[0]))
+    yvals = img2x(np.arange(0, img.shape[1]))
+    K, uff, Vff, Tff = controller.gains_ff[k]
+    K = K * 100  # this needs to get scaled with the square of dN
+    dx = np.zeros((12, *img.shape[:-1]))
+    dx[9, :] = (xvals - Tff.translation()[0]).reshape((-1, 1))
+    dx[11, :] = (yvals - Tff.translation()[2]).reshape((1, -1))
+    uvals = np.einsum('i,ijk->jk', K[cablei, :], dx) + uff[cablei]
+    uvals = (uvals + 500) / 1000
+    for ci in range(3):
+        img[:, :, ci] = np.uint8(uvals * COLORS[cablei][ci])
+    return img
+
+
+def draw_controller_anim(cdpr: Cdpr,
+                         controller: CdprControllerIlqr,
+                         result: gtsam.Values,
+                         N,
+                         step=1):
+    """Animates the cdpr and feedback gains in side-by-side subplots.
+
+    Args:
+        cdpr (Cdpr): cable robot object
+        controller (CdprControllerIlqr): cable robot controller object
+        result (gtsam.Values): the data from the simulation including poses and tensions
+        N (int): number of discrete samples
+        step (int, optional): number of time steps to update the animation by each time. 1 doesn't skip any frames, and e.g. 10 would skip 9 frames at a time and update at a period of 10*dt. Defaults to 1.
+
+    Returns:
+        matplotlib.animation.FuncAnimation: a matplotlib animation object
+    """
+    # extract useful variables as lists
+    act_T = [gtd.Pose(result, cdpr.ee_id(), k) for k in range(N+1)]
+    act_xy = np.array([pose32xy(pose) for pose in act_T]).T
+    w, h = np.max(cdpr.params.a_locs, axis=0)[[0, 2]]
+
+    # plot
+    def get_imgs(k):
+        if k == 0:
+            k = N - 20
+        img_cdpr = 255 * np.ones((int(x2img(h)+5), int(x2img(w)+5), 3), dtype=np.uint8)
+        cv2.polylines(img_cdpr, [condition(act_xy[:, :k])], False, (0, 0, 0))
+        draw_cdpr(img_cdpr, cdpr, act_T[k])
+        img_cdpr = img_cdpr
+        imgs = []
+        for i in range(4):
+            img = np.zeros((int(x2img(h)+5), int(x2img(w)+5), 3), dtype=np.uint8)
+            draw_controller_one(img, cdpr, controller, k, i)
+            cv2.polylines(img_cdpr, [condition(act_xy[:, :k])], False, (0, 0, 0))
+            draw_cdpr(img, cdpr, act_T[k])
+            imgs.append(img)
+        img_traj = 255 * np.ones((int(x2img(h)+5), int(x2img(w)+5), 4), dtype=np.uint8)
+        img_traj[:, :, 3] = 1
+        img_traj = draw_traj(img_traj, cdpr, controller, condition(act_xy).astype(np.int), N)
+        return img_cdpr, imgs, img_traj
+
+    fig, axes = plt.subplots(1, 5, figsize=(18, 3))
+    def plot_imgs(axes, k):
+        img, imgs, _ = get_imgs(k)
+        axes[0].imshow(img, origin='lower')
+        for i in range(4):
+            axes[i+1].imshow(imgs[i], origin='lower')
+    plot_imgs(axes, N - 1)
+    def update_line(k):
+        plot_imgs(axes, k)
+        return []
+    return animation.FuncAnimation(fig,
+                                   update_line,
+                                   frames=range(0, N, step),
+                                   interval=0.01 * step * 1e3,
+                                   blit=True)
diff --git a/gtdynamics/cablerobot/src/gerry00_planar_sim.py b/gtdynamics/cablerobot/src/gerry00_planar_sim.py
index 84b7f989..c4d02da4 100644
--- a/gtdynamics/cablerobot/src/gerry00_planar_sim.py
+++ b/gtdynamics/cablerobot/src/gerry00_planar_sim.py
@@ -17,8 +17,8 @@
 from gtsam import Pose3, Rot3
 
 from cdpr_planar import Cdpr
-from cdpr_planar_controller import CdprControllerBase
-from cdpr_planar_sim import cdpr_sim
+from cdpr_controller import CdprControllerBase
+from cdpr_planar_sim import CdprSimulator
 
 
 class DummyController(CdprControllerBase):
@@ -43,7 +43,8 @@ def main():
     gtd.InsertPose(x0, cdpr.ee_id(), 0, Pose3(Rot3(), (1.5, 0, 1.5)))
     gtd.InsertTwist(x0, cdpr.ee_id(), 0, np.zeros(6))
     # run simulation
-    result = cdpr_sim(cdpr, x0, controller, dt=dt, N=N)
+    sim = CdprSimulator(cdpr, x0, controller, dt=dt)
+    result = sim.run(N=N)
     poses = [gtd.Pose(result, cdpr.ee_id(), k) for k in range(N)]
 
     plt.figure(1)
diff --git a/gtdynamics/cablerobot/src/gerry01_planar_tracking.ipynb b/gtdynamics/cablerobot/src/gerry01_planar_tracking.ipynb
new file mode 100644
index 00000000..8d476287
--- /dev/null
+++ b/gtdynamics/cablerobot/src/gerry01_planar_tracking.ipynb
@@ -0,0 +1,191 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "adolescent-campus",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import gtdynamics as gtd\n",
+    "import gtsam\n",
+    "import numpy as np\n",
+    "from gtsam import Pose3, Rot3\n",
+    "\n",
+    "from cdpr_planar import Cdpr\n",
+    "from cdpr_controller_ilqr import CdprControllerIlqr as CdprController\n",
+    "# from cdpr_controller_tension_dist import CdprControllerTensionDist as CdprController\n",
+    "from cdpr_planar_sim import CdprSimulator"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "absolute-royalty",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# problem parameters\n",
+    "Tf = 1\n",
+    "dt = 0.01\n",
+    "N = int(Tf / dt)\n",
+    "cdpr = Cdpr()\n",
+    "# set up controller\n",
+    "x_des = [\n",
+    "    gtsam.Pose3(gtsam.Rot3(),\n",
+    "                (1.5 + np.cos(2 * np.pi * i / N), 0, 1.5 + np.sin(2 * np.pi * i / N)))\n",
+    "    for i in range(N)\n",
+    "]\n",
+    "# x_des[0] = x_des[1]"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "traditional-running",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# initial state\n",
+    "x0 = gtsam.Values()\n",
+    "gtd.InsertPose(x0, cdpr.ee_id(), 0, x_des[0])\n",
+    "gtd.InsertTwist(x0, cdpr.ee_id(), 0, x_des[0].localCoordinates(x_des[1])*N)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "northern-pottery",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# controller\n",
+    "controller = CdprController(cdpr,\n",
+    "                            x0,\n",
+    "                            x_des,\n",
+    "                            dt=dt,\n",
+    "                            Q=np.array([0, 1, 0, 1e3, 0, 1e3]),\n",
+    "                            R=np.array([1e-3]))"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "enclosed-capacity",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# run simulation\n",
+    "sim = CdprSimulator(cdpr, x0, controller, dt=dt);\n",
+    "result = sim.run(N=N, verbose=False);"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "copyrighted-model",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# plot utils\n",
+    "poses = [gtd.Pose(result, cdpr.ee_id(), k) for k in range(N+1)]\n",
+    "posesxy = np.array([[pose.x() for pose in poses], [pose.z() for pose in poses]])\n",
+    "desposesxy = np.array([[pose.x() for pose in x_des], [pose.z() for pose in x_des]]);\n",
+    "torques = np.array([[gtd.TorqueDouble(result, ji, k) for ji in range(4)] for k in range(N)]);\n",
+    "boxinds = np.array([0,1,2,3,0]).reshape(5,1)\n",
+    "def ee_coords(k):\n",
+    "    points = []\n",
+    "    for corner in [0,1,2,3,0]:\n",
+    "        pose = poses[k]\n",
+    "        point = pose.transformFrom(cdpr.params.b_locs[corner])\n",
+    "        points.append(point)\n",
+    "    points = np.array(points)\n",
+    "    return points[:, [0,2]].T\n",
+    "    # return (posesxy[:, k]+cdpr.params.b_locs[boxinds, [0,2]]).T\n",
+    "frame_coords = cdpr.params.a_locs[boxinds, [0,2]].T\n",
+    "def cable_coords(k, ji):\n",
+    "    point = poses[k].transformFrom(cdpr.params.b_locs[ji])\n",
+    "    return np.array([[cdpr.params.a_locs[ji][0], point[0]],\n",
+    "                     [cdpr.params.a_locs[ji][2], point[2]]])\n",
+    "    return np.array([[cdpr.params.a_locs[ji][0], posesxy[0][k]+cdpr.params.b_locs[ji][0]],\n",
+    "                     [cdpr.params.a_locs[ji][2], posesxy[1][k]+cdpr.params.b_locs[ji][2]]])"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "announced-pittsburgh",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# plot\n",
+    "import matplotlib.pyplot as plt\n",
+    "fig = plt.figure(1, figsize=(12,4))\n",
+    "# xy plot\n",
+    "plt.subplot(1,2,1)\n",
+    "plt.plot(*frame_coords, 'k-')\n",
+    "plt.plot(*desposesxy, 'r*') # desired trajectory\n",
+    "ltraj, = plt.plot(*posesxy, 'k-') # actual trajectory\n",
+    "lscables = []\n",
+    "for ji in range(4):\n",
+    "    lscables.append(plt.plot(*cable_coords(0, ji))[0])\n",
+    "lee, = plt.plot(*ee_coords(0))\n",
+    "plt.axis('equal')\n",
+    "plt.xlabel('x(m)');plt.ylabel('y(m)');plt.title('Trajectory')\n",
+    "# controls\n",
+    "plt.subplot(1,2,2)\n",
+    "lsctrl = plt.plot(np.arange(0,Tf,dt), torques)\n",
+    "plt.xlabel('time (s)');plt.ylabel('Cable tension (N)');plt.title('Control Inputs');"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "faced-compact",
+   "metadata": {
+    "scrolled": false
+   },
+   "outputs": [],
+   "source": [
+    "# animate\n",
+    "plt.rcParams[\"savefig.dpi\"] = 80\n",
+    "import matplotlib.animation as animation\n",
+    "from IPython.display import HTML\n",
+    "\n",
+    "def update_line(num):\n",
+    "    ltraj.set_data(posesxy[0][:num], posesxy[1][:num])\n",
+    "    for ji in range(4):\n",
+    "        lsctrl[ji].set_data(np.arange(0,Tf,dt)[:num], torques[:num, ji])\n",
+    "        lscables[ji].set_data(cable_coords(num, ji))\n",
+    "    lee.set_data(*ee_coords(num))\n",
+    "    lines = [ltraj, *lsctrl, *lscables, lee]\n",
+    "    return lines\n",
+    "\n",
+    "anim = animation.FuncAnimation(fig, update_line, len(posesxy[0]),\n",
+    "                               interval=100, blit=True)\n",
+    "HTML(anim.to_html5_video())"
+   ]
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Python 3",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.8.3"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 5
+}
diff --git a/gtdynamics/cablerobot/src/gerry01_planar_tracking.py b/gtdynamics/cablerobot/src/gerry01_planar_tracking.py
index a85d8733..533e0c01 100644
--- a/gtdynamics/cablerobot/src/gerry01_planar_tracking.py
+++ b/gtdynamics/cablerobot/src/gerry01_planar_tracking.py
@@ -17,12 +17,14 @@
 from gtsam import Pose3, Rot3
 
 from cdpr_planar import Cdpr
-from cdpr_planar_controller import CdprController
-from cdpr_planar_sim import cdpr_sim
+from cdpr_controller_ilqr import CdprControllerIlqr
+from cdpr_planar_sim import CdprSimulator
+
+import cProfile
 
 def main():
     Tf = 1
-    dt = 0.05
+    dt = 0.01
     N = int(Tf / dt)
     cdpr = Cdpr()
     # set up controller
@@ -32,29 +34,55 @@ def main():
         for i in range(N)
     ]
     x_des[0] = x_des[1]
+    # initial state
     x0 = gtsam.Values()
     gtd.InsertPose(x0, cdpr.ee_id(), 0, x_des[0])
     gtd.InsertTwist(x0, cdpr.ee_id(), 0, np.zeros(6))
-    controller = CdprController(cdpr,
+    # controller
+    controller = CdprControllerIlqr(cdpr,
                                 x0,
                                 x_des,
                                 dt=dt,
                                 Q=np.array([0, 1, 0, 1e3, 0, 1e3]),
                                 R=np.array([1e-3]))
     # run simulation
-    result = cdpr_sim(cdpr, x0, controller, dt=dt, N=N, verbose=True)
-    poses = [gtd.Pose(result, cdpr.ee_id(), k) for k in range(N)]
+    sim = CdprSimulator(cdpr, x0, controller, dt=dt)
+    result = sim.run(N=N, verbose=False)
+
+    return Tf, dt, N, cdpr, x_des, result
 
-    # print(poses)
+def plot(Tf, dt, N, cdpr, x_des, result):
+    # extract useful variables as lists
+    poses = [gtd.Pose(result, cdpr.ee_id(), k) for k in range(N+1)]
+    posesxy = np.array([[pose.x() for pose in poses], [pose.z() for pose in poses]])
+    desposesxy = np.array([[pose.x() for pose in x_des], [pose.z() for pose in x_des]])
+    torques = np.array([[gtd.TorqueDouble(result, ji, k) for ji in range(4)] for k in range(N)])
 
-    plt.figure(1)
-    plt.plot([pose.x() for pose in x_des], [pose.z() for pose in x_des], 'r-')
-    plt.plot([pose.x() for pose in poses], [pose.z() for pose in poses], 'k--')
-    plt.plot([*cdpr.params.a_locs[:, 0], cdpr.params.a_locs[0, 0]],
-             [*cdpr.params.a_locs[:, 2], cdpr.params.a_locs[0, 2]], 'k-')
+    # plot utils
+    boxinds = np.array([0,1,2,3,0]).reshape(5,1)
+    def ee_coords(k):
+        return (posesxy[:, k]+cdpr.params.b_locs[boxinds, [0,2]]).T
+    frame_coords = cdpr.params.a_locs[boxinds, [0,2]].T
+    def cable_coords(k, ji):
+        return np.array([[cdpr.params.a_locs[ji][0], posesxy[0][k]+cdpr.params.b_locs[ji][0]],
+                        [cdpr.params.a_locs[ji][2], posesxy[1][k]+cdpr.params.b_locs[ji][2]]])
+    # plot
+    fig = plt.figure(1, figsize=(12,4))
+    # xy plot
+    plt.subplot(1,2,1)
+    plt.plot(*frame_coords, 'k-')
+    plt.plot(*desposesxy, 'r*') # desired trajectory
+    ltraj, = plt.plot(*posesxy, 'k-') # actual trajectory
+    lscables = plt.plot(np.zeros((2,4)), np.zeros((2,4)))
+    lee, = plt.plot(*ee_coords(0))
     plt.axis('equal')
-    plt.show()
+    plt.xlabel('x(m)');plt.ylabel('y(m)');plt.title('Trajectory')
+    # controls
+    plt.subplot(1,2,2)
+    lsctrl = plt.plot(np.arange(0,Tf,dt), torques)
+    plt.xlabel('time (s)');plt.ylabel('Cable tension (N)');plt.title('Control Inputs')
 
 
 if __name__ == '__main__':
-    main()
+    cProfile.run('vals = main()')
+    plot(*vals)
diff --git a/gtdynamics/cablerobot/src/gerry02_traj_tracking.ipynb b/gtdynamics/cablerobot/src/gerry02_traj_tracking.ipynb
new file mode 100644
index 00000000..0701f6bb
--- /dev/null
+++ b/gtdynamics/cablerobot/src/gerry02_traj_tracking.ipynb
@@ -0,0 +1,133 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "impressive-plumbing",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "%load_ext autoreload\n",
+    "%autoreload 2\n",
+    "import numpy as np\n",
+    "import matplotlib.pyplot as plt\n",
+    "from IPython.display import Image, display\n",
+    "%aimport -np\n",
+    "%aimport -plt\n",
+    "%aimport -display\n",
+    "\n",
+    "from draw_cdpr import plot_trajectory\n",
+    "from draw_controller import draw_controller_anim\n",
+    "from gerry02_traj_tracking import main, plot, save_controller"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "fifteen-crawford",
+   "metadata": {
+    "scrolled": true
+   },
+   "outputs": [],
+   "source": [
+    "# cdpr, controller, result, N, dt, pdes = main(Q=np.ones(6)*1e3, R=np.ones(1)*1e-3, debug=False)\n",
+    "# cdpr, controller, result, N, dt, pdes = main(Q=np.ones(6)*1e3, R=np.ones(1)*1e-3, N0=9700, dN=10, debug=False)\n",
+    "# cdpr, controller, result, N, dt, pdes = main(Q=np.ones(6)*1e3, R=np.ones(1)*1e-3, N0=9700, debug=False)\n",
+    "cdpr, controller, result, N, dt, pdes = main(fname='/Users/gerry/Downloads/104494979_svg_output_3mps2.h', Q=np.ones(6)*1e1, R=np.ones(1)*1e-3, dN=1, N=None, debug=False)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "generic-calibration",
+   "metadata": {
+    "scrolled": true
+   },
+   "outputs": [],
+   "source": [
+    "# Plot Trajectory\n",
+    "anim = plot_trajectory(cdpr, result, dt*N, dt, N, pdes, step=10);\n",
+    "plt.suptitle('Motor inertia: {:.1e} kg.m$^2$'.format(cdpr.params.winch_params.inertia_));"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "daily-visiting",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Animate Trajectory\n",
+    "import matplotlib\n",
+    "matplotlib.rcParams['animation.embed_limit'] = 25.0\n",
+    "from IPython.display import HTML\n",
+    "HTML(anim.to_html5_video())"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "joint-batch",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Plot Controller Gains\n",
+    "anim2 = draw_controller_anim(cdpr, controller, result, N, step=50);"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "expired-inclusion",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Animate Controller Gains\n",
+    "from IPython.display import HTML\n",
+    "HTML(anim2.to_html5_video())"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "other-calvin",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "save_controller('/Users/gerry/Downloads/104494979_svg_output_3mps2_controller_full.h', controller)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "developmental-official",
+   "metadata": {},
+   "outputs": [],
+   "source": []
+  }
+ ],
+ "metadata": {
+  "interpreter": {
+   "hash": "402f513bd64bb05ccdfd11315d0c88453571d1d1d73db48414a1b2a41f771ebc"
+  },
+  "kernelspec": {
+   "display_name": "Python 3.9.6 ('base')",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.9.6"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 5
+}
diff --git a/gtdynamics/cablerobot/src/gerry02_traj_tracking.py b/gtdynamics/cablerobot/src/gerry02_traj_tracking.py
new file mode 100644
index 00000000..e56ec281
--- /dev/null
+++ b/gtdynamics/cablerobot/src/gerry02_traj_tracking.py
@@ -0,0 +1,167 @@
+"""
+GTDynamics Copyright 2021, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+See LICENSE for the license information
+
+@file  gerry02_traj_tracking.py
+@brief Computes the linearized feedforward controllers for tracking a trajectory.
+@author Frank Dellaert
+@author Gerry Chen
+
+Given a .h file containing the trajectory, we compute the time-varying linearized iLQR gains &
+feedforward terms and write them out to a new .h file.
+"""
+
+import gtdynamics as gtd
+import gtsam
+import matplotlib.pyplot as plt
+import numpy as np
+from gtsam import Pose3, Rot3
+
+from cdpr_planar import Cdpr, CdprParams
+from cdpr_controller_ilqr import CdprControllerIlqr
+from cdpr_planar_sim import CdprSimulator
+from paint_parse import ParseFile, writeControls
+from draw_cdpr import plot_trajectory
+from draw_controller import draw_controller_anim
+
+import cProfile
+from pstats import SortKey
+
+FRAME_WIDTH, FRAME_HEIGHT = 2.92, 2.32
+EE_WIDTH, EE_HEIGHT = 0.15, 0.30
+
+def create_cdpr():
+    """Creates a cdpr with parameters matching the real CDPR in lab.
+    """
+    # cdpr object
+    aw, ah = FRAME_WIDTH, FRAME_HEIGHT
+    bw, bh = EE_WIDTH, EE_HEIGHT
+    params = CdprParams()
+    params.mass = 0.5
+    params.gravity = np.array([0, 0, -9.8]).reshape((3, 1))
+    params.a_locs = np.array([[aw, 0, 0], [aw, 0, ah], [0, 0, ah], [0, 0, 0]])
+    params.b_locs = np.array([[bw, 0., -bh], [bw, 0., bh], [-bw, 0., bh], [-bw, 0, -bh]]) / 2
+    params.b_locs = params.b_locs - [0, 0, bh * 0.4]
+    params.winch_params.inertia_ = 9.26e-5 * 890 / 420 # https://bit.ly/3sOF2Wt
+    params.winch_params.radius_ = 0.0127
+    return Cdpr(params)
+
+def print_data(isPaints, colorinds, colorpalette, traj, N=100):
+    """Prints out the paint trajectory for sanity check"""
+    def paintString(isPaint, colori):
+        return '{:d}, {:d}, {:d}'.format(*colorpalette[colori]) if isPaint else 'paint off'
+    i = 0
+    print(traj.shape)
+    for isPaint, colori, point in zip(isPaints, colorinds, traj):
+        print('{:5.2f}, {:5.2f}\t-\t{}'.format(*point, paintString(isPaint, colori)))
+        if i > N:
+            return
+        i += 1
+
+def xy2Pose3(traj):
+    """Converts an Nx2 numpy array into a list of gtsam.Pose3 objects"""
+    des_T = []
+    for xy in traj:
+        des_T.append(gtsam.Pose3(gtsam.Rot3(), (xy[0], 0, xy[1])))
+    return des_T
+
+def main(fname='data/ATL.h',
+         debug=False,
+         Q=np.ones(6) * 1e2,
+         R=np.ones(1) * 1e-2,
+         N0=0,
+         N=None,
+         dN=1,
+         speed_multiplier=1,
+         progress=None):
+    """Runs a simulation of the iLQR controller trying to execute a predefined trajectory.
+
+    Args:
+        fname (str, optional): The trajectory filename. Defaults to 'data/ATL.h'.
+        debug (bool, optional): Whether to print debug information. Defaults to False.
+        Q (np.ndarray, optional): Vector of weights to apply to the state objectives.  The real
+        weight matrix will be diag(Q). Defaults to np.ones(6)*1e2.
+        R (np.ndarray, optional): Vector of weights to apply to the control costs.  The real weight
+        matrix will be diag(R). Defaults to np.ones(1)*1e-2.
+        N0 (int, optional): The initial timestep of the trajectory to start at, since running the
+        full trajectory is very time consuming. Defaults to 0.
+        N (int, optional): The number of timesteps of the trajectory to run, since running the full
+        trajectory is very time consuming. Defaults to 500.
+        dN (int, optional): Skips some timesteps from the input trajectory, to make things faster.
+        The controller and simulation both will only use each dN'th time step.  Defaults to 1.
+
+    Returns:
+        tuple(Cdpr, CdprControllerIlqr, gtsam.Values, int, float, list[gtsam.Pose3]): The relevant
+        output data including:
+            - cdpr: the cable robot object
+            - controller: the controller
+            - result: the Values object containing the full state and controls of the robot in
+            open-loop
+            - N: the number of time steps (equal to N passed in)
+            - dt: the time step size
+            - des_T: the desired poses
+    """
+    # cdpr object
+    cdpr = create_cdpr()
+
+    # import data
+    isPaints, colorinds, colorpalette, traj = ParseFile(fname)
+    N = len(traj) - N0 - 1 if N is None else N
+    dt = (0.01 / speed_multiplier
+          ) * dN  # this is a hardcoded constant.  TODO(gerry): include this in the .h file.
+    N = int(N/dN)  # scale time by dN
+    N0 = int(N0/dN)
+    width, _, height = cdpr.params.a_locs[1] - cdpr.params.a_locs[3]  # rescale trajectory to be
+    traj = (traj - [width/2, height/2]) * 0.85 + [width/2, height/2]  # smaller
+    traj = traj[::dN, :]
+    if debug:
+        print_data(isPaints, colorinds, colorpalette, traj, N=100)
+    # only simulate a subset of the trajectory, since the trajectory is very large
+    des_T = xy2Pose3(traj[N0:N0+N, :])
+
+
+    # initial configuration
+    x0 = gtsam.Values()
+    # gtd.InsertPose(x0, cdpr.ee_id(), 0, gtsam.Pose3(gtsam.Rot3(), (1.5, 0, 1.5)))
+    gtd.InsertPose(x0, cdpr.ee_id(), 0, des_T[0])
+    gtd.InsertTwist(x0, cdpr.ee_id(), 0, (0, 0, 0, 0, 0, 0))
+
+    # controller
+    controller = CdprControllerIlqr(cdpr, x0, des_T, dt, Q, R, progress=progress)
+    # feedforward control
+    xff = np.zeros((N, 2))
+    uff = np.zeros((N, 4))
+    for t in range(N):
+        xff[t, :] = gtd.Pose(controller.result, cdpr.ee_id(), t).translation()[[0, 2]]
+        uff[t, :] = [gtd.Torque(controller.result, ji, t) for ji in range(4)]
+    if debug:
+        print(xff)
+        print(uff)
+
+    # simulate
+    sim = CdprSimulator(cdpr, x0, controller, dt=dt)
+    result = sim.run(N=N)
+    if debug:
+        print(result)
+
+    return cdpr, controller, result, N, dt, des_T
+
+def plot(cdpr, controller, result, N, dt, des_T):
+    """Plots the results"""
+    plot_trajectory(cdpr, result, dt*N, dt, N, des_T, step=1)
+
+def save_controller(fname, controller):
+    writeControls(fname, controller.gains_ff)
+
+if __name__ == '__main__':
+    # cProfile.run('results = main(N=100)', sort=SortKey.TIME)
+    # results = main(fname='data/ATL_filled.h', Q=np.ones(6)*1e1, R=np.ones(1)*1e-3, dN=1, debug=False, speed_multiplier=1)
+    # plot(*results)
+    # plt.show()
+    # save_controller('data/tmp.h', results[1])
+    results = main(fname='/Users/gerry/Downloads/105074147_svg_output_3mps2.h', Q=np.ones(6)*1e1, R=np.ones(1)*1e-3, dN=1, N=1000, debug=False, speed_multiplier=1)
+    save_controller('/Users/gerry/Downloads/105074147_svg_output_3mps2_controller_10s.h', results[1])
+    plot(*results)
+    plt.show()
diff --git a/gtdynamics/cablerobot/src/gerry03_stroke_tracking.ipynb b/gtdynamics/cablerobot/src/gerry03_stroke_tracking.ipynb
new file mode 100644
index 00000000..7a85e0d6
--- /dev/null
+++ b/gtdynamics/cablerobot/src/gerry03_stroke_tracking.ipynb
@@ -0,0 +1,136 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "08dd84c0",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "%load_ext autoreload\n",
+    "%autoreload 2\n",
+    "import time\n",
+    "import numpy as np\n",
+    "import matplotlib.pyplot as plt\n",
+    "from IPython.display import Image, display\n",
+    "%aimport -np\n",
+    "%aimport -plt\n",
+    "%aimport -display\n",
+    "\n",
+    "from draw_cdpr import plot_trajectory\n",
+    "from draw_controller import draw_controller_anim\n",
+    "import gerry02_traj_tracking\n",
+    "import gerry03_stroke_tracking"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "d191f422",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "def timed(func, *args, **kwargs):\n",
+    "    tstart = time.time()\n",
+    "    rets = func(*args, **kwargs)\n",
+    "    print(\"Time elapsed: \", time.time() - tstart)\n",
+    "    return rets"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "cb4afa56",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "fname='data/ATL_filled.h'"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "72dcedc4",
+   "metadata": {
+    "scrolled": true
+   },
+   "outputs": [],
+   "source": [
+    "cdpr, controller, result, des_poses, dt = timed(gerry03_stroke_tracking.main, fname=fname, Q=np.ones(6)*1e1, R=np.ones(1)*1e-3, speed_multiplier=1)\n"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "0151948c",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "cdpr_2, controller_2, result_2, N, dt, pdes = timed(gerry02_traj_tracking.main, fname=fname, Q=np.ones(6)*1e1, R=np.ones(1)*1e-3, dN=1, debug=False, speed_multiplier=1)\n"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "095ea4e5",
+   "metadata": {
+    "scrolled": false
+   },
+   "outputs": [],
+   "source": [
+    "gerry02_traj_tracking.plot(cdpr, controller, result, len(des_poses), dt, des_poses)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "d4947413",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "gerry02_traj_tracking.plot(cdpr_2, controller_2, result_2, len(pdes), dt, pdes)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "6c0951da",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "gerry03_stroke_tracking.save_controller('data/test_params2.h', all_controllers)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "0feaf679",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "cProfile.run('_ = gerry03_stroke_tracking.main(fname=fname)', sort=SortKey.TIME)"
+   ]
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Python 3 (ipykernel)",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.9.6"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 5
+}
diff --git a/gtdynamics/cablerobot/src/gerry03_stroke_tracking.py b/gtdynamics/cablerobot/src/gerry03_stroke_tracking.py
new file mode 100644
index 00000000..a3110cdd
--- /dev/null
+++ b/gtdynamics/cablerobot/src/gerry03_stroke_tracking.py
@@ -0,0 +1,206 @@
+"""
+GTDynamics Copyright 2021, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+See LICENSE for the license information
+
+@file  gerry03_stroke_tracking.py
+@brief Computes the controllers stroke-by-stroke for tracking a trajectory.
+@author Frank Dellaert
+@author Gerry Chen
+"""
+
+import gtdynamics as gtd
+import gtsam
+import matplotlib.pyplot as plt
+import numpy as np
+from gtsam import Pose3, Rot3
+
+from cdpr_planar import Cdpr, CdprParams
+from cdpr_controller_ilqr import CdprControllerIlqr
+from cdpr_planar_sim import CdprSimulator
+from paint_parse import ParseFile, writeControls
+from draw_cdpr import plot_trajectory
+from draw_controller import draw_controller_anim
+import utils
+import gerry02_traj_tracking
+
+import cProfile
+from pstats import SortKey
+import tqdm
+
+DT = 0.01  # hardcoded constant.  TODO(gerry): include in .h file.
+
+
+def preprocessTraj(cdpr: Cdpr, is_paint_on: list[bool], traj: np.ndarray, dN: int):
+    """Preprocesses the trajectory to (1) rescale the trajectory, and (2) take every dN'th index.
+    `traj` is an Nx2 array.
+    """
+    # rescale trajectory to be smaller
+    width, _, height = cdpr.params.a_locs[1] - cdpr.params.a_locs[3]  # rescale trajectory to be
+    traj = (traj - [width / 2, height / 2]) * 0.85 + [width / 2, height / 2]  # smaller
+    # extract the part of the trajectory we care about
+    return np.array(is_paint_on)[::dN], traj[::dN, :]
+
+
+def splitTrajByStroke(is_paint_on: list[bool], all_des_poses: list[gtsam.Pose3]):
+    """Splits up a trajectory according to paint changes
+    """
+    prev_index = 0
+    for transition_index in np.argwhere(np.diff(is_paint_on)) + 1:
+        yield all_des_poses[prev_index:transition_index[0]]
+        prev_index = transition_index[0]
+    yield all_des_poses[prev_index:]
+
+
+def combineResults(all_results: list[gtsam.Values]) -> gtsam.Values:
+    """Combines values objects, taking care to remove duplicates
+    """
+    combined_results = gtsam.Values()
+    combined_keys = set()
+    for results in all_results:
+        # remove duplicates
+        new_keys = set(results.keys())
+        for key in (combined_keys & new_keys):
+            results.erase(key)
+        combined_keys |= new_keys
+        # insert
+        combined_results.insert(results)
+    return combined_results
+
+
+def optimizeStroke(cdpr: Cdpr,
+                   stroke: list[gtsam.Pose3],
+                   x0: gtsam.Pose3,
+                   v0,
+                   Q,
+                   R,
+                   dt: float,
+                   x_guess: gtsam.Values = None):
+    """Computes the optimal controller for a single stroke
+    """
+    N = len(stroke)
+
+    # initial configuration
+    X_init = gtsam.Values()
+    gtd.InsertPose(X_init, cdpr.ee_id(), 0, x0)
+    gtd.InsertTwist(X_init, cdpr.ee_id(), 0, v0)
+
+    # controller
+    controller = CdprControllerIlqr(cdpr, X_init, stroke, dt, Q, R, x_guess=x_guess)
+    # feedforward control
+    xff = np.zeros((N, 2))
+    uff = np.zeros((N, 4))
+    for t in range(N):
+        xff[t, :] = gtd.Pose(controller.result, cdpr.ee_id(), t).translation()[[0, 2]]
+        uff[t, :] = [gtd.TorqueDouble(controller.result, ji, t) for ji in range(4)]
+
+    # simulate
+    sim = CdprSimulator(cdpr, X_init, controller, dt=dt)
+    result = sim.run(N=N)
+    xf = gtd.Pose(result, cdpr.ee_id(), N)
+    vf = gtd.Twist(result, cdpr.ee_id(), N)
+
+    return xf, vf, controller, result
+
+
+def optimizeStrokes(cdpr: Cdpr, strokes: list[list[gtsam.Pose3]], Q, R, dt: float):
+    """Finds the optimal controllers for a set of strokes
+    """
+    all_controllers = []
+    all_results = []
+    cur_x, cur_v = strokes[0][0], (0, 0, 0, 0, 0, 0)
+    k = 0
+
+    # TODO(Gerry): figure out multithreading.  Tried concurrent.futures.ThreadPoolExecutor but it
+    # didn't help at all due to GIL.  Couldn't get ProcessPoolExecutor to work - it wouldn't
+    # actually execute the thread.
+    for stroke in tqdm.tqdm(strokes):  # tqdm is iteration timer
+        cur_x, cur_v, controller, result = optimizeStroke(cdpr, stroke, cur_x, cur_v, Q, R, dt)
+
+        # shift timesteps
+        new = gtsam.Values()
+        for key in result.keys():
+            utils.UpdateFromValues(result, new, gtd.DynamicsSymbol(key), shift_time_by=k)
+        result = new
+
+        # save and update
+        all_controllers.append(controller)
+        all_results.append(result)
+        k += len(stroke)
+
+    return cdpr, all_controllers, all_results, strokes, dt
+
+
+def main(fname: str = 'data/ATL_filled.h',
+         Q=np.ones(6) * 1e2,
+         R=np.ones(1) * 1e-2,
+         dN: int = 1,
+         speed_multiplier: float = 1):
+    """Runs a simulation of the iLQR controller trying to execute a predefined trajectory.
+
+    Args:
+        fname (str, optional): The trajectory filename. Defaults to 'data/ATL_filled.h'.
+        Q (np.ndarray, optional): Vector of weights to apply to the state objectives.  The real
+        weight matrix will be diag(Q). Defaults to np.ones(6)*1e2.
+        R (np.ndarray, optional): Vector of weights to apply to the control costs.  The real weight
+        matrix will be diag(R). Defaults to np.ones(1)*1e-2.
+        dN (int, optional): Skips some timesteps from the input trajectory, to make things faster.
+        The controller and simulation both will only use each dN'th time step.  Defaults to 1.
+        speed_multiplier (float, optional): Makes the entire trajectory faster or slower
+
+    Returns:
+        tuple(Cdpr, CdprControllerIlqr, gtsam.Values, list[gtsam.Pose3], float): The relevant
+        output data including:
+            - cdpr: the cable robot object
+            - controller: the controller
+            - result: the Values object containing the full state and controls of the robot in
+            open-loop
+            - des_T: the desired poses
+            - dt: the time step size
+    """
+    cdpr = gerry02_traj_tracking.create_cdpr()
+    dt = (DT / speed_multiplier) * dN
+
+    # import data
+    is_paint_on, _, _, all_trajs = ParseFile(fname)
+    is_paint_on, all_trajs = preprocessTraj(cdpr, is_paint_on, all_trajs, dN)
+    all_des_poses = gerry02_traj_tracking.xy2Pose3(all_trajs)
+
+    # run per-stroke
+    print("Running per-stroke optimization...")
+    strokes = list(splitTrajByStroke(is_paint_on, all_des_poses))
+    cdpr, all_controllers, all_results, strokes, dt = optimizeStrokes(cdpr, strokes, Q, R, dt)
+
+    # combine and run together to smooth out
+    print("Combining per-stroke results and running one final optimization...")
+    x_guess = combineResults(all_results)
+    _, _, controller, result = optimizeStroke(cdpr,
+                                              all_des_poses,
+                                              all_des_poses[0], (0, 0, 0, 0, 0, 0),
+                                              Q,
+                                              R,
+                                              dt,
+                                              x_guess=x_guess)
+
+    print("Done.")
+    return cdpr, controller, result, all_des_poses, dt
+
+
+def plot(cdpr, all_results, strokes, dt):
+    """Plots the results"""
+    N = sum(len(s) for s in strokes[:len(all_results)])
+    plot_trajectory(cdpr, combineResults(all_results), dt * N, dt, N, sum(strokes, []), step=1)
+
+
+def save_controller(fname, all_controllers):
+    gains_ff = sum((controller.gains_ff for controller in all_controllers), [])
+    writeControls(fname, gains_ff)
+
+
+if __name__ == '__main__':
+    # cProfile.run('cdpr, controller, result, des_poses, dt = main(dN=10)', sort=SortKey.TIME)
+    cdpr, controller, result, des_poses, dt = main(dN=1)
+    gerry02_traj_tracking.plot(cdpr, controller, result, len(des_poses), dt, des_poses)
+    plt.show()
+    gerry02_traj_tracking.save_controller('data/tmp.h', controller)
diff --git a/gtdynamics/cablerobot/src/gerry04_lqr_style.py b/gtdynamics/cablerobot/src/gerry04_lqr_style.py
new file mode 100644
index 00000000..83e755a4
--- /dev/null
+++ b/gtdynamics/cablerobot/src/gerry04_lqr_style.py
@@ -0,0 +1,35 @@
+import gerry02_traj_tracking as gerry02
+import draw_cdpr
+
+import numpy as np
+import gtdynamics as gtd
+import matplotlib.pyplot as plt
+
+def plot(ax, cdpr, controller, result, N, dt, des_T):
+    """Plots the results"""
+    # plot_trajectory(cdpr, result, dt*N, dt, N, des_T, step=1)
+
+    act_T = [gtd.Pose(result, cdpr.ee_id(), k) for k in range(N+1)]
+    act_xy = np.array([draw_cdpr.pose32xy(pose) for pose in act_T]).T
+    des_xy = np.array([draw_cdpr.pose32xy(pose) for pose in des_T]).T
+
+    # ls = draw_cdpr.draw_cdpr(ax, cdpr, gtd.Pose(result, cdpr.ee_id(), 0))
+    # ls[1].remove()
+    # for l in ls[2]:
+    #     l.remove()
+    # for l in ls[1:]:
+    #     l.remove()
+    ls = draw_cdpr.draw_traj(ax, cdpr, des_xy, act_xy)
+    # plt.legend(ls, [r'$\bf{x}_d$', r'$\bf{x}_{ff}$'])
+    # plt.legend(loc='upper right')
+
+
+if __name__ == '__main__':
+    # cProfile.run('results = main(N=100)', sort=SortKey.TIME)
+    # fname = 'data/ATL_filled_output_7mps2.h'
+    # fname = 'data/concentric_diamonds2_output_2mps_20mps2.h'
+    fname = 'data/ATL_outline_output.h'
+    Q, R = 1e-1, 1e-3
+    results = gerry02.main(fname=fname, Q=np.ones(6)*Q, R=np.ones(1)*R, dN=1, debug=False, speed_multiplier=1)
+    plot(*results)
+    plt.show()
diff --git a/gtdynamics/cablerobot/src/paint_parse.py b/gtdynamics/cablerobot/src/paint_parse.py
new file mode 100644
index 00000000..83e41e43
--- /dev/null
+++ b/gtdynamics/cablerobot/src/paint_parse.py
@@ -0,0 +1,86 @@
+"""
+GTDynamics Copyright 2021, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+See LICENSE for the license information
+
+@file  paint_parse.py
+@brief Parses a .h file containing the paint trajectory.
+@author Frank Dellaert
+@author Gerry Chen
+"""
+
+import numpy as np
+import re
+
+UINTEXPR = '([0-9]*?)'
+FLOATEXPR = '(-?[0-9]*?\.?[0-9]*?)'
+
+def ParseFile(fname):
+    with open(fname) as file:
+        f = filter(lambda line: not line.startswith('//'), file)
+        # for _ in range(4):
+        #     next(f)
+
+        # painton
+        assert next(f) == 'const bool PROGMEM painton[] = {\n', "variable `painton` not found: \n{:}\n{:}".format()
+        paintons = [bool(int(e)) for e in next(f).strip().split(',')]
+        assert next(f) == '};\n', "parse error on variable `painton`"
+
+        # colorinds
+        assert next(f) == 'const uint8_t PROGMEM colorinds[] = {\n', "variable `colorinds` not found"
+        colorinds = [int(e) for e in next(f).strip().split(',')]
+        assert next(f) == '};\n', "parse error on variable `colorinds`"
+
+        # colorpalette
+        assert next(f) == 'const uint8_t PROGMEM colorpalette[][3] = {\n', "variable `colorpalette` not found"
+        colors = []
+        while True:
+            matches = re.search(f'\\{{{UINTEXPR}, {UINTEXPR}, {UINTEXPR}\\}},?', next(f))
+            if matches is None:
+                break  # next(f) was '};\n'
+            colors.append([int(g) for g in matches.groups()])
+        colors = np.array(colors)
+
+        # trajectory
+        assert next(f) == 'const float PROGMEM traj[][2] = {\n', "variable `traj` not found"
+        traj = []
+        while True:
+            matches = re.search(f'\{{{FLOATEXPR},{FLOATEXPR}}},?',
+                                re.sub('\s', '', next(f)))  # remove any whitespace
+            if matches is None:
+                break  # next(f) was '};\n'
+            traj.append([float(g) for g in matches.groups()])
+        traj = np.array(traj)
+
+        # EOF
+        try:
+            next(f)
+        except StopIteration:
+            return paintons, colorinds, colors, traj
+        assert False, 'End of file expected'
+
+def writeControls(fname, gains_ff):
+    with open(fname, 'w') as f:
+        Ks, uffs, vffs, xffs = zip(*gains_ff)
+        f.write('// u = K * ([v;x]-[vff;xff]) + uff\n')
+        f.write('const float PROGMEM xffs[][2] = {\n')
+        for xff in xffs:
+            f.write('\t{{{:f}, {:f}}},\n'.format(*xff.translation()[[0, 2]]))
+        f.write('};\n')
+        f.write('const float PROGMEM vffs[][2] = {\n')
+        for vff in vffs:
+            f.write('\t{{{:f}, {:f}}},\n'.format(*vff[[3, 5]]))
+        f.write('};\n')
+        f.write('const float PROGMEM uffs[][4] = {\n')
+        for uff in uffs:
+            f.write('\t{{{:f}, {:f}, {:f}, {:f}}},\n'.format(*uff))
+        f.write('};\n')
+        f.write('// vx, vy, x, y\n')
+        f.write('const float PROGMEM Ks[][4][4] = {\n\t')
+        for K in Ks:
+            f.write('{\n')
+            for Krow in K:
+                f.write('\t {{{:f}, {:f}, {:f}, {:f}}},\n'.format(*Krow[[3,5,9,11]]))
+            f.write('\t},')
+        f.write('\n};\n')
diff --git a/gtdynamics/cablerobot/src/test_cdpr_controller_ilqr.py b/gtdynamics/cablerobot/src/test_cdpr_controller_ilqr.py
new file mode 100644
index 00000000..7b6fec86
--- /dev/null
+++ b/gtdynamics/cablerobot/src/test_cdpr_controller_ilqr.py
@@ -0,0 +1,141 @@
+"""
+GTDynamics Copyright 2021, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+See LICENSE for the license information
+
+@file  test_cdpr_controller_ilqr.py
+@brief Unit tests for CDPR.
+@author Frank Dellaert
+@author Gerry Chen
+"""
+
+import unittest
+
+import gtdynamics as gtd
+import gtsam
+from gtsam import Pose3, Rot3
+import numpy as np
+from cdpr_planar import Cdpr
+from cdpr_controller_ilqr import CdprControllerIlqr
+from cdpr_planar_sim import CdprSimulator
+from gtsam.utils.test_case import GtsamTestCase
+
+class TestCdprControllerIlqr(GtsamTestCase):
+    def testTrajFollow(self):
+        """Tests trajectory tracking controller
+        """
+        cdpr = Cdpr()
+
+        x0 = gtsam.Values()
+        gtd.InsertPose(x0, cdpr.ee_id(), 0, Pose3(Rot3(), (1.5, 0, 1.5)))
+        gtd.InsertTwist(x0, cdpr.ee_id(), 0, np.zeros(6))
+
+        x_des = [Pose3(Rot3(), (1.5+k/20.0, 0, 1.5)) for k in range(9)]
+        x_des = x_des[0:1] + x_des
+        controller = CdprControllerIlqr(cdpr, x0=x0, pdes=x_des, dt=0.1)
+
+        sim = CdprSimulator(cdpr, x0, controller, dt=0.1)
+        result = sim.run(N=10)
+        pAct = [gtd.Pose(result, cdpr.ee_id(), k) for k in range(10)]
+
+        if False:
+            print()
+            for k, (des, act) in enumerate(zip(x_des, pAct)):
+                print(('k: {:d}  --  des: {:.3f}, {:.3f}, {:.3f}  --  act: {:.3f}, {:.3f}, {:.3f}' +
+                       '  --  u: {:.3e},   {:.3e},   {:.3e},   {:.3e}').format(
+                           k, *des.translation(), *act.translation(),
+                           *[gtd.TorqueDouble(result, ji, k) for ji in range(4)]))
+
+        for k, (des, act) in enumerate(zip(x_des, pAct)):
+            self.gtsamAssertEquals(des, act, tol=1e-3)
+
+    def testGainsNearConstrained(self):
+        """Tests locally linear, time-varying feedback gains
+        """
+        cdpr = Cdpr()
+        cdpr.params.tmin = -1
+        cdpr.params.tmax = 1
+        cdpr.params.collocation_mode = 0
+        dt = 0.01
+
+        x0 = gtsam.Values()
+        gtd.InsertPose(x0, cdpr.ee_id(), 0, Pose3(Rot3(), (1.5, 0, 1.5)))
+        gtd.InsertTwist(x0, cdpr.ee_id(), 0, np.zeros(6))
+        x_des = [Pose3(Rot3(), (1.5, 0, 1.5)),
+                 Pose3(Rot3(), (1.5, 0, 1.5)),
+                 Pose3(Rot3(), (1.5, 0, 1.5))]  # don't move
+        controller = CdprControllerIlqr(cdpr, x0=x0, pdes=x_des, dt=dt, Q=np.ones(6)*1e12, R=np.array([1.]))
+
+        # notation: x_K_y means xstar = K * dy
+        #           where xstar is optimal x and dy is (desired_y - actual_y)
+        #           note: when multiplying gain matrices together, be mindful of negative signs
+        # position gain (Kp) - time 0, cable 0, pose gain
+        actual_0c0_K_0x = controller.gains_ff[0][0][0:1, 6:]
+        expected_1v_K_0x = np.diag([0, -1, 0, -1, 0, -1]) / dt  # v at t=1 in response to x at t=0
+        expected_0c0_K_1v = np.array([0, 1e9, 0,
+                                      -1 / np.sqrt(2) / 2, 0, 1 / np.sqrt(2) / 2]).reshape((1, -1)) * \
+                            cdpr.params.mass / dt  # cable 0 tension at t=0 in response to v at t=1
+        expected_0c0_K_0x = -expected_0c0_K_1v @ expected_1v_K_0x
+        self.gtsamAssertEquals(expected_0c0_K_0x[:, 3:], actual_0c0_K_0x[:, 3:], tol=1/dt)
+
+        # velocity gain (Kd) - time 0, cable 0, twist gain
+        actual_0c0_K_0v = controller.gains_ff[0][0][0:1, :6]
+        expected_0c0_K_0v = 2 * expected_0c0_K_1v
+        self.gtsamAssertEquals(actual_0c0_K_0v[:, 3:], expected_0c0_K_0v[:, 3:], tol=dt)
+
+    def testGainsLongHorizon(self):
+        """Approximate infinite horizon problem by doing very long horizon.  Compare with matlab.
+        """
+        cdpr = Cdpr()
+        cdpr.params.tmin = -1
+        cdpr.params.tmax = 1
+        dt = 0.01
+
+        x0 = gtsam.Values()
+        gtd.InsertPose(x0, cdpr.ee_id(), 0, Pose3(Rot3(), (1.5, 0, 1.5)))
+        gtd.InsertTwist(x0, cdpr.ee_id(), 0, np.zeros(6))
+        x_des = [Pose3(Rot3(), (1.5, 0, 1.5)) for t in range(1000)]
+        controller = CdprControllerIlqr(cdpr, x0=x0, pdes=x_des, dt=dt,
+                                        Q=np.array([0,1,0,2,0,3]), R=np.array([0.12]))
+        ''' matlab code:
+        dt = 0.01;
+        m = 1;
+
+        A = [zeros(2), eye(2);...
+             zeros(2), zeros(2)];
+        B_F = [zeros(2);...
+               eye(2) / m];
+        F_T = [1, 1, -1, -1;...
+               -1, 1, 1, -1] / sqrt(2);
+        B = B_F * F_T;
+        Q = diag([2, 3, 0, 0]);
+        R = diag(repmat([0.12], 4, 1));
+        [K, ~, ~] = lqrd(A, B, Q, R, dt)
+        '''
+        expected_0c_K = -np.array([ # minus sign because matlab uses u=-Kx convention
+            #  vx      vy       x       y
+            [ 2.0069, -2.4534,  1.1912, -1.3171],
+            [ 2.0069,  2.4534,  1.1912,  1.3171],
+            [-2.0069,  2.4534, -1.1912,  1.3171],
+            [-2.0069, -2.4534, -1.1912, -1.3171]
+        ])
+        actual_0c_K = controller.gains_ff[0][0]
+        actual_0c_K = actual_0c_K[:, [9, 11, 3, 5]]
+        self.gtsamAssertEquals(expected_0c_K, actual_0c_K, 2e-2)
+
+    def testRun(self):
+        """Tests that controller will not "compile" (aka run without errors)
+        """
+        cdpr = Cdpr()
+        dt = 0.01
+
+        x0 = gtsam.Values()
+        gtd.InsertPose(x0, cdpr.ee_id(), 0, Pose3(Rot3(), (1.5, 0, 1.5)))
+        gtd.InsertTwist(x0, cdpr.ee_id(), 0, np.zeros(6))
+        x_des = [Pose3(Rot3(), (1.5, 0, 1.5))]
+        controller = CdprControllerIlqr(cdpr, x0=x0, pdes=x_des, dt=dt)
+
+
+if __name__ == "__main__":
+    unittest.main()
diff --git a/gtdynamics/cablerobot/src/test_cdpr_planar_controller.py b/gtdynamics/cablerobot/src/test_cdpr_controller_tension_dist.py
similarity index 83%
rename from gtdynamics/cablerobot/src/test_cdpr_planar_controller.py
rename to gtdynamics/cablerobot/src/test_cdpr_controller_tension_dist.py
index 7a0e55de..7780c10b 100644
--- a/gtdynamics/cablerobot/src/test_cdpr_planar_controller.py
+++ b/gtdynamics/cablerobot/src/test_cdpr_controller_tension_dist.py
@@ -4,7 +4,7 @@
 All Rights Reserved
 See LICENSE for the license information
 
-@file  test_cdpr_planar_controller.py
+@file  test_cdpr_controller_tension_dist.py
 @brief Unit tests for CDPR.
 @author Frank Dellaert
 @author Gerry Chen
@@ -17,16 +17,16 @@
 from gtsam import Pose3, Rot3
 import numpy as np
 from cdpr_planar import Cdpr
-from cdpr_planar_controller import CdprController
+from cdpr_controller_tension_dist import CdprControllerTensionDist as CdprController
 from cdpr_planar_sim import CdprSimulator
 from gtsam.utils.test_case import GtsamTestCase
 
-class TestCdprPlanar(GtsamTestCase):
+class TestCdprControllerTensionDist(GtsamTestCase):
     def testTrajFollow(self):
         """Tests trajectory tracking controller
         """
         cdpr = Cdpr()
-
+        cdpr.params.collocation_mode = 1
         x0 = gtsam.Values()
         gtd.InsertPose(x0, cdpr.ee_id(), 0, Pose3(Rot3(), (1.5, 0, 1.5)))
         gtd.InsertTwist(x0, cdpr.ee_id(), 0, np.zeros(6))
@@ -36,10 +36,11 @@ def testTrajFollow(self):
         controller = CdprController(cdpr, x0=x0, pdes=x_des, dt=0.1)
 
         sim = CdprSimulator(cdpr, x0, controller, dt=0.1)
-        result = sim.run(N=10)
+        result = sim.run(N=9)
         pAct = [gtd.Pose(result, cdpr.ee_id(), k) for k in range(10)]
 
         if False:
+            [gtd.InsertTorqueDouble(result, ji, 9, np.nan) for ji in range(4)]
             print()
             for k, (des, act) in enumerate(zip(x_des, pAct)):
                 print(('k: {:d}  --  des: {:.3f}, {:.3f}, {:.3f}  --  act: {:.3f}, {:.3f}, {:.3f}' +
diff --git a/gtdynamics/cablerobot/src/test_cdpr_planar.py b/gtdynamics/cablerobot/src/test_cdpr_planar.py
index f15a97ba..d373eda8 100644
--- a/gtdynamics/cablerobot/src/test_cdpr_planar.py
+++ b/gtdynamics/cablerobot/src/test_cdpr_planar.py
@@ -17,6 +17,7 @@
 import numpy as np
 from gtsam import Pose3, Rot3
 from gtsam.utils.test_case import GtsamTestCase
+from utils import MyLMParams
 
 from cdpr_planar import Cdpr
 
@@ -55,78 +56,88 @@ def zeroValues():
         self.assertEqual(0.0, kfg.error(values))
         # try optimizing IK
         ikgraph = gtsam.NonlinearFactorGraph(kfg)
-        ikgraph.push_back(
-            cdpr.priors_ik([0], [gtd.Pose(values, cdpr.ee_id(), 0)],
-                           [gtd.Twist(values, cdpr.ee_id(), 0)]))
-        ikres = gtsam.LevenbergMarquardtOptimizer(ikgraph,
-                                                  zeroValues()).optimize()
+        ik1 = cdpr.priors_ik(ks=[0],
+                             Ts=[gtd.Pose(values, cdpr.ee_id(), 0)],
+                             Vs=[gtd.Twist(values, cdpr.ee_id(), 0)])
+        ik2 = cdpr.priors_ik(ks=[0], values=values)
+        self.gtsamAssertEquals(ik1, ik2)
+        ikgraph.push_back(ik1)
+        ikres = gtsam.LevenbergMarquardtOptimizer(ikgraph, zeroValues(), MyLMParams()).optimize()
         self.gtsamAssertEquals(ikres, values)  # should match with full sol
         # try optimizing FK
         fkgraph = gtsam.NonlinearFactorGraph(kfg)
-        fkgraph.push_back(
-            cdpr.priors_fk([0],
-                           [[gtd.JointAngle(values, ji, 0)
-                             for ji in range(4)]],
-                           [[gtd.JointVel(values, ji, 0) for ji in range(4)]]))
-        params = gtsam.LevenbergMarquardtParams()
-        params.setAbsoluteErrorTol(
-            1e-20)  # FK less sensitive so we need to decrease the tolerance
-        fkres = gtsam.LevenbergMarquardtOptimizer(fkgraph, zeroValues(),
-                                                  params).optimize()
-        self.gtsamAssertEquals(fkres, values,
-                               tol=1e-5)  # should match with full sol
+        fk1 = cdpr.priors_fk(ks=[0],
+                             ls=[[gtd.JointAngle(values, ji, 0) for ji in range(4)]],
+                             ldots=[[gtd.JointVel(values, ji, 0) for ji in range(4)]])
+        fk2 = cdpr.priors_fk(ks=[0], values=values)
+        self.gtsamAssertEquals(fk1, fk2)
+        fkgraph.push_back(fk1)
+        # FK less sensitive so we need to decrease the tolerance to 1e-20
+        fkres = gtsam.LevenbergMarquardtOptimizer(fkgraph, zeroValues(), MyLMParams(1e-20)).optimize()
+        self.gtsamAssertEquals(fkres, values, tol=1e-5)  # should match with full sol
 
     def testDynamicsInstantaneous(self):
         """Test dynamics factors: relates torque to wrench+twistaccel.  Also tests ID solving
         """
         cdpr = Cdpr()
+        cdpr.params.winch_params.inertia_ = 1.23
         dfg = cdpr.dynamics_factors(ks=[0])
         values = gtsam.Values()
-        # things needed to define kinematic
+        # things needed to define IK
         gtd.InsertPose(values, cdpr.ee_id(), 0, Pose3(Rot3(), (1.5, 0, 1.5)))
         gtd.InsertTwist(values, cdpr.ee_id(), 0, np.zeros(6))
         # things needed to define ID
-        for j, tau in zip(range(4), [1, 0, 0, 1]):
+        for j, tau in zip(range(4), [1 + 1.23, 0 - 1.23, 0 - 1.23, 1 + 1.23]):
             gtd.InsertTorque(values, j, 0, tau)
         # things needed to define FD
-        gtd.InsertTwistAccel(values, cdpr.ee_id(), 0,
-                             (0, 0, 0, 0, 0, -np.sqrt(2)))
+        for j, lddot in zip(range(4), [-1, 1, 1, -1]):
+            gtd.InsertJointAccel(values, j, 0, lddot)
         # things needed intermediaries
-        gtd.InsertWrench(values, cdpr.ee_id(), 0,
-                         [0, 0, 0, 1 / np.sqrt(2), 0, -1 / np.sqrt(2)])
-        gtd.InsertWrench(values, cdpr.ee_id(), 1, [0, 0, 0, 0, 0, 0])
-        gtd.InsertWrench(values, cdpr.ee_id(), 2, [0, 0, 0, 0, 0, 0])
-        gtd.InsertWrench(values, cdpr.ee_id(), 3,
-                         [0, 0, 0, -1 / np.sqrt(2), 0, -1 / np.sqrt(2)])
+        gtd.InsertWrench(values, cdpr.ee_id(), 0, [0,0,0,1/np.sqrt(2),0,-1/np.sqrt(2)])
+        gtd.InsertWrench(values, cdpr.ee_id(), 1, [0,0,0,0,0,0])
+        gtd.InsertWrench(values, cdpr.ee_id(), 2, [0,0,0,0,0,0])
+        gtd.InsertWrench(values, cdpr.ee_id(), 3, [0,0,0,-1/np.sqrt(2),0,-1/np.sqrt(2)])
+        gtd.InsertTwistAccel(values, cdpr.ee_id(), 0, (0, 0, 0, 0, 0, -np.sqrt(2)))
+        for ji, t in enumerate([1, 0, 0, 1]):
+            gtd.InsertTension(values, ji, 0, t)
+        for ji in range(4):
+            gtd.InsertJointVel(values, ji, 0, 0)  # for WinchFactor, ugh
         # check FD/ID is valid
         self.assertAlmostEqual(0.0, dfg.error(values), 10)
-        # build FD graph
+        # build ID graph
         dfg.push_back(
-            cdpr.priors_ik([0], [gtd.Pose(values, cdpr.ee_id(), 0)],
-                           [gtd.Twist(values, cdpr.ee_id(), 0)]))
-        dfg.push_back(
-            cdpr.priors_fd([0], [gtd.TwistAccel(values, cdpr.ee_id(), 0)]))
+            cdpr.priors_ik([0],
+                           Ts=[gtd.Pose(values, cdpr.ee_id(), 0)],
+                           Vs=[gtd.Twist(values, cdpr.ee_id(), 0)]))
+        id1 = cdpr.priors_id(ks=[0],
+                             lddotss=[[gtd.JointAccel(values, ji, 0) for ji in range(4)]])
+        id2 = cdpr.priors_id(ks=[0], values=values)
+        self.gtsamAssertEquals(id1, id2)
+        dfg.push_back(id1)
         # redundancy resolution
         dfg.push_back(
             gtd.PriorFactorDouble(
-                gtd.TorqueKey(1, 0).key(), 0.0,
-                gtsam.noiseModel.Unit.Create(1)))
+                gtd.TorqueKey(1, 0).key(), -1.23, gtsam.noiseModel.Unit.Create(1)))
         dfg.push_back(
             gtd.PriorFactorDouble(
-                gtd.TorqueKey(2, 0).key(), 0.0,
-                gtsam.noiseModel.Unit.Create(1)))
+                gtd.TorqueKey(2, 0).key(), -1.23, gtsam.noiseModel.Unit.Create(1)))
         # initialize and solve
         init = gtsam.Values(values)
         for ji in range(4):
             init.erase(gtd.TorqueKey(ji, 0).key())
             gtd.InsertTorque(init, ji, 0, -1)
-        results = gtsam.LevenbergMarquardtOptimizer(dfg, init).optimize()
-        self.gtsamAssertEquals(results, values)
+        results = gtsam.LevenbergMarquardtOptimizer(dfg, init, MyLMParams(1e-20)).optimize()
+        self.gtsamAssertEquals(values, results)
+        # check FD priors functions
+        fd1 = cdpr.priors_fd(ks=[0], torquess=[[gtd.Torque(results, ji, 0) for ji in range(4)]])
+        fd2 = cdpr.priors_fd(ks=[0], values=results)
+        self.gtsamAssertEquals(fd1, fd2)
 
     def testDynamicsCollocation(self):
         """Test dynamics factors across multiple timesteps by using collocation.
         """
         cdpr = Cdpr()
+        cdpr.params.winch_params.inertia_ = 1.23
         # kinematics
         fg = cdpr.kinematics_factors(ks=[0, 1, 2])
         # dynamics
@@ -135,14 +146,10 @@ def testDynamicsCollocation(self):
         fg.push_back(cdpr.collocation_factors(ks=[0, 1], dt=0.01))
         # initial state
         fg.push_back(
-            cdpr.priors_ik(ks=[0],
-                           Ts=[Pose3(Rot3(), (1.5, 0, 1.5))],
-                           Vs=[np.zeros(6)]))
-        # torque inputs (ID priors)
-        fg.push_back(
-            cdpr.priors_id(ks=[0, 1, 2], torquess=[
-                [1, 1, 0, 0],
-            ] * 3))
+            cdpr.priors_ik(ks=[0], Ts=[Pose3(Rot3(), (1.5, 0, 1.5))], Vs=[np.zeros(6)]))
+        # torque inputs (FD priors)
+        torques = [1 + 1.23, 1 + 1.23, 0 - 1.23, 0 - 1.23]
+        fg.push_back(cdpr.priors_fd(ks=[0, 1, 2], torquess=[torques,]*3))
         # construct initial guess
         init = gtsam.Values()
         init.insert(0, 0.01)
@@ -150,13 +157,15 @@ def testDynamicsCollocation(self):
             for j in range(4):
                 gtd.InsertJointAngle(init, j, t, 1)
                 gtd.InsertJointVel(init, j, t, 1)
+                gtd.InsertJointAccel(init, j, t, 1)
                 gtd.InsertTorque(init, j, t, 1)
+                gtd.InsertTension(init, j, t, 1)
                 gtd.InsertWrench(init, cdpr.ee_id(), j, t, np.ones(6))
             gtd.InsertPose(init, cdpr.ee_id(), t, Pose3(Rot3(), (1.5, 1, 1.5)))
             gtd.InsertTwist(init, cdpr.ee_id(), t, np.ones(6))
             gtd.InsertTwistAccel(init, cdpr.ee_id(), t, np.ones(6))
         # optimize
-        optimizer = gtsam.LevenbergMarquardtOptimizer(fg, init)
+        optimizer = gtsam.LevenbergMarquardtOptimizer(fg, init, MyLMParams())
         result = optimizer.optimize()
         # correctness checks:
         # timestep 0
diff --git a/gtdynamics/cablerobot/src/test_cdpr_planar_sim.py b/gtdynamics/cablerobot/src/test_cdpr_planar_sim.py
index 6307e9fa..0005c360 100644
--- a/gtdynamics/cablerobot/src/test_cdpr_planar_sim.py
+++ b/gtdynamics/cablerobot/src/test_cdpr_planar_sim.py
@@ -17,11 +17,11 @@
 from gtsam import Pose3, Rot3
 import numpy as np
 from cdpr_planar import Cdpr
-from cdpr_planar_controller import CdprControllerBase
+from cdpr_controller import CdprControllerBase
 from cdpr_planar_sim import CdprSimulator
 from gtsam.utils.test_case import GtsamTestCase
 
-class TestCdprPlanar(GtsamTestCase):
+class TestCdprSimulator(GtsamTestCase):
     def testSim(self):
         """Tests the simulation: given a controller and initial state, it will run through and
         simulate the system over multiple timesteps
diff --git a/gtdynamics/cablerobot/src/test_paint_parse.py b/gtdynamics/cablerobot/src/test_paint_parse.py
new file mode 100644
index 00000000..67a71243
--- /dev/null
+++ b/gtdynamics/cablerobot/src/test_paint_parse.py
@@ -0,0 +1,44 @@
+"""
+GTDynamics Copyright 2021, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+See LICENSE for the license information
+
+@file  test_paint_parse.py
+@brief Unit tests for parsing a paint trajectory in a .h file.
+@author Frank Dellaert
+@author Gerry Chen
+"""
+
+import unittest
+
+from paint_parse import ParseFile
+
+import numpy as np
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestPaintParse(GtsamTestCase):
+    """Unit tests for paint trajectory file parsing"""
+    def testParse(self):
+        e_painton = np.array([1, 1, 0, 1, 1, 1])
+        e_colorinds = np.array([0, 0, 1, 1, 2, 3])
+        e_colorpalette = np.array([[4, 49, 75],
+                                   [209, 4, 32],
+                                   [236, 237, 237],
+                                   [0, 0, 0]])
+        e_traj = np.array([[0.1, 1.1],
+                           [0.2, 1.0],
+                           [0.3, 0.9],
+                           [0.4, 1.1],
+                           [0.5, 1.2],
+                           [0.1, 1.15]])
+        a_painton, a_colorinds, a_colorpalette, a_traj = ParseFile('data/test_traj.h')
+
+        np.testing.assert_equal(e_painton, a_painton)
+        np.testing.assert_equal(e_colorinds, a_colorinds)
+        np.testing.assert_equal(e_colorpalette, a_colorpalette)
+        np.testing.assert_equal(e_traj, a_traj)
+
+if __name__ == "__main__":
+    unittest.main()
diff --git a/gtdynamics/cablerobot/src/utils.py b/gtdynamics/cablerobot/src/utils.py
index 2e346702..896b1770 100644
--- a/gtdynamics/cablerobot/src/utils.py
+++ b/gtdynamics/cablerobot/src/utils.py
@@ -31,9 +31,111 @@ def zerovalues(lid, ts=[], dt=0.01):
         for j in range(4):
             gtd.InsertJointAngle(zero, j, t, 0)
             gtd.InsertJointVel(zero, j, t, 0)
+            gtd.InsertJointAccel(zero, j, t, 0)
+            gtd.InsertTension(zero, j, t, 0)
             gtd.InsertTorque(zero, j, t, 0)
             gtd.InsertWrench(zero, lid, j, t, np.zeros(6))
         gtd.InsertPose(zero, lid, t, gtsam.Pose3(gtsam.Rot3(), (1.5, 0, 1.5)))
         gtd.InsertTwist(zero, lid, t, np.zeros(6))
         gtd.InsertTwistAccel(zero, lid, t, np.zeros(6))
     return zero
+
+def InsertZeroByLabel(values, key, dt=None):
+    label, l, j, t = key.label(), key.linkIdx(), key.jointIdx(), key.time()
+    if label == "q":  # JointAngleKey
+        gtd.InsertJointAngle(values, j, t, 0.0)
+    elif label == "v":  # JointVelKey
+        gtd.InsertJointVel(values, j, t, 0.0)
+    elif label == "a":  # JointAccelKey
+        gtd.InsertJointAccel(values, j, t, 0.0)
+    elif label == "t":  # TensionKey
+        gtd.InsertTension(values, j, t, 0.0)
+    elif label == "T":  # TorqueKey
+        gtd.InsertTorque(values, j, t, 0.0)
+    elif label == "p":  # PoseKey
+        gtd.InsertPose(values, l, t, gtsam.Pose3())
+    elif label == "V":  # TwistKey
+        gtd.InsertTwist(values, l, t, np.zeros(6))
+    elif label == "A":  # TwistAccelKey
+        gtd.InsertTwistAccel(values, l, t, np.zeros(6))
+    elif label == "F":  # WrenchKey
+        gtd.InsertWrench(values, l, j, t, np.zeros(6))
+    elif (dt is not None) and (key.key() == 0):
+        values.insert(key.key(), dt)
+    else:
+        print(key)
+        raise RuntimeError("Unknown key label type: ", label)
+
+def ZeroValues(graph, dt=None):
+    z = gtsam.Values()
+    for key in graph.keyVector():
+        key = gtd.DynamicsSymbol(key)
+        InsertZeroByLabel(z, key, dt)
+    return z
+
+
+def UpdateFromValues(src, dst, key, shift_time_by=0):
+    label = key.label()
+    if shift_time_by:
+        new_key = gtd.DynamicsSymbol.LinkJointSymbol(key.label(), key.linkIdx(), key.jointIdx(),
+                                                     key.time() + shift_time_by)
+    else:
+        new_key = key
+
+    if label in "qvatT":  # joint angle, vel, acc, tension, torque
+        dst.insert(new_key.key(), src.atDouble(key.key()))
+    elif label == "p":  # PoseKey
+        dst.insert(new_key.key(), src.atPose3(key.key()))
+    elif label == "V":  # TwistKey
+        gtd.InsertTwist(dst, new_key.linkIdx(), new_key.time(),
+                        gtd.Twist(src, key.linkIdx(), key.time()))
+    elif label == "A":  # TwistAccelKey
+        gtd.InsertTwistAccel(dst, new_key.linkIdx(), new_key.time(),
+                             gtd.TwistAccel(src, key.linkIdx(), key.time()))
+    elif label == "F":  # WrenchKey
+        gtd.InsertWrench(dst, new_key.linkIdx(), new_key.time(),
+                         gtd.Wrench(src, key.linkIdx(), key.time()))
+    elif key.key() == 0:
+        dst.insert(new_key.key(), src.atDouble(key.key()))
+    else:
+        print(key)
+        raise RuntimeError("Unknown key label type: ", label)
+
+
+def InitValues(graph, values=None, dt=None):
+    init = gtsam.Values()
+    existing_keys = set(values.keys()) if values is not None else set()
+    graphkeys = set(graph.keyVector())
+    for key in graphkeys & existing_keys:
+        UpdateFromValues(values, init, gtd.DynamicsSymbol(key))
+    for key in graphkeys - existing_keys:
+        InsertZeroByLabel(init, gtd.DynamicsSymbol(key), dt)
+    return init
+
+def InsertPose(dest, link_id, k, source):
+    gtd.InsertPose(dest, link_id, k, gtd.Pose(source, link_id, k))
+def InsertTwist(dest, link_id, k, source):
+    gtd.InsertTwist(dest, link_id, k, gtd.Twist(source, link_id, k))
+def InsertTwistAccel(dest, link_id, k, source):
+    gtd.InsertTwistAccel(dest, link_id, k, gtd.TwistAccel(source, link_id, k))
+def InsertJointAngles(dest, k, source):
+    for ji in range(4):
+        gtd.InsertJointAngle(dest, ji, k, gtd.JointAngle(source, ji, k))
+def InsertJointVels(dest, k, source):
+    for ji in range(4):
+        gtd.InsertJointVel(dest, ji, k, gtd.JointVel(source, ji, k))
+def InsertTorques(dest, k, source):
+    for ji in range(4):
+        gtd.InsertTorque(dest, ji, k, gtd.Torque(source, ji, k))
+def InsertWrenches(dest, link_id, k, source):
+    for ji in range(4):
+        gtd.InsertWrench(dest, link_id, ji, k, gtd.Wrench(source, link_id, ji, k))
+
+def MyLMParams(abs_err_tol=1e-15):
+    params = gtsam.LevenbergMarquardtParams()
+    if abs_err_tol is not None:
+        params.setRelativeErrorTol(0)
+        params.setAbsoluteErrorTol(0)
+        params.setErrorTol(abs_err_tol)
+    params.setLinearSolverType("MULTIFRONTAL_QR")
+    return params
diff --git a/gtdynamics/cablerobot/tests/testCableAccelerationFactor.cpp b/gtdynamics/cablerobot/tests/testCableAccelerationFactor.cpp
new file mode 100644
index 00000000..4b541724
--- /dev/null
+++ b/gtdynamics/cablerobot/tests/testCableAccelerationFactor.cpp
@@ -0,0 +1,83 @@
+/**
+ * @file  testCableAccelerationFactor.cpp
+ * @brief test cable accel factor
+ * @author Frank Dellaert
+ * @author Gerry Chen
+ */
+
+#include <gtdynamics/cablerobot/factors/CableAccelerationFactor.h>
+
+#include <gtdynamics/utils/values.h>
+
+#include <gtsam/base/Vector.h>
+#include <gtsam/inference/Symbol.h>
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/nonlinear/factorTesting.h>
+
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+using namespace gtdynamics;
+using namespace gtdynamics::internal;
+
+/**
+ * Test cable factor
+ */
+TEST(CableAccelerationFactor, error) {
+  // noise model
+  noiseModel::Gaussian::shared_ptr cost_model =
+      noiseModel::Isotropic::Sigma(1, 1.0);
+
+  int jid = 0;
+  int lid = 0;
+  Point3 frameLoc = Point3(0.1, 0.2, 0.3);
+  Point3 eeLoc = Point3(-0.15, 0, -0.15);
+
+  CableAccelerationFactor factor(JointAccelKey(jid), PoseKey(lid),
+                                 TwistKey(lid), TwistAccelKey(lid), cost_model,
+                                 frameLoc, eeLoc);
+
+  // Test configuration
+  Values values;
+  InsertJointAccel(&values, jid, 1.0);
+  InsertPose(&values, lid, Pose3(Rot3(), Point3(1.5, 0, 1.5)));
+  InsertTwist(&values, lid, (Vector6() << 0, 1, 0, 1.1, 0, 1.2).finished());
+  InsertTwistAccel(&values, lid,
+                   (Vector6() << 0, 1.3, 0, 1.4, 0, 1.5).finished());
+
+  // "expected" calculation
+  Vector3 dir = normalize(Point3(1.25, -.2, 1.05));
+  Vector3 linaccel = Vector3(1.4, 0, 1.5);  // translational part of VA
+  Vector3 rotaccel = 1.3 * normalize(Vector3(-0.15, 0, 0.15));  // rot of VA
+  Vector3 coraccel =
+      0.9 * 0.9 * 0.15 * sqrt(2) * normalize(-eeLoc);  // centripet
+  // TODO(gerry): temporarily ignoring rotational effects because it's annoying
+  // and relatively insignificant
+  double expected_lddot = dot(dir, linaccel + 0 * rotaccel + 0 * coraccel);
+  Vector1 expected_errors{JointAccel(values, jid) - expected_lddot};
+
+  // evaluateError
+  Vector1 actual_errors =
+      factor.evaluateError(JointAccel(values, jid), Pose(values, lid),
+                           Twist(values, lid), TwistAccel(values, lid));
+
+  EXPECT(assert_equal(expected_errors, actual_errors, 1e-4));
+
+  // jacobians
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-3);
+  values.update(PoseKey(lid), Pose3(Rot3::Rz(0.4), Point3(0.2, 0.3, 0.4)));
+  values.update(TwistKey(lid),
+                (Vector6() << 0.12, 0.13, 0.14, 0.18, 0.19, 0.21).finished());
+  values.update(TwistAccelKey(lid),
+                (Vector6() << 0.21, 0.22, 0.23, 0.24, 0.25, 0.26).finished());
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-3);
+}
+
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
diff --git a/gtdynamics/cablerobot/tests/testCustomWrap.cpp b/gtdynamics/cablerobot/tests/testCustomWrap.cpp
new file mode 100644
index 00000000..74433fa8
--- /dev/null
+++ b/gtdynamics/cablerobot/tests/testCustomWrap.cpp
@@ -0,0 +1,80 @@
+/**
+ * @file  testCustomWrap.cpp
+ * @brief test utilities in custom wrap file
+ * @author Frank Dellaert
+ * @author Gerry Chen
+ */
+
+#include <gtdynamics/cablerobot/utils/CustomWrap.h>
+
+#include <gtsam/linear/GaussianFactorGraph.h>
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
+
+#include <boost/assign/list_of.hpp>
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+using namespace gtdynamics;
+using boost::assign::list_of;
+
+/**
+ * Test eliminate sequential
+ */
+TEST(EliminateSequential, fullelimination) {
+  GaussianFactorGraph gfg;
+  gfg.add(0, Vector1(1), Vector1(0), noiseModel::Unit::Create(1));
+  gfg.add(1, Vector1(1), Vector1(0), noiseModel::Unit::Create(1));
+  gfg.add(2, Vector1(1),  //
+          0, Vector1(1),  //
+          Vector1(0), noiseModel::Unit::Create(1));
+
+  // specify the ordering as 0 -> 1 -> 2
+  Ordering ordering(list_of(0)(1)(2));
+  auto actual = EliminateSequential(gfg, ordering);
+
+  // expected Bayes net
+  GaussianBayesNet expected;
+  expected.push_back(GaussianConditional(0, Vector1(0), Vector1(sqrt(2)),  //
+                                         2, Vector1(1) / sqrt(2)));
+  expected.push_back(GaussianConditional(1, Vector1(0), Vector1(1)));
+  expected.push_back(GaussianConditional(2, Vector1(0), Vector1(1 / sqrt(2))));
+
+  // check if the result matches
+  EXPECT(assert_equal(expected, *actual));
+}
+
+/**
+ * Test block eliminate sequential
+ */
+TEST(EliminateSequential, block) {
+  GaussianFactorGraph gfg;
+  gfg.add(0, Vector1(1), Vector1(0), noiseModel::Unit::Create(1));
+  gfg.add(1, Vector1(1), Vector1(0), noiseModel::Unit::Create(1));
+  gfg.add(2, Vector1(1),  //
+          0, Vector1(1),  //
+          Vector1(0), noiseModel::Unit::Create(1));
+
+  // specify the ordering as (0;1) -> 2
+  BlockOrdering ordering{list_of(0)(1), list_of(2)};
+  auto actual = BlockEliminateSequential(gfg, ordering);
+
+  // expected Bayes net
+  GaussianBayesNet expected;
+  vector<pair<Key, Matrix>> terms{make_pair(0, Vector2(sqrt(2), 0)),
+                                  make_pair(1, Vector2(0, 1)),
+                                  make_pair(2, Vector2(1 / sqrt(2), 0))};
+  expected.push_back(GaussianConditional(terms, 2, Vector2(0, 0)));
+  expected.push_back(GaussianConditional(2, Vector1(0), Vector1(1 / sqrt(2))));
+
+  // check if the result matches
+  EXPECT(assert_equal(expected, *actual));
+}
+
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
diff --git a/gtdynamics/cablerobot/tests/testWinchFactor.cpp b/gtdynamics/cablerobot/tests/testWinchFactor.cpp
new file mode 100644
index 00000000..83780205
--- /dev/null
+++ b/gtdynamics/cablerobot/tests/testWinchFactor.cpp
@@ -0,0 +1,74 @@
+/**
+ * @file  testWinchFactor.cpp
+ * @brief test winch factor
+ * @author Gerry Chen
+ */
+
+#include <gtdynamics/cablerobot/factors/WinchFactor.h>
+
+#include <gtsam/base/Vector.h>
+#include <gtsam/inference/Symbol.h>
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/nonlinear/factorTesting.h>
+
+#include <iostream>
+#include <array>
+
+using namespace std;
+using namespace gtsam;
+using namespace gtdynamics;
+
+/**
+ * Test winch factor
+ */
+TEST(WinchFactor, error) {
+  // noise model
+  noiseModel::Gaussian::shared_ptr cost_model =
+      noiseModel::Isotropic::Sigma(1, 1.0);
+  WinchParams params(0.12,   // radius
+                     1.23,   // inerita
+                     0.34,   // static friction
+                     0.56);  // viscous friction
+
+  Symbol torqueKey = symbol('q', 0);
+  Symbol tensionKey = symbol('t', 0);
+  Symbol jointVelKey = symbol('v', 0);
+  Symbol jointAccKey = symbol('a', 0);
+  WinchFactor factor(torqueKey, tensionKey, jointVelKey, jointAccKey,
+                     cost_model, params);
+
+  //                                                    q   t   v   a
+  EXPECT(assert_equal(Vector1(0), factor.evaluateError( 0,  0,  0,  0)));
+  EXPECT(assert_equal(Vector1(0), factor.evaluateError(.12, 1,  0,  0)));
+  // inertia cancels out tension exactly
+  EXPECT(assert_equal(Vector1(0),
+                      factor.evaluateError(0, 1, 0, 0.12 * 0.12 / 1.23)));
+  // viscous friction
+  EXPECT(assert_equal(
+      Vector1(0),  // forward velocity -> extending cable -> negative torque
+      factor.evaluateError(-0.4621171573 * 0.34 - 0.01 / .12 * 0.56, 0, .01,
+                           0)));
+  EXPECT(assert_equal(Vector1(0),
+                      factor.evaluateError(-0.34 - 1 / 0.12 * 0.56, 0, 1, 0)));
+  EXPECT(assert_equal(Vector1(0),
+                      factor.evaluateError(+0.34 + 1 / 0.12 * 0.56, 0, -1, 0)));
+  // viscous friction with tension
+  EXPECT(assert_equal(
+      Vector1(0),
+      factor.evaluateError(0.12 + 0.34 + 1 / 0.12 * 0.56, 1, -1, 0)));
+
+  Values values;
+  values.insert(torqueKey, 1.);
+  values.insert(tensionKey, 2.);
+  values.insert(jointVelKey, .03);
+  values.insert(jointAccKey, 4.);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-3);
+}
+
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
diff --git a/gtdynamics/cablerobot/utils/CMakeLists.txt b/gtdynamics/cablerobot/utils/CMakeLists.txt
new file mode 100644
index 00000000..e69de29b
diff --git a/gtdynamics/cablerobot/utils/CustomWrap.cpp b/gtdynamics/cablerobot/utils/CustomWrap.cpp
new file mode 100644
index 00000000..f55bb7ed
--- /dev/null
+++ b/gtdynamics/cablerobot/utils/CustomWrap.cpp
@@ -0,0 +1,56 @@
+/**
+ * @file  CustomWrap.cpp
+ * @brief Custom wrap utilities
+ * @author Frank Dellaert
+ * @author Gerry Chen
+ */
+
+#include <gtdynamics/cablerobot/utils/CustomWrap.h>
+
+#include <gtsam/linear/GaussianBayesNet.h>
+#include <gtsam/linear/GaussianConditional.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+
+#include <boost/assign/list_of.hpp>
+
+using namespace gtsam;
+
+namespace gtdynamics {
+
+/******************************************************************************/
+GaussianBayesNet::shared_ptr EliminateSequential(GaussianFactorGraph graph,
+                                                 const Ordering& ordering) {
+  BlockOrdering blockOrdering;
+  for (auto k : ordering)
+    blockOrdering.push_back(boost::assign::list_of(k));
+  return BlockEliminateSequential(graph, blockOrdering);
+}
+
+/******************************************************************************/
+GaussianBayesNet::shared_ptr BlockEliminateSequential(
+    GaussianFactorGraph graph, const BlockOrdering &ordering) {
+  // setup
+  VariableIndex variableIndex(graph);  // maps keys to factor indices
+  auto bn = boost::make_shared<GaussianBayesNet>();
+
+  // loop
+  for (auto keys : ordering) {
+    // collect factors
+    GaussianFactorGraph factors;
+    for (auto key : keys)
+      for (size_t factorindex : variableIndex[key]) {
+        factors.push_back(graph.at(factorindex));
+        graph.remove(factorindex);
+      }
+    // eliminate
+    auto [conditional, newfactor] =
+        EliminationTraits<GaussianFactorGraph>::DefaultEliminate(factors, keys);
+    bn->push_back(conditional);
+    // add new joint factor
+    graph.push_back(newfactor);
+    variableIndex.augment(GaussianFactorGraph(newfactor));
+  }
+  return bn;
+}
+
+}  // namespace gtdynamics
diff --git a/gtdynamics/cablerobot/utils/CustomWrap.h b/gtdynamics/cablerobot/utils/CustomWrap.h
new file mode 100644
index 00000000..1dfc98e8
--- /dev/null
+++ b/gtdynamics/cablerobot/utils/CustomWrap.h
@@ -0,0 +1,75 @@
+/**
+ * @file  CustomWrap.h
+ * @brief Custom wrap utilities
+ * @author Frank Dellaert
+ * @author Gerry Chen
+ */
+
+#pragma once
+
+#include <gtdynamics/utils/DynamicsSymbol.h>
+#include <gtdynamics/utils/values.h>
+
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/GaussianBayesNet.h>
+#include <gtsam/linear/GaussianConditional.h>
+#include <gtsam/nonlinear/Values.h>
+
+namespace gtdynamics {
+
+using BlockOrdering = std::vector<gtsam::Ordering>;
+
+/// Performs sequential elimination and preserves correct bayes net order
+gtsam::GaussianBayesNet::shared_ptr EliminateSequential(
+    gtsam::GaussianFactorGraph graph, const gtsam::Ordering& ordering);
+
+/// Performs sequential block elimination and preserves correct bayes net order
+gtsam::GaussianBayesNet::shared_ptr BlockEliminateSequential(
+    gtsam::GaussianFactorGraph graph,
+    const BlockOrdering& ordering);
+
+///@name TensionKey
+///@{
+
+/// Shorthand for t_j_k, for j-th tension at time step k.
+inline DynamicsSymbol TensionKey(int j, int k = 0) {
+  return DynamicsSymbol::JointSymbol("t", j, k);
+}
+
+/**
+ * @brief Insert j-th tension at time k.
+ *
+ * @param values Values pointer to insert tension into.
+ * @param j The joint id.
+ * @param k Time step.
+ * @param value The tension value.
+ */
+void InsertTension(gtsam::Values *values, int j, int k, double value) {
+  values->insert(TensionKey(j, k), value);
+}
+
+/**
+ * @brief Insert j-th tension at time 0.
+ *
+ * @param values Values pointer to insert tension into.
+ * @param j The joint id.
+ * @param value The tension value.
+ */
+void InsertTension(gtsam::Values *values, int j, double value) {
+  values->insert(TensionKey(j), value);
+}
+
+/**
+ * @brief Retrieve j-th tension at time k.
+ *
+ * @param values Values dictionary containing the tension.
+ * @param j The joint id.
+ * @param k Time step.
+ * @return The tension.
+ */
+double Tension(const gtsam::Values &values, int j, int k = 0) {
+  return at<double>(values, TensionKey(j, k));
+}
+///@}
+
+}  // namespace gtdynamics