Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/algorithm/contact-dynamics.hxx	Lines:	83	83	100.0 %
Date:	2024-04-26 13:14:21	Branches:	87	416	20.9 %


//
// Copyright (c) 2016-2020 CNRS INRIA
//

#ifndef __pinocchio_contact_dynamics_hxx__
#define __pinocchio_contact_dynamics_hxx__

#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/algorithm/cholesky.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/check.hpp"

#include <Eigen/Cholesky>

namespace pinocchio
{

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename TangentVectorType,
  typename ConstraintMatrixType, typename DriftVectorType>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
  forwardDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                  DataTpl<Scalar,Options,JointCollectionTpl> & data,
                  const Eigen::MatrixBase<TangentVectorType> & tau,
                  const Eigen::MatrixBase<ConstraintMatrixType> & J,
                  const Eigen::MatrixBase<DriftVectorType> & gamma,
                  const Scalar inv_damping)
  {
    PINOCCHIO_CHECK_ARGUMENT_SIZE(tau.size(), model.nv);
    PINOCCHIO_CHECK_ARGUMENT_SIZE(J.cols(), model.nv);
    PINOCCHIO_CHECK_ARGUMENT_SIZE(J.rows(), gamma.size());
    assert(model.check(data) && "data is not consistent with model.");

    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

    typename Data::TangentVectorType & a = data.ddq;
    typename Data::VectorXs & lambda_c = data.lambda_c;

    // Compute the UDUt decomposition of data.M
    cholesky::decompose(model, data);

    // Compute the dynamic drift (control - nle)
    data.torque_residual = tau - data.nle;
    cholesky::solve(model, data, data.torque_residual);

    data.sDUiJt = J.transpose();
    // Compute U^-1 * J.T
    cholesky::Uiv(model, data, data.sDUiJt);
    for(Eigen::DenseIndex k=0;k<model.nv;++k)
      data.sDUiJt.row(k) /= sqrt(data.D[k]);

    data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt;

    data.JMinvJt.diagonal().array() += inv_damping;
    data.llt_JMinvJt.compute(data.JMinvJt);

    // Compute the Lagrange Multipliers
    lambda_c.noalias() = -J*data.torque_residual;
    lambda_c -= gamma;
    data.llt_JMinvJt.solveInPlace(lambda_c);

    // Compute the joint acceleration
    a.noalias() = J.transpose() * lambda_c;
    cholesky::solve(model, data, a);
    a += data.torque_residual;

    return a;
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
  typename ConstraintMatrixType, typename DriftVectorType>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
  forwardDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                  DataTpl<Scalar,Options,JointCollectionTpl> & data,
                  const Eigen::MatrixBase<ConfigVectorType> & q,
                  const Eigen::MatrixBase<TangentVectorType1> & v,
                  const Eigen::MatrixBase<TangentVectorType2> & tau,
                  const Eigen::MatrixBase<ConstraintMatrixType> & J,
                  const Eigen::MatrixBase<DriftVectorType> & gamma,
                  const Scalar inv_damping)
  {
    PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq);
    PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);

    computeAllTerms(model, data, q, v);

    return forwardDynamics(model,data,tau,J,gamma,inv_damping);
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
           typename ConfigVectorType, typename ConstraintMatrixType, typename KKTMatrixType>
  void computeKKTContactDynamicMatrixInverse(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                             DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                             const Eigen::MatrixBase<ConfigVectorType> & q,
                                             const Eigen::MatrixBase<ConstraintMatrixType> & J,
                                             const Eigen::MatrixBase<KKTMatrixType> & KKTMatrix_inv,
                                             const Scalar & inv_damping)
  {
    assert(model.check(data));
    PINOCCHIO_CHECK_INPUT_ARGUMENT(inv_damping >= 0., "mu must be positive.");

    // Compute the mass matrix.
    crbaMinimal(model,data,q);

    // Compute the UDUt decomposition of data.M.
    cholesky::decompose(model, data);

    using std::sqrt;
    data.sDUiJt = J.transpose();
    // Compute U^-1 * J.T
    cholesky::Uiv(model, data, data.sDUiJt);
    for(Eigen::DenseIndex k=0;k<model.nv;++k)
      data.sDUiJt.row(k) /= sqrt(data.D[k]);

    data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt;

    data.JMinvJt.diagonal().array() += inv_damping;
    data.llt_JMinvJt.compute(data.JMinvJt);

    getKKTContactDynamicMatrixInverse(model,data,J,KKTMatrix_inv.const_cast_derived());
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
  typename ConstraintMatrixType, typename KKTMatrixType>
  inline void getKKTContactDynamicMatrixInverse(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                                const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                                const Eigen::MatrixBase<ConstraintMatrixType> & J,
                                                const Eigen::MatrixBase<KKTMatrixType> & KKTMatrix_inv)
  {
    assert(model.check(data));
    PINOCCHIO_CHECK_ARGUMENT_SIZE(KKTMatrix_inv.cols(), data.JMinvJt.cols() + model.nv);
    PINOCCHIO_CHECK_ARGUMENT_SIZE(KKTMatrix_inv.rows(), data.JMinvJt.rows() + model.nv);

    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
    const Eigen::DenseIndex nc = data.JMinvJt.cols();

    KKTMatrixType & KKTMatrix_inv_ = PINOCCHIO_EIGEN_CONST_CAST(KKTMatrixType,KKTMatrix_inv);

    typedef Eigen::Block<KKTMatrixType> BlockType;
    BlockType topLeft = KKTMatrix_inv_.topLeftCorner(model.nv, model.nv);
    BlockType topRight = KKTMatrix_inv_.topRightCorner(model.nv, nc);
    BlockType bottomLeft = KKTMatrix_inv_.bottomLeftCorner(nc, model.nv);
    BlockType bottomRight = KKTMatrix_inv_.bottomRightCorner(nc, nc);

    bottomRight = -Data::MatrixXs::Identity(nc,nc); data.llt_JMinvJt.solveInPlace(bottomRight);
    topLeft.setIdentity(); cholesky::solve(model, data, topLeft);

    bottomLeft.noalias() = J*topLeft;
    topRight.noalias() = bottomLeft.transpose() * (-bottomRight);
    topLeft.noalias() -= topRight*bottomLeft;
    bottomLeft = topRight.transpose();
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ConstraintMatrixType>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
  impulseDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                  DataTpl<Scalar,Options,JointCollectionTpl> & data,
                  const Eigen::MatrixBase<ConfigVectorType> & q,
                  const Eigen::MatrixBase<TangentVectorType> & v_before,
                  const Eigen::MatrixBase<ConstraintMatrixType> & J,
                  const Scalar r_coeff,
                  const Scalar inv_damping)
  {
    PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq);

    // Compute the mass matrix
    crba(model, data, q);

    return impulseDynamics(model,data,v_before,J,r_coeff,inv_damping);
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename TangentVectorType, typename ConstraintMatrixType>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
  impulseDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                  DataTpl<Scalar,Options,JointCollectionTpl> & data,
                  const Eigen::MatrixBase<TangentVectorType> & v_before,
                  const Eigen::MatrixBase<ConstraintMatrixType> & J,
                  const Scalar r_coeff,
                  const Scalar inv_damping)
  {
    PINOCCHIO_CHECK_ARGUMENT_SIZE(v_before.size(), model.nv);
    PINOCCHIO_CHECK_ARGUMENT_SIZE(J.cols(), model.nv);
    assert(model.check(data) && "data is not consistent with model.");

    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

    typename Data::VectorXs & impulse_c = data.impulse_c;
    typename Data::TangentVectorType & dq_after = data.dq_after;

    // Compute the UDUt decomposition of data.M
    cholesky::decompose(model, data);

    data.sDUiJt = J.transpose();
    // Compute U^-1 * J.T
    cholesky::Uiv(model, data, data.sDUiJt);
    for(int k=0;k<model.nv;++k) data.sDUiJt.row(k) /= sqrt(data.D[k]);

    data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt;

    data.JMinvJt.diagonal().array() += inv_damping;
    data.llt_JMinvJt.compute(data.JMinvJt);

    // Compute the Lagrange Multipliers related to the contact impulses
    impulse_c.noalias() = (-r_coeff - 1.) * (J * v_before);
    data.llt_JMinvJt.solveInPlace(impulse_c);

    // Compute the joint velocity after impacts
    dq_after.noalias() = J.transpose() * impulse_c;
    cholesky::solve(model, data, dq_after);
    dq_after += v_before;

    return dq_after;
  }
} // namespace pinocchio

#endif // ifndef __pinocchio_contact_dynamics_hxx__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2016-2020 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_contact_dynamics_hxx__
6			#define __pinocchio_contact_dynamics_hxx__
7
8			#include "pinocchio/algorithm/compute-all-terms.hpp"
9			#include "pinocchio/algorithm/cholesky.hpp"
10			#include "pinocchio/algorithm/crba.hpp"
11			#include "pinocchio/algorithm/check.hpp"
12
13			#include <Eigen/Cholesky>
14
15			namespace pinocchio
16			{
17
18			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename TangentVectorType,
19			typename ConstraintMatrixType, typename DriftVectorType>
20			inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
21		199	forwardDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
22			DataTpl<Scalar,Options,JointCollectionTpl> & data,
23			const Eigen::MatrixBase<TangentVectorType> & tau,
24			const Eigen::MatrixBase<ConstraintMatrixType> & J,
25			const Eigen::MatrixBase<DriftVectorType> & gamma,
26			const Scalar inv_damping)
27			{
28	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	199	PINOCCHIO_CHECK_ARGUMENT_SIZE(tau.size(), model.nv);
29	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	199	PINOCCHIO_CHECK_ARGUMENT_SIZE(J.cols(), model.nv);
30	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗	199	PINOCCHIO_CHECK_ARGUMENT_SIZE(J.rows(), gamma.size());
31	✓✗	199	assert(model.check(data) && "data is not consistent with model.");
32
33			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
34
35		199	typename Data::TangentVectorType & a = data.ddq;
36		199	typename Data::VectorXs & lambda_c = data.lambda_c;
37
38			// Compute the UDUt decomposition of data.M
39		199	cholesky::decompose(model, data);
40
41			// Compute the dynamic drift (control - nle)
42	✓✗	199	data.torque_residual = tau - data.nle;
43		199	cholesky::solve(model, data, data.torque_residual);
44
45	✓✗	199	data.sDUiJt = J.transpose();
46			// Compute U^-1 * J.T
47		199	cholesky::Uiv(model, data, data.sDUiJt);
48	✓✓	6553	for(Eigen::DenseIndex k=0;k<model.nv;++k)
49	✓✗✓✗	6354	data.sDUiJt.row(k) /= sqrt(data.D[k]);
50
51	✓✗✓✗ ✓✗	199	data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt;
52
53	✓✗✓✗	199	data.JMinvJt.diagonal().array() += inv_damping;
54		199	data.llt_JMinvJt.compute(data.JMinvJt);
55
56			// Compute the Lagrange Multipliers
57	✓✗✓✗ ✓✗	199	lambda_c.noalias() = -J*data.torque_residual;
58		199	lambda_c -= gamma;
59		199	data.llt_JMinvJt.solveInPlace(lambda_c);
60
61			// Compute the joint acceleration
62	✓✗✓✗ ✓✗	199	a.noalias() = J.transpose() * lambda_c;
63		199	cholesky::solve(model, data, a);
64		199	a += data.torque_residual;
65
66		199	return a;
67			}
68
69			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
70			typename ConstraintMatrixType, typename DriftVectorType>
71			inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
72		199	forwardDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
73			DataTpl<Scalar,Options,JointCollectionTpl> & data,
74			const Eigen::MatrixBase<ConfigVectorType> & q,
75			const Eigen::MatrixBase<TangentVectorType1> & v,
76			const Eigen::MatrixBase<TangentVectorType2> & tau,
77			const Eigen::MatrixBase<ConstraintMatrixType> & J,
78			const Eigen::MatrixBase<DriftVectorType> & gamma,
79			const Scalar inv_damping)
80			{
81	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	199	PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq);
82	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	199	PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);
83
84		199	computeAllTerms(model, data, q, v);
85
86		199	return forwardDynamics(model,data,tau,J,gamma,inv_damping);
87			}
88
89			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
90			typename ConfigVectorType, typename ConstraintMatrixType, typename KKTMatrixType>
91		1	void computeKKTContactDynamicMatrixInverse(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
92			DataTpl<Scalar,Options,JointCollectionTpl> & data,
93			const Eigen::MatrixBase<ConfigVectorType> & q,
94			const Eigen::MatrixBase<ConstraintMatrixType> & J,
95			const Eigen::MatrixBase<KKTMatrixType> & KKTMatrix_inv,
96			const Scalar & inv_damping)
97			{
98	✗✓	1	assert(model.check(data));
99	✗✓✗✗	1	PINOCCHIO_CHECK_INPUT_ARGUMENT(inv_damping >= 0., "mu must be positive.");
100
101			// Compute the mass matrix.
102		1	crbaMinimal(model,data,q);
103
104			// Compute the UDUt decomposition of data.M.
105		1	cholesky::decompose(model, data);
106
107			using std::sqrt;
108	✓✗	1	data.sDUiJt = J.transpose();
109			// Compute U^-1 * J.T
110		1	cholesky::Uiv(model, data, data.sDUiJt);
111	✓✓	33	for(Eigen::DenseIndex k=0;k<model.nv;++k)
112	✓✗✓✗	32	data.sDUiJt.row(k) /= sqrt(data.D[k]);
113
114	✓✗✓✗ ✓✗	1	data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt;
115
116	✓✗✓✗	1	data.JMinvJt.diagonal().array() += inv_damping;
117		1	data.llt_JMinvJt.compute(data.JMinvJt);
118
119		1	getKKTContactDynamicMatrixInverse(model,data,J,KKTMatrix_inv.const_cast_derived());
120		1	}
121
122			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
123			typename ConstraintMatrixType, typename KKTMatrixType>
124		3	inline void getKKTContactDynamicMatrixInverse(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
125			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
126			const Eigen::MatrixBase<ConstraintMatrixType> & J,
127			const Eigen::MatrixBase<KKTMatrixType> & KKTMatrix_inv)
128			{
129	✓✗✗✓	3	assert(model.check(data));
130	✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗	3	PINOCCHIO_CHECK_ARGUMENT_SIZE(KKTMatrix_inv.cols(), data.JMinvJt.cols() + model.nv);
131	✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗	3	PINOCCHIO_CHECK_ARGUMENT_SIZE(KKTMatrix_inv.rows(), data.JMinvJt.rows() + model.nv);
132
133			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
134	✓✗	3	const Eigen::DenseIndex nc = data.JMinvJt.cols();
135
136		3	KKTMatrixType & KKTMatrix_inv_ = PINOCCHIO_EIGEN_CONST_CAST(KKTMatrixType,KKTMatrix_inv);
137
138			typedef Eigen::Block<KKTMatrixType> BlockType;
139	✓✗	3	BlockType topLeft = KKTMatrix_inv_.topLeftCorner(model.nv, model.nv);
140	✓✗	3	BlockType topRight = KKTMatrix_inv_.topRightCorner(model.nv, nc);
141	✓✗	3	BlockType bottomLeft = KKTMatrix_inv_.bottomLeftCorner(nc, model.nv);
142	✓✗	3	BlockType bottomRight = KKTMatrix_inv_.bottomRightCorner(nc, nc);
143
144	✓✗✓✗ ✓✗✓✗	3	bottomRight = -Data::MatrixXs::Identity(nc,nc); data.llt_JMinvJt.solveInPlace(bottomRight);
145	✓✗✓✗	3	topLeft.setIdentity(); cholesky::solve(model, data, topLeft);
146
147	✓✗✓✗ ✓✗	3	bottomLeft.noalias() = J*topLeft;
148	✓✗✓✗ ✓✗✓✗ ✓✗	3	topRight.noalias() = bottomLeft.transpose() * (-bottomRight);
149	✓✗✓✗ ✓✗	3	topLeft.noalias() -= topRight*bottomLeft;
150	✓✗✓✗	3	bottomLeft = topRight.transpose();
151		3	}
152
153			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ConstraintMatrixType>
154			inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
155		2	impulseDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
156			DataTpl<Scalar,Options,JointCollectionTpl> & data,
157			const Eigen::MatrixBase<ConfigVectorType> & q,
158			const Eigen::MatrixBase<TangentVectorType> & v_before,
159			const Eigen::MatrixBase<ConstraintMatrixType> & J,
160			const Scalar r_coeff,
161			const Scalar inv_damping)
162			{
163	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	2	PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq);
164
165			// Compute the mass matrix
166		2	crba(model, data, q);
167
168		2	return impulseDynamics(model,data,v_before,J,r_coeff,inv_damping);
169			}
170
171			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename TangentVectorType, typename ConstraintMatrixType>
172			inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
173		2	impulseDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
174			DataTpl<Scalar,Options,JointCollectionTpl> & data,
175			const Eigen::MatrixBase<TangentVectorType> & v_before,
176			const Eigen::MatrixBase<ConstraintMatrixType> & J,
177			const Scalar r_coeff,
178			const Scalar inv_damping)
179			{
180	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	2	PINOCCHIO_CHECK_ARGUMENT_SIZE(v_before.size(), model.nv);
181	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	2	PINOCCHIO_CHECK_ARGUMENT_SIZE(J.cols(), model.nv);
182	✓✗	2	assert(model.check(data) && "data is not consistent with model.");
183
184			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
185
186		2	typename Data::VectorXs & impulse_c = data.impulse_c;
187		2	typename Data::TangentVectorType & dq_after = data.dq_after;
188
189			// Compute the UDUt decomposition of data.M
190		2	cholesky::decompose(model, data);
191
192	✓✗	2	data.sDUiJt = J.transpose();
193			// Compute U^-1 * J.T
194		2	cholesky::Uiv(model, data, data.sDUiJt);
195	✓✓✓✗ ✓✗	66	for(int k=0;k<model.nv;++k) data.sDUiJt.row(k) /= sqrt(data.D[k]);
196
197	✓✗✓✗ ✓✗	2	data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt;
198
199	✓✗✓✗	2	data.JMinvJt.diagonal().array() += inv_damping;
200		2	data.llt_JMinvJt.compute(data.JMinvJt);
201
202			// Compute the Lagrange Multipliers related to the contact impulses
203	✓✗✓✗ ✓✗	2	impulse_c.noalias() = (-r_coeff - 1.) * (J * v_before);
204		2	data.llt_JMinvJt.solveInPlace(impulse_c);
205
206			// Compute the joint velocity after impacts
207	✓✗✓✗ ✓✗	2	dq_after.noalias() = J.transpose() * impulse_c;
208		2	cholesky::solve(model, data, dq_after);
209		2	dq_after += v_before;
210
211		2	return dq_after;
212			}
213			} // namespace pinocchio
214
215			#endif // ifndef __pinocchio_contact_dynamics_hxx__