Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/factory/actuation.cpp	Lines:	69	76	90.8 %
Date:	2024-02-13 11:12:33	Branches:	46	95	48.4 %


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

#include "actuation.hpp"

#include "crocoddyl/core/actuation/actuation-squashing.hpp"
#include "crocoddyl/core/actuation/squashing-base.hpp"
#include "crocoddyl/core/actuation/squashing/smooth-sat.hpp"
#include "crocoddyl/core/utils/exception.hpp"
#include "crocoddyl/multibody/actuations/floating-base-thrusters.hpp"
#include "crocoddyl/multibody/actuations/floating-base.hpp"
#include "crocoddyl/multibody/actuations/full.hpp"

namespace crocoddyl {
namespace unittest {

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

std::ostream& operator<<(std::ostream& os, ActuationModelTypes::Type type) {
  switch (type) {
    case ActuationModelTypes::ActuationModelFull:
      os << "ActuationModelFull";
      break;
    case ActuationModelTypes::ActuationModelFloatingBase:
      os << "ActuationModelFloatingBase";
      break;
    case ActuationModelTypes::ActuationModelFloatingBaseThrusters:
      os << "ActuationModelFloatingBaseThrusters";
      break;
    case ActuationModelTypes::ActuationModelSquashingFull:
      os << "ActuationModelSquashingFull";
      break;
    case ActuationModelTypes::NbActuationModelTypes:
      os << "NbActuationModelTypes";
      break;
    default:
      break;
  }
  return os;
}

ActuationModelFactory::ActuationModelFactory() {}
ActuationModelFactory::~ActuationModelFactory() {}

boost::shared_ptr<crocoddyl::ActuationModelAbstract>
ActuationModelFactory::create(ActuationModelTypes::Type actuation_type,
                              StateModelTypes::Type state_type) const {
  boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation;
  StateModelFactory factory;
  boost::shared_ptr<crocoddyl::StateAbstract> state =
      factory.create(state_type);
  boost::shared_ptr<crocoddyl::StateMultibody> state_multibody;
  // Thruster objects
  std::vector<crocoddyl::Thruster> ps;
  const double d_cog = 0.1525;
  const double cf = 6.6e-5;
  const double cm = 1e-6;
  pinocchio::SE3 p1(Eigen::Matrix3d::Identity(), Eigen::Vector3d(d_cog, 0, 0));
  pinocchio::SE3 p2(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0, d_cog, 0));
  pinocchio::SE3 p3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(-d_cog, 0, 0));
  pinocchio::SE3 p4(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0, -d_cog, 0));
  ps.push_back(crocoddyl::Thruster(p1, cm / cf, crocoddyl::ThrusterType::CCW));
  ps.push_back(crocoddyl::Thruster(p2, cm / cf, crocoddyl::ThrusterType::CW));
  ps.push_back(crocoddyl::Thruster(p3, cm / cf, crocoddyl::ThrusterType::CW));
  ps.push_back(crocoddyl::Thruster(p4, cm / cf, crocoddyl::ThrusterType::CCW));
  // Actuation Squashing objects
  boost::shared_ptr<crocoddyl::ActuationModelAbstract> act;
  boost::shared_ptr<crocoddyl::SquashingModelSmoothSat> squash;
  Eigen::VectorXd lb;
  Eigen::VectorXd ub;
  switch (actuation_type) {
    case ActuationModelTypes::ActuationModelFull:
      state_multibody =
          boost::static_pointer_cast<crocoddyl::StateMultibody>(state);
      actuation =
          boost::make_shared<crocoddyl::ActuationModelFull>(state_multibody);
      break;
    case ActuationModelTypes::ActuationModelFloatingBase:
      state_multibody =
          boost::static_pointer_cast<crocoddyl::StateMultibody>(state);
      actuation = boost::make_shared<crocoddyl::ActuationModelFloatingBase>(
          state_multibody);
      break;
    case ActuationModelTypes::ActuationModelFloatingBaseThrusters:
      state_multibody =
          boost::static_pointer_cast<crocoddyl::StateMultibody>(state);
      actuation =
          boost::make_shared<crocoddyl::ActuationModelFloatingBaseThrusters>(
              state_multibody, ps);
      break;
    case ActuationModelTypes::ActuationModelSquashingFull:
      state_multibody =
          boost::static_pointer_cast<crocoddyl::StateMultibody>(state);

      act = boost::make_shared<crocoddyl::ActuationModelFull>(state_multibody);

      lb = Eigen::VectorXd::Zero(state->get_nv());
      ub = Eigen::VectorXd::Zero(state->get_nv());
      lb.fill(-100.0);
      ub.fill(100.0);
      squash = boost::make_shared<crocoddyl::SquashingModelSmoothSat>(
          lb, ub, state->get_nv());

      actuation = boost::make_shared<crocoddyl::ActuationSquashingModel>(
          act, squash, state->get_nv());
      break;
    default:
      throw_pretty(__FILE__ ":\n Construct wrong ActuationModelTypes::Type");
      break;
  }
  return actuation;
}

