Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/math/rpy.hxx	Lines:	80	86	93.0 %
Date:	2024-04-26 13:14:21	Branches:	100	195	51.3 %


//
// Copyright (c) 2016-2020 CNRS INRIA
//

#ifndef __pinocchio_math_rpy_hxx__
#define __pinocchio_math_rpy_hxx__

#include <Eigen/Geometry>

#include "pinocchio/math/sincos.hpp"

namespace pinocchio
{
  namespace rpy
  {
    template<typename Scalar>
    Eigen::Matrix<Scalar,3,3>
    rpyToMatrix(const Scalar& r,
                const Scalar& p,
                const Scalar& y)
    {
      typedef Eigen::AngleAxis<Scalar> AngleAxis;
      typedef Eigen::Matrix<Scalar,3,1> Vector3s;
      return (AngleAxis(y, Vector3s::UnitZ())
              * AngleAxis(p, Vector3s::UnitY())
              * AngleAxis(r, Vector3s::UnitX())
             ).toRotationMatrix();
    }


    template<typename Vector3Like>
    Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
    rpyToMatrix(const Eigen::MatrixBase<Vector3Like> & rpy)
    {
      PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, rpy, 3, 1);
      return rpyToMatrix(rpy[0], rpy[1], rpy[2]);
    }


    template<typename Matrix3Like>
    Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
    matrixToRpy(const Eigen::MatrixBase<Matrix3Like> & R)
    {
      PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, R, 3, 3);
      assert(R.isUnitary() && "R is not a unitary matrix");

      typedef typename Matrix3Like::Scalar Scalar;
      typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> ReturnType;
      static const Scalar pi = PI<Scalar>();

      ReturnType res = R.eulerAngles(2,1,0).reverse();

      if(res[1] < -pi/2)
        res[1] += 2*pi;

      if(res[1] > pi/2)
      {
        res[1] = pi - res[1];
        if(res[0] < Scalar(0))
          res[0] += pi;
        else
          res[0] -= pi;
        // res[2] > 0 according to Eigen's eulerAngles doc, no need to check its sign
        res[2] -= pi;
      }

