Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/multibody/impulses/impulse-6d.hxx	Lines:	74	75	98.7 %
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"

namespace crocoddyl {

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

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

template <typename Scalar>
void ImpulseModel6DTpl<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;
      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;
      break;
  }
}

template <typename Scalar>
void ImpulseModel6DTpl<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 * 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_, type_);
      pinocchio::skew(d->v0.linear(), d->vv_skew);
      pinocchio::skew(d->v0.angular(), d->vw_skew);
      d->vv_world_skew.noalias() = d->vv_skew * oRf;
      d->vw_world_skew.noalias() = d->vw_skew * oRf;
      data->dv0_dq.noalias() = d->lwaMl.toActionMatrix() * d->dv0_local_dq;
      d->dv0_dq.template topRows<3>().noalias() -=
          d->vv_world_skew * d->fJf.template bottomRows<3>();
      d->dv0_dq.template bottomRows<3>().noalias() -=
          d->vw_world_skew * d->fJf.template bottomRows<3>();
      break;
  }
}

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

template <typename Scalar>
void ImpulseModel6DTpl<Scalar>::print(std::ostream& os) const {
  os << "ImpulseModel^D {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
15			namespace crocoddyl {
16
17			template <typename Scalar>
18		194	ImpulseModel6DTpl<Scalar>::ImpulseModel6DTpl(
19			boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
20			const pinocchio::ReferenceFrame type)
21		194	: Base(state, type, 6) {
22		194	id_ = id;
23		194	}
24
25			template <typename Scalar>
26		392	ImpulseModel6DTpl<Scalar>::~ImpulseModel6DTpl() {}
27
28			template <typename Scalar>
29		11531	void ImpulseModel6DTpl<Scalar>::calc(
30			const boost::shared_ptr<ImpulseDataAbstract>& data,
31			const Eigen::Ref<const VectorXs>&) {
32		23062	boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
33	✓✗	11531	pinocchio::updateFramePlacement<Scalar>(*state_->get_pinocchio().get(),
34		11531	*d->pinocchio, id_);
35	✓✗	11531	pinocchio::getFrameJacobian(state_->get_pinocchio().get(), d->pinocchio,
36		11531	id_, pinocchio::LOCAL, d->fJf);
37	✓✓✗	11531	switch (type_) {
38		6489	case pinocchio::ReferenceFrame::LOCAL:
39	✓✗	6489	data->Jc = d->fJf;
40		6489	break;
41		5042	case pinocchio::ReferenceFrame::WORLD:
42			case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
43	✓✗✓✗	5042	d->lwaMl.rotation(d->pinocchio->oMf[id_].rotation());
44	✓✗✓✗ ✓✗✓✗	5042	data->Jc.noalias() = d->lwaMl.toActionMatrix() * d->fJf;
45		5042	break;
46			}
47		11531	}
48
49			template <typename Scalar>
50		326	void ImpulseModel6DTpl<Scalar>::calcDiff(
51			const boost::shared_ptr<ImpulseDataAbstract>& data,
52			const Eigen::Ref<const VectorXs>&) {
53		652	boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
54		326	const pinocchio::JointIndex joint =
55		326	state_->get_pinocchio()->frames[d->frame].parent;
56	✓✗	326	pinocchio::getJointVelocityDerivatives(*state_->get_pinocchio().get(),
57		326	*d->pinocchio, joint, pinocchio::LOCAL,
58		326	d->v_partial_dq, d->v_partial_dv);
59	✓✗✓✗ ✓✗	326	d->dv0_local_dq.noalias() = d->fXj * d->v_partial_dq;
60
61	✓✓✗	326	switch (type_) {
62		178	case pinocchio::ReferenceFrame::LOCAL:
63	✓✗	178	data->dv0_dq = d->dv0_local_dq;
64		178	break;
65		148	case pinocchio::ReferenceFrame::WORLD:
66			case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
67	✓✗✓✗	148	const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
68	✓✗	148	d->v0 = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(),
69		148	*d->pinocchio, id_, type_);
70	✓✗✓✗	148	pinocchio::skew(d->v0.linear(), d->vv_skew);
71	✓✗✓✗	148	pinocchio::skew(d->v0.angular(), d->vw_skew);
72	✓✗✓✗ ✓✗	148	d->vv_world_skew.noalias() = d->vv_skew * oRf;
73	✓✗✓✗ ✓✗	148	d->vw_world_skew.noalias() = d->vw_skew * oRf;
74	✓✗✓✗ ✓✗✓✗	148	data->dv0_dq.noalias() = d->lwaMl.toActionMatrix() * d->dv0_local_dq;
75	✓✗✓✗ ✓✗	148	d->dv0_dq.template topRows<3>().noalias() -=
76	✓✗✓✗	148	d->vv_world_skew * d->fJf.template bottomRows<3>();
77	✓✗✓✗ ✓✗	148	d->dv0_dq.template bottomRows<3>().noalias() -=
78	✓✗✓✗	148	d->vw_world_skew * d->fJf.template bottomRows<3>();
79		148	break;
80			}
81		326	}
82
83			template <typename Scalar>
84		11491	void ImpulseModel6DTpl<Scalar>::updateForce(
85			const boost::shared_ptr<ImpulseDataAbstract>& data, const VectorXs& force) {
86	✗✓	11491	if (force.size() != 6) {
87			throw_pretty("Invalid argument: "
88			<< "lambda has wrong dimension (it should be 6)");
89			}
90		11491	Data* d = static_cast<Data*>(data.get());
91	✓✗	11491	data->f = pinocchio::ForceTpl<Scalar>(force);
92	✓✓✗	11491	switch (type_) {
93		6463	case pinocchio::ReferenceFrame::LOCAL:
94	✓✗	6463	data->fext = data->jMf.act(data->f);
95		6463	data->dtau_dq.setZero();
96		6463	break;
97		5028	case pinocchio::ReferenceFrame::WORLD:
98			case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
99	✓✗	5028	d->f_local = d->lwaMl.actInv(data->f);
100	✓✗	5028	data->fext = data->jMf.act(d->f_local);
101	✓✗	5028	pinocchio::skew(d->f_local.linear(), d->fv_skew);
102	✓✗	5028	pinocchio::skew(d->f_local.angular(), d->fw_skew);
103	✓✗✓✗ ✓✗	5028	d->fJf_df.template topRows<3>().noalias() =
104	✓✗	5028	d->fv_skew * d->fJf.template bottomRows<3>();
105	✓✗✓✗ ✓✗	5028	d->fJf_df.template bottomRows<3>().noalias() =
106	✓✗	5028	d->fw_skew * d->fJf.template bottomRows<3>();
107	✓✗✓✗ ✓✗✓✗	5028	d->dtau_dq.noalias() = -d->fJf.transpose() * d->fJf_df;
108		5028	break;
109			}
110		11491	}
111
112			template <typename Scalar>
113			boost::shared_ptr<ImpulseDataAbstractTpl<Scalar> >
114		6936	ImpulseModel6DTpl<Scalar>::createData(pinocchio::DataTpl<Scalar>* const data) {
115			return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
116	✓✗	6936	data);
117			}
118
119			template <typename Scalar>
120		15	void ImpulseModel6DTpl<Scalar>::print(std::ostream& os) const {
121		15	os << "ImpulseModel^D {frame=" << state_->get_pinocchio()->frames[id_].name
122		30	<< ", type=" << type_ << "}";
123		15	}
124
125			} // namespace crocoddyl