Skip to content

Commit

Permalink
Allow model_instances for MBP::CalcJacobianCenterOfMassTranslationalV…
Browse files Browse the repository at this point in the history 
…elocity() (RobotLocomotion#15387)
  • Loading branch information
@mitiguy
mitiguy authored Jul 21, 2021
1 parent ac38993 commit cff0f37
Show file tree
Hide file tree
Showing 6 changed files with 249 additions and 54 deletions.
2 changes: 1 addition & 1 deletion bindings/pydrake/multibody/plant_py.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -334,7 +334,7 @@ void DoScalarDependentDefinitions(py::module m, T) {
},
py::arg("context"), py::arg("with_respect_to"), py::arg("frame_A"),
py::arg("frame_E"),
cls_doc.CalcJacobianCenterOfMassTranslationalVelocity.doc)
cls_doc.CalcJacobianCenterOfMassTranslationalVelocity.doc_5args)
.def("GetFreeBodyPose", &Class::GetFreeBodyPose, py::arg("context"),
py::arg("body"), cls_doc.GetFreeBodyPose.doc)
.def("SetFreeBodyPose",
Expand Down
63 changes: 51 additions & 12 deletions multibody/plant/multibody_plant.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3206,12 +3206,11 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
Js_v_ABi_E);
}

/// This method computes J𝑠_v_ACcm_E, point Ccm's translational velocity
/// Jacobian in frame A with respect to "speeds" 𝑠, expressed in frame E,
/// where point Ccm is the composite center of mass of the system of all
/// bodies in the MultibodyPlant (except world_body()).
///
/// @param[in] context The state of the multibody system.
/// Calculates J𝑠_v_ACcm_E, point Ccm's translational velocity Jacobian in
/// frame A with respect to "speeds" 𝑠, expressed in frame E, where point CCm
/// is the center of mass of the system of all non-world bodies contained in
/// `this` MultibodyPlant.
/// @param[in] context contains the state of the model.
/// @param[in] with_respect_to Enum equal to JacobianWrtVariable::kQDot or
/// JacobianWrtVariable::kV, indicating whether the Jacobian `J𝑠_v_ACcm_E` is
/// partial derivatives with respect to 𝑠 = q̇ (time-derivatives of generalized
Expand All @@ -3227,20 +3226,60 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
/// pulled from the context).
/// @throws std::exception if CCm does not exist, which occurs if there
/// are no massive bodies in MultibodyPlant (except world_body()).
/// @throws std::exception if composite_mass <= 0, where composite_mass is
/// the total mass of all bodies except world_body() in MultibodyPlant.
/// @throws std::exception if mₛ ≤ 0 (where mₛ is the mass of all non-world
/// bodies contained in `this` MultibodyPlant).
void CalcJacobianCenterOfMassTranslationalVelocity(
const systems::Context<T>& context, JacobianWrtVariable with_respect_to,
const Frame<T>& frame_A, const Frame<T>& frame_E,
const systems::Context<T>& context,
JacobianWrtVariable with_respect_to,
const Frame<T>& frame_A,
const Frame<T>& frame_E,
EigenPtr<Matrix3X<T>> Js_v_ACcm_E) const {
// TODO(yangwill): Add an optional parameter to calculate this for a
// subset of bodies instead of the full system
this->ValidateContext(context);
DRAKE_DEMAND(Js_v_ACcm_E != nullptr);
internal_tree().CalcJacobianCenterOfMassTranslationalVelocity(
context, with_respect_to, frame_A, frame_E, Js_v_ACcm_E);
}

