Skip to content

Commit

Permalink
Caches shift vector p_PoBo_W. (RobotLocomotion#12724)
Browse files Browse the repository at this point in the history
  • Loading branch information
@amcastro-tri
amcastro-tri authored Feb 12, 2020
1 parent 8d34a1c commit a17a77f
Show file tree
Hide file tree
Showing 2 changed files with 58 additions and 48 deletions.
69 changes: 26 additions & 43 deletions multibody/tree/body_node.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -337,12 +337,7 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {

// Shift vector between the parent body P and this node's body B,
// expressed in the world frame W.
// TODO(amcastro-tri): consider computing p_PB_W in
// 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_R_WP(pc);
const Vector3<T> p_PB_W = R_WP * get_X_PB(pc).translation();
const Vector3<T>& p_PB_W = get_p_PoBo_W(pc);

// Since we are in a base-to-tip recursion the parent body P's spatial
// velocity is already available in the cache.
Expand Down Expand Up @@ -514,11 +509,7 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {

// Shift vector between the parent body P and this node's body B,
// expressed in the world frame W.
// TODO(amcastro-tri): consider computing p_PB_W in
// CalcPositionKinematicsCache_BaseToTip() and saving the result in the
// position kinematics cache.
const Vector3<T>& p_PB_P = get_X_PB(pc).translation();
const Vector3<T> p_PB_W = R_WP * p_PB_P;
const Vector3<T>& p_PB_W = get_p_PoBo_W(pc);

if (vc != nullptr) {
// Since we are in a base-to-tip recursion the parent body P's spatial
Expand Down Expand Up @@ -717,10 +708,8 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {
for (const BodyNode<T>* child_node : children_) {
BodyNodeIndex child_node_index = child_node->index();

// Pose of child body C in this node's body frame B.
const math::RigidTransform<T>& X_BC = child_node->get_X_PB(pc);
const Vector3<T>& p_BoCo_B = X_BC.translation();
const Vector3<T> p_BoCo_W = R_WB * p_BoCo_B;
// Shift vector from Bo to Co, expressed in the world frame W.
const Vector3<T>& p_BoCo_W = child_node->get_p_PoBo_W(pc);

// p_CoMc_W:
const Frame<T>& frame_Mc = child_node->outboard_frame();
Expand Down Expand Up @@ -993,22 +982,15 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {
// = P_B_W - g_PB_W H_PB_Wᵀ P_B_W
// = P_B_W - g_PB_W * U_B_W (7)

// Get pose of B in W and its rotation matrix R_WB.
const math::RigidTransform<T>& X_WB = get_X_WB(pc);
const math::RotationMatrix<T>& R_WB = X_WB.rotation();

// Compute articulated body inertia for body using (1).
ArticulatedBodyInertia<T>& P_B_W = get_mutable_P_B_W(abic);
P_B_W = ArticulatedBodyInertia<T>(M_B_W);

// Add articulated body inertia contributions from all children.
for (const BodyNode<T>* child : children_) {
// Get X_BC (which is X_PB for child).
const math::RigidTransform<T>& X_BC = child->get_X_PB(pc);

// Compute shift vector p_CoBo_W.
const Vector3<T> p_CoBo_B = -X_BC.translation();
const Vector3<T> p_CoBo_W = R_WB * p_CoBo_B;
// Shift vector p_CoBo_W.
const Vector3<T>& p_BoCo_W = child->get_p_PoBo_W(pc);
const Vector3<T> p_CoBo_W = -p_BoCo_W;

// Pull Pplus_BC_W from cache (which is Pplus_PB_W for child).
const ArticulatedBodyInertia<T>& Pplus_BC_W
Expand Down Expand Up @@ -1134,12 +1116,6 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {
// internal_forward_dynamics for a detailed description of the algorithm and
// notation inuse.

// Get pose of B in W.
const math::RigidTransform<T>& X_WB = get_X_WB(pc);

// Get R_WB.
const math::RotationMatrix<T>& R_WB = X_WB.rotation();

SpatialAcceleration<T>& Ab_WB = get_mutable_Ab_WB(aba_force_bias_cache);
Ab_WB.SetZero();
if (vc != nullptr) {
Expand Down Expand Up @@ -1214,12 +1190,9 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {

// Add residual spatial force contributions from all children.
for (const BodyNode<T>* child : children_) {
// Get X_BC (which is X_PB for child).
const math::RigidTransform<T>& X_BC = child->get_X_PB(pc);

// Compute shift vector p_CoBo_W.
const Vector3<T> p_CoBo_B = -X_BC.translation();
const Vector3<T> p_CoBo_W = R_WB * p_CoBo_B;
// Shift vector from Co to Bo.
const Vector3<T>& p_BoCo_W = child->get_p_PoBo_W(pc);
const Vector3<T> p_CoBo_W = -p_BoCo_W;

// Pull Zplus_BC_W from cache (which is Zplus_PB_W for child).
const SpatialForce<T>& Zplus_BC_W =
Expand Down Expand Up @@ -1298,13 +1271,10 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {
// and the notation in use.

// Get the spatial acceleration of the parent.
const SpatialAcceleration<T> A_WP = parent_node_->get_A_WB(*ac);
const SpatialAcceleration<T>& A_WP = parent_node_->get_A_WB(*ac);

// 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_R_WP(pc);
const Vector3<T> p_PoBo_W = R_WP * p_PoBo_P;
// Shift vector p_PoBo_W from the parent origin to the body origin.
const Vector3<T>& p_PoBo_W = get_p_PoBo_W(pc);

const int nv = get_num_mobilizer_velocities();

Expand Down Expand Up @@ -1437,6 +1407,14 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {
return pc->get_mutable_X_PB(topology_.index);
}

const Vector3<T>& get_p_PoBo_W(const PositionKinematicsCache<T>& pc) const {
return pc.get_p_PoBo_W(topology_.index);
}

Vector3<T>& get_mutable_p_PoBo_W(PositionKinematicsCache<T>* pc) const {
return pc->get_mutable_p_PoBo_W(topology_.index);
}

// =========================================================================
// VelocityKinematicsCache Accessors and Mutators.

Expand Down Expand Up @@ -1754,6 +1732,11 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {
X_PB = frame_F.CalcOffsetPoseInBody(context, X_FB);

X_WB = X_WP * X_PB;

// Compute shift vector p_PoBo_W from the parent origin to the body origin.
const Vector3<T>& p_PoBo_P = X_PB.translation();
const math::RotationMatrix<T>& R_WP = X_WP.rotation();
get_mutable_p_PoBo_W(pc) = R_WP * p_PoBo_P;
}

// Computes position dependent kinematics associated with `this` mobilizer
Expand Down
37 changes: 32 additions & 5 deletions multibody/tree/position_kinematics_cache.h
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,8 @@
#pragma once

#include <limits>
#include <vector>

#include "drake/common/default_scalars.h"
#include "drake/common/drake_assert.h"
#include "drake/common/drake_copyable.h"
Expand Down Expand Up @@ -115,6 +118,19 @@ class PositionKinematicsCache {
return X_FM_pool_[body_node_index];
}

/// Position of node B, with index `body_node_index`, measured in the inboard
/// body frame P, expressed in the world frame W.
const Vector3<T>& get_p_PoBo_W(BodyNodeIndex body_node_index) const {
DRAKE_ASSERT(0 <= body_node_index && body_node_index < num_nodes_);
return p_PoBo_W_pool_[body_node_index];
}

/// Mutable version of get_p_PoBo_W().
Vector3<T>& get_mutable_p_PoBo_W(BodyNodeIndex body_node_index) {
DRAKE_ASSERT(0 <= body_node_index && body_node_index < num_nodes_);
return p_PoBo_W_pool_[body_node_index];
}

private:
// Pool types:
// Pools store entries in the same order multibody tree nodes are
Expand All @@ -123,7 +139,10 @@ class PositionKinematicsCache {
// `get_X_WB()` for instance.

// The type of pools for storing poses.
typedef eigen_aligned_std_vector<RigidTransform<T>> X_PoolType;
typedef std::vector<RigidTransform<T>> X_PoolType;

// The type of pools for storing 3D vectors.
typedef std::vector<Vector3<T>> Vector3PoolType;

// Allocates resources for this position kinematics cache.
void Allocate() {
Expand All @@ -140,6 +159,12 @@ class PositionKinematicsCache {

X_MB_pool_.resize(num_nodes_);
X_MB_pool_[world_index()] = NaNPose(); // It should never be used.

p_PoBo_W_pool_.resize(num_nodes_);
// p_PoBo_W for the world body should never be used.
p_PoBo_W_pool_[world_index()].setConstant(
std::numeric_limits<
typename Eigen::NumTraits<T>::Literal>::quiet_NaN());
}

// Helper method to initialize poses to garbage values including NaNs.
Expand All @@ -154,10 +179,12 @@ class PositionKinematicsCache {

// Number of body nodes in the corresponding MultibodyTree.
int num_nodes_{0};
X_PoolType X_WB_pool_; // Indexed by BodyNodeIndex.
X_PoolType X_PB_pool_; // Indexed by BodyNodeIndex.
X_PoolType X_FM_pool_; // Indexed by BodyNodeIndex.
X_PoolType X_MB_pool_; // Indexed by BodyNodeIndex.
// Pools indexed by BodyNodeIndex.
X_PoolType X_WB_pool_;
X_PoolType X_PB_pool_;
X_PoolType X_FM_pool_;
X_PoolType X_MB_pool_;
Vector3PoolType p_PoBo_W_pool_;
};

DRAKE_DEFINE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN_T(PositionKinematicsCache)
Expand Down

0 comments on commit a17a77f

Please sign in to comment.