GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5			// Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			namespace crocoddyl {
11
12			template <typename Scalar>
13		✗	ContactModel3DTpl<Scalar>::ContactModel3DTpl(
14			std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
15			const Vector3s& xref, const pinocchio::ReferenceFrame type,
16			const std::size_t nu, const Vector2s& gains)
17		✗	: Base(state, type, 3, nu), xref_(xref), gains_(gains) {
18		✗	id_ = id;
19		✗	}
20
21			template <typename Scalar>
22		✗	ContactModel3DTpl<Scalar>::ContactModel3DTpl(
23			std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
24			const Vector3s& xref, const pinocchio::ReferenceFrame type,
25			const Vector2s& gains)
26		✗	: Base(state, type, 3), xref_(xref), gains_(gains) {
27		✗	id_ = id;
28		✗	}
29
30			#pragma GCC diagnostic push // TODO: Remove once the deprecated FrameXX has
31			// been removed in a future release
32			#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
33
34			template <typename Scalar>
35		✗	ContactModel3DTpl<Scalar>::ContactModel3DTpl(
36			std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
37			const Vector3s& xref, const std::size_t nu, const Vector2s& gains)
38			: Base(state, pinocchio::ReferenceFrame::LOCAL, 3, nu),
39		✗	xref_(xref),
40		✗	gains_(gains) {
41		✗	id_ = id;
42		✗	std::cerr << "Deprecated: Use constructor that passes the type of contact, "
43			"this assumes is pinocchio::LOCAL."
44		✗	<< std::endl;
45		✗	}
46
47			template <typename Scalar>
48		✗	ContactModel3DTpl<Scalar>::ContactModel3DTpl(
49			std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
50			const Vector3s& xref, const Vector2s& gains)
51			: Base(state, pinocchio::ReferenceFrame::LOCAL, 3),
52		✗	xref_(xref),
53		✗	gains_(gains) {
54		✗	id_ = id;
55		✗	std::cerr << "Deprecated: Use constructor that passes the type of contact, "
56			"this assumes is pinocchio::LOCAL."
57		✗	<< std::endl;
58		✗	}
59
60			#pragma GCC diagnostic pop
61
62			template <typename Scalar>
63		✗	void ContactModel3DTpl<Scalar>::calc(
64			const std::shared_ptr<ContactDataAbstract>& data,
65			const Eigen::Ref<const VectorXs>&) {
66		✗	Data* d = static_cast<Data*>(data.get());
67		✗	pinocchio::updateFramePlacement(*state_->get_pinocchio().get(), *d->pinocchio,
68			id_);
69		✗	pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio,
70		✗	id_, pinocchio::LOCAL, d->fJf);
71		✗	d->v = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(),
72		✗	*d->pinocchio, id_);
73		✗	d->a0_local =
74			pinocchio::getFrameClassicalAcceleration(
75		✗	*state_->get_pinocchio().get(), *d->pinocchio, id_, pinocchio::LOCAL)
76		✗	.linear();
77
78		✗	const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
79		✗	if (gains_[0] != 0.) {
80		✗	d->dp = d->pinocchio->oMf[id_].translation() - xref_;
81		✗	d->dp_local.noalias() = oRf.transpose() * d->dp;
82		✗	d->a0_local += gains_[0] * d->dp_local;
83			}
84		✗	if (gains_[1] != 0.) {
85		✗	d->a0_local += gains_[1] * d->v.linear();
86			}
87		✗	switch (type_) {
88		✗	case pinocchio::ReferenceFrame::LOCAL:
89		✗	d->Jc = d->fJf.template topRows<3>();
90		✗	d->a0 = d->a0_local;
91		✗	break;
92		✗	case pinocchio::ReferenceFrame::WORLD:
93			case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
94		✗	d->Jc.noalias() = oRf * d->fJf.template topRows<3>();
95		✗	d->a0.noalias() = oRf * d->a0_local;
96		✗	break;
97			}
98		✗	}
99
100			template <typename Scalar>
101		✗	void ContactModel3DTpl<Scalar>::calcDiff(
102			const std::shared_ptr<ContactDataAbstract>& data,
103			const Eigen::Ref<const VectorXs>&) {
104		✗	Data* d = static_cast<Data*>(data.get());
105		✗	const pinocchio::JointIndex joint =
106		✗	state_->get_pinocchio()->frames[d->frame].parentJoint;
107		✗	pinocchio::getJointAccelerationDerivatives(
108		✗	*state_->get_pinocchio().get(), *d->pinocchio, joint, pinocchio::LOCAL,
109		✗	d->v_partial_dq, d->a_partial_dq, d->a_partial_dv, d->a_partial_da);
110		✗	const std::size_t nv = state_->get_nv();
111		✗	pinocchio::skew(d->v.linear(), d->vv_skew);
112		✗	pinocchio::skew(d->v.angular(), d->vw_skew);
113		✗	d->fXjdv_dq.noalias() = d->fXj * d->v_partial_dq;
114		✗	d->fXjda_dq.noalias() = d->fXj * d->a_partial_dq;
115		✗	d->fXjda_dv.noalias() = d->fXj * d->a_partial_dv;
116		✗	d->da0_local_dx.leftCols(nv) = d->fXjda_dq.template topRows<3>();
117		✗	d->da0_local_dx.leftCols(nv).noalias() +=
118		✗	d->vw_skew * d->fXjdv_dq.template topRows<3>();
119		✗	d->da0_local_dx.leftCols(nv).noalias() -=
120		✗	d->vv_skew * d->fXjdv_dq.template bottomRows<3>();
121		✗	d->da0_local_dx.rightCols(nv) = d->fXjda_dv.template topRows<3>();
122		✗	d->da0_local_dx.rightCols(nv).noalias() +=
123		✗	d->vw_skew * d->fJf.template topRows<3>();
124		✗	d->da0_local_dx.rightCols(nv).noalias() -=
125		✗	d->vv_skew * d->fJf.template bottomRows<3>();
126		✗	const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
127
128		✗	if (gains_[0] != 0.) {
129		✗	pinocchio::skew(d->dp_local, d->dp_skew);
130		✗	d->da0_local_dx.leftCols(nv).noalias() +=
131		✗	gains_[0] * d->dp_skew * d->fJf.template bottomRows<3>();
132		✗	d->da0_local_dx.leftCols(nv).noalias() +=
133		✗	gains_[0] * d->fJf.template topRows<3>();
134			}
135		✗	if (gains_[1] != 0.) {
136		✗	d->da0_local_dx.leftCols(nv).noalias() +=
137		✗	gains_[1] * d->fXjdv_dq.template topRows<3>();
138		✗	d->da0_local_dx.rightCols(nv).noalias() +=
139		✗	gains_[1] * d->fJf.template topRows<3>();
140			}
141		✗	switch (type_) {
142		✗	case pinocchio::ReferenceFrame::LOCAL:
143		✗	d->da0_dx = d->da0_local_dx;
144		✗	break;
145		✗	case pinocchio::ReferenceFrame::WORLD:
146			case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
147			// Recalculate the constrained accelerations after imposing contact
148			// constraints. This is necessary for the forward-dynamics case.
149		✗	d->a0_local = pinocchio::getFrameClassicalAcceleration(
150		✗	*state_->get_pinocchio().get(), *d->pinocchio, id_,
151			pinocchio::LOCAL)
152		✗	.linear();
153		✗	if (gains_[0] != 0.) {
154		✗	d->a0_local += gains_[0] * d->dp_local;
155			}
156		✗	if (gains_[1] != 0.) {
157		✗	d->a0_local += gains_[1] * d->v.linear();
158			}
159		✗	d->a0.noalias() = oRf * d->a0_local;
160
161		✗	pinocchio::skew(d->a0.template head<3>(), d->a0_skew);
162		✗	d->a0_world_skew.noalias() = d->a0_skew * oRf;
163		✗	d->da0_dx.noalias() = oRf * d->da0_local_dx;
164		✗	d->da0_dx.leftCols(nv).noalias() -=
165		✗	d->a0_world_skew * d->fJf.template bottomRows<3>();
166		✗	break;
167			}
168		✗	}
169
170			template <typename Scalar>
171		✗	void ContactModel3DTpl<Scalar>::updateForce(
172			const std::shared_ptr<ContactDataAbstract>& data, const VectorXs& force) {
173		✗	if (force.size() != 3) {
174		✗	throw_pretty(
175			"Invalid argument: " << "lambda has wrong dimension (it should be 3)");
176			}
177		✗	Data* d = static_cast<Data*>(data.get());
178		✗	data->f.linear() = force;
179		✗	data->f.angular().setZero();
180		✗	switch (type_) {
181		✗	case pinocchio::ReferenceFrame::LOCAL:
182		✗	data->fext = d->jMf.act(data->f);
183		✗	data->dtau_dq.setZero();
184		✗	break;
185		✗	case pinocchio::ReferenceFrame::WORLD:
186			case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
187		✗	const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
188		✗	d->f_local.linear().noalias() = oRf.transpose() * force;
189		✗	d->f_local.angular().setZero();
190		✗	data->fext = data->jMf.act(d->f_local);
191		✗	pinocchio::skew(d->f_local.linear(), d->f_skew);
192		✗	d->fJf_df.noalias() = d->f_skew * d->fJf.template bottomRows<3>();
193		✗	data->dtau_dq.noalias() =
194		✗	-d->fJf.template topRows<3>().transpose() * d->fJf_df;
195		✗	break;
196			}
197		✗	}
198
199			template <typename Scalar>
200			std::shared_ptr<ContactDataAbstractTpl<Scalar> >
201		✗	ContactModel3DTpl<Scalar>::createData(pinocchio::DataTpl<Scalar>* const data) {
202		✗	return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
203		✗	data);
204			}
205
206			template <typename Scalar>
207			template <typename NewScalar>
208		✗	ContactModel3DTpl<NewScalar> ContactModel3DTpl<Scalar>::cast() const {
209			typedef ContactModel3DTpl<NewScalar> ReturnType;
210			typedef StateMultibodyTpl<NewScalar> StateType;
211		✗	ReturnType ret(
212		✗	std::make_shared<StateType>(state_->template cast<NewScalar>()), id_,
213		✗	xref_.template cast<NewScalar>(), type_, nu_,
214		✗	gains_.template cast<NewScalar>());
215		✗	return ret;
216			}
217
218			template <typename Scalar>
219		✗	void ContactModel3DTpl<Scalar>::print(std::ostream& os) const {
220		✗	os << "ContactModel3D {frame=" << state_->get_pinocchio()->frames[id_].name
221		✗	<< ", type=" << type_ << "}";
222		✗	}
223
224			template <typename Scalar>
225			const typename MathBaseTpl<Scalar>::Vector3s&
226		✗	ContactModel3DTpl<Scalar>::get_reference() const {
227		✗	return xref_;
228			}
229
230			template <typename Scalar>
231			const typename MathBaseTpl<Scalar>::Vector2s&
232		✗	ContactModel3DTpl<Scalar>::get_gains() const {
233		✗	return gains_;
234			}
235
236			template <typename Scalar>
237		✗	void ContactModel3DTpl<Scalar>::set_reference(const Vector3s& reference) {
238		✗	xref_ = reference;
239		✗	}
240
241			} // namespace crocoddyl
242