Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/multibody/residuals/contact-friction-cone.hxx	Lines:	53	69	76.8 %
Date:	2024-02-13 11:12:33	Branches:	49	118	41.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 "crocoddyl/multibody/residuals/contact-friction-cone.hpp"

namespace crocoddyl {

template <typename Scalar>
ResidualModelContactFrictionConeTpl<Scalar>::
    ResidualModelContactFrictionConeTpl(boost::shared_ptr<StateMultibody> state,
                                        const pinocchio::FrameIndex id,
                                        const FrictionCone& fref,
                                        const std::size_t nu, const bool fwddyn)
    : Base(state, fref.get_nf() + 1, nu, fwddyn ? true : false,
           fwddyn ? true : false, true),
      fwddyn_(fwddyn),
      update_jacobians_(true),
      id_(id),
      fref_(fref) {
  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>
ResidualModelContactFrictionConeTpl<Scalar>::
    ResidualModelContactFrictionConeTpl(boost::shared_ptr<StateMultibody> state,
                                        const pinocchio::FrameIndex id,
                                        const FrictionCone& fref)
    : Base(state, fref.get_nf() + 1),
      fwddyn_(true),
      update_jacobians_(true),
      id_(id),
      fref_(fref) {
  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>
ResidualModelContactFrictionConeTpl<
    Scalar>::~ResidualModelContactFrictionConeTpl() {}

template <typename Scalar>
void ResidualModelContactFrictionConeTpl<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 residual of the friction cone. Note that we need to transform
  // the force to the contact frame
  data->r.noalias() = fref_.get_A() * d->contact->f.linear();
}

template <typename Scalar>
void ResidualModelContactFrictionConeTpl<Scalar>::calc(
    const boost::shared_ptr<ResidualDataAbstract>& data,
    const Eigen::Ref<const VectorXs>&) {
  data->r.setZero();
}

template <typename Scalar>
void ResidualModelContactFrictionConeTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ResidualDataAbstract>& data,
    const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
  if (fwddyn_ || update_jacobians_) {
    updateJacobians(data);
  }
}

template <typename Scalar>
void ResidualModelContactFrictionConeTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ResidualDataAbstract>& data,
    const Eigen::Ref<const VectorXs>&) {
  data->Rx.setZero();
}

template <typename Scalar>
boost::shared_ptr<ResidualDataAbstractTpl<Scalar> >
ResidualModelContactFrictionConeTpl<Scalar>::createData(
    DataCollectorAbstract* const data) {
  boost::shared_ptr<ResidualDataAbstract> d = boost::allocate_shared<Data>(
      Eigen::aligned_allocator<Data>(), this, data);
  if (!fwddyn_) {
    updateJacobians(d);
  }
  return d;
}

template <typename Scalar>
void ResidualModelContactFrictionConeTpl<Scalar>::updateJacobians(
    const boost::shared_ptr<ResidualDataAbstract>& data) {
  Data* d = static_cast<Data*>(data.get());

  const MatrixXs& df_dx = d->contact->df_dx;
  const MatrixXs& df_du = d->contact->df_du;
  const MatrixX3s& A = fref_.get_A();
  switch (d->contact_type) {
    case Contact2D: {
      // Valid for xz plane
      data->Rx.noalias() = A.col(0) * df_dx.row(0) + A.col(2) * df_dx.row(1);
      data->Ru.noalias() = A.col(0) * df_du.row(0) + A.col(2) * df_du.row(1);
      break;
    }
    case Contact3D:
      data->Rx.noalias() = A * df_dx;
      data->Ru.noalias() = A * df_du;
      break;
    case Contact6D:
      data->Rx.noalias() = A * df_dx.template topRows<3>();
      data->Ru.noalias() = A * df_du.template topRows<3>();
      break;
    default:
      break;
  }
  update_jacobians_ = false;
}

template <typename Scalar>
void ResidualModelContactFrictionConeTpl<Scalar>::print(
    std::ostream& os) const {
  boost::shared_ptr<StateMultibody> s =
      boost::static_pointer_cast<StateMultibody>(state_);
  os << "ResidualModelContactFrictionCone {frame="
     << s->get_pinocchio()->frames[id_].name << ", mu=" << fref_.get_mu()
     << "}";
}

template <typename Scalar>
bool ResidualModelContactFrictionConeTpl<Scalar>::is_fwddyn() const {
  return fwddyn_;
}

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

template <typename Scalar>
const FrictionConeTpl<Scalar>&
ResidualModelContactFrictionConeTpl<Scalar>::get_reference() const {
  return fref_;
}

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

template <typename Scalar>
void ResidualModelContactFrictionConeTpl<Scalar>::set_reference(
    const FrictionCone& reference) {
  fref_ = reference;
  update_jacobians_ = true;
}

}  // 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 "crocoddyl/multibody/residuals/contact-friction-cone.hpp"
11
12			namespace crocoddyl {
13
14			template <typename Scalar>
15		871	ResidualModelContactFrictionConeTpl<Scalar>::
16			ResidualModelContactFrictionConeTpl(boost::shared_ptr<StateMultibody> state,
17			const pinocchio::FrameIndex id,
18			const FrictionCone& fref,
19			const std::size_t nu, const bool fwddyn)
20		871	: Base(state, fref.get_nf() + 1, nu, fwddyn ? true : false,
21			fwddyn ? true : false, true),
22			fwddyn_(fwddyn),
23			update_jacobians_(true),
24			id_(id),
25	✓✓✓✓ ✓✗✓✗	1742	fref_(fref) {
26	✗✓	871	if (static_cast<pinocchio::FrameIndex>(state->get_pinocchio()->nframes) <=
27			id) {
28			throw_pretty(
29			"Invalid argument: "
30			<< "the frame index is wrong (it does not exist in the robot)");
31			}
32		871	}
33
34			template <typename Scalar>
35		1	ResidualModelContactFrictionConeTpl<Scalar>::
36			ResidualModelContactFrictionConeTpl(boost::shared_ptr<StateMultibody> state,
37			const pinocchio::FrameIndex id,
38			const FrictionCone& fref)
39		1	: Base(state, fref.get_nf() + 1),
40			fwddyn_(true),
41			update_jacobians_(true),
42			id_(id),
43	✓✗✓✗	1	fref_(fref) {
44	✗✓	1	if (static_cast<pinocchio::FrameIndex>(state->get_pinocchio()->nframes) <=
45			id) {
46			throw_pretty(
47			"Invalid argument: "
48			<< "the frame index is wrong (it does not exist in the robot)");
49			}
50		1	}
51
52			template <typename Scalar>
53		1748	ResidualModelContactFrictionConeTpl<
54		1748	Scalar>::~ResidualModelContactFrictionConeTpl() {}
55
56			template <typename Scalar>
57		60012	void ResidualModelContactFrictionConeTpl<Scalar>::calc(
58			const boost::shared_ptr<ResidualDataAbstract>& data,
59			const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
60		60012	Data* d = static_cast<Data*>(data.get());
61
62			// Compute the residual of the friction cone. Note that we need to transform
63			// the force to the contact frame
64	✓✗✓✗ ✓✗	60012	data->r.noalias() = fref_.get_A() * d->contact->f.linear();
65		60012	}
66
67			template <typename Scalar>
68		12150	void ResidualModelContactFrictionConeTpl<Scalar>::calc(
69			const boost::shared_ptr<ResidualDataAbstract>& data,
70			const Eigen::Ref<const VectorXs>&) {
71		12150	data->r.setZero();
72		12150	}
73
74			template <typename Scalar>
75		8684	void ResidualModelContactFrictionConeTpl<Scalar>::calcDiff(
76			const boost::shared_ptr<ResidualDataAbstract>& data,
77			const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
78	✓✓✗✓	8684	if (fwddyn_ \|\| update_jacobians_) {
79		4922	updateJacobians(data);
80			}
81		8684	}
82
83			template <typename Scalar>
84		349	void ResidualModelContactFrictionConeTpl<Scalar>::calcDiff(
85			const boost::shared_ptr<ResidualDataAbstract>& data,
86			const Eigen::Ref<const VectorXs>&) {
87		349	data->Rx.setZero();
88		349	}
89
90			template <typename Scalar>
91			boost::shared_ptr<ResidualDataAbstractTpl<Scalar> >
92		72435	ResidualModelContactFrictionConeTpl<Scalar>::createData(
93			DataCollectorAbstract* const data) {
94	✓✗	144870	boost::shared_ptr<ResidualDataAbstract> d = boost::allocate_shared<Data>(
95			Eigen::aligned_allocator<Data>(), this, data);
96	✓✓	72435	if (!fwddyn_) {
97	✓✗	30942	updateJacobians(d);
98			}
99		72435	return d;
100			}
101
102			template <typename Scalar>
103		35864	void ResidualModelContactFrictionConeTpl<Scalar>::updateJacobians(
104			const boost::shared_ptr<ResidualDataAbstract>& data) {
105		35864	Data* d = static_cast<Data*>(data.get());
106
107		35864	const MatrixXs& df_dx = d->contact->df_dx;
108		35864	const MatrixXs& df_du = d->contact->df_du;
109		35864	const MatrixX3s& A = fref_.get_A();
110	✓✓✓✗	35864	switch (d->contact_type) {
111		627	case Contact2D: {
112			// Valid for xz plane
113	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	627	data->Rx.noalias() = A.col(0) * df_dx.row(0) + A.col(2) * df_dx.row(1);
114	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	627	data->Ru.noalias() = A.col(0) * df_du.row(0) + A.col(2) * df_du.row(1);
115		627	break;
116			}
117		27315	case Contact3D:
118	✓✗✓✗	27315	data->Rx.noalias() = A * df_dx;
119	✓✗✓✗	27315	data->Ru.noalias() = A * df_du;
120		27315	break;
121		7922	case Contact6D:
122	✓✗✓✗ ✓✗	7922	data->Rx.noalias() = A * df_dx.template topRows<3>();
123	✓✗✓✗ ✓✗	7922	data->Ru.noalias() = A * df_du.template topRows<3>();
124		7922	break;
125			default:
126			break;
127			}
128		35864	update_jacobians_ = false;
129		35864	}
130
131			template <typename Scalar>
132			void ResidualModelContactFrictionConeTpl<Scalar>::print(
133			std::ostream& os) const {
134			boost::shared_ptr<StateMultibody> s =
135			boost::static_pointer_cast<StateMultibody>(state_);
136			os << "ResidualModelContactFrictionCone {frame="
137			<< s->get_pinocchio()->frames[id_].name << ", mu=" << fref_.get_mu()
138			<< "}";
139			}
140
141			template <typename Scalar>
142			bool ResidualModelContactFrictionConeTpl<Scalar>::is_fwddyn() const {
143			return fwddyn_;
144			}
145
146			template <typename Scalar>
147		72435	pinocchio::FrameIndex ResidualModelContactFrictionConeTpl<Scalar>::get_id()
148			const {
149		72435	return id_;
150			}
151
152			template <typename Scalar>
153			const FrictionConeTpl<Scalar>&
154			ResidualModelContactFrictionConeTpl<Scalar>::get_reference() const {
155			return fref_;
156			}
157
158			template <typename Scalar>
159			void ResidualModelContactFrictionConeTpl<Scalar>::set_id(
160			const pinocchio::FrameIndex id) {
161			id_ = id;
162			}
163
164			template <typename Scalar>
165			void ResidualModelContactFrictionConeTpl<Scalar>::set_reference(
166			const FrictionCone& reference) {
167			fref_ = reference;
168			update_jacobians_ = true;
169			}
170
171			} // namespace crocoddyl