Skip to content
GitLab
Projects
Groups
Snippets
Help
Loading...
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
P
pinocchio
Project overview
Project overview
Details
Activity
Releases
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Issues
0
Issues
0
List
Boards
Labels
Service Desk
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Operations
Operations
Incidents
Environments
Packages & Registries
Packages & Registries
Container Registry
Analytics
Analytics
CI / CD
Repository
Value Stream
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Stack Of Tasks
pinocchio
Commits
247e573d
Verified
Commit
247e573d
authored
Jun 07, 2019
by
Mikhail Katliar
Committed by
Justin Carpentier
Jun 21, 2019
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Added LLT specialization for matrices of casadi::SX
parent
1401d481
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
with
325 additions
and
7 deletions
+325
-7
.gitignore
.gitignore
+1
-0
src/math/casadi.hpp
src/math/casadi.hpp
+284
-0
unittest/casadi-basic.cpp
unittest/casadi-basic.cpp
+40
-7
No files found.
.gitignore
View file @
247e573d
...
...
@@ -3,4 +3,5 @@ Xcode*
*~
*.pyc
coverage*
.travis
.vscode*
src/math/casadi.hpp
View file @
247e573d
...
...
@@ -6,6 +6,48 @@
#define __pinocchio_math_casadi_hpp__
#include <casadi/casadi.hpp>
// #include <Eigen/Core>
#include <Eigen/Dense>
// Copy casadi matrix to Eigen matrix
template
<
typename
MT
>
inline
void
copy
(
casadi
::
SX
const
&
src
,
Eigen
::
MatrixBase
<
MT
>&
dst
)
{
Eigen
::
Index
const
m
=
src
.
size1
();
Eigen
::
Index
const
n
=
src
.
size2
();
dst
.
resize
(
m
,
n
);
for
(
Eigen
::
Index
i
=
0
;
i
<
m
;
++
i
)
for
(
Eigen
::
Index
j
=
0
;
j
<
n
;
++
j
)
dst
(
i
,
j
)
=
src
(
i
,
j
);
}
// Copy Eigen matrix to casadi matrix
template
<
typename
MT
>
inline
void
copy
(
Eigen
::
MatrixBase
<
MT
>
const
&
src
,
casadi
::
SX
&
dst
)
{
Eigen
::
Index
const
m
=
src
.
rows
();
Eigen
::
Index
const
n
=
src
.
cols
();
dst
.
resize
(
m
,
n
);
for
(
Eigen
::
Index
i
=
0
;
i
<
m
;
++
i
)
for
(
Eigen
::
Index
j
=
0
;
j
<
n
;
++
j
)
dst
(
i
,
j
)
=
src
(
i
,
j
);
}
// Make an Eigen matrix consisting of pure casadi symbolics
template
<
typename
MT
>
inline
void
sym
(
Eigen
::
MatrixBase
<
MT
>&
eig_mat
,
std
::
string
const
&
name
)
{
for
(
Eigen
::
Index
i
=
0
;
i
<
eig_mat
.
rows
();
++
i
)
for
(
Eigen
::
Index
j
=
0
;
j
<
eig_mat
.
cols
();
++
j
)
eig_mat
(
i
,
j
)
=
casadi
::
SX
::
sym
(
name
+
"_"
+
std
::
to_string
(
i
)
+
"_"
+
std
::
to_string
(
j
));
}
namespace
Eigen
{
...
...
@@ -28,6 +70,8 @@ namespace Eigen
namespace
Eigen
{
/// @brief Eigen::NumTraits<> specialization for casadi::SX
///
template
<
>
struct
NumTraits
<
casadi
::
SX
>
{
using
Real
=
casadi
::
SX
;
...
...
@@ -68,6 +112,246 @@ namespace Eigen
return
std
::
numeric_limits
<
double
>::
digits10
;
}
};
namespace
internal
{
/// @brief LLT_Traits for casadi::SX-valued matrices
///
/// LLT specialization for matrices of casadi::SX containts in m_matrix the L such as LL'=A,
/// regardless of the value of UpLo.
/// Therefore we define two identical specializations of LLT_Traits for the two values of UpLo.
template
<
int
M
,
int
N
,
int
Options
>
struct
LLT_Traits
<
Matrix
<
casadi
::
SX
,
M
,
N
,
Options
>
,
Lower
>
{
using
MatrixType
=
Matrix
<
casadi
::
SX
,
M
,
N
,
Options
>
;
typedef
const
TriangularView
<
const
MatrixType
,
Lower
>
MatrixL
;
typedef
const
TriangularView
<
const
typename
MatrixType
::
AdjointReturnType
,
Upper
>
MatrixU
;
static
inline
MatrixL
getL
(
const
MatrixType
&
m
)
{
return
MatrixL
(
m
);
}
static
inline
MatrixU
getU
(
const
MatrixType
&
m
)
{
return
MatrixU
(
m
.
adjoint
());
}
static
bool
inplace_decomposition
(
MatrixType
&
m
)
{
return
llt_inplace
<
typename
MatrixType
::
Scalar
,
Lower
>::
blocked
(
m
)
==-
1
;
}
};
/// @brief LLT_Traits for casadi::SX-valued matrices
///
/// LLT specialization for matrices of casadi::SX containts in m_matrix the L such as LL'=A,
/// regardless of the value of UpLo.
/// Therefore we define two identical specializations of LLT_Traits for the two values of UpLo.
template
<
int
M
,
int
N
,
int
Options
>
struct
LLT_Traits
<
Matrix
<
casadi
::
SX
,
M
,
N
,
Options
>
,
Upper
>
{
using
MatrixType
=
Matrix
<
casadi
::
SX
,
M
,
N
,
Options
>
;
typedef
const
TriangularView
<
const
typename
MatrixType
::
AdjointReturnType
,
Lower
>
MatrixL
;
typedef
const
TriangularView
<
const
MatrixType
,
Upper
>
MatrixU
;
static
inline
MatrixL
getL
(
const
MatrixType
&
m
)
{
return
MatrixL
(
m
);
}
static
inline
MatrixU
getU
(
const
MatrixType
&
m
)
{
return
MatrixU
(
m
.
adjoint
());
}
static
bool
inplace_decomposition
(
MatrixType
&
m
)
{
return
llt_inplace
<
typename
MatrixType
::
Scalar
,
Upper
>::
blocked
(
m
)
==-
1
;
}
};
}
/// @brief LLT template specialization supporting casadi::SX
///
template
<
int
M
,
int
N
,
int
Options
,
int
_UpLo
>
class
LLT
<
Matrix
<
casadi
::
SX
,
M
,
N
,
Options
>
,
_UpLo
>
{
public:
typedef
Matrix
<
casadi
::
SX
,
M
,
N
,
Options
>
MatrixType
;
enum
{
RowsAtCompileTime
=
MatrixType
::
RowsAtCompileTime
,
ColsAtCompileTime
=
MatrixType
::
ColsAtCompileTime
,
MaxColsAtCompileTime
=
MatrixType
::
MaxColsAtCompileTime
};
typedef
typename
MatrixType
::
Scalar
Scalar
;
typedef
typename
NumTraits
<
typename
MatrixType
::
Scalar
>::
Real
RealScalar
;
typedef
Eigen
::
Index
Index
;
///< \deprecated since Eigen 3.3
typedef
typename
MatrixType
::
StorageIndex
StorageIndex
;
enum
{
PacketSize
=
internal
::
packet_traits
<
Scalar
>::
size
,
AlignmentMask
=
int
(
PacketSize
)
-
1
,
UpLo
=
_UpLo
};
typedef
internal
::
LLT_Traits
<
MatrixType
,
UpLo
>
Traits
;
/**
* \brief Default Constructor.
*
* The default constructor is useful in cases in which the user intends to
* perform decompositions via LLT::compute(const MatrixType&).
*/
LLT
()
:
m_matrix
(),
m_isInitialized
(
false
)
{}
/** \brief Default Constructor with memory preallocation
*
* Like the default constructor but with preallocation of the internal data
* according to the specified problem \a size.
* \sa LLT()
*/
explicit
LLT
(
Index
size
)
:
m_matrix
(
size
,
size
),
m_isInitialized
(
false
)
{}
template
<
typename
InputType
>
explicit
LLT
(
const
EigenBase
<
InputType
>&
matrix
)
:
m_matrix
(
matrix
.
rows
(),
matrix
.
cols
()),
m_isInitialized
(
false
)
{
compute
(
matrix
.
derived
());
}
/** \brief Constructs a LDLT factorization from a given matrix
*
* This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when
* \c MatrixType is a Eigen::Ref.
*
* \sa LLT(const EigenBase&)
*/
template
<
typename
InputType
>
explicit
LLT
(
EigenBase
<
InputType
>&
matrix
)
:
m_matrix
(
matrix
.
derived
()),
m_isInitialized
(
false
)
{
compute
(
matrix
.
derived
());
}
/** \returns a view of the upper triangular matrix U */
inline
typename
Traits
::
MatrixU
matrixU
()
const
{
eigen_assert
(
m_isInitialized
&&
"LLT is not initialized."
);
return
Traits
::
getU
(
m_matrix
);
}
/** \returns a view of the lower triangular matrix L */
inline
typename
Traits
::
MatrixL
matrixL
()
const
{
eigen_assert
(
m_isInitialized
&&
"LLT is not initialized."
);
return
Traits
::
getL
(
m_matrix
);
}
/** \returns the solution x of \f$ A x = b \f$ using the current decomposition of A.
*
* Since this LLT class assumes anyway that the matrix A is invertible, the solution
* theoretically exists and is unique regardless of b.
*
* Example: \include LLT_solve.cpp
* Output: \verbinclude LLT_solve.out
*
* \sa solveInPlace(), MatrixBase::llt(), SelfAdjointView::llt()
*/
template
<
typename
Rhs
>
inline
const
Solve
<
LLT
,
Rhs
>
solve
(
const
MatrixBase
<
Rhs
>&
b
)
const
{
eigen_assert
(
m_isInitialized
&&
"LLT is not initialized."
);
eigen_assert
(
m_matrix
.
rows
()
==
b
.
rows
()
&&
"LLT::solve(): invalid number of rows of the right hand side matrix b"
);
return
Solve
<
LLT
,
Rhs
>
(
*
this
,
b
.
derived
());
}
template
<
typename
Derived
>
void
solveInPlace
(
const
MatrixBase
<
Derived
>
&
bAndX
)
const
;
template
<
typename
InputType
>
LLT
&
compute
(
const
EigenBase
<
InputType
>&
matrix
)
{
// Copy the triangular part of matrix that will be used for the decompositon to a self-adjoint m_matrix,
// taking into account the UpLo flag.
m_matrix
=
SelfAdjointView
<
InputType
const
,
UpLo
>
(
matrix
);
// The Eigen version of LLT::compute() does not compile for matrices of casadi::SX,
// because operator>() for casadi::SX is not defined.
// Therefore, we use casadi::chol() to do the decomposition, and then copy the result to an Eigen matrix of casadi::SX.
// Copy m_matrix to a temporary casadi::SX matrix
casadi
::
SX
cs_matrix
;
copy
(
m_matrix
,
cs_matrix
);
// Let casadi do the decomposition.
// The result needs to be transposed, because casadi returns an upper triangular R such that R'R = A
// http://casadi.sourceforge.net/api/html/da/d45/classcasadi_1_1Matrix.html
// and we need L such that LL' = A.
cs_matrix
=
transpose
(
chol
(
cs_matrix
));
// Copy the result to m_matrix and set the isInitialized flag.
copy
(
cs_matrix
,
m_matrix
);
m_isInitialized
=
true
;
return
*
this
;
}
/** \returns an estimate of the reciprocal condition number of the matrix of
* which \c *this is the Cholesky decomposition.
*/
RealScalar
rcond
()
const
{
eigen_assert
(
m_isInitialized
&&
"LLT is not initialized."
);
eigen_assert
(
m_info
==
Success
&&
"LLT failed because matrix appears to be negative"
);
return
internal
::
rcond_estimate_helper
(
m_l1_norm
,
*
this
);
}
/** \returns the LLT decomposition matrix
*
* TODO: document the storage layout
*/
inline
const
MatrixType
&
matrixLLT
()
const
{
eigen_assert
(
m_isInitialized
&&
"LLT is not initialized."
);
return
m_matrix
;
}
MatrixType
reconstructedMatrix
()
const
;
/** \brief Reports whether previous computation was successful.
*
* \returns \c Success if computation was succesful,
* \c NumericalIssue if the matrix.appears not to be positive definite.
*/
ComputationInfo
info
()
const
{
eigen_assert
(
m_isInitialized
&&
"LLT is not initialized."
);
return
m_info
;
}
/** \returns the adjoint of \c *this, that is, a const reference to the decomposition itself as the underlying matrix is self-adjoint.
*
* This method is provided for compatibility with other matrix decompositions, thus enabling generic code such as:
* \code x = decomposition.adjoint().solve(b) \endcode
*/
const
LLT
&
adjoint
()
const
{
return
*
this
;
};
inline
Index
rows
()
const
{
return
m_matrix
.
rows
();
}
inline
Index
cols
()
const
{
return
m_matrix
.
cols
();
}
template
<
typename
VectorType
>
LLT
rankUpdate
(
const
VectorType
&
vec
,
const
RealScalar
&
sigma
=
1
);
#ifndef EIGEN_PARSED_BY_DOXYGEN
template
<
typename
RhsType
,
typename
DstType
>
EIGEN_DEVICE_FUNC
void
_solve_impl
(
const
RhsType
&
rhs
,
DstType
&
dst
)
const
;
#endif
protected:
static
void
check_template_parameters
()
{
EIGEN_STATIC_ASSERT_NON_INTEGER
(
Scalar
);
}
/** \internal
* Used to compute and store L
* The strict upper part is not used and even not initialized.
*/
MatrixType
m_matrix
;
RealScalar
m_l1_norm
;
bool
m_isInitialized
;
ComputationInfo
m_info
;
};
}
#endif // #ifndef __pinocchio_math_casadi_hpp__
unittest/casadi-basic.cpp
View file @
247e573d
//
// Copyright (c) 2019 INRIA
//
#include <boost/variant.hpp> // to avoid C99 warnings
#include <
casadi
/casadi.hpp>
#include <Eigen/
Cor
e>
#include <
pinocchio/math
/casadi.hpp>
#include <Eigen/
Dens
e>
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
...
...
@@ -37,8 +36,6 @@ eigenFun(Eigen::MatrixBase<T1> const& A,
BOOST_AUTO_TEST_CASE
(
test_example
)
{
// Declare casadi symbolic matrix arguments
// casadi::SX cs_A = casadi::SX::sym("A", 3, 3);
// casadi::SX cs_B = casadi::SX::sym("B", 3, 3);
casadi
::
SX
cs_a
=
casadi
::
SX
::
sym
(
"a"
,
4
);
casadi
::
SX
cs_b
=
casadi
::
SX
::
sym
(
"b"
,
3
);
...
...
@@ -52,8 +49,8 @@ BOOST_AUTO_TEST_CASE(test_example)
{
for
(
Eigen
::
Index
j
=
0
;
j
<
A
.
cols
();
++
j
)
{
A
(
i
,
j
)
=
10
*
i
+
j
;
B
(
i
,
j
)
=
-
10
*
i
-
j
;
A
(
i
,
j
)
=
10
.
*
static_cast
<
double
>
(
i
)
+
static_cast
<
double
>
(
j
)
;
B
(
i
,
j
)
=
-
10
.
*
static_cast
<
double
>
(
i
)
-
static_cast
<
double
>
(
j
)
;
}
}
...
...
@@ -107,5 +104,41 @@ BOOST_AUTO_TEST_CASE(test_jacobian)
std
::
cout
<<
"dy/dx = "
<<
dy_dx
<<
std
::
endl
;
}
BOOST_AUTO_TEST_CASE
(
test_copy_casadi_to_eigen
)
{
casadi
::
SX
cs_mat
=
casadi
::
SX
::
sym
(
"A"
,
3
,
4
);
Eigen
::
Matrix
<
casadi
::
SX
,
3
,
4
>
eig_mat
;
copy
(
cs_mat
,
eig_mat
);
std
::
cout
<<
eig_mat
<<
std
::
endl
;
}
BOOST_AUTO_TEST_CASE
(
test_copy_eigen_to_casadi
)
{
Eigen
::
Matrix
<
casadi
::
SX
,
3
,
4
>
eig_mat
;
sym
(
eig_mat
,
"A"
);
casadi
::
SX
cs_mat
;
copy
(
eig_mat
,
cs_mat
);
std
::
cout
<<
cs_mat
<<
std
::
endl
;
}
BOOST_AUTO_TEST_CASE
(
test_chol
)
{
using
EigenSX
=
Eigen
::
Matrix
<
casadi
::
SX
,
2
,
2
>
;
EigenSX
A
;
sym
(
A
,
"A"
);
Eigen
::
LLT
<
EigenSX
>
llt
(
A
);
EigenSX
L
=
llt
.
matrixL
();
EigenSX
U
=
llt
.
matrixU
();
std
::
cout
<<
"L = "
<<
std
::
endl
<<
L
<<
std
::
endl
<<
std
::
endl
;
std
::
cout
<<
"U = "
<<
std
::
endl
<<
U
<<
std
::
endl
<<
std
::
endl
;
std
::
cout
<<
"L*L' = "
<<
std
::
endl
<<
L
*
L
.
transpose
()
<<
std
::
endl
<<
std
::
endl
;
std
::
cout
<<
"U'*U = "
<<
std
::
endl
<<
U
.
transpose
()
*
U
<<
std
::
endl
<<
std
::
endl
;
}
BOOST_AUTO_TEST_SUITE_END
()
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment