Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/multibody/force-set.hpp	Lines:	38	38	100.0 %
Date:	2024-04-26 13:14:21	Branches:	48	96	50.0 %


//
// Copyright (c) 2015 CNRS
//

#ifndef __pinocchio_force_set_hpp__
#define __pinocchio_force_set_hpp__

#include "pinocchio/spatial/fwd.hpp"
#include <Eigen/Geometry>

namespace pinocchio
{
  template<typename _Scalar, int _Options>
  class ForceSetTpl
  {
  public:
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
    typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
    typedef SE3Tpl<Scalar,Options> SE3;

    typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic,Options> Matrix3x;
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> Matrix6x;

  public:
    // Constructors
    ForceSetTpl(const int & ncols ) : size(ncols),m_f(3,ncols), m_n(3,ncols)
    { m_f.fill(NAN); m_n.fill(NAN); }
    ForceSetTpl(const Matrix3x & linear, const Matrix3x & angular)
      : size((int)linear.cols()),m_f(linear), m_n(angular)
    {  assert( linear.cols() == angular.cols() ); }

    Matrix6x matrix() const
    {
      Matrix6x F(6,size); F << m_f, m_n;
      // F.template topRows<3>() = m_f;
      // F.template bottomRows<3>()  = m_n;
      return F;
    }
    operator Matrix6x () const { return matrix(); }

    // Getters
    const Matrix3x & linear() const { return m_f; }
    const Matrix3x & angular() const { return m_n; }

    /// af = aXb.act(bf)
    ForceSetTpl se3Action(const SE3 & m) const
    {
      Matrix3x Rf (m.rotation()*linear());
      return ForceSetTpl(Rf,skew(m.translation())*Rf+m.rotation()*angular());
      // TODO check if nothing better than explicitely calling skew
    }
    /// bf = aXb.actInv(af)
    ForceSetTpl se3ActionInverse(const SE3 & m) const
    {
      return ForceSetTpl(m.rotation().transpose()*linear(),
		      m.rotation().transpose()*(angular() - skew(m.translation())*linear()) );
      // TODO check if nothing better than explicitely calling skew
    }

    friend std::ostream & operator << (std::ostream & os, const ForceSetTpl & phi)
    {
      os
	<< "F =\n" << phi.linear() << std::endl
	<< "Tau =\n" << phi.angular() << std::endl;
      return os;
    }

    /* --- BLOCK ------------------------------------------------------------ */
    struct Block
    {
      ForceSetTpl & ref;
      int idx,len;
      Block( ForceSetTpl & ref, const int & idx, const int & len )
	: ref(ref), idx(idx), len(len) {}

      Eigen::Block<ForceSetTpl::Matrix3x> linear() { return ref.m_f.block(0,idx,3,len); }
      Eigen::Block<ForceSetTpl::Matrix3x> angular() { return ref.m_n.block(0,idx,3,len); }
      Eigen::Block<const ForceSetTpl::Matrix3x> linear()  const
      { return ((const ForceSetTpl::Matrix3x &)(ref.m_f)).block(0,idx,3,len); }
      Eigen::Block<const ForceSetTpl::Matrix3x> angular() const
      { return ((const ForceSetTpl::Matrix3x &)(ref.m_n)).block(0,idx,3,len); }

      ForceSetTpl::Matrix6x matrix() const
      {
	ForceSetTpl::Matrix6x res(6,len); res << linear(),angular();
	return res;
      }

      Block& operator= (const ForceSetTpl & copy)
      {
	assert(copy.size == len);
	linear() = copy.linear(); //ref.m_f.block(0,idx,3,len) = copy.m_f;
	angular() = copy.angular(); //ref.m_n.block(0,idx,3,len) = copy.m_n;
	return *this;
      }

      Block& operator= (const ForceSetTpl::Block & copy)
      {
	assert(copy.len == len);
	linear() = copy.linear(); //ref.m_f.block(0,idx,3,len) = copy.ref.m_f.block(0,copy.idx,3,copy.len);
	angular() = copy.angular(); //ref.m_n.block(0,idx,3,len) = copy.ref.m_n.block(0,copy.idx,3,copy.len);
	return *this;
      }

      template <typename D>
      Block& operator= (const Eigen::MatrixBase<D> & m)
      {
        eigen_assert(D::RowsAtCompileTime == 6);
        assert(m.cols() == len);
        linear() = m.template topRows<3>();
        angular() = m.template bottomRows<3>();
        return *this;
      }

