Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/multibody/impulses/impulse-3d.hxx	Lines:	70	71	98.6 %
Date:	2024-02-13 11:12:33	Branches:	67	141	47.5 %


///////////////////////////////////////////////////////////////////////////////
// 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/impulses/impulse-3d.hpp"

namespace crocoddyl {

template <typename Scalar>
ImpulseModel3DTpl<Scalar>::ImpulseModel3DTpl(
    boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
    const pinocchio::ReferenceFrame type)
    : Base(state, type, 3) {
  id_ = id;
}

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

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

  switch (type_) {
    case pinocchio::ReferenceFrame::LOCAL:
      data->Jc = d->fJf.template topRows<3>();
      break;
    case pinocchio::ReferenceFrame::WORLD:
    case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
      data->Jc.noalias() =
          d->pinocchio->oMf[id_].rotation() * d->fJf.template topRows<3>();
      break;
  }
}

template <typename Scalar>
void ImpulseModel3DTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ImpulseDataAbstract>& data,
    const Eigen::Ref<const VectorXs>&) {
  boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
  const pinocchio::JointIndex joint =
      state_->get_pinocchio()->frames[d->frame].parent;
  pinocchio::getJointVelocityDerivatives(*state_->get_pinocchio().get(),
                                         *d->pinocchio, joint, pinocchio::LOCAL,
                                         d->v_partial_dq, d->v_partial_dv);
  d->dv0_local_dq.noalias() = d->fXj.template topRows<3>() * d->v_partial_dq;

  switch (type_) {
    case pinocchio::ReferenceFrame::LOCAL:
      data->dv0_dq = d->dv0_local_dq;
      break;
    case pinocchio::ReferenceFrame::WORLD:
    case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
      const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
      d->v0 = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(),
                                          *d->pinocchio, id_,
                                          pinocchio::LOCAL_WORLD_ALIGNED)
                  .linear();
      pinocchio::skew(d->v0, d->v0_skew);
      d->v0_world_skew.noalias() = d->v0_skew * oRf;
      data->dv0_dq.noalias() = oRf * d->dv0_local_dq;
      data->dv0_dq.noalias() -=
          d->v0_world_skew * d->fJf.template bottomRows<3>();
      break;
  }
}

