GCC Code Coverage Report

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