Drake
RevoluteMobilizer< T > Class Template Referencefinal

This Mobilizer allows two frames to rotate relatively to one another around an axis that is constant when measured in either this mobilizer's inboard or outboard frames, while the distance between the two frames does not vary. More...

#include <multibody/multibody_tree/revolute_mobilizer.h>

Inheritance diagram for RevoluteMobilizer< T >:
[legend]
Collaboration diagram for RevoluteMobilizer< T >:
[legend]

Public Member Functions

 RevoluteMobilizer (const Frame< T > &inboard_frame_F, const Frame< T > &outboard_frame_M, const Vector3< double > &axis_F)
 Constructor for a RevoluteMobilizer between the inboard frame F inboard_frame_F and the outboard frame M outboard_frame_F granting a single rotational degree of freedom about axis axis_F expressed in the inboard frame F. More...
 
const Vector3< double > & get_revolute_axis () const
 
const T & get_angle (const systems::Context< T > &context) const
 Gets the rotation angle of this mobilizer from context. More...
 
const RevoluteMobilizer< T > & set_angle (systems::Context< T > *context, const T &angle) const
 Sets the context so that the generalized coordinate corresponding to the rotation angle of this mobilizer equals angle. More...
 
const T & get_angular_rate (const systems::Context< T > &context) const
 Gets the rate of change, in radians per second, of this mobilizer's angle (see get_angle()) from context. More...
 
const RevoluteMobilizer< T > & set_angular_rate (systems::Context< T > *context, const T &theta_dot) const
 Sets the rate of change, in radians per second, of this this mobilizer's angle to theta_dot. More...
 
void set_zero_state (const systems::Context< T > &context, systems::State< T > *state) const override
 Sets state to store a zero angle and angular rate. More...
 
Isometry3< T > CalcAcrossMobilizerTransform (const MultibodyTreeContext< T > &context) const override
 Computes the across-mobilizer transform X_FM(q) between the inboard frame F and the outboard frame M as a function of the rotation angle about this mobilizer's axis (. More...
 
SpatialVelocity< T > CalcAcrossMobilizerSpatialVelocity (const MultibodyTreeContext< T > &context, const Eigen::Ref< const VectorX< T >> &v) const override
 Computes the across-mobilizer velocity V_FM(q, v) of the outboard frame M measured and expressed in frame F as a function of the rotation angle and input angular velocity v about this mobilizer's axis (. More...
 
SpatialAcceleration< T > CalcAcrossMobilizerSpatialAcceleration (const MultibodyTreeContext< T > &context, const Eigen::Ref< const VectorX< T >> &vdot) const override
 Computes the across-mobilizer acceleration A_FM(q, v, v̇) of the outboard frame M in the inboard frame F. More...
 
void ProjectSpatialForce (const MultibodyTreeContext< T > &context, const SpatialForce< T > &F_Mo_F, Eigen::Ref< VectorX< T >> tau) const override
 Projects the spatial force F_Mo_F on this mobilizer's outboard frame M onto its rotation axis (. More...
 
void MapVelocityToQDot (const MultibodyTreeContext< T > &context, const Eigen::Ref< const VectorX< T >> &v, EigenPtr< VectorX< T >> qdot) const override
 Computes the kinematic mapping q̇ = N(q)⋅v between generalized velocities v and time derivatives of the generalized positions qdot. More...
 
void MapQDotToVelocity (const MultibodyTreeContext< T > &context, const Eigen::Ref< const VectorX< T >> &qdot, EigenPtr< VectorX< T >> v) const override
 Computes the mapping v = N⁺(q)⋅q̇ from time derivatives of the generalized positions qdot to generalized velocities v, where N⁺(q) is the left pseudo-inverse of N(q) defined by MapVelocityToQDot(). More...
 
Does not allow copy, move, or assignment
 RevoluteMobilizer (const RevoluteMobilizer &)=delete
 
RevoluteMobilizeroperator= (const RevoluteMobilizer &)=delete
 
 RevoluteMobilizer (RevoluteMobilizer &&)=delete
 
RevoluteMobilizeroperator= (RevoluteMobilizer &&)=delete
 
- Public Member Functions inherited from MobilizerImpl< T, 1, 1 >
 MobilizerImpl (const Frame< T > &inboard_frame, const Frame< T > &outboard_frame)
 As with Mobilizer this the only constructor available for this base class. More...
 
