Skip to content

Commit

Permalink
Implement per model instance net actuation output port (RobotLocomoti…
Browse files Browse the repository at this point in the history
  • Loading branch information
amcastro-tri authored Nov 16, 2023

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature. The key has expired.
1 parent a8e1012 commit 615242a
Showing 6 changed files with 112 additions and 36 deletions.
11 changes: 9 additions & 2 deletions bindings/pydrake/multibody/plant_py.cc
Original file line number Diff line number Diff line change
@@ -917,8 +917,15 @@ void DoScalarDependentDefinitions(py::module m, T) {
py::arg("model_instance"), py_rvp::reference_internal,
cls_doc.get_actuation_input_port.doc_1args)
.def("get_net_actuation_output_port",
&Class::get_net_actuation_output_port, py_rvp::reference_internal,
cls_doc.get_net_actuation_output_port.doc)
overload_cast_explicit<const systems::OutputPort<T>&>(
&Class::get_net_actuation_output_port),
py_rvp::reference_internal,
cls_doc.get_net_actuation_output_port.doc_0args)
.def("get_net_actuation_output_port",
overload_cast_explicit<const systems::OutputPort<T>&,
ModelInstanceIndex>(&Class::get_net_actuation_output_port),
py::arg("model_instance"), py_rvp::reference_internal,
cls_doc.get_net_actuation_output_port.doc_1args)
.def("get_desired_state_input_port",
overload_cast_explicit<const systems::InputPort<T>&,
multibody::ModelInstanceIndex>(
2 changes: 2 additions & 0 deletions bindings/pydrake/multibody/test/plant_test.py
Original file line number Diff line number Diff line change
@@ -434,6 +434,8 @@ def check_repr(element, expected):
plant.get_actuation_input_port(), InputPort)
self.assertIsInstance(
plant.get_net_actuation_output_port(), OutputPort)
self.assertIsInstance(
plant.get_net_actuation_output_port(model_instance), OutputPort)
self.assertIsInstance(
plant.get_state_output_port(), OutputPort)
self.assertIsInstance(
33 changes: 31 additions & 2 deletions multibody/plant/multibody_plant.cc
Original file line number Diff line number Diff line change
@@ -369,6 +369,7 @@ MultibodyPlant<T>::MultibodyPlant(const MultibodyPlant<U>& other)
// in FinalizePlantOnly():
// -instance_actuation_ports_
// -actuation_port_
// -instance_net_actuation_ports_
// -net_actuation_port_
// -applied_generalized_force_input_port_
// -applied_spatial_force_input_port_
@@ -2781,10 +2782,28 @@ void MultibodyPlant<T>::DeclareStateCacheAndPorts() {
this->DeclareVectorInputPort("actuation", num_actuated_dofs())
.get_index();

// Total applied actuation. For discrete models it can contain constraint
// terms.
// Net actuation ports.
// N.B. We intentionally declare a dependency on kinematics in the continuous
// mode in anticipation for adding PD support in continuous mode.
instance_net_actuation_ports_.resize(num_model_instances());
for (ModelInstanceIndex model_instance_index(0);
model_instance_index < num_model_instances(); ++model_instance_index) {
const int instance_num_dofs = num_actuated_dofs(model_instance_index);
instance_net_actuation_ports_[model_instance_index] =
this->DeclareVectorOutputPort(
GetModelInstanceName(model_instance_index) + "_net_actuation",
instance_num_dofs,
[this, model_instance_index](const systems::Context<T>& context,
systems::BasicVector<T>* result) {
const VectorX<T>& net_actuation =
get_net_actuation_output_port().Eval(context);
result->SetFromVector(this->GetActuationFromArray(
model_instance_index, net_actuation));
},
{state_ticket, this->all_input_ports_ticket(),
this->all_parameters_ticket()})
.get_index();
}
net_actuation_port_ = this->DeclareVectorOutputPort(
"net_actuation", num_actuated_dofs(),
&MultibodyPlant::CalcActuationOutput,
@@ -3192,6 +3211,16 @@ const systems::OutputPort<T>& MultibodyPlant<T>::get_net_actuation_output_port()
return systems::System<T>::get_output_port(net_actuation_port_);
}

template <typename T>
const systems::OutputPort<T>& MultibodyPlant<T>::get_net_actuation_output_port(
ModelInstanceIndex model_instance) const {
DRAKE_MBP_THROW_IF_NOT_FINALIZED();
DRAKE_THROW_UNLESS(model_instance.is_valid());
DRAKE_THROW_UNLESS(model_instance < num_model_instances());
return systems::System<T>::get_output_port(
instance_net_actuation_ports_.at(model_instance));
}

template <typename T>
const systems::InputPort<T>& MultibodyPlant<T>::get_desired_state_input_port(
ModelInstanceIndex model_instance) const {
47 changes: 35 additions & 12 deletions multibody/plant/multibody_plant.h
Original file line number Diff line number Diff line change
@@ -214,6 +214,7 @@ name: MultibodyPlant
model_instance_name[i]</em>_generalized_acceleration'
- '<em style="color:gray">
model_instance_name[i]</em>_generalized_contact_forces'
- <em style="color:gray">model_instance_name[i]</em>_net_actuation
- <span style="color:green">geometry_pose</span>
@endsystem

@@ -848,6 +849,23 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
/// @throws std::exception if called before Finalize().
const systems::InputPort<T>& get_actuation_input_port() const;

/// Returns a constant reference to the input port for external actuation for
/// a specific model instance. This is a vector valued port with entries
/// ordered by monotonically increasing @ref JointActuatorIndex within
/// `model_instance`. Refer to @ref mbp_actuation "Actuation" for further
/// details.
///
/// Every model instance in `this` plant model has an actuation input port,
/// even if zero sized (for model instance with no actuators).
///
/// @see GetJointActuatorIndices(), GetActuatedJointIndices().
///
/// @pre Finalize() was already called on `this` plant.
/// @throws std::exception if called before Finalize().
/// @throws std::exception if the model instance does not exist.
const systems::InputPort<T>& get_actuation_input_port(
ModelInstanceIndex model_instance) const;

/// Returns a constant reference to the output port that reports actuation
/// values applied through joint actuators. This output port is a vector
/// valued port indexed by @ref JointActuatorIndex, see
@@ -860,21 +878,21 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
/// @throws std::exception if called before Finalize().
const systems::OutputPort<T>& get_net_actuation_output_port() const;

/// Returns a constant reference to the input port for external actuation for
/// a specific model instance. This is a vector valued port with entries
/// ordered by monotonically increasing @ref JointActuatorIndex within
/// `model_instance`. Refer to @ref mbp_actuation "Actuation" for further
/// details.
///
/// Every model instance in `this` plant model has an actuation input port,
/// even if zero sized (for model instance with no actuators).
/// Returns a constant reference to the output port that reports actuation
/// values applied through joint actuators, for a specific model instance.
/// Models that include PD controllers will include their contribution in this
/// port, refer to @ref mbp_actuation "Actuation" for further details. This is
/// a vector valued port with entries ordered by monotonically increasing @ref
/// JointActuatorIndex within `model_instance`.
///
/// @see GetJointActuatorIndices(), GetActuatedJointIndices().
/// Every model instance in `this` plant model has a net actuation output
/// port, even if zero sized (for model instance with no actuators).
///
/// @note PD controllers are not considered for actuators on locked joints,
/// see Joint::Lock(). Therefore they do not contribute to this port.
/// @pre Finalize() was already called on `this` plant.
/// @throws std::exception if called before Finalize().
/// @throws std::exception if the model instance does not exist.
const systems::InputPort<T>& get_actuation_input_port(
const systems::OutputPort<T>& get_net_actuation_output_port(
ModelInstanceIndex model_instance) const;

/// For models with PD controlled joint actuators, returns the port to provide
@@ -5199,7 +5217,7 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
// ports. The return value is indexed by JointActuatorIndex.
VectorX<T> AssembleActuationInput(const systems::Context<T>& context) const;

// Computes the total applied actuation through actuators. For continuous
// Computes the net applied actuation through actuators. For continuous
// models (thus far) this only inludes values coming from the
// actuation_input_port. For discrete models, it includes actuator
// controllers, see @ref mbp_actuation. Similarly to AssembleActuationInput(),
@@ -5701,6 +5719,11 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
// Net actuation applied through actuators.
systems::OutputPortIndex net_actuation_port_;

// Vector of net actuation output ports per model instance. Every model
// instance has a corresponding port, which might be zero sized for model
// instances with no actuators.
std::vector<systems::OutputPortIndex> instance_net_actuation_ports_;

std::vector<systems::InputPortIndex> instance_desired_state_ports_;

// A port for externally applied generalized forces u.
50 changes: 31 additions & 19 deletions multibody/plant/test/actuated_models_test.cc
Original file line number Diff line number Diff line change
@@ -242,10 +242,29 @@ class ActuatedIiiwaArmTest : public ::testing::Test {
acrobot_u(1) /* Acrobot elbow */).finished();
// clang-format on

// Verify that actuation output is an exact copy of the inputs.
const VectorXd actuation_output =
plant_->get_net_actuation_output_port().Eval(*context_);
EXPECT_EQ(actuation_output, expected_u);
// Verify that net actuation output is an exact copy of the inputs.
VerifyNetActuationOutputPorts(arm_u, acrobot_u, gripper_u, expected_u);
}

// Helper to verify that the net actuation output ports report the values
// provided within `tolerance`.
void VerifyNetActuationOutputPorts(const VectorXd& arm_u,
const VectorXd& acrobot_u,
const VectorXd& gripper_u,
const VectorXd& u,
double tolerance = 0) const {
EXPECT_TRUE(CompareMatrices(
plant_->get_net_actuation_output_port(arm_model_).Eval(*context_),
arm_u, tolerance, MatrixCompareType::relative));
EXPECT_TRUE(CompareMatrices(
plant_->get_net_actuation_output_port(gripper_model_).Eval(*context_),
gripper_u, tolerance, MatrixCompareType::relative));
EXPECT_TRUE(CompareMatrices(
plant_->get_net_actuation_output_port(acrobot_model_).Eval(*context_),
acrobot_u, tolerance, MatrixCompareType::relative));
EXPECT_TRUE(
CompareMatrices(plant_->get_net_actuation_output_port().Eval(*context_),
u, tolerance, MatrixCompareType::relative));
}

protected:
@@ -527,11 +546,9 @@ TEST_F(ActuatedIiiwaArmTest,
EXPECT_TRUE(CompareMatrices(x_actuation, x_tau, kTolerance,
MatrixCompareType::relative));

// Verify the actuation values reported by the plant.
const VectorXd actuation_output =
plant_->get_net_actuation_output_port().Eval(*context_);
EXPECT_TRUE(CompareMatrices(actuation_output, expected_u, kTolerance,
MatrixCompareType::relative));
// Verify the net actuation values reported by the plant.
VerifyNetActuationOutputPorts(arm_u, acrobot_u, gripper_u, expected_u,
kTolerance);
}

// We verify that the PD controlled actuators exert effort limits.
@@ -600,10 +617,8 @@ TEST_F(ActuatedIiiwaArmTest,
MatrixCompareType::relative));

// Verify the actuation values reported by the plant.
const VectorXd actuation_output =
plant_->get_net_actuation_output_port().Eval(*context_);
EXPECT_TRUE(CompareMatrices(actuation_output, expected_u, kTolerance,
MatrixCompareType::relative));
VerifyNetActuationOutputPorts(arm_u_clamped, acrobot_u, gripper_u, expected_u,
kTolerance);

// Joint 4 is actuated beyond its effort limits. Here we verify that when the
// joint is locked (and only for this joint), it's PD controller is ignored
@@ -614,12 +629,9 @@ TEST_F(ActuatedIiiwaArmTest,
(VectorXd(7) << 300, 300, 55, -350, -300, -300, -40).finished();
const VectorXd expected_u_when_joint4_is_locked = AssembleFullModelActuation(
arm_u_when_joint4_is_locked, acrobot_u, gripper_u);

const VectorXd actuation_output_when_joint4_is_locked =
plant_->get_net_actuation_output_port().Eval(*context_);
EXPECT_TRUE(CompareMatrices(actuation_output_when_joint4_is_locked,
expected_u_when_joint4_is_locked, kTolerance,
MatrixCompareType::relative));
VerifyNetActuationOutputPorts(arm_u_when_joint4_is_locked, acrobot_u,
gripper_u, expected_u_when_joint4_is_locked,
kTolerance);
}

// This test verifies that for continuous models the actuation output port
5 changes: 4 additions & 1 deletion multibody/plant/test/multibody_plant_test.cc
Original file line number Diff line number Diff line change
@@ -1725,9 +1725,12 @@ bool VerifyFeedthroughPorts(const MultibodyPlant<double>& plant) {
ok_to_feedthrough.insert(plant.get_reaction_forces_output_port().get_index());
ok_to_feedthrough.insert(
plant.get_generalized_acceleration_output_port().get_index());
for (ModelInstanceIndex i(0); i < plant.num_model_instances(); ++i)
for (ModelInstanceIndex i(0); i < plant.num_model_instances(); ++i) {
ok_to_feedthrough.insert(
plant.get_generalized_acceleration_output_port(i).get_index());
ok_to_feedthrough.insert(
plant.get_net_actuation_output_port(i).get_index());
}
ok_to_feedthrough.insert(
plant.get_body_spatial_accelerations_output_port().get_index());
if (plant.is_discrete()) {

0 comments on commit 615242a

Please sign in to comment.