crba.hpp 5.12 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
#ifndef __se3_crba_hpp__
19
20
21
22
#define __se3_crba_hpp__

#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
23
24
#include "pinocchio/spatial/act-on-set.hpp"

25
#include <iostream>
26
27
28
  
namespace se3
{
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
  ///
  /// \brief Computes the upper triangular part of the joint space inertia matrix M by
  ///        using the Composite Rigid Body Algorithm (Chapter 6, Rigid-Body Dynamics Algorithms, R. Featherstone, 2008).
  ///        The result is accessible throw data.M.
  ///
  /// \note You can easly get data.M symetric by copying the stricly upper trinangular part
  ///       in the stricly lower tringular part with
  ///       data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] q The joint configuration vector (dim model.nq).
  ///
  /// \return The joint space inertia matrix with only the upper triangular part computed.
  ///
  inline const Eigen::MatrixXd &
  crba(const Model & model,
       Data & data,
47
       const Eigen::VectorXd & q);
48
49
50
51
52
53
54
55
56

} // namespace se3 

/* --- Details -------------------------------------------------------------------- */
namespace se3 
{
  struct CrbaForwardStep : public fusion::JointVisitor<CrbaForwardStep>
  {
    typedef boost::fusion::vector< const se3::Model&,
57
58
59
				   se3::Data&,
				   const Eigen::VectorXd &
				   > ArgsType;
60
61
62
63
64
65
66
67
68
69
70
71

    JOINT_VISITOR_INIT(CrbaForwardStep);

    template<typename JointModel>
    static void algo(const se3::JointModelBase<JointModel> & jmodel,
		     se3::JointDataBase<typename JointModel::JointData> & jdata,
		     const se3::Model& model,
		     se3::Data& data,
		     const Eigen::VectorXd & q)
    {
      using namespace Eigen;
      using namespace se3;
72

73
      const Model::Index & i = (Model::Index) jmodel.id();
74
75
76
77
78
79
80
81
82
83
84
      jmodel.calc(jdata.derived(),q);
      
      data.liMi[i] = model.jointPlacements[i]*jdata.M();
      data.Ycrb[i] = model.inertias[i];
    }

  };

  struct CrbaBackwardStep : public fusion::JointVisitor<CrbaBackwardStep>
  {
    typedef boost::fusion::vector<const Model&,
85
				  Data&>  ArgsType;
86
87
88
89
90
91
92
    
    JOINT_VISITOR_INIT(CrbaBackwardStep);

    template<typename JointModel>
    static void algo(const JointModelBase<JointModel> & jmodel,
		     JointDataBase<typename JointModel::JointData> & jdata,
		     const Model& model,
93
		     Data& data)
94
95
    {
      /*
96
97
98
99
100
       * F[1:6,i] = Y*S
       * M[i,SUBTREE] = S'*F[1:6,SUBTREE]
       * if li>0 
       *   Yli += liXi Yi
       *   F[1:6,SUBTREE] = liXi F[1:6,SUBTREE]
101
       */
102
      const Model::Index & i = (Model::Index) jmodel.id();
103

Nicolas Mansard's avatar
Nicolas Mansard committed
104
      /* F[1:6,i] = Y*S */
105
106
      //data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S();
      jmodel.jointCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S();
107

Nicolas Mansard's avatar
Nicolas Mansard committed
108
      /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */
109
      data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]) 
110
	= jdata.S().transpose()*data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
111

112
      const Model::Index & parent   = model.parents[i];
113
      if(parent>0) 
jcarpent's avatar
jcarpent committed
114
115
116
117
118
119
120
121
122
123
124
      {
        /*   Yli += liXi Yi */
        data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
        
        /*   F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] */
        Eigen::Block<typename Data::Matrix6x> jF
        = data.Fcrb[parent].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
        Eigen::Block<typename Data::Matrix6x> iF
        = data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
        forceSet::se3Action(data.liMi[i], iF, jF);
      }
125
126
127
128
129
      
      // std::cout << "iYi = " << (Inertia::Matrix6)data.Ycrb[i] << std::endl;
      // std::cout << "iSi = " << ConstraintXd(jdata.S()).matrix() << std::endl;
      // std::cout << "liFi = " << jdata.F() << std::endl;
      // std::cout << "M = " <<  data.M << std::endl;
130
131
132
133
134
135
136
    }
  };

  inline const Eigen::MatrixXd&
  crba(const Model & model, Data& data,
       const Eigen::VectorXd & q)
  {
137
    for( Model::Index i=1;i<(Model::Index)(model.nbody);++i )
138
139
      {
	CrbaForwardStep::run(model.joints[i],data.joints[i],
140
			     CrbaForwardStep::ArgsType(model,data,q));
141
142
      }
    
143
    for( Model::Index i=(Model::Index)(model.nbody-1);i>0;--i )
144
145
      {
	CrbaBackwardStep::run(model.joints[i],data.joints[i],
146
			      CrbaBackwardStep::ArgsType(model,data));
147
148
149
150
151
152
153
154
      }

    return data.M;
  }
} // namespace se3

#endif // ifndef __se3_crba_hpp__