Drake
QuaternionFloatingMobilizer< T > Class Template Referencefinal

This Mobilizer allows two frames to move freely relatively to one another. More...

#include <drake/multibody/multibody_tree/quaternion_floating_mobilizer.h>

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

Public Member Functions

 QuaternionFloatingMobilizer (const Frame< T > &inboard_frame_F, const Frame< T > &outboard_frame_M)
 Constructor for a QuaternionFloatingMobilizer granting six degrees of freedom to an outboard frame M with respect to an inboard frame F. More...
 
Does not allow copy, move, or assignment
 QuaternionFloatingMobilizer (const QuaternionFloatingMobilizer &)=delete
 
QuaternionFloatingMobilizeroperator= (const QuaternionFloatingMobilizer &)=delete
 
 QuaternionFloatingMobilizer (QuaternionFloatingMobilizer &&)=delete
 
QuaternionFloatingMobilizeroperator= (QuaternionFloatingMobilizer &&)=delete
 
Methods to get and set the state for a QuaternionFloatingMobilizer

These methods throw an exception if the provided systems::Context does not correspond to a valid MultibodyTreeContext.

Quaternion< T > get_quaternion (const systems::Context< T > &context) const
 Gets the quaternion q_FM that represents the orientation of outboard frame M in the inboard frame F. More...
 
Vector3< T > get_position (const systems::Context< T > &context) const
 Returns the position p_FM of the outboard frame M's origin as measured and expressed in the inboard frame F. More...
 
const QuaternionFloatingMobilizer< T > & set_quaternion (systems::Context< T > *context, const Quaternion< T > &q_FM) const
 Sets context so that the orientation of frame M in F is given by the input quaternion q_FM. More...
 
const QuaternionFloatingMobilizer< T > & set_quaternion (const systems::Context< T > &context, const Quaternion< T > &q_FM, systems::State< T > *state) const
 Alternative signature to set_quaternion(context, q_FM) to set state to store the orientation of M in F given by the equaternion q_FM. More...
 
const QuaternionFloatingMobilizer< T > & set_position (systems::Context< T > *context, const Vector3< T > &p_FM) const
 Sets context to store the position p_FM of frame M's origin Mo measured and expressed in frame F. More...
 
const QuaternionFloatingMobilizer< T > & set_position (const systems::Context< T > &context, const Vector3< T > &p_FM, systems::State< T > *state) const
 Alternative signature to set_position(context, p_FM) to set state to store the position p_FM of M in F. More...
 
const QuaternionFloatingMobilizer< T > & SetFromRotationMatrix (systems::Context< T > *context, const Matrix3< T > &R_FM) const
 Sets context to store the quaternion q_FM which represents the same orientation of M in F as given by the rotation matrix R_FM. More...
 
Vector3< T > get_angular_velocity (const systems::Context< T > &context) const
 Returns the angular velocity w_FM of frame M in F stored in context. More...
 
const QuaternionFloatingMobilizer< T > & set_angular_velocity (systems::Context< T > *context, const Vector3< T > &w_FM) const
 Sets context to store the angular velocity w_FM of frame M in frame F. More...
 
const QuaternionFloatingMobilizer< T > & set_angular_velocity (const systems::Context< T > &, const Vector3< T > &w_FM, systems::State< T > *state) const
 Alternative signature to set_angular_velocity(context, w_FM) to set state to store the angular velocity w_FM of M in F. More...
 
Vector3< T > get_translational_velocity (const systems::Context< T > &context) const
 Retrieves and returns from context the translational velocity v_FM of frame M's origin as measured and expressed in frame F. More...
 
const QuaternionFloatingMobilizer< T > & set_translational_velocity (systems::Context< T > *context, const Vector3< T > &v_FM) const
 Sets context to store the translational velocity v_FM of frame M in frame F. More...
 
const QuaternionFloatingMobilizer< T > & set_translational_velocity (const systems::Context< T > &, const Vector3< T > &v_FM, systems::State< T > *state) const
 Alternative signature to set_translational_velocity(context, v_FM) to set state to store the translational velocity v_FM of M in F. More...
 
