Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/factory/action.cpp	Lines:	145	154	94.2 %
Date:	2024-02-13 11:12:33	Branches:	144	292	49.3 %


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

#include "action.hpp"

#include "cost.hpp"
#include "crocoddyl/core/actions/lqr.hpp"
#include "crocoddyl/core/actions/unicycle.hpp"
#include "crocoddyl/core/activations/quadratic-barrier.hpp"
#include "crocoddyl/core/activations/quadratic.hpp"
#include "crocoddyl/core/costs/residual.hpp"
#include "crocoddyl/core/utils/exception.hpp"
#include "crocoddyl/multibody/impulses/impulse-3d.hpp"
#include "crocoddyl/multibody/impulses/impulse-6d.hpp"
#include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
#include "crocoddyl/multibody/residuals/contact-force.hpp"
#include "crocoddyl/multibody/residuals/contact-friction-cone.hpp"
#include "crocoddyl/multibody/residuals/contact-wrench-cone.hpp"
#include "impulse.hpp"

namespace crocoddyl {
namespace unittest {

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

std::ostream& operator<<(std::ostream& os, ActionModelTypes::Type type) {
  switch (type) {
    case ActionModelTypes::ActionModelUnicycle:
      os << "ActionModelUnicycle";
      break;
    case ActionModelTypes::ActionModelLQRDriftFree:
      os << "ActionModelLQRDriftFree";
      break;
    case ActionModelTypes::ActionModelLQR:
      os << "ActionModelLQR";
      break;
    case ActionModelTypes::ActionModelImpulseFwdDynamics_HyQ:
      os << "ActionModelImpulseFwdDynamics_HyQ";
      break;
    case ActionModelTypes::ActionModelImpulseFwdDynamics_Talos:
      os << "ActionModelImpulseFwdDynamics_Talos";
      break;
    case ActionModelTypes::NbActionModelTypes:
      os << "NbActionModelTypes";
      break;
    default:
      break;
  }
  return os;
}

ActionModelFactory::ActionModelFactory() {}
ActionModelFactory::~ActionModelFactory() {}

boost::shared_ptr<crocoddyl::ActionModelAbstract> ActionModelFactory::create(
    ActionModelTypes::Type type, bool secondInstance) const {
  boost::shared_ptr<crocoddyl::ActionModelAbstract> action;
  switch (type) {
    case ActionModelTypes::ActionModelUnicycle:
      action = boost::make_shared<crocoddyl::ActionModelUnicycle>();
      break;
    case ActionModelTypes::ActionModelLQRDriftFree:
      if (secondInstance) {
        action = boost::make_shared<crocoddyl::ActionModelLQR>(8, 4, true);
      } else {
        action = boost::make_shared<crocoddyl::ActionModelLQR>(8, 2, true);
      }
      break;
    case ActionModelTypes::ActionModelLQR:
      if (secondInstance) {
        action = boost::make_shared<crocoddyl::ActionModelLQR>(8, 4, false);
      } else {
        action = boost::make_shared<crocoddyl::ActionModelLQR>(8, 2, false);
      }
      break;
    case ActionModelTypes::ActionModelImpulseFwdDynamics_HyQ:
      action = create_impulseFwdDynamics(StateModelTypes::StateMultibody_HyQ);
      break;
    case ActionModelTypes::ActionModelImpulseFwdDynamics_Talos:
      action = create_impulseFwdDynamics(StateModelTypes::StateMultibody_Talos);
      break;
    default:
      throw_pretty(__FILE__ ": Wrong ActionModelTypes::Type given");
      break;
  }
  return action;
}

boost::shared_ptr<crocoddyl::ActionModelImpulseFwdDynamics>
ActionModelFactory::create_impulseFwdDynamics(
    StateModelTypes::Type state_type) const {
  boost::shared_ptr<crocoddyl::ActionModelImpulseFwdDynamics> action;
  boost::shared_ptr<crocoddyl::StateMultibody> state;
  boost::shared_ptr<crocoddyl::ImpulseModelMultiple> impulse;
  boost::shared_ptr<crocoddyl::CostModelSum> cost;
  state = boost::static_pointer_cast<crocoddyl::StateMultibody>(
      StateModelFactory().create(state_type));
  impulse = boost::make_shared<crocoddyl::ImpulseModelMultiple>(state);
  cost = boost::make_shared<crocoddyl::CostModelSum>(state, 0);
  double r_coeff = 0.;  // TODO(cmastall): random_real_in_range(1e-16, 1e-2);
  double damping = 0.;  // TODO(cmastall): random_real_in_range(1e-16, 1e-2);

  Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
  pinocchio::Force force = pinocchio::Force::Zero();
  crocoddyl::FrictionCone friction_cone(R, 0.8, 4, false);
  crocoddyl::WrenchCone wrench_cone(R, 0.8, Eigen::Vector2d(0.1, 0.1), 4,
                                    false);
  crocoddyl::ActivationBounds friction_bounds(friction_cone.get_lb(),
                                              friction_cone.get_ub());
  crocoddyl::ActivationBounds wrench_bounds(wrench_cone.get_lb(),
                                            wrench_cone.get_ub());
  boost::shared_ptr<crocoddyl::ActivationModelAbstract> friction_activation =
      boost::make_shared<crocoddyl::ActivationModelQuadraticBarrier>(
          friction_bounds);
  boost::shared_ptr<crocoddyl::ActivationModelAbstract> wrench_activation =
      boost::make_shared<crocoddyl::ActivationModelQuadraticBarrier>(
          wrench_bounds);

  switch (state_type) {
    case StateModelTypes::StateMultibody_HyQ:
      impulse->addImpulse("lf", ImpulseModelFactory().create(
                                    ImpulseModelTypes::ImpulseModel3D_LOCAL,
                                    PinocchioModelTypes::HyQ, "lf_foot"));
      impulse->addImpulse("rf", ImpulseModelFactory().create(
                                    ImpulseModelTypes::ImpulseModel3D_WORLD,
                                    PinocchioModelTypes::HyQ, "rf_foot"));
      impulse->addImpulse("lh", ImpulseModelFactory().create(
                                    ImpulseModelTypes::ImpulseModel3D_LWA,
                                    PinocchioModelTypes::HyQ, "lh_foot"));
      impulse->addImpulse("rh", ImpulseModelFactory().create(
                                    ImpulseModelTypes::ImpulseModel3D_LOCAL,
                                    PinocchioModelTypes::HyQ, "rh_foot"));

      // friction cone
      cost->addCost(
          "lf_cone",
          boost::make_shared<crocoddyl::CostModelResidual>(
              state, friction_activation,
              boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
                  state, state->get_pinocchio()->getFrameId("lf_foot"),
                  friction_cone, 0)),
          0.1);
      cost->addCost(
          "rf_cone",
          boost::make_shared<crocoddyl::CostModelResidual>(
              state, friction_activation,
              boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
                  state, state->get_pinocchio()->getFrameId("rf_foot"),
                  friction_cone, 0)),
          0.1);
      cost->addCost(
          "lh_cone",
          boost::make_shared<crocoddyl::CostModelResidual>(
              state, friction_activation,
              boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
                  state, state->get_pinocchio()->getFrameId("lh_foot"),
                  friction_cone, 0)),
          0.1);
      cost->addCost(
          "rh_cone",
          boost::make_shared<crocoddyl::CostModelResidual>(
              state, friction_activation,
              boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
                  state, state->get_pinocchio()->getFrameId("rh_foot"),
                  friction_cone, 0)),
          0.1);
      // force regularization
      cost->addCost(
          "lf_forceReg",
          boost::make_shared<crocoddyl::CostModelResidual>(
              state, boost::make_shared<crocoddyl::ResidualModelContactForce>(
                         state, state->get_pinocchio()->getFrameId("lf_foot"),
                         force, 3, 0)),
          0.1);
      cost->addCost(
          "rf_forceReg",
          boost::make_shared<crocoddyl::CostModelResidual>(
              state, boost::make_shared<crocoddyl::ResidualModelContactForce>(
                         state, state->get_pinocchio()->getFrameId("rf_foot"),
                         force, 3, 0)),
          0.1);
      cost->addCost(
          "lh_forceReg",
          boost::make_shared<crocoddyl::CostModelResidual>(
              state, boost::make_shared<crocoddyl::ResidualModelContactForce>(
                         state, state->get_pinocchio()->getFrameId("lh_foot"),
                         force, 3, 0)),
          0.1);
      cost->addCost(
          "rh_forceReg",
          boost::make_shared<crocoddyl::CostModelResidual>(
              state, boost::make_shared<crocoddyl::ResidualModelContactForce>(
                         state, state->get_pinocchio()->getFrameId("rh_foot"),
                         force, 3, 0)),
          0.1);
      break;
    case StateModelTypes::StateMultibody_Talos:
      impulse->addImpulse("lf",
                          ImpulseModelFactory().create(
                              ImpulseModelTypes::ImpulseModel6D_LOCAL,
                              PinocchioModelTypes::Talos, "left_sole_link"));
      impulse->addImpulse("rf",
                          ImpulseModelFactory().create(
                              ImpulseModelTypes::ImpulseModel6D_WORLD,
                              PinocchioModelTypes::Talos, "right_sole_link"));

      // friction / wrench cone
      cost->addCost(
          "lf_cone",
          boost::make_shared<crocoddyl::CostModelResidual>(
              state, friction_activation,
              boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
                  state, state->get_pinocchio()->getFrameId("left_sole_link"),
                  friction_cone, 0)),
          0.01);
      cost->addCost(
          "rf_cone",
          boost::make_shared<crocoddyl::CostModelResidual>(
              state, wrench_activation,
              boost::make_shared<crocoddyl::ResidualModelContactWrenchCone>(
                  state, state->get_pinocchio()->getFrameId("right_sole_link"),
                  wrench_cone, 0)),
          0.01);
      // force regularization
      cost->addCost(
          "lf_forceReg",
          boost::make_shared<crocoddyl::CostModelResidual>(
              state,
              boost::make_shared<crocoddyl::ResidualModelContactForce>(
                  state, state->get_pinocchio()->getFrameId("left_sole_link"),
                  force, 6, 0)),
          0.01);
      cost->addCost(
          "rf_forceReg",
          boost::make_shared<crocoddyl::CostModelResidual>(
              state,
              boost::make_shared<crocoddyl::ResidualModelContactForce>(
                  state, state->get_pinocchio()->getFrameId("right_sole_link"),
                  force, 6, 0)),
          0.01);
      break;
    default:
      throw_pretty(__FILE__ ": Wrong StateModelTypes::Type given");
      break;
  }
  cost->addCost("state",
                CostModelFactory().create(
                    CostModelTypes::CostModelResidualState, state_type,
                    ActivationModelTypes::ActivationModelQuad, 0),
                0.1);
  action = boost::make_shared<crocoddyl::ActionModelImpulseFwdDynamics>(
      state, impulse, cost, r_coeff, damping, true);
  return action;
}

}  // 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, LAAS-CNRS,
5			// Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#include "action.hpp"
11
12			#include "cost.hpp"
13			#include "crocoddyl/core/actions/lqr.hpp"
14			#include "crocoddyl/core/actions/unicycle.hpp"
15			#include "crocoddyl/core/activations/quadratic-barrier.hpp"
16			#include "crocoddyl/core/activations/quadratic.hpp"
17			#include "crocoddyl/core/costs/residual.hpp"
18			#include "crocoddyl/core/utils/exception.hpp"
19			#include "crocoddyl/multibody/impulses/impulse-3d.hpp"
20			#include "crocoddyl/multibody/impulses/impulse-6d.hpp"
21			#include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
22			#include "crocoddyl/multibody/residuals/contact-force.hpp"
23			#include "crocoddyl/multibody/residuals/contact-friction-cone.hpp"
24			#include "crocoddyl/multibody/residuals/contact-wrench-cone.hpp"
25			#include "impulse.hpp"
26
27			namespace crocoddyl {
28			namespace unittest {
29
30			const std::vector<ActionModelTypes::Type> ActionModelTypes::all(
31			ActionModelTypes::init_all());
32
33		45	std::ostream& operator<<(std::ostream& os, ActionModelTypes::Type type) {
34	✓✓✓✓ ✓✗✗	45	switch (type) {
35		13	case ActionModelTypes::ActionModelUnicycle:
36		13	os << "ActionModelUnicycle";
37		13	break;
38		13	case ActionModelTypes::ActionModelLQRDriftFree:
39		13	os << "ActionModelLQRDriftFree";
40		13	break;
41		13	case ActionModelTypes::ActionModelLQR:
42		13	os << "ActionModelLQR";
43		13	break;
44		3	case ActionModelTypes::ActionModelImpulseFwdDynamics_HyQ:
45		3	os << "ActionModelImpulseFwdDynamics_HyQ";
46		3	break;
47		3	case ActionModelTypes::ActionModelImpulseFwdDynamics_Talos:
48		3	os << "ActionModelImpulseFwdDynamics_Talos";
49		3	break;
50			case ActionModelTypes::NbActionModelTypes:
51			os << "NbActionModelTypes";
52			break;
53			default:
54			break;
55			}
56		45	return os;
57			}
58
59		157	ActionModelFactory::ActionModelFactory() {}
60		157	ActionModelFactory::~ActionModelFactory() {}
61
62		115	boost::shared_ptr<crocoddyl::ActionModelAbstract> ActionModelFactory::create(
63			ActionModelTypes::Type type, bool secondInstance) const {
64		115	boost::shared_ptr<crocoddyl::ActionModelAbstract> action;
65	✓✓✓✓ ✓✗	115	switch (type) {
66		31	case ActionModelTypes::ActionModelUnicycle:
67	✓✗	31	action = boost::make_shared<crocoddyl::ActionModelUnicycle>();
68		31	break;
69		31	case ActionModelTypes::ActionModelLQRDriftFree:
70	✓✓	31	if (secondInstance) {
71	✓✗	12	action = boost::make_shared<crocoddyl::ActionModelLQR>(8, 4, true);
72			} else {
73	✓✗	19	action = boost::make_shared<crocoddyl::ActionModelLQR>(8, 2, true);
74			}
75		31	break;
76		31	case ActionModelTypes::ActionModelLQR:
77	✓✓	31	if (secondInstance) {
78	✓✗	12	action = boost::make_shared<crocoddyl::ActionModelLQR>(8, 4, false);
79			} else {
80	✓✗	19	action = boost::make_shared<crocoddyl::ActionModelLQR>(8, 2, false);
81			}
82		31	break;
83		11	case ActionModelTypes::ActionModelImpulseFwdDynamics_HyQ:
84	✓✗	11	action = create_impulseFwdDynamics(StateModelTypes::StateMultibody_HyQ);
85		11	break;
86		11	case ActionModelTypes::ActionModelImpulseFwdDynamics_Talos:
87	✓✗	11	action = create_impulseFwdDynamics(StateModelTypes::StateMultibody_Talos);
88		11	break;
89			default:
90			throw_pretty(__FILE__ ": Wrong ActionModelTypes::Type given");
91			break;
92			}
93		115	return action;
94			}
95
96			boost::shared_ptr<crocoddyl::ActionModelImpulseFwdDynamics>
97		64	ActionModelFactory::create_impulseFwdDynamics(
98			StateModelTypes::Type state_type) const {
99		64	boost::shared_ptr<crocoddyl::ActionModelImpulseFwdDynamics> action;
100		64	boost::shared_ptr<crocoddyl::StateMultibody> state;
101		64	boost::shared_ptr<crocoddyl::ImpulseModelMultiple> impulse;
102		64	boost::shared_ptr<crocoddyl::CostModelSum> cost;
103		128	state = boost::static_pointer_cast<crocoddyl::StateMultibody>(
104	✓✗✓✗	192	StateModelFactory().create(state_type));
105	✓✗	64	impulse = boost::make_shared<crocoddyl::ImpulseModelMultiple>(state);
106	✓✗	64	cost = boost::make_shared<crocoddyl::CostModelSum>(state, 0);
107		64	double r_coeff = 0.; // TODO(cmastall): random_real_in_range(1e-16, 1e-2);
108		64	double damping = 0.; // TODO(cmastall): random_real_in_range(1e-16, 1e-2);
109
110	✓✗✓✗	64	Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
111	✓✗	64	pinocchio::Force force = pinocchio::Force::Zero();
112	✓✗	128	crocoddyl::FrictionCone friction_cone(R, 0.8, 4, false);
113	✓✗	64	crocoddyl::WrenchCone wrench_cone(R, 0.8, Eigen::Vector2d(0.1, 0.1), 4,
114	✓✗	192	false);
115		64	crocoddyl::ActivationBounds friction_bounds(friction_cone.get_lb(),
116	✓✗	192	friction_cone.get_ub());
117		64	crocoddyl::ActivationBounds wrench_bounds(wrench_cone.get_lb(),
118	✓✗	192	wrench_cone.get_ub());
119			boost::shared_ptr<crocoddyl::ActivationModelAbstract> friction_activation =
120	✓✗	64	boost::make_shared<crocoddyl::ActivationModelQuadraticBarrier>(
121		128	friction_bounds);
122			boost::shared_ptr<crocoddyl::ActivationModelAbstract> wrench_activation =
123	✓✗	64	boost::make_shared<crocoddyl::ActivationModelQuadraticBarrier>(
124		128	wrench_bounds);
125
126	✓✓✗	64	switch (state_type) {
127		23	case StateModelTypes::StateMultibody_HyQ:
128	✓✗✓✗ ✓✗✓✗ ✓✗	23	impulse->addImpulse("lf", ImpulseModelFactory().create(
129			ImpulseModelTypes::ImpulseModel3D_LOCAL,
130			PinocchioModelTypes::HyQ, "lf_foot"));
131	✓✗✓✗ ✓✗✓✗ ✓✗	23	impulse->addImpulse("rf", ImpulseModelFactory().create(
132			ImpulseModelTypes::ImpulseModel3D_WORLD,
133			PinocchioModelTypes::HyQ, "rf_foot"));
134	✓✗✓✗ ✓✗✓✗ ✓✗	23	impulse->addImpulse("lh", ImpulseModelFactory().create(
135			ImpulseModelTypes::ImpulseModel3D_LWA,
136			PinocchioModelTypes::HyQ, "lh_foot"));
137	✓✗✓✗ ✓✗✓✗ ✓✗	23	impulse->addImpulse("rh", ImpulseModelFactory().create(
138			ImpulseModelTypes::ImpulseModel3D_LOCAL,
139			PinocchioModelTypes::HyQ, "rh_foot"));
140
141			// friction cone
142	✓✗✓✗	46	cost->addCost(
143			"lf_cone",
144	✓✗	46	boost::make_shared<crocoddyl::CostModelResidual>(
145			state, friction_activation,
146		23	boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
147	✓✗✓✗ ✓✗	46	state, state->get_pinocchio()->getFrameId("lf_foot"),
148		23	friction_cone, 0)),
149			0.1);
150	✓✗✓✗	46	cost->addCost(
151			"rf_cone",
152	✓✗	46	boost::make_shared<crocoddyl::CostModelResidual>(
153			state, friction_activation,
154		23	boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
155	✓✗✓✗ ✓✗	46	state, state->get_pinocchio()->getFrameId("rf_foot"),
156		23	friction_cone, 0)),
157			0.1);
158	✓✗✓✗	46	cost->addCost(
159			"lh_cone",
160	✓✗	46	boost::make_shared<crocoddyl::CostModelResidual>(
161			state, friction_activation,
162		23	boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
163	✓✗✓✗ ✓✗	46	state, state->get_pinocchio()->getFrameId("lh_foot"),
164		23	friction_cone, 0)),
165			0.1);
166	✓✗✓✗	46	cost->addCost(
167			"rh_cone",
168	✓✗	46	boost::make_shared<crocoddyl::CostModelResidual>(
169			state, friction_activation,
170		23	boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
171	✓✗✓✗ ✓✗	46	state, state->get_pinocchio()->getFrameId("rh_foot"),
172		23	friction_cone, 0)),
173			0.1);
174			// force regularization
175	✓✗✓✗	46	cost->addCost(
176			"lf_forceReg",
177	✓✗	46	boost::make_shared<crocoddyl::CostModelResidual>(
178		23	state, boost::make_shared<crocoddyl::ResidualModelContactForce>(
179	✓✗✓✗ ✓✗	46	state, state->get_pinocchio()->getFrameId("lf_foot"),
180		23	force, 3, 0)),
181			0.1);
182	✓✗✓✗	46	cost->addCost(
183			"rf_forceReg",
184	✓✗	46	boost::make_shared<crocoddyl::CostModelResidual>(
185		23	state, boost::make_shared<crocoddyl::ResidualModelContactForce>(
186	✓✗✓✗ ✓✗	46	state, state->get_pinocchio()->getFrameId("rf_foot"),
187		23	force, 3, 0)),
188			0.1);
189	✓✗✓✗	46	cost->addCost(
190			"lh_forceReg",
191	✓✗	46	boost::make_shared<crocoddyl::CostModelResidual>(
192		23	state, boost::make_shared<crocoddyl::ResidualModelContactForce>(
193	✓✗✓✗ ✓✗	46	state, state->get_pinocchio()->getFrameId("lh_foot"),
194		23	force, 3, 0)),
195			0.1);
196	✓✗✓✗	46	cost->addCost(
197			"rh_forceReg",
198	✓✗	46	boost::make_shared<crocoddyl::CostModelResidual>(
199		23	state, boost::make_shared<crocoddyl::ResidualModelContactForce>(
200	✓✗✓✗ ✓✗	46	state, state->get_pinocchio()->getFrameId("rh_foot"),
201		23	force, 3, 0)),
202			0.1);
203		23	break;
204		41	case StateModelTypes::StateMultibody_Talos:
205	✓✗✓✗	82	impulse->addImpulse("lf",
206	✓✗✓✗ ✓✗	82	ImpulseModelFactory().create(
207			ImpulseModelTypes::ImpulseModel6D_LOCAL,
208			PinocchioModelTypes::Talos, "left_sole_link"));
209	✓✗✓✗	82	impulse->addImpulse("rf",
210	✓✗✓✗ ✓✗	82	ImpulseModelFactory().create(
211			ImpulseModelTypes::ImpulseModel6D_WORLD,
212			PinocchioModelTypes::Talos, "right_sole_link"));
213
214			// friction / wrench cone
215	✓✗✓✗	82	cost->addCost(
216			"lf_cone",
217	✓✗	82	boost::make_shared<crocoddyl::CostModelResidual>(
218			state, friction_activation,
219		41	boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
220	✓✗✓✗ ✓✗	82	state, state->get_pinocchio()->getFrameId("left_sole_link"),
221		41	friction_cone, 0)),
222			0.01);
223	✓✗✓✗	82	cost->addCost(
224			"rf_cone",
225	✓✗	82	boost::make_shared<crocoddyl::CostModelResidual>(
226			state, wrench_activation,
227		41	boost::make_shared<crocoddyl::ResidualModelContactWrenchCone>(
228	✓✗✓✗ ✓✗	82	state, state->get_pinocchio()->getFrameId("right_sole_link"),
229		41	wrench_cone, 0)),
230			0.01);
231			// force regularization
232	✓✗✓✗	82	cost->addCost(
233			"lf_forceReg",
234	✓✗	82	boost::make_shared<crocoddyl::CostModelResidual>(
235			state,
236		41	boost::make_shared<crocoddyl::ResidualModelContactForce>(
237	✓✗✓✗ ✓✗	82	state, state->get_pinocchio()->getFrameId("left_sole_link"),
238		41	force, 6, 0)),
239			0.01);
240	✓✗✓✗	82	cost->addCost(
241			"rf_forceReg",
242	✓✗	82	boost::make_shared<crocoddyl::CostModelResidual>(
243			state,
244		41	boost::make_shared<crocoddyl::ResidualModelContactForce>(
245	✓✗✓✗ ✓✗	82	state, state->get_pinocchio()->getFrameId("right_sole_link"),
246		41	force, 6, 0)),
247			0.01);
248		41	break;
249			default:
250			throw_pretty(__FILE__ ": Wrong StateModelTypes::Type given");
251			break;
252			}
253	✓✗✓✗	128	cost->addCost("state",
254	✓✗✓✗	128	CostModelFactory().create(
255			CostModelTypes::CostModelResidualState, state_type,
256			ActivationModelTypes::ActivationModelQuad, 0),
257			0.1);
258		64	action = boost::make_shared<crocoddyl::ActionModelImpulseFwdDynamics>(
259	✓✗	64	state, impulse, cost, r_coeff, damping, true);
260		128	return action;
261			}
262
263			} // namespace unittest
264			} // namespace crocoddyl