orocos_kdl
ldl_solver_eigen.cpp
Go to the documentation of this file.
1 // Copyright (C) 2018 Craig Carignan <craigc at ssl dot umd dot edu>
2 
3 // Version: 1.0
4 // Author: Craig Carignan
5 // Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
6 // URL: http://www.orocos.org/kdl
7 
8 // This library is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU Lesser General Public
10 // License as published by the Free Software Foundation; either
11 // version 2.1 of the License, or (at your option) any later version.
12 
13 // This library is distributed in the hope that it will be useful,
14 // but WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 // Lesser General Public License for more details.
17 
18 // You should have received a copy of the GNU Lesser General Public
19 // License along with this library; if not, write to the Free Software
20 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
21 
22 #include "ldl_solver_eigen.hpp"
23 
24 namespace KDL{
25 
26  int ldl_solver_eigen(const Eigen::MatrixXd& A, const Eigen::VectorXd& v, Eigen::MatrixXd& L, Eigen::VectorXd& D, Eigen::VectorXd& vtmp, Eigen::VectorXd& q)
27  {
28  const int n = static_cast<int>(A.rows());
29  int error=SolverI::E_NOERROR;
30 
31  //Check sizes
32  if(A.cols()!=n || v.rows()!=n || L.rows()!=n || L.cols()!=n || D.rows()!=n || vtmp.rows()!=n || q.rows()!=n)
33  return (error = SolverI::E_SIZE_MISMATCH);
34 
35  for(int i=0;i<n;++i) {
36  D(i)=A(i,i);
37  if(i>0) {
38  for(int j=0;j<=i-1;++j)
39  D(i) -= D(j)*L(i,j)*L(i,j);
40  }
41  for(int j=1;j<n;++j) {
42  if(j>i) {
43  L(j,i)=A(i,j)/D(i);
44  if(i>0) {
45  for(int k=0;k<=i-1;++k)
46  L(j,i) -= L(j,k)*L(i,k)*D(k)/D(i);
47  }
48  }
49  }
50  }
51  for(int i=0;i<n;++i) {
52  vtmp(i)=v(i);
53  if(i>0) {
54  for(int j=0;j<=i-1;++j)
55  vtmp(i) -= L(i,j)*vtmp(j);
56  }
57  }
58  for(int i=n-1;i>=0;--i) {
59  q(i)=vtmp(i)/D(i);
60  if(i<n-1) {
61  for(int j=i+1;j<n;++j)
62  q(i) -= L(j,i)*q(j);
63  }
64  }
65  // optional: changes diagonal elements of L to 1 as per LDL decomposition
66  // A = L * Ddiag * L^T where Ddiag(i,i) = D(i)
67  for(int i=0;i<n;++i) {
68  L(i,i) = 1.;
69  }
70  // optional: sets upper triangular, off-diagonal elements of L to 0
71  // because algorithm does not do this automatically
72  if ( n > 1 )
73  {
74  for(int i=0;i<n;++i) {
75  for(int j=i+1;j<n;++j) {
76  L(i,j) = 0.0;
77  }
78  }
79  }
80  return(error);
81  }
82 }
KDL
Definition: kukaLWR_DHnew.cpp:25
ldl_solver_eigen.hpp
KDL::SolverI::E_SIZE_MISMATCH
@ E_SIZE_MISMATCH
Input size does not match internal state.
Definition: solveri.hpp:99
KDL::ldl_solver_eigen
int ldl_solver_eigen(const Eigen::MatrixXd &A, const Eigen::VectorXd &v, Eigen::MatrixXd &L, Eigen::VectorXd &D, Eigen::VectorXd &vtmp, Eigen::VectorXd &q)
Solves the system of equations Aq = v for q via LDL decomposition, where A is a square positive defin...
Definition: ldl_solver_eigen.cpp:26
KDL::SolverI::E_NOERROR
@ E_NOERROR
No error.
Definition: solveri.hpp:91