Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/multibody/liegroup/vector-space.hpp	Lines:	85	120	70.8 %
Date:	2024-04-26 13:14:21	Branches:	43	87	49.4 %


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

#ifndef __pinocchio_multibody_liegroup_vector_space_operation_hpp__
#define __pinocchio_multibody_liegroup_vector_space_operation_hpp__

#include "pinocchio/multibody/liegroup/liegroup-base.hpp"

#include <stdexcept>
#include <boost/integer/static_min_max.hpp>

namespace pinocchio
{
  template<int Dim, typename Scalar, int Options = 0> struct VectorSpaceOperationTpl;

  template<int Dim, typename _Scalar, int _Options>
  struct traits< VectorSpaceOperationTpl<Dim,_Scalar,_Options> >
  {
    typedef _Scalar Scalar;
    enum {
      Options = _Options,
      NQ = Dim,
      NV = Dim
    };
  };

  template<int Dim, typename _Scalar, int _Options>
  struct VectorSpaceOperationTpl
  : public LieGroupBase< VectorSpaceOperationTpl<Dim,_Scalar,_Options> >
  {
    PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(VectorSpaceOperationTpl);

    /// Constructor
    /// \param size size of the vector space: should be the equal to template
    ///        argument for static sized vector-spaces.
    VectorSpaceOperationTpl(int size = boost::static_signed_max<0,Dim>::value)
    : size_(size)
    {
      assert(size_.value() >= 0);
    }

    /// Constructor
    /// \param other other VectorSpaceOperationTpl from which to retrieve size
    VectorSpaceOperationTpl(const VectorSpaceOperationTpl & other)
    : Base(), size_(other.size_.value())
    {
      assert(size_.value() >= 0);
    }

    VectorSpaceOperationTpl& operator=(const VectorSpaceOperationTpl& other)
    {
      size_.setValue(other.size_.value());
      assert(size_.value() >= 0);
      return *this;
    }

    Index nq () const
    {
      return size_.value();
    }
    Index nv () const
    {
      return size_.value();
    }

    ConfigVector_t neutral () const
    {
      return ConfigVector_t::Zero(size_.value());
    }

    std::string name () const
    {
      std::ostringstream oss; oss << "R^" << nq();
      return oss.str ();
    }

    template <class ConfigL_t, class ConfigR_t, class Tangent_t>
    static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                const Eigen::MatrixBase<ConfigR_t> & q1,
                                const Eigen::MatrixBase<Tangent_t> & d)
    {
      PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d) = q1 - q0;
    }

    template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
    void dDifference_impl (const Eigen::MatrixBase<ConfigL_t> &,
                           const Eigen::MatrixBase<ConfigR_t> &,
                           const Eigen::MatrixBase<JacobianOut_t> & J) const
    {
      if (arg == ARG0)
        PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J) = -JacobianMatrix_t::Identity(size_.value(),size_.value());
      else if (arg == ARG1)
        PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J).setIdentity();
    }

    template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
    static void dDifference_product_impl(const ConfigL_t &,
                                  const ConfigR_t &,
                                  const JacobianIn_t & Jin,
                                  JacobianOut_t & Jout,
                                  bool,
                                  const AssignmentOperatorType op)
    {
      switch (op) {
        case SETTO:
          if (arg == ARG0) Jout = - Jin;
          else             Jout =   Jin;
          return;
        case ADDTO:
          if (arg == ARG0) Jout -= Jin;
          else             Jout += Jin;
          return;
        case RMTO:
          if (arg == ARG0) Jout += Jin;
          else             Jout -= Jin;
          return;
      }
    }

    template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
    static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
                               const Eigen::MatrixBase<Velocity_t> & v,
                               const Eigen::MatrixBase<ConfigOut_t> & qout)
    {
      PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout) = q + v;
    }

    template <class Config_t, class Jacobian_t>
    static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> &,
                                                const Eigen::MatrixBase<Jacobian_t> & J)
    {
      PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).setIdentity();
    }

    template <class Config_t, class Tangent_t, class JacobianOut_t>
    static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
                                   const Eigen::MatrixBase<Tangent_t> & /*v*/,
                                   const Eigen::MatrixBase<JacobianOut_t> & J,
                                   const AssignmentOperatorType op=SETTO)
    {
      Eigen::MatrixBase<JacobianOut_t>& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
      switch(op)
        {
        case SETTO:
          Jout.setIdentity();
          break;
        case ADDTO:
          Jout.diagonal().array() += Scalar(1);
          break;
        case RMTO:
          Jout.diagonal().array() -= Scalar(1);
          break;
        default:
          assert(false && "Wrong Op requesed value");
          break;
        }
    }

    template <class Config_t, class Tangent_t, class JacobianOut_t>
    static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
                                   const Eigen::MatrixBase<Tangent_t> & /*v*/,
                                   const Eigen::MatrixBase<JacobianOut_t> & J,
                                   const AssignmentOperatorType op=SETTO)
    {
      Eigen::MatrixBase<JacobianOut_t>& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
      switch(op)
        {
        case SETTO:
          Jout.setIdentity();
          break;
        case ADDTO:
          Jout.diagonal().array() += Scalar(1);
          break;
        case RMTO:
          Jout.diagonal().array() -= Scalar(1);
          break;
        default:
          assert(false && "Wrong Op requesed value");
          break;
        }
    }

    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    static void dIntegrate_product_impl(const Config_t &,
                                 const Tangent_t &,
                                 const JacobianIn_t & Jin,
                                 JacobianOut_t & Jout,
                                 bool,
                                 const ArgumentPosition,
                                 const AssignmentOperatorType op)
    {
      switch(op) {
        case SETTO:
          Jout = Jin;
          break;
        case ADDTO:
          Jout += Jin;
          break;
        case RMTO:
          Jout -= Jin;
          break;
        default:
          assert(false && "Wrong Op requesed value");
          break;
      }
    }

    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
                                     const Eigen::MatrixBase<Tangent_t> & /*v*/,
                                     const Eigen::MatrixBase<JacobianIn_t> & Jin,
                                     const Eigen::MatrixBase<JacobianOut_t> & Jout)
    {
      PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout) = Jin;
    }

    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
                                     const Eigen::MatrixBase<Tangent_t> & /*v*/,
                                     const Eigen::MatrixBase<JacobianIn_t> & Jin,
                                     const Eigen::MatrixBase<JacobianOut_t> & Jout)
    {
      PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout) = Jin;
    }

    template <class Config_t, class Tangent_t, class Jacobian_t>
    static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
                                     const Eigen::MatrixBase<Tangent_t> & /*v*/,
                                     const Eigen::MatrixBase<Jacobian_t> & /*J*/) {}

    template <class Config_t, class Tangent_t, class Jacobian_t>
    static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
                                     const Eigen::MatrixBase<Tangent_t> & /*v*/,
                                     const Eigen::MatrixBase<Jacobian_t> & /*J*/) {}



    // template <class ConfigL_t, class ConfigR_t>
    // static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                       // const Eigen::MatrixBase<ConfigR_t> & q1)

    template <class Config_t>
    static void normalize_impl (const Eigen::MatrixBase<Config_t>& /*qout*/)
    {}

    template <class Config_t>
    static bool isNormalized_impl (const Eigen::MatrixBase<Config_t>& /*qout*/, const Scalar& /*prec*/)
    {
      return true;
    }

    template <class Config_t>
    static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
    {
      PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).setRandom();
    }

    template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
    void randomConfiguration_impl
    (const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
     const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
     const Eigen::MatrixBase<ConfigOut_t> & qout) const
    {
      ConfigOut_t & res = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout).derived();
      for (int i = 0; i < nq (); ++i)
      {
        if(lower_pos_limit[i] == -std::numeric_limits<typename ConfigL_t::Scalar>::infinity() ||
           upper_pos_limit[i] ==  std::numeric_limits<typename ConfigR_t::Scalar>::infinity() )
        {
          std::ostringstream error;
          error << "non bounded limit. Cannot uniformly sample joint at rank " << i;
          // assert(false && "non bounded limit. Cannot uniformly sample joint revolute");
          throw std::range_error(error.str());
        }
        res[i] = lower_pos_limit[i] + (( upper_pos_limit[i] - lower_pos_limit[i]) * rand())/RAND_MAX;
      }
    }

    bool isEqual_impl (const VectorSpaceOperationTpl& other) const
    {
      return size_.value() == other.size_.value();
    }

  private:

    Eigen::internal::variable_if_dynamic<Index, Dim> size_;
  }; // struct VectorSpaceOperationTpl

} // namespace pinocchio