      return res;
    }


    template<typename Vector3Like>
    Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
    computeRpyJacobian(const Eigen::MatrixBase<Vector3Like> & rpy, const ReferenceFrame rf)
    {
      typedef typename Vector3Like::Scalar Scalar;
      typedef Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> ReturnType;
      ReturnType J;
      const Scalar p = rpy[1];
      Scalar sp, cp;
      SINCOS(p, &sp, &cp);
      switch (rf)
      {
        case LOCAL:
        {
          const Scalar r = rpy[0];
          Scalar sr, cr; SINCOS(r, &sr, &cr);
          J << Scalar(1.0), Scalar(0.0),   -sp,
               Scalar(0.0),          cr, sr*cp,
               Scalar(0.0),         -sr, cr*cp;
          return J;
        }
        case WORLD:
        case LOCAL_WORLD_ALIGNED:
        {
          const Scalar y = rpy[2];
          Scalar sy, cy; SINCOS(y, &sy, &cy);
          J << cp*cy,         -sy, Scalar(0.0),
               cp*sy,          cy, Scalar(0.0),
                 -sp, Scalar(0.0), Scalar(1.0);
          return J;
        }
        default:
        {
          throw std::invalid_argument("Bad reference frame.");
        }
      }
    }


    template<typename Vector3Like>
    Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
    computeRpyJacobianInverse(const Eigen::MatrixBase<Vector3Like> & rpy, const ReferenceFrame rf)
    {
      typedef typename Vector3Like::Scalar Scalar;
      typedef Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> ReturnType;
      ReturnType J;
      const Scalar p = rpy[1];
      Scalar sp, cp;
      SINCOS(p, &sp, &cp);
      Scalar tp = sp/cp;
      switch (rf)
      {
        case LOCAL:
        {
          const Scalar r = rpy[0];
          Scalar sr, cr; SINCOS(r, &sr, &cr);
          J << Scalar(1.0), sr*tp, cr*tp,
               Scalar(0.0),    cr,   -sr,
               Scalar(0.0), sr/cp, cr/cp;
          return J;
        }
        case WORLD:
        case LOCAL_WORLD_ALIGNED:
        {
          const Scalar y = rpy[2];
          Scalar sy, cy; SINCOS(y, &sy, &cy);
          J << cy/cp,  sy/cp, Scalar(0.0),
                 -sy,     cy, Scalar(0.0),
               cy*tp,  sy*tp, Scalar(1.0);
          return J;
        }
        default:
        {
          throw std::invalid_argument("Bad reference frame.");
        }
      }
    }

    template<typename Vector3Like0, typename Vector3Like1>
    Eigen::Matrix<typename Vector3Like0::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like0)::Options>
    computeRpyJacobianTimeDerivative(const Eigen::MatrixBase<Vector3Like0> & rpy, const Eigen::MatrixBase<Vector3Like1> & rpydot, const ReferenceFrame rf)
    {
      typedef typename Vector3Like0::Scalar Scalar;
      typedef Eigen::Matrix<typename Vector3Like0::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like0)::Options> ReturnType;
      ReturnType J;
      const Scalar p = rpy[1];
      const Scalar dp = rpydot[1];
      Scalar sp, cp;
      SINCOS(p, &sp, &cp);
      switch (rf)
      {
        case LOCAL:
        {
          const Scalar r = rpy[0];
          const Scalar dr = rpydot[0];
          Scalar sr, cr; SINCOS(r, &sr, &cr);
          J << Scalar(0.0), Scalar(0.0),               -cp*dp,
               Scalar(0.0),      -sr*dr,  cr*cp*dr - sr*sp*dp,
               Scalar(0.0),      -cr*dr, -sr*cp*dr - cr*sp*dp;
          return J;
        }
        case WORLD:
        case LOCAL_WORLD_ALIGNED:
        {
          const Scalar y = rpy[2];
          const Scalar dy = rpydot[2];
          Scalar sy, cy; SINCOS(y, &sy, &cy);
          J << -sp*cy*dp - cp*sy*dy,      -cy*dy, Scalar(0.0),
                cp*cy*dy - sp*sy*dp,      -sy*dy, Scalar(0.0),
                             -cp*dp, Scalar(0.0), Scalar(0.0);
          return J;
        }
        default:
        {
          throw std::invalid_argument("Bad reference frame.");
        }
      }
    }

  } // namespace rpy
}
#endif //#ifndef __pinocchio_math_rpy_hxx__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2016-2020 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_math_rpy_hxx__
6			#define __pinocchio_math_rpy_hxx__
7
8			#include <Eigen/Geometry>
9
10			#include "pinocchio/math/sincos.hpp"
11
12			namespace pinocchio
13			{
14			namespace rpy
15			{
16			template<typename Scalar>
17			Eigen::Matrix<Scalar,3,3>
18		307	rpyToMatrix(const Scalar& r,
19			const Scalar& p,
20			const Scalar& y)
21			{
22			typedef Eigen::AngleAxis<Scalar> AngleAxis;
23			typedef Eigen::Matrix<Scalar,3,1> Vector3s;
24			return (AngleAxis(y, Vector3s::UnitZ())
25			* AngleAxis(p, Vector3s::UnitY())
26			* AngleAxis(r, Vector3s::UnitX())
27	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	307	).toRotationMatrix();
28			}
29
30
31			template<typename Vector3Like>
32			Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
33		303	rpyToMatrix(const Eigen::MatrixBase<Vector3Like> & rpy)
34			{
35	✓✗✓✗	303	PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, rpy, 3, 1);
36		303	return rpyToMatrix(rpy[0], rpy[1], rpy[2]);
37			}
38
39
40			template<typename Matrix3Like>
41			Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
42		300	matrixToRpy(const Eigen::MatrixBase<Matrix3Like> & R)
43			{
44	✓✗✓✗	300	PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, R, 3, 3);
45	✓✗✓✗	300	assert(R.isUnitary() && "R is not a unitary matrix");
46
47			typedef typename Matrix3Like::Scalar Scalar;
48			typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> ReturnType;
49	✓✓✓✗	300	static const Scalar pi = PI<Scalar>();
50
51	✓✗✓✗	300	ReturnType res = R.eulerAngles(2,1,0).reverse();
52
53	✓✓	300	if(res[1] < -pi/2)
54		32	res[1] += 2*pi;
55
56	✓✓	300	if(res[1] > pi/2)
57			{
58		52	res[1] = pi - res[1];
59	✓✓	52	if(res[0] < Scalar(0))
60		28	res[0] += pi;
61			else
62		24	res[0] -= pi;
63			// res[2] > 0 according to Eigen's eulerAngles doc, no need to check its sign
64		52	res[2] -= pi;
65			}
66
67		300	return res;
68			}
69
70
71			template<typename Vector3Like>
72			Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
73		20	computeRpyJacobian(const Eigen::MatrixBase<Vector3Like> & rpy, const ReferenceFrame rf)
74			{
75			typedef typename Vector3Like::Scalar Scalar;
76			typedef Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> ReturnType;
77	✓✗	20	ReturnType J;
78	✓✗	20	const Scalar p = rpy[1];
79			Scalar sp, cp;
80		20	SINCOS(p, &sp, &cp);
81	✓✓✗	20	switch (rf)
82			{
83		10	case LOCAL:
84			{
85	✓✗	10	const Scalar r = rpy[0];
86		10	Scalar sr, cr; SINCOS(r, &sr, &cr);
87	✓✗✓✗ ✓✗	10	J << Scalar(1.0), Scalar(0.0), -sp,
88	✓✗✓✗ ✓✗	10	Scalar(0.0), cr, sr*cp,
89	✓✗✓✗ ✓✗	10	Scalar(0.0), -sr, cr*cp;
90		10	return J;
91			}
92		10	case WORLD:
93			case LOCAL_WORLD_ALIGNED:
94			{
95	✓✗	10	const Scalar y = rpy[2];
96		10	Scalar sy, cy; SINCOS(y, &sy, &cy);
97	✓✗✓✗ ✓✗	10	J << cp*cy, -sy, Scalar(0.0),
98	✓✗✓✗ ✓✗✓✗	10	cp*sy, cy, Scalar(0.0),
99	✓✗✓✗	10	-sp, Scalar(0.0), Scalar(1.0);
100		10	return J;
101			}
102			default:
103			{
104			throw std::invalid_argument("Bad reference frame.");
105			}
106			}
107			}
108
109
110			template<typename Vector3Like>
111			Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
112		4	computeRpyJacobianInverse(const Eigen::MatrixBase<Vector3Like> & rpy, const ReferenceFrame rf)
113			{
114			typedef typename Vector3Like::Scalar Scalar;
115			typedef Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> ReturnType;
116	✓✗	4	ReturnType J;
117	✓✗	4	const Scalar p = rpy[1];
118			Scalar sp, cp;
119		4	SINCOS(p, &sp, &cp);
120		4	Scalar tp = sp/cp;
121	✓✓✗	4	switch (rf)
122			{
123		2	case LOCAL:
124			{
125	✓✗	2	const Scalar r = rpy[0];
126		2	Scalar sr, cr; SINCOS(r, &sr, &cr);
127	✓✗✓✗ ✓✗	2	J << Scalar(1.0), srtp, crtp,
128	✓✗✓✗ ✓✗	2	Scalar(0.0), cr, -sr,
129	✓✗✓✗ ✓✗	2	Scalar(0.0), sr/cp, cr/cp;
130		2	return J;
131			}
132		2	case WORLD:
133			case LOCAL_WORLD_ALIGNED:
134			{
135	✓✗	2	const Scalar y = rpy[2];
136		2	Scalar sy, cy; SINCOS(y, &sy, &cy);
137	✓✗✓✗ ✓✗✓✗	2	J << cy/cp, sy/cp, Scalar(0.0),
138	✓✗✓✗	2	-sy, cy, Scalar(0.0),
139	✓✗✓✗ ✓✗	2	cytp, sytp, Scalar(1.0);
140		2	return J;
141			}
142			default:
143			{
144			throw std::invalid_argument("Bad reference frame.");
145			}
146			}
147			}
148
149			template<typename Vector3Like0, typename Vector3Like1>
150			Eigen::Matrix<typename Vector3Like0::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like0)::Options>
151		8	computeRpyJacobianTimeDerivative(const Eigen::MatrixBase<Vector3Like0> & rpy, const Eigen::MatrixBase<Vector3Like1> & rpydot, const ReferenceFrame rf)
152			{
153			typedef typename Vector3Like0::Scalar Scalar;
154			typedef Eigen::Matrix<typename Vector3Like0::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like0)::Options> ReturnType;
155	✓✗	8	ReturnType J;
156	✓✗	8	const Scalar p = rpy[1];
157	✓✗	8	const Scalar dp = rpydot[1];
158			Scalar sp, cp;
159		8	SINCOS(p, &sp, &cp);
160	✓✓✗	8	switch (rf)
161			{
162		4	case LOCAL:
163			{
164	✓✗	4	const Scalar r = rpy[0];
165	✓✗	4	const Scalar dr = rpydot[0];
166		4	Scalar sr, cr; SINCOS(r, &sr, &cr);
167	✓✗✓✗ ✓✗	4	J << Scalar(0.0), Scalar(0.0), -cp*dp,
168	✓✗✓✗ ✓✗	4	Scalar(0.0), -srdr, crcpdr - srsp*dp,
169	✓✗✓✗ ✓✗	4	Scalar(0.0), -crdr, -srcpdr - crsp*dp;
170		4	return J;
171			}
172		4	case WORLD:
173			case LOCAL_WORLD_ALIGNED:
174			{
175	✓✗	4	const Scalar y = rpy[2];
176	✓✗	4	const Scalar dy = rpydot[2];
177		4	Scalar sy, cy; SINCOS(y, &sy, &cy);
178	✓✗✓✗ ✓✗	4	J << -spcydp - cpsydy, -cy*dy, Scalar(0.0),
179	✓✗✓✗ ✓✗	4	cpcydy - spsydp, -sy*dy, Scalar(0.0),
180	✓✗✓✗ ✓✗	4	-cp*dp, Scalar(0.0), Scalar(0.0);
181		4	return J;
182			}
183			default:
184			{
185			throw std::invalid_argument("Bad reference frame.");
186			}
187			}
188			}
189
190			} // namespace rpy
191			}
192			#endif //#ifndef __pinocchio_math_rpy_hxx__