Drake

Mobilizer is a fundamental object within Drake's multibody engine used to specify the allowed motions between two Frame objects within a MultibodyTree. More...
#include <multibody/multibody_tree/mobilizer.h>
Public Member Functions  
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...  
virtual int  num_positions () const =0 
Returns the number of generalized coordinates granted by this mobilizer. More...  
virtual int  num_velocities () const =0 
Returns the number of generalized velocities granted by this mobilizer. 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 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...  
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...  
virtual std::unique_ptr< internal::BodyNode< T > >  CreateBodyNode (const internal::BodyNode< T > *parent_node, const Body< T > *body, const Mobilizer< T > *mobilizer) const =0 
For MultibodyTree internal use only. More...  
Does not allow copy, move, or assignment  
Mobilizer (const Mobilizer &)=delete  
Mobilizer &  operator= (const Mobilizer &)=delete 
Mobilizer (Mobilizer &&)=delete  
Mobilizer &  operator= (Mobilizer &&)=delete 
Methods that define a Mobilizer  
virtual void  set_zero_state (const systems::Context< T > &context, systems::State< T > *state) const =0 
Sets the state to what will be considered to be the zero configuration for this mobilizer. More...  
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...  
virtual Isometry3< T >  CalcAcrossMobilizerTransform (const MultibodyTreeContext< T > &context) const =0 
Computes the acrossmobilizer 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...  
virtual SpatialVelocity< T >  CalcAcrossMobilizerSpatialVelocity (const MultibodyTreeContext< T > &context, const Eigen::Ref< const VectorX< T >> &v) const =0 
Computes the acrossmobilizer spatial velocity V_FM(q, v) of the outboard frame M in the inboard frame F. More...  
virtual SpatialAcceleration< T >  CalcAcrossMobilizerSpatialAcceleration (const MultibodyTreeContext< T > &context, const Eigen::Ref< const VectorX< T >> &vdot) const =0 
Computes the acrossmobilizer spatial accelerations A_FM(q, v, v̇) of the outboard frame M in the inboard frame F. More...  
virtual void  ProjectSpatialForce (const MultibodyTreeContext< T > &context, const SpatialForce< T > &F_Mo_F, Eigen::Ref< VectorX< T >> tau) const =0 
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...  
virtual void  MapVelocityToQDot (const MultibodyTreeContext< T > &context, const Eigen::Ref< const VectorX< T >> &v, EigenPtr< VectorX< T >> qdot) const =0 
Computes the kinematic mapping q̇ = N(q)⋅v between generalized velocities v and time derivatives of the generalized positions qdot . More...  
virtual void  MapQDotToVelocity (const MultibodyTreeContext< T > &context, const Eigen::Ref< const VectorX< T >> &qdot, EigenPtr< VectorX< T >> v) const =0 
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 pseudoinverse of N(q) defined by MapVelocityToQDot(). More...  
Protected Member Functions  
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.  
virtual std::unique_ptr< Mobilizer< double > >  DoCloneToScalar (const MultibodyTree< double > &tree_clone) const =0 
Clones this Mobilizer (templated on T) to a mobilizer templated on double . More...  
virtual std::unique_ptr< Mobilizer< AutoDiffXd > >  DoCloneToScalar (const MultibodyTree< AutoDiffXd > &tree_clone) const =0 
Clones this Mobilizer (templated on T) to a mobilizer templated on AutoDiffXd. More...  
Mobilizer is a fundamental object within Drake's multibody engine used to specify the allowed motions between two Frame objects within a MultibodyTree.
Specifying the allowed motions between two Frame objects effectively also specifies a kinematic relationship between the two bodies associated with those two frames. Consider the following example to build a simple pendulum system:
A Mobilizer induces a tree structure within a MultibodyTree model, connecting an inboard (topologically closer to the world) frame to an outboard (topologically further from the world) frame. Every time a Mobilizer is added to a MultibodyTree (using the MultibodyTree::AddMobilizer() method), a number of degrees of freedom associated with the particular type of Mobilizer are added to the multibody system. In the example above for the single pendulum, adding a RevoluteMobilizer has two purposes:
A Mobilizer describes the kinematics relationship between an inboard frame F and an outboard frame M, introducing an nqdimensional vector of generalized coordinates q and an nvdimensional vector of generalized velocities v. Notice that in general nq != nv
, though nq == nv
is a very common case. The kinematic relationships introduced by a Mobilizer are fully specified by, [Seth 2010]:
v ∈ ℝⁿᵛ
and the spatial velocity V_FM ∈ M⁶
. This Jacobian can be thought of as the application: v ∈ ℝⁿᵛ → M⁶: V_FM(q, v) = H_FM(q) * v
, where 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). A Mobilizer implements this operator in the method CalcAcrossMobilizerSpatialVelocity().H_FM(q)
describing the relationship between the spatial force F_Mo_F ∈ F⁶
and the generalized forces tau ∈ ℝⁿᵛ
, where 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.) This mathematical object can be thought of as the application: F_Mo_F ∈ F⁶ → ℝⁿᵛ: tau = H_FMᵀ(q) * F_Mo_F
, where Mo
is M's origin (see Frames and Bodies for the monogram notation in use.) A Mobilizer implements this operator in the method ProjectSpatialForce().A_FM(q, v, v̇)
between the F and M frames as the application: v̇ ∈ ℝⁿᵛ → M⁶: A_FM(q, v, v̇) = H_FM(q) * v̇ + Ḣ_FM(q, v) * v
. A Mobilizer implements this application in CalcAcrossMobilizerSpatialAcceleration().q̇ = N(q)⋅v
, [Seth 2010]. N(q) is an nq x nv
matrix. A Mobilizer implements this application in MapVelocityToQDot().N(q)
. N⁺(q)
can be used to invert the relationship q̇ = N(q)⋅v
without residual error, provided that q̇
is in the range space of N(q)
(that is, if it could have been produced as q̇ = N(q)⋅v
for some v
). The application v = N⁺(q)⋅q̇
is implemented in MapQDotToVelocity().In general, nv != nq
. As an example, consider a quaternion mobilizer that would allow frame M to move freely with respect to frame F. For such a mobilizer the generalized positions vector might contain a quaternion to describe rotations plus a position vector to describe translations. However, we might choose the angular velocity w_FM
and the linear velocity v_FM
as the generalized velocities (or more generally, the spatial velocity V_FM
.) In such a case nq = 7
(4 dofs for a quaternion plus 3 dofs for a position vector) and nv = 6
(3 dofs for an angular velocity and 3 dofs for a linear velocity).
For a detailed discussion on the concept of a mobilizer please refer to [Seth 2010]. The Jacobian or "hinge" matrix H_FM(q)
is introduced in [Jain 2010], though be aware that what [Jain 2010] calls the hinge matrix is the transpose of the Jacobian H_FM matrix here in Drake. For details in the monogram notation used above please refer to Spatial Algebra.
Mobilizer is an abstract base class defining the minimum functionality that derived Mobilizer objects must implement in order to fully define the kinematic relationship between the two frames they connect. Geometric and analytical Jacobian matrices in the context of differential kinematics are described in [Sciavicco 2000].
The time derivative of the acrossmobilizer transform X_FM
is intimately related to the acrossmobilizer spatial velocity V_FM
. This relationship immediately implies a relationship between the analytical Jacobian dX_FM/dq
and the geometric Jacobian matrix H_FM
. The linear component of the spatial velocity V_FM
relates to the time derivative of X_FM
by:
v_FM = V_FM.translational() = dp_FM/dt = Xdot_FM.translational()
where p_FM = X_FM.translational()
and Xdot_FM = dX_FM/dt
. The time derivative of p_FM
can be rewritten as:
dp_FM/dt = dp_FM/dq * N(q) * v = Hv_FM * v
where Hv_FM
denotes the last three rows in H_FM
related with the translational component of the Jacobian matrix Therefore:
Hv_FM = dp_FM/dq(q) * N(q)
Similarly, for the rotational component:
dR_FM/dt = Xdot_FM.linear() = [w_FM] * R_FM = [Hw_FM * v] * R_FM
where [w_FM]
is the cross product matrix of the acrossmobilizer angular velocity w_FM
, R_FM
is the orientation of M in F, and Hw_FM
corresponds to the first three rows in H_FM
related to the angular component of the geometric Jacobian matrix. The time derivative of the orientation R_FM
can be expressed in terms of the analytic Jacobian of R_FM
as:
dR_FM/dt = dR_FM/dq * N(q) * v
These last two equations show that the angular components of the Jacobian matrix Hw_FM
are directly related to the gradients of the rotation matrix R_FM
. This relationhip is:
[Hwi_FM(q)] * R_FM(q) = dR_FM/dqi(q) * N(q)
corresponding to the ith generalized position qi
where Hwi_FM(q)
is the ith column of Hw_FM(q)
and dR_FM/dqi(q)
is the partial derivative of R_FM
with respect to the ith generalized coordinate for this mobilizer.
The power generated by a mobilizer can be computed in two equivalent ways. That is, the power can be computed in terms of the spatial force F_Mo
and the spatial velocity V_FM
as:
P = F_Moᵀ * V_FM
or in terms of the generalized forces tau = H_FMᵀ(q) * F_Mo
and the generalized velocities v as:
P = tauᵀ * v
Notice that spatial forces in the null space of H_FM(q)
do not perform any work. Since the result from the previous two expressions must be equal, the Jacobian operator H_FM(q)
and the transpose operator H_FMᵀ(q)
are constrained by:
(H_FMᵀ(q) * F) * v = Fᵀ * (H_FM(q) * v), ∀ v ∈ ℝⁿᵛ ∧ `F ∈ F⁶`
Therefore, this enforces a relationship to the operations implemented by CalcAcrossMobilizerSpatialVelocity() and ProjectSpatialForce() for any Mobilizer object.
T  The scalar type. Must be a valid Eigen scalar. 
The minimum amount of information that we need to define a Mobilizer is the knowledge of the inboard and outboard frames it connects.
Subclasses of Mobilizer are therefore required to provide this information in their respective constructors.
std::runtime_error  if inboard_frame and outboard_frame reference the same frame object. 

