GCC Code Coverage Report

Directory:	./
File:	unittest/factory/cost.cpp
Date:	2025-06-03 08:14:12

	Total	Coverage
Lines:	124	0.0%
Functions:	7	0.0%
Branches:	132	0.0%

  
      Line
      Branch
      Exec
      Source
    
      ///////////////////////////////////////////////////////////////////////////////
    
      // BSD 3-Clause License
    
      //
    
      // Copyright (C) 2019-2025, 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/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"
    
      #ifdef CROCODDYL_WITH_PAIR_COLLISION
    
      #include "crocoddyl/multibody/residuals/pair-collision.hpp"
    
      #endif  // CROCODDYL_WITH_PAIR_COLLISION
    
      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
    
      #ifdef CROCODDYL_WITH_PAIR_COLLISION
    
      const std::vector<CostModelCollisionTypes::Type> CostModelCollisionTypes::all(
    
          CostModelCollisionTypes::init_all());
    
      #endif  // CROCODDYL_WITH_PAIR_COLLISION
    
      #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
    
      #ifdef CROCODDYL_WITH_PAIR_COLLISION
    
      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  // CROCODDYL_WITH_PAIR_COLLISION
    
      #endif  // PINOCCHIO_WITH_HPP_FCL
    
      ✗
      CostModelFactory::CostModelFactory() {}
    
      ✗
      CostModelFactory::~CostModelFactory() {}
    
      ✗
      std::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;
    
      ✗
        std::shared_ptr<crocoddyl::CostModelAbstract> cost;
    
        std::shared_ptr<crocoddyl::StateMultibody> state =
    
            std::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 = std::make_shared<crocoddyl::CostModelResidual>(
    
      ✗
                state, activation_factory.create(activation_type, state->get_ndx()),
    
      ✗
                std::make_shared<crocoddyl::ResidualModelState>(state, state->zero(),
    
      ✗
                                                                nu));
    
      ✗
            break;
    
      ✗
          case CostModelTypes::CostModelResidualControl:
    
      ✗
            cost = std::make_shared<crocoddyl::CostModelResidual>(
    
      ✗
                state, activation_factory.create(activation_type, nu),
    
      ✗
                std::make_shared<crocoddyl::ResidualModelControl>(
    
      ✗
                    state, Eigen::VectorXd::Random(nu)));
    
      ✗
            break;
    
      ✗
          case CostModelTypes::CostModelResidualCoMPosition:
    
      ✗
            cost = std::make_shared<crocoddyl::CostModelResidual>(
    
      ✗
                state, activation_factory.create(activation_type, 3),
    
      ✗
                std::make_shared<crocoddyl::ResidualModelCoMPosition>(
    
      ✗
                    state, Eigen::Vector3d::Random(), nu));
    
      ✗
            break;
    
          // case CostModelTypes::CostModelResidualCentroidalMomentum:
    
          //   cost = std::make_shared<crocoddyl::CostModelResidual>(
    
          //       state,
    
          //       std::make_shared<crocoddyl::ResidualModelCentroidalMomentum>(state,
    
          //       Vector6d::Random(), nu), activation_factory.create(activation_type,
    
          //       6));
    
          //   break;
    
      ✗
          case CostModelTypes::CostModelResidualFramePlacement:
    
      ✗
            cost = std::make_shared<crocoddyl::CostModelResidual>(
    
      ✗
                state, activation_factory.create(activation_type, 6),
    
      ✗
                std::make_shared<crocoddyl::ResidualModelFramePlacement>(
    
      ✗
                    state, frame_index, frame_SE3, nu));
    
      ✗
            break;
    
      ✗
          case CostModelTypes::CostModelResidualFrameRotation:
    
      ✗
            cost = std::make_shared<crocoddyl::CostModelResidual>(
    
      ✗
                state, activation_factory.create(activation_type, 3),
    
      ✗
                std::make_shared<crocoddyl::ResidualModelFrameRotation>(
    
      ✗
                    state, frame_index, frame_SE3.rotation(), nu));
    
      ✗
            break;
    
      ✗
          case CostModelTypes::CostModelResidualFrameTranslation:
    
      ✗
            cost = std::make_shared<crocoddyl::CostModelResidual>(
    
      ✗
                state, activation_factory.create(activation_type, 3),
    
      ✗
                std::make_shared<crocoddyl::ResidualModelFrameTranslation>(
    
      ✗
                    state, frame_index, frame_SE3.translation(), nu));
    
      ✗
            break;
    
      ✗
          case CostModelTypes::CostModelResidualFrameVelocity:
    
      ✗
            cost = std::make_shared<crocoddyl::CostModelResidual>(
    
      ✗
                state, activation_factory.create(activation_type, 6),
    
      ✗
                std::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;
    
      ✗
      }
    
      ✗
      std::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;
    
      ✗
        std::shared_ptr<crocoddyl::CostModelAbstract> cost;
    
        std::shared_ptr<crocoddyl::StateMultibody> state =
    
            std::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 = std::make_shared<crocoddyl::CostModelResidual>(
    
      ✗
                state, activation_factory.create(activation_type, state->get_nv()),
    
      ✗
                std::make_shared<ResidualModelControlGrav>(state, nu));
    
      ✗
            break;
    
      ✗
          default:
    
      ✗
            throw_pretty(__FILE__ ": Wrong CostModelTypes::Type given");
    
            break;
    
        }
    
      ✗
        return cost;
    
      ✗
      }
    
      #ifdef PINOCCHIO_WITH_HPP_FCL
    
      #ifdef CROCODDYL_WITH_PAIR_COLLISION
    
      std::shared_ptr<crocoddyl::CostModelAbstract> CostModelFactory::create(
    
          CostModelCollisionTypes::Type cost_type, StateModelTypes::Type state_type,
    
          std::size_t nu) const {
    
        StateModelFactory state_factory;
    
        std::shared_ptr<crocoddyl::CostModelAbstract> cost;
    
        std::shared_ptr<crocoddyl::StateMultibody> state =
    
            std::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]);
    
        std::shared_ptr<pinocchio::GeometryModel> geometry =
    
            std::make_shared<pinocchio::GeometryModel>(pinocchio::GeometryModel());
    
        pinocchio::GeomIndex ig_frame =
    
            geometry->addGeometryObject(pinocchio::GeometryObject(
    
                "frame", frame_index,
    
                state->get_pinocchio()->frames[frame_index].parentJoint,
    
                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")]
    
                    .parentJoint,
    
                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 = std::make_shared<crocoddyl::CostModelResidual>(
    
                state, std::make_shared<crocoddyl::ActivationModelQuad>(3),
    
                std::make_shared<crocoddyl::ResidualModelPairCollision>(
    
                    state, nu, geometry, 0,
    
                    state->get_pinocchio()->frames[frame_index].parentJoint));
    
            break;
    
          default:
    
            throw_pretty(__FILE__ ": Wrong CostModelTypes::Type given");
    
            break;
    
        }
    
        return cost;
    
      }
    
      #endif  // CROCODDYL_WITH_PAIR_COLLISION
    
      #endif  // PINOCCHIO_WITH_HPP_FCL
    
      ✗
      std::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

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