void updateActuation(
    const boost::shared_ptr<crocoddyl::ActuationModelAbstract>& model,
    const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& data,
    const Eigen::VectorXd& x, const Eigen::VectorXd& u) {
  model->calc(data, x, u);
}

}  // 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-2024, University of Edinburgh, Heriot-Watt University
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#include "actuation.hpp"
10
11			#include "crocoddyl/core/actuation/actuation-squashing.hpp"
12			#include "crocoddyl/core/actuation/squashing-base.hpp"
13			#include "crocoddyl/core/actuation/squashing/smooth-sat.hpp"
14			#include "crocoddyl/core/utils/exception.hpp"
15			#include "crocoddyl/multibody/actuations/floating-base-thrusters.hpp"
16			#include "crocoddyl/multibody/actuations/floating-base.hpp"
17			#include "crocoddyl/multibody/actuations/full.hpp"
18
19			namespace crocoddyl {
20			namespace unittest {
21
22			const std::vector<ActuationModelTypes::Type> ActuationModelTypes::all(
23			ActuationModelTypes::init_all());
24
25		259	std::ostream& operator<<(std::ostream& os, ActuationModelTypes::Type type) {
26	✓✓✓✓ ✗✗	259	switch (type) {
27		57	case ActuationModelTypes::ActuationModelFull:
28		57	os << "ActuationModelFull";
29		57	break;
30		116	case ActuationModelTypes::ActuationModelFloatingBase:
31		116	os << "ActuationModelFloatingBase";
32		116	break;
33		31	case ActuationModelTypes::ActuationModelFloatingBaseThrusters:
34		31	os << "ActuationModelFloatingBaseThrusters";
35		31	break;
36		55	case ActuationModelTypes::ActuationModelSquashingFull:
37		55	os << "ActuationModelSquashingFull";
38		55	break;
39			case ActuationModelTypes::NbActuationModelTypes:
40			os << "NbActuationModelTypes";
41			break;
42			default:
43			break;
44			}
45		259	return os;
46			}
47
48		1709	ActuationModelFactory::ActuationModelFactory() {}
49		1709	ActuationModelFactory::~ActuationModelFactory() {}
50
51			boost::shared_ptr<crocoddyl::ActuationModelAbstract>
52		1709	ActuationModelFactory::create(ActuationModelTypes::Type actuation_type,
53			StateModelTypes::Type state_type) const {
54		1709	boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation;
55	✓✗	3418	StateModelFactory factory;
56			boost::shared_ptr<crocoddyl::StateAbstract> state =
57	✓✗	3418	factory.create(state_type);
58		1709	boost::shared_ptr<crocoddyl::StateMultibody> state_multibody;
59			// Thruster objects
60		3418	std::vector<crocoddyl::Thruster> ps;
61		1709	const double d_cog = 0.1525;
62		1709	const double cf = 6.6e-5;
63		1709	const double cm = 1e-6;
64	✓✗✓✗ ✓✗	1709	pinocchio::SE3 p1(Eigen::Matrix3d::Identity(), Eigen::Vector3d(d_cog, 0, 0));
65	✓✗✓✗ ✓✗	1709	pinocchio::SE3 p2(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0, d_cog, 0));
66	✓✗✓✗ ✓✗	1709	pinocchio::SE3 p3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(-d_cog, 0, 0));
67	✓✗✓✗ ✓✗	1709	pinocchio::SE3 p4(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0, -d_cog, 0));
68	✓✗✓✗	1709	ps.push_back(crocoddyl::Thruster(p1, cm / cf, crocoddyl::ThrusterType::CCW));
69	✓✗✓✗	1709	ps.push_back(crocoddyl::Thruster(p2, cm / cf, crocoddyl::ThrusterType::CW));
70	✓✗✓✗	1709	ps.push_back(crocoddyl::Thruster(p3, cm / cf, crocoddyl::ThrusterType::CW));
71	✓✗✓✗	1709	ps.push_back(crocoddyl::Thruster(p4, cm / cf, crocoddyl::ThrusterType::CCW));
72			// Actuation Squashing objects
73		1709	boost::shared_ptr<crocoddyl::ActuationModelAbstract> act;
74		1709	boost::shared_ptr<crocoddyl::SquashingModelSmoothSat> squash;
75	✓✗	3418	Eigen::VectorXd lb;
76	✓✗	3418	Eigen::VectorXd ub;
77	✓✓✓✓ ✗	1709	switch (actuation_type) {
78		542	case ActuationModelTypes::ActuationModelFull:
79			state_multibody =
80		542	boost::static_pointer_cast<crocoddyl::StateMultibody>(state);
81			actuation =
82	✓✗	542	boost::make_shared<crocoddyl::ActuationModelFull>(state_multibody);
83		542	break;
84		692	case ActuationModelTypes::ActuationModelFloatingBase:
85			state_multibody =
86		692	boost::static_pointer_cast<crocoddyl::StateMultibody>(state);
87	✓✗	1384	actuation = boost::make_shared<crocoddyl::ActuationModelFloatingBase>(
88		692	state_multibody);
89		692	break;
90		196	case ActuationModelTypes::ActuationModelFloatingBaseThrusters:
91			state_multibody =
92		196	boost::static_pointer_cast<crocoddyl::StateMultibody>(state);
93			actuation =
94	✓✗	392	boost::make_shared<crocoddyl::ActuationModelFloatingBaseThrusters>(
95		196	state_multibody, ps);
96		196	break;
97		279	case ActuationModelTypes::ActuationModelSquashingFull:
98			state_multibody =
99		279	boost::static_pointer_cast<crocoddyl::StateMultibody>(state);
100
101	✓✗	279	act = boost::make_shared<crocoddyl::ActuationModelFull>(state_multibody);
102
103	✓✗✓✗	279	lb = Eigen::VectorXd::Zero(state->get_nv());
104	✓✗✓✗	279	ub = Eigen::VectorXd::Zero(state->get_nv());
105	✓✗	279	lb.fill(-100.0);
106	✓✗	279	ub.fill(100.0);
107		279	squash = boost::make_shared<crocoddyl::SquashingModelSmoothSat>(
108	✓✗	279	lb, ub, state->get_nv());
109
110		279	actuation = boost::make_shared<crocoddyl::ActuationSquashingModel>(
111	✓✗	279	act, squash, state->get_nv());
112		279	break;
113			default:
114			throw_pretty(__FILE__ ":\n Construct wrong ActuationModelTypes::Type");
115			break;
116			}
117		3418	return actuation;
118			}
119
120		19935	void updateActuation(
121			const boost::shared_ptr<crocoddyl::ActuationModelAbstract>& model,
122			const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& data,
123			const Eigen::VectorXd& x, const Eigen::VectorXd& u) {
124	✓✗✓✗	19935	model->calc(data, x, u);
125		19935	}
126
127			} // namespace unittest
128			} // namespace crocoddyl