kinematics.hpp 3.37 KB
Newer Older
1
2
3
4
5
6
7
8
#ifndef __se3_kinematics_hpp__
#define __se3_kinematics_hpp__

#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
  
namespace se3
{
9
10
11
12
  inline void geometry(const Model & model,
                       Data & data,
                       const Eigen::VectorXd & q);

13
  inline void kinematics(const Model & model,
14
			 Data & data,
15
16
			 const Eigen::VectorXd & q,
			 const Eigen::VectorXd & v);
17

18
19
20
21
22
} // namespace se3 

/* --- Details -------------------------------------------------------------------- */
namespace se3 
{
23
24
25
26
  struct GeometryStep : public fusion::JointVisitor<GeometryStep>
  {
    typedef boost::fusion::vector<const se3::Model &,
                                  se3::Data &,
27
                                  const size_t,
28
29
30
31
32
33
34
35
36
37
                                  const Eigen::VectorXd &
                                  > ArgsType;

    JOINT_VISITOR_INIT (GeometryStep);

    template<typename JointModel>
    static void algo(const se3::JointModelBase<JointModel> & jmodel,
                     se3::JointDataBase<typename JointModel::JointData> & jdata,
                     const se3::Model & model,
                     se3::Data & data,
38
                     const size_t i,
39
40
41
42
43
44
45
46
47
48
                     const Eigen::VectorXd & q)
    {
      using namespace se3;

      jmodel.calc (jdata.derived (), q);

      const Model::Index & parent = model.parents[i];
      data.liMi[i] = model.jointPlacements[i] * jdata.M ();

      if (parent>0)
49
        data.oMi[i] = data.oMi[(size_t) parent] * data.liMi[i];
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
      else
        data.oMi[i] = data.liMi[i];
    }
    
  };

  inline void
  geometry(const Model & model,
           Data & data,
           const Eigen::VectorXd & q)
  {
    for (size_t i=1; i < (size_t) model.nbody; ++i)
    {
      GeometryStep::run(model.joints[i],
                        data.joints[i],
                        GeometryStep::ArgsType (model,data,i,q)
                        );
    }
  }

70
71
72
73
  struct KinematicsStep : public fusion::JointVisitor<KinematicsStep>
  {
    typedef boost::fusion::vector< const se3::Model&,
				   se3::Data&,
74
				   const size_t,
75
76
77
78
79
80
81
82
83
84
85
				   const Eigen::VectorXd &,
				   const Eigen::VectorXd &
				   > ArgsType;

    JOINT_VISITOR_INIT(KinematicsStep);

    template<typename JointModel>
    static void algo(const se3::JointModelBase<JointModel> & jmodel,
		    se3::JointDataBase<typename JointModel::JointData> & jdata,
		    const se3::Model& model,
		    se3::Data& data,
86
		    const size_t i,
87
88
89
90
91
92
93
94
95
		    const Eigen::VectorXd & q,
		    const Eigen::VectorXd & v)
    {
      using namespace Eigen;
      using namespace se3;
      
      jmodel.calc(jdata.derived(),q,v);
      
      const Model::Index & parent = model.parents[i];
96
      data.v[i] = jdata.v();
97
98
      data.liMi[i] = model.jointPlacements[i]*jdata.M();
      
99
100
      if(parent>0)
      {
101
102
        data.oMi[i] = data.oMi[(size_t) parent]*data.liMi[i];
        data.v[i] += data.liMi[i].actInv(data.v[(size_t) parent]);
103
104
105
      }
      else
        data.oMi[i] = data.liMi[i];
106
107
108
109
110
111
112
113
114
115
116
    }

  };

  inline void
  kinematics(const Model & model, Data& data,
	     const Eigen::VectorXd & q,
	     const Eigen::VectorXd & v)
  {
    data.v[0] = Motion::Zero();

117
    for( size_t i=1; i<(size_t) model.nbody; ++i )
118
119
120
121
122
123
124
125
126
      {
	KinematicsStep::run(model.joints[i],data.joints[i],
			    KinematicsStep::ArgsType(model,data,i,q,v));
      }
  }
} // namespace se3

#endif // ifndef __se3_kinematics_hpp__