Drake
QuaternionFloatingJoint Class Reference

Defines a 6 dof tree joint (mobilizer) that uses a unit quaternion as the generalized orientation coordinates. More...

#include <drake/multibody/joints/quaternion_floating_joint.h>

Inheritance diagram for QuaternionFloatingJoint:
[legend]
Collaboration diagram for QuaternionFloatingJoint:
[legend]

Public Member Functions

 QuaternionFloatingJoint (const std::string &name, const Eigen::Isometry3d &transform_to_parent_body)
 
virtual ~QuaternionFloatingJoint ()
 
template<typename DerivedQ >
Eigen::Transform< typename DerivedQ::Scalar, 3, Eigen::Isometry > jointTransform (const Eigen::MatrixBase< DerivedQ > &q) const
 Returns the transform X_PB(q) where P is the parent body and B the child body connected by this joint. More...
 
template<typename DerivedQ , typename DerivedMS >
void motionSubspace (const Eigen::MatrixBase< DerivedQ > &q, Eigen::MatrixBase< DerivedMS > &motion_subspace, typename drake::math::Gradient< DerivedMS, Eigen::Dynamic >::type *dmotion_subspace=nullptr) const
 
template<typename DerivedQ , typename DerivedV >
void motionSubspaceDotTimesV (const Eigen::MatrixBase< DerivedQ > &q, const Eigen::MatrixBase< DerivedV > &v, Eigen::Matrix< typename DerivedQ::Scalar, 6, 1 > &motion_subspace_dot_times_v, typename drake::math::Gradient< Eigen::Matrix< typename DerivedQ::Scalar, 6, 1 >, Eigen::Dynamic >::type *dmotion_subspace_dot_times_vdq=nullptr, typename drake::math::Gradient< Eigen::Matrix< typename DerivedQ::Scalar, 6, 1 >, Eigen::Dynamic >::type *dmotion_subspace_dot_times_vdv=nullptr) const
 
template<typename DerivedQ >
void qdot2v (const Eigen::MatrixBase< DerivedQ > &q, Eigen::Matrix< typename DerivedQ::Scalar, Eigen::Dynamic, Eigen::Dynamic, 0, DrakeJoint::MAX_NUM_VELOCITIES, DrakeJoint::MAX_NUM_POSITIONS > &qdot_to_v, Eigen::Matrix< typename DerivedQ::Scalar, Eigen::Dynamic, Eigen::Dynamic > *dqdot_to_v) const
 For the QuaternionFloatingJoint, computes the matrix N⁺(q)∊ℝ⁶ˣ⁷ that maps generalized coordinate time derivatives qdot to generalized velocities v, with v=N⁺ qdot. More...
 
template<typename DerivedQ >
void v2qdot (const Eigen::MatrixBase< DerivedQ > &q, Eigen::Matrix< typename DerivedQ::Scalar, Eigen::Dynamic, Eigen::Dynamic, 0, DrakeJoint::MAX_NUM_POSITIONS, DrakeJoint::MAX_NUM_VELOCITIES > &v_to_qdot, Eigen::Matrix< typename DerivedQ::Scalar, Eigen::Dynamic, Eigen::Dynamic > *dv_to_qdot) const
 For the QuaternionFloatingJoint, computes the matrix N(q)∊ℝ⁷ˣ⁶ that maps generalized velocities v to generalized coordinate time derivatives qdot, with qdot=N v. More...
 
template<typename DerivedV >
Eigen::Matrix< typename DerivedV::Scalar, Eigen::Dynamic, 1 > frictionTorque (const Eigen::MatrixBase< DerivedV > &v) const
 
bool is_floating () const override
 
bool isFloating () const override
 
std::string get_position_name (int index) const override
 
std::string get_velocity_name (int index) const override
 
Eigen::VectorXd zeroConfiguration () const override
 
Eigen::VectorXd randomConfiguration (std::default_random_engine &generator) const override
 
std::string getPositionName (int index) const override
 
std::string getVelocityName (int index) const override
 

Protected Member Functions

std::unique_ptr< DrakeJointDoClone () const final
 
void DoInitializeClone (DrakeJoint *) const final
 

Detailed Description

Defines a 6 dof tree joint (mobilizer) that uses a unit quaternion as the generalized orientation coordinates.

Generalized coordinates (configuration variables)

There are 7 generalized coordinates q, organized as a position vector and quaternion. A tree joint connects an inboard (parent) body P to an outboard (child) body B. In those terms this joint's generalized coordinates are:

         --------- ------------- T
    q = | p_PB_P  |    q_PB     |
         --------- -------------  7×1
         px py pz   qw qx qy qz

where p_PB_P is the position vector from P's origin Po to B's origin Bo, expressed in the P basis, and q_PB is the quaternion that is equivalent to the rotation matrix R_PB. The second line shows the 7 generalized coordinate scalars in order. Note that qw is the scalar part of the quaternion while [qx qy qz] is the vector part. See Spatial Pose and Transform for more information about this notation.

The time derivatives qdot of the generalized coordinates, not to be confused with the generalized velocity variables v, are:

         --------- ------------- T
 qdot = | v_PB_P  |   qdot_PB   |
         --------- -------------  7×1

where v_PB_P = d_P/dt p_PB_P is the velocity of point Bo measured and expressed in the P frame, where we have emphasized that the derivative is taken in P, and qdot_PB = d/dt q_PB is the time derivative of the quaternion.

Generalized velocity

There are 6 generalized velocity variables v, organized as follows:

         --------- -------- T
    v = | ω_PB_B  | v_PB_B |
         --------- --------  6×1

where ω_PB_B is B's angular velocity in P, expressed in B, and v_PB_B is point Bo's translational velocity in P, expressed in B.

Note that the rotational and translational quantities are in the reverse order from those in the generalized coordinates, and that the velocities are expressed in the child frame B rather than in the parent. Clearly these are not the derivatives of the generalized coordinates!

The time derivatives of the generalized velocities are:

         --------- ----------- T
 vdot = | α_PB_B  | vdot_PB_B |
         --------- -----------  6×1

where α_PB_B is B's angular acceleration in P, expressed in B, and vdot_PB_B = d_B/dt v_PB_B where we have emphasized that the derivative is taken in the B frame, so this is not the acceleration of Bo in P. That acceleration is given by a_PB_B = vdot_PB_B + ω_PB_B × v_PB_B (still in B). Re-expressing a_PB_B in P provides the configuration second derivative a_PB_P = d²_P/dt² p_PB_P = R_PB*a_PB_B.

Constructor & Destructor Documentation

QuaternionFloatingJoint ( const std::string &  name,
const Eigen::Isometry3d &  transform_to_parent_body 
)
inline
virtual ~QuaternionFloatingJoint ( )
inlinevirtual

Member Function Documentation

std::unique_ptr< DrakeJoint > DoClone ( ) const
finalprotected

Here is the caller graph for this function:

void DoInitializeClone ( DrakeJoint ) const
inlinefinalprotected
Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> frictionTorque ( const Eigen::MatrixBase< DerivedV > &  v) const
inline

Here is the call graph for this function:

std::string get_position_name ( int  index) const
override

Here is the caller graph for this function:

std::string get_velocity_name ( int  index) const
override

Here is the caller graph for this function:

std::string getPositionName ( int  index) const
override

Here is the call graph for this function:

Here is the caller graph for this function:

std::string getVelocityName ( int  index) const
override

Here is the call graph for this function:

Here is the caller graph for this function:

bool is_floating ( ) const
inlineoverride

Here is the caller graph for this function:

bool isFloating ( ) const
inlineoverride

Here is the call graph for this function:

Eigen::Transform<typename DerivedQ::Scalar, 3, Eigen::Isometry> jointTransform ( const Eigen::MatrixBase< DerivedQ > &  q) const
inline

Returns the transform X_PB(q) where P is the parent body and B the child body connected by this joint.

Here is the call graph for this function:

void motionSubspace ( const Eigen::MatrixBase< DerivedQ > &  q,
Eigen::MatrixBase< DerivedMS > &  motion_subspace,
typename drake::math::Gradient< DerivedMS, Eigen::Dynamic >::type *  dmotion_subspace = nullptr 
) const
inline

Here is the call graph for this function:

void motionSubspaceDotTimesV ( const Eigen::MatrixBase< DerivedQ > &  q,
const Eigen::MatrixBase< DerivedV > &  v,
Eigen::Matrix< typename DerivedQ::Scalar, 6, 1 > &  motion_subspace_dot_times_v,
typename drake::math::Gradient< Eigen::Matrix< typename DerivedQ::Scalar, 6, 1 >, Eigen::Dynamic >::type *  dmotion_subspace_dot_times_vdq = nullptr,
typename drake::math::Gradient< Eigen::Matrix< typename DerivedQ::Scalar, 6, 1 >, Eigen::Dynamic >::type *  dmotion_subspace_dot_times_vdv = nullptr 
) const
inline

Here is the call graph for this function:

void qdot2v ( const Eigen::MatrixBase< DerivedQ > &  q,
Eigen::Matrix< typename DerivedQ::Scalar, Eigen::Dynamic, Eigen::Dynamic, 0, DrakeJoint::MAX_NUM_VELOCITIES, DrakeJoint::MAX_NUM_POSITIONS > &  qdot_to_v,
Eigen::Matrix< typename DerivedQ::Scalar, Eigen::Dynamic, Eigen::Dynamic > *  dqdot_to_v 
) const
inline

For the QuaternionFloatingJoint, computes the matrix N⁺(q)∊ℝ⁶ˣ⁷ that maps generalized coordinate time derivatives qdot to generalized velocities v, with v=N⁺ qdot.

The name signifies that N⁺=pinv(N) where N(q) is the matrix that maps v to qdot with qdot=N v and pinv() is the pseudoinverse (in this case the left pseudoinverse).

See the class description for precise definitions of the generalized coordinates and velocities. Because the velocities are not the time derivatives of the coordinates, rotations and translations are reversed, and different expressed-in frames are employed, N⁺ has the following elaborate structure:

       -------- -----------
      |  0₃ₓ₃  | Nq⁺_PB_B  |
 N⁺ = |--------|-----------|
      |  R_BP  |   0₃ₓ₄    |
       -------- ----------- 6×7

where Nq_PB_B is the matrix that maps angular velocity ω_PB_B to quaternion time derivative qdot_PB such that qdot_PB=Nq_PB_B*ω_PB_B, and Nq⁺_PB_B is the left pseudoinverse of Nq_PB_B.

Parameters
[in]qThe 7-element generalized configuration variable. See the class documentation for details. See warning below regarding the effect if the contained quaternion is not normalized.
[out]qdot_to_vThe matrix N⁺.
dqdot_to_vUnused, must be nullptr on entry.
Warning
Let s be the norm of the quaternion in q. If s ≠ 1, then we will calculate s*Nq⁺_PB_B in the upper right block of N⁺ so the resulting angular velocity vector will be scaled by s as well. This method neither performs a normalization check nor normalizes the quaternion orientation parameters. Implications for integration techniques must be carefully considered.

Here is the call graph for this function:

VectorXd randomConfiguration ( std::default_random_engine &  generator) const
override

Here is the call graph for this function:

Here is the caller graph for this function:

void v2qdot ( const Eigen::MatrixBase< DerivedQ > &  q,
Eigen::Matrix< typename DerivedQ::Scalar, Eigen::Dynamic, Eigen::Dynamic, 0, DrakeJoint::MAX_NUM_POSITIONS, DrakeJoint::MAX_NUM_VELOCITIES > &  v_to_qdot,
Eigen::Matrix< typename DerivedQ::Scalar, Eigen::Dynamic, Eigen::Dynamic > *  dv_to_qdot 
) const
inline

For the QuaternionFloatingJoint, computes the matrix N(q)∊ℝ⁷ˣ⁶ that maps generalized velocities v to generalized coordinate time derivatives qdot, with qdot=N v.

See the class description for precise definitions of the generalized coordinates and velocities. Because the velocities are not the time derivatives of the coordinates, rotations and translations are reversed, and different expressed-in frames are employed, N has the following elaborate structure:

       ------- ------
      |  0₃ₓ₃ | R_PB |
      |-------|------|
  N = |       |      |
      |Nq_PB_B| 0₄ₓ₃ |
      |       |      |
       -------------- 7×6

where Nq_PB_B is the matrix that maps angular velocity ω_PB_B to quaternion time derivative qdot_PB such that qdot_PB=Nq_PB_B*ω_PB_B.

Parameters
[in]qThe 7-element generalized configuration variable. See the class documentation for details. See warning below regarding the effect if the contained quaternion is not normalized.
[out]v_to_qdotThe matrix N.
dv_to_qdotUnused, must be nullptr on entry.
Warning
Let s be the norm of the quaternion in q. If s ≠ 1, then we will calculate s*Nq_PB_B in the lower left block of N so the resulting quaternion derivative will be scaled by s as well. This method neither performs a normalization check nor normalizes the quaternion orientation parameters. Implications for integration techniques must be carefully considered.

Here is the call graph for this function:

VectorXd zeroConfiguration ( ) const
override

Here is the caller graph for this function:


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