crba.hpp 3.52 KB
Newer Older
1
//
jcarpent's avatar
jcarpent committed
2
// Copyright (c) 2015-2018 CNRS
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
//
// 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
#define __se3_crba_hpp__

#include "pinocchio/multibody/model.hpp"
jcarpent's avatar
jcarpent committed
22
#include "pinocchio/multibody/data.hpp"
23
#include "pinocchio/algorithm/check.hpp"
24
25
26
  
namespace se3
{
27
28
29
  ///
  /// \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).
Valenza Florian's avatar
Valenza Florian committed
30
  ///        The result is accessible through data.M.
31
32
33
34
35
36
37
38
39
40
41
42
43
44
  ///
  /// \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,
45
       const Eigen::VectorXd & q);
46
  
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
  ///
  /// \brief Computes the Centroidal Momentum Matrix, the Composite Ridig Body Inertia as well as the centroidal momenta
  ///        according to the current joint configuration and velocity.
  ///
  ///
  /// \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).
  /// \param[in] v The joint velocity vector (dim model.nv).
  ///
  /// \return The Centroidal Momentum Matrix Ag. 
  ///
  inline const Data::Matrix6x &
  ccrba(const Model & model,
        Data & data,
        const Eigen::VectorXd & q,
        const Eigen::VectorXd & v);
  
65
  ///
66
  /// \brief Computes the time derivative of the Centroidal Momentum Matrix according to the current configuration and velocity vectors.
67
  ///
68
  /// \note The computed terms allow to decomposed the spatial momentum variation as following: \f$ \dot{h} = A_g \ddot{q} + \dot{A_g}(q,\dot{q})\dot{q}\f$.
69
70
71
72
73
74
  ///
  /// \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).
  /// \param[in] v The joint configuration vector (dim model.nv).
  ///
75
  /// \return The Centroidal Momentum Matrix time derivative dAg
76
77
  ///
  inline const Data::Matrix6x &
78
  dccrba(const Model & model,
79
80
81
        Data & data,
        const Eigen::VectorXd & q,
        const Eigen::VectorXd & v);
82

83
84
  DEFINE_ALGO_CHECKER(CRBA);

85
86
87
} // namespace se3 

/* --- Details -------------------------------------------------------------------- */
88
#include "pinocchio/algorithm/crba.hxx"
89
90

#endif // ifndef __se3_crba_hpp__