Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/factory/contact_cost.cpp	Lines:	84	93	90.3 %
Date:	2024-02-13 11:12:33	Branches:	77	152	50.7 %


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

#include "contact_cost.hpp"

#include "crocoddyl/core/costs/residual.hpp"
#include "crocoddyl/core/utils/exception.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 "diff_action.hpp"

namespace crocoddyl {
namespace unittest {

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

std::ostream& operator<<(std::ostream& os, ContactCostModelTypes::Type type) {
  switch (type) {
    case ContactCostModelTypes::CostModelResidualContactForce:
      os << "CostModelResidualContactForce";
      break;
    case ContactCostModelTypes::CostModelResidualContactCoPPosition:
      os << "CostModelResidualContactCoPPosition";
      break;
    case ContactCostModelTypes::CostModelResidualContactFrictionCone:
      os << "CostModelResidualContactFrictionCone";
      break;
    case ContactCostModelTypes::CostModelResidualContactWrenchCone:
      os << "CostModelResidualContactWrenchCone";
      break;
    case ContactCostModelTypes::CostModelResidualContactControlGrav:
      os << "CostModelResidualContactControlGrav";
      break;
    case ContactCostModelTypes::NbContactCostModelTypes:
      os << "NbContactCostModelTypes";
      break;
    default:
      break;
  }
  return os;
}

ContactCostModelFactory::ContactCostModelFactory() {}
ContactCostModelFactory::~ContactCostModelFactory() {}

boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract>
ContactCostModelFactory::create(
    ContactCostModelTypes::Type cost_type, PinocchioModelTypes::Type model_type,
    ActivationModelTypes::Type activation_type,
    ActuationModelTypes::Type actuation_type) const {
  // Create contact action model with no cost
  boost::shared_ptr<crocoddyl::DifferentialActionModelContactFwdDynamics>
      action;
  switch (model_type) {
    case PinocchioModelTypes::Talos:
      action = DifferentialActionModelFactory().create_contactFwdDynamics(
          StateModelTypes::StateMultibody_Talos, actuation_type, false);
      break;
    case PinocchioModelTypes::HyQ:
      action = DifferentialActionModelFactory().create_contactFwdDynamics(
          StateModelTypes::StateMultibody_HyQ, actuation_type, false);
      break;
    default:
      throw_pretty(__FILE__ ": Wrong PinocchioModelTypes::Type given");
      break;
  }
  action->get_costs()->removeCost("state");
  action->get_costs()->removeCost("control");

  // Create cost
  boost::shared_ptr<crocoddyl::CostModelAbstract> cost;
  PinocchioModelFactory model_factory(model_type);
  boost::shared_ptr<crocoddyl::StateMultibody> state =
      boost::static_pointer_cast<crocoddyl::StateMultibody>(
          action->get_state());
  const std::size_t nu = action->get_actuation()->get_nu();
  Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
  std::vector<std::size_t> frame_ids = model_factory.get_frame_ids();
  switch (cost_type) {
    case ContactCostModelTypes::CostModelResidualContactForce:
      for (std::size_t i = 0; i < frame_ids.size(); ++i) {
        cost = boost::make_shared<crocoddyl::CostModelResidual>(
            state,
            ActivationModelFactory().create(activation_type,
                                            model_factory.get_contact_nc()),
            boost::make_shared<crocoddyl::ResidualModelContactForce>(
                state, frame_ids[i], pinocchio::Force::Random(),
                model_factory.get_contact_nc(), nu));
        action->get_costs()->addCost("cost_" + std::to_string(i), cost, 0.001);
      }
      break;
    case ContactCostModelTypes::CostModelResidualContactCoPPosition:
      for (std::size_t i = 0; i < frame_ids.size(); ++i) {
        cost = boost::make_shared<crocoddyl::CostModelResidual>(
            state, ActivationModelFactory().create(activation_type, 4),
            boost::make_shared<crocoddyl::ResidualModelContactCoPPosition>(
                state, frame_ids[i],
                crocoddyl::CoPSupport(R, Eigen::Vector2d(0.1, 0.1)), nu));
        action->get_costs()->addCost("cost_" + std::to_string(i), cost, 0.001);
      }
      break;
    case ContactCostModelTypes::CostModelResidualContactFrictionCone:
      for (std::size_t i = 0; i < frame_ids.size(); ++i) {
        cost = boost::make_shared<crocoddyl::CostModelResidual>(
            state, ActivationModelFactory().create(activation_type, 5),
            boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
                state, frame_ids[i], crocoddyl::FrictionCone(R, 1.), nu));
        action->get_costs()->addCost("cost_" + std::to_string(i), cost, 0.001);
      }
      break;
    case ContactCostModelTypes::CostModelResidualContactWrenchCone:
      for (std::size_t i = 0; i < frame_ids.size(); ++i) {
        cost = boost::make_shared<crocoddyl::CostModelResidual>(
            state, ActivationModelFactory().create(activation_type, 17),
            boost::make_shared<crocoddyl::ResidualModelContactWrenchCone>(
                state, frame_ids[i],
                crocoddyl::WrenchCone(R, 1., Eigen::Vector2d(0.1, 0.1)), nu));
        action->get_costs()->addCost("cost_" + std::to_string(i), cost, 0.001);
      }
      break;
    case ContactCostModelTypes::CostModelResidualContactControlGrav:
      for (std::size_t i = 0; i < frame_ids.size(); ++i) {
        cost = boost::make_shared<crocoddyl::CostModelResidual>(
            state,
            ActivationModelFactory().create(activation_type, state->get_nv()),
            boost::make_shared<crocoddyl::ResidualModelContactControlGrav>(
                state, nu));
        action->get_costs()->addCost("cost_" + std::to_string(i), cost, 0.001);
      }
      break;
    default:
      throw_pretty(__FILE__ ": Wrong ContactCostModelTypes::Type given");
      break;
  }
  // Include the cost in the contact model
  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-2022, 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_cost.hpp"
10
11			#include "crocoddyl/core/costs/residual.hpp"
12			#include "crocoddyl/core/utils/exception.hpp"
13			#include "crocoddyl/multibody/residuals/contact-control-gravity.hpp"
14			#include "crocoddyl/multibody/residuals/contact-cop-position.hpp"
15			#include "crocoddyl/multibody/residuals/contact-force.hpp"
16			#include "crocoddyl/multibody/residuals/contact-friction-cone.hpp"
17			#include "crocoddyl/multibody/residuals/contact-wrench-cone.hpp"
18			#include "diff_action.hpp"
19
20			namespace crocoddyl {
21			namespace unittest {
22
23			const std::vector<ContactCostModelTypes::Type> ContactCostModelTypes::all(
24			ContactCostModelTypes::init_all());
25
26		48	std::ostream& operator<<(std::ostream& os, ContactCostModelTypes::Type type) {
27	✓✓✓✓ ✓✗✗	48	switch (type) {
28		12	case ContactCostModelTypes::CostModelResidualContactForce:
29		12	os << "CostModelResidualContactForce";
30		12	break;
31		6	case ContactCostModelTypes::CostModelResidualContactCoPPosition:
32		6	os << "CostModelResidualContactCoPPosition";
33		6	break;
34		12	case ContactCostModelTypes::CostModelResidualContactFrictionCone:
35		12	os << "CostModelResidualContactFrictionCone";
36		12	break;
37		6	case ContactCostModelTypes::CostModelResidualContactWrenchCone:
38		6	os << "CostModelResidualContactWrenchCone";
39		6	break;
40		12	case ContactCostModelTypes::CostModelResidualContactControlGrav:
41		12	os << "CostModelResidualContactControlGrav";
42		12	break;
43			case ContactCostModelTypes::NbContactCostModelTypes:
44			os << "NbContactCostModelTypes";
45			break;
46			default:
47			break;
48			}
49		48	return os;
50			}
51
52		48	ContactCostModelFactory::ContactCostModelFactory() {}
53		48	ContactCostModelFactory::~ContactCostModelFactory() {}
54
55			boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract>
56		48	ContactCostModelFactory::create(
57			ContactCostModelTypes::Type cost_type, PinocchioModelTypes::Type model_type,
58			ActivationModelTypes::Type activation_type,
59			ActuationModelTypes::Type actuation_type) const {
60			// Create contact action model with no cost
61			boost::shared_ptr<crocoddyl::DifferentialActionModelContactFwdDynamics>
62		48	action;
63	✓✓✗	48	switch (model_type) {
64		30	case PinocchioModelTypes::Talos:
65	✓✗✓✗	60	action = DifferentialActionModelFactory().create_contactFwdDynamics(
66		30	StateModelTypes::StateMultibody_Talos, actuation_type, false);
67		30	break;
68		18	case PinocchioModelTypes::HyQ:
69	✓✗✓✗	36	action = DifferentialActionModelFactory().create_contactFwdDynamics(
70		18	StateModelTypes::StateMultibody_HyQ, actuation_type, false);
71		18	break;
72			default:
73			throw_pretty(__FILE__ ": Wrong PinocchioModelTypes::Type given");
74			break;
75			}
76	✓✗✓✗	48	action->get_costs()->removeCost("state");
77	✓✗✓✗	48	action->get_costs()->removeCost("control");
78
79			// Create cost
80		48	boost::shared_ptr<crocoddyl::CostModelAbstract> cost;
81	✓✗	96	PinocchioModelFactory model_factory(model_type);
82			boost::shared_ptr<crocoddyl::StateMultibody> state =
83			boost::static_pointer_cast<crocoddyl::StateMultibody>(
84		96	action->get_state());
85		48	const std::size_t nu = action->get_actuation()->get_nu();
86	✓✗✓✗	48	Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
87	✓✗	48	std::vector<std::size_t> frame_ids = model_factory.get_frame_ids();
88	✓✓✓✓ ✓✗	48	switch (cost_type) {
89		12	case ContactCostModelTypes::CostModelResidualContactForce:
90	✓✓	48	for (std::size_t i = 0; i < frame_ids.size(); ++i) {
91	✓✗	72	cost = boost::make_shared<crocoddyl::CostModelResidual>(
92			state,
93	✓✗✓✗ ✓✗	72	ActivationModelFactory().create(activation_type,
94			model_factory.get_contact_nc()),
95	✓✗	72	boost::make_shared<crocoddyl::ResidualModelContactForce>(
96		36	state, frame_ids[i], pinocchio::Force::Random(),
97	✓✗✓✗	72	model_factory.get_contact_nc(), nu));
98	✓✗✓✗ ✓✗	36	action->get_costs()->addCost("cost_" + std::to_string(i), cost, 0.001);
99			}
100		12	break;
101		6	case ContactCostModelTypes::CostModelResidualContactCoPPosition:
102	✓✓	18	for (std::size_t i = 0; i < frame_ids.size(); ++i) {
103	✓✗	24	cost = boost::make_shared<crocoddyl::CostModelResidual>(
104	✓✗✓✗	24	state, ActivationModelFactory().create(activation_type, 4),
105	✓✗	24	boost::make_shared<crocoddyl::ResidualModelContactCoPPosition>(
106		12	state, frame_ids[i],
107	✓✗✓✗	36	crocoddyl::CoPSupport(R, Eigen::Vector2d(0.1, 0.1)), nu));
108	✓✗✓✗ ✓✗	12	action->get_costs()->addCost("cost_" + std::to_string(i), cost, 0.001);
109			}
110		6	break;
111		12	case ContactCostModelTypes::CostModelResidualContactFrictionCone:
112	✓✓	48	for (std::size_t i = 0; i < frame_ids.size(); ++i) {
113	✓✗	72	cost = boost::make_shared<crocoddyl::CostModelResidual>(
114	✓✗✓✗	72	state, ActivationModelFactory().create(activation_type, 5),
115	✓✗	72	boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
116	✓✗	108	state, frame_ids[i], crocoddyl::FrictionCone(R, 1.), nu));
117	✓✗✓✗ ✓✗	36	action->get_costs()->addCost("cost_" + std::to_string(i), cost, 0.001);
118			}
119		12	break;
120		6	case ContactCostModelTypes::CostModelResidualContactWrenchCone:
121	✓✓	18	for (std::size_t i = 0; i < frame_ids.size(); ++i) {
122	✓✗	24	cost = boost::make_shared<crocoddyl::CostModelResidual>(
123	✓✗✓✗	24	state, ActivationModelFactory().create(activation_type, 17),
124	✓✗	24	boost::make_shared<crocoddyl::ResidualModelContactWrenchCone>(
125		12	state, frame_ids[i],
126	✓✗✓✗	36	crocoddyl::WrenchCone(R, 1., Eigen::Vector2d(0.1, 0.1)), nu));
127	✓✗✓✗ ✓✗	12	action->get_costs()->addCost("cost_" + std::to_string(i), cost, 0.001);
128			}
129		6	break;
130		12	case ContactCostModelTypes::CostModelResidualContactControlGrav:
131	✓✓	48	for (std::size_t i = 0; i < frame_ids.size(); ++i) {
132	✓✗	72	cost = boost::make_shared<crocoddyl::CostModelResidual>(
133			state,
134	✓✗✓✗	72	ActivationModelFactory().create(activation_type, state->get_nv()),
135	✓✗	72	boost::make_shared<crocoddyl::ResidualModelContactControlGrav>(
136		36	state, nu));
137	✓✗✓✗ ✓✗	36	action->get_costs()->addCost("cost_" + std::to_string(i), cost, 0.001);
138			}
139		12	break;
140			default:
141			throw_pretty(__FILE__ ": Wrong ContactCostModelTypes::Type given");
142			break;
143			}
144			// Include the cost in the contact model
145		96	return action;
146			}
147
148			} // namespace unittest
149			} // namespace crocoddyl