Drake
SpaceXYZMobilizer< T > Class Template Referencefinal

This mobilizer models a gimbal joint between an inboard frame F and an outboard frame M that allows frame M to rotate freely with respect to F ( though a gimbal joint provides arbitrary orientation like a ball joint but with some restrictions, discussed below). More...

#include <multibody/multibody_tree/space_xyz_mobilizer.h>

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

Public Member Functions

 SpaceXYZMobilizer (const Frame< T > &inboard_frame_F, const Frame< T > &outboard_frame_M)
 Constructor for a SpaceXYZMobilizer between an inboard frame F inboard_frame_F and an outboard frame M outboard_frame_M granting three rotational degree of freedom corresponding to angles θ₁, θ₂, θ₃ as described in this class's documentation. More...
 
Vector3< Tget_angles (const systems::Context< T > &context) const
 Retrieves from context the three space x-y-z angles θ₁, θ₂, θ₃ which describe the state for this mobilizer as documented in this class's documentation. More...
 
const SpaceXYZMobilizer< T > & set_angles (systems::Context< T > *context, const Vector3< T > &angles) const
 Sets in context the state for this mobilizer to have the space x-y-z angles θ₁, θ₂, θ₃, provided in the input argument angles, which stores the with the format angles = [θ₁, θ₂, θ₃]. More...
 
const SpaceXYZMobilizer< T > & SetFromRotationMatrix (systems::Context< T > *context, const Matrix3< T > &R_FM) const
 Given a desired orientation R_FM of frame M in F as a rotation matrix, This method sets context so that the generalized coordinates corresponding to the space x-y-z angles θ₁, θ₂, θ₃ of this mobilizer represent this rotation. More...
 
Vector3< Tget_angular_velocity (const systems::Context< T > &context) const
 Retrieves from context the angular velocity w_FM of the outboard frame M in the inboard frame F, expressed in F. More...
 
const SpaceXYZMobilizer< T > & set_angular_velocity (systems::Context< T > *context, const Vector3< T > &w_FM) const
 Sets in context the state for this mobilizer so that the angular velocity of the outboard frame M in the inboard frame F is w_FM. More...
 
const SpaceXYZMobilizer< T > & set_angular_velocity (const systems::Context< T > &context, const Vector3< T > &w_FM, systems::State< T > *state) const
 Stores in state the angular velocity w_FM of the outboard frame M in the inboard frame F corresponding to this mobilizer. More...
 
void set_zero_state (const systems::Context< T > &context, systems::State< T > *state) const override
 Sets state to store zero space x-y-z angles θ₁, θ₂, θ₃ and zero across mobilizer angular velocity w_FM. More...
 
Isometry3< TCalcAcrossMobilizerTransform (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 space x-y-z angles θ₁, θ₂, θ₃ stored in context. More...
 
SpatialVelocity< TCalcAcrossMobilizerSpatialVelocity (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 space x-y-z angles θ₁, θ₂, θ₃ stored in context and of the input generalized velocity v which contains the components of the angular velocity w_FM expressed in frame F. More...
 
SpatialAcceleration< TCalcAcrossMobilizerSpatialAcceleration (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 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
 Maps the generalized velocity v, which corresponds to the angular velocity w_FM, to time derivatives of space x-y-z angles θ₁, θ₂, θ₃ in qdot. More...
 
void MapQDotToVelocity (const MultibodyTreeContext< T > &context, const Eigen::Ref< const VectorX< T >> &qdot, EigenPtr< VectorX< T >> v) const override
 Maps time derivatives of the space x-y-z angles θ₁, θ₂, θ₃ in qdot to the generalized velocity v, which corresponds to the angular velocity w_FM. More...
 
Does not allow copy, move, or assignment
 SpaceXYZMobilizer (const SpaceXYZMobilizer &)=delete
 
SpaceXYZMobilizeroperator= (const SpaceXYZMobilizer &)=delete
 
 SpaceXYZMobilizer (SpaceXYZMobilizer &&)=delete
 
SpaceXYZMobilizeroperator= (SpaceXYZMobilizer &&)=delete
 
- Public Member Functions inherited from MobilizerImpl< T, 3, 3 >
 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, 3, 3 >
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, 3, 3 >
enum  
 

Detailed Description

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

This mobilizer models a gimbal joint between an inboard frame F and an outboard frame M that allows frame M to rotate freely with respect to F ( though a gimbal joint provides arbitrary orientation like a ball joint but with some restrictions, discussed below).

No translational motion of M in F is allowed and the inboard frame origin Fo and the outboard frame origin Mo are coincident at all times.

The orientation R_FM of the outboard frame M in F is parametrized with space x-y-z Euler angles (also known as extrinsic angles). That is, the generalized coordinates for this mobilizer correspond to angles θ₁, θ₂, θ₃, for a sequence of rotations about the x̂, ŷ, ẑ axes solidary with frame F, respectively. Mathematically, rotation R_FM is given in terms of angles θ₁, θ₂, θ₃ by:

  R_FM(q) = Rz(θ₃) * Ry(θ₂) * Rx(θ₁)

where Rx(θ), Ry(θ) and Rz(θ) correspond to the elemental rotations in amount of θ about the x, y and z axes respectively. Zero θ₁, θ₂, θ₃ angles define the "zero configuration" which corresponds to frames F and M being coincident, see set_zero_configuration(). Angles θ₁, θ₂, θ₃ are defined to be positive according to the right-hand-rule with the thumb aligned in the direction of their respective axes.

The generalized velocities for this mobilizer correspond to the angular velocity w_FM of frame M in F, expressed in frame F. MapVelocityToQDot() maps the angular velocity w_FM to Euler angles's rates while MapQDotToVelocity() maps Euler angles's rates to angular velocity w_FM. While the mapping MapVelocityToQDot() always exists, its inverse MapQDotToVelocity() is singular for values of θ₂ (many times referred to as the pitch angle) such that θ₂ = π/2 + kπ, ∀ k ∈ ℤ.

Note
Space x-y-z angles (extrinsic) are equivalent to Body z-y-x angles (intrinsic).
This particular choice of generalized coordinates θ₁, θ₂, θ₃ for this mobilizer is many times referred to as the roll, pitch and yaw angles by many dynamicists. They are also known as the Tait-Bryan angles or Cardan angles.
The mapping from angular velocity to Euler angle's rates is singular for angle θ₂ (many times referred to as the pitch angle) such that θ₂ = π/2 + kπ, ∀ k ∈ ℤ.
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

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

Constructor for a SpaceXYZMobilizer between an inboard frame F inboard_frame_F and an outboard frame M outboard_frame_M granting three rotational degree of freedom corresponding to angles θ₁, θ₂, θ₃ as described in this class's documentation.

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.

The acceleration A_FM will be a function of the generalized positions q (space x-y-z angles) stored in context, of the generalized velocities v (angular velocity w_FM) also stored in context and of the supplied generalized accelerations vdot, which in this case correspond to angular acceleration of M in F alpha_FM = Dt_F(w_FM) (see Time Derivatives of Multibody Quantities for our notation of time derivatives in different reference frames).

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 space x-y-z angles θ₁, θ₂, θ₃ stored in context and of the input generalized velocity v which contains the components of the angular velocity w_FM expressed in 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 space x-y-z angles θ₁, θ₂, θ₃ stored in context.

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_angles ( const systems::Context< T > &  context) const

Retrieves from context the three space x-y-z angles θ₁, θ₂, θ₃ which describe the state for this mobilizer as documented in this class's documentation.

Parameters
[in]contextThe context of the MultibodyTree this mobilizer belongs to.
Return values
anglesThe three space x-y-z angles θ₁, θ₂, θ₃, associated with the sequence of rotations about the space fixed axes x̂, ŷ, ẑ, respectively packed and returned as a Vector3 with entries angles(0) = θ₁, angles(1) = θ₂, angles(2) = θ₃.
Exceptions
std::logic_errorif context is not a valid MultibodyTreeContext.

Here is the caller graph for this function:

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

Retrieves from context the angular velocity w_FM of the outboard frame M in the inboard frame F, expressed in F.

Parameters
[in]contextThe context of the MultibodyTree this mobilizer belongs to.
Return values
w_FMA vector in ℝ³ with the angular velocity of the outboard frame M in the inboard frame F, expressed in F.
Note
Many dynamicists follow the convention of expressing angular velocity in the outboard frame M; we return it expressed in the inboard frame F. That is, this method returns W_FM_F.
Exceptions
std::logic_errorif context is not a valid MultibodyTreeContext.

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

Maps time derivatives of the space x-y-z angles θ₁, θ₂, θ₃ in qdot to the generalized velocity v, which corresponds to the angular velocity w_FM.

Parameters
[in]contextThe context of the MultibodyTree this mobilizer belongs to.
[in]qdotA vector containing the time derivatives of the space x-y-z angles θ₁, θ₂, θ₃ in qdot(0), qdot(1) and qdot(2), respectively.
[out]vA vector of generalized velocities for this Mobilizer which should correspond to a vector in ℝ³ for an angular velocity w_FM of M in F.

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

Maps the generalized velocity v, which corresponds to the angular velocity w_FM, to time derivatives of space x-y-z angles θ₁, θ₂, θ₃ in qdot.

Parameters
[in]contextThe context of the MultibodyTree this mobilizer belongs to.
[in]vA vector of generalized velocities for this Mobilizer which should correspond to a vector in ℝ³ for an angular velocity w_FM of M in F.
[out]qdotA Vector3 packing of the time derivatives of the space x-y-z angles θ₁, θ₂, θ₃ in qdot(0), qdot(1) and qdot(2), respectively.
Warning
The mapping from Euler angle's rates to angular velocity is singular for angle θ₂ such that θ₂ = π/2 + kπ, ∀ k ∈ ℤ. To avoid working close to this singularity (which could potentially result in large errors for qdot), this method aborts when the absolute value of the cosine of θ₂ is smaller than 10⁻³, a number arbitrarily chosen to this end.

Implements Mobilizer< T >.

Here is the call graph for this function:

Here is the caller graph for this function:

SpaceXYZMobilizer& operator= ( SpaceXYZMobilizer< T > &&  )
delete
SpaceXYZMobilizer& operator= ( const SpaceXYZMobilizer< 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 get_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 get_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 SpaceXYZMobilizer< T > & set_angles ( systems::Context< T > *  context,
const Vector3< T > &  angles 
) const

Sets in context the state for this mobilizer to have the space x-y-z angles θ₁, θ₂, θ₃, provided in the input argument angles, which stores the with the format angles = [θ₁, θ₂, θ₃].

Parameters
[out]contextThe context of the MultibodyTree this mobilizer belongs to.
[in]anglesA Vector3 which must pack values for the space x-y-z angles θ₁, θ₂, θ₃, described in this class's documentation, at entries angles(0), angles(1) and angles(2), respectively.
Returns
a constant reference to this mobilizer.
Exceptions
std::logic_errorif context is not a valid MultibodyTreeContext.

Here is the caller graph for this function:

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

Sets in context the state for this mobilizer so that the angular velocity of the outboard frame M in the inboard frame F is w_FM.

Parameters
[out]contextThe context of the MultibodyTree this mobilizer belongs to.
[in]w_FMA vector in ℝ³ with the desired angular velocity of the outboard frame M in the inboard frame 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 SpaceXYZMobilizer< T > & set_angular_velocity ( const systems::Context< T > &  context,
const Vector3< T > &  w_FM,
systems::State< T > *  state 
) const

Stores in state the angular velocity w_FM of the outboard frame M in the inboard frame F corresponding to this mobilizer.

Parameters
[in]contextThe context of the MultibodyTree this mobilizer belongs to.
[out]stateOn return, state will store the angular velocity w_FM of frame F in frame M.
[in]w_FMA vector in ℝ³ with the desired angular velocity of the outboard frame M in the inboard frame F, expressed in F.
Returns
a constant reference to this mobilizer.
void set_zero_state ( const systems::Context< T > &  context,
systems::State< T > *  state 
) const
overridevirtual

Sets state to store zero space x-y-z angles θ₁, θ₂, θ₃ and zero across mobilizer angular velocity w_FM.

Implements Mobilizer< T >.

Here is the caller graph for this function:

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

Given a desired orientation R_FM of frame M in F as a rotation matrix, This method sets context so that the generalized coordinates corresponding to the space x-y-z angles θ₁, θ₂, θ₃ of this mobilizer represent this rotation.

Parameters
[in]contextThe context of the MultibodyTree this mobilizer belongs to.
[in]R_FMThe desired pose of M in F. A valid element of SO(3).
Returns
a constant reference to this mobilizer.
Warning
Ideally, R_FM would correspond to a valid rotation in the special orthogonal group SO(3). To eliminate possible round-off errors in the input matrix R_FM this method performs a projection of R_FM into its closest element in SO(3) and then computes the space x-y-z angles θ₁, θ₂, θ₃ that correspond to this rotation. See math::ProjectMatToOrthonormalMat().
Exceptions
std::logic_errorif context is not a valid MultibodyTreeContext.
std::logic_errorif an improper rotation results after projection of R_FM, that is, if the projected matrix's determinant is -1.

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: