Skip to content

Commit

Permalink
Improve rotation matrix methods in Frame and MultibodyTree. (RobotLoc…
Browse files Browse the repository at this point in the history 
…omotion#12683)
  • Loading branch information
@mitiguy
mitiguy authored Feb 7, 2020
1 parent 6e39060 commit f6564bd
Show file tree
Hide file tree
Showing 12 changed files with 231 additions and 70 deletions.
6 changes: 3 additions & 3 deletions manipulation/planner/test/differential_inverse_kinematics_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -173,9 +173,9 @@ TEST_F(DifferentialInverseKinematicsTest, OverConstrainedTest) {
TEST_F(DifferentialInverseKinematicsTest, GainTest) {
const VectorXd q = plant_->GetPositions(*context_);

const math::RigidTransform<double> X_WE =
frame_E_->CalcPoseInWorld(*context_);
const math::RotationMatrix<double> R_EW = X_WE.rotation().transpose();
const math::RotationMatrix<double> R_WE =
frame_E_->CalcRotationMatrixInWorld(*context_);
const math::RotationMatrix<double> R_EW = R_WE.inverse();

const multibody::SpatialVelocity<double> V_WE_W_desired(
Vector3d(0.1, -0.2, 0.3) / 2, Vector3d(-0.3, 0.2, -0.1) / 2);
Expand Down
8 changes: 4 additions & 4 deletions multibody/plant/multibody_plant.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2803,8 +2803,8 @@ void MultibodyPlant<T>::CalcReactionForces(

// Now we need to shift the application point from Jp to Jc.
// First we need to find the position vector p_JpJc_W.
const RigidTransform<T> X_WJp = frame_Jp.CalcPoseInWorld(context);
const RotationMatrix<T>& R_WJp = X_WJp.rotation();
const RotationMatrix<T> R_WJp =
frame_Jp.CalcRotationMatrixInWorld(context);
const RigidTransform<T> X_JpJc = frame_Jc.CalcPose(context, frame_Jp);
const Vector3<T> p_JpJc_Jp = X_JpJc.translation();
const Vector3<T> p_JpJc_W = R_WJp * p_JpJc_Jp;
Expand All @@ -2814,8 +2814,8 @@ void MultibodyPlant<T>::CalcReactionForces(
}

// Re-express in the joint's child frame Jc.
const RigidTransform<T> X_WJc = frame_Jc.CalcPoseInWorld(context);
const RotationMatrix<T> R_JcW = X_WJc.rotation().transpose();
const RotationMatrix<T> R_WJc = frame_Jc.CalcRotationMatrixInWorld(context);
const RotationMatrix<T> R_JcW = R_WJc.inverse();
F_CJc_Jc_array->at(joint_index) = R_JcW * F_CJc_W;
}
}
Expand Down
14 changes: 14 additions & 0 deletions multibody/plant/test/frame_kinematics_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,12 @@ TEST_F(KukaIiwaModelTests, FramesKinematics) {
X_WH.GetAsMatrix34(), X_WH_expected.GetAsMatrix34(),
kTolerance, MatrixCompareType::relative));

const RotationMatrix<double> R_WH =
frame_H_->CalcRotationMatrixInWorld(*context_);
const RotationMatrix<double> R_WH_expected = X_WH_expected.rotation();
EXPECT_TRUE(CompareMatrices(R_WH.matrix(), R_WH_expected.matrix(), kTolerance,
MatrixCompareType::relative));

const Body<double>& link3 = plant_->GetBodyByName("iiwa_link_3");
const RigidTransform<double> X_HL3 =
link3.body_frame().CalcPose(*context_, *frame_H_);
Expand All @@ -40,6 +46,14 @@ TEST_F(KukaIiwaModelTests, FramesKinematics) {
X_HL3.GetAsMatrix34(), X_HL3_expected.GetAsMatrix34(),
kTolerance, MatrixCompareType::relative));

const RotationMatrix<double> R_HL3 =
link3.body_frame().CalcRotationMatrix(*context_, *frame_H_);
const RotationMatrix<double> R_WL3 =
link3.body_frame().CalcRotationMatrixInWorld(*context_);
const RotationMatrix<double> R_HL3_expected = R_WH.inverse() * R_WL3;
EXPECT_TRUE(CompareMatrices(R_HL3.matrix(), R_HL3_expected.matrix(),
kTolerance, MatrixCompareType::relative));