int get_num_positions () const final
 Returns the number of generalized coordinates granted by this mobilizer. More...
 
int get_num_velocities () const final
 Returns the number of generalized velocities granted by this mobilizer. More...
 
std::unique_ptr< internal::BodyNode< T > > CreateBodyNode (const internal::BodyNode< T > *parent_node, const Body< T > *body, const Mobilizer< T > *mobilizer) const final
 For MultibodyTree internal use only. More...
 
 MobilizerImpl (const MobilizerImpl &)=delete
 
 MobilizerImpl (MobilizerImpl &&)=delete
 
MobilizerImploperator= (const MobilizerImpl &)=delete
 
MobilizerImploperator= (MobilizerImpl &&)=delete
 
- Public Member Functions inherited from Mobilizer< T >
 Mobilizer (const Frame< T > &inboard_frame, const Frame< T > &outboard_frame)
 The minimum amount of information that we need to define a Mobilizer is the knowledge of the inboard and outboard frames it connects. More...
 
const Frame< T > & get_inboard_frame () const
 Returns a constant reference to the inboard frame. More...
 
const Frame< T > & get_outboard_frame () const
 Returns a constant reference to the outboard frame. More...
 
const Body< T > & get_inboard_body () const
 Returns a constant reference to the body associated with this mobilizer's inboard frame. More...
 
const Body< T > & get_outboard_body () const
 Returns a constant reference to the body associated with this mobilizer's outboard frame. More...
 
const MobilizerTopologyget_topology () const
 Returns the topology information for this mobilizer. More...
 
Eigen::VectorBlock< const Eigen::Ref< const VectorX< T > > > get_positions_from_array (const Eigen::Ref< const VectorX< T >> &q_array) const
 Returns a const Eigen expression of the vector of generalized positions for this mobilizer from a vector q_array of generalized positions for the entire MultibodyTree model. More...
 
Eigen::VectorBlock< Eigen::Ref< VectorX< T > > > get_mutable_positions_from_array (EigenPtr< VectorX< T >> q_array) const
 Mutable version of get_positions_from_array(). More...
 
Eigen::VectorBlock< const Eigen::Ref< const VectorX< T > > > get_velocities_from_array (const Eigen::Ref< const VectorX< T >> &v_array) const
 Returns a const Eigen expression of the vector of generalized velocities for this mobilizer from a vector v_array of generalized velocities for the entire MultibodyTree model. More...
 
Eigen::VectorBlock< Eigen::Ref< VectorX< T > > > get_mutable_velocities_from_array (EigenPtr< VectorX< T >> v_array) const
 Mutable version of get_velocities_from_array(). More...
 
Eigen::VectorBlock< const Eigen::Ref< const VectorX< T > > > get_accelerations_from_array (const Eigen::Ref< const VectorX< T >> &vdot_array) const
 Returns a const Eigen expression of the vector of generalized accelerations for this mobilizer from a vector vdot_array of generalized accelerations for the entire MultibodyTree model. More...
 
Eigen::VectorBlock< Eigen::Ref< VectorX< T > > > get_mutable_accelerations_from_array (EigenPtr< VectorX< T >> vdot_array) const
 Mutable version of get_accelerations_from_array(). More...
 
Eigen::VectorBlock< const Eigen::Ref< const VectorX< T > > > get_generalized_forces_from_array (const Eigen::Ref< const VectorX< T >> &tau_array) const
 Returns a const Eigen expression of the vector of generalized forces for this mobilizer from a vector of generalized forces for the entire MultibodyTree model. More...
 
Eigen::VectorBlock< Eigen::Ref< VectorX< T > > > get_mutable_generalized_forces_from_array (EigenPtr< VectorX< T >> tau_array) const
 Mutable version of get_generalized_forces_from_array(). More...
 
Eigen::VectorBlock< const VectorX< T > > get_generalized_forces_from_array (const VectorX< T > &tau_array) const
 Returns a const Eigen expression of the vector of generalized velocities for this mobilizer from a vector of generalized velocities for the entire MultibodyTree model. More...
 
