cholesky.hpp 8.77 KB
Newer Older
1
//
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/>.

Nicolas Mansard's avatar
Nicolas Mansard committed
18
19
20
21
#ifndef __se3_cholesky_hpp__
#define __se3_cholesky_hpp__

#include "pinocchio/multibody/model.hpp"
jcarpent's avatar
jcarpent committed
22
#include "pinocchio/multibody/data.hpp"
Nicolas Mansard's avatar
Nicolas Mansard committed
23
24
25
  
namespace se3
{
26
27
  namespace cholesky
  {
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
    
    ///
    /// \brief Compute the Cholesky decomposition of the joint space inertia matrix M contained in data.
    ///
    /// \note The Cholesky decomposition corresponds to
    ///       \f$ M = U D U^{\top}\f$ with \f$U\f$ an upper triangular matrix with ones on its main diagonal and \f$D\f$ a diagonal matrix.
    ///
    ///       The result stored in data.U and data.D matrices. One can retrieve the matrice M by performing the
    ///       computation data.U * data.D * data.U.transpose()
    ///
    ///       See https://en.wikipedia.org/wiki/Cholesky_decomposition for futher details.
    ///
    ///
    /// \param[in] model The model structure of the rigid body system.
    /// \param[in] data The data structure of the rigid body system.
    ///
jcarpent's avatar
jcarpent committed
44
45
    /// \return A reference to the upper triangular matrix \f$U\f$.
    ///
46
47
48
    inline const Eigen::MatrixXd &
    decompose(const Model & model,
              Data & data);
49

50
51
52
53
54
55
56
57
58
59
    ///
    /// \brief Return the solution \f$x\f$ of \f$ M x = y \f$ using the Cholesky decomposition stored in data given the entry \f$ y \f$. Act like solveInPlace of Eigen::LLT.
    ///
    /// \note This algorithm is useful to compute the forward dynamics, retriving the joint acceleration \f$ \ddot{q} \f$ from the current joint torque \f$ \tau \f$
    ///       \f$
    ///           M(q) \ddot{q} + b(q, \dot{q}) = \tau \iff \ddot{q} = M(q)^{-1} (\tau - b(q, \dot{q}))
    ///       \f$
    ///
    /// \param[in] model The model structure of the rigid body system.
    /// \param[in] data The data structure of the rigid body system.
60
    /// \param[inout] y The input matrix to inverse which also contains the result \f$x\f$ of the inversion.
61
    ///
62
    template<typename Mat>
63
    Mat & solve(const Model & model, const Data & data,
64
                const Eigen::MatrixBase<Mat> & y);
65

66
    ///
67
    /// \brief Performs the multiplication \f$ M v \f$ by using the sparsity pattern of the M matrix.
68
69
70
    ///
    /// \param[in] model The model structure of the rigid body system.
    /// \param[in] data The data structure of the rigid body system.
71
    /// \param[in] min The input matrix to multiply with data.M.
72
    ///
73
    /// \return A the result of \f$ Mv \f$.
74
    ///
75
    template<typename Mat>
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
    typename EIGEN_PLAIN_TYPE(Mat) Mv(const Model & model,
                                      const Data & data,
                                      const Eigen::MatrixBase<Mat> & min);
    
