Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/multibody/contacts/contact-3d.hxx	Lines:	116	123	94.3 %
Date:	2024-02-13 11:12:33	Branches:	181	365	49.6 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh,
//                          Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

namespace crocoddyl {

template <typename Scalar>
ContactModel3DTpl<Scalar>::ContactModel3DTpl(
    boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
    const Vector3s& xref, const pinocchio::ReferenceFrame type,
    const std::size_t nu, const Vector2s& gains)
    : Base(state, type, 3, nu), xref_(xref), gains_(gains) {
  id_ = id;
}

template <typename Scalar>
ContactModel3DTpl<Scalar>::ContactModel3DTpl(
    boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
    const Vector3s& xref, const pinocchio::ReferenceFrame type,
    const Vector2s& gains)
    : Base(state, type, 3), xref_(xref), gains_(gains) {
  id_ = id;
}

#pragma GCC diagnostic push  // TODO: Remove once the deprecated FrameXX has
                             // been removed in a future release
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"

template <typename Scalar>
ContactModel3DTpl<Scalar>::ContactModel3DTpl(
    boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
    const Vector3s& xref, const std::size_t nu, const Vector2s& gains)
    : Base(state, pinocchio::ReferenceFrame::LOCAL, 3, nu),
      xref_(xref),
      gains_(gains) {
  id_ = id;
  std::cerr << "Deprecated: Use constructor that passes the type of contact, "
               "this assumes is pinocchio::LOCAL."
            << std::endl;
}

template <typename Scalar>
ContactModel3DTpl<Scalar>::ContactModel3DTpl(
    boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
    const Vector3s& xref, const Vector2s& gains)
    : Base(state, pinocchio::ReferenceFrame::LOCAL, 3),
      xref_(xref),
      gains_(gains) {
  id_ = id;
  std::cerr << "Deprecated: Use constructor that passes the type of contact, "
               "this assumes is pinocchio::LOCAL."
            << std::endl;
}

#pragma GCC diagnostic pop

template <typename Scalar>
ContactModel3DTpl<Scalar>::~ContactModel3DTpl() {}

template <typename Scalar>
void ContactModel3DTpl<Scalar>::calc(
    const boost::shared_ptr<ContactDataAbstract>& data,
    const Eigen::Ref<const VectorXs>&) {
  Data* d = static_cast<Data*>(data.get());
  pinocchio::updateFramePlacement(*state_->get_pinocchio().get(), *d->pinocchio,
                                  id_);
  pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio,
                              id_, pinocchio::LOCAL, d->fJf);
  d->v = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(),
                                     *d->pinocchio, id_);
  d->a0_local =
      pinocchio::getFrameClassicalAcceleration(
          *state_->get_pinocchio().get(), *d->pinocchio, id_, pinocchio::LOCAL)
          .linear();

  const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
  if (gains_[0] != 0.) {
    d->dp = d->pinocchio->oMf[id_].translation() - xref_;
    d->dp_local.noalias() = oRf.transpose() * d->dp;
    d->a0_local += gains_[0] * d->dp_local;
  }
  if (gains_[1] != 0.) {
    d->a0_local += gains_[1] * d->v.linear();
  }
  switch (type_) {
    case pinocchio::ReferenceFrame::LOCAL:
      d->Jc = d->fJf.template topRows<3>();
      d->a0 = d->a0_local;
      break;
    case pinocchio::ReferenceFrame::WORLD:
    case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
      d->Jc.noalias() = oRf * d->fJf.template topRows<3>();
      d->a0.noalias() = oRf * d->a0_local;
      break;
  }
}

