Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/factory/contact_constraint.cpp	Lines:	123	134	91.8 %
Date:	2024-02-13 11:12:33	Branches:	104	210	49.5 %


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

#include "contact_constraint.hpp"

#include "contact.hpp"
#include "cost.hpp"
#include "crocoddyl/core/constraints/residual.hpp"
#include "crocoddyl/core/utils/exception.hpp"
#include "crocoddyl/multibody/actions/contact-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"

namespace crocoddyl {
namespace unittest {

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

std::ostream &operator<<(std::ostream &os,
                         ContactConstraintModelTypes::Type type) {
  switch (type) {
    case ContactConstraintModelTypes::
        ConstraintModelResidualContactForceEquality:
      os << "ConstraintModelResidualContactForceEquality";
      break;
    case ContactConstraintModelTypes::
        ConstraintModelResidualContactCoPPositionInequality:
      os << "ConstraintModelResidualContactCoPPositionInequality";
      break;
    case ContactConstraintModelTypes::
        ConstraintModelResidualContactFrictionConeInequality:
      os << "ConstraintModelResidualContactFrictionConeInequality";
      break;
    case ContactConstraintModelTypes::
        ConstraintModelResidualContactWrenchConeInequality:
      os << "ConstraintModelResidualContactWrenchConeInequality";
      break;
    case ContactConstraintModelTypes::
        ConstraintModelResidualContactControlGravInequality:
      os << "ConstraintModelResidualContactControlGravInequality";
      break;
    case ContactConstraintModelTypes::NbContactConstraintModelTypes:
      os << "NbContactConstraintModelTypes";
      break;
    default:
      break;
  }
  return os;
}

ContactConstraintModelFactory::ContactConstraintModelFactory() {}
ContactConstraintModelFactory::~ContactConstraintModelFactory() {}

boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract>
ContactConstraintModelFactory::create(
    ContactConstraintModelTypes::Type constraint_type,
    PinocchioModelTypes::Type model_type,
    ActuationModelTypes::Type actuation_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 contact contact diff-action model with standard cost functions
  boost::shared_ptr<crocoddyl::DifferentialActionModelContactFwdDynamics>
      action;
  boost::shared_ptr<crocoddyl::StateMultibody> state;
  boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation;
  boost::shared_ptr<crocoddyl::ContactModelMultiple> contact;
  boost::shared_ptr<crocoddyl::CostModelSum> cost;
  boost::shared_ptr<crocoddyl::ConstraintModelManager> constraint;
  state = boost::static_pointer_cast<crocoddyl::StateMultibody>(
      StateModelFactory().create(state_type));
  actuation = ActuationModelFactory().create(actuation_type, state_type);
  contact = boost::make_shared<crocoddyl::ContactModelMultiple>(
      state, actuation->get_nu());
  cost =
      boost::make_shared<crocoddyl::CostModelSum>(state, actuation->get_nu());
  constraint = boost::make_shared<crocoddyl::ConstraintModelManager>(
      state, actuation->get_nu());
  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 contact model
  switch (state_type) {
    case StateModelTypes::StateMultibody_Talos:
    case StateModelTypes::StateMultibody_RandomHumanoid:
      for (std::size_t i = 0; i < frame_names.size(); ++i) {
        contact->addContact(frame_names[i],
                            ContactModelFactory().create(
                                ContactModelTypes::ContactModel6D_LOCAL,
                                model_type, Eigen::Vector2d::Random(),
                                frame_names[i], actuation->get_nu()));
      }
      break;
    case StateModelTypes::StateMultibody_HyQ:
      for (std::size_t i = 0; i < frame_names.size(); ++i) {
        contact->addContact(frame_names[i],
                            ContactModelFactory().create(
                                ContactModelTypes::ContactModel3D_LOCAL,
                                model_type, Eigen::Vector2d::Random(),
                                frame_names[i], actuation->get_nu()));
      }
      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, actuation->get_nu()),
      0.1);
  cost->addCost(
      "control",
      CostModelFactory().create(
          CostModelTypes::CostModelResidualControl, state_type,
          ActivationModelTypes::ActivationModelQuad, actuation->get_nu()),
      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 ContactConstraintModelTypes::
        ConstraintModelResidualContactForceEquality:
      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(), actuation->get_nu())));
      }
      break;
    case ContactConstraintModelTypes::
        ConstraintModelResidualContactCoPPositionInequality:
      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, actuation->get_nu()),
                lb, ub));
      }
      break;
    case ContactConstraintModelTypes::
        ConstraintModelResidualContactFrictionConeInequality:
      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, actuation->get_nu()),
                lb, ub),
            true);
      }
      break;
    case ContactConstraintModelTypes::
        ConstraintModelResidualContactWrenchConeInequality:
      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, actuation->get_nu()),
                lb, ub),
            true);
      }
      break;
    case ContactConstraintModelTypes::
        ConstraintModelResidualContactControlGravInequality:
      lb = Eigen::VectorXd::Zero(state->get_nv());
      ub = Eigen::VectorXd::Random(state->get_nv()).cwiseAbs();
      constraint->addConstraint(
          "constraint_0",
          boost::make_shared<crocoddyl::ConstraintModelResidual>(
              state,
              boost::make_shared<crocoddyl::ResidualModelContactControlGrav>(
                  state, actuation->get_nu()),
              lb, ub));
      break;
    default:
      throw_pretty(__FILE__ ": Wrong ContactConstraintModelTypes::Type given");
      break;
  }

  action =
      boost::make_shared<crocoddyl::DifferentialActionModelContactFwdDynamics>(
          state, actuation, contact, cost, constraint, 0., 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			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#include "contact_constraint.hpp"
10
11			#include "contact.hpp"
12			#include "cost.hpp"
13			#include "crocoddyl/core/constraints/residual.hpp"
14			#include "crocoddyl/core/utils/exception.hpp"
15			#include "crocoddyl/multibody/actions/contact-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
22			namespace crocoddyl {
23			namespace unittest {
24
25			const std::vector<ContactConstraintModelTypes::Type>
26			ContactConstraintModelTypes::all(ContactConstraintModelTypes::init_all());
27
28		13	std::ostream &operator<<(std::ostream &os,
29			ContactConstraintModelTypes::Type type) {
30	✓✓✓✓ ✓✗✗	13	switch (type) {
31		3	case ContactConstraintModelTypes::
32			ConstraintModelResidualContactForceEquality:
33		3	os << "ConstraintModelResidualContactForceEquality";
34		3	break;
35		2	case ContactConstraintModelTypes::
36			ConstraintModelResidualContactCoPPositionInequality:
37		2	os << "ConstraintModelResidualContactCoPPositionInequality";
38		2	break;
39		3	case ContactConstraintModelTypes::
40			ConstraintModelResidualContactFrictionConeInequality:
41		3	os << "ConstraintModelResidualContactFrictionConeInequality";
42		3	break;
43		2	case ContactConstraintModelTypes::
44			ConstraintModelResidualContactWrenchConeInequality:
45		2	os << "ConstraintModelResidualContactWrenchConeInequality";
46		2	break;
47		3	case ContactConstraintModelTypes::
48			ConstraintModelResidualContactControlGravInequality:
49		3	os << "ConstraintModelResidualContactControlGravInequality";
50		3	break;
51			case ContactConstraintModelTypes::NbContactConstraintModelTypes:
52			os << "NbContactConstraintModelTypes";
53			break;
54			default:
55			break;
56			}
57		13	return os;
58			}
59
60		13	ContactConstraintModelFactory::ContactConstraintModelFactory() {}
61		13	ContactConstraintModelFactory::~ContactConstraintModelFactory() {}
62
63			boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract>
64		13	ContactConstraintModelFactory::create(
65			ContactConstraintModelTypes::Type constraint_type,
66			PinocchioModelTypes::Type model_type,
67			ActuationModelTypes::Type actuation_type) const {
68			// Identify the state type given the model type
69			StateModelTypes::Type state_type;
70	✓✗	26	PinocchioModelFactory model_factory(model_type);
71	✓✓✓✗	13	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		3	case PinocchioModelTypes::HyQ:
79		3	state_type = StateModelTypes::StateMultibody_HyQ;
80		3	break;
81			default:
82			throw_pretty(__FILE__ ": Wrong PinocchioModelTypes::Type given");
83			break;
84			}
85
86			// Create contact contact diff-action model with standard cost functions
87			boost::shared_ptr<crocoddyl::DifferentialActionModelContactFwdDynamics>
88		13	action;
89		13	boost::shared_ptr<crocoddyl::StateMultibody> state;
90		13	boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation;
91		13	boost::shared_ptr<crocoddyl::ContactModelMultiple> contact;
92		13	boost::shared_ptr<crocoddyl::CostModelSum> cost;
93		13	boost::shared_ptr<crocoddyl::ConstraintModelManager> constraint;
94		26	state = boost::static_pointer_cast<crocoddyl::StateMultibody>(
95	✓✗✓✗	39	StateModelFactory().create(state_type));
96	✓✗✓✗	13	actuation = ActuationModelFactory().create(actuation_type, state_type);
97		13	contact = boost::make_shared<crocoddyl::ContactModelMultiple>(
98	✓✗	13	state, actuation->get_nu());
99			cost =
100	✓✗	13	boost::make_shared<crocoddyl::CostModelSum>(state, actuation->get_nu());
101		13	constraint = boost::make_shared<crocoddyl::ConstraintModelManager>(
102	✓✗	13	state, actuation->get_nu());
103	✓✗	26	std::vector<std::size_t> frame_ids = model_factory.get_frame_ids();
104	✓✗	26	std::vector<std::string> frame_names = model_factory.get_frame_names();
105			// Define the contact model
106	✓✓✗	13	switch (state_type) {
107		10	case StateModelTypes::StateMultibody_Talos:
108			case StateModelTypes::StateMultibody_RandomHumanoid:
109	✓✓	30	for (std::size_t i = 0; i < frame_names.size(); ++i) {
110	✓✗	40	contact->addContact(frame_names[i],
111	✓✗✓✗ ✓✗✓✗	100	ContactModelFactory().create(
112			ContactModelTypes::ContactModel6D_LOCAL,
113	✓✗	40	model_type, Eigen::Vector2d::Random(),
114		20	frame_names[i], actuation->get_nu()));
115			}
116		10	break;
117		3	case StateModelTypes::StateMultibody_HyQ:
118	✓✓	15	for (std::size_t i = 0; i < frame_names.size(); ++i) {
119	✓✗	24	contact->addContact(frame_names[i],
120	✓✗✓✗ ✓✗✓✗	60	ContactModelFactory().create(
121			ContactModelTypes::ContactModel3D_LOCAL,
122	✓✗	24	model_type, Eigen::Vector2d::Random(),
123		12	frame_names[i], actuation->get_nu()));
124			}
125		3	break;
126			default:
127			throw_pretty(__FILE__ ": Wrong StateModelTypes::Type given");
128			break;
129			}
130			// Define standard cost functions
131	✓✗✓✗	26	cost->addCost(
132			"state",
133	✓✗✓✗	26	CostModelFactory().create(
134			CostModelTypes::CostModelResidualState, state_type,
135			ActivationModelTypes::ActivationModelQuad, actuation->get_nu()),
136			0.1);
137	✓✗✓✗	26	cost->addCost(
138			"control",
139	✓✗✓✗	26	CostModelFactory().create(
140			CostModelTypes::CostModelResidualControl, state_type,
141			ActivationModelTypes::ActivationModelQuad, actuation->get_nu()),
142			0.1);
143
144			// Define the constraint function
145	✓✗✓✗	13	Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
146	✓✗	26	crocoddyl::FrictionCone friction_cone(R, 1.);
147	✓✗✓✗	26	crocoddyl::WrenchCone wrench_cone(R, 1., Eigen::Vector2d(0.1, 0.1));
148	✓✗✓✗	26	crocoddyl::CoPSupport cop_support(R, Eigen::Vector2d(0.1, 0.1));
149	✓✗✓✗	26	Eigen::VectorXd lb, ub;
150	✓✓✓✓ ✓✗	13	switch (constraint_type) {
151		3	case ContactConstraintModelTypes::
152			ConstraintModelResidualContactForceEquality:
153	✓✓	11	for (std::size_t i = 0; i < frame_ids.size(); ++i) {
154	✓✗	24	constraint->addConstraint(
155	✓✗✓✗	16	"constraint_" + std::to_string(i),
156	✓✗	16	boost::make_shared<crocoddyl::ConstraintModelResidual>(
157			state,
158	✓✗	16	boost::make_shared<crocoddyl::ResidualModelContactForce>(
159		8	state, frame_ids[i], pinocchio::Force::Random(),
160	✓✗✓✗	16	model_factory.get_contact_nc(), actuation->get_nu())));
161			}
162		3	break;
163		2	case ContactConstraintModelTypes::
164			ConstraintModelResidualContactCoPPositionInequality:
165	✓✗	2	lb = cop_support.get_lb();
166	✓✗	2	ub = cop_support.get_ub();
167	✓✓	6	for (std::size_t i = 0; i < frame_ids.size(); ++i) {
168	✓✗	12	constraint->addConstraint(
169	✓✗✓✗	8	"constraint_" + std::to_string(i),
170	✓✗	8	boost::make_shared<crocoddyl::ConstraintModelResidual>(
171			state,
172	✓✗	8	boost::make_shared<crocoddyl::ResidualModelContactCoPPosition>(
173		8	state, frame_ids[i], cop_support, actuation->get_nu()),
174			lb, ub));
175			}
176		2	break;
177		3	case ContactConstraintModelTypes::
178			ConstraintModelResidualContactFrictionConeInequality:
179	✓✗	3	lb = friction_cone.get_lb();
180	✓✗	3	ub = friction_cone.get_ub();
181	✓✓	11	for (std::size_t i = 0; i < frame_ids.size(); ++i) {
182	✓✗	24	constraint->addConstraint(
183	✓✗✓✗	16	"constraint_" + std::to_string(i),
184	✓✗	16	boost::make_shared<crocoddyl::ConstraintModelResidual>(
185			state,
186	✓✗	16	boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
187		16	state, frame_ids[i], friction_cone, actuation->get_nu()),
188			lb, ub),
189			true);
190			}
191		3	break;
192		2	case ContactConstraintModelTypes::
193			ConstraintModelResidualContactWrenchConeInequality:
194	✓✗	2	lb = wrench_cone.get_lb();
195	✓✗	2	ub = wrench_cone.get_ub();
196	✓✓	6	for (std::size_t i = 0; i < frame_ids.size(); ++i) {
197	✓✗	12	constraint->addConstraint(
198	✓✗✓✗	8	"constraint_" + std::to_string(i),
199	✓✗	8	boost::make_shared<crocoddyl::ConstraintModelResidual>(
200			state,
201	✓✗	8	boost::make_shared<crocoddyl::ResidualModelContactWrenchCone>(
202		8	state, frame_ids[i], wrench_cone, actuation->get_nu()),
203			lb, ub),
204			true);
205			}
206		2	break;
207		3	case ContactConstraintModelTypes::
208			ConstraintModelResidualContactControlGravInequality:
209	✓✗✓✗	3	lb = Eigen::VectorXd::Zero(state->get_nv());
210	✓✗✓✗ ✓✗	3	ub = Eigen::VectorXd::Random(state->get_nv()).cwiseAbs();
211	✓✗✓✗	6	constraint->addConstraint(
212			"constraint_0",
213	✓✗	6	boost::make_shared<crocoddyl::ConstraintModelResidual>(
214			state,
215		3	boost::make_shared<crocoddyl::ResidualModelContactControlGrav>(
216	✓✗	3	state, actuation->get_nu()),
217			lb, ub));
218		3	break;
219			default:
220			throw_pretty(__FILE__ ": Wrong ContactConstraintModelTypes::Type given");
221			break;
222			}
223
224			action =
225		13	boost::make_shared<crocoddyl::DifferentialActionModelContactFwdDynamics>(
226	✓✗	13	state, actuation, contact, cost, constraint, 0., true);
227		26	return action;
228			}
229
230			} // namespace unittest
231			} // namespace crocoddyl