/// Calculates J𝑠_v_ACcm_E, point Ccm's translational velocity Jacobian in
/// frame A with respect to "speeds" 𝑠, expressed in frame E, where point CCm
/// is the center of mass of the system of all non-world bodies contained in
/// model_instances.
/// @param[in] context contains the state of the model.
/// @param[in] model_instances Vector of selected model instances. If a model
/// instance is repeated in the vector (unusual), it is only counted once.
/// @param[in] with_respect_to Enum equal to JacobianWrtVariable::kQDot or
/// JacobianWrtVariable::kV, indicating whether the Jacobian `J𝑠_v_ACcm_E` is
/// partial derivatives with respect to 𝑠 = q̇ (time-derivatives of generalized
/// positions) or with respect to 𝑠 = v (generalized velocities).
/// @param[in] frame_A The frame in which the translational velocity
/// v_ACcm and its Jacobian J𝑠_v_ACcm are measured.
/// @param[in] frame_E The frame in which the Jacobian J𝑠_v_ACcm is
/// expressed on output.
/// @param[out] J𝑠_v_ACcm_E Point Ccm's translational velocity Jacobian in
/// frame A with respect to speeds 𝑠 (𝑠 = q̇ or 𝑠 = v), expressed in frame E.
/// J𝑠_v_ACcm_E is a 3 x n matrix, where n is the number of elements in 𝑠.
/// The Jacobian is a function of only generalized positions q (which are
/// pulled from the context).
/// @throws std::exception if mₛ ≤ 0 (where mₛ is the mass of all non-world
/// bodies contained in model_instances).
/// @throws std::exception if model_instances is empty or only has world body.
/// @note The world_body() is ignored. J𝑠_v_ACcm_ = ∑ (mᵢ Jᵢ) / mₛ, where
/// mₛ = ∑ mᵢ, mᵢ is the mass of the iᵗʰ body contained in model_instances,
/// and Jᵢ is Bcm's translational velocity Jacobian in frame A, expressed in
/// frame E (Bcm is the center of mass of the iᵗʰ body).
void CalcJacobianCenterOfMassTranslationalVelocity(
const systems::Context<T>& context,
const std::vector<ModelInstanceIndex>& model_instances,
JacobianWrtVariable with_respect_to,
const Frame<T>& frame_A,
const Frame<T>& frame_E,
EigenPtr<Matrix3X<T>> Js_v_ACcm_E) const {
this->ValidateContext(context);
DRAKE_DEMAND(Js_v_ACcm_E != nullptr);
internal_tree().CalcJacobianCenterOfMassTranslationalVelocity(context,
model_instances, with_respect_to, frame_A, frame_E, Js_v_ACcm_E);
}

/// Calculates abias_ACcm_E, point Ccm's translational "bias" acceleration
/// term in frame A with respect to "speeds" 𝑠, expressed in frame E, where
/// point Ccm is the composite center of mass of the system of all bodies
Expand Down
39 changes: 36 additions & 3 deletions multibody/plant/test/multibody_plant_com_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -236,6 +236,15 @@ TEST_F(MultibodyPlantCenterOfMassTest, CenterOfMassPosition) {
"CalcCenterOfMassTranslationalVelocityInWorld\\(\\): There must be at "
"least one non-world body contained in model_instances.");

Eigen::MatrixXd Js_v_WCcm_W(3, plant_.num_velocities());
const Frame<double>& frame_W = plant_.world_frame();
DRAKE_EXPECT_THROWS_MESSAGE(
plant_.CalcJacobianCenterOfMassTranslationalVelocity(
*context_, model_instances, JacobianWrtVariable::kV,
frame_W, frame_W, &Js_v_WCcm_W),
"CalcJacobianCenterOfMassTranslationalVelocity\\(\\): There must be at "
"least one non-world body contained in model_instances.");

// Ensure an exception is thrown when a model instance has one world body.
const ModelInstanceIndex world_model_instance =
multibody::world_model_instance();
Expand All @@ -247,6 +256,13 @@ TEST_F(MultibodyPlantCenterOfMassTest, CenterOfMassPosition) {
"CalcCenterOfMassTranslationalVelocityInWorld\\(\\): There must be at "
"least one non-world body contained in model_instances.");

DRAKE_EXPECT_THROWS_MESSAGE(
plant_.CalcJacobianCenterOfMassTranslationalVelocity(
*context_, model_instances, JacobianWrtVariable::kV,
frame_W, frame_W, &Js_v_WCcm_W),
"CalcJacobianCenterOfMassTranslationalVelocity\\(\\): There must be at "
"least one non-world body contained in model_instances.");