template <typename Scalar>
void ContactModel3DTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ContactDataAbstract>& data,
    const Eigen::Ref<const VectorXs>&) {
  Data* d = static_cast<Data*>(data.get());
  const pinocchio::JointIndex joint =
      state_->get_pinocchio()->frames[d->frame].parent;
  pinocchio::getJointAccelerationDerivatives(
      *state_->get_pinocchio().get(), *d->pinocchio, joint, pinocchio::LOCAL,
      d->v_partial_dq, d->a_partial_dq, d->a_partial_dv, d->a_partial_da);
  const std::size_t nv = state_->get_nv();
  pinocchio::skew(d->v.linear(), d->vv_skew);
  pinocchio::skew(d->v.angular(), d->vw_skew);
  d->fXjdv_dq.noalias() = d->fXj * d->v_partial_dq;
  d->fXjda_dq.noalias() = d->fXj * d->a_partial_dq;
  d->fXjda_dv.noalias() = d->fXj * d->a_partial_dv;
  d->da0_local_dx.leftCols(nv) = d->fXjda_dq.template topRows<3>();
  d->da0_local_dx.leftCols(nv).noalias() +=
      d->vw_skew * d->fXjdv_dq.template topRows<3>();
  d->da0_local_dx.leftCols(nv).noalias() -=
      d->vv_skew * d->fXjdv_dq.template bottomRows<3>();
  d->da0_local_dx.rightCols(nv) = d->fXjda_dv.template topRows<3>();
  d->da0_local_dx.rightCols(nv).noalias() +=
      d->vw_skew * d->fJf.template topRows<3>();
  d->da0_local_dx.rightCols(nv).noalias() -=
      d->vv_skew * d->fJf.template bottomRows<3>();
  const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();

  if (gains_[0] != 0.) {
    pinocchio::skew(d->dp_local, d->dp_skew);
    d->da0_local_dx.leftCols(nv).noalias() +=
        gains_[0] * d->dp_skew * d->fJf.template bottomRows<3>();
    d->da0_local_dx.leftCols(nv).noalias() +=
        gains_[0] * d->fJf.template topRows<3>();
  }
  if (gains_[1] != 0.) {
    d->da0_local_dx.leftCols(nv).noalias() +=
        gains_[1] * d->fXjdv_dq.template topRows<3>();
    d->da0_local_dx.rightCols(nv).noalias() +=
        gains_[1] * d->fJf.template topRows<3>();
  }
  switch (type_) {
    case pinocchio::ReferenceFrame::LOCAL:
      d->da0_dx = d->da0_local_dx;
      break;
    case pinocchio::ReferenceFrame::WORLD:
    case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
      // Recalculate the constrained accelerations after imposing contact
      // constraints. This is necessary for the forward-dynamics case.
      d->a0_local = pinocchio::getFrameClassicalAcceleration(
                        *state_->get_pinocchio().get(), *d->pinocchio, id_,
                        pinocchio::LOCAL)
                        .linear();
      if (gains_[0] != 0.) {
        d->a0_local += gains_[0] * d->dp_local;
      }
      if (gains_[1] != 0.) {
        d->a0_local += gains_[1] * d->v.linear();
      }
      d->a0.noalias() = oRf * d->a0_local;

      pinocchio::skew(d->a0.template head<3>(), d->a0_skew);
      d->a0_world_skew.noalias() = d->a0_skew * oRf;
      d->da0_dx.noalias() = oRf * d->da0_local_dx;
      d->da0_dx.leftCols(nv).noalias() -=
          d->a0_world_skew * d->fJf.template bottomRows<3>();
      break;
  }
}

template <typename Scalar>
void ContactModel3DTpl<Scalar>::updateForce(
    const boost::shared_ptr<ContactDataAbstract>& data, const VectorXs& force) {
  if (force.size() != 3) {
    throw_pretty("Invalid argument: "
                 << "lambda has wrong dimension (it should be 3)");
  }
  Data* d = static_cast<Data*>(data.get());
  data->f.linear() = force;
  data->f.angular().setZero();
  switch (type_) {
    case pinocchio::ReferenceFrame::LOCAL:
      data->fext = d->jMf.act(data->f);
      data->dtau_dq.setZero();
      break;
    case pinocchio::ReferenceFrame::WORLD:
    case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
      const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
      d->f_local.linear().noalias() = oRf.transpose() * force;
      d->f_local.angular().setZero();
      data->fext = data->jMf.act(d->f_local);
      pinocchio::skew(d->f_local.linear(), d->f_skew);
      d->fJf_df.noalias() = d->f_skew * d->fJf.template bottomRows<3>();
      data->dtau_dq.noalias() =
          -d->fJf.template topRows<3>().transpose() * d->fJf_df;
      break;
  }
}

