forked from RobotLocomotion/drake
-
Notifications
You must be signed in to change notification settings - Fork 8
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
multibody: adds a Propeller model for use with MultibodyPlant (RobotL…
…ocomotion#13135) * multibody: adds a Propeller model for use with MultibodyPlant Includes python bindings. Partially resolves RobotLocomotion#12401.
- Loading branch information
1 parent
27ddee8
commit 0ab3801
Showing
6 changed files
with
452 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,64 @@ | ||
| #include "drake/multibody/plant/propeller.h" | ||
|
|
||
| namespace drake { | ||
| namespace multibody { | ||
|
|
||
| template <typename T> | ||
| Propeller<T>::Propeller(const BodyIndex& body_index, | ||
| const math::RigidTransform<double>& X_BP, | ||
| double thrust_ratio, double moment_ratio) | ||
| : Propeller({PropellerInfo(body_index, X_BP, thrust_ratio, | ||
| moment_ratio)}) {} | ||
|
|
||
| template <typename T> | ||
| Propeller<T>::Propeller(const std::vector<PropellerInfo>& propeller_info) | ||
| : systems::LeafSystem<T>(systems::SystemTypeTag<Propeller>{}), | ||
| info_(propeller_info) { | ||
| this->DeclareInputPort("command", systems::kVectorValued, num_propellers()); | ||
|
|
||
| this->DeclareAbstractInputPort("body_poses", | ||
| Value<std::vector<math::RigidTransform<T>>>()); | ||
|
|
||
| this->DeclareAbstractOutputPort( | ||
| "spatial_forces", | ||
| std::vector<ExternallyAppliedSpatialForce<T>>(num_propellers()), | ||
| &Propeller<T>::CalcSpatialForces); | ||
| } | ||
|
|
||
| template <typename T> | ||
| void Propeller<T>::CalcSpatialForces( | ||
| const systems::Context<T>& context, | ||
| std::vector<ExternallyAppliedSpatialForce<T>>* spatial_forces) const { | ||
| spatial_forces->resize(num_propellers()); | ||
|
|
||
| const auto& command = get_command_input_port().Eval(context); | ||
| const auto& poses = | ||
| get_body_poses_input_port() | ||
| .template Eval<std::vector<math::RigidTransform<T>>>(context); | ||
|
|
||
| for (int i = 0; i < num_propellers(); i++) { | ||
| const PropellerInfo& prop = info_[i]; | ||
|
|
||
| // Map to the ExternalSpatialForce structure: | ||
| // - the origin of my frame P is Po == Bq, and | ||
| // - the origin of my frame B is Bo. | ||
| const math::RigidTransform<T>& X_WB = poses[prop.body_index]; | ||
| const Eigen::Vector3d& p_BoPo_B = prop.X_BP.translation(); | ||
| const math::RigidTransform<T> X_WP = X_WB * prop.X_BP.cast<T>(); | ||
| const math::RotationMatrix<T>& R_WP = X_WP.rotation(); | ||
|
|
||
| const SpatialForce<T> F_BPo_P( | ||
| Vector3<T>(0, 0, command[i] * prop.moment_ratio), | ||
| Vector3<T>(0, 0, command[i] * prop.thrust_ratio)); | ||
| const SpatialForce<T> F_BPo_W = R_WP * F_BPo_P; | ||
| spatial_forces->at(i).body_index = prop.body_index; | ||
| spatial_forces->at(i).p_BoBq_B = p_BoPo_B; | ||
| spatial_forces->at(i).F_Bq_W = F_BPo_W; | ||
| } | ||
| } | ||
|
|
||
| } // namespace multibody | ||
| } // namespace drake | ||
|
|
||
| DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( | ||
| class drake::multibody::Propeller) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,141 @@ | ||
| #pragma once | ||
|
|
||
| #include <vector> | ||
|
|
||
| #include "drake/common/default_scalars.h" | ||
| #include "drake/common/eigen_types.h" | ||
| #include "drake/multibody/plant/externally_applied_spatial_force.h" | ||
| #include "drake/multibody/tree/body.h" | ||
| #include "drake/systems/framework/leaf_system.h" | ||
|
|
||
| namespace drake { | ||
| namespace multibody { | ||
|
|
||
| /** Parameters that describe the kinematic frame and force-production properties | ||
| of a single propeller. */ | ||
| struct PropellerInfo { | ||
| DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(PropellerInfo); | ||
|
|
||
| explicit PropellerInfo(const BodyIndex& body_index_, | ||
| const math::RigidTransform<double>& X_BP_ = | ||
| math::RigidTransform<double>::Identity(), | ||
| double thrust_ratio_ = 1.0, double moment_ratio_ = 0.0) | ||
| : body_index(body_index_), | ||
| X_BP(X_BP_), | ||
| thrust_ratio(thrust_ratio_), | ||
| moment_ratio(moment_ratio_) {} | ||
|
|
||
| /** The BodyIndex of a Body in the MultibodyPlant to which the propeller is | ||
| attached. The spatial forces will be applied to this body. */ | ||
| BodyIndex body_index; | ||
|
|
||
| /** Pose of the propeller frame P measured in the body frame B. @default is | ||
| the identity matrix. */ | ||
| math::RigidTransform<double> X_BP{}; | ||
|
|
||
| /** The z component (in frame P) of the spatial force will have magnitude | ||
| `thrust_ratio*command` in Newtons. The default is 1 (command in Newtons), but | ||
| this can also be used to scale an actuator command to the resulting force. */ | ||
| double thrust_ratio{1.0}; | ||
|
|
||
| /** The moment about the z axis (in frame P) of the spatial force will have | ||
| magnitude `moment_ratio*command` in Newton-meters. The default is 0, which makes the propeller a simple Cartesian force generator. */ | ||
| double moment_ratio{0.0}; | ||
| }; | ||
|
|
||
| /** A System that connects to the MultibodyPlant in order to model the effects | ||
| of one or more controlled propellers acting on a Body. | ||
| @system{Propeller, | ||
| @input_port{command} | ||
| @input_port{body_poses}, | ||
| @output_port{spatial_forces} | ||
| } | ||
| - The command input is a BasicVector<T> with one element per propeller. | ||
| - It is expected that the body_poses input should be connected to the | ||
| @ref MultibodyPlant::get_body_poses_output_port() "MultibodyPlant body_poses | ||
| output port". | ||
| - The output is of type std::vector<ExternallyAppliedSpatialForce<T>>; it is | ||
| expected that this output will be connected to the @ref | ||
| MultibodyPlant::get_applied_spatial_force_input_port() | ||
| "externally_applied_spatial_force input port" of the MultibodyPlant. | ||
| - This system does not have any state. | ||
| The resulting iᵗʰ spatial force will have a force component in the z-axis of | ||
| the iᵗʰ propeller frame with magnitude `thrust_ratio * command` Newtons, and a | ||
| moment around the z-axis with magnitude `moment_ratio * command` Newton-meters. | ||
| (Including these moments tends to be important -- a quadrotor does not have a | ||
| stabilizable linearization around a hovering fixed point in 3D without them). | ||
| @note Set PropellerInfo::moment_ratio to zero if you want a simple thruster | ||
| which applies only a force (no moment) in the Propeller coordinates. | ||
| @tparam_default_scalar | ||
| @ingroup multibody | ||
| */ | ||
| template <typename T> | ||
| class Propeller final : public systems::LeafSystem<T> { | ||
| public: | ||
| DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Propeller); | ||
|
|
||
| /** Constructs a system describing a single propeller. | ||
| @see PropellerInfo for details on the arguments. */ | ||
| Propeller(const BodyIndex& body_index, | ||
| const math::RigidTransform<double>& X_BP = | ||
| math::RigidTransform<double>::Identity(), | ||
| double thrust_ratio = 1.0, double moment_ratio = 0.0); | ||
|
|
||
| /** Constructs a system describing multiple propellers. | ||
| @see PropellerInfo. */ | ||
| explicit Propeller(const std::vector<PropellerInfo>& propeller_info); | ||
|
|
||
| /** Scalar-converting copy constructor. See @ref system_scalar_conversion. */ | ||
| template <typename U> | ||
| explicit Propeller(const Propeller<U>& other) | ||
| : Propeller<T>(std::vector<PropellerInfo>(other.info_.begin(), | ||
| other.info_.end())) {} | ||
|
|
||
| /** Returns the number of propellers modeled by this system. */ | ||
| int num_propellers() const { return info_.size(); } | ||
|
|
||
| /** Returns a reference to the vector-valued input port for the propeller | ||
| commands. It has size `num_propellers()`. */ | ||
| const systems::InputPort<T>& get_command_input_port() const { | ||
| return this->get_input_port(0); | ||
| } | ||
|
|
||
| /** Returns a reference to the body_poses input port. It is anticipated | ||
| that this port will be connected the body_poses output port of a | ||
| MultibodyPlant. */ | ||
| const systems::InputPort<T>& get_body_poses_input_port() const { | ||
| return this->get_input_port(1); | ||
| } | ||
|
|
||
| /** Returns a reference to the spatial_forces output port. It is anticipated | ||
| that this port will be connected to the @ref | ||
| MultibodyPlant::get_applied_spatial_force_input_port() "applied_spatial_force" | ||
| input port of a MultibodyPlant. */ | ||
| const systems::OutputPort<T>& get_spatial_forces_output_port() const { | ||
| return this->get_output_port(0); | ||
| } | ||
|
|
||
| private: | ||
| // Calculates the spatial forces in the world frame as expected by the | ||
| // applied_spatial_force input port of MultibodyPlant. | ||
| void CalcSpatialForces( | ||
| const systems::Context<T>& context, | ||
| std::vector<ExternallyAppliedSpatialForce<T>>* spatial_forces) const; | ||
|
|
||
| std::vector<PropellerInfo> info_; | ||
|
|
||
| // Declare friendship to enable scalar conversion. | ||
| template <typename U> | ||
| friend class Propeller; | ||
| }; | ||
|
|
||
| } // namespace multibody | ||
| } // namespace drake | ||
|
|
||
| DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( | ||
| class drake::multibody::Propeller) |
Oops, something went wrong.