const SpatialVelocity<double> V_WE =
end_effector_link_->EvalSpatialVelocityInWorld(*context_);
const SpatialVelocity<double> V_WH =
Expand Down
24 changes: 22 additions & 2 deletions multibody/tree/body.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,21 +72,41 @@ class BodyFrame final : public Frame<T> {
return math::RigidTransform<T>::Identity();
}

math::RotationMatrix<T> CalcRotationMatrixInBodyFrame(
const systems::Context<T>&) const override {
return math::RotationMatrix<T>::Identity();
}

math::RigidTransform<T> CalcOffsetPoseInBody(
const systems::Context<T>&,
const math::RigidTransform<T>& X_FQ) const override {
return X_FQ;
}

math::RotationMatrix<T> CalcOffsetRotationMatrixInBody(
const systems::Context<T>&,
const math::RotationMatrix<T>& R_FQ) const override {
return R_FQ;
}

math::RigidTransform<T> GetFixedPoseInBodyFrame() const override {
return math::RigidTransform<T>::Identity();
}

math::RotationMatrix<T> GetFixedRotationMatrixInBodyFrame() const override {
return math::RotationMatrix<T>::Identity();
}

math::RigidTransform<T> GetFixedOffsetPoseInBody(
const math::RigidTransform<T>& X_FQ) const override {
return X_FQ;
}

math::RotationMatrix<T> GetFixedRotationMatrixInBody(
const math::RotationMatrix<T>& R_FQ) const override {
return R_FQ;
}

