MBT methods to compute relative transforms and convert positions betw…

…een frames (RobotLocomotion#7485)

* Added a bunch of random notes to aid development based off what IK needs

* Adds notes on notation and proposed API

* Adds notes on notation and proposed API

* Notes on orientation Jacobians

* Created a tentative action list

* Introduces API for the computation of geometric Jacobian

* Write implementation in terms of soon-to-get-cached J_PB_W

* Compute velocity kinematics in terms of H_PB_W

* Addresses reviews

* Addresses reviews

* Implements geometric Jacobian computation

* Adds unit tests for PointsPositionsAndRelativeTransform

* Adds doc

* Removes unwanted code for this PR

* More unwanted changes

* Add DEATH test for when the arrays have the wrong sizes

* Addresses reviews

* small doc update

drake/multibody/multibody_tree/body_node.h

@@ -852,13 +852,13 @@ class BodyNode : public MultibodyTreeElement<BodyNode<T>, BodyNodeIndex> {
 
   /// Mutable version of GetJacobianFromArray().
   Eigen::Map<MatrixUpTo6<T>> GetMutableJacobianFromArray(
-      std::vector<Vector6<T>>* H_PB_array) const {
+      std::vector<Vector6<T>>* H_array) const {
     const int start_index_in_v = get_topology().mobilizer_velocities_start_in_v;
     const int num_velocities = get_topology().num_mobilizer_velocities;
     // The first column of this node's Jacobian matrix H_PB_W:
-    Vector6<T>& H_PB_W_col0 = (*H_PB_array)[start_index_in_v];
+    Vector6<T>& H_col0 = (*H_array)[start_index_in_v];
     // Create an Eigen map to the full H_PB_W for this node:
-    return Eigen::Map<MatrixUpTo6<T>>(H_PB_W_col0.data(), 6, num_velocities);
+    return Eigen::Map<MatrixUpTo6<T>>(H_col0.data(), 6, num_velocities);
   }
 
  protected:

drake/multibody/multibody_tree/multibody_tree.cc

@@ -6,6 +6,7 @@
 
 #include "drake/common/autodiff.h"
 #include "drake/common/drake_assert.h"
+#include "drake/common/drake_throw.h"
 #include "drake/common/eigen_types.h"
 #include "drake/multibody/multibody_tree/body_node_welded.h"
 #include "drake/multibody/multibody_tree/rigid_body.h"
@@ -519,6 +520,38 @@ void MultibodyTree<T>::DoCalcBiasTerm(
                       &A_WB_array, &F_BMo_W_array, Cv);
 }
 
+template <typename T>
+Isometry3<T> MultibodyTree<T>::CalcRelativeTransform(
+    const systems::Context<T>& context,
+    const Frame<T>& to_frame_A, const Frame<T>& from_frame_B) const {
+  // TODO(amcastro-tri): retrieve (Eval) pc from the cache.
+  PositionKinematicsCache<T> pc(this->get_topology());
+  CalcPositionKinematicsCache(context, &pc);
+  const Isometry3<T>& X_WA =
+      pc.get_X_WB(to_frame_A.get_body().get_node_index());
+  const Isometry3<T>& X_WB =
+      pc.get_X_WB(from_frame_B.get_body().get_node_index());
+  return X_WA.inverse() * X_WB;
+}
+
+template <typename T>
+void MultibodyTree<T>::CalcPointsPositions(
+    const systems::Context<T>& context,
+    const Frame<T>& from_frame_B,
+    const Eigen::Ref<const MatrixX<T>>& p_BQi,
+    const Frame<T>& to_frame_A,
+    EigenPtr<MatrixX<T>> p_AQi) const {
+  DRAKE_THROW_UNLESS(p_BQi.rows() == 3);
+  DRAKE_THROW_UNLESS(p_AQi != nullptr);
+  DRAKE_THROW_UNLESS(p_AQi->rows() == 3);
+  DRAKE_THROW_UNLESS(p_AQi->cols() == p_BQi.cols());
+  const Isometry3<T> X_AB =
+      CalcRelativeTransform(context, to_frame_A, from_frame_B);
+  // We demanded above that these matrices have three rows. Therefore we tell
+  // Eigen so.
+  p_AQi->template topRows<3>() = X_AB * p_BQi.template topRows<3>();
+}
+
 template <typename T>
 void MultibodyTree<T>::CalcAcrossNodeGeometricJacobianExpressedInWorld(
     const systems::Context<T>& context,

drake/multibody/multibody_tree/multibody_tree.h

@@ -1000,6 +1000,62 @@ class MultibodyTree {
   void CalcBiasTerm(
       const systems::Context<T>& context, EigenPtr<VectorX<T>> Cv) const;
 
+  /// Computes the relative transform `X_AB(q)` from a frame B to a frame A, as
+  /// a function of the generalized positions q of the model.
+  /// That is, the position `p_AQ` of a point Q measured and expressed in
+  /// frame A can be computed from the position `p_BQ` of this point measured
+  /// and expressed in frame B using the transformation `p_AQ = X_AB⋅p_BQ`.
+  ///
+  /// @param[in] context
+  ///   The context containing the state of the %MultibodyTree model. It stores
+  ///   the generalized positions q of the model.
+  /// @param[in] to_frame_A
+  ///   The target frame A in the computed relative transform `X_AB`.
+  /// @param[in] from_frame_B
+  ///   The source frame B in the computed relative transform `X_AB`.
+  /// @retval X_AB
+  ///   The relative transform from frame B to frame A, such that
+  ///   `p_AQ = X_AB⋅p_BQ`.
+  Isometry3<T> CalcRelativeTransform(
+      const systems::Context<T>& context,
+      const Frame<T>& to_frame_A, const Frame<T>& from_frame_B) const;
+
+  ///  Given the positions `p_BQi` for a set of points `Qi` measured and
+  ///  expressed in a frame B, this method computes the positions `p_AQi(q)` of
+  ///  each point `Qi` in the set as measured and expressed in another frame A,
+  ///  as a function of the generalized positions q of the model.
+  ///
+  /// @param[in] context
+  ///   The context containing the state of the %MultibodyTree model. It stores
+  ///   the generalized positions q of the model.
+  /// @param[in] from_frame_B
+  ///   The frame B in which the positions `p_BQi` of a set of points `Qi` are
+  ///   given.
+  /// @param[in] p_BQi
+  ///   The input positions of each point `Qi` in frame B. `p_BQi ∈ ℝ³ˣⁿᵖ` with
+  ///   `np` the number of points in the set. Each column of `p_BQi` corresponds
+  ///   to a vector in ℝ³ holding the position of one of the points in the set
+  ///   as measured and expressed in frame B.
+  /// @param[in] to_frame_A
+  ///   The frame A in which it is desired to compute the positions `p_AQi` of
+  ///   each point `Qi` in the set.
+  /// @param[out] p_BQi
+  ///   The output positions of each point `Qi` now computed as measured and
+  ///   expressed in frame A. The output `p_AQi` **must** have the same size as
+  ///   the input `p_BQi` or otherwise this method aborts. That is `p_AQi`
+  ///   **must** be in `ℝ³ˣⁿᵖ`.
+  ///
+  /// @note Both `p_BQi` and `p_BQi` must have three rows. Otherwise this
+  /// method will throw a std::runtime_error exception. This method also throws
+  /// a std::runtime_error exception if `p_BQi` and `p_BQi` differ in the number
+  /// of columns.
+  void CalcPointsPositions(
+      const systems::Context<T>& context,
+      const Frame<T>& from_frame_B,
+      const Eigen::Ref<const MatrixX<T>>& p_BQi,
+      const Frame<T>& to_frame_A,
+      EigenPtr<MatrixX<T>> p_AQi) const;
+
   /// Transforms generalized velocities v to time derivatives `qdot` of the
   /// generalized positions vector `q` (stored in `context`). `v` and `qdot`
   /// are related linearly by `q̇ = N(q)⋅v`.

drake/multibody/multibody_tree/test/double_pendulum_test.cc

@@ -1252,6 +1252,90 @@ TEST_F(PendulumKinematicTests, CalcVelocityKinematicsWithAutoDiffXd) {
   }  // iw_shoulder
 }
 
+TEST_F(PendulumKinematicTests, PointsPositionsAndRelativeTransform) {
+  // This is the minimum factor of the machine precision within which these
+  // tests pass.
+  const int kEpsilonFactor = 3;
+  const double kTolerance = kEpsilonFactor * kEpsilon;
+
+  shoulder_mobilizer_->set_angle(context_.get(), M_PI / 4.0);
+  elbow_mobilizer_->set_angle(context_.get(), M_PI / 4.0);
+
+  // Set of points Qi measured and expressed in the lower link's frame L.
+  Matrix3X<double> p_LQi_set(3, 3);
+  p_LQi_set.col(0) << 0.0, -2.0, 0.0;  // At the end effector.
+  p_LQi_set.col(1) << 0.0, -1.0, 0.0;  // In the middle of the lower link.
+  p_LQi_set.col(2) << 0.0,  0.0, 0.0;  // At the elbow.
+
+  // World positions of the set of points Qi:
+  Matrix3X<double> p_WQi_set(3, 3);
+  model_->CalcPointsPositions(
+      *context_,
+      lower_link_->get_body_frame(), p_LQi_set,
+      model_->get_world_frame(), &p_WQi_set);
+
+  Matrix3X<double> p_WQi_set_expected(3, 3);
+  p_WQi_set_expected.col(0) << 2.0 + M_SQRT1_2, -M_SQRT1_2, 0.0;
+  p_WQi_set_expected.col(1) << 1.0 + M_SQRT1_2, -M_SQRT1_2, 0.0;
+  p_WQi_set_expected.col(2) << M_SQRT1_2, -M_SQRT1_2, 0.0;
+
+  EXPECT_TRUE(CompareMatrices(
+      p_WQi_set, p_WQi_set_expected, kTolerance, MatrixCompareType::relative));
+
+  // A scond set of points Pi but measured and expressed in the upper link's
+  // frame U.
+  Matrix3X<double> p_UPi_set(3, 3);
+  p_UPi_set.col(0) << 0.0,  1.0, 0.0;  // Projecting 0.5 out from the shoulder.
+  p_UPi_set.col(1) << 0.0,  0.0, 0.0;  // In the middle of the upper link.
+  p_UPi_set.col(2) << 0.0, -1.0, 0.0;  // Projecting 0.5 out from the elbow.
+
+  // World positions of the set of points Qi:
+  Matrix3X<double> p_WPi_set(3, 3);
+  model_->CalcPointsPositions(
+      *context_,
+      upper_link_->get_body_frame(), p_UPi_set,
+      model_->get_world_frame(), &p_WPi_set);
+
+  Matrix3X<double> p_WPi_set_expected(3, 3);
+  p_WPi_set_expected.col(0) = 0.5 * Vector3d(-M_SQRT1_2, M_SQRT1_2, 0.0);
+  p_WPi_set_expected.col(1) = -0.5 * Vector3d(-M_SQRT1_2, M_SQRT1_2, 0.0);
+  p_WPi_set_expected.col(2) = -1.5 * Vector3d(-M_SQRT1_2, M_SQRT1_2, 0.0);
+
+  EXPECT_TRUE(CompareMatrices(
+      p_WPi_set, p_WPi_set_expected, kTolerance, MatrixCompareType::relative));
+
+  const Isometry3d X_UL = model_->CalcRelativeTransform(
+      *context_, upper_link_->get_body_frame(), lower_link_->get_body_frame());
+  const Vector3d p_UL = X_UL.translation();
+  const Matrix3d R_UL = X_UL.linear();
+
+  const Vector3d p_UL_expected(0.0, -0.5, 0.0);
+  const Matrix3d R_UL_expected(Eigen::AngleAxisd(M_PI_4, Vector3d::UnitZ()));
+
+  EXPECT_TRUE(CompareMatrices(
+      p_UL, p_UL_expected, kTolerance, MatrixCompareType::relative));
+  EXPECT_TRUE(CompareMatrices(
+      R_UL, R_UL_expected, kTolerance, MatrixCompareType::relative));
+}
+
+TEST_F(PendulumKinematicTests, PointsHaveTheWrongSize) {
+  ::testing::FLAGS_gtest_death_test_style = "threadsafe";
+
+  shoulder_mobilizer_->set_angle(context_.get(), M_PI / 4.0);
+  elbow_mobilizer_->set_angle(context_.get(), M_PI / 4.0);
+
+  // Create a set of points with the wrong number of rows (it must be 3).
+  // The values do not matter for this test, just the size.
+  MatrixX<double> p_LQi_set = MatrixX<double>::Zero(4, 3);
+
+  // World positions of the set of points Qi:
+  Matrix3X<double> p_WQi_set(3, 3);
+  EXPECT_THROW(model_->CalcPointsPositions(
+      *context_,
+      lower_link_->get_body_frame(), p_LQi_set,
+      model_->get_world_frame(), &p_WQi_set), std::runtime_error);
+}
+
 }  // namespace
 }  // namespace multibody
 }  // namespace drake