#endif // ifndef __pinocchio_multibody_liegroup_vector_space_operation_hpp__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2016-2020 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_multibody_liegroup_vector_space_operation_hpp__
6			#define __pinocchio_multibody_liegroup_vector_space_operation_hpp__
7
8			#include "pinocchio/multibody/liegroup/liegroup-base.hpp"
9
10			#include <stdexcept>
11			#include <boost/integer/static_min_max.hpp>
12
13			namespace pinocchio
14			{
15			template<int Dim, typename Scalar, int Options = 0> struct VectorSpaceOperationTpl;
16
17			template<int Dim, typename _Scalar, int _Options>
18			struct traits< VectorSpaceOperationTpl<Dim,_Scalar,_Options> >
19			{
20			typedef _Scalar Scalar;
21			enum {
22			Options = _Options,
23			NQ = Dim,
24			NV = Dim
25			};
26			};
27
28			template<int Dim, typename _Scalar, int _Options>
29			struct VectorSpaceOperationTpl
30			: public LieGroupBase< VectorSpaceOperationTpl<Dim,_Scalar,_Options> >
31			{
32			PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(VectorSpaceOperationTpl);
33
34			/// Constructor
35			/// \param size size of the vector space: should be the equal to template
36			/// argument for static sized vector-spaces.
37		473424	VectorSpaceOperationTpl(int size = boost::static_signed_max<0,Dim>::value)
38		473424	: size_(size)
39			{
40	✗✓	473424	assert(size_.value() >= 0);
41		473424	}
42
43			/// Constructor
44			/// \param other other VectorSpaceOperationTpl from which to retrieve size
45		862	VectorSpaceOperationTpl(const VectorSpaceOperationTpl & other)
46		862	: Base(), size_(other.size_.value())
47			{
48	✗✓	862	assert(size_.value() >= 0);
49		862	}
50
51			VectorSpaceOperationTpl& operator=(const VectorSpaceOperationTpl& other)
52			{
53			size_.setValue(other.size_.value());
54			assert(size_.value() >= 0);
55			return *this;
56			}
57
58		372770	Index nq () const
59			{
60		372770	return size_.value();
61			}
62		44672	Index nv () const
63			{
64		89344	return size_.value();
65			}
66
67		1001	ConfigVector_t neutral () const
68			{
69	✓✗	1001	return ConfigVector_t::Zero(size_.value());
70			}
71
72		526	std::string name () const
73			{
74	✓✗✓✗ ✓✓✗✓ ✗	1052	std::ostringstream oss; oss << "R^" << nq();
75	✓✗	1052	return oss.str ();
76			}
77
78			template <class ConfigL_t, class ConfigR_t, class Tangent_t>
79		83588	static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
80			const Eigen::MatrixBase<ConfigR_t> & q1,
81			const Eigen::MatrixBase<Tangent_t> & d)
82			{
83	✓✗	83588	PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d) = q1 - q0;
84		83588	}
85
86			template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
87		672	void dDifference_impl (const Eigen::MatrixBase<ConfigL_t> &,
88			const Eigen::MatrixBase<ConfigR_t> &,
89			const Eigen::MatrixBase<JacobianOut_t> & J) const
90			{
91			if (arg == ARG0)
92	✓✗✓✗	256	PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J) = -JacobianMatrix_t::Identity(size_.value(),size_.value());
93			else if (arg == ARG1)
94		416	PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J).setIdentity();
95		672	}
96
97			template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
98			static void dDifference_product_impl(const ConfigL_t &,
99			const ConfigR_t &,
100			const JacobianIn_t & Jin,
101			JacobianOut_t & Jout,
102			bool,
103			const AssignmentOperatorType op)
104			{
105			switch (op) {
106			case SETTO:
107			if (arg == ARG0) Jout = - Jin;
108			else Jout = Jin;
109			return;
110			case ADDTO:
111			if (arg == ARG0) Jout -= Jin;
112			else Jout += Jin;
113			return;
114			case RMTO:
115			if (arg == ARG0) Jout += Jin;
116			else Jout -= Jin;
117			return;
118			}
119			}
120
121			template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
122		177990	static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
123			const Eigen::MatrixBase<Velocity_t> & v,
124			const Eigen::MatrixBase<ConfigOut_t> & qout)
125			{
126	✓✗	177990	PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout) = q + v;
127		177990	}
128
129			template <class Config_t, class Jacobian_t>
130		212	static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> &,
131			const Eigen::MatrixBase<Jacobian_t> & J)
132			{
133		212	PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).setIdentity();
134		212	}
135
136			template <class Config_t, class Tangent_t, class JacobianOut_t>
137		320	static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /q/,
138			const Eigen::MatrixBase<Tangent_t> & /v/,
139			const Eigen::MatrixBase<JacobianOut_t> & J,
140			const AssignmentOperatorType op=SETTO)
141			{
142		320	Eigen::MatrixBase<JacobianOut_t>& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
143	✓✓✓✗	320	switch(op)
144			{
145		296	case SETTO:
146		296	Jout.setIdentity();
147		296	break;
148		12	case ADDTO:
149	✓✗✓✗ ✓✗	12	Jout.diagonal().array() += Scalar(1);
150		12	break;
151		12	case RMTO:
152	✓✗✓✗ ✓✗	12	Jout.diagonal().array() -= Scalar(1);
153		12	break;
154			default:
155			assert(false && "Wrong Op requesed value");
156			break;
157			}
158		320	}
159
160			template <class Config_t, class Tangent_t, class JacobianOut_t>
161		480	static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /q/,
162			const Eigen::MatrixBase<Tangent_t> & /v/,
163			const Eigen::MatrixBase<JacobianOut_t> & J,
164			const AssignmentOperatorType op=SETTO)
165			{
166		480	Eigen::MatrixBase<JacobianOut_t>& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
167	✓✓✓✗	480	switch(op)
168			{
169		456	case SETTO:
170		456	Jout.setIdentity();
171		456	break;
172		12	case ADDTO:
173	✓✗✓✗ ✓✗	12	Jout.diagonal().array() += Scalar(1);
174		12	break;
175		12	case RMTO:
176	✓✗✓✗ ✓✗	12	Jout.diagonal().array() -= Scalar(1);
177		12	break;
178			default:
179			assert(false && "Wrong Op requesed value");
180			break;
181			}
182		480	}
183
184			template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
185			static void dIntegrate_product_impl(const Config_t &,
186			const Tangent_t &,
187			const JacobianIn_t & Jin,
188			JacobianOut_t & Jout,
189			bool,
190			const ArgumentPosition,
191			const AssignmentOperatorType op)
192			{
193			switch(op) {
194			case SETTO:
195			Jout = Jin;
196			break;
197			case ADDTO:
198			Jout += Jin;
199			break;
200			case RMTO:
201			Jout -= Jin;
202			break;
203			default:
204			assert(false && "Wrong Op requesed value");
205			break;
206			}
207			}
208
209			template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
210		12	static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /q/,
211			const Eigen::MatrixBase<Tangent_t> & /v/,
212			const Eigen::MatrixBase<JacobianIn_t> & Jin,
213			const Eigen::MatrixBase<JacobianOut_t> & Jout)
214			{
215		12	PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout) = Jin;
216		12	}
217
218			template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
219		12	static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /q/,
220			const Eigen::MatrixBase<Tangent_t> & /v/,
221			const Eigen::MatrixBase<JacobianIn_t> & Jin,
222			const Eigen::MatrixBase<JacobianOut_t> & Jout)
223			{
224		12	PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout) = Jin;
225		12	}
226
227			template <class Config_t, class Tangent_t, class Jacobian_t>
228		12	static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /q/,
229			const Eigen::MatrixBase<Tangent_t> & /v/,
230		12	const Eigen::MatrixBase<Jacobian_t> & /J/) {}
231
232			template <class Config_t, class Tangent_t, class Jacobian_t>
233		12	static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /q/,
234			const Eigen::MatrixBase<Tangent_t> & /v/,
235		12	const Eigen::MatrixBase<Jacobian_t> & /J/) {}
236
237
238
239			// template <class ConfigL_t, class ConfigR_t>
240			// static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
241			// const Eigen::MatrixBase<ConfigR_t> & q1)
242
243			template <class Config_t>
244		16680	static void normalize_impl (const Eigen::MatrixBase<Config_t>& /qout/)
245		16680	{}
246
247			template <class Config_t>
248		65692	static bool isNormalized_impl (const Eigen::MatrixBase<Config_t>& /qout/, const Scalar& /prec/)
249			{
250		65692	return true;
251			}
252
253			template <class Config_t>
254		1548	static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
255			{
256		1548	PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).setRandom();
257		1548	}
258
259			template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
260		65766	void randomConfiguration_impl
261			(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
262			const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
263			const Eigen::MatrixBase<ConfigOut_t> & qout) const
264			{
265		65766	ConfigOut_t & res = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout).derived();
266	✓✓	174908	for (int i = 0; i < nq (); ++i)
267			{
268	✓✓✗✓ ✓✓	218286	if(lower_pos_limit[i] == -std::numeric_limits<typename ConfigL_t::Scalar>::infinity() \|\|
269		109142	upper_pos_limit[i] == std::numeric_limits<typename ConfigR_t::Scalar>::infinity() )
270			{
271	✓✗	4	std::ostringstream error;
272	✓✗✓✗	2	error << "non bounded limit. Cannot uniformly sample joint at rank " << i;
273			// assert(false && "non bounded limit. Cannot uniformly sample joint revolute");
274	✓✗✓✗	2	throw std::range_error(error.str());
275			}
276		109142	res[i] = lower_pos_limit[i] + (( upper_pos_limit[i] - lower_pos_limit[i]) * rand())/RAND_MAX;
277			}
278		65764	}
279
280		5	bool isEqual_impl (const VectorSpaceOperationTpl& other) const
281			{
282		5	return size_.value() == other.size_.value();
283			}
284
285			private:
286
287			Eigen::internal::variable_if_dynamic<Index, Dim> size_;
288			}; // struct VectorSpaceOperationTpl
289
290			} // namespace pinocchio
291
292			#endif // ifndef __pinocchio_multibody_liegroup_vector_space_operation_hpp__