pure virtual 
Computes the acrossmobilizer 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().
[in]  context  The 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]  vdot  The vector of generalized velocities' time derivatives v̇. It must live in ℝⁿᵛ. 
A_FM  The acrossmobilizer spatial acceleration of the outboard frame M measured and expressed in the inboard frame F. 
Implemented in SpaceXYZMobilizer< T >, QuaternionFloatingMobilizer< T >, and RevoluteMobilizer< T >.

pure virtual 
Computes the acrossmobilizer 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().
[in]  context  The context of the parent tree that owns this mobilizer. This mobilizer's generalized positions q are inferred from this context. 
[in]  v  A vector of generalized velocities. It must live in ℝⁿᵛ. 
V_FM  The acrossmobilizer spatial velocity of the outboard frame M measured and expressed in the inboard frame F. 
Implemented in SpaceXYZMobilizer< T >, QuaternionFloatingMobilizer< T >, and RevoluteMobilizer< T >.

pure virtual 
Computes the acrossmobilizer 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 fixedsize vector of generalized positions for this
mobilizer from context
with:
Additionally, context
can provide any other parameters the mobilizer could depend on.
Implemented in SpaceXYZMobilizer< T >, QuaternionFloatingMobilizer< T >, and RevoluteMobilizer< T >.

inline 
NVI to DoCloneToScalar() templated on the scalar type of the new clone to be created.
This method is mostly intended to be called by MultibodyTree::CloneToScalar(). Most users should not call this clone method directly but rather clone the entire parent MultibodyTree if needed.