    ///
    /// \brief Performs the multiplication \f$ M v \f$ by using the sparsity pattern of the M matrix.
    ///
    /// \param[in] model The model structure of the rigid body system.
    /// \param[in] data The data structure of the rigid body system.
    /// \param[in] min The input matrix to multiply with data.M.
    /// \param[out] mout The output matrix where the result of \f$ Mv \f$ is stored.
    ///
    /// \return A reference of the result of \f$ Mv \f$.
    ///
    template<typename Mat, typename MatRes>
    MatRes & Mv(const Model & model,
                const Data & data,
                const Eigen::MatrixBase<Mat> & min,
                const Eigen::MatrixBase<MatRes> & mout);
    
    
    ///
    /// \brief Performs the multiplication \f$ M v \f$ by using the Cholesky decomposition of M stored in data.
    ///
    /// \param[in] model The model structure of the rigid body system.
    /// \param[in] data The data structure of the rigid body system.
    /// \param[inout] m The input matrix where the result of \f$ Mv \f$ is stored.
    ///
    /// \return A reference of the result of \f$ Mv \f$.
    ///
    template<typename Mat>
    Mat & UDUtv(const Model & model,
                const Data & data,
                const Eigen::MatrixBase<Mat> & m);
110
    
111
112
113
114
115
    ///
    /// \brief Perform the sparse multiplication \f$ Uv \f$ using the Cholesky decomposition stored in data and acting in place.
    ///
    /// \param[in] model The model structure of the rigid body system.
    /// \param[in] data The data structure of the rigid body system.
116
    /// \param[inout] v The input matrix to multiply with data.U and also storing the result.
117
118
119
    ///
    /// \return A reference to the result of \f$ Uv \f$ stored in v.
    ///
Nicolas Mansard's avatar
Indent.    
Nicolas Mansard committed
120
    template<typename Mat>
121
122
    Mat & Uv(const Model & model,
             const Data & data,
123
             const Eigen::MatrixBase<Mat> & v);
124
    
125
126
127
128
129
    ///
    /// \brief Perform the sparse multiplication \f$ U^{\top}v \f$ using the Cholesky decomposition stored in data and acting in place.
    ///
    /// \param[in] model The model structure of the rigid body system.
    /// \param[in] data The data structure of the rigid body system.
130
    /// \param[inout] v The input matrix to multiply with data.U.tranpose() and also storing the result.
131
132
133
    ///
    /// \return A reference to the result of \f$ U^{\top}v \f$ stored in v.
    ///
Nicolas Mansard's avatar
Indent.    
Nicolas Mansard committed
134
    template<typename Mat>
135
136
    Mat & Utv(const Model & model,
              const Data & data,
137
              const Eigen::MatrixBase<Mat> & v);
138
    
139
140
141
142
143
    ///
    /// \brief Perform the pivot inversion \f$ U^{-1}v \f$ using the Cholesky decomposition stored in data and acting in place.
    ///
    /// \param[in] model The model structure of the rigid body system.
    /// \param[in] data The data structure of the rigid body system.
144
    /// \param[inout] v The input matrix to multiply with data.U^{-1} and also storing the result.
145
146
147
148
149
    ///
    /// \return A reference to the result of \f$ U^{-1}v \f$ stored in v.
    ///
    /// \remark The result is similar to the code data.U.triangularView<Eigen::Upper> ().solveInPlace(v).
    ///
Nicolas Mansard's avatar
Indent.    
Nicolas Mansard committed
150
    template<typename Mat>
151
152
    Mat & Uiv(const Model & model,
              const Data & data ,
153
              const Eigen::MatrixBase<Mat> & v);
154
    
155
156
157
158
159
    ///
    /// \brief Perform the pivot inversion \f$ U^{-\top}v \f$ using the Cholesky decomposition stored in data and acting in place.
    ///
    /// \param[in] model The model structure of the rigid body system.
    /// \param[in] data The data structure of the rigid body system.
160
    /// \param[inout] v The input matrix to multiply with data.U^{-\top} and also storing the result.
161
162
163
164
165
    ///
    /// \return A reference to the result of \f$ U^{-\top}v \f$ stored in v.
    ///
    /// \remark The result is similar to the code data.U.triangularView<Eigen::Upper> ().transpose().solveInPlace(v).
    ///
166
    template<typename Mat>
167
168
    Mat & Utiv(const Model & model,
               const Data & data ,
169
               const Eigen::MatrixBase<Mat> & v);
170
    
171
172
173
174
175
    ///
    /// \brief Perform the sparse inversion \f$ M^{-1}v \f$ using the Cholesky decomposition stored in data and acting in place.
    ///
    /// \param[in] model The model structure of the rigid body system.
    /// \param[in] data The data structure of the rigid body system.
176
    /// \param[inout] v The input matrix to multiply with data.M^{-1} and also storing the result.
177
178
179
    ///
    /// \return A reference to the result of \f$ M^{-1}v \f$ stored in v.
    ///
180
    template<typename Mat>
181
182
    Mat & solve(const Model & model,
                const Data & data ,
183
                const Eigen::MatrixBase<Mat> & v);
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
    
    ///
    /// \brief PComputes the inverse of the joint inertia matrix M from its Cholesky factorization.
    ///
    /// \param[in] model The model structure of the rigid body system.
    /// \param[in] data The data structure of the rigid body system.
    /// \param[out] Minv The output matrix where the result is stored.
    ///
    /// \return A reference to the result.
    ///
    template<typename Mat>
    Mat & computeMinv(const Model & model,
                      const Data & data,
                      const Eigen::MatrixBase<Mat> & Minv);
    
199
  } // namespace cholesky  
Nicolas Mansard's avatar
Nicolas Mansard committed
200
} // namespace se3 
201
202
203
204
205
206

/* --- Details -------------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------------- */
#include "pinocchio/algorithm/cholesky.hxx"

Nicolas Mansard's avatar
Nicolas Mansard committed
207
#endif // ifndef __se3_cholesky_hpp__