GCC Code Coverage Report

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