template <typename Scalar>
void ImpulseModel3DTpl<Scalar>::updateForce(
    const boost::shared_ptr<ImpulseDataAbstract>& 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<ImpulseDataAbstractTpl<Scalar> >
ImpulseModel3DTpl<Scalar>::createData(pinocchio::DataTpl<Scalar>* const data) {
  return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
                                      data);
}

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

}  // 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			#include <pinocchio/algorithm/frames.hpp>
11			#include <pinocchio/algorithm/kinematics-derivatives.hpp>
12
13			#include "crocoddyl/core/utils/exception.hpp"
14			#include "crocoddyl/multibody/impulses/impulse-3d.hpp"
15
16			namespace crocoddyl {
17
18			template <typename Scalar>
19		209	ImpulseModel3DTpl<Scalar>::ImpulseModel3DTpl(
20			boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
21			const pinocchio::ReferenceFrame type)
22		209	: Base(state, type, 3) {
23		209	id_ = id;
24		209	}
25
26			template <typename Scalar>
27		422	ImpulseModel3DTpl<Scalar>::~ImpulseModel3DTpl() {}
28
29			template <typename Scalar>
30		5043	void ImpulseModel3DTpl<Scalar>::calc(
31			const boost::shared_ptr<ImpulseDataAbstract>& data,
32			const Eigen::Ref<const VectorXs>&) {
33		10086	boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
34	✓✗	5043	pinocchio::updateFramePlacement<Scalar>(*state_->get_pinocchio().get(),
35		5043	*d->pinocchio, id_);
36	✓✗	5043	pinocchio::getFrameJacobian(state_->get_pinocchio().get(), d->pinocchio,
37		5043	id_, pinocchio::LOCAL, d->fJf);
38
39	✓✓✗	5043	switch (type_) {
40		2681	case pinocchio::ReferenceFrame::LOCAL:
41	✓✗✓✗	2681	data->Jc = d->fJf.template topRows<3>();
42		2681	break;
43		2362	case pinocchio::ReferenceFrame::WORLD:
44			case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
45	✓✗✓✗	2362	data->Jc.noalias() =
46	✓✗✓✗ ✓✗	2362	d->pinocchio->oMf[id_].rotation() * d->fJf.template topRows<3>();
47		2362	break;
48			}
49		5043	}
50
51			template <typename Scalar>
52		450	void ImpulseModel3DTpl<Scalar>::calcDiff(
53			const boost::shared_ptr<ImpulseDataAbstract>& data,
54			const Eigen::Ref<const VectorXs>&) {
55		900	boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
56		450	const pinocchio::JointIndex joint =
57		450	state_->get_pinocchio()->frames[d->frame].parent;
58	✓✗	450	pinocchio::getJointVelocityDerivatives(*state_->get_pinocchio().get(),
59		450	*d->pinocchio, joint, pinocchio::LOCAL,
60		450	d->v_partial_dq, d->v_partial_dv);
61	✓✗✓✗ ✓✗✓✗	450	d->dv0_local_dq.noalias() = d->fXj.template topRows<3>() * d->v_partial_dq;
62
63	✓✓✗	450	switch (type_) {
64		238	case pinocchio::ReferenceFrame::LOCAL:
65	✓✗	238	data->dv0_dq = d->dv0_local_dq;
66		238	break;
67		212	case pinocchio::ReferenceFrame::WORLD:
68			case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
69	✓✗✓✗	424	const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
70	✓✗✓✗ ✓✗	212	d->v0 = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(),
71		212	*d->pinocchio, id_,
72			pinocchio::LOCAL_WORLD_ALIGNED)
73			.linear();
74	✓✗	212	pinocchio::skew(d->v0, d->v0_skew);
75	✓✗✓✗ ✓✗	212	d->v0_world_skew.noalias() = d->v0_skew * oRf;
76	✓✗✓✗ ✓✗	212	data->dv0_dq.noalias() = oRf * d->dv0_local_dq;
77	✓✗✓✗	212	data->dv0_dq.noalias() -=
78	✓✗✓✗	212	d->v0_world_skew * d->fJf.template bottomRows<3>();
79		212	break;
80			}
81		450	}
82
83			template <typename Scalar>
84		4998	void ImpulseModel3DTpl<Scalar>::updateForce(
85			const boost::shared_ptr<ImpulseDataAbstract>& data, const VectorXs& force) {
86	✗✓	4998	if (force.size() != 3) {
87			throw_pretty("Invalid argument: "
88			<< "lambda has wrong dimension (it should be 3)");
89			}
90		4998	Data* d = static_cast<Data*>(data.get());
91	✓✗	4998	data->f.linear() = force;
92	✓✗	4998	data->f.angular().setZero();
93	✓✓✗	4998	switch (type_) {
94		2650	case pinocchio::ReferenceFrame::LOCAL:
95	✓✗✓✗	2650	data->fext = d->jMf.act(data->f);
96	✓✗	2650	data->dtau_dq.setZero();
97		2650	break;
98		2348	case pinocchio::ReferenceFrame::WORLD:
99			case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
100	✓✗✓✗	4696	const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
101	✓✗✓✗ ✓✗✓✗ ✓✗	2348	d->f_local.linear().noalias() = oRf.transpose() * force;
102	✓✗✓✗	2348	d->f_local.angular().setZero();
103	✓✗✓✗	2348	data->fext = data->jMf.act(d->f_local);
104	✓✗✓✗	2348	pinocchio::skew(d->f_local.linear(), d->f_skew);
105	✓✗✓✗ ✓✗✓✗	2348	d->fJf_df.noalias() = d->f_skew * d->fJf.template bottomRows<3>();
106	✓✗✓✗ ✓✗✓✗ ✓✗	2348	data->dtau_dq.noalias() =
107	✓✗	2348	-d->fJf.template topRows<3>().transpose() * d->fJf_df;
108		2348	break;
109			}
110		4998	}
111
112			template <typename Scalar>
113			boost::shared_ptr<ImpulseDataAbstractTpl<Scalar> >
114		3500	ImpulseModel3DTpl<Scalar>::createData(pinocchio::DataTpl<Scalar>* const data) {
115			return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
116	✓✗	3500	data);
117			}
118
119			template <typename Scalar>
120		15	void ImpulseModel3DTpl<Scalar>::print(std::ostream& os) const {
121		15	os << "ImpulseModel3D {frame=" << state_->get_pinocchio()->frames[id_].name
122		30	<< ", type=" << type_ << "}";
123		15	}
124
125			} // namespace crocoddyl