For certain robots (whose joint transforms are algebraic functions of joint variables, for example revolute/prismatic/floating-base joints), we can represent the pose (position, orientation) of each body, as rational functions, namely n(s) / d(s) where both the numerator n(s) and denominator d(s) are polynomials of s, and s is some variable related to the generalized position.
One example is that for a rotation matrix with angle θ and axis a, the rotation matrix can be written as I + sinθ A + (1-cosθ) A², where A is the skew-symmetric matrix from axis a. We can use the stereographic projection to substitute the trigonometric function sinθ and cosθ as cosθ = cos(θ*+Δθ) = cos(θ*)•cos(Δθ) - sin(θ*)•sin(Δθ) = (1-s²)/(1+s²) cos(θ*)- 2s/(1+s²) sin(θ*) (1) sinθ = sin(θ*+Δθ) = sin(θ*)•cos(Δθ) - cos(θ*)•sin(Δθ) = (1-s²)/(1+s²) sin(θ*)- 2s/(1+s²) cos(θ*) (2) where θ* is the angle around which we take the stereographic projection. θ = θ*+Δθ, and s = tan(Δθ/2). With (1) and (2), both sinθ and cosθ are written as a rational function of s. Thus the rotation matrix can be written as rational functions of s.
We will use q* to denote the nominal posture. For a revolute joint, the q* is the variable θ*. Note that the stereographic projection has a singularity at θ = θ* ± π, as s = tan(Δθ/2)=tan(±π/2) is ±infinity.
For prismatic joint, we define s = d - d*, where d is the displacement of the joint, and d* is the joint value in q*.
Currently we only support robots with revolute, prismatic and weld joints.
Throughout this file, we use the following convention
- q denotes the generalized position of the entire robot. It includes the angle of a revolute joint, the displacement of a prismatic joint, etc.
- θ denotes the revolute joint angle.
- t=tan((θ-θ*)/2)
- s is the vector of variables, such that the robot body pose is written as an algebraic function (rational function) of s.
|
| | RationalForwardKinematics (const MultibodyPlant< double > *plant) |
| Pose< symbolic::Polynomial > | CalcBodyPoseAsMultilinearPolynomial (const Eigen::Ref< const Eigen::VectorXd > &q_star, BodyIndex body_index, BodyIndex expressed_body_index) const |
| | Computes the pose X_AB as a multilinear polynomial function.
|
| symbolic::RationalFunction | ConvertMultilinearPolynomialToRationalFunction (const symbolic::Polynomial &e) const |
| | Given a polynomial whose indeterminates are cos_delta() and sin_delta() (typically this polynomial is obtained from calling CalcBodyPoseAsMultilinearPolynomial()), converts this polynomial to a rational function with indeterminates s().
|
| template<typename Derived> |
| std::enable_if_t< is_eigen_vector< Derived >::value, VectorX< typename Derived::Scalar > > | ComputeSValue (const Eigen::MatrixBase< Derived > &q_val, const Eigen::Ref< const Eigen::VectorXd > &q_star_val, bool angles_wrap_to_inf=false) const |
| | Computes values of s from q_val and q_star_val, while handling the index matching between q and s (we don't guarantee that s(i) is computed from q(i)).
|
| template<typename Derived> |
| std::enable_if_t< is_eigen_vector< Derived >::value, VectorX< typename Derived::Scalar > > | ComputeQValue (const Eigen::MatrixBase< Derived > &s_val, const Eigen::Ref< const Eigen::VectorXd > &q_star_val) const |
| | Computes values of q from s_val and q_star_val, while handling the index matching between q and s (we don't guarantee that s(i) is computed from q(i)).
|
| const MultibodyPlant< double > & | plant () const |
| Eigen::Map< const VectorX< symbolic::Variable > > | s () const |
| const std::vector< int > | map_mobilizer_to_s_index () const |
| | map_mobilizer_to_s_index_[mobilizer_index] returns the starting index of the mobilizer's variable in s_ (the variable will be contiguous in s for the same mobilizer).
|
| | RationalForwardKinematics (const RationalForwardKinematics &)=delete |
| RationalForwardKinematics & | operator= (const RationalForwardKinematics &)=delete |
| | RationalForwardKinematics (RationalForwardKinematics &&)=delete |
| RationalForwardKinematics & | operator= (RationalForwardKinematics &&)=delete |
