Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/multibody/contacts/contact-6d.hxx	Lines:	103	110	93.6 %
Date:	2024-02-13 11:12:33	Branches:	108	219	49.3 %



///////////////////////////////////////////////////////////////////////////////
// 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.
///////////////////////////////////////////////////////////////////////////////

#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/kinematics-derivatives.hpp>

#include "crocoddyl/core/utils/exception.hpp"
#include "crocoddyl/multibody/contacts/contact-6d.hpp"

namespace crocoddyl {

template <typename Scalar>
ContactModel6DTpl<Scalar>::ContactModel6DTpl(
    boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
    const SE3& pref, const pinocchio::ReferenceFrame type, const std::size_t nu,
    const Vector2s& gains)
    : Base(state, type, 6, nu), pref_(pref), gains_(gains) {
  id_ = id;
}

template <typename Scalar>
ContactModel6DTpl<Scalar>::ContactModel6DTpl(
    boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
    const SE3& pref, const pinocchio::ReferenceFrame type,
    const Vector2s& gains)
    : Base(state, type, 6), pref_(pref), 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>
ContactModel6DTpl<Scalar>::ContactModel6DTpl(
    boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
    const SE3& pref, const std::size_t nu, const Vector2s& gains)
    : Base(state, pinocchio::ReferenceFrame::LOCAL, 6, nu),
      pref_(pref),
      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>
ContactModel6DTpl<Scalar>::ContactModel6DTpl(
    boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
    const SE3& pref, const Vector2s& gains)
    : Base(state, pinocchio::ReferenceFrame::LOCAL, 6),
      pref_(pref),
      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>
ContactModel6DTpl<Scalar>::~ContactModel6DTpl() {}

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

  if (gains_[0] != 0.) {
    d->rMf = pref_.actInv(d->pinocchio->oMf[id_]);
    d->a0_local += gains_[0] * pinocchio::log6(d->rMf);
  }
  if (gains_[1] != 0.) {
    d->v = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(),
                                       *d->pinocchio, id_);
    d->a0_local += gains_[1] * d->v;
  }
  switch (type_) {
    case pinocchio::ReferenceFrame::LOCAL:
      data->Jc = d->fJf;
      data->a0 = d->a0_local.toVector();
      break;
    case pinocchio::ReferenceFrame::WORLD:
    case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
      d->lwaMl.rotation(d->pinocchio->oMf[id_].rotation());
      data->Jc.noalias() = d->lwaMl.toActionMatrix() * d->fJf;
      data->a0.noalias() = d->lwaMl.act(d->a0_local).toVector();
      break;
  }
}

template <typename Scalar>
void ContactModel6DTpl<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();
  d->da0_local_dx.leftCols(nv).noalias() = d->fXj * d->a_partial_dq;
  d->da0_local_dx.rightCols(nv).noalias() = d->fXj * d->a_partial_dv;

  if (gains_[0] != 0.) {
    pinocchio::Jlog6(d->rMf, d->rMf_Jlog6);
    d->da0_local_dx.leftCols(nv).noalias() += gains_[0] * d->rMf_Jlog6 * d->fJf;
  }
  if (gains_[1] != 0.) {
    d->da0_local_dx.leftCols(nv).noalias() +=
        gains_[1] * d->fXj * d->v_partial_dq;
    d->da0_local_dx.rightCols(nv).noalias() += gains_[1] * d->fJf;
  }
  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::getFrameAcceleration(
          *state_->get_pinocchio().get(), *d->pinocchio, id_);
      if (gains_[0] != 0.) {
        d->a0_local += gains_[0] * pinocchio::log6(d->rMf);
      }
      if (gains_[1] != 0.) {
        d->a0_local += gains_[1] * d->v;
      }
      data->a0.noalias() = d->lwaMl.act(d->a0_local).toVector();

      const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
      pinocchio::skew(d->a0.template head<3>(), d->av_skew);
      pinocchio::skew(d->a0.template tail<3>(), d->aw_skew);
      d->av_world_skew.noalias() = d->av_skew * oRf;
      d->aw_world_skew.noalias() = d->aw_skew * oRf;
      d->da0_dx.noalias() = d->lwaMl.toActionMatrix() * d->da0_local_dx;
      d->da0_dx.leftCols(nv).template topRows<3>().noalias() -=
          d->av_world_skew * d->fJf.template bottomRows<3>();
      d->da0_dx.leftCols(nv).template bottomRows<3>().noalias() -=
          d->aw_world_skew * d->fJf.template bottomRows<3>();
      break;
  }
}

template <typename Scalar>
void ContactModel6DTpl<Scalar>::updateForce(
    const boost::shared_ptr<ContactDataAbstract>& data, const VectorXs& force) {
  if (force.size() != 6) {
    throw_pretty("Invalid argument: "
                 << "lambda has wrong dimension (it should be 6)");
  }
  Data* d = static_cast<Data*>(data.get());
  data->f = pinocchio::ForceTpl<Scalar>(force);
  switch (type_) {
    case pinocchio::ReferenceFrame::LOCAL:
      data->fext = data->jMf.act(data->f);
      data->dtau_dq.setZero();
      break;
    case pinocchio::ReferenceFrame::WORLD:
    case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
      d->f_local = d->lwaMl.actInv(data->f);
      data->fext = data->jMf.act(d->f_local);
      pinocchio::skew(d->f_local.linear(), d->fv_skew);
      pinocchio::skew(d->f_local.angular(), d->fw_skew);
      d->fJf_df.template topRows<3>().noalias() =
          d->fv_skew * d->fJf.template bottomRows<3>();
      d->fJf_df.template bottomRows<3>().noalias() =
          d->fw_skew * d->fJf.template bottomRows<3>();
      d->dtau_dq.noalias() = -d->fJf.transpose() * d->fJf_df;
      break;
  }
}

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

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

template <typename Scalar>
const pinocchio::SE3Tpl<Scalar>& ContactModel6DTpl<Scalar>::get_reference()
    const {
  return pref_;
}

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

template <typename Scalar>
void ContactModel6DTpl<Scalar>::set_reference(const SE3& reference) {
  pref_ = reference;
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

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