pure virtual 
For MultibodyTree internal use only.
Implemented in MobilizerImpl< T, compile_time_num_positions, compile_time_num_velocities >, MobilizerImpl< T, 7, 6 >, MobilizerImpl< T, 1, 1 >, and MobilizerImpl< T, 3, 3 >.

protectedpure virtual 
Clones this Mobilizer (templated on T) to a mobilizer templated on double
.
tree_clone
. Implemented in SpaceXYZMobilizer< T >, QuaternionFloatingMobilizer< T >, and RevoluteMobilizer< T >.

protectedpure virtual 
Clones this Mobilizer (templated on T) to a mobilizer templated on AutoDiffXd.
tree_clone
. Implemented in SpaceXYZMobilizer< T >, QuaternionFloatingMobilizer< T >, and RevoluteMobilizer< T >.

inline 
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.
This method aborts if the input array is not of size MultibodyTree::num_velocities().

inline 
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.
This method aborts if the input array is not of size MultibodyTree::num_velocities().

inline 
Mutable version of get_accelerations_from_array().

inline 
Mutable version of get_generalized_forces_from_array().

inline 
Mutable version of get_positions_from_array().

inline 
Mutable version of get_velocities_from_array().

inline 
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.
This method aborts if q_array
is not of size MultibodyTree::num_positions().

