No more TransformFrom

gtdynamics/universal_robot/Joint.h

@@ -218,6 +218,12 @@ class Joint : public boost::enable_shared_from_this<Joint> {
    * @{
    */
 
+  /**
+   * Abstract method: Return joint type for use in reconstructing robot from
+   * JointParams.
+   */
+  virtual Type type() const = 0;
+
   /**
    * Abstract method. Return the transform from the other link com to this link
    * com frame given a Values object containing this joint's angle Value
@@ -226,12 +232,6 @@ class Joint : public boost::enable_shared_from_this<Joint> {
       size_t t, const LinkSharedPtr &link, const gtsam::Values &q,
       boost::optional<gtsam::Matrix &> H_q = boost::none) const = 0;
 
-  /**
-   * Abstract method: Return joint type for use in reconstructing robot from
-   * JointParams.
-   */
-  virtual Type type() const = 0;
-
   /** Abstract method. Return the twist of the other link given this link's
    * twist and a Values object containing this joint's angle Value.
    */
@@ -414,79 +414,23 @@ class Joint : public boost::enable_shared_from_this<Joint> {
   /**
    * Return the pose of this link com given a Values object containing this
    * joint's angle Value, and also given the other link's pose.
-   * Equivalent to T_other.compose(transformFrom(link, q)).
    */
   Pose3 transformTo(
       size_t t, const LinkSharedPtr &link, const gtsam::Values &q,
       const gtsam::Pose3 &T_other,
       boost::optional<gtsam::Matrix &> H_q = boost::none,
       gtsam::OptionalJacobian<6, 6> H_T_other = boost::none) const {
+    LinkSharedPtr other = otherLink(link);
     if (!H_q) {
-      return T_other.compose(transformFrom(t, link, q), H_T_other);
+      return T_other.compose(transformTo(t, other, q), H_T_other);
     } else {
       gtsam::Matrix66 H_relPose;
       Pose3 error =
-          T_other.compose(transformFrom(t, link, q, H_q), H_T_other, H_relPose);
+          T_other.compose(transformTo(t, other, q, H_q), H_T_other, H_relPose);
       *H_q = H_relPose * (*H_q);
       return error;
     }
   }
-
-  /**
-   * Return the transform from this link com to the other link com frame given a
-   * Values object containing this joint's angle value.
-   */
-  Pose3 transformFrom(
-      size_t t, const LinkSharedPtr &link, const gtsam::Values &q,
-      boost::optional<gtsam::Matrix &> H_q = boost::none) const {
-    return transformTo(t, otherLink(link), q, H_q);
-  }
-
-  /**
-   * Return the pose of other link com given a Values object containing other
-   * joint's angle Value, and also given the this link's pose.
-   * Equivalent to T_this.compose(transformTo(link, q))
-   */
-  Pose3 transformFrom(
-      size_t t, const LinkSharedPtr &link, const gtsam::Values &q,
-      const gtsam::Pose3 &T_this,
-      boost::optional<gtsam::Matrix &> H_q = boost::none,
-      boost::optional<gtsam::Matrix &> H_T_this = boost::none) const {
-    return transformTo(t, otherLink(link), q, T_this, H_q, H_T_this);
-  }
-
-  /**
-   * Return the twist of the other link given this link's twist and a Values
-   * object containing this joint's angle value.
-   */
-  gtsam::Vector6 transformTwistFrom(
-      size_t t, const LinkSharedPtr &link, const gtsam::Values &q_and_q_dot,
-      boost::optional<gtsam::Vector6> this_twist = boost::none,
-      boost::optional<gtsam::Matrix &> H_q = boost::none,
-      boost::optional<gtsam::Matrix &> H_q_dot = boost::none,
-      boost::optional<gtsam::Matrix &> H_this_twist = boost::none) const {
-    return transformTwistTo(t, otherLink(link), q_and_q_dot, this_twist, H_q,
-                            H_q_dot, H_this_twist);
-  }
-
-  /**
-   * Return the twist acceleration of the other link given this link's twist
-   * accel and a Values object containing this joint's angle value and
-   * derivatives.
-   */
-  gtsam::Vector6 transformTwistAccelFrom(
-      size_t t, const LinkSharedPtr &link, const gtsam::Values &q_and_q_dot_and_q_ddot,
-      boost::optional<gtsam::Vector6> other_twist = boost::none,
-      boost::optional<gtsam::Vector6> this_twist_accel = boost::none,
-      boost::optional<gtsam::Matrix &> H_q = boost::none,
-      boost::optional<gtsam::Matrix &> H_q_dot = boost::none,
-      boost::optional<gtsam::Matrix &> H_q_ddot = boost::none,
-      boost::optional<gtsam::Matrix &> H_other_twist = boost::none,
-      boost::optional<gtsam::Matrix &> H_this_twist_accel = boost::none) const {
-    return transformTwistAccelTo(t, otherLink(link), q_and_q_dot_and_q_ddot,
-                                 other_twist, this_twist_accel, H_q, H_q_dot,
-                                 H_q_ddot, H_other_twist, H_this_twist_accel);
-  }
 };
 
 }  // namespace gtdynamics

gtdynamics/universal_robot/JointTyped.h

@@ -30,7 +30,7 @@ namespace gtdynamics {
 
 // TODO(Gerry) JointTyped was an intermediate step towards adding ball and
 // sphere joints but we never finished it because for other joint types,
-// transformFrom and transformTo can't just use a double as the joint angle
+// transformTo can't just use a double as the joint angle
 // argument, they need Unit3 or Rot3
 
 class JointTyped : public Joint {
@@ -52,11 +52,6 @@ class JointTyped : public Joint {
   /// Inherit constructors
   using Joint::Joint;
 
-  /// Inherit overloaded functions
-  using Joint::transformFrom;
-  using Joint::transformTwistAccelFrom;
-  using Joint::transformTwistFrom;
-
   /**
    * @name Abstract
    * Joint classes must implement these methods.
@@ -124,64 +119,15 @@ class JointTyped : public Joint {
       gtsam::OptionalJacobian<6, N> H_q = boost::none,
       gtsam::OptionalJacobian<6, 6> H_T_other = boost::none) const {
     gtsam::Matrix66 H_relPose;
-    Pose3 error = T_other.compose(transformFrom(link, q, H_q), H_T_other,
+    LinkSharedPtr other = otherLink(link);
+    Pose3 error = T_other.compose(transformTo(other, q, H_q), H_T_other,
                                   H_q ? &H_relPose : 0);
     if (H_q) {
       *H_q = H_relPose * (*H_q);
     }
     return error;
   }
 
-  /**
-   * Convenience method to return the transform from this link com to the other
-   * link com frame.
-   */
-  Pose3 transformFrom(const LinkSharedPtr &link, JointCoordinate q,
-                      gtsam::OptionalJacobian<6, N> H_q = boost::none) const {
-    return transformTo(otherLink(link), q, H_q);
-  }
-
-  /// Convenience method to return the pose of other link com.
-  Pose3 transformFrom(
-      const LinkSharedPtr &link, JointCoordinate q, const gtsam::Pose3 &T_this,
-      gtsam::OptionalJacobian<6, N> H_q = boost::none,
-      gtsam::OptionalJacobian<6, 6> H_T_this = boost::none) const {
-    return transformTo(otherLink(link), q, T_this, H_q, H_T_this);
-  }
-
-  /**
-   * Convenience method to return the twist of the other link given this link's
-   * twist and joint angle.
-   */
-  gtsam::Vector6 transformTwistFrom(
-      const LinkSharedPtr &link, JointCoordinate q, JointVelocity q_dot,
-      boost::optional<gtsam::Vector6> this_twist = boost::none,
-      gtsam::OptionalJacobian<6, N> H_q = boost::none,
-      gtsam::OptionalJacobian<6, N> H_q_dot = boost::none,
-      gtsam::OptionalJacobian<6, 6> H_this_twist = boost::none) const {
-    return transformTwistTo(otherLink(link), q, q_dot, this_twist, H_q, H_q_dot,
-                            H_this_twist);
-  }
-
-  /** Convenience method. Return the twist acceleration of the other link given
-   * this link's twist accel and a Values object containing this joint's angle
-   * value and derivatives.
-   */
-  gtsam::Vector6 transformTwistAccelFrom(
-      const LinkSharedPtr &link, JointCoordinate q, JointVelocity q_dot,
-      JointAcceleration q_ddot,
-      boost::optional<gtsam::Vector6> other_twist = boost::none,
-      boost::optional<gtsam::Vector6> this_twist_accel = boost::none,
-      gtsam::OptionalJacobian<6, N> H_q = boost::none,
-      gtsam::OptionalJacobian<6, N> H_q_dot = boost::none,
-      gtsam::OptionalJacobian<6, N> H_q_ddot = boost::none,
-      gtsam::OptionalJacobian<6, 6> H_other_twist = boost::none,
-      gtsam::OptionalJacobian<6, 6> H_this_twist_accel = boost::none) const {
-    return transformTwistAccelTo(otherLink(link), q, q_dot, q_ddot, other_twist,
-                                 this_twist_accel, H_q, H_q_dot, H_q_ddot,
-                                 H_other_twist, H_this_twist_accel);
-  }
-
   /**
    * Return the transform from the other link com to this link com frame given a
    * Values object containing this joint's angle value.

gtdynamics/universal_robot/Robot.cpp

@@ -180,7 +180,7 @@ gtsam::Values Robot::forwardKinematics(
 
       // Compute the twist. TODO(frank): pass just one value.
       const Vector6 V_2 =
-          joint->transformTwistFrom(t, link1, known_values, V_1);
+          joint->transformTwistTo(t, link2, known_values, V_1);
 
       // Save pose and twist if link 2 has not been assigned yet.
       auto pose_key = internal::PoseKey(link2->id(), t);
@@ -259,8 +259,8 @@ FKResults Robot::forwardKinematics(
 
       // If joint_velocities are provided, compute the twist, else default to zero.
       const Vector6 V_2 =
-          joint_velocities ? joint_ptr->transformTwistFrom(
-                                 link1, joint_angle,
+          joint_velocities ? joint_ptr->transformTwistTo(
+                                 link2, joint_angle,
                                  joint_velocities->at(joint_ptr->name()), V_1)
                            : gtsam::Z_6x1;
 

tests/testPoseFactor.cpp

@@ -105,6 +105,7 @@ TEST(PoseFactor, breaking_rr) {
 
   double joint_angle = M_PI / 4;
 
+  auto l1 = robot.link("l1");
   auto l2 = robot.link("l2");
   auto j1 = boost::dynamic_pointer_cast<gtdynamics::ScrewJointBase>(
       robot.joint("j1"));
@@ -116,11 +117,11 @@ TEST(PoseFactor, breaking_rr) {
   PoseFactor factor(example::pose_p_key, example::pose_c_key, example::qKey,
                     example::cost_model, joint);
 
-  EXPECT(assert_equal(
-      gtsam::Z_6x1,
-      factor.evaluateError(base_pose, j1->transformFrom(l2, joint_angle),
-                           joint_angle),
-      1e-6));
+  EXPECT(assert_equal(gtsam::Z_6x1,
+                      factor.evaluateError(base_pose,
+                                           j1->transformTo(l1, joint_angle),
+                                           joint_angle),
+                      1e-6));
 }
 
 // Test non-zero cMp rotation case

tests/testPrismaticJoint.cpp

@@ -64,16 +64,12 @@ TEST(Joint, params_constructor_prismatic) {
   // rest transform
   Pose3 T_12comRest(Rot3::Rx(1.5707963268), Point3(0, -1, 1));
   Pose3 T_21comRest(Rot3::Rx(-1.5707963268), Point3(0, -1, -1));
-  EXPECT(assert_equal(T_12comRest, j1->transformFrom(l2, 0.0), 1e-5));
+  EXPECT(assert_equal(T_12comRest, j1->transformTo(l1, 0.0), 1e-5));
   EXPECT(assert_equal(T_21comRest, j1->transformTo(l2, 0.0), 1e-5));
 
-  // transform from (translating +1)
+  // transform to (translating +1)
   Pose3 T_12com(Rot3::Rx(1.5707963268), Point3(0, -2, 1));
   Pose3 T_21com(Rot3::Rx(-1.5707963268), Point3(0, -1, -2));
-  EXPECT(assert_equal(T_12com, j1->transformFrom(l2, 1), 1e-5));
-  EXPECT(assert_equal(T_21com, j1->transformFrom(l1, 1), 1e-5));
-
-  // transfrom to (translating +1)
   EXPECT(assert_equal(T_12com, j1->transformTo(l1, 1), 1e-5));
   EXPECT(assert_equal(T_21com, j1->transformTo(l2, 1), 1e-5));
 

tests/testRevoluteJoint.cpp

@@ -14,14 +14,28 @@
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/numericalDerivative.h>
 
 #include "gtdynamics/universal_robot/Link.h"
 #include "gtdynamics/universal_robot/RevoluteJoint.h"
 #include "gtdynamics/universal_robot/RobotModels.h"
 #include "gtdynamics/universal_robot/sdf.h"
 #include "gtdynamics/utils/utils.h"
-
-using gtsam::assert_equal, gtsam::Pose3, gtsam::Point3, gtsam::Rot3;
+#include "gtdynamics/utils/values.h"
+
+using gtsam::assert_equal;
+using gtsam::Matrix;
+using gtsam::Matrix61;
+using gtsam::numericalDerivative11;
+using gtsam::Point3;
+using gtsam::Pose3;
+using gtsam::Rot3;
+using gtsam::Values;
+using gtsam::Vector;
+using gtsam::Vector3;
+using gtsam::Vector6;
+
+using namespace gtdynamics;
 
 /**
  * Construct a Revolute joint via Parameters and ensure all values are as
@@ -32,46 +46,66 @@ TEST(Joint, params_constructor) {
   auto l1 = robot.link("l1");
   auto l2 = robot.link("l2");
 
-  using gtdynamics::Joint;
-  gtdynamics::JointParams parameters;
-  parameters.effort_type = gtdynamics::JointEffortType::Actuated;
+  JointParams parameters;
+  parameters.effort_type = JointEffortType::Actuated;
   parameters.scalar_limits.value_lower_limit = -1.57;
   parameters.scalar_limits.value_upper_limit = 1.57;
   parameters.scalar_limits.value_limit_threshold = 0;
 
-  const gtsam::Vector3 axis = (gtsam::Vector(3) << 1, 0, 0).finished();
+  const Vector3 axis = (Vector(3) << 1, 0, 0).finished();
 
-  gtdynamics::RevoluteJoint j1(1, "j1", Pose3(Rot3(), Point3(0, 0, 2)), l1, l2,
-                               parameters, axis);
+  RevoluteJoint j1(1, "j1", Pose3(Rot3(), Point3(0, 0, 2)), l1, l2, parameters,
+                   axis);
 
   // name
   EXPECT(assert_equal(j1.name(), "j1"));
 
   // joint effort type
-  EXPECT(j1.parameters().effort_type == gtdynamics::JointEffortType::Actuated);
+  EXPECT(j1.parameters().effort_type == JointEffortType::Actuated);
 
   // other link
   EXPECT(j1.otherLink(l2) == l1);
   EXPECT(j1.otherLink(l1) == l2);
 
+  // Rotating joint by -M_PI / 2
+  double q = -M_PI / 2;
+  Pose3 T_12com(Rot3::Rx(q), Point3(0, 1, 1));
+  Pose3 T_21com(Rot3::Rx(-q), Point3(0, 1, -1));
+
   // rest transform
   Pose3 T_21comRest(Rot3::Rx(0), Point3(0, 0, -2));
   EXPECT(assert_equal(T_21comRest, j1.transformTo(l2, 0.0)));
-
-  EXPECT(assert_equal(T_21comRest.inverse(), j1.transformFrom(l2, 0.0)));
-
-  // transform from (rotating -pi/2)
-  Pose3 T_12com(Rot3::Rx(-M_PI / 2), Point3(0, 1, 1));
-  Pose3 T_21com(Rot3::Rx(M_PI / 2), Point3(0, 1, -1));
-  EXPECT(assert_equal(T_12com, j1.transformFrom(l2, -M_PI / 2)));
-  EXPECT(assert_equal(T_21com, j1.transformFrom(l1, -M_PI / 2)));
-
-  // transfrom to (rotating -pi/2)
-  EXPECT(assert_equal(T_12com, j1.transformTo(l1, -M_PI / 2)));
-  EXPECT(assert_equal(T_21com, j1.transformTo(l2, -M_PI / 2)));
+  EXPECT(assert_equal(T_21comRest.inverse(), j1.transformTo(l1, 0.0)));
+
+  // transform from
+  EXPECT(assert_equal(T_12com, j1.transformTo(l1, q)));
+  EXPECT(assert_equal(T_21com, j1.transformTo(l2, q)));
+
+  // Calculate enumerical derivatives of transformTo.
+  auto f1 = [&](double q) { return j1.transformTo(l1, q); };
+  Matrix61 numericalH1 = numericalDerivative11<Pose3, double>(f1, q);
+  auto f2 = [&](double q) { return j1.transformTo(l2, q); };
+  Matrix61 numericalH2 = numericalDerivative11<Pose3, double>(f2, q);
+
+  // Check transformTo with derivatives.
+  Matrix61 H1, H2;
+  EXPECT(assert_equal(T_12com, j1.transformTo(l1, q, H1)));
+  EXPECT(assert_equal(T_21com, j1.transformTo(l2, q, H2)));
+  EXPECT(assert_equal(numericalH1, H1));
+  EXPECT(assert_equal(numericalH2, H2));
+
+  // Check values-based TransformTo, with derivatives.
+  Values values;
+  const size_t t = 0;
+  InsertJointAngle(&values, 1, q);
+  Matrix H1v, H2v;
+  EXPECT(assert_equal(T_12com, j1.transformTo(t, l1, values, H1v)));
+  EXPECT(assert_equal(T_21com, j1.transformTo(t, l2, values, H2v)));
+  EXPECT(assert_equal(numericalH1, H1v));
+  EXPECT(assert_equal(numericalH2, H2v));
 
   // screw axis
-  gtsam::Vector6 screw_axis_l1, screw_axis_l2;
+  Vector6 screw_axis_l1, screw_axis_l2;
   screw_axis_l1 << -1, 0, 0, 0, -1, 0;
   screw_axis_l2 << 1, 0, 0, 0, -1, 0;
   EXPECT(assert_equal(screw_axis_l1, j1.screwAxis(l1)));

tests/testScrewJoint.cpp

@@ -67,16 +67,12 @@ TEST(Joint, params_constructor) {
   // rest transform
   Pose3 T_12comRest(Rot3::Rx(0), Point3(0, 0, 2));
   Pose3 T_21comRest(Rot3::Rx(0), Point3(0, 0, -2));
-  EXPECT(assert_equal(T_12comRest, j1->transformFrom(l2, 0.0)));
+  EXPECT(assert_equal(T_12comRest, j1->transformTo(l1, 0.0)));
   EXPECT(assert_equal(T_21comRest, j1->transformTo(l2, 0.0)));
 
-  // transform from (rotating -pi/2)
+  // transform to (rotating -pi/2)
   Pose3 T_12com(Rot3::Rx(-M_PI / 2), Point3(-0.125, 1, 1));
   Pose3 T_21com(Rot3::Rx(M_PI / 2), Point3(0.125, 1, -1));
-  EXPECT(assert_equal(T_12com, j1->transformFrom(l2, -M_PI / 2)));
-  EXPECT(assert_equal(T_21com, j1->transformFrom(l1, -M_PI / 2)));
-
-  // transform to (rotating -pi/2)
   EXPECT(assert_equal(T_12com, j1->transformTo(l1, -M_PI / 2)));
   EXPECT(assert_equal(T_21com, j1->transformTo(l2, -M_PI / 2)));