// Ensure an exception is thrown when a model instance has two world bodies.
world_model_instance_array.push_back(world_model_instance);
DRAKE_EXPECT_THROWS_MESSAGE(
Expand All @@ -255,7 +271,15 @@ TEST_F(MultibodyPlantCenterOfMassTest, CenterOfMassPosition) {
"CalcCenterOfMassTranslationalVelocityInWorld\\(\\): There must be at "
"least one non-world body contained in model_instances.");

// Try one instance in model_instances.
DRAKE_EXPECT_THROWS_MESSAGE(
plant_.CalcJacobianCenterOfMassTranslationalVelocity(
*context_, model_instances, JacobianWrtVariable::kV,
frame_W, frame_W, &Js_v_WCcm_W),
"CalcJacobianCenterOfMassTranslationalVelocity\\(\\): There must be at "
"least one non-world body contained in model_instances.");

// Verify CalcCenterOfMassPositionInWorld() works for 1 instance in
// model_instances.
model_instances.push_back(triangle_instance_);
p_WCcm = plant_.CalcCenterOfMassPositionInWorld(*context_, model_instances);
EXPECT_TRUE(CompareMatrices(p_WCcm, p_WTo_W + p_TTcm_T_, kTolerance));
Expand Down Expand Up @@ -290,8 +314,13 @@ TEST_F(MultibodyPlantCenterOfMassTest, CenterOfMassPosition) {
"CalcCenterOfMassTranslationalVelocityInWorld\\(\\): The "
"system's total mass must be greater than zero.");

Eigen::MatrixXd Js_v_WCcm_W(3, plant_.num_velocities());
const Frame<double>& frame_W = plant_.world_frame();
DRAKE_EXPECT_THROWS_MESSAGE(
plant_.CalcJacobianCenterOfMassTranslationalVelocity(
*context_, model_instances, JacobianWrtVariable::kV,
frame_W, frame_W, &Js_v_WCcm_W),
"CalcJacobianCenterOfMassTranslationalVelocity\\(\\): The "
"system's total mass must be greater than zero.");

DRAKE_EXPECT_THROWS_MESSAGE(
plant_.CalcJacobianCenterOfMassTranslationalVelocity(
*context_, JacobianWrtVariable::kV, frame_W, frame_W, &Js_v_WCcm_W),
Expand All @@ -313,6 +342,10 @@ TEST_F(MultibodyPlantCenterOfMassTest, CenterOfMassPosition) {
EXPECT_THROW(plant_.CalcCenterOfMassTranslationalVelocityInWorld(
*context_, model_instances),
std::exception);
EXPECT_THROW(plant_.CalcJacobianCenterOfMassTranslationalVelocity(
*context_, model_instances, JacobianWrtVariable::kV,
frame_W, frame_W, &Js_v_WCcm_W),
std::exception);
}

} // namespace
Expand Down
107 changes: 71 additions & 36 deletions multibody/plant/test/multibody_plant_jacobians_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -305,8 +305,10 @@ class TwoDOFPlanarPendulumTest : public ::testing::Test {

// Create an empty MultibodyPlant and then add the two links.
plant_ = std::make_unique<MultibodyPlant<double>>(0.0);
bodyA_ = &plant_->AddRigidBody("BodyA", M_Bcm);
bodyB_ = &plant_->AddRigidBody("BodyB", M_Bcm);
bodyA_instance_ = plant_->AddModelInstance("bodyAInstanceName");
bodyB_instance_ = plant_->AddModelInstance("bodyBInstanceName");
bodyA_ = &plant_->AddRigidBody("bodyA", bodyA_instance_, M_Bcm);
bodyB_ = &plant_->AddRigidBody("bodyB", bodyB_instance_, M_Bcm);

// Create a revolute joint that connects point Wo of the world frame to a
// unnamed point of link A that is a distance of link_length/2 from link A's
Expand Down Expand Up @@ -357,6 +359,8 @@ class TwoDOFPlanarPendulumTest : public ::testing::Test {
const RigidBody<double>* bodyB_{nullptr};
const RevoluteJoint<double>* joint1_{nullptr};
const RevoluteJoint<double>* joint2_{nullptr};
ModelInstanceIndex bodyA_instance_;
ModelInstanceIndex bodyB_instance_;
};

TEST_F(TwoDOFPlanarPendulumTest, CalcBiasAccelerations) {
Expand Down Expand Up @@ -457,40 +461,6 @@ TEST_F(TwoDOFPlanarPendulumTest,
joint1_->set_angular_rate(context_.get(), state[2]);
joint2_->set_angular_rate(context_.get(), state[3]);

// Test for CalcJacobianCenterOfMassTranslationalVelocity().
const Frame<double>& frame_W = plant_->world_frame();
Eigen::MatrixXd Js_v_WScm_W(3, plant_->num_velocities());
plant_->CalcJacobianCenterOfMassTranslationalVelocity(
*context_, JacobianWrtVariable::kV, frame_W, frame_W, &Js_v_WScm_W);

Eigen::MatrixXd Js_v_WScm_W_expected(3, plant_->num_velocities());
// Denoting Scm as the center of mass of the system formed by links A and B,
// Scm's velocity in world W is expected to be (L wAz_ + 0.25 L wBz_) Wy,
// hence Scm's translational Jacobian with respect to {wAz_ , wBz_} is
// { L Wy, 0.25 L Wy } = { [0, L, 0] [0, 0.25 L, 0] }
Js_v_WScm_W_expected << 0.0, 0.0,
link_length_, 0.25 * link_length_,
0.0, 0.0;
const Vector3d v_WScm_W_expected =
(wAz_ * link_length_ + wBz_ * 0.25 * link_length_) * Vector3d::UnitY();
EXPECT_TRUE(CompareMatrices(Js_v_WScm_W, Js_v_WScm_W_expected, kTolerance));
EXPECT_TRUE(
CompareMatrices(Js_v_WScm_W * state.tail(plant_->num_velocities()),
v_WScm_W_expected, kTolerance));

// Test for CalcBiasCenterOfMassTranslationalAcceleration()
const Vector3<double>& abias_WScm_W =
plant_->CalcBiasCenterOfMassTranslationalAcceleration(
*context_, JacobianWrtVariable::kV, plant_->world_frame(),
plant_->world_frame());

// Scm's bias translational in world W is expected to be
// abias_WScm = -L (wAz_² + 0.5 wAz_ wBz_ + 0.25 wBz_²) 𝐖𝐱
Vector3d abias_WScm_W_expected =
-link_length_ * (wAz_ * wAz_ + 0.5 * wAz_ * wBz_ + 0.25 * wBz_ * wBz_) *
Vector3d::UnitX();
EXPECT_TRUE(CompareMatrices(abias_WScm_W, abias_WScm_W_expected, kTolerance));

// Shortcuts to rigid bodies.
const RigidBody<double>& body_A = rigid_bodyA();
const RigidBody<double>& body_B = rigid_bodyB();
Expand All @@ -512,11 +482,76 @@ TEST_F(TwoDOFPlanarPendulumTest,
// Verify MultibodyPlant::CalcCenterOfMassTranslationalVelocityInWorld().
const Vector3d v_WScm_W =
plant_->CalcCenterOfMassTranslationalVelocityInWorld(*context_);
const Vector3d v_WScm_W_expected =
(wAz_ * link_length_ + wBz_ * 0.25 * link_length_) * Vector3d::UnitY();
EXPECT_TRUE(CompareMatrices(v_WScm_W, v_WScm_W_expected, kTolerance));
const Vector3d v_WScm_W_alternate = (mass_link_ * v_WAcm_W_expected
+ mass_link_ * v_WBcm_W_expected)
/ (2 * mass_link_);
EXPECT_TRUE(CompareMatrices(v_WScm_W, v_WScm_W_alternate, kTolerance));

// Test CalcJacobianCenterOfMassTranslationalVelocity() for full MBP.
const int num_velocities = plant_->num_velocities();
const Frame<double>& frame_W = plant_->world_frame();
Eigen::MatrixXd Js_v_WScm_W(3, num_velocities);
plant_->CalcJacobianCenterOfMassTranslationalVelocity(
*context_, JacobianWrtVariable::kV, frame_W, frame_W, &Js_v_WScm_W);

Eigen::MatrixXd Js_v_WScm_W_expected(3, num_velocities);
// Denoting Scm as the center of mass of the system formed by links A and B,
// Scm's velocity in world W is expected to be (L wAz_ + 0.25 L wBz_) Wy,
// hence Scm's translational Jacobian with respect to {wAz_ , wBz_} is
// { L Wy, 0.25 L Wy } = { [0, L, 0] [0, 0.25 L, 0] }
Js_v_WScm_W_expected << 0.0, 0.0,
link_length_, 0.25 * link_length_,
0.0, 0.0;
EXPECT_TRUE(CompareMatrices(Js_v_WScm_W, Js_v_WScm_W_expected, kTolerance));
EXPECT_TRUE(
CompareMatrices(Js_v_WScm_W * state.tail(num_velocities),
v_WScm_W_expected, kTolerance));

// Test CalcJacobianCenterOfMassTranslationalVelocity() for 1 model instance.
std::vector<ModelInstanceIndex> model_instances;
const ModelInstanceIndex bodyA_instance_index =
plant_->GetModelInstanceByName("bodyAInstanceName");
model_instances.push_back(bodyA_instance_index);
Eigen::MatrixXd Js_v_WAcm_W(3, num_velocities);
plant_->CalcJacobianCenterOfMassTranslationalVelocity(*context_,
model_instances, JacobianWrtVariable::kV, frame_W, frame_W, &Js_v_WAcm_W);

// Acm's velocity in world W is expected to be 0.5 L wAz_ Wy,
// hence Acm's translational Jacobian with respect to {wAz_ , wBz_} is
// { 0.5 L Wy, 0 } = { [0, 0.5 L, 0] [0, 0, 0] }
Eigen::MatrixXd Js_v_WAcm_W_expected(3, num_velocities);
Js_v_WAcm_W_expected << 0.0, 0.0,
0.5 * link_length_, 0.0,
0.0, 0.0;
EXPECT_TRUE(CompareMatrices(Js_v_WAcm_W, Js_v_WAcm_W_expected, kTolerance));
EXPECT_TRUE(
CompareMatrices(Js_v_WAcm_W * state.tail(num_velocities),
v_WAcm_W_expected, kTolerance));

// Test CalcJacobianCenterOfMassTranslationalVelocity() for 2 model instances.
// This should produce the same results as Scm (system center of mass).
const ModelInstanceIndex bodyB_instance_index =
plant_->GetModelInstanceByName("bodyBInstanceName");
model_instances.push_back(bodyB_instance_index);
plant_->CalcJacobianCenterOfMassTranslationalVelocity(*context_,
model_instances, JacobianWrtVariable::kV, frame_W, frame_W, &Js_v_WScm_W);
EXPECT_TRUE(CompareMatrices(Js_v_WAcm_W, Js_v_WAcm_W_expected, kTolerance));

// Test for CalcBiasCenterOfMassTranslationalAcceleration().
const Vector3<double>& abias_WScm_W =
plant_->CalcBiasCenterOfMassTranslationalAcceleration(
*context_, JacobianWrtVariable::kV, plant_->world_frame(),
plant_->world_frame());

// Scm's bias translational in world W is expected to be
// abias_WScm = -L (wAz_² + 0.5 wAz_ wBz_ + 0.25 wBz_²) 𝐖𝐱
Vector3d abias_WScm_W_expected =
-link_length_ * (wAz_ * wAz_ + 0.5 * wAz_ * wBz_ + 0.25 * wBz_ * wBz_) *
Vector3d::UnitX();
EXPECT_TRUE(CompareMatrices(abias_WScm_W, abias_WScm_W_expected, kTolerance));
}

// Fixture for two degree-of-freedom 3D satellite tracker with bodies A and B.
Expand Down
Loading

0 comments on commit cff0f37

Please sign in to comment.