Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/factory/contact.cpp	Lines:	114	129	88.4 %
Date:	2024-02-13 11:12:33	Branches:	81	141	57.4 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2023, University of Edinburgh, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#include "contact.hpp"

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

namespace crocoddyl {
namespace unittest {

const std::vector<ContactModelTypes::Type> ContactModelTypes::all(
    ContactModelTypes::init_all());

std::ostream& operator<<(std::ostream& os,
                         const ContactModelTypes::Type& type) {
  switch (type) {
    case ContactModelTypes::ContactModel1D_LOCAL:
      os << "ContactModel1D_LOCAL";
      break;
    case ContactModelTypes::ContactModel1D_WORLD:
      os << "ContactModel1D_WORLD";
      break;
    case ContactModelTypes::ContactModel1D_LWA:
      os << "ContactModel1D_LWA";
      break;
    case ContactModelTypes::ContactModel2D:
      os << "ContactModel2D";
      break;
    case ContactModelTypes::ContactModel3D_LOCAL:
      os << "ContactModel3D_LOCAL";
      break;
    case ContactModelTypes::ContactModel3D_WORLD:
      os << "ContactModel3D_WORLD";
      break;
    case ContactModelTypes::ContactModel3D_LWA:
      os << "ContactModel3D_LWA";
      break;
    case ContactModelTypes::ContactModel6D_LOCAL:
      os << "ContactModel6D_LOCAL";
      break;
    case ContactModelTypes::ContactModel6D_WORLD:
      os << "ContactModel6D_WORLD";
      break;
    case ContactModelTypes::ContactModel6D_LWA:
      os << "ContactModel6D_LWA";
      break;
    case ContactModelTypes::NbContactModelTypes:
      os << "NbContactModelTypes";
      break;
    default:
      os << "Unknown type";
      break;
  }
  return os;
}

ContactModelFactory::ContactModelFactory() {}
ContactModelFactory::~ContactModelFactory() {}

boost::shared_ptr<crocoddyl::ContactModelAbstract> ContactModelFactory::create(
    ContactModelTypes::Type contact_type, PinocchioModelTypes::Type model_type,
    Eigen::Vector2d gains, const std::string frame_name, std::size_t nu) const {
  PinocchioModelFactory model_factory(model_type);
  boost::shared_ptr<crocoddyl::StateMultibody> state =
      boost::make_shared<crocoddyl::StateMultibody>(model_factory.create());
  boost::shared_ptr<crocoddyl::ContactModelAbstract> contact;
  std::size_t frame_id = 0;
  if (frame_name == "") {
    frame_id = model_factory.get_frame_ids()[0];
  } else {
    frame_id = state->get_pinocchio()->getFrameId(frame_name);
  }
  if (nu == std::numeric_limits<std::size_t>::max()) {
    nu = state->get_nv();
  }
  switch (contact_type) {
    case ContactModelTypes::ContactModel1D_LOCAL: {
      pinocchio::SE3 M = pinocchio::SE3::Random();
      gains[0] =
          0;  // TODO(cmastalli): remove hard-coded zero when fixed the contact
      contact = boost::make_shared<crocoddyl::ContactModel1D>(
          state, frame_id, 0., pinocchio::ReferenceFrame::LOCAL, M.rotation(),
          nu, gains);
      break;
    }
    case ContactModelTypes::ContactModel1D_WORLD: {
      pinocchio::SE3 M = pinocchio::SE3::Random();
      gains[0] =
          0;  // TODO(cmastalli): remove hard-coded zero when fixed the contact
      contact = boost::make_shared<crocoddyl::ContactModel1D>(
          state, frame_id, 0., pinocchio::ReferenceFrame::WORLD, M.rotation(),
          nu, gains);
      break;
    }
    case ContactModelTypes::ContactModel1D_LWA: {
      pinocchio::SE3 M = pinocchio::SE3::Random();
      gains[0] =
          0;  // TODO(cmastalli): remove hard-coded zero when fixed the contact
      contact = boost::make_shared<crocoddyl::ContactModel1D>(
          state, frame_id, 0., pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED,
          M.rotation(), nu, gains);
      break;
    }
    case ContactModelTypes::ContactModel2D:
      gains[0] =
          0;  // TODO(cmastalli): remove hard-coded zero when fixed the contact
      contact = boost::make_shared<crocoddyl::ContactModel2D>(
          state, frame_id, Eigen::Vector2d::Zero(), nu, gains);
      break;
    case ContactModelTypes::ContactModel3D_LOCAL:
      contact = boost::make_shared<crocoddyl::ContactModel3D>(
          state, frame_id, Eigen::Vector3d::Zero(),
          pinocchio::ReferenceFrame::LOCAL, nu, gains);
      break;
    case ContactModelTypes::ContactModel3D_WORLD:
      contact = boost::make_shared<crocoddyl::ContactModel3D>(
          state, frame_id, Eigen::Vector3d::Zero(),
          pinocchio::ReferenceFrame::WORLD, nu, gains);
      break;
    case ContactModelTypes::ContactModel3D_LWA:
      contact = boost::make_shared<crocoddyl::ContactModel3D>(
          state, frame_id, Eigen::Vector3d::Zero(),
          pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, nu, gains);
      break;
    case ContactModelTypes::ContactModel6D_LOCAL:
      contact = boost::make_shared<crocoddyl::ContactModel6D>(
          state, frame_id, pinocchio::SE3::Identity(),
          pinocchio::ReferenceFrame::LOCAL, nu, gains);
      break;
    case ContactModelTypes::ContactModel6D_WORLD:
      contact = boost::make_shared<crocoddyl::ContactModel6D>(
          state, frame_id, pinocchio::SE3::Identity(),
          pinocchio::ReferenceFrame::WORLD, nu, gains);
      break;
    case ContactModelTypes::ContactModel6D_LWA:
      contact = boost::make_shared<crocoddyl::ContactModel6D>(
          state, frame_id, pinocchio::SE3::Identity(),
          pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, nu, gains);
      break;
    default:
      throw_pretty(__FILE__ ": Wrong ContactModelTypes::Type given");
      break;
  }
  return contact;
}

boost::shared_ptr<crocoddyl::ContactModelAbstract> create_random_contact() {
  static bool once = true;
  if (once) {
    srand((unsigned)time(NULL));
    once = false;
  }
  boost::shared_ptr<crocoddyl::ContactModelAbstract> contact;
  ContactModelFactory factory;
  if (rand() % 4 == 0) {
    contact = factory.create(ContactModelTypes::ContactModel1D_LOCAL,
                             PinocchioModelTypes::RandomHumanoid,
                             Eigen::Vector2d::Random());
  } else if (rand() % 4 == 1) {
    contact = factory.create(ContactModelTypes::ContactModel2D,
                             PinocchioModelTypes::RandomHumanoid,
                             Eigen::Vector2d::Random());
  } else if (rand() % 4 == 2) {
    contact = factory.create(ContactModelTypes::ContactModel3D_LOCAL,
                             PinocchioModelTypes::RandomHumanoid,
                             Eigen::Vector2d::Random());
  } else {
    contact = factory.create(ContactModelTypes::ContactModel6D_LOCAL,
                             PinocchioModelTypes::RandomHumanoid,
                             Eigen::Vector2d::Random());
  }
  return contact;
}

}  // namespace unittest
}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2023, University of Edinburgh, Heriot-Watt University
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#include "contact.hpp"
10
11			#include "crocoddyl/core/utils/exception.hpp"
12			#include "crocoddyl/multibody/contacts/contact-1d.hpp"
13			#include "crocoddyl/multibody/contacts/contact-2d.hpp"
14			#include "crocoddyl/multibody/contacts/contact-3d.hpp"
15			#include "crocoddyl/multibody/contacts/contact-6d.hpp"
16
17			namespace crocoddyl {
18			namespace unittest {
19
20			const std::vector<ContactModelTypes::Type> ContactModelTypes::all(
21			ContactModelTypes::init_all());
22
23		50	std::ostream& operator<<(std::ostream& os,
24			const ContactModelTypes::Type& type) {
25	✓✓✓✓ ✓✓✓✓ ✓✓✗✗	50	switch (type) {
26		5	case ContactModelTypes::ContactModel1D_LOCAL:
27		5	os << "ContactModel1D_LOCAL";
28		5	break;
29		5	case ContactModelTypes::ContactModel1D_WORLD:
30		5	os << "ContactModel1D_WORLD";
31		5	break;
32		5	case ContactModelTypes::ContactModel1D_LWA:
33		5	os << "ContactModel1D_LWA";
34		5	break;
35		5	case ContactModelTypes::ContactModel2D:
36		5	os << "ContactModel2D";
37		5	break;
38		5	case ContactModelTypes::ContactModel3D_LOCAL:
39		5	os << "ContactModel3D_LOCAL";
40		5	break;
41		5	case ContactModelTypes::ContactModel3D_WORLD:
42		5	os << "ContactModel3D_WORLD";
43		5	break;
44		5	case ContactModelTypes::ContactModel3D_LWA:
45		5	os << "ContactModel3D_LWA";
46		5	break;
47		5	case ContactModelTypes::ContactModel6D_LOCAL:
48		5	os << "ContactModel6D_LOCAL";
49		5	break;
50		5	case ContactModelTypes::ContactModel6D_WORLD:
51		5	os << "ContactModel6D_WORLD";
52		5	break;
53		5	case ContactModelTypes::ContactModel6D_LWA:
54		5	os << "ContactModel6D_LWA";
55		5	break;
56			case ContactModelTypes::NbContactModelTypes:
57			os << "NbContactModelTypes";
58			break;
59			default:
60			os << "Unknown type";
61			break;
62			}
63		50	return os;
64			}
65
66		2003	ContactModelFactory::ContactModelFactory() {}
67		2003	ContactModelFactory::~ContactModelFactory() {}
68
69		2003	boost::shared_ptr<crocoddyl::ContactModelAbstract> ContactModelFactory::create(
70			ContactModelTypes::Type contact_type, PinocchioModelTypes::Type model_type,
71			Eigen::Vector2d gains, const std::string frame_name, std::size_t nu) const {
72	✓✗	4006	PinocchioModelFactory model_factory(model_type);
73			boost::shared_ptr<crocoddyl::StateMultibody> state =
74	✓✗✓✗	4006	boost::make_shared<crocoddyl::StateMultibody>(model_factory.create());
75		2003	boost::shared_ptr<crocoddyl::ContactModelAbstract> contact;
76		2003	std::size_t frame_id = 0;
77	✓✓	2003	if (frame_name == "") {
78	✓✗	339	frame_id = model_factory.get_frame_ids()[0];
79			} else {
80	✓✗	1664	frame_id = state->get_pinocchio()->getFrameId(frame_name);
81			}
82	✓✓	2003	if (nu == std::numeric_limits<std::size_t>::max()) {
83		339	nu = state->get_nv();
84			}
85	✓✓✓✓ ✓✓✓✓ ✓✓✗	2003	switch (contact_type) {
86		41	case ContactModelTypes::ContactModel1D_LOCAL: {
87	✓✗	41	pinocchio::SE3 M = pinocchio::SE3::Random();
88	✓✗	41	gains[0] =
89			0; // TODO(cmastalli): remove hard-coded zero when fixed the contact
90		41	contact = boost::make_shared<crocoddyl::ContactModel1D>(
91	✓✗✓✗	41	state, frame_id, 0., pinocchio::ReferenceFrame::LOCAL, M.rotation(),
92		41	nu, gains);
93		41	break;
94			}
95		30	case ContactModelTypes::ContactModel1D_WORLD: {
96	✓✗	30	pinocchio::SE3 M = pinocchio::SE3::Random();
97	✓✗	30	gains[0] =
98			0; // TODO(cmastalli): remove hard-coded zero when fixed the contact
99		30	contact = boost::make_shared<crocoddyl::ContactModel1D>(
100	✓✗✓✗	30	state, frame_id, 0., pinocchio::ReferenceFrame::WORLD, M.rotation(),
101		30	nu, gains);
102		30	break;
103			}
104		30	case ContactModelTypes::ContactModel1D_LWA: {
105	✓✗	30	pinocchio::SE3 M = pinocchio::SE3::Random();
106	✓✗	30	gains[0] =
107			0; // TODO(cmastalli): remove hard-coded zero when fixed the contact
108		30	contact = boost::make_shared<crocoddyl::ContactModel1D>(
109			state, frame_id, 0., pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED,
110	✓✗✓✗	30	M.rotation(), nu, gains);
111		30	break;
112			}
113		136	case ContactModelTypes::ContactModel2D:
114	✓✗	136	gains[0] =
115			0; // TODO(cmastalli): remove hard-coded zero when fixed the contact
116	✓✗	272	contact = boost::make_shared<crocoddyl::ContactModel2D>(
117	✓✗	272	state, frame_id, Eigen::Vector2d::Zero(), nu, gains);
118		136	break;
119		684	case ContactModelTypes::ContactModel3D_LOCAL:
120	✓✗	1368	contact = boost::make_shared<crocoddyl::ContactModel3D>(
121			state, frame_id, Eigen::Vector3d::Zero(),
122	✓✗	1368	pinocchio::ReferenceFrame::LOCAL, nu, gains);
123		684	break;
124		248	case ContactModelTypes::ContactModel3D_WORLD:
125	✓✗	496	contact = boost::make_shared<crocoddyl::ContactModel3D>(
126			state, frame_id, Eigen::Vector3d::Zero(),
127	✓✗	496	pinocchio::ReferenceFrame::WORLD, nu, gains);
128		248	break;
129		248	case ContactModelTypes::ContactModel3D_LWA:
130	✓✗	496	contact = boost::make_shared<crocoddyl::ContactModel3D>(
131			state, frame_id, Eigen::Vector3d::Zero(),
132	✓✗	496	pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, nu, gains);
133		248	break;
134		296	case ContactModelTypes::ContactModel6D_LOCAL:
135	✓✗	592	contact = boost::make_shared<crocoddyl::ContactModel6D>(
136			state, frame_id, pinocchio::SE3::Identity(),
137	✓✗	592	pinocchio::ReferenceFrame::LOCAL, nu, gains);
138		296	break;
139		260	case ContactModelTypes::ContactModel6D_WORLD:
140	✓✗	520	contact = boost::make_shared<crocoddyl::ContactModel6D>(
141			state, frame_id, pinocchio::SE3::Identity(),
142	✓✗	520	pinocchio::ReferenceFrame::WORLD, nu, gains);
143		260	break;
144		30	case ContactModelTypes::ContactModel6D_LWA:
145	✓✗	60	contact = boost::make_shared<crocoddyl::ContactModel6D>(
146			state, frame_id, pinocchio::SE3::Identity(),
147	✓✗	60	pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, nu, gains);
148		30	break;
149			default:
150			throw_pretty(__FILE__ ": Wrong ContactModelTypes::Type given");
151			break;
152			}
153		4006	return contact;
154			}
155
156		39	boost::shared_ptr<crocoddyl::ContactModelAbstract> create_random_contact() {
157			static bool once = true;
158	✓✓	39	if (once) {
159		1	srand((unsigned)time(NULL));
160		1	once = false;
161			}
162		39	boost::shared_ptr<crocoddyl::ContactModelAbstract> contact;
163	✓✗	78	ContactModelFactory factory;
164	✓✓	39	if (rand() % 4 == 0) {
165	✓✗✓✗ ✓✗	44	contact = factory.create(ContactModelTypes::ContactModel1D_LOCAL,
166			PinocchioModelTypes::RandomHumanoid,
167	✓✗	33	Eigen::Vector2d::Random());
168	✓✓	28	} else if (rand() % 4 == 1) {
169	✓✗✓✗ ✓✗	24	contact = factory.create(ContactModelTypes::ContactModel2D,
170			PinocchioModelTypes::RandomHumanoid,
171	✓✗	18	Eigen::Vector2d::Random());
172	✓✓	22	} else if (rand() % 4 == 2) {
173	✓✗✓✗ ✓✗	24	contact = factory.create(ContactModelTypes::ContactModel3D_LOCAL,
174			PinocchioModelTypes::RandomHumanoid,
175	✓✗	18	Eigen::Vector2d::Random());
176			} else {
177	✓✗✓✗ ✓✗	64	contact = factory.create(ContactModelTypes::ContactModel6D_LOCAL,
178			PinocchioModelTypes::RandomHumanoid,
179	✓✗	48	Eigen::Vector2d::Random());
180			}
181		78	return contact;
182			}
183
184			} // namespace unittest
185			} // namespace crocoddyl