Eigen::VectorBlock< Eigen::Ref< VectorX< T > > > get_mutable_generalized_forces_from_array (Eigen::Ref< VectorX< T >> tau_array) const
 Mutable version of get_velocities_from_array(). More...
 
template<typename ToScalar >
std::unique_ptr< Mobilizer< ToScalar > > CloneToScalar (const MultibodyTree< ToScalar > &cloned_tree) const
 NVI to DoCloneToScalar() templated on the scalar type of the new clone to be created. More...
 
 Mobilizer (const Mobilizer &)=delete
 
Mobilizeroperator= (const Mobilizer &)=delete
 
 Mobilizer (Mobilizer &&)=delete
 
Mobilizeroperator= (Mobilizer &&)=delete
 
void set_zero_configuration (systems::Context< T > *context) const
 Sets the state stored in context to a zero configuration as defined by set_zero_state(). More...
 

Protected Member Functions

std::unique_ptr< Mobilizer< double > > DoCloneToScalar (const MultibodyTree< double > &tree_clone) const override
 Clones this Mobilizer (templated on T) to a mobilizer templated on double. More...
 
std::unique_ptr< Mobilizer< AutoDiffXd > > DoCloneToScalar (const MultibodyTree< AutoDiffXd > &tree_clone) const override
 Clones this Mobilizer (templated on T) to a mobilizer templated on AutoDiffXd. More...
 
- Protected Member Functions inherited from MobilizerImpl< T, 1, 1 >
const MultibodyTreeContext< T > & GetMultibodyTreeContextOrThrow (const systems::Context< T > &context) const
 Helper method to retrieve a const reference to the MultibodyTreeContext object referenced by context. More...
 
MultibodyTreeContext< T > & GetMutableMultibodyTreeContextOrThrow (systems::Context< T > *context) const
 Helper method to retrieve a mutable pointer to the MultibodyTreeContext object referenced by context. More...
 
void set_default_zero_state (const systems::Context< T > &, systems::State< T > *state) const
 Helper to set state to a default zero state with all generalized positions and generalized velocities related to this mobilizer to zero. More...
 
Eigen::VectorBlock< const VectorX< T >, kNqget_positions (const MultibodyTreeContext< T > &context) const
 Helper to return a const fixed-size Eigen::VectorBlock referencing the segment in the state vector corresponding to this mobilizer's state. More...
 
Eigen::VectorBlock< VectorX< T >, kNqget_mutable_positions (MultibodyTreeContext< T > *context) const
 Helper to return a mutable fixed-size Eigen::VectorBlock referencing the segment in the state vector corresponding to this mobilizer's state. More...
 
Eigen::VectorBlock< VectorX< T >, kNqget_mutable_positions (systems::State< T > *state) const
 Helper variant to return a const fixed-size Eigen::VectorBlock referencing the segment in the state corresponding to this mobilizer's generalized positions. More...
 
Eigen::VectorBlock< VectorX< T >, kNqget_mutable_velocities (systems::State< T > *state) const
 Helper variant to return a const fixed-size Eigen::VectorBlock referencing the segment in the state corresponding to this mobilizer's generalized velocities. More...
 
Eigen::VectorBlock< VectorX< T >, kNvget_mutable_velocities (MultibodyTreeContext< T > *context) const
 Helper to return a mutable fixed-size Eigen::VectorBlock referencing the segment in the state vector corresponding to this mobilizer's state. More...
 
Eigen::VectorBlock< const VectorX< T >, kNvget_velocities (const MultibodyTreeContext< T > &context) const
 Helper to return a const fixed-size Eigen::VectorBlock referencing the segment in the state vector corresponding to this mobilizer's state. More...
 
Methods to make a clone templated on different scalar types.

The only const argument to these methods is the new MultibodyTree clone under construction, which is required to already own the clones of the inboard and outboard frames of the mobilizer being cloned.

Additional Inherited Members

- Protected Types inherited from MobilizerImpl< T, 1, 1 >
enum  
 

Detailed Description

template<typename T>
class drake::multibody::RevoluteMobilizer< T >

This Mobilizer allows two frames to rotate relatively to one another around an axis that is constant when measured in either this mobilizer's inboard or outboard frames, while the distance between the two frames does not vary.

To fully specify this mobilizer a user must provide the inboard frame F, the outboard (or "mobilized") frame M and the axis axis_F (expressed in frame F) about which frame M rotates with respect to F. The single generalized coordinate q introduced by this mobilizer corresponds to the rotation angle in radians of frame M with respect to frame F about the rotation axis axis_F. When q = 0, frames F and M are coincident. The rotation angle is defined to be positive according to the right-hand-rule with the thumb aligned in the direction of the axis_F. Notice that the components of the rotation axis as expressed in either frame F or M are constant. That is, axis_F and axis_M remain unchanged w.r.t. both frames by this mobilizer's motion.

Template Parameters
TThe scalar type. Must be a valid Eigen scalar.

Instantiated templates for the following kinds of T's are provided:

  • double
  • AutoDiffXd

They are already available to link against in the containing library. No other values for T are currently supported.

Constructor & Destructor Documentation

RevoluteMobilizer ( const RevoluteMobilizer< T > &  )
delete
RevoluteMobilizer ( RevoluteMobilizer< T > &&  )
delete
RevoluteMobilizer ( const Frame< T > &  inboard_frame_F,
const Frame< T > &  outboard_frame_M,
const Vector3< double > &  axis_F 
)
inline

Constructor for a RevoluteMobilizer between the inboard frame F inboard_frame_F and the outboard frame M outboard_frame_F granting a single rotational degree of freedom about axis axis_F expressed in the inboard frame F.

Precondition
axis_F must be a unit vector within at least 1.0e-6. This rather loose tolerance (at least for simulation) allows users to provide "near unity" axis vectors originated, for instance, during the parsing of a file with limited precision. Internally, we re-normalize the axis to within machine precision.
Exceptions
std::runtime_errorif the provided rotational axis is not a unit vector.

Member Function Documentation

SpatialAcceleration< T > CalcAcrossMobilizerSpatialAcceleration ( const MultibodyTreeContext< T > &  context,
const Eigen::Ref< const VectorX< T >> &  vdot 
) const
overridevirtual

Computes the across-mobilizer acceleration A_FM(q, v, v̇) of the outboard frame M in the inboard frame F.

By definition A_FM = d_F(V_FM)/dt = H_FM(q) * v̇ + Ḣ_FM * v. The acceleration A_FM will be a function of the rotation angle q, its rate of change v for the current state in context and of the input generalized acceleration v̇ = dv/dt, the rate of change of v. See class documentation for the angle sign convention. This method aborts in Debug builds if vdot.size() is not one.

Implements Mobilizer< T >.

Here is the caller graph for this function:

SpatialVelocity< T > CalcAcrossMobilizerSpatialVelocity ( const MultibodyTreeContext< T > &  context,
const Eigen::Ref< const VectorX< T >> &  v 
) const
overridevirtual

Computes the across-mobilizer velocity V_FM(q, v) of the outboard frame M measured and expressed in frame F as a function of the rotation angle and input angular velocity v about this mobilizer's axis (.

See also
get_revolute_axis()). The generalized coordinate q for this mobilizer (the rotation angle) is stored in context. This method aborts in Debug builds if v.size() is not one.

Implements Mobilizer< T >.

Here is the caller graph for this function:

Isometry3< T > CalcAcrossMobilizerTransform ( const MultibodyTreeContext< T > &  context) const
overridevirtual

Computes the across-mobilizer transform X_FM(q) between the inboard frame F and the outboard frame M as a function of the rotation angle about this mobilizer's axis (.

See also
get_revolute_axis().) The generalized coordinate q for this mobilizer (the rotation angle) is stored in context. This method aborts in Debug builds if v.size() is not one.

Implements Mobilizer< T >.

Here is the caller graph for this function:

std::unique_ptr< Mobilizer< double > > DoCloneToScalar ( const MultibodyTree< double > &  tree_clone) const
overrideprotectedvirtual

Clones this Mobilizer (templated on T) to a mobilizer templated on double.

Precondition
Inboard and outboard frames for this mobilizer already have a clone in tree_clone.

Implements Mobilizer< T >.

Here is the caller graph for this function:

std::unique_ptr< Mobilizer< AutoDiffXd > > DoCloneToScalar ( const MultibodyTree< AutoDiffXd > &  tree_clone) const
overrideprotectedvirtual

Clones this Mobilizer (templated on T) to a mobilizer templated on AutoDiffXd.

Precondition
Inboard and outboard frames for this mobilizer already have a clone in tree_clone.

Implements Mobilizer< T >.

const T & get_angle ( const systems::Context< T > &  context) const

Gets the rotation angle of this mobilizer from context.

See class documentation for sign convention.

Exceptions
std::logic_errorif context is not a valid MultibodyTreeContext.
Parameters
[in]contextThe context of the MultibodyTree this mobilizer belongs to.
Returns
The angle coordinate of this mobilizer in the context.

Here is the caller graph for this function:

const T & get_angular_rate ( const systems::Context< T > &  context) const

Gets the rate of change, in radians per second, of this mobilizer's angle (see get_angle()) from context.

See class documentation for the angle sign convention.

Parameters
[in]contextThe context of the MultibodyTree this mobilizer belongs to.
Returns
The rate of change of this mobilizer's angle in the context.

Here is the caller graph for this function:

const Vector3<double>& get_revolute_axis ( ) const
inline
Return values
axis_FThe rotation axis as a unit vector expressed in the inboard frame F.

Here is the call graph for this function:

void MapQDotToVelocity ( const MultibodyTreeContext< T > &  context,
const Eigen::Ref< const VectorX< T >> &  qdot,
EigenPtr< VectorX< T >>  v 
) const
overridevirtual

Computes the mapping v = N⁺(q)⋅q̇ from time derivatives of the generalized positions qdot to generalized velocities v, where N⁺(q) is the left pseudo-inverse of N(q) defined by MapVelocityToQDot().

The generalized positions vector is stored in context.

Implements Mobilizer< T >.

Here is the call graph for this function:

Here is the caller graph for this function:

void MapVelocityToQDot ( const MultibodyTreeContext< T > &  context,
const Eigen::Ref< const VectorX< T >> &  v,
EigenPtr< VectorX< T >>  qdot 
) const
overridevirtual

Computes the kinematic mapping q̇ = N(q)⋅v between generalized velocities v and time derivatives of the generalized positions qdot.

The generalized positions vector is stored in context.

Implements Mobilizer< T >.

Here is the caller graph for this function:

RevoluteMobilizer& operator= ( RevoluteMobilizer< T > &&  )
delete
RevoluteMobilizer& operator= ( const RevoluteMobilizer< T > &  )
delete
void ProjectSpatialForce ( const MultibodyTreeContext< T > &  context,
const SpatialForce< T > &  F_Mo_F,
Eigen::Ref< VectorX< T >>  tau 
) const
overridevirtual

Projects the spatial force F_Mo_F on this mobilizer's outboard frame M onto its rotation axis (.

See also
get_revolute_axis().) Mathematically:
   tau = F_Mo_F.rotational().dot(axis_F)
Therefore, the result of this method is the scalar value of the torque at the axis of this mobilizer. This method aborts in Debug builds if tau.size() is not one.

Implements Mobilizer< T >.

Here is the call graph for this function:

Here is the caller graph for this function:

const RevoluteMobilizer< T > & set_angle ( systems::Context< T > *  context,
const T &  angle 
) const

Sets the context so that the generalized coordinate corresponding to the rotation angle of this mobilizer equals angle.

Exceptions
std::logic_errorif context is not a valid MultibodyTreeContext.
Parameters
[in]contextThe context of the MultibodyTree this mobilizer belongs to.
[in]angleThe desired angle in radians.
Returns
a constant reference to this mobilizer.

Here is the caller graph for this function:

const RevoluteMobilizer< T > & set_angular_rate ( systems::Context< T > *  context,
const T &  theta_dot 
) const

Sets the rate of change, in radians per second, of this this mobilizer's angle to theta_dot.

The new rate of change theta_dot gets stored in context. See class documentation for the angle sign convention.

Parameters
[in]contextThe context of the MultibodyTree this mobilizer belongs to.
[in]theta_dotThe desired rate of change of this mobilizer's angle in radians per second.
Returns
a constant reference to this mobilizer.

Here is the caller graph for this function:

void set_zero_state ( const systems::Context< T > &  context,
systems::State< T > *  state 
) const
overridevirtual

Sets state to store a zero angle and angular rate.

Implements Mobilizer< T >.

Here is the caller graph for this function:


The documentation for this class was generated from the following files: