Drake

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>
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< T >  get_angles (const systems::Context< T > &context) const 
Retrieves from context the three space xyz 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 xyz 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 xyz angles θ₁, θ₂, θ₃ of this mobilizer represent this rotation. More...  
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. 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 xyz angles θ₁, θ₂, θ₃ and zero across mobilizer angular velocity w_FM . More...  
Isometry3< T >  CalcAcrossMobilizerTransform (const MultibodyTreeContext< T > &context) const override 
Computes the acrossmobilizer transform X_FM(q) between the inboard frame F and the outboard frame M as a function of the space xyz angles θ₁, θ₂, θ₃ stored in context . More...  
SpatialVelocity< T >  CalcAcrossMobilizerSpatialVelocity (const MultibodyTreeContext< T > &context, const Eigen::Ref< const VectorX< T >> &v) const override 
Computes the acrossmobilizer velocity V_FM(q, v) of the outboard frame M measured and expressed in frame F as a function of the space xyz 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< T >  CalcAcrossMobilizerSpatialAcceleration (const MultibodyTreeContext< T > &context, const Eigen::Ref< const VectorX< T >> &vdot) const override 
Computes the acrossmobilizer 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 subspace 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 xyz 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 xyz 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  
SpaceXYZMobilizer &  operator= (const SpaceXYZMobilizer &)=delete 
SpaceXYZMobilizer (SpaceXYZMobilizer &&)=delete  
SpaceXYZMobilizer &  operator= (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  
MobilizerImpl &  operator= (const MobilizerImpl &)=delete 
MobilizerImpl &  operator= (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 MobilizerTopology &  get_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  
Mobilizer &  operator= (const Mobilizer &)=delete 
Mobilizer (Mobilizer &&)=delete  
Mobilizer &  operator= (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 >, kNq >  get_positions (const MultibodyTreeContext< T > &context) const 
Helper to return a const fixedsize Eigen::VectorBlock referencing the segment in the state vector corresponding to this mobilizer's state. More...  
Eigen::VectorBlock< VectorX< T >, kNq >  get_mutable_positions (MultibodyTreeContext< T > *context) const 
Helper to return a mutable fixedsize Eigen::VectorBlock referencing the segment in the state vector corresponding to this mobilizer's state. More...  
Eigen::VectorBlock< VectorX< T >, kNq >  get_mutable_positions (systems::State< T > *state) const 
Helper variant to return a const fixedsize Eigen::VectorBlock referencing the segment in the state corresponding to this mobilizer's generalized positions. More...  
Eigen::VectorBlock< VectorX< T >, kNq >  get_mutable_velocities (systems::State< T > *state) const 
Helper variant to return a const fixedsize Eigen::VectorBlock referencing the segment in the state corresponding to this mobilizer's generalized velocities. More...  
Eigen::VectorBlock< VectorX< T >, kNv >  get_mutable_velocities (MultibodyTreeContext< T > *context) const 
Helper to return a mutable fixedsize Eigen::VectorBlock referencing the segment in the state vector corresponding to this mobilizer's state. More...  
Eigen::VectorBlock< const VectorX< T >, kNv >  get_velocities (const MultibodyTreeContext< T > &context) const 
Helper to return a const fixedsize 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  
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 xyz
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 righthandrule 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 ∈ ℤ
.
xyz
angles (extrinsic) are equivalent to Body zyx
angles (intrinsic).θ₂
(many times referred to as the pitch angle) such that θ₂ = π/2 + kπ, ∀ k ∈ ℤ
.T  The scalar type. Must be a valid Eigen scalar. 
Instantiated templates for the following kinds of T's are provided:
They are already available to link against in the containing library. No other values for T are currently supported.

delete 

delete 

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.

overridevirtual 
Computes the acrossmobilizer 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 xyz 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 >.

overridevirtual 
Computes the acrossmobilizer velocity V_FM(q, v)
of the outboard frame M measured and expressed in frame F as a function of the space xyz 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 >.

overridevirtual 
Computes the acrossmobilizer transform X_FM(q)
between the inboard frame F and the outboard frame M as a function of the space xyz angles θ₁, θ₂, θ₃ stored in context
.
Implements Mobilizer< T >.

overrideprotectedvirtual 
Clones this Mobilizer (templated on T) to a mobilizer templated on double
.
tree_clone
. Implements Mobilizer< T >.

overrideprotectedvirtual 
Clones this Mobilizer (templated on T) to a mobilizer templated on AutoDiffXd.
tree_clone
. Implements Mobilizer< T >.
Vector3< T > get_angles  (  const systems::Context< T > &  context  )  const 
Retrieves from context
the three space xyz angles θ₁, θ₂, θ₃ which describe the state for this
mobilizer as documented in this class's documentation.
[in]  context  The context of the MultibodyTree this mobilizer belongs to. 
angles  The three space xyz 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) = θ₃ . 
std::logic_error  if context is not a valid MultibodyTreeContext. 
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.
[in]  context  The context of the MultibodyTree this mobilizer belongs to. 
w_FM  A vector in ℝ³ with the angular velocity of the outboard frame M in the inboard frame F, expressed in F. 
W_FM_F
.std::logic_error  if context is not a valid MultibodyTreeContext. 

overridevirtual 
Maps time derivatives of the space xyz angles θ₁, θ₂, θ₃ in qdot
to the generalized velocity v, which corresponds to the angular velocity w_FM
.
[in]  context  The context of the MultibodyTree this mobilizer belongs to. 
[in]  qdot  A vector containing the time derivatives of the space xyz angles θ₁, θ₂, θ₃ in qdot(0) , qdot(1) and qdot(2) , respectively. 
[out]  v  A 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 >.

overridevirtual 
Maps the generalized velocity v, which corresponds to the angular velocity w_FM
, to time derivatives of space xyz angles θ₁, θ₂, θ₃ in qdot
.
[in]  context  The context of the MultibodyTree this mobilizer belongs to. 
[in]  v  A 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]  qdot  A Vector3 packing of the time derivatives of the space xyz angles θ₁, θ₂, θ₃ in qdot(0) , qdot(1) and qdot(2) , respectively. 
θ₂
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 >.

delete 

delete 

overridevirtual 
Projects the spatial force F_Mo
on this
mobilizer's outboard frame M onto the subspace of motions spanned by the geometric Jacobian H_FM(q)
to obtain the generalized forces tau
(i.e.
the active components of F_Mo
).
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().
[in]  context  The context of the parent tree that owns this mobilizer. This mobilizer's generalized positions q are stored in this context. 
[in]  F_Mo_F  A SpatialForce applied at this mobilizer's outboard frame origin Mo , expressed in the inboard frame F. 
tau  The vector of generalized forces. It must live in ℝⁿᵛ. 
Implements Mobilizer< T >.
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 xyz angles θ₁, θ₂, θ₃, provided in the input argument angles
, which stores the with the format angles = [θ₁, θ₂, θ₃]
.
[out]  context  The context of the MultibodyTree this mobilizer belongs to. 
[in]  angles  A Vector3 which must pack values for the space xyz angles θ₁, θ₂, θ₃, described in this class's documentation, at entries angles(0) , angles(1) and angles(2) , respectively. 
this
mobilizer.std::logic_error  if context is not a valid MultibodyTreeContext. 
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
.
[out]  context  The context of the MultibodyTree this mobilizer belongs to. 
[in]  w_FM  A vector in ℝ³ with the desired angular velocity of the outboard frame M in the inboard frame F, expressed in F. 
this
mobilizer. 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.
[in]  context  The context of the MultibodyTree this mobilizer belongs to. 
[out]  state  On return, state will store the angular velocity w_FM of frame F in frame M. 
[in]  w_FM  A vector in ℝ³ with the desired angular velocity of the outboard frame M in the inboard frame F, expressed in F. 
this
mobilizer.

overridevirtual 
Sets state
to store zero space xyz angles θ₁, θ₂, θ₃ and zero across mobilizer angular velocity w_FM
.
Implements Mobilizer< T >.
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 xyz angles θ₁, θ₂, θ₃ of this
mobilizer represent this rotation.
[in]  context  The context of the MultibodyTree this mobilizer belongs to. 
[in]  R_FM  The desired pose of M in F. A valid element of SO(3) . 
this
mobilizer.R_FM
would correspond to a valid rotation in the special orthogonal group SO(3)
. To eliminate possible roundoff 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 xyz angles θ₁, θ₂, θ₃ that correspond to this rotation. See math::ProjectMatToOrthonormalMat().std::logic_error  if context is not a valid MultibodyTreeContext. 
std::logic_error  if an improper rotation results after projection of R_FM , that is, if the projected matrix's determinant is 1 . 