Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/multibody/residuals/frame-rotation.hxx	Lines:	32	43	74.4 %
Date:	2024-02-13 11:12:33	Branches:	34	90	37.8 %


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

#include <pinocchio/algorithm/frames.hpp>

#include "crocoddyl/multibody/residuals/frame-rotation.hpp"

namespace crocoddyl {

template <typename Scalar>
ResidualModelFrameRotationTpl<Scalar>::ResidualModelFrameRotationTpl(
    boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
    const Matrix3s& Rref, const std::size_t nu)
    : Base(state, 3, nu, true, false, false),
      id_(id),
      Rref_(Rref),
      oRf_inv_(Rref.transpose()),
      pin_model_(state->get_pinocchio()) {
  if (static_cast<pinocchio::FrameIndex>(state->get_pinocchio()->nframes) <=
      id) {
    throw_pretty(
        "Invalid argument: "
        << "the frame index is wrong (it does not exist in the robot)");
  }
}

template <typename Scalar>
ResidualModelFrameRotationTpl<Scalar>::ResidualModelFrameRotationTpl(
    boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
    const Matrix3s& Rref)
    : Base(state, 3, true, false, false),
      id_(id),
      Rref_(Rref),
      oRf_inv_(Rref.transpose()),
      pin_model_(state->get_pinocchio()) {
  if (static_cast<pinocchio::FrameIndex>(state->get_pinocchio()->nframes) <=
      id) {
    throw_pretty(
        "Invalid argument: "
        << "the frame index is wrong (it does not exist in the robot)");
  }
}

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

template <typename Scalar>
void ResidualModelFrameRotationTpl<Scalar>::calc(
    const boost::shared_ptr<ResidualDataAbstract>& data,
    const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
  Data* d = static_cast<Data*>(data.get());

  // Compute the frame rotation w.r.t. the reference frame
  pinocchio::updateFramePlacement(*pin_model_.get(), *d->pinocchio, id_);
  d->rRf.noalias() = oRf_inv_ * d->pinocchio->oMf[id_].rotation();
  data->r = pinocchio::log3(d->rRf);
}

template <typename Scalar>
void ResidualModelFrameRotationTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ResidualDataAbstract>& data,
    const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
  Data* d = static_cast<Data*>(data.get());

  // Compute the frame Jacobian at the error point
  pinocchio::Jlog3(d->rRf, d->rJf);
  pinocchio::getFrameJacobian(*pin_model_.get(), *d->pinocchio, id_,
                              pinocchio::LOCAL, d->fJf);

  // Compute the derivatives of the frame rotation
  const std::size_t nv = state_->get_nv();
  data->Rx.leftCols(nv).noalias() = d->rJf * d->fJf.template bottomRows<3>();
}

template <typename Scalar>
boost::shared_ptr<ResidualDataAbstractTpl<Scalar> >
ResidualModelFrameRotationTpl<Scalar>::createData(
    DataCollectorAbstract* const data) {
  return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
                                      data);
}

template <typename Scalar>
void ResidualModelFrameRotationTpl<Scalar>::print(std::ostream& os) const {
  const Eigen::IOFormat fmt(2, Eigen::DontAlignCols, ", ", ";\n", "", "", "[",
                            "]");
  typename pinocchio::SE3Tpl<Scalar>::Quaternion qref;
  pinocchio::quaternion::assignQuaternion(qref, Rref_);
  os << "ResidualModelFrameRotation {frame=" << pin_model_->frames[id_].name
     << ", qref=" << qref.coeffs().transpose().format(fmt) << "}";
}

template <typename Scalar>
pinocchio::FrameIndex ResidualModelFrameRotationTpl<Scalar>::get_id() const {
  return id_;
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::Matrix3s&
ResidualModelFrameRotationTpl<Scalar>::get_reference() const {
  return Rref_;
}

template <typename Scalar>
void ResidualModelFrameRotationTpl<Scalar>::set_id(
    const pinocchio::FrameIndex id) {
  id_ = id;
}

template <typename Scalar>
void ResidualModelFrameRotationTpl<Scalar>::set_reference(
    const Matrix3s& rotation) {
  Rref_ = rotation;
  oRf_inv_ = rotation.transpose();
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2021-2022, LAAS-CNRS, University of Edinburgh
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#include <pinocchio/algorithm/frames.hpp>
10
11			#include "crocoddyl/multibody/residuals/frame-rotation.hpp"
12
13			namespace crocoddyl {
14
15			template <typename Scalar>
16		348	ResidualModelFrameRotationTpl<Scalar>::ResidualModelFrameRotationTpl(
17			boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
18			const Matrix3s& Rref, const std::size_t nu)
19			: Base(state, 3, nu, true, false, false),
20			id_(id),
21			Rref_(Rref),
22			oRf_inv_(Rref.transpose()),
23	✓✗✓✗ ✓✗✓✗	348	pin_model_(state->get_pinocchio()) {
24	✗✓	348	if (static_cast<pinocchio::FrameIndex>(state->get_pinocchio()->nframes) <=
25			id) {
26			throw_pretty(
27			"Invalid argument: "
28			<< "the frame index is wrong (it does not exist in the robot)");
29			}
30		348	}
31
32			template <typename Scalar>
33		3	ResidualModelFrameRotationTpl<Scalar>::ResidualModelFrameRotationTpl(
34			boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
35			const Matrix3s& Rref)
36			: Base(state, 3, true, false, false),
37			id_(id),
38			Rref_(Rref),
39			oRf_inv_(Rref.transpose()),
40	✓✗✓✗ ✓✗✓✗	3	pin_model_(state->get_pinocchio()) {
41	✗✓	3	if (static_cast<pinocchio::FrameIndex>(state->get_pinocchio()->nframes) <=
42			id) {
43			throw_pretty(
44			"Invalid argument: "
45			<< "the frame index is wrong (it does not exist in the robot)");
46			}
47		3	}
48
49			template <typename Scalar>
50		706	ResidualModelFrameRotationTpl<Scalar>::~ResidualModelFrameRotationTpl() {}
51
52			template <typename Scalar>
53		8219	void ResidualModelFrameRotationTpl<Scalar>::calc(
54			const boost::shared_ptr<ResidualDataAbstract>& data,
55			const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
56		8219	Data* d = static_cast<Data*>(data.get());
57
58			// Compute the frame rotation w.r.t. the reference frame
59		8219	pinocchio::updateFramePlacement(pin_model_.get(), d->pinocchio, id_);
60	✓✗✓✗	8219	d->rRf.noalias() = oRf_inv_ * d->pinocchio->oMf[id_].rotation();
61	✓✗	8219	data->r = pinocchio::log3(d->rRf);
62		8219	}
63
64			template <typename Scalar>
65		280	void ResidualModelFrameRotationTpl<Scalar>::calcDiff(
66			const boost::shared_ptr<ResidualDataAbstract>& data,
67			const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
68		280	Data* d = static_cast<Data*>(data.get());
69
70			// Compute the frame Jacobian at the error point
71		280	pinocchio::Jlog3(d->rRf, d->rJf);
72		280	pinocchio::getFrameJacobian(pin_model_.get(), d->pinocchio, id_,
73		280	pinocchio::LOCAL, d->fJf);
74
75			// Compute the derivatives of the frame rotation
76		280	const std::size_t nv = state_->get_nv();
77	✓✗✓✗ ✓✗✓✗	280	data->Rx.leftCols(nv).noalias() = d->rJf * d->fJf.template bottomRows<3>();
78		280	}
79
80			template <typename Scalar>
81			boost::shared_ptr<ResidualDataAbstractTpl<Scalar> >
82		9554	ResidualModelFrameRotationTpl<Scalar>::createData(
83			DataCollectorAbstract* const data) {
84			return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
85	✓✗	9554	data);
86			}
87
88			template <typename Scalar>
89		63	void ResidualModelFrameRotationTpl<Scalar>::print(std::ostream& os) const {
90	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	126	const Eigen::IOFormat fmt(2, Eigen::DontAlignCols, ", ", ";\n", "", "", "[",
91			"]");
92	✓✗	63	typename pinocchio::SE3Tpl<Scalar>::Quaternion qref;
93	✓✗	63	pinocchio::quaternion::assignQuaternion(qref, Rref_);
94		63	os << "ResidualModelFrameRotation {frame=" << pin_model_->frames[id_].name
95	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	126	<< ", qref=" << qref.coeffs().transpose().format(fmt) << "}";
96		63	}
97
98			template <typename Scalar>
99			pinocchio::FrameIndex ResidualModelFrameRotationTpl<Scalar>::get_id() const {
100			return id_;
101			}
102
103			template <typename Scalar>
104			const typename MathBaseTpl<Scalar>::Matrix3s&
105			ResidualModelFrameRotationTpl<Scalar>::get_reference() const {
106			return Rref_;
107			}
108
109			template <typename Scalar>
110			void ResidualModelFrameRotationTpl<Scalar>::set_id(
111			const pinocchio::FrameIndex id) {
112			id_ = id;
113			}
114
115			template <typename Scalar>
116			void ResidualModelFrameRotationTpl<Scalar>::set_reference(
117			const Matrix3s& rotation) {
118			Rref_ = rotation;
119			oRf_inv_ = rotation.transpose();
120			}
121
122			} // namespace crocoddyl