template <typename Scalar>
boost::shared_ptr<ContactDataAbstractTpl<Scalar> >
ContactModel3DTpl<Scalar>::createData(pinocchio::DataTpl<Scalar>* const data) {
  return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
                                      data);
}

template <typename Scalar>
void ContactModel3DTpl<Scalar>::print(std::ostream& os) const {
  os << "ContactModel3D {frame=" << state_->get_pinocchio()->frames[id_].name
     << ", type=" << type_ << "}";
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::Vector3s&
ContactModel3DTpl<Scalar>::get_reference() const {
  return xref_;
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::Vector2s&
ContactModel3DTpl<Scalar>::get_gains() const {
  return gains_;
}

template <typename Scalar>
void ContactModel3DTpl<Scalar>::set_reference(const Vector3s& reference) {
  xref_ = reference;
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2023, 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		1181	ContactModel3DTpl<Scalar>::ContactModel3DTpl(
14			boost::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	✓✗✓✗	1181	: Base(state, type, 3, nu), xref_(xref), gains_(gains) {
18		1181	id_ = id;
19		1181	}
20
21			template <typename Scalar>
22		5	ContactModel3DTpl<Scalar>::ContactModel3DTpl(
23			boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
24			const Vector3s& xref, const pinocchio::ReferenceFrame type,
25			const Vector2s& gains)
26	✓✗✓✗	5	: Base(state, type, 3), xref_(xref), gains_(gains) {
27		5	id_ = id;
28		5	}
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			boost::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			boost::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		2376	ContactModel3DTpl<Scalar>::~ContactModel3DTpl() {}
64
65			template <typename Scalar>
66		80637	void ContactModel3DTpl<Scalar>::calc(
67			const boost::shared_ptr<ContactDataAbstract>& data,
68			const Eigen::Ref<const VectorXs>&) {
69		80637	Data* d = static_cast<Data*>(data.get());
70	✓✗	80637	pinocchio::updateFramePlacement(state_->get_pinocchio().get(), d->pinocchio,
71			id_);
72	✓✗	80637	pinocchio::getFrameJacobian(state_->get_pinocchio().get(), d->pinocchio,
73		80637	id_, pinocchio::LOCAL, d->fJf);
74	✓✗	80637	d->v = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(),
75		80637	*d->pinocchio, id_);
76	✓✗✓✗ ✓✗	80637	d->a0_local =
77			pinocchio::getFrameClassicalAcceleration(
78		80637	state_->get_pinocchio().get(), d->pinocchio, id_, pinocchio::LOCAL)
79			.linear();
80
81	✓✗✓✗	80637	const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
82	✓✗✓✓	80637	if (gains_[0] != 0.) {
83	✓✗✓✗ ✓✗	80573	d->dp = d->pinocchio->oMf[id_].translation() - xref_;
84	✓✗✓✗ ✓✗✓✗	80573	d->dp_local.noalias() = oRf.transpose() * d->dp;
85	✓✗✓✗ ✓✗	80573	d->a0_local += gains_[0] * d->dp_local;
86			}
87	✓✗✓✓	80637	if (gains_[1] != 0.) {
88	✓✗✓✗ ✓✗✓✗	80573	d->a0_local += gains_[1] * d->v.linear();
89			}
90	✓✓✗	80637	switch (type_) {
91		46131	case pinocchio::ReferenceFrame::LOCAL:
92	✓✗✓✗	46131	d->Jc = d->fJf.template topRows<3>();
93	✓✗	46131	d->a0 = d->a0_local;
94		46131	break;
95		34506	case pinocchio::ReferenceFrame::WORLD:
96			case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
97	✓✗✓✗ ✓✗✓✗	34506	d->Jc.noalias() = oRf * d->fJf.template topRows<3>();
98	✓✗✓✗ ✓✗	34506	d->a0.noalias() = oRf * d->a0_local;
99		34506	break;
100			}
101		80637	}
102
103			template <typename Scalar>
104		12667	void ContactModel3DTpl<Scalar>::calcDiff(
105			const boost::shared_ptr<ContactDataAbstract>& data,
106			const Eigen::Ref<const VectorXs>&) {
107		12667	Data* d = static_cast<Data*>(data.get());
108		12667	const pinocchio::JointIndex joint =
109		12667	state_->get_pinocchio()->frames[d->frame].parent;
110	✓✗	12667	pinocchio::getJointAccelerationDerivatives(
111		12667	state_->get_pinocchio().get(), d->pinocchio, joint, pinocchio::LOCAL,
112		12667	d->v_partial_dq, d->a_partial_dq, d->a_partial_dv, d->a_partial_da);
113		12667	const std::size_t nv = state_->get_nv();
114	✓✗✓✗	12667	pinocchio::skew(d->v.linear(), d->vv_skew);
115	✓✗✓✗	12667	pinocchio::skew(d->v.angular(), d->vw_skew);
116	✓✗✓✗ ✓✗	12667	d->fXjdv_dq.noalias() = d->fXj * d->v_partial_dq;
117	✓✗✓✗ ✓✗	12667	d->fXjda_dq.noalias() = d->fXj * d->a_partial_dq;
118	✓✗✓✗ ✓✗	12667	d->fXjda_dv.noalias() = d->fXj * d->a_partial_dv;
119	✓✗✓✗ ✓✗	12667	d->da0_local_dx.leftCols(nv) = d->fXjda_dq.template topRows<3>();
120	✓✗✓✗ ✓✗	12667	d->da0_local_dx.leftCols(nv).noalias() +=
121	✓✗✓✗	12667	d->vw_skew * d->fXjdv_dq.template topRows<3>();
122	✓✗✓✗ ✓✗	12667	d->da0_local_dx.leftCols(nv).noalias() -=
123	✓✗✓✗	12667	d->vv_skew * d->fXjdv_dq.template bottomRows<3>();
124	✓✗✓✗ ✓✗	12667	d->da0_local_dx.rightCols(nv) = d->fXjda_dv.template topRows<3>();
125	✓✗✓✗ ✓✗	12667	d->da0_local_dx.rightCols(nv).noalias() +=
126	✓✗✓✗	12667	d->vw_skew * d->fJf.template topRows<3>();
127	✓✗✓✗ ✓✗	12667	d->da0_local_dx.rightCols(nv).noalias() -=
128	✓✗✓✗	12667	d->vv_skew * d->fJf.template bottomRows<3>();
129	✓✗✓✗	12667	const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
130
131	✓✗✓✗	12667	if (gains_[0] != 0.) {
132	✓✗	12667	pinocchio::skew(d->dp_local, d->dp_skew);
133	✓✗✓✗ ✓✗✓✗	12667	d->da0_local_dx.leftCols(nv).noalias() +=
134	✓✗✓✗ ✓✗	12667	gains_[0] * d->dp_skew * d->fJf.template bottomRows<3>();
135	✓✗✓✗ ✓✗	12667	d->da0_local_dx.leftCols(nv).noalias() +=
136	✓✗✓✗ ✓✗	12667	gains_[0] * d->fJf.template topRows<3>();
137			}
138	✓✗✓✗	12667	if (gains_[1] != 0.) {
139	✓✗✓✗ ✓✗	12667	d->da0_local_dx.leftCols(nv).noalias() +=
140	✓✗✓✗ ✓✗	12667	gains_[1] * d->fXjdv_dq.template topRows<3>();
141	✓✗✓✗ ✓✗	12667	d->da0_local_dx.rightCols(nv).noalias() +=
142	✓✗✓✗ ✓✗	12667	gains_[1] * d->fJf.template topRows<3>();
143			}
144	✓✓✗	12667	switch (type_) {
145		7593	case pinocchio::ReferenceFrame::LOCAL:
146	✓✗	7593	d->da0_dx = d->da0_local_dx;
147		7593	break;
148		5074	case pinocchio::ReferenceFrame::WORLD:
149			case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
150			// Recalculate the constrained accelerations after imposing contact
151			// constraints. This is necessary for the forward-dynamics case.
152	✓✗✓✗ ✓✗	5074	d->a0_local = pinocchio::getFrameClassicalAcceleration(
153		5074	state_->get_pinocchio().get(), d->pinocchio, id_,
154			pinocchio::LOCAL)
155			.linear();
156	✓✗✓✗	5074	if (gains_[0] != 0.) {
157	✓✗✓✗ ✓✗	5074	d->a0_local += gains_[0] * d->dp_local;
158			}
159	✓✗✓✗	5074	if (gains_[1] != 0.) {
160	✓✗✓✗ ✓✗✓✗	5074	d->a0_local += gains_[1] * d->v.linear();
161			}
162	✓✗✓✗ ✓✗	5074	d->a0.noalias() = oRf * d->a0_local;
163
164	✓✗✓✗	5074	pinocchio::skew(d->a0.template head<3>(), d->a0_skew);
165	✓✗✓✗ ✓✗	5074	d->a0_world_skew.noalias() = d->a0_skew * oRf;
166	✓✗✓✗ ✓✗	5074	d->da0_dx.noalias() = oRf * d->da0_local_dx;
167	✓✗✓✗ ✓✗	5074	d->da0_dx.leftCols(nv).noalias() -=
168	✓✗✓✗	5074	d->a0_world_skew * d->fJf.template bottomRows<3>();
169		5074	break;
170			}
171		12667	}
172
173			template <typename Scalar>
174		73534	void ContactModel3DTpl<Scalar>::updateForce(
175			const boost::shared_ptr<ContactDataAbstract>& data, const VectorXs& force) {
176	✗✓	73534	if (force.size() != 3) {
177			throw_pretty("Invalid argument: "
178			<< "lambda has wrong dimension (it should be 3)");
179			}
180		73534	Data* d = static_cast<Data*>(data.get());
181	✓✗	73534	data->f.linear() = force;
182	✓✗	73534	data->f.angular().setZero();
183	✓✓✗	73534	switch (type_) {
184		42042	case pinocchio::ReferenceFrame::LOCAL:
185	✓✗✓✗	42042	data->fext = d->jMf.act(data->f);
186	✓✗	42042	data->dtau_dq.setZero();
187		42042	break;
188		31492	case pinocchio::ReferenceFrame::WORLD:
189			case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
190	✓✗✓✗	31492	const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
191	✓✗✓✗ ✓✗✓✗ ✓✗	31492	d->f_local.linear().noalias() = oRf.transpose() * force;
192	✓✗✓✗	31492	d->f_local.angular().setZero();
193	✓✗✓✗	31492	data->fext = data->jMf.act(d->f_local);
194	✓✗✓✗	31492	pinocchio::skew(d->f_local.linear(), d->f_skew);
195	✓✗✓✗ ✓✗✓✗	31492	d->fJf_df.noalias() = d->f_skew * d->fJf.template bottomRows<3>();
196	✓✗✓✗ ✓✗✓✗ ✓✗	31492	data->dtau_dq.noalias() =
197	✓✗	31492	-d->fJf.template topRows<3>().transpose() * d->fJf_df;
198		31492	break;
199			}
200		73534	}
201
202			template <typename Scalar>
203			boost::shared_ptr<ContactDataAbstractTpl<Scalar> >
204		92587	ContactModel3DTpl<Scalar>::createData(pinocchio::DataTpl<Scalar>* const data) {
205			return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
206	✓✗	92587	data);
207			}
208
209			template <typename Scalar>
210		15	void ContactModel3DTpl<Scalar>::print(std::ostream& os) const {
211		15	os << "ContactModel3D {frame=" << state_->get_pinocchio()->frames[id_].name
212		30	<< ", type=" << type_ << "}";
213		15	}
214
215			template <typename Scalar>
216			const typename MathBaseTpl<Scalar>::Vector3s&
217			ContactModel3DTpl<Scalar>::get_reference() const {
218			return xref_;
219			}
220
221			template <typename Scalar>
222			const typename MathBaseTpl<Scalar>::Vector2s&
223			ContactModel3DTpl<Scalar>::get_gains() const {
224			return gains_;
225			}
226
227			template <typename Scalar>
228			void ContactModel3DTpl<Scalar>::set_reference(const Vector3s& reference) {
229			xref_ = reference;
230			}
231
232			} // namespace crocoddyl