force-ref.hpp 3.86 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
//
// Copyright (c) 2017-2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

#ifndef __se3_force_ref_hpp__
#define __se3_force_ref_hpp__

namespace se3
{
  
  template<typename Vector6ArgType>
  struct traits< ForceRef<Vector6ArgType> >
  {
    typedef typename Vector6ArgType::Scalar Scalar;
    typedef typename EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
    enum {
      LINEAR = 0,
      ANGULAR = 3,
      Options = Vector6::Options
    };
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
    typedef Matrix6 ActionMatrixType;
    typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type LinearType;
    typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type AngularType;
    typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
    typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
    typedef ForceTpl<Scalar,Options> ForcePlain;
    typedef typename EIGEN_REF_TYPE(Vector6ArgType) DataRefType;
    typedef DataRefType ToVectorReturnType;
    typedef typename EIGEN_REF_CONSTTYPE(Vector6ArgType) ConstDataRefType;
    typedef ConstDataRefType ToVectorConstReturnType;
    
  }; // traits ForceRef
  
  namespace internal
  {
    template<typename Vector6ArgType>
    struct SE3GroupAction< ForceRef<Vector6ArgType> >
    {
      typedef typename traits< ForceRef<Vector6ArgType> >::ForcePlain ReturnType;
    };
    
    template<typename Vector6ArgType>
    struct MotionAlgebraAction< ForceRef<Vector6ArgType> >
    {
      typedef typename traits< ForceRef<Vector6ArgType> >::ForcePlain ReturnType;
    };
  }
  
  template<typename Vector6ArgType>
  class ForceRef : public ForceDense< ForceRef<Vector6ArgType> >
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef ForceDense<ForceRef> Base;
    typedef typename traits<ForceRef>::DataRefType DataRefType;
    FORCE_TYPEDEF_TPL(ForceRef);
    
    using Base::operator=;
    
75
76
    ForceRef(const Eigen::MatrixBase<Vector6ArgType> & f_like)
    : ref(const_cast<Vector6ArgType &>(f_like.derived()))
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
    {
      EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
                          YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
      assert(f_like.size() == 6);
    }
    
    ToVectorConstReturnType toVector_impl() const { return ref; }
    ToVectorReturnType toVector_impl() { return ref; }
    
    // Getters
    ConstAngularType angular_impl() const { return ConstAngularType(ref.derived(),ANGULAR); }
    ConstLinearType linear_impl()  const { return ConstLinearType(ref.derived(),LINEAR); }
    AngularType angular_impl() { return ref.template segment<3> (ANGULAR); }
    LinearType linear_impl()  { return ref.template segment<3> (LINEAR); }
    
    template<typename V3>
    void angular_impl(const Eigen::MatrixBase<V3> & w)
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
      angular_impl()=w;
    }
    
    template<typename V3>
    void linear_impl(const Eigen::MatrixBase<V3> & v)
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
      linear_impl()=v;
    }
    
  protected:
    DataRefType ref;
    
  }; // class MotionTpl
  
} // namespace se3

#endif // ifndef __se3_force_ref_hpp__