pinocchio
3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
Loading...
Searching...
No Matches
gram-schmidt-orthonormalisation.hpp
1
//
2
// Copyright (c) 2024 INRIA
3
//
4
5
#ifndef __pinocchio_math_gram_schmidt_orthonormalisation_hpp__
6
#define __pinocchio_math_gram_schmidt_orthonormalisation_hpp__
7
8
#include "pinocchio/math/fwd.hpp"
9
#include <Eigen/Core>
10
11
namespace
pinocchio
12
{
19
template
<
typename
MatrixType,
typename
VectorType>
20
void
orthonormalisation
(
21
const
Eigen::MatrixBase<MatrixType> &
basis
,
const
Eigen::MatrixBase<VectorType> &
vec_
)
22
{
23
typedef
typename
VectorType::Scalar Scalar;
24
VectorType
&
vec
=
vec_
.const_cast_derived();
25
26
PINOCCHIO_CHECK_ARGUMENT_SIZE(
basis
.rows(),
vec
.size());
27
assert
((
basis
.transpose() *
basis
).isIdentity() &&
"The input basis is not orthonormal."
);
28
29
for
(Eigen::DenseIndex
col_id
= 0;
col_id
<
basis
.cols(); ++
col_id
)
30
{
31
const
auto
col
=
basis
.col(
col_id
);
32
const
Scalar
alpha
=
col
.dot(
vec
);
33
vec
-=
alpha
*
col
;
34
}
35
36
assert
((
basis
.transpose() *
vec
).isZero());
37
}
38
}
// namespace pinocchio
39
40
#endif
// ifndef __pinocchio_math_gram_schmidt_orthonormalisation_hpp__
pinocchio
Main pinocchio namespace.
Definition
treeview.dox:11
pinocchio::orthonormalisation
void orthonormalisation(const Eigen::MatrixBase< MatrixType > &basis, const Eigen::MatrixBase< VectorType > &vec_)
Perform the Gram-Schmidt orthonormalisation on the input/output vector for a given input basis.
Definition
gram-schmidt-orthonormalisation.hpp:20
pinocchio::ContactSolverBaseTpl
Definition
contact-solver-base.hpp:20
include
pinocchio
math
gram-schmidt-orthonormalisation.hpp
Generated by
1.9.8