void set_zero_state (const systems::Context< T > &context, systems::State< T > *state) const override
 Sets state to store a configuration in which M coincides with F (i.e. More...
 
Mobilizer overrides

Refer to the Mobilizer class documentation for details.

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 vector of generalized postions q. More...
 
SpatialVelocity< T > CalcAcrossMobilizerSpatialVelocity (const MultibodyTreeContext< T > &context, const Eigen::Ref< const VectorX< T >> &v) const override
 Computes the across-mobilizer spatial velocity V_FM(q, v) of the outboard frame M in the inboard frame F. More...
 
SpatialAcceleration< T > CalcAcrossMobilizerSpatialAcceleration (const MultibodyTreeContext< T > &context, const Eigen::Ref< const VectorX< T >> &vdot) const override
 Computes the across-mobilizer spatial accelerations 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 on this mobilizer's outboard frame M onto the sub-space of motions spanned by the geometric Jacobian H_FM(q) to obtain the generalized forces tau (i.e. 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...
 
- Public Member Functions inherited from MobilizerImpl< T, 7, 6 >
 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 num_positions () const final
 Returns the number of generalized coordinates granted by this mobilizer. More...
 
int 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 > & inboard_frame () const
 Returns a constant reference to the inboard frame. More...
 
const Frame< T > & outboard_frame () const
 Returns a constant reference to the outboard frame. More...
 
const Body< T > & inboard_body () const
 Returns a constant reference to the body associated with this mobilizer's inboard frame. More...
 
const Body< T > & 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...
 
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, 7, 6 >
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 > &context, 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 (const systems::Context< T > &context, 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 > > get_mutable_state_vector (const systems::Context< T > &context, systems::State< T > *state) const
 Returns a mutable reference to the state vector stored in state as an Eigen::VectorBlock<VectorX<T>>. More...
 
Eigen::VectorBlock< VectorX< T >, kNvget_mutable_velocities (const systems::Context< T > &context, 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, 7, 6 >
enum  
 
- Static Protected Member Functions inherited from MobilizerImpl< T, 7, 6 >
static const MultibodyTreeContext< T > & GetMultibodyTreeContextOrThrow (const systems::Context< T > &context)
 Helper method to retrieve a const reference to the MultibodyTreeContext object referenced by context. More...
 

Detailed Description

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

This Mobilizer allows two frames to move freely relatively to one another.

To fully specify this mobilizer a user must provide an inboard frame F and an outboard frame M. This mobilizer introduces six degrees of freedom which allow frame M to freely move with respect to frame F. This mobilizer introduces four generalized positions to describe the orientation R_FM of frame M in F with a quaternion q_FM, and three generalized positions to describe the position of frame M's origin in F with a position vector p_FM. As generalized velocities, this mobilizer introduces the angular velocity w_FM of frame M in F and the linear velocity v_FM of frame M's origin in frame F.

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

QuaternionFloatingMobilizer ( const Frame< T > &  inboard_frame_F,
const Frame< T > &  outboard_frame_M 
)
inline

Constructor for a QuaternionFloatingMobilizer granting six degrees of freedom to an outboard frame M with respect to an inboard frame F.

The orientation of frame M in F is represented by a quaternion q_FM while the position of F in M is given by a position vector p_FM expressed in frame F.

Parameters
[in]inboard_frame_Fthe inboard frame F.
[in]outboard_frame_Mthe outboard frame M which can move freely with respect to frame F.

Here is the call graph for this function:

Member Function Documentation

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

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

This method can be thought of as the application of the operation v̇ ∈ ℝⁿᵛ → M⁶: A_FM(q, v, v̇) = H_FM(q) * v̇ + Ḣ_FM(q) * v, where nv is the number of generalized velocities of this mobilizer (see num_velocities()) and M⁶ is the vector space of "motion vectors" (be aware that while M⁶ is introduced in [Featherstone 2008, Ch. 2] spatial vectors in Drake are not Plücker vectors as in Featherstone's book). Therefore, we say this method is in its operator form; the Jacobian matrix H_FM(q) is not explicitly formed. This method aborts in Debug builds if the dimension of the input vector of generalized accelerations has a size different from num_velocities().

Parameters
[in]contextThe context of the parent tree that owns this mobilizer. This mobilizer's generalized positions q and generalized velocities v are taken from this context.
[in]vdotThe vector of generalized velocities' time derivatives v̇. It must live in ℝⁿᵛ.
Return values
A_FMThe across-mobilizer spatial acceleration of the outboard frame M measured and expressed in the inboard frame F.

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 spatial velocity V_FM(q, v) of the outboard frame M in the inboard frame F.

This method can be thought of as the application of the operator H_FM(q) to the input vector of generalized velocities v, i.e. the output of this method is the application v ∈ ℝⁿᵛ → M⁶: V_FM(q, v) = H_FM(q) * v, where nv is the number of generalized velocities of this mobilizer (see num_velocities()) and M⁶ is the vector space of "motion vectors" (be aware that while M⁶ is introduced in [Featherstone 2008, Ch. 2] spatial velocities in Drake are not Plücker vectors as in Featherstone's book). Therefore we say this method is the operator form of the Jacobian matrix H_FM(q). This method aborts in Debug builds if the dimension of the input vector of generalized velocities has a size different from num_velocities().

Parameters
[in]contextThe context of the parent tree that owns this mobilizer. This mobilizer's generalized positions q are inferred from this context.
[in]vA vector of generalized velocities. It must live in ℝⁿᵛ.
Return values
V_FMThe across-mobilizer spatial velocity of the outboard frame M measured and expressed in the inboard frame F.

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 vector of generalized postions q.

Mobilizer subclasses implementing this method can retrieve the fixed-size vector of generalized positions for this mobilizer from context with:

auto q = this->get_positions(context);

Additionally, context can provide any other parameters the mobilizer could depend on.

Implements Mobilizer< T >.

Here is the call graph for this function:

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 >.

Vector3< T > get_angular_velocity ( const systems::Context< T > &  context) const

Returns the angular velocity w_FM of frame M in F stored in context.

Parameters
[in]contextThe context of the MultibodyTree this mobilizer belongs to.
Return values
w_FMThe angular velocity of the outboard frame M in the inboard frame F.

Here is the caller graph for this function:

Vector3< T > get_position ( const systems::Context< T > &  context) const

Returns the position p_FM of the outboard frame M's origin as measured and expressed in the inboard frame F.

Refer to the documentation for this class for details.

Parameters
[in]contextThe context storing the state of the MultibodyTree this mobilizer belongs to.
Return values
p_FMThe position vector of frame M's origin in frame F.

Here is the caller graph for this function:

Quaternion< T > get_quaternion ( const systems::Context< T > &  context) const

Gets the quaternion q_FM that represents the orientation of outboard frame M in the inboard frame F.

Refer to the documentation for this class for details.

Parameters
[in]contextThe context storing the state of the MultibodyTree this mobilizer belongs to.
Return values
q_FMThe quaternion representing the orientaiton of frame M in F.

Here is the caller graph for this function:

Vector3< T > get_translational_velocity ( const systems::Context< T > &  context) const

Retrieves and returns from context the translational velocity v_FM of frame M's origin as measured and expressed in frame F.

Parameters
[in]contextThe context of the MultibodyTree to which this mobilizer belongs to.
Return values
v_FMThe translational velocity of the outboard frame M in the inboard frame F, expressed in F.

Here is the caller 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:

QuaternionFloatingMobilizer& operator= ( const QuaternionFloatingMobilizer< 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 on this mobilizer's outboard frame M onto the sub-space of motions spanned by the geometric Jacobian H_FM(q) to obtain the generalized forces tau (i.e.

the active components of F_Mo).

See also
CalcAcrossMobilizerSpatialVelocity() and this class' documentation for the definition of the geometric Jacobian H_FM(q).

This method can be thought of as the application of the transpose operator H_FMᵀ(q) to the input spatial force F_Mo_F, i.e. the output of this method is the application F_Mo_F ∈ F⁶ → ℝⁿᵛ: tau = H_FMᵀ(q) * F_Mo_F, where nv is the number of generalized velocities of this mobilizer (see num_velocities()) and F⁶ is the vector space of "force vectors" (be aware that while F⁶ is introduced in [Featherstone 2008, Ch. 2] spatial forces in Drake are not Plücker vectors as in Featherstone's book). Therefore we say this method is the operator form of the Jacobian matrix transpose H_FMᵀ(q). This method aborts in Debug builds if the dimension of the output vector of generalized forces has a size different from num_velocities().

Parameters
[in]contextThe context of the parent tree that owns this mobilizer. This mobilizer's generalized positions q are stored in this context.
[in]F_Mo_FA SpatialForce applied at this mobilizer's outboard frame origin Mo, expressed in the inboard frame F.
Return values
tauThe vector of generalized forces. It must live in ℝⁿᵛ.

Implements Mobilizer< T >.

Here is the call graph for this function:

Here is the caller graph for this function:

const QuaternionFloatingMobilizer< T > & set_angular_velocity ( systems::Context< T > *  context,
const Vector3< T > &  w_FM 
) const

Sets context to store the angular velocity w_FM of frame M in frame F.

Parameters
[out]contextThe context of the MultibodyTree this mobilizer belongs to.
[in]w_FMThe desired angular velocity of frame M in F, expressed in F.
Returns
a constant reference to this mobilizer.

Here is the call graph for this function:

Here is the caller graph for this function:

const QuaternionFloatingMobilizer< T > & set_angular_velocity ( const systems::Context< T > &  context,
const Vector3< T > &  w_FM,
systems::State< T > *  state 
) const

Alternative signature to set_angular_velocity(context, w_FM) to set state to store the angular velocity w_FM of M in F.

const QuaternionFloatingMobilizer< T > & set_position ( systems::Context< T > *  context,
const Vector3< T > &  p_FM 
) const

Sets context to store the position p_FM of frame M's origin Mo measured and expressed in frame F.

Parameters
[out]contextThe context storing the state of the MultibodyTree this mobilizer belongs to.
[in]p_FMThe desired position of frame M in F to be stored in context.
Returns
a constant reference to this mobilizer.

Here is the call graph for this function:

Here is the caller graph for this function:

const QuaternionFloatingMobilizer< T > & set_position ( const systems::Context< T > &  context,
const Vector3< T > &  p_FM,
systems::State< T > *  state 
) const

Alternative signature to set_position(context, p_FM) to set state to store the position p_FM of M in F.

const QuaternionFloatingMobilizer< T > & set_quaternion ( systems::Context< T > *  context,
const Quaternion< T > &  q_FM 
) const

Sets context so that the orientation of frame M in F is given by the input quaternion q_FM.

Parameters
[out]contextThe context storing the state of the MultibodyTree this mobilizer belongs to.
[in]q_FMThe desired orientation of M in F to be stored in context.
Returns
a constant reference to this mobilizer.

Here is the call graph for this function:

Here is the caller graph for this function:

const QuaternionFloatingMobilizer< T > & set_quaternion ( const systems::Context< T > &  context,
const Quaternion< T > &  q_FM,
systems::State< T > *  state 
) const

Alternative signature to set_quaternion(context, q_FM) to set state to store the orientation of M in F given by the equaternion q_FM.

const QuaternionFloatingMobilizer< T > & set_translational_velocity ( systems::Context< T > *  context,
const Vector3< T > &  v_FM 
) const

Sets context to store the translational velocity v_FM of frame M in frame F.

Parameters
[out]contextThe context of the MultibodyTree this mobilizer belongs to.
[in]v_FMThe desired translational velocity of frame M in F, expressed in F.
Returns
a constant reference to this mobilizer.

Here is the call graph for this function:

Here is the caller graph for this function:

const QuaternionFloatingMobilizer< T > & set_translational_velocity ( const systems::Context< T > &  context,
const Vector3< T > &  v_FM,
systems::State< T > *  state 
) const

Alternative signature to set_translational_velocity(context, v_FM) to set state to store the translational velocity v_FM of M in F.

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

Sets state to store a configuration in which M coincides with F (i.e.

q_FM is the identity quaternion) and the spatial velocity V_FM of M in F is zero.

Implements Mobilizer< T >.

Here is the caller graph for this function:

const QuaternionFloatingMobilizer< T > & SetFromRotationMatrix ( systems::Context< T > *  context,
const Matrix3< T > &  R_FM 
) const

Sets context to store the quaternion q_FM which represents the same orientation of M in F as given by the rotation matrix R_FM.

Parameters
[out]contextThe context of the MultibodyTree to which this mobilizer belongs to.
[in]R_FMThe desired orientation of M in F given as a rotation matrix.
Returns
a constant reference to this mobilizer.

Here is the call graph for this function:

Here is the caller graph for this function:


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