protected:
// Frame<T>::DoCloneToScalar() overrides.
std::unique_ptr<Frame<double>> DoCloneToScalar(
Expand Down Expand Up @@ -327,8 +347,8 @@ class Body : public MultibodyElement<Body, T, BodyIndex> {
DRAKE_THROW_UNLESS(forces != nullptr);
DRAKE_THROW_UNLESS(
forces->CheckHasRightSizeForModel(this->get_parent_tree()));
const math::RigidTransform<T> X_WE = frame_E.CalcPoseInWorld(context);
const math::RotationMatrix<T>& R_WE = X_WE.rotation();
const math::RotationMatrix<T> R_WE =
frame_E.CalcRotationMatrixInWorld(context);
const Vector3<T> p_PB_W = -(R_WE * p_BP_E);
const SpatialForce<T> F_Bo_W = (R_WE * F_Bp_E).Shift(p_PB_W);
AddInForceInWorld(context, F_Bo_W, forces);
Expand Down
51 changes: 30 additions & 21 deletions multibody/tree/body_node.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -341,7 +341,7 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {
// CalcPositionKinematicsCache_BaseToTip() and saving the result in the
// position kinematics cache.
/* p_PB_W = R_WP * p_PB */
const math::RotationMatrix<T>& R_WP = get_X_WP(pc).rotation();
const math::RotationMatrix<T>& R_WP = get_R_WP(pc);
const Vector3<T> p_PB_W = R_WP * get_X_PB(pc).translation();

// Since we are in a base-to-tip recursion the parent body P's spatial
Expand Down Expand Up @@ -477,18 +477,19 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {

// TODO(amcastro-tri): consider caching these. Especially true if bodies are
// flexible. Also used in velocity kinematics.
const math::RigidTransform<T> X_PF = frame_F.CalcPoseInBodyFrame(context);
const math::RotationMatrix<T> R_PF =
frame_F.CalcRotationMatrixInBodyFrame(context);
const math::RigidTransform<T> X_MB =
frame_M.CalcPoseInBodyFrame(context).inverse();

// Pose of the parent body P in world frame W.
// Form the rotation matrix relating the world frame W and parent body P.
// Available since we are called within a base-to-tip recursion.
const math::RigidTransform<T>& X_WP = get_X_WP(pc);
const math::RotationMatrix<T>& R_WP = get_R_WP(pc);

// Orientation (rotation) of frame F with respect to the world frame W.
// TODO(amcastro-tri): consider caching X_WF since it is also used to
// compute H_PB_W.
const math::RotationMatrix<T> R_WF = X_WP.rotation() * X_PF.rotation();
const math::RotationMatrix<T> R_WF = R_WP * R_PF;

// Vector from Mo to Bo expressed in frame F as needed below:
// TODO(amcastro-tri): consider caching this since it is also used to
Expand Down Expand Up @@ -516,7 +517,6 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {
// TODO(amcastro-tri): consider computing p_PB_W in
// CalcPositionKinematicsCache_BaseToTip() and saving the result in the
// position kinematics cache.
const math::RotationMatrix<T>& R_WP = get_X_WP(pc).rotation();
const Vector3<T>& p_PB_P = get_X_PB(pc).translation();
const Vector3<T> p_PB_W = R_WP * p_PB_P;

Expand Down Expand Up @@ -757,13 +757,13 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {

// Re-express F_BMo_W in the inboard frame F before projecting it onto the
// sub-space generated by H_FM(q).
const math::RigidTransform<T> X_PF =
inboard_frame().CalcPoseInBodyFrame(context);
const math::RigidTransform<T>& X_WP = get_X_WP(pc);
// TODO(amcastro-tri): consider caching X_WF since also used in position and
const math::RotationMatrix<T> R_PF =
inboard_frame().CalcRotationMatrixInBodyFrame(context);
const math::RotationMatrix<T>& R_WP = get_R_WP(pc);
// TODO(amcastro-tri): consider caching R_WF since also used in position and
// velocity kinematics.
const math::RotationMatrix<T> R_WF = X_WP.rotation() * X_PF.rotation();
const SpatialForce<T> F_BMo_F = R_WF.transpose() * F_BMo_W;
const math::RotationMatrix<T> R_WF = R_WP * R_PF;
const SpatialForce<T> F_BMo_F = R_WF.inverse() * F_BMo_W;

// Generalized velocities and forces use the same indexing.
auto tau = get_mutable_generalized_forces_from_array(tau_array);
Expand Down Expand Up @@ -813,15 +813,16 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {
// Outboard frame M of this node's mobilizer.
const Frame<T>& frame_M = outboard_frame();

const math::RigidTransform<T> X_PF = frame_F.CalcPoseInBodyFrame(context);
const math::RotationMatrix<T> R_PF =
frame_F.CalcRotationMatrixInBodyFrame(context);
const math::RigidTransform<T> X_MB =
frame_M.CalcPoseInBodyFrame(context).inverse();

// Pose of the parent body P in world frame W.
const math::RigidTransform<T>& X_WP = get_X_WP(pc);
// Form the rotation matrix relating the world frame W and parent body P.
const math::RotationMatrix<T>& R_WP = get_R_WP(pc);

// Orientation (rotation) of frame F with respect to the world frame W.
const math::RotationMatrix<T> R_WF = X_WP.rotation() * X_PF.rotation();
const math::RotationMatrix<T> R_WF = R_WP * R_PF;

// Vector from Mo to Bo expressed in frame F as needed below:
const math::RotationMatrix<T>& R_FM = get_X_FM(pc).rotation();
Expand Down Expand Up @@ -1146,16 +1147,17 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {
const Frame<T>& frame_F = inboard_frame();
const Frame<T>& frame_M = outboard_frame();

// Compute X_PF and X_MB.
const math::RigidTransform<T> X_PF = frame_F.CalcPoseInBodyFrame(context);
// Compute R_PF and X_MB.
const math::RotationMatrix<T> R_PF =
frame_F.CalcRotationMatrixInBodyFrame(context);
const math::RigidTransform<T> X_MB =
frame_M.CalcPoseInBodyFrame(context).inverse();

// Parent position in the world is available from the position kinematics.
const math::RigidTransform<T>& X_WP = get_X_WP(pc);
const math::RotationMatrix<T>& R_WP = get_R_WP(pc);

// TODO(amcastro-tri): consider caching R_WF.
const math::RotationMatrix<T> R_WF = X_WP.rotation() * X_PF.rotation();
const math::RotationMatrix<T> R_WF = R_WP * R_PF;

// Compute shift vector p_MoBo_F.
const Vector3<T> p_MoBo_F = get_X_FM(pc).rotation() * X_MB.translation();
Expand Down Expand Up @@ -1301,7 +1303,7 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {
// Compute shift vector p_PoBo_W from the parent origin to the body origin.
// TODO(amcastro-tri): Consider getting this value from cache.
const Vector3<T>& p_PoBo_P = get_X_PB(pc).translation();
const math::RotationMatrix<T>& R_WP = get_X_WP(pc).rotation();
const math::RotationMatrix<T>& R_WP = get_R_WP(pc);
const Vector3<T> p_PoBo_W = R_WP * p_PoBo_P;

const int nv = get_num_mobilizer_velocities();
Expand Down Expand Up @@ -1400,6 +1402,13 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {
return pc.get_X_WB(topology_.parent_body_node);
}

// Returns a const reference to the rotation matrix `R_WP` that relates the
// orientation of the world frame W to the parent frame P.
const math::RotationMatrix<T>& get_R_WP(
const PositionKinematicsCache<T>& pc) const {
return pc.get_R_WB(topology_.parent_body_node);
}

// Returns a constant reference to the across-mobilizer pose of the outboard
// frame M as measured and expressed in the inboard frame F.
const math::RigidTransform<T>& get_X_FM(
Expand Down
14 changes: 14 additions & 0 deletions multibody/tree/fixed_offset_frame.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,11 +83,25 @@ class FixedOffsetFrame final : public Frame<T> {
return parent_frame_.CalcOffsetPoseInBody(context, X_PF_.cast<T>());
}

math::RotationMatrix<T> CalcRotationMatrixInBodyFrame(
const systems::Context<T>& context) const override {
// R_BF = R_BP * R_PF
const math::RotationMatrix<double>& R_PF = X_PF_.rotation();
return parent_frame_.CalcOffsetRotationMatrixInBody(context,
R_PF.cast<T>());
}

math::RigidTransform<T> GetFixedPoseInBodyFrame() const override {
// X_BF = X_BP * X_PF
return parent_frame_.GetFixedOffsetPoseInBody(X_PF_.cast<T>());
}

math::RotationMatrix<T> GetFixedRotationMatrixInBodyFrame() const override {
// R_BF = R_BP * R_PF
const math::RotationMatrix<double>& R_PF = X_PF_.rotation();
return parent_frame_.GetFixedRotationMatrixInBody(R_PF.cast<T>());
}

protected:
/// @name Methods to make a clone templated on different scalar types.
///
Expand Down
64 changes: 61 additions & 3 deletions multibody/tree/frame.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,12 @@ class Frame : public FrameBase<T> {
virtual math::RigidTransform<T> CalcPoseInBodyFrame(
const systems::Context<T>& context) const = 0;

/// Returns the rotation matrix `R_BF` that relates body frame B to `this`
/// frame F (B is the body frame to which `this` frame F is attached).
/// @note If `this` is B, this method returns the identity RotationMatrix.
virtual math::RotationMatrix<T> CalcRotationMatrixInBodyFrame(
const systems::Context<T>& context) const = 0;

/// Variant of CalcPoseInBodyFrame() that returns the fixed pose `X_BF` of
/// `this` frame F in the body frame B associated with this frame.
/// @throws std::logic_error if called on a %Frame that does not have a
Expand All @@ -77,6 +83,21 @@ class Frame : public FrameBase<T> {
"', which does not support this operation.");
}

/// Returns the rotation matrix `R_BF` that relates body frame B to `this`
/// frame F (B is the body frame to which `this` frame F is attached).
/// @throws std::logic_error if `this` frame F is a %Frame that does not have
/// a fixed offset in the body frame B (i.e., `R_BF` is not constant).
/// %Frame sub-classes that have a constant `R_BF` must override this method.
/// An example of a frame sub-class not implementing this method would be that
/// of a frame on a soft body, for which its pose in the body frame depends
/// on the state of deformation of the body.
virtual math::RotationMatrix<T> GetFixedRotationMatrixInBodyFrame() const {
throw std::logic_error(
"Unable to retrieve a fixed rotation matrix from a frame of type '" +
drake::NiceTypeName::Get(*this) +
"', which does not support this method.");
}

/// Given the offset pose `X_FQ` of a frame Q in `this` frame F, this method
/// computes the pose `X_BQ` of frame Q in the body frame B to which this
/// frame is attached.
Expand All @@ -93,6 +114,16 @@ class Frame : public FrameBase<T> {
return CalcPoseInBodyFrame(context) * X_FQ;
}

/// Calculates and returns the rotation matrix `R_BQ` that relates body frame
/// B to frame Q via `this` intermediate frame F, i.e., `R_BQ = R_BF * R_FQ`
/// (B is the body frame to which `this` frame F is attached).
/// @param[in] R_FQ rotation matrix that relates frame F to frame Q.
virtual math::RotationMatrix<T> CalcOffsetRotationMatrixInBody(
const systems::Context<T>& context,
const math::RotationMatrix<T>& R_FQ) const {
return CalcRotationMatrixInBodyFrame(context) * R_FQ;
}

/// Variant of CalcOffsetPoseInBody() that given the offset pose `X_FQ` of a
/// frame Q in `this` frame F, returns the pose `X_BQ` of frame Q in the body
/// frame B to which this frame is attached.
Expand All @@ -103,6 +134,17 @@ class Frame : public FrameBase<T> {
return GetFixedPoseInBodyFrame() * X_FQ;
}

/// Calculates and returns the rotation matrix `R_BQ` that relates body frame
/// B to frame Q via `this` intermediate frame F, i.e., `R_BQ = R_BF * R_FQ`
/// (B is the body frame to which `this` frame F is attached).
/// @param[in] R_FQ rotation matrix that relates frame F to frame Q.
/// @throws std::logic_error if `this` frame F is a %Frame that does not have
/// a fixed offset in the body frame B (i.e., `R_BF` is not constant).
virtual math::RotationMatrix<T> GetFixedRotationMatrixInBody(
const math::RotationMatrix<T>& R_FQ) const {
return GetFixedRotationMatrixInBodyFrame() * R_FQ;
}

/// Computes and returns the pose `X_WF` of `this` frame F in the world
/// frame W as a function of the state of the model stored in `context`.
/// @note Body::EvalPoseInWorld() provides a more efficient way to obtain
Expand All @@ -122,6 +164,22 @@ class Frame : public FrameBase<T> {
context, frame_M, *this);
}

/// Calculates and returns the rotation matrix `R_MF` that relates `frame_M`
/// and `this` frame F as a function of the state stored in `context`.
math::RotationMatrix<T> CalcRotationMatrix(
const systems::Context<T>& context, const Frame<T>& frame_M) const {
return this->get_parent_tree().CalcRelativeRotationMatrix(
context, frame_M, *this);
}

/// Calculates and returns the rotation matrix `R_WF` that relates the world
/// frame W and `this` frame F as a function of the state stored in `context`.
math::RotationMatrix<T> CalcRotationMatrixInWorld(
const systems::Context<T>& context) const {
return this->get_parent_tree().CalcRelativeRotationMatrix(
context, this->get_parent_tree().world_frame(), *this);
}

/// Computes and returns the spatial velocity `V_WF` of `this` frame F in the
/// world frame W as a function of the state of the model stored in `context`.
/// @note Body::EvalSpatialVelocityInWorld() provides a more efficient way to
Expand All @@ -145,7 +203,7 @@ class Frame : public FrameBase<T> {
const systems::Context<T>& context,
const Frame<T>& frame_M, const Frame<T>& frame_E) const {
const math::RotationMatrix<T> R_WM =
frame_M.CalcPoseInWorld(context).rotation();
frame_M.CalcRotationMatrixInWorld(context);
const Vector3<T> p_MF_M = this->CalcPose(context, frame_M).translation();
const Vector3<T> p_MF_W = R_WM * p_MF_M;
const SpatialVelocity<T> V_WM = frame_M.CalcSpatialVelocityInWorld(context);
Expand All @@ -156,8 +214,8 @@ class Frame : public FrameBase<T> {
// If expressed-in frame_E is not the world, perform additional
// transformation.
const math::RotationMatrix<T> R_WE =
frame_E.CalcPoseInWorld(context).rotation();
return R_WE.transpose() * V_MF_W;
frame_E.CalcRotationMatrixInWorld(context);
return R_WE.inverse() * V_MF_W;
}

/// (Advanced) NVI to DoCloneToScalar() templated on the scalar type of the
Expand Down
Loading

0 comments on commit f6564bd

Please sign in to comment.