joint-generic.hpp 4.34 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
//
// Copyright (c) 2015 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/>.

18
19
20
21
22
#ifndef __se3_joint_generic_hpp__
#define __se3_joint_generic_hpp__

#include "pinocchio/exception.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
23
#include "pinocchio/multibody/joint/joint-dense.hpp"
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
#include "pinocchio/multibody/joint/joint-variant.hpp"

namespace se3
{

  struct JointGeneric;
  struct JointModelGeneric;
  struct JointDataGeneric;

  template<>
  struct traits<JointGeneric>
  {
    typedef JointDataGeneric JointData;
    typedef JointModelGeneric JointModel;
    typedef ConstraintXd Constraint_t;
    typedef SE3 Transformation_t;
    typedef Motion Motion_t;
    typedef Motion Bias_t;
    enum {
43
44
      NQ = -1,
      NV = -1
45
46
    };
  };
47
48
  template<> struct traits<JointDataGeneric> { typedef JointGeneric JointDerived; };
  template<> struct traits<JointModelGeneric> { typedef JointGeneric JointDerived; };
49
50
51

  struct JointDataGeneric : public JointDataBase<JointDataGeneric> 
  {
52
    typedef JointGeneric JointDerived;
53
54
55
56
57
58
59
60
61
62
63
64
65
    SE3_JOINT_TYPEDEF;

    Constraint_t S;
    Transformation_t M;
    Motion_t v;
    Bias_t c;

    JointDataGeneric()  {}
    JointDataGeneric( const JointDataVariant & jvar );
  };

  struct JointModelGeneric : public JointModelBase<JointModelGeneric> 
  {
66
    typedef JointGeneric JointDerived;
67
68
69
70
71
72
73
74
75
76
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
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
    SE3_JOINT_TYPEDEF;
    SE3_JOINT_USE_INDEXES;

    JointData createData() const { return JointData(); }

    virtual void jcalc(JointData& /*data*/, 
		       const Eigen::VectorXd & /*qs*/) const 
    {
      throw se3::Exception("Virtual function calc not implemented in the generic class. Derive it.");
    }
    virtual void jcalc(JointData& /*data*/, 
		       const Eigen::VectorXd & /*qs*/, 
		       const Eigen::VectorXd & /*vs*/ ) const 
    {
      throw se3::Exception("Virtual function calc not implemented in the generic class. Derive it.");
    }

    JointModelGeneric() {}
    JointModelGeneric( const JointModelVariant & jvar );
    JointModelGeneric & operator= (const JointModelGeneric& clone)
    { 
      setIndexes(clone.idx_q(),clone.idx_v()); return *this;
    }
  };

  /* --- Convert Data ------------------------------------------------------ */
  namespace internal
  {
    struct JointDataVariantToGeneric : public boost::static_visitor< JointDataGeneric >
    {
      template <typename D>
      JointDataGeneric operator() ( const JointDataBase<D> & jdata ) const
      {
	JointDataGeneric jgen;
	jgen.S = jdata.S();
	jgen.M = jdata.M();
	jgen.v = jdata.v();
	jgen.c = jdata.c();
	return jgen;
      }

      static JointDataGeneric run ( const JointDataVariant & jdata )
      {
	return boost::apply_visitor( JointDataVariantToGeneric(),jdata );
      }
    };
  }// namespace internal

  JointDataGeneric::JointDataGeneric( const JointDataVariant & jvar )
  {
    const JointDataGeneric & clone = internal::JointDataVariantToGeneric::run(jvar);
    (*this) = clone;
  }
  
  /* --- Convert Model ----------------------------------------------------- */
  namespace internal
  {
    struct JointModelVariantToGeneric : public boost::static_visitor< JointModelGeneric >
    {
      template <typename D>
      JointModelGeneric operator() ( const JointModelBase<D> & jmodel ) const
      {
	JointModelGeneric jgen;
	jgen.setIndexes(jmodel.idx_q(),jmodel.idx_v());
	return jgen;
      }

      static JointModelGeneric run ( const JointModelVariant & jmodel )
      {
	return boost::apply_visitor( JointModelVariantToGeneric(),jmodel );
      }
    };
  }// namespace internal

  JointModelGeneric::JointModelGeneric( const JointModelVariant & jvar )
  {
    const JointModelGeneric & clone = internal::JointModelVariantToGeneric::run(jvar);
    (*this) = clone;
  }

} // namespace se3


#endif // ifndef __se3_joint_generic_hpp__