Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/factory/impulse_constraint.cpp	Lines:	115	126	91.3 %
Date:	2024-02-13 11:12:33	Branches:	89	180	49.4 %


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

#include "impulse_constraint.hpp"

#include "cost.hpp"
#include "crocoddyl/core/constraints/residual.hpp"
#include "crocoddyl/core/utils/exception.hpp"
#include "crocoddyl/multibody/actions/impulse-fwddyn.hpp"
#include "crocoddyl/multibody/residuals/contact-control-gravity.hpp"
#include "crocoddyl/multibody/residuals/contact-cop-position.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 "crocoddyl/multibody/residuals/impulse-com.hpp"
#include "impulse.hpp"

namespace crocoddyl {
namespace unittest {

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

std::ostream &operator<<(std::ostream &os,
                         ImpulseConstraintModelTypes::Type type) {
  switch (type) {
    case ImpulseConstraintModelTypes::CostModelResidualImpulseCoMEquality:
      os << "CostModelResidualImpulseCoMEquality";
      break;
    case ImpulseConstraintModelTypes::
        ConstraintModelResidualImpulseForceEquality:
      os << "ConstraintModelResidualImpulseForceEquality";
      break;
    case ImpulseConstraintModelTypes::
        ConstraintModelResidualImpulseCoPPositionInequality:
      os << "ConstraintModelResidualImpulseCoPPositionInequality";
      break;
    case ImpulseConstraintModelTypes::
        ConstraintModelResidualImpulseFrictionConeInequality:
      os << "ConstraintModelResidualImpulseFrictionConeInequality";
      break;
    case ImpulseConstraintModelTypes::
        ConstraintModelResidualImpulseWrenchConeInequality:
      os << "ConstraintModelResidualImpulseWrenchConeInequality";
      break;
    case ImpulseConstraintModelTypes::NbImpulseConstraintModelTypes:
      os << "NbImpulseConstraintModelTypes";
      break;
    default:
      break;
  }
  return os;
}

ImpulseConstraintModelFactory::ImpulseConstraintModelFactory() {}
ImpulseConstraintModelFactory::~ImpulseConstraintModelFactory() {}

boost::shared_ptr<crocoddyl::ActionModelAbstract>
ImpulseConstraintModelFactory::create(
    ImpulseConstraintModelTypes::Type constraint_type,
    PinocchioModelTypes::Type model_type) const {
  // Identify the state type given the model type
  StateModelTypes::Type state_type;
  PinocchioModelFactory model_factory(model_type);
  switch (model_type) {
    case PinocchioModelTypes::Talos:
      state_type = StateModelTypes::StateMultibody_Talos;
      break;
    case PinocchioModelTypes::RandomHumanoid:
      state_type = StateModelTypes::StateMultibody_RandomHumanoid;
      break;
    case PinocchioModelTypes::HyQ:
      state_type = StateModelTypes::StateMultibody_HyQ;
      break;
    default:
      throw_pretty(__FILE__ ": Wrong PinocchioModelTypes::Type given");
      break;
  }

  // Create impulse impulse diff-action model with standard cost functions
  boost::shared_ptr<crocoddyl::ActionModelImpulseFwdDynamics> action;
  boost::shared_ptr<crocoddyl::StateMultibody> state;
  boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation;
  boost::shared_ptr<crocoddyl::ImpulseModelMultiple> impulse;
  boost::shared_ptr<crocoddyl::CostModelSum> cost;
  boost::shared_ptr<crocoddyl::ConstraintModelManager> constraint;
  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);
  constraint = boost::make_shared<crocoddyl::ConstraintModelManager>(state, 0);
  std::vector<std::size_t> frame_ids = model_factory.get_frame_ids();
  std::vector<std::string> frame_names = model_factory.get_frame_names();
  // Define the impulse model
  switch (state_type) {
    case StateModelTypes::StateMultibody_Talos:
    case StateModelTypes::StateMultibody_RandomHumanoid:
      for (std::size_t i = 0; i < frame_names.size(); ++i) {
        impulse->addImpulse(frame_names[i],
                            ImpulseModelFactory().create(
                                ImpulseModelTypes::ImpulseModel6D_LOCAL,
                                model_type, frame_names[i]));
      }
      break;
    case StateModelTypes::StateMultibody_HyQ:
      for (std::size_t i = 0; i < frame_names.size(); ++i) {
        impulse->addImpulse(frame_names[i],
                            ImpulseModelFactory().create(
                                ImpulseModelTypes::ImpulseModel3D_LOCAL,
                                model_type, frame_names[i]));
      }
      break;
    default:
      throw_pretty(__FILE__ ": Wrong StateModelTypes::Type given");
      break;
  }
  // Define standard cost functions
  cost->addCost("state",
                CostModelFactory().create(
                    CostModelTypes::CostModelResidualState, state_type,
                    ActivationModelTypes::ActivationModelQuad, 0),
                0.1);

  // Define the constraint function
  Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
  crocoddyl::FrictionCone friction_cone(R, 1.);
  crocoddyl::WrenchCone wrench_cone(R, 1., Eigen::Vector2d(0.1, 0.1));
  crocoddyl::CoPSupport cop_support(R, Eigen::Vector2d(0.1, 0.1));
  Eigen::VectorXd lb, ub;
  switch (constraint_type) {
    case ImpulseConstraintModelTypes::CostModelResidualImpulseCoMEquality:
      constraint->addConstraint(
          "constraint",
          boost::make_shared<crocoddyl::ConstraintModelResidual>(
              state,
              boost::make_shared<crocoddyl::ResidualModelImpulseCoM>(state)));
      break;
    case ImpulseConstraintModelTypes::
        ConstraintModelResidualImpulseForceEquality:
      for (std::size_t i = 0; i < frame_ids.size(); ++i) {
        constraint->addConstraint(
            "constraint_" + std::to_string(i),
            boost::make_shared<crocoddyl::ConstraintModelResidual>(
                state, boost::make_shared<crocoddyl::ResidualModelContactForce>(
                           state, frame_ids[i], pinocchio::Force::Random(),
                           model_factory.get_contact_nc(), 0)));
      }
      break;
    case ImpulseConstraintModelTypes::
        ConstraintModelResidualImpulseCoPPositionInequality:
      lb = cop_support.get_lb();
      ub = cop_support.get_ub();
      for (std::size_t i = 0; i < frame_ids.size(); ++i) {
        constraint->addConstraint(
            "constraint_" + std::to_string(i),
            boost::make_shared<crocoddyl::ConstraintModelResidual>(
                state,
                boost::make_shared<crocoddyl::ResidualModelContactCoPPosition>(
                    state, frame_ids[i], cop_support, 0),
                lb, ub));
      }
      break;
    case ImpulseConstraintModelTypes::
        ConstraintModelResidualImpulseFrictionConeInequality:
      lb = friction_cone.get_lb();
      ub = friction_cone.get_ub();
      for (std::size_t i = 0; i < frame_ids.size(); ++i) {
        constraint->addConstraint(
            "constraint_" + std::to_string(i),
            boost::make_shared<crocoddyl::ConstraintModelResidual>(
                state,
                boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
                    state, frame_ids[i], friction_cone, 0),
                lb, ub));
      }
      break;
    case ImpulseConstraintModelTypes::
        ConstraintModelResidualImpulseWrenchConeInequality:
      lb = wrench_cone.get_lb();
      ub = wrench_cone.get_ub();
      for (std::size_t i = 0; i < frame_ids.size(); ++i) {
        constraint->addConstraint(
            "constraint_" + std::to_string(i),
            boost::make_shared<crocoddyl::ConstraintModelResidual>(
                state,
                boost::make_shared<crocoddyl::ResidualModelContactWrenchCone>(
                    state, frame_ids[i], wrench_cone, 0),
                lb, ub));
      }
      break;
    default:
      throw_pretty(__FILE__ ": Wrong ImpulseConstraintModelTypes::Type given");
      break;
  }

  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);
  action = boost::make_shared<crocoddyl::ActionModelImpulseFwdDynamics>(
      state, impulse, cost, constraint, 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) 2021-2023, University of Edinburgh, Heriot-Watt University,
5			// Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#include "impulse_constraint.hpp"
11
12			#include "cost.hpp"
13			#include "crocoddyl/core/constraints/residual.hpp"
14			#include "crocoddyl/core/utils/exception.hpp"
15			#include "crocoddyl/multibody/actions/impulse-fwddyn.hpp"
16			#include "crocoddyl/multibody/residuals/contact-control-gravity.hpp"
17			#include "crocoddyl/multibody/residuals/contact-cop-position.hpp"
18			#include "crocoddyl/multibody/residuals/contact-force.hpp"
19			#include "crocoddyl/multibody/residuals/contact-friction-cone.hpp"
20			#include "crocoddyl/multibody/residuals/contact-wrench-cone.hpp"
21			#include "crocoddyl/multibody/residuals/impulse-com.hpp"
22			#include "impulse.hpp"
23
24			namespace crocoddyl {
25			namespace unittest {
26
27			const std::vector<ImpulseConstraintModelTypes::Type>
28			ImpulseConstraintModelTypes::all(ImpulseConstraintModelTypes::init_all());
29
30		12	std::ostream &operator<<(std::ostream &os,
31			ImpulseConstraintModelTypes::Type type) {
32	✓✓✓✓ ✓✗✗	12	switch (type) {
33		2	case ImpulseConstraintModelTypes::CostModelResidualImpulseCoMEquality:
34		2	os << "CostModelResidualImpulseCoMEquality";
35		2	break;
36		3	case ImpulseConstraintModelTypes::
37			ConstraintModelResidualImpulseForceEquality:
38		3	os << "ConstraintModelResidualImpulseForceEquality";
39		3	break;
40		2	case ImpulseConstraintModelTypes::
41			ConstraintModelResidualImpulseCoPPositionInequality:
42		2	os << "ConstraintModelResidualImpulseCoPPositionInequality";
43		2	break;
44		3	case ImpulseConstraintModelTypes::
45			ConstraintModelResidualImpulseFrictionConeInequality:
46		3	os << "ConstraintModelResidualImpulseFrictionConeInequality";
47		3	break;
48		2	case ImpulseConstraintModelTypes::
49			ConstraintModelResidualImpulseWrenchConeInequality:
50		2	os << "ConstraintModelResidualImpulseWrenchConeInequality";
51		2	break;
52			case ImpulseConstraintModelTypes::NbImpulseConstraintModelTypes:
53			os << "NbImpulseConstraintModelTypes";
54			break;
55			default:
56			break;
57			}
58		12	return os;
59			}
60
61		12	ImpulseConstraintModelFactory::ImpulseConstraintModelFactory() {}
62		12	ImpulseConstraintModelFactory::~ImpulseConstraintModelFactory() {}
63
64			boost::shared_ptr<crocoddyl::ActionModelAbstract>
65		12	ImpulseConstraintModelFactory::create(
66			ImpulseConstraintModelTypes::Type constraint_type,
67			PinocchioModelTypes::Type model_type) const {
68			// Identify the state type given the model type
69			StateModelTypes::Type state_type;
70	✓✗	24	PinocchioModelFactory model_factory(model_type);
71	✓✓✓✗	12	switch (model_type) {
72		5	case PinocchioModelTypes::Talos:
73		5	state_type = StateModelTypes::StateMultibody_Talos;
74		5	break;
75		5	case PinocchioModelTypes::RandomHumanoid:
76		5	state_type = StateModelTypes::StateMultibody_RandomHumanoid;
77		5	break;
78		2	case PinocchioModelTypes::HyQ:
79		2	state_type = StateModelTypes::StateMultibody_HyQ;
80		2	break;
81			default:
82			throw_pretty(__FILE__ ": Wrong PinocchioModelTypes::Type given");
83			break;
84			}
85
86			// Create impulse impulse diff-action model with standard cost functions
87		12	boost::shared_ptr<crocoddyl::ActionModelImpulseFwdDynamics> action;
88		12	boost::shared_ptr<crocoddyl::StateMultibody> state;
89		12	boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation;
90		12	boost::shared_ptr<crocoddyl::ImpulseModelMultiple> impulse;
91		12	boost::shared_ptr<crocoddyl::CostModelSum> cost;
92		12	boost::shared_ptr<crocoddyl::ConstraintModelManager> constraint;
93		24	state = boost::static_pointer_cast<crocoddyl::StateMultibody>(
94	✓✗✓✗	36	StateModelFactory().create(state_type));
95	✓✗	12	impulse = boost::make_shared<crocoddyl::ImpulseModelMultiple>(state);
96	✓✗	12	cost = boost::make_shared<crocoddyl::CostModelSum>(state, 0);
97	✓✗	12	constraint = boost::make_shared<crocoddyl::ConstraintModelManager>(state, 0);
98	✓✗	24	std::vector<std::size_t> frame_ids = model_factory.get_frame_ids();
99	✓✗	24	std::vector<std::string> frame_names = model_factory.get_frame_names();
100			// Define the impulse model
101	✓✓✗	12	switch (state_type) {
102		10	case StateModelTypes::StateMultibody_Talos:
103			case StateModelTypes::StateMultibody_RandomHumanoid:
104	✓✓	30	for (std::size_t i = 0; i < frame_names.size(); ++i) {
105	✓✗	40	impulse->addImpulse(frame_names[i],
106	✓✗✓✗ ✓✗	60	ImpulseModelFactory().create(
107			ImpulseModelTypes::ImpulseModel6D_LOCAL,
108		20	model_type, frame_names[i]));
109			}
110		10	break;
111		2	case StateModelTypes::StateMultibody_HyQ:
112	✓✓	10	for (std::size_t i = 0; i < frame_names.size(); ++i) {
113	✓✗	16	impulse->addImpulse(frame_names[i],
114	✓✗✓✗ ✓✗	24	ImpulseModelFactory().create(
115			ImpulseModelTypes::ImpulseModel3D_LOCAL,
116		8	model_type, frame_names[i]));
117			}
118		2	break;
119			default:
120			throw_pretty(__FILE__ ": Wrong StateModelTypes::Type given");
121			break;
122			}
123			// Define standard cost functions
124	✓✗✓✗	24	cost->addCost("state",
125	✓✗✓✗	24	CostModelFactory().create(
126			CostModelTypes::CostModelResidualState, state_type,
127			ActivationModelTypes::ActivationModelQuad, 0),
128			0.1);
129
130			// Define the constraint function
131	✓✗✓✗	12	Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
132	✓✗	24	crocoddyl::FrictionCone friction_cone(R, 1.);
133	✓✗✓✗	24	crocoddyl::WrenchCone wrench_cone(R, 1., Eigen::Vector2d(0.1, 0.1));
134	✓✗✓✗	24	crocoddyl::CoPSupport cop_support(R, Eigen::Vector2d(0.1, 0.1));
135	✓✗✓✗	24	Eigen::VectorXd lb, ub;
136	✓✓✓✓ ✓✗	12	switch (constraint_type) {
137		2	case ImpulseConstraintModelTypes::CostModelResidualImpulseCoMEquality:
138	✓✗✓✗	4	constraint->addConstraint(
139			"constraint",
140	✓✗	4	boost::make_shared<crocoddyl::ConstraintModelResidual>(
141			state,
142	✓✗	4	boost::make_shared<crocoddyl::ResidualModelImpulseCoM>(state)));
143		2	break;
144		3	case ImpulseConstraintModelTypes::
145			ConstraintModelResidualImpulseForceEquality:
146	✓✓	11	for (std::size_t i = 0; i < frame_ids.size(); ++i) {
147	✓✗	24	constraint->addConstraint(
148	✓✗✓✗	16	"constraint_" + std::to_string(i),
149	✓✗	16	boost::make_shared<crocoddyl::ConstraintModelResidual>(
150	✓✗	16	state, boost::make_shared<crocoddyl::ResidualModelContactForce>(
151		8	state, frame_ids[i], pinocchio::Force::Random(),
152	✓✗✓✗	16	model_factory.get_contact_nc(), 0)));
153			}
154		3	break;
155		2	case ImpulseConstraintModelTypes::
156			ConstraintModelResidualImpulseCoPPositionInequality:
157	✓✗	2	lb = cop_support.get_lb();
158	✓✗	2	ub = cop_support.get_ub();
159	✓✓	6	for (std::size_t i = 0; i < frame_ids.size(); ++i) {
160	✓✗	12	constraint->addConstraint(
161	✓✗✓✗	8	"constraint_" + std::to_string(i),
162	✓✗	8	boost::make_shared<crocoddyl::ConstraintModelResidual>(
163			state,
164	✓✗	8	boost::make_shared<crocoddyl::ResidualModelContactCoPPosition>(
165		8	state, frame_ids[i], cop_support, 0),
166			lb, ub));
167			}
168		2	break;
169		3	case ImpulseConstraintModelTypes::
170			ConstraintModelResidualImpulseFrictionConeInequality:
171	✓✗	3	lb = friction_cone.get_lb();
172	✓✗	3	ub = friction_cone.get_ub();
173	✓✓	11	for (std::size_t i = 0; i < frame_ids.size(); ++i) {
174	✓✗	24	constraint->addConstraint(
175	✓✗✓✗	16	"constraint_" + std::to_string(i),
176	✓✗	16	boost::make_shared<crocoddyl::ConstraintModelResidual>(
177			state,
178	✓✗	16	boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
179		16	state, frame_ids[i], friction_cone, 0),
180			lb, ub));
181			}
182		3	break;
183		2	case ImpulseConstraintModelTypes::
184			ConstraintModelResidualImpulseWrenchConeInequality:
185	✓✗	2	lb = wrench_cone.get_lb();
186	✓✗	2	ub = wrench_cone.get_ub();
187	✓✓	6	for (std::size_t i = 0; i < frame_ids.size(); ++i) {
188	✓✗	12	constraint->addConstraint(
189	✓✗✓✗	8	"constraint_" + std::to_string(i),
190	✓✗	8	boost::make_shared<crocoddyl::ConstraintModelResidual>(
191			state,
192	✓✗	8	boost::make_shared<crocoddyl::ResidualModelContactWrenchCone>(
193		8	state, frame_ids[i], wrench_cone, 0),
194			lb, ub));
195			}
196		2	break;
197			default:
198			throw_pretty(__FILE__ ": Wrong ImpulseConstraintModelTypes::Type given");
199			break;
200			}
201
202		12	double r_coeff = 0.; // TODO(cmastall): random_real_in_range(1e-16, 1e-2);
203		12	double damping = 0.; // TODO(cmastall): random_real_in_range(1e-16, 1e-2);
204		12	action = boost::make_shared<crocoddyl::ActionModelImpulseFwdDynamics>(
205	✓✗	12	state, impulse, cost, constraint, r_coeff, damping, true);
206		24	return action;
207			}
208
209			} // namespace unittest
210			} // namespace crocoddyl