Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/factory/cost.cpp	Lines:	141	164	86.0 %
Date:	2024-02-13 11:12:33	Branches:	106	223	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 "cost.hpp"

#include "crocoddyl/core/costs/residual.hpp"
#include "crocoddyl/core/residuals/control.hpp"
#include "crocoddyl/multibody/residuals/com-position.hpp"
#include "crocoddyl/multibody/residuals/control-gravity.hpp"
#include "crocoddyl/multibody/residuals/state.hpp"
// #include "crocoddyl/multibody/residuals/centroidal-momentum.hpp"
#include "crocoddyl/core/activations/quadratic.hpp"
#include "crocoddyl/core/costs/cost-sum.hpp"
#include "crocoddyl/core/utils/exception.hpp"
#include "crocoddyl/multibody/residuals/contact-friction-cone.hpp"
#include "crocoddyl/multibody/residuals/contact-wrench-cone.hpp"
#include "crocoddyl/multibody/residuals/frame-placement.hpp"
#include "crocoddyl/multibody/residuals/frame-rotation.hpp"
#include "crocoddyl/multibody/residuals/frame-translation.hpp"
#include "crocoddyl/multibody/residuals/frame-velocity.hpp"
#include "crocoddyl/multibody/residuals/pair-collision.hpp"

namespace crocoddyl {
namespace unittest {

const std::vector<CostModelTypes::Type> CostModelTypes::all(
    CostModelTypes::init_all());
const std::vector<CostModelNoFFTypes::Type> CostModelNoFFTypes::all(
    CostModelNoFFTypes::init_all());
#ifdef PINOCCHIO_WITH_HPP_FCL
const std::vector<CostModelCollisionTypes::Type> CostModelCollisionTypes::all(
    CostModelCollisionTypes::init_all());
#endif  // PINOCCHIO_WITH_HPP_FCL

std::ostream& operator<<(std::ostream& os, CostModelTypes::Type type) {
  switch (type) {
    case CostModelTypes::CostModelResidualState:
      os << "CostModelResidualState";
      break;
    case CostModelTypes::CostModelResidualControl:
      os << "CostModelResidualControl";
      break;
    case CostModelTypes::CostModelResidualCoMPosition:
      os << "CostModelResidualCoMPosition";
      break;
    // case CostModelTypes::CostModelResidualCentroidalMomentum:
    //   os << "CostModelResidualCentroidalMomentum";
    //   break;
    case CostModelTypes::CostModelResidualFramePlacement:
      os << "CostModelResidualFramePlacement";
      break;
    case CostModelTypes::CostModelResidualFrameRotation:
      os << "CostModelResidualFrameRotation";
      break;
    case CostModelTypes::CostModelResidualFrameTranslation:
      os << "CostModelResidualFrameTranslation";
      break;
    case CostModelTypes::CostModelResidualFrameVelocity:
      os << "CostModelResidualFrameVelocity";
      break;
    case CostModelTypes::NbCostModelTypes:
      os << "NbCostModelTypes";
      break;
    default:
      break;
  }
  return os;
}

std::ostream& operator<<(std::ostream& os, CostModelNoFFTypes::Type type) {
  switch (type) {
    case CostModelNoFFTypes::CostModelResidualControlGrav:
      os << "CostModelResidualControlGrav";
      break;
    case CostModelNoFFTypes::NbCostModelNoFFTypes:
      os << "NbCostModelNoFFTypes";
      break;
    default:
      break;
  }
  return os;
}

#ifdef PINOCCHIO_WITH_HPP_FCL
std::ostream& operator<<(std::ostream& os, CostModelCollisionTypes::Type type) {
  switch (type) {
    case CostModelCollisionTypes::CostModelResidualPairCollision:
      os << "CostModelResidualPairCollision";
      break;
    case CostModelCollisionTypes::NbCostModelCollisionTypes:
      os << "NbCostModelCollisionTypes";
      break;
    default:
      break;
  }
  return os;
}
#endif  // PINOCCHIO_WITH_HPP_FCL

CostModelFactory::CostModelFactory() {}
CostModelFactory::~CostModelFactory() {}

boost::shared_ptr<crocoddyl::CostModelAbstract> CostModelFactory::create(
    CostModelTypes::Type cost_type, StateModelTypes::Type state_type,
    ActivationModelTypes::Type activation_type, std::size_t nu) const {
  StateModelFactory state_factory;
  ActivationModelFactory activation_factory;
  boost::shared_ptr<crocoddyl::CostModelAbstract> cost;
  boost::shared_ptr<crocoddyl::StateMultibody> state =
      boost::static_pointer_cast<crocoddyl::StateMultibody>(
          state_factory.create(state_type));

  pinocchio::FrameIndex frame_index = state->get_pinocchio()->frames.size() - 1;
  pinocchio::SE3 frame_SE3 = pinocchio::SE3::Random();

  if (nu == std::numeric_limits<std::size_t>::max()) {
    nu = state->get_nv();
  }
  switch (cost_type) {
    case CostModelTypes::CostModelResidualState:
      cost = boost::make_shared<crocoddyl::CostModelResidual>(
          state, activation_factory.create(activation_type, state->get_ndx()),
          boost::make_shared<crocoddyl::ResidualModelState>(state,
                                                            state->rand(), nu));
      break;
    case CostModelTypes::CostModelResidualControl:
      cost = boost::make_shared<crocoddyl::CostModelResidual>(
          state, activation_factory.create(activation_type, nu),
          boost::make_shared<crocoddyl::ResidualModelControl>(
              state, Eigen::VectorXd::Random(nu)));
      break;
    case CostModelTypes::CostModelResidualCoMPosition:
      cost = boost::make_shared<crocoddyl::CostModelResidual>(
          state, activation_factory.create(activation_type, 3),
          boost::make_shared<crocoddyl::ResidualModelCoMPosition>(
              state, Eigen::Vector3d::Random(), nu));
      break;
    // case CostModelTypes::CostModelResidualCentroidalMomentum:
    //   cost = boost::make_shared<crocoddyl::CostModelResidual>(
    //       state,
    //       boost::make_shared<crocoddyl::ResidualModelCentroidalMomentum>(state,
    //       Vector6d::Random(), nu), activation_factory.create(activation_type,
    //       6));
    //   break;
    case CostModelTypes::CostModelResidualFramePlacement:
      cost = boost::make_shared<crocoddyl::CostModelResidual>(
          state, activation_factory.create(activation_type, 6),
          boost::make_shared<crocoddyl::ResidualModelFramePlacement>(
              state, frame_index, frame_SE3, nu));
      break;
    case CostModelTypes::CostModelResidualFrameRotation:
      cost = boost::make_shared<crocoddyl::CostModelResidual>(
          state, activation_factory.create(activation_type, 3),
          boost::make_shared<crocoddyl::ResidualModelFrameRotation>(
              state, frame_index, frame_SE3.rotation(), nu));
      break;
    case CostModelTypes::CostModelResidualFrameTranslation:
      cost = boost::make_shared<crocoddyl::CostModelResidual>(
          state, activation_factory.create(activation_type, 3),
          boost::make_shared<crocoddyl::ResidualModelFrameTranslation>(
              state, frame_index, frame_SE3.translation(), nu));
      break;
    case CostModelTypes::CostModelResidualFrameVelocity:
      cost = boost::make_shared<crocoddyl::CostModelResidual>(
          state, activation_factory.create(activation_type, 6),
          boost::make_shared<crocoddyl::ResidualModelFrameVelocity>(
              state, frame_index, pinocchio::Motion::Random(),
              pinocchio::ReferenceFrame::LOCAL, nu));
      break;
    default:
      throw_pretty(__FILE__ ": Wrong CostModelTypes::Type given");
      break;
  }
  return cost;
}

boost::shared_ptr<crocoddyl::CostModelAbstract> CostModelFactory::create(
    CostModelNoFFTypes::Type cost_type,
    ActivationModelTypes::Type activation_type, std::size_t nu) const {
  StateModelFactory state_factory;
  ActivationModelFactory activation_factory;
  boost::shared_ptr<crocoddyl::CostModelAbstract> cost;
  boost::shared_ptr<crocoddyl::StateMultibody> state =
      boost::static_pointer_cast<crocoddyl::StateMultibody>(
          state_factory.create(StateModelTypes::StateMultibody_TalosArm));
  if (nu == std::numeric_limits<std::size_t>::max()) {
    nu = state->get_nv();
  }

  switch (cost_type) {
    case CostModelNoFFTypes::CostModelResidualControlGrav:
      cost = boost::make_shared<crocoddyl::CostModelResidual>(
          state, activation_factory.create(activation_type, state->get_nv()),
          boost::make_shared<ResidualModelControlGrav>(state, nu));
      break;
    default:
      throw_pretty(__FILE__ ": Wrong CostModelTypes::Type given");
      break;
  }
  return cost;
}

#ifdef PINOCCHIO_WITH_HPP_FCL
boost::shared_ptr<crocoddyl::CostModelAbstract> CostModelFactory::create(
    CostModelCollisionTypes::Type cost_type, StateModelTypes::Type state_type,
    std::size_t nu) const {
  StateModelFactory state_factory;
  boost::shared_ptr<crocoddyl::CostModelAbstract> cost;
  boost::shared_ptr<crocoddyl::StateMultibody> state =
      boost::static_pointer_cast<crocoddyl::StateMultibody>(
          state_factory.create(state_type));
  if (nu == std::numeric_limits<std::size_t>::max()) {
    nu = state->get_nv();
  }
  pinocchio::FrameIndex frame_index = state->get_pinocchio()->frames.size() - 1;
  pinocchio::SE3 frame_SE3 = pinocchio::SE3::Random();
  pinocchio::SE3 frame_SE3_obstacle = pinocchio::SE3::Random();
  double alpha = fabs(Eigen::VectorXd::Random(1)[0]);
  double beta = fabs(Eigen::VectorXd::Random(1)[0]);

  boost::shared_ptr<pinocchio::GeometryModel> geometry =
      boost::make_shared<pinocchio::GeometryModel>(pinocchio::GeometryModel());
  pinocchio::GeomIndex ig_frame =
      geometry->addGeometryObject(pinocchio::GeometryObject(
          "frame", frame_index,
          state->get_pinocchio()->frames[frame_index].parent,
          CollisionGeometryPtr(new hpp::fcl::Capsule(0, alpha)), frame_SE3));
  pinocchio::GeomIndex ig_obs =
      geometry->addGeometryObject(pinocchio::GeometryObject(
          "obs", state->get_pinocchio()->getFrameId("universe"),
          state->get_pinocchio()
              ->frames[state->get_pinocchio()->getFrameId("universe")]
              .parent,
          CollisionGeometryPtr(new hpp::fcl::Capsule(0, beta)),
          frame_SE3_obstacle));
  geometry->addCollisionPair(pinocchio::CollisionPair(ig_frame, ig_obs));

  switch (cost_type) {
    case CostModelCollisionTypes::CostModelResidualPairCollision:
      cost = boost::make_shared<crocoddyl::CostModelResidual>(
          state, boost::make_shared<crocoddyl::ActivationModelQuad>(3),
          boost::make_shared<crocoddyl::ResidualModelPairCollision>(
              state, nu, geometry, 0,
              state->get_pinocchio()->frames[frame_index].parent));
      break;
    default:
      throw_pretty(__FILE__ ": Wrong CostModelTypes::Type given");
      break;
  }
  return cost;
}
#endif  // PINOCCHIO_WITH_HPP_FCL

boost::shared_ptr<crocoddyl::CostModelAbstract> create_random_cost(
    StateModelTypes::Type state_type, std::size_t nu) {
  static bool once = true;
  if (once) {
    srand((unsigned)time(NULL));
    once = false;
  }

  CostModelFactory factory;
  CostModelTypes::Type rand_type = static_cast<CostModelTypes::Type>(
      rand() % CostModelTypes::NbCostModelTypes);
  return factory.create(rand_type, state_type,
                        ActivationModelTypes::ActivationModelQuad, nu);
}

}  // 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, 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 "cost.hpp"
11
12			#include "crocoddyl/core/costs/residual.hpp"
13			#include "crocoddyl/core/residuals/control.hpp"
14			#include "crocoddyl/multibody/residuals/com-position.hpp"
15			#include "crocoddyl/multibody/residuals/control-gravity.hpp"
16			#include "crocoddyl/multibody/residuals/state.hpp"
17			// #include "crocoddyl/multibody/residuals/centroidal-momentum.hpp"
18			#include "crocoddyl/core/activations/quadratic.hpp"
19			#include "crocoddyl/core/costs/cost-sum.hpp"
20			#include "crocoddyl/core/utils/exception.hpp"
21			#include "crocoddyl/multibody/residuals/contact-friction-cone.hpp"
22			#include "crocoddyl/multibody/residuals/contact-wrench-cone.hpp"
23			#include "crocoddyl/multibody/residuals/frame-placement.hpp"
24			#include "crocoddyl/multibody/residuals/frame-rotation.hpp"
25			#include "crocoddyl/multibody/residuals/frame-translation.hpp"
26			#include "crocoddyl/multibody/residuals/frame-velocity.hpp"
27			#include "crocoddyl/multibody/residuals/pair-collision.hpp"
28
29			namespace crocoddyl {
30			namespace unittest {
31
32			const std::vector<CostModelTypes::Type> CostModelTypes::all(
33			CostModelTypes::init_all());
34			const std::vector<CostModelNoFFTypes::Type> CostModelNoFFTypes::all(
35			CostModelNoFFTypes::init_all());
36			#ifdef PINOCCHIO_WITH_HPP_FCL
37			const std::vector<CostModelCollisionTypes::Type> CostModelCollisionTypes::all(
38			CostModelCollisionTypes::init_all());
39			#endif // PINOCCHIO_WITH_HPP_FCL
40
41		315	std::ostream& operator<<(std::ostream& os, CostModelTypes::Type type) {
42	✓✓✓✓ ✓✓✓✗ ✗	315	switch (type) {
43		45	case CostModelTypes::CostModelResidualState:
44		45	os << "CostModelResidualState";
45		45	break;
46		45	case CostModelTypes::CostModelResidualControl:
47		45	os << "CostModelResidualControl";
48		45	break;
49		45	case CostModelTypes::CostModelResidualCoMPosition:
50		45	os << "CostModelResidualCoMPosition";
51		45	break;
52			// case CostModelTypes::CostModelResidualCentroidalMomentum:
53			// os << "CostModelResidualCentroidalMomentum";
54			// break;
55		45	case CostModelTypes::CostModelResidualFramePlacement:
56		45	os << "CostModelResidualFramePlacement";
57		45	break;
58		45	case CostModelTypes::CostModelResidualFrameRotation:
59		45	os << "CostModelResidualFrameRotation";
60		45	break;
61		45	case CostModelTypes::CostModelResidualFrameTranslation:
62		45	os << "CostModelResidualFrameTranslation";
63		45	break;
64		45	case CostModelTypes::CostModelResidualFrameVelocity:
65		45	os << "CostModelResidualFrameVelocity";
66		45	break;
67			case CostModelTypes::NbCostModelTypes:
68			os << "NbCostModelTypes";
69			break;
70			default:
71			break;
72			}
73		315	return os;
74			}
75
76		9	std::ostream& operator<<(std::ostream& os, CostModelNoFFTypes::Type type) {
77	✓✗✗	9	switch (type) {
78		9	case CostModelNoFFTypes::CostModelResidualControlGrav:
79		9	os << "CostModelResidualControlGrav";
80		9	break;
81			case CostModelNoFFTypes::NbCostModelNoFFTypes:
82			os << "NbCostModelNoFFTypes";
83			break;
84			default:
85			break;
86			}
87		9	return os;
88			}
89
90			#ifdef PINOCCHIO_WITH_HPP_FCL
91		2	std::ostream& operator<<(std::ostream& os, CostModelCollisionTypes::Type type) {
92	✓✗✗	2	switch (type) {
93		2	case CostModelCollisionTypes::CostModelResidualPairCollision:
94		2	os << "CostModelResidualPairCollision";
95		2	break;
96			case CostModelCollisionTypes::NbCostModelCollisionTypes:
97			os << "NbCostModelCollisionTypes";
98			break;
99			default:
100			break;
101			}
102		2	return os;
103			}
104			#endif // PINOCCHIO_WITH_HPP_FCL
105
106		4197	CostModelFactory::CostModelFactory() {}
107		4197	CostModelFactory::~CostModelFactory() {}
108
109		4142	boost::shared_ptr<crocoddyl::CostModelAbstract> CostModelFactory::create(
110			CostModelTypes::Type cost_type, StateModelTypes::Type state_type,
111			ActivationModelTypes::Type activation_type, std::size_t nu) const {
112	✓✗	8284	StateModelFactory state_factory;
113	✓✗	8284	ActivationModelFactory activation_factory;
114		4142	boost::shared_ptr<crocoddyl::CostModelAbstract> cost;
115			boost::shared_ptr<crocoddyl::StateMultibody> state =
116			boost::static_pointer_cast<crocoddyl::StateMultibody>(
117	✓✗	8284	state_factory.create(state_type));
118
119		4142	pinocchio::FrameIndex frame_index = state->get_pinocchio()->frames.size() - 1;
120	✓✗	4142	pinocchio::SE3 frame_SE3 = pinocchio::SE3::Random();
121
122	✓✓	4142	if (nu == std::numeric_limits<std::size_t>::max()) {
123		1647	nu = state->get_nv();
124			}
125	✓✓✓✓ ✓✓✓✗	4142	switch (cost_type) {
126		1372	case CostModelTypes::CostModelResidualState:
127	✓✗	2744	cost = boost::make_shared<crocoddyl::CostModelResidual>(
128	✓✗	2744	state, activation_factory.create(activation_type, state->get_ndx()),
129	✓✗	2744	boost::make_shared<crocoddyl::ResidualModelState>(state,
130	✓✗	4116	state->rand(), nu));
131		1372	break;
132		1296	case CostModelTypes::CostModelResidualControl:
133	✓✗	2592	cost = boost::make_shared<crocoddyl::CostModelResidual>(
134	✓✗	2592	state, activation_factory.create(activation_type, nu),
135	✓✗	2592	boost::make_shared<crocoddyl::ResidualModelControl>(
136	✓✗	2592	state, Eigen::VectorXd::Random(nu)));
137		1296	break;
138		235	case CostModelTypes::CostModelResidualCoMPosition:
139	✓✗	470	cost = boost::make_shared<crocoddyl::CostModelResidual>(
140	✓✗	470	state, activation_factory.create(activation_type, 3),
141	✓✗	470	boost::make_shared<crocoddyl::ResidualModelCoMPosition>(
142	✓✗	470	state, Eigen::Vector3d::Random(), nu));
143		235	break;
144			// case CostModelTypes::CostModelResidualCentroidalMomentum:
145			// cost = boost::make_shared<crocoddyl::CostModelResidual>(
146			// state,
147			// boost::make_shared<crocoddyl::ResidualModelCentroidalMomentum>(state,
148			// Vector6d::Random(), nu), activation_factory.create(activation_type,
149			// 6));
150			// break;
151		532	case CostModelTypes::CostModelResidualFramePlacement:
152	✓✗	1064	cost = boost::make_shared<crocoddyl::CostModelResidual>(
153	✓✗	1064	state, activation_factory.create(activation_type, 6),
154	✓✗	1064	boost::make_shared<crocoddyl::ResidualModelFramePlacement>(
155		532	state, frame_index, frame_SE3, nu));
156		532	break;
157		238	case CostModelTypes::CostModelResidualFrameRotation:
158	✓✗	476	cost = boost::make_shared<crocoddyl::CostModelResidual>(
159	✓✗	476	state, activation_factory.create(activation_type, 3),
160		238	boost::make_shared<crocoddyl::ResidualModelFrameRotation>(
161	✓✗✓✗	476	state, frame_index, frame_SE3.rotation(), nu));
162		238	break;
163		234	case CostModelTypes::CostModelResidualFrameTranslation:
164	✓✗	468	cost = boost::make_shared<crocoddyl::CostModelResidual>(
165	✓✗	468	state, activation_factory.create(activation_type, 3),
166		234	boost::make_shared<crocoddyl::ResidualModelFrameTranslation>(
167	✓✗✓✗	468	state, frame_index, frame_SE3.translation(), nu));
168		234	break;
169		235	case CostModelTypes::CostModelResidualFrameVelocity:
170	✓✗	470	cost = boost::make_shared<crocoddyl::CostModelResidual>(
171	✓✗	470	state, activation_factory.create(activation_type, 6),
172	✓✗	470	boost::make_shared<crocoddyl::ResidualModelFrameVelocity>(
173			state, frame_index, pinocchio::Motion::Random(),
174	✓✗	470	pinocchio::ReferenceFrame::LOCAL, nu));
175		235	break;
176			default:
177			throw_pretty(__FILE__ ": Wrong CostModelTypes::Type given");
178			break;
179			}
180		8284	return cost;
181			}
182
183		45	boost::shared_ptr<crocoddyl::CostModelAbstract> CostModelFactory::create(
184			CostModelNoFFTypes::Type cost_type,
185			ActivationModelTypes::Type activation_type, std::size_t nu) const {
186	✓✗	90	StateModelFactory state_factory;
187	✓✗	90	ActivationModelFactory activation_factory;
188		45	boost::shared_ptr<crocoddyl::CostModelAbstract> cost;
189			boost::shared_ptr<crocoddyl::StateMultibody> state =
190			boost::static_pointer_cast<crocoddyl::StateMultibody>(
191	✓✗	90	state_factory.create(StateModelTypes::StateMultibody_TalosArm));
192	✓✗	45	if (nu == std::numeric_limits<std::size_t>::max()) {
193		45	nu = state->get_nv();
194			}
195
196	✓✗	45	switch (cost_type) {
197		45	case CostModelNoFFTypes::CostModelResidualControlGrav:
198	✓✗	90	cost = boost::make_shared<crocoddyl::CostModelResidual>(
199	✓✗	90	state, activation_factory.create(activation_type, state->get_nv()),
200	✓✗	135	boost::make_shared<ResidualModelControlGrav>(state, nu));
201		45	break;
202			default:
203			throw_pretty(__FILE__ ": Wrong CostModelTypes::Type given");
204			break;
205			}
206		90	return cost;
207			}
208
209			#ifdef PINOCCHIO_WITH_HPP_FCL
210		10	boost::shared_ptr<crocoddyl::CostModelAbstract> CostModelFactory::create(
211			CostModelCollisionTypes::Type cost_type, StateModelTypes::Type state_type,
212			std::size_t nu) const {
213	✓✗	20	StateModelFactory state_factory;
214		10	boost::shared_ptr<crocoddyl::CostModelAbstract> cost;
215			boost::shared_ptr<crocoddyl::StateMultibody> state =
216			boost::static_pointer_cast<crocoddyl::StateMultibody>(
217	✓✗	20	state_factory.create(state_type));
218	✓✗	10	if (nu == std::numeric_limits<std::size_t>::max()) {
219		10	nu = state->get_nv();
220			}
221		10	pinocchio::FrameIndex frame_index = state->get_pinocchio()->frames.size() - 1;
222	✓✗	10	pinocchio::SE3 frame_SE3 = pinocchio::SE3::Random();
223	✓✗	10	pinocchio::SE3 frame_SE3_obstacle = pinocchio::SE3::Random();
224	✓✗✓✗	10	double alpha = fabs(Eigen::VectorXd::Random(1)[0]);
225	✓✗✓✗	10	double beta = fabs(Eigen::VectorXd::Random(1)[0]);
226
227			boost::shared_ptr<pinocchio::GeometryModel> geometry =
228	✓✗✓✗	20	boost::make_shared<pinocchio::GeometryModel>(pinocchio::GeometryModel());
229			pinocchio::GeomIndex ig_frame =
230	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	30	geometry->addGeometryObject(pinocchio::GeometryObject(
231			"frame", frame_index,
232		10	state->get_pinocchio()->frames[frame_index].parent,
233	✓✗✓✗ ✓✗	20	CollisionGeometryPtr(new hpp::fcl::Capsule(0, alpha)), frame_SE3));
234			pinocchio::GeomIndex ig_obs =
235	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	30	geometry->addGeometryObject(pinocchio::GeometryObject(
236		10	"obs", state->get_pinocchio()->getFrameId("universe"),
237		10	state->get_pinocchio()
238	✓✗✓✗	30	->frames[state->get_pinocchio()->getFrameId("universe")]
239			.parent,
240	✓✗✓✗ ✓✗	20	CollisionGeometryPtr(new hpp::fcl::Capsule(0, beta)),
241			frame_SE3_obstacle));
242	✓✗✓✗	10	geometry->addCollisionPair(pinocchio::CollisionPair(ig_frame, ig_obs));
243
244	✓✗	10	switch (cost_type) {
245		10	case CostModelCollisionTypes::CostModelResidualPairCollision:
246	✓✗	20	cost = boost::make_shared<crocoddyl::CostModelResidual>(
247	✓✗	20	state, boost::make_shared<crocoddyl::ActivationModelQuad>(3),
248		10	boost::make_shared<crocoddyl::ResidualModelPairCollision>(
249			state, nu, geometry, 0,
250	✓✗	20	state->get_pinocchio()->frames[frame_index].parent));
251		10	break;
252			default:
253			throw_pretty(__FILE__ ": Wrong CostModelTypes::Type given");
254			break;
255			}
256		20	return cost;
257			}
258			#endif // PINOCCHIO_WITH_HPP_FCL
259
260		72	boost::shared_ptr<crocoddyl::CostModelAbstract> create_random_cost(
261			StateModelTypes::Type state_type, std::size_t nu) {
262			static bool once = true;
263	✓✓	72	if (once) {
264		1	srand((unsigned)time(NULL));
265		1	once = false;
266			}
267
268	✓✗	144	CostModelFactory factory;
269			CostModelTypes::Type rand_type = static_cast<CostModelTypes::Type>(
270		72	rand() % CostModelTypes::NbCostModelTypes);
271			return factory.create(rand_type, state_type,
272	✓✗	144	ActivationModelTypes::ActivationModelQuad, nu);
273			}
274
275			} // namespace unittest
276			} // namespace crocoddyl