Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/factory/impulse_cost.cpp	Lines:	82	92	89.1 %
Date:	2024-02-13 11:12:33	Branches:	76	152	50.0 %


///////////////////////////////////////////////////////////////////////////////
// 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 "impulse_cost.hpp"

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

namespace crocoddyl {
namespace unittest {

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

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

ImpulseCostModelFactory::ImpulseCostModelFactory() {}
ImpulseCostModelFactory::~ImpulseCostModelFactory() {}

boost::shared_ptr<crocoddyl::ActionModelAbstract>
ImpulseCostModelFactory::create(
    ImpulseCostModelTypes::Type cost_type, PinocchioModelTypes::Type model_type,
    ActivationModelTypes::Type activation_type) const {
  // Create impulse action model with no cost
  boost::shared_ptr<crocoddyl::ActionModelImpulseFwdDynamics> action;
  switch (model_type) {
    case PinocchioModelTypes::Talos:
      action = ActionModelFactory().create_impulseFwdDynamics(
          StateModelTypes::StateMultibody_Talos);
      break;
    case PinocchioModelTypes::HyQ:
      action = ActionModelFactory().create_impulseFwdDynamics(
          StateModelTypes::StateMultibody_HyQ);
      break;
    default:
      throw_pretty(__FILE__ ": Wrong PinocchioModelTypes::Type given");
      break;
  }
  action->get_costs()->removeCost("state");

  // 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());
  Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
  std::vector<std::size_t> frame_ids = model_factory.get_frame_ids();
  switch (cost_type) {
    case ImpulseCostModelTypes::CostModelResidualImpulseCoM:
      cost = boost::make_shared<crocoddyl::CostModelResidual>(
          state, ActivationModelFactory().create(activation_type, 3),
          boost::make_shared<crocoddyl::ResidualModelImpulseCoM>(state));
      action->get_costs()->addCost("cost_0", cost, 0.01);
      break;
    case ImpulseCostModelTypes::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(), 0));
        action->get_costs()->addCost("cost_" + std::to_string(i), cost, 0.01);
      }
      break;
    case ImpulseCostModelTypes::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],
                CoPSupport(Eigen::Matrix3d::Identity(),
                           Eigen::Vector2d(0.1, 0.1)),
                0));
        action->get_costs()->addCost("cost_" + std::to_string(i), cost, 0.01);
      }
      break;
    case ImpulseCostModelTypes::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.), 0));
        action->get_costs()->addCost("cost_" + std::to_string(i), cost, 0.01);
      }
      break;
    case ImpulseCostModelTypes::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)), 0));
        action->get_costs()->addCost("cost_" + std::to_string(i), cost, 0.01);
      }
      break;
    default:
      throw_pretty(__FILE__ ": Wrong ImpulseCostModelTypes::Type given");
      break;
  }

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