5 #ifndef __pinocchio_algorithm_contact_cholesky_hpp__
6 #define __pinocchio_algorithm_contact_cholesky_hpp__
21 template<
typename MatrixLike,
int ColsAtCompileTime = MatrixLike::ColsAtCompileTime>
24 template<
typename MatrixLike,
int ColsAtCompileTime = MatrixLike::ColsAtCompileTime>
27 template<
typename MatrixLike,
int ColsAtCompileTime = MatrixLike::ColsAtCompileTime>
30 template<
typename MatrixLike,
int ColsAtCompileTime = MatrixLike::ColsAtCompileTime>
33 template<
typename Scalar,
int Options,
typename VectorLike>
36 const Eigen::DenseIndex col,
37 const Eigen::MatrixBase<VectorLike> & vec);
40 template<
typename _ContactCholeskyDecomposition>
54 template<
typename _Scalar,
int _Options>
58 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
69 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> Vector;
70 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> Matrix;
78 typedef Eigen::Matrix<Eigen::DenseIndex, Eigen::Dynamic, 1, Options> IndexVector;
80 typedef Eigen::Matrix<bool, Eigen::Dynamic, 1, Options> BooleanVector;
86 Slice(
const Eigen::DenseIndex & first_index,
const Eigen::DenseIndex &
size)
87 : first_index(first_index)
92 Eigen::DenseIndex first_index;
93 Eigen::DenseIndex
size;
97 DelassusCholeskyExpression;
100 typedef std::vector<Slice> SliceVector;
101 typedef std::vector<SliceVector> VectorOfSliceVector;
116 template<
typename S1,
int O1,
template<
typename,
int>
class JointCollectionTpl>
124 allocate(
model, empty_contact_models);
137 template<
typename S1,
int O1,
template<
typename,
int>
class JointCollectionTpl,
class Allocator>
160 template<
typename S1,
int O1,
template<
typename,
int>
class JointCollectionTpl,
class Allocator>
170 Matrix getInverseOperationalSpaceInertiaMatrix()
const
172 Matrix
res(constraintDim(), constraintDim());
173 getInverseOperationalSpaceInertiaMatrix(
res);
177 template<
typename MatrixType>
178 void getInverseOperationalSpaceInertiaMatrix(
const Eigen::MatrixBase<MatrixType> &
res)
const
183 const ConstBlockXpr U1 =
U.topLeftCorner(constraintDim(), constraintDim());
187 OSIMinv_tmp.noalias() =
D.head(constraintDim()).asDiagonal() * U1.adjoint();
188 res_.noalias() = -U1 * OSIMinv_tmp;
194 DelassusCholeskyExpression getDelassusCholeskyExpression()
const
196 return DelassusCholeskyExpression(*
this);
202 Matrix getOperationalSpaceInertiaMatrix()
const
204 Matrix
res(constraintDim(), constraintDim());
205 getOperationalSpaceInertiaMatrix(
res);
209 template<
typename MatrixType>
210 void getOperationalSpaceInertiaMatrix(
const Eigen::MatrixBase<MatrixType> & res_)
const
216 const Eigen::TriangularView<ConstBlockXpr, Eigen::UnitUpper> U1 =
217 U.topLeftCorner(constraintDim(), constraintDim())
218 .template triangularView<Eigen::UnitUpper>();
222 U1.solveInPlace(U1inv);
223 OSIMinv_tmp.noalias() = -U1inv.adjoint() * Dinv.head(constraintDim()).asDiagonal();
224 res.noalias() = OSIMinv_tmp * U1inv;
228 Matrix getInverseMassMatrix()
const
231 getInverseMassMatrix(
res);
235 template<
typename MatrixType>
236 void getInverseMassMatrix(
const Eigen::MatrixBase<MatrixType> & res_)
const
242 const Eigen::TriangularView<ConstBlockXpr, Eigen::UnitUpper> U4 =
243 U.bottomRightCorner(
nv,
nv).template triangularView<Eigen::UnitUpper>();
247 U4.solveInPlace(U4inv);
248 Minv_tmp.noalias() = U4inv.adjoint() * Dinv.tail(
nv).asDiagonal();
249 res.noalias() = Minv_tmp * U4inv;
253 template<
typename MatrixType>
254 void getJMinv(
const Eigen::MatrixBase<MatrixType> & res_)
const
259 const Eigen::TriangularView<ConstBlockXpr, Eigen::UnitUpper> U4 =
260 U.bottomRightCorner(
nv,
nv).template triangularView<Eigen::UnitUpper>();
261 ConstBlockXpr U2 =
U.topRightCorner(constraintDim(),
nv);
264 U4.solveInPlace(U4inv);
265 res.noalias() = U2 * U4inv;
291 template<
typename,
int>
292 class JointCollectionTpl,
293 class ConstraintModelAllocator,
294 class ConstraintDataAllocator>
300 const S1
mu = S1(0.))
324 template<
typename,
int>
325 class JointCollectionTpl,
326 class ConstraintModelAllocator,
327 class ConstraintDataAllocator,
334 const Eigen::MatrixBase<VectorLike> & mus);
344 template<
typename VectorLike>
345 void updateDamping(
const Eigen::MatrixBase<VectorLike> & mus);
354 void updateDamping(
const Scalar &
mu);
357 Eigen::DenseIndex
size()
const
364 Eigen::DenseIndex constraintDim()
const
370 Eigen::DenseIndex numContacts()
const
386 template<
typename MatrixLike>
387 void solveInPlace(
const Eigen::MatrixBase<MatrixLike> &
mat)
const;
398 template<
typename MatrixLike>
399 Matrix
solve(
const Eigen::MatrixBase<MatrixLike> &
mat)
const;
406 template<
typename S1,
int O1,
template<
typename,
int>
class JointCollectionTpl>
412 template<
typename MatrixLike>
413 void Uv(
const Eigen::MatrixBase<MatrixLike> &
mat)
const;
415 template<
typename MatrixLike>
416 void Utv(
const Eigen::MatrixBase<MatrixLike> &
mat)
const;
418 template<
typename MatrixLike>
419 void Uiv(
const Eigen::MatrixBase<MatrixLike> &
mat)
const;
421 template<
typename MatrixLike>
422 void Utiv(
const Eigen::MatrixBase<MatrixLike> &
mat)
const;
426 Matrix matrix()
const;
429 template<
typename MatrixType>
430 void matrix(
const Eigen::MatrixBase<MatrixType> &
res)
const;
436 template<
typename MatrixType>
437 void inverse(
const Eigen::MatrixBase<MatrixType> &
res)
const;
445 template<
typename MatrixLike,
int ColsAtCompileTime>
448 template<
typename MatrixLike,
int ColsAtCompileTime>
451 template<
typename MatrixLike,
int ColsAtCompileTime>
454 template<
typename MatrixLike,
int ColsAtCompileTime>
460 template<
typename Scalar,
int Options,
typename VectorLike>
463 const Eigen::DenseIndex col,
464 const Eigen::MatrixBase<VectorLike> &
vec);
467 template<
typename S1,
int O1>
472 if (
nv != other.nv || num_contacts != other.num_contacts)
476 D.size() != other.D.size() || Dinv.size() != other.Dinv.size() ||
U.rows() != other.U.rows()
477 ||
U.cols() != other.U.cols())
480 is_same &= (
D == other.D);
481 is_same &= (Dinv == other.Dinv);
482 is_same &= (
U == other.U);
484 is_same &= (parents_fromRow == other.parents_fromRow);
485 is_same &= (nv_subtree_fromRow == other.nv_subtree_fromRow);
486 is_same &= (last_child == other.last_child);
492 template<
typename S1,
int O1>
495 return !(*
this == other);
500 IndexVector parents_fromRow;
501 IndexVector nv_subtree_fromRow;
504 IndexVector last_child;
509 Eigen::DenseIndex
nv;
512 Eigen::DenseIndex num_contacts;
514 VectorOfSliceVector rowise_sparsity_pattern;
517 mutable Matrix U1inv;
519 mutable Matrix U4inv;
521 mutable RowMatrix OSIMinv_tmp, Minv_tmp;
524 template<
typename ContactCholeskyDecomposition>
532 typedef typename ContactCholeskyDecomposition::Matrix
Matrix;
533 typedef typename ContactCholeskyDecomposition::Vector
Vector;
536 template<
typename _ContactCholeskyDecomposition>
542 typedef typename ContactCholeskyDecomposition::Vector
Vector;
543 typedef typename ContactCholeskyDecomposition::Matrix
Matrix;
544 typedef typename ContactCholeskyDecomposition::RowMatrix
RowMatrix;
564 template<
typename MatrixIn,
typename MatrixOut>
566 const Eigen::MatrixBase<MatrixIn> & x,
const Eigen::MatrixBase<MatrixOut> &
res)
const
575 self.U.topLeftCorner(
self.constraintDim(),
self.constraintDim());
577 if (
x.cols() <=
self.constraintDim())
581 self.constraintDim(),
x.cols());
583 triangularMatrixMatrixProduct<Eigen::UnitLower>(U1.adjoint(),
x.derived(), tmp_mat);
584 tmp_mat.array().colwise() *= -
self.D.head(
self.constraintDim()).array();
586 triangularMatrixMatrixProduct<Eigen::UnitUpper>(U1, tmp_mat,
res.const_cast_derived());
592 triangularMatrixMatrixProduct<Eigen::UnitLower>(U1.adjoint(),
x.derived(), tmp_mat);
593 tmp_mat.array().colwise() *= -
self.D.head(
self.constraintDim()).array();
595 triangularMatrixMatrixProduct<Eigen::UnitUpper>(U1, tmp_mat,
res.const_cast_derived());
601 template<
typename MatrixDerived>
603 operator*(
const Eigen::MatrixBase<MatrixDerived> & x)
const
606 ReturnType
res(
self.constraintDim(),
x.cols());
611 template<
typename MatrixDerived>
612 void solveInPlace(
const Eigen::MatrixBase<MatrixDerived> & x)
const
616 const Eigen::TriangularView<RowMatrixConstBlockXpr, Eigen::UnitUpper> U1 =
617 self.U.topLeftCorner(
self.constraintDim(),
self.constraintDim())
618 .template triangularView<Eigen::UnitUpper>();
621 U1.solveInPlace(
x.const_cast_derived());
622 x.const_cast_derived().array().colwise() *= -
self.Dinv.head(
self.constraintDim()).array();
623 U1.adjoint().solveInPlace(x);
627 template<
typename MatrixDerivedIn,
typename MatrixDerivedOut>
629 const Eigen::MatrixBase<MatrixDerivedIn> & x,
630 const Eigen::MatrixBase<MatrixDerivedOut> &
res)
const
632 res.const_cast_derived() =
x;
633 solveInPlace(
res.const_cast_derived());
636 template<
typename MatrixDerived>
638 solve(
const Eigen::MatrixBase<MatrixDerived> & x)
const
641 ReturnType
res(
self.constraintDim(),
x.cols());
655 return self.getInverseOperationalSpaceInertiaMatrix();
660 return self.getOperationalSpaceInertiaMatrix();
670 template<
typename VectorLike>
671 void updateDamping(
const Eigen::MatrixBase<VectorLike> & mus)
690 return self.constraintDim();
707 #include "pinocchio/algorithm/contact-cholesky.hxx"
709 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
710 #include "pinocchio/algorithm/contact-cholesky.txx"
711 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
713 #endif // ifndef __pinocchio_algorithm_contact_cholesky_hpp__