inline 
Returns the topology information for this mobilizer.
Users should not need to call this method since MobilizerTopology is an internal bookkeeping detail.

inline 
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.
This method aborts if the input array is not of size MultibodyTree::num_velocities().

inline 
Returns a constant reference to the body associated with this
mobilizer's inboard frame.

inline 
Returns a constant reference to the inboard frame.

pure virtual 
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 pseudoinverse of N(q)
defined by MapVelocityToQDot().
The generalized positions vector is stored in context
.
Implemented in SpaceXYZMobilizer< T >, QuaternionFloatingMobilizer< T >, and RevoluteMobilizer< T >.

pure virtual 
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
.
Implemented in SpaceXYZMobilizer< T >, QuaternionFloatingMobilizer< T >, and RevoluteMobilizer< T >.

pure virtual 
Returns the number of generalized coordinates granted by this mobilizer.
As an example, consider RevoluteMobilizer, for which num_positions() == 1
since RevoluteMobilizer adds a single generalized coordinate representing the rotational degree of freedom about a given axis between the inboard and outboard frames. Another example would be a 6 DOF "free" mobilizer internally using a quaternion representation to parametrize free rotations and a position vector to parametrize free translations; this method would return 7 (a quaternion plus a position vector).
Implemented in MobilizerImpl< T, compile_time_num_positions, compile_time_num_velocities >, MobilizerImpl< T, 7, 6 >, MobilizerImpl< T, 1, 1 >, and MobilizerImpl< T, 3, 3 >.

pure virtual 
Returns the number of generalized velocities granted by this mobilizer.
Given that all physics occurs in the generalized velocities space, the number of generalized velocities exactly matches the number of degrees of freedom granted by the mobilizer. As an example, consider RevoluteMobilizer, for which num_velocities() == 1
since for RevoluteMobilizer its one and only generalized velocity describes the magnitude of the angular velocity about a given axis between the inboard and outboard frames.
Implemented in MobilizerImpl< T, compile_time_num_positions, compile_time_num_velocities >, MobilizerImpl< T, 7, 6 >, MobilizerImpl< T, 1, 1 >, and MobilizerImpl< T, 3, 3 >.

inline 
Returns a constant reference to the body associated with this
mobilizer's outboard frame.

inline 
Returns a constant reference to the outboard frame.

pure virtual 
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 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().
[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 ℝⁿᵛ. 
Implemented in SpaceXYZMobilizer< T >, QuaternionFloatingMobilizer< T >, and RevoluteMobilizer< T >.

inline 
Sets the state stored in context
to a zero configuration as defined by set_zero_state().
See set_zero_state() for details.

pure virtual 
Sets the state
to what will be considered to be the zero configuration for this
mobilizer.
For most mobilizers the zero configuration corresponds to the value of generalized positions at which the inboard frame F and the outboard frame coincide or, in other words, when X_FM = Id
is the identity pose. In the general case however, the zero configuration will correspond to a value of the generalized positions for which X_FM = X_FM_ref
where X_FM_ref
may generally be different from the identity transformation. In other words, X_FM_ref = CalcAcrossMobilizerTransform(ref_context)
where ref_context
is a Context storing a State set to the zero configuration with set_zero_state(). In addition, all generalized velocities are set to zero in the zero configuration.
Most often the zero configuration will correspond to setting the vector of generalized positions related to this mobilizer to zero. However, in the general case, setting all generalized coordinates to zero does not correspond to the zero configuration and it might even not represent a mathematicaly valid one. Consider for instance a quaternion mobilizer, for which its zero configuration corresponds to the quaternion [1, 0, 0, 0].
Implemented in SpaceXYZMobilizer< T >, QuaternionFloatingMobilizer< T >, and RevoluteMobilizer< T >.