      /// af = aXb.act(bf)
      ForceSetTpl se3Action(const SE3 & m) const
      {
	// const Eigen::Block<const Matrix3x> linear = ref.linear().block(0,idx,3,len);
	// const Eigen::Block<const Matrix3x> angular = ref.angular().block(0,idx,3,len);
	Matrix3x Rf ((m.rotation()*linear()));
	return ForceSetTpl(Rf,skew(m.translation())*Rf+m.rotation()*angular());
	// TODO check if nothing better than explicitely calling skew
      }
      /// bf = aXb.actInv(af)
      ForceSetTpl se3ActionInverse(const SE3 & m) const
      {
	// const Eigen::Block<const Matrix3x> linear = ref.linear().block(0,idx,3,len);
	// const Eigen::Block<const Matrix3x> angular = ref.angular().block(0,idx,3,len);
	return ForceSetTpl(m.rotation().transpose()*linear(),
			   m.rotation().transpose()*(angular() - skew(m.translation())*linear()) );
	// TODO check if nothing better than explicitely calling skew
      }

    };

    Block block(const int & idx, const int & len) { return Block(*this,idx,len); }

    /* CRBA joint operators
     *   - ForceSet::Block = ForceSet
     *   - ForceSet operator* (Inertia Y,Constraint S)
     *   - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
     *   - SE3::act(ForceSet::Block)
     */


  public: //
  private:
    int size;
    Matrix3x m_f,m_n;
  };

  typedef ForceSetTpl<double,0> ForceSet;

  template<>
  struct SE3GroupAction<ForceSet::Block> { typedef ForceSet ReturnType; };


} // namespace pinocchio

#endif // ifndef __pinocchio_force_set_hpp__



Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015 CNRS
3			//
4
5			#ifndef __pinocchio_force_set_hpp__
6			#define __pinocchio_force_set_hpp__
7
8			#include "pinocchio/spatial/fwd.hpp"
9			#include <Eigen/Geometry>
10
11			namespace pinocchio
12			{
13			template<typename _Scalar, int _Options>
14			class ForceSetTpl
15			{
16			public:
17			typedef _Scalar Scalar;
18			enum { Options = _Options };
19			typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
20			typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
21			typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
22			typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
23			typedef SE3Tpl<Scalar,Options> SE3;
24
25			typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic,Options> Matrix3x;
26			typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> Matrix6x;
27
28			public:
29			// Constructors
30	✓✗✓✗	3	ForceSetTpl(const int & ncols ) : size(ncols),m_f(3,ncols), m_n(3,ncols)
31	✓✗✓✗	3	{ m_f.fill(NAN); m_n.fill(NAN); }
32		9	ForceSetTpl(const Matrix3x & linear, const Matrix3x & angular)
33	✓✗	9	: size((int)linear.cols()),m_f(linear), m_n(angular)
34	✓✗✓✗ ✗✓	9	{ assert( linear.cols() == angular.cols() ); }
35
36		12	Matrix6x matrix() const
37			{
38	✓✗✓✗ ✓✗	12	Matrix6x F(6,size); F << m_f, m_n;
39			// F.template topRows<3>() = m_f;
40			// F.template bottomRows<3>() = m_n;
41		12	return F;
42			}
43			operator Matrix6x () const { return matrix(); }
44
45			// Getters
46		6	const Matrix3x & linear() const { return m_f; }
47		6	const Matrix3x & angular() const { return m_n; }
48
49			/// af = aXb.act(bf)
50		4	ForceSetTpl se3Action(const SE3 & m) const
51			{
52	✓✗✓✗ ✓✗	4	Matrix3x Rf (m.rotation()*linear());
53	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	8	return ForceSetTpl(Rf,skew(m.translation())Rf+m.rotation()angular());
54			// TODO check if nothing better than explicitely calling skew
55			}
56			/// bf = aXb.actInv(af)
57			ForceSetTpl se3ActionInverse(const SE3 & m) const
58			{
59			return ForceSetTpl(m.rotation().transpose()*linear(),
60			m.rotation().transpose()(angular() - skew(m.translation())linear()) );
61			// TODO check if nothing better than explicitely calling skew
62			}
63
64			friend std::ostream & operator << (std::ostream & os, const ForceSetTpl & phi)
65			{
66			os
67			<< "F =\n" << phi.linear() << std::endl
68			<< "Tau =\n" << phi.angular() << std::endl;
69			return os;
70			}
71
72			/* --- BLOCK ------------------------------------------------------------ */
73			struct Block
74			{
75			ForceSetTpl & ref;
76			int idx,len;
77		6	Block( ForceSetTpl & ref, const int & idx, const int & len )
78		6	: ref(ref), idx(idx), len(len) {}
79
80		3	Eigen::Block<ForceSetTpl::Matrix3x> linear() { return ref.m_f.block(0,idx,3,len); }
81		3	Eigen::Block<ForceSetTpl::Matrix3x> angular() { return ref.m_n.block(0,idx,3,len); }
82		3	Eigen::Block<const ForceSetTpl::Matrix3x> linear() const
83		3	{ return ((const ForceSetTpl::Matrix3x &)(ref.m_f)).block(0,idx,3,len); }
84		3	Eigen::Block<const ForceSetTpl::Matrix3x> angular() const
85		3	{ return ((const ForceSetTpl::Matrix3x &)(ref.m_n)).block(0,idx,3,len); }
86
87		1	ForceSetTpl::Matrix6x matrix() const
88			{
89	✓✗✓✗ ✓✗✓✗ ✓✗	1	ForceSetTpl::Matrix6x res(6,len); res << linear(),angular();
90		1	return res;
91			}
92
93		2	Block& operator= (const ForceSetTpl & copy)
94			{
95	✗✓	2	assert(copy.size == len);
96	✓✗	2	linear() = copy.linear(); //ref.m_f.block(0,idx,3,len) = copy.m_f;
97	✓✗	2	angular() = copy.angular(); //ref.m_n.block(0,idx,3,len) = copy.m_n;
98		2	return *this;
99			}
100
101			Block& operator= (const ForceSetTpl::Block & copy)
102			{
103			assert(copy.len == len);
104			linear() = copy.linear(); //ref.m_f.block(0,idx,3,len) = copy.ref.m_f.block(0,copy.idx,3,copy.len);
105			angular() = copy.angular(); //ref.m_n.block(0,idx,3,len) = copy.ref.m_n.block(0,copy.idx,3,copy.len);
106			return *this;
107			}
108
109			template <typename D>
110		1	Block& operator= (const Eigen::MatrixBase<D> & m)
111			{
112			eigen_assert(D::RowsAtCompileTime == 6);
113	✗✓	1	assert(m.cols() == len);
114	✓✗✓✗	1	linear() = m.template topRows<3>();
115	✓✗✓✗	1	angular() = m.template bottomRows<3>();
116		1	return *this;
117			}
118
119			/// af = aXb.act(bf)
120		2	ForceSetTpl se3Action(const SE3 & m) const
121			{
122			// const Eigen::Block<const Matrix3x> linear = ref.linear().block(0,idx,3,len);
123			// const Eigen::Block<const Matrix3x> angular = ref.angular().block(0,idx,3,len);
124	✓✗✓✗ ✓✗✓✗	2	Matrix3x Rf ((m.rotation()*linear()));
125	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	return ForceSetTpl(Rf,skew(m.translation())Rf+m.rotation()angular());
126			// TODO check if nothing better than explicitely calling skew
127			}
128			/// bf = aXb.actInv(af)
129			ForceSetTpl se3ActionInverse(const SE3 & m) const
130			{
131			// const Eigen::Block<const Matrix3x> linear = ref.linear().block(0,idx,3,len);
132			// const Eigen::Block<const Matrix3x> angular = ref.angular().block(0,idx,3,len);
133			return ForceSetTpl(m.rotation().transpose()*linear(),
134			m.rotation().transpose()(angular() - skew(m.translation())linear()) );
135			// TODO check if nothing better than explicitely calling skew
136			}
137
138			};
139
140		6	Block block(const int & idx, const int & len) { return Block(*this,idx,len); }
141
142			/* CRBA joint operators
143			* - ForceSet::Block = ForceSet
144			* - ForceSet operator* (Inertia Y,Constraint S)
145			* - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
146			* - SE3::act(ForceSet::Block)
147			*/
148
149
150			public: //
151			private:
152			int size;
153			Matrix3x m_f,m_n;
154			};
155
156			typedef ForceSetTpl<double,0> ForceSet;
157
158			template<>
159			struct SE3GroupAction<ForceSet::Block> { typedef ForceSet ReturnType; };
160
161
162			} // namespace pinocchio
163
164			#endif // ifndef __pinocchio_force_set_hpp__
165