Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/factory/residual.cpp	Lines:	95	112	84.8 %
Date:	2024-02-13 11:12:33	Branches:	74	145	51.0 %


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

#include "residual.hpp"

#include "crocoddyl/core/residuals/control.hpp"
#include "crocoddyl/core/utils/exception.hpp"
#include "crocoddyl/multibody/residuals/centroidal-momentum.hpp"
#include "crocoddyl/multibody/residuals/com-position.hpp"
#include "crocoddyl/multibody/residuals/control-gravity.hpp"
#include "crocoddyl/multibody/residuals/frame-placement.hpp"
#include "crocoddyl/multibody/residuals/frame-rotation.hpp"
#include "crocoddyl/multibody/residuals/frame-translation.hpp"
#include "crocoddyl/multibody/residuals/frame-velocity.hpp"
#include "crocoddyl/multibody/residuals/pair-collision.hpp"
#include "crocoddyl/multibody/residuals/state.hpp"

namespace crocoddyl {
namespace unittest {

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

std::ostream& operator<<(std::ostream& os, ResidualModelTypes::Type type) {
  switch (type) {
    case ResidualModelTypes::ResidualModelState:
      os << "ResidualModelState";
      break;
    case ResidualModelTypes::ResidualModelControl:
      os << "ResidualModelControl";
      break;
    case ResidualModelTypes::ResidualModelCoMPosition:
      os << "ResidualModelCoMPosition";
      break;
    case ResidualModelTypes::ResidualModelCentroidalMomentum:
      os << "ResidualModelCentroidalMomentum";
      break;
    case ResidualModelTypes::ResidualModelFramePlacement:
      os << "ResidualModelFramePlacement";
      break;
    case ResidualModelTypes::ResidualModelFrameRotation:
      os << "ResidualModelFrameRotation";
      break;
    case ResidualModelTypes::ResidualModelFrameTranslation:
      os << "ResidualModelFrameTranslation";
      break;
    case ResidualModelTypes::ResidualModelFrameVelocity:
      os << "ResidualModelFrameVelocity";
      break;
    case ResidualModelTypes::ResidualModelControlGrav:
      os << "ResidualModelControlGrav";
      break;
#ifdef PINOCCHIO_WITH_HPP_FCL
    case ResidualModelTypes::ResidualModelPairCollision:
      os << "ResidualModelPairCollision";
      break;
#endif  // PINOCCHIO_WITH_HPP_FCL
    case ResidualModelTypes::NbResidualModelTypes:
      os << "NbResidualModelTypes";
      break;
    default:
      break;
  }
  return os;
}

ResidualModelFactory::ResidualModelFactory() {}
ResidualModelFactory::~ResidualModelFactory() {}

boost::shared_ptr<crocoddyl::ResidualModelAbstract>
ResidualModelFactory::create(ResidualModelTypes::Type residual_type,
                             StateModelTypes::Type state_type,
                             std::size_t nu) const {
  StateModelFactory state_factory;
  boost::shared_ptr<crocoddyl::ResidualModelAbstract> residual;
  boost::shared_ptr<crocoddyl::StateMultibody> state =
      boost::static_pointer_cast<crocoddyl::StateMultibody>(
          state_factory.create(state_type));
  pinocchio::FrameIndex frame_index = state->get_pinocchio()->frames.size() - 1;
  pinocchio::SE3 frame_SE3 = pinocchio::SE3::Random();

#ifdef PINOCCHIO_WITH_HPP_FCL
  pinocchio::SE3 frame_SE3_obstacle = pinocchio::SE3::Random();
  boost::shared_ptr<pinocchio::GeometryModel> geometry =
      boost::make_shared<pinocchio::GeometryModel>(pinocchio::GeometryModel());
  pinocchio::GeomIndex ig_frame =
      geometry->addGeometryObject(pinocchio::GeometryObject(
          "frame", frame_index,
          state->get_pinocchio()->frames[frame_index].parent,
          CollisionGeometryPtr(new hpp::fcl::Sphere(0)), frame_SE3));
  pinocchio::GeomIndex ig_obs =
      geometry->addGeometryObject(pinocchio::GeometryObject(
          "obs", state->get_pinocchio()->getFrameId("universe"),
          state->get_pinocchio()
              ->frames[state->get_pinocchio()->getFrameId("universe")]
              .parent,
          CollisionGeometryPtr(new hpp::fcl::Sphere(0)), frame_SE3_obstacle));
  geometry->addCollisionPair(pinocchio::CollisionPair(ig_frame, ig_obs));
#endif  // PINOCCHIO_WITH_HPP_FCL
  if (nu == std::numeric_limits<std::size_t>::max()) {
    nu = state->get_nv();
  }
  switch (residual_type) {
    case ResidualModelTypes::ResidualModelState:
      residual = boost::make_shared<crocoddyl::ResidualModelState>(
          state, state->rand(), nu);
      break;
    case ResidualModelTypes::ResidualModelControl:
      residual = boost::make_shared<crocoddyl::ResidualModelControl>(
          state, Eigen::VectorXd::Random(nu));
      break;
    case ResidualModelTypes::ResidualModelCoMPosition:
      residual = boost::make_shared<crocoddyl::ResidualModelCoMPosition>(
          state, Eigen::Vector3d::Random(), nu);
      break;
    case ResidualModelTypes::ResidualModelCentroidalMomentum:
      residual = boost::make_shared<crocoddyl::ResidualModelCentroidalMomentum>(
          state, Vector6d::Random(), nu);
      break;
    case ResidualModelTypes::ResidualModelFramePlacement:
      residual = boost::make_shared<crocoddyl::ResidualModelFramePlacement>(
          state, frame_index, frame_SE3, nu);
      break;
    case ResidualModelTypes::ResidualModelFrameRotation:
      residual = boost::make_shared<crocoddyl::ResidualModelFrameRotation>(
          state, frame_index, frame_SE3.rotation(), nu);
      break;
    case ResidualModelTypes::ResidualModelFrameTranslation:
      residual = boost::make_shared<crocoddyl::ResidualModelFrameTranslation>(
          state, frame_index, frame_SE3.translation(), nu);
      break;
    case ResidualModelTypes::ResidualModelFrameVelocity:
      residual = boost::make_shared<crocoddyl::ResidualModelFrameVelocity>(
          state, frame_index, pinocchio::Motion::Random(),
          static_cast<pinocchio::ReferenceFrame>(rand() % 2),
          nu);  // the code cannot test LOCAL_WORLD_ALIGNED
      break;
    case ResidualModelTypes::ResidualModelControlGrav:
      residual =
          boost::make_shared<crocoddyl::ResidualModelControlGrav>(state, nu);
      break;
#ifdef PINOCCHIO_WITH_HPP_FCL
    case ResidualModelTypes::ResidualModelPairCollision:
      residual = boost::make_shared<crocoddyl::ResidualModelPairCollision>(
          state, nu, geometry, 0,
          state->get_pinocchio()->frames[frame_index].parent);
      break;
#endif  // PINOCCHIO_WITH_HPP_FCL
    default:
      throw_pretty(__FILE__ ": Wrong ResidualModelTypes::Type given");
      break;
  }
  return residual;
}

boost::shared_ptr<crocoddyl::ResidualModelAbstract> create_random_residual(
    StateModelTypes::Type state_type) {
  static bool once = true;
  if (once) {
    srand((unsigned)time(NULL));
    once = false;
  }

  ResidualModelFactory factory;
  ResidualModelTypes::Type rand_type = static_cast<ResidualModelTypes::Type>(
      rand() % ResidualModelTypes::NbResidualModelTypes);
  return factory.create(rand_type, state_type);
}

}  // 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, LAAS-CNRS
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#include "residual.hpp"
10
11			#include "crocoddyl/core/residuals/control.hpp"
12			#include "crocoddyl/core/utils/exception.hpp"
13			#include "crocoddyl/multibody/residuals/centroidal-momentum.hpp"
14			#include "crocoddyl/multibody/residuals/com-position.hpp"
15			#include "crocoddyl/multibody/residuals/control-gravity.hpp"
16			#include "crocoddyl/multibody/residuals/frame-placement.hpp"
17			#include "crocoddyl/multibody/residuals/frame-rotation.hpp"
18			#include "crocoddyl/multibody/residuals/frame-translation.hpp"
19			#include "crocoddyl/multibody/residuals/frame-velocity.hpp"
20			#include "crocoddyl/multibody/residuals/pair-collision.hpp"
21			#include "crocoddyl/multibody/residuals/state.hpp"
22
23			namespace crocoddyl {
24			namespace unittest {
25
26			const std::vector<ResidualModelTypes::Type> ResidualModelTypes::all(
27			ResidualModelTypes::init_all());
28
29		180	std::ostream& operator<<(std::ostream& os, ResidualModelTypes::Type type) {
30	✓✓✓✓ ✓✓✓✓ ✓✓✗✗	180	switch (type) {
31		18	case ResidualModelTypes::ResidualModelState:
32		18	os << "ResidualModelState";
33		18	break;
34		18	case ResidualModelTypes::ResidualModelControl:
35		18	os << "ResidualModelControl";
36		18	break;
37		18	case ResidualModelTypes::ResidualModelCoMPosition:
38		18	os << "ResidualModelCoMPosition";
39		18	break;
40		18	case ResidualModelTypes::ResidualModelCentroidalMomentum:
41		18	os << "ResidualModelCentroidalMomentum";
42		18	break;
43		18	case ResidualModelTypes::ResidualModelFramePlacement:
44		18	os << "ResidualModelFramePlacement";
45		18	break;
46		18	case ResidualModelTypes::ResidualModelFrameRotation:
47		18	os << "ResidualModelFrameRotation";
48		18	break;
49		18	case ResidualModelTypes::ResidualModelFrameTranslation:
50		18	os << "ResidualModelFrameTranslation";
51		18	break;
52		18	case ResidualModelTypes::ResidualModelFrameVelocity:
53		18	os << "ResidualModelFrameVelocity";
54		18	break;
55		18	case ResidualModelTypes::ResidualModelControlGrav:
56		18	os << "ResidualModelControlGrav";
57		18	break;
58			#ifdef PINOCCHIO_WITH_HPP_FCL
59		18	case ResidualModelTypes::ResidualModelPairCollision:
60		18	os << "ResidualModelPairCollision";
61		18	break;
62			#endif // PINOCCHIO_WITH_HPP_FCL
63			case ResidualModelTypes::NbResidualModelTypes:
64			os << "NbResidualModelTypes";
65			break;
66			default:
67			break;
68			}
69		180	return os;
70			}
71
72		541	ResidualModelFactory::ResidualModelFactory() {}
73		541	ResidualModelFactory::~ResidualModelFactory() {}
74
75			boost::shared_ptr<crocoddyl::ResidualModelAbstract>
76		540	ResidualModelFactory::create(ResidualModelTypes::Type residual_type,
77			StateModelTypes::Type state_type,
78			std::size_t nu) const {
79	✓✗	1080	StateModelFactory state_factory;
80		540	boost::shared_ptr<crocoddyl::ResidualModelAbstract> residual;
81			boost::shared_ptr<crocoddyl::StateMultibody> state =
82			boost::static_pointer_cast<crocoddyl::StateMultibody>(
83	✓✗	1080	state_factory.create(state_type));
84		540	pinocchio::FrameIndex frame_index = state->get_pinocchio()->frames.size() - 1;
85	✓✗	540	pinocchio::SE3 frame_SE3 = pinocchio::SE3::Random();
86
87			#ifdef PINOCCHIO_WITH_HPP_FCL
88	✓✗	540	pinocchio::SE3 frame_SE3_obstacle = pinocchio::SE3::Random();
89			boost::shared_ptr<pinocchio::GeometryModel> geometry =
90	✓✗✓✗	1080	boost::make_shared<pinocchio::GeometryModel>(pinocchio::GeometryModel());
91			pinocchio::GeomIndex ig_frame =
92	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	1620	geometry->addGeometryObject(pinocchio::GeometryObject(
93			"frame", frame_index,
94		540	state->get_pinocchio()->frames[frame_index].parent,
95	✓✗✓✗ ✓✗	1080	CollisionGeometryPtr(new hpp::fcl::Sphere(0)), frame_SE3));
96			pinocchio::GeomIndex ig_obs =
97	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	1620	geometry->addGeometryObject(pinocchio::GeometryObject(
98		540	"obs", state->get_pinocchio()->getFrameId("universe"),
99		540	state->get_pinocchio()
100	✓✗✓✗	1620	->frames[state->get_pinocchio()->getFrameId("universe")]
101			.parent,
102	✓✗✓✗ ✓✗	1080	CollisionGeometryPtr(new hpp::fcl::Sphere(0)), frame_SE3_obstacle));
103	✓✗✓✗	540	geometry->addCollisionPair(pinocchio::CollisionPair(ig_frame, ig_obs));
104			#endif // PINOCCHIO_WITH_HPP_FCL
105	✗✓	540	if (nu == std::numeric_limits<std::size_t>::max()) {
106			nu = state->get_nv();
107			}
108	✓✓✓✓ ✓✓✓✓ ✓✓✗	540	switch (residual_type) {
109		54	case ResidualModelTypes::ResidualModelState:
110	✓✗	108	residual = boost::make_shared<crocoddyl::ResidualModelState>(
111	✓✗	162	state, state->rand(), nu);
112		54	break;
113		54	case ResidualModelTypes::ResidualModelControl:
114	✓✗	108	residual = boost::make_shared<crocoddyl::ResidualModelControl>(
115	✓✗	108	state, Eigen::VectorXd::Random(nu));
116		54	break;
117		54	case ResidualModelTypes::ResidualModelCoMPosition:
118	✓✗	108	residual = boost::make_shared<crocoddyl::ResidualModelCoMPosition>(
119	✓✗	108	state, Eigen::Vector3d::Random(), nu);
120		54	break;
121		54	case ResidualModelTypes::ResidualModelCentroidalMomentum:
122	✓✗	108	residual = boost::make_shared<crocoddyl::ResidualModelCentroidalMomentum>(
123	✓✗	108	state, Vector6d::Random(), nu);
124		54	break;
125		54	case ResidualModelTypes::ResidualModelFramePlacement:
126	✓✗	108	residual = boost::make_shared<crocoddyl::ResidualModelFramePlacement>(
127		54	state, frame_index, frame_SE3, nu);
128		54	break;
129		54	case ResidualModelTypes::ResidualModelFrameRotation:
130		54	residual = boost::make_shared<crocoddyl::ResidualModelFrameRotation>(
131	✓✗✓✗	54	state, frame_index, frame_SE3.rotation(), nu);
132		54	break;
133		54	case ResidualModelTypes::ResidualModelFrameTranslation:
134		54	residual = boost::make_shared<crocoddyl::ResidualModelFrameTranslation>(
135	✓✗✓✗	54	state, frame_index, frame_SE3.translation(), nu);
136		54	break;
137		54	case ResidualModelTypes::ResidualModelFrameVelocity:
138	✓✗	108	residual = boost::make_shared<crocoddyl::ResidualModelFrameVelocity>(
139			state, frame_index, pinocchio::Motion::Random(),
140	✓✗	54	static_cast<pinocchio::ReferenceFrame>(rand() % 2),
141		54	nu); // the code cannot test LOCAL_WORLD_ALIGNED
142		54	break;
143		54	case ResidualModelTypes::ResidualModelControlGrav:
144			residual =
145	✓✗	54	boost::make_shared<crocoddyl::ResidualModelControlGrav>(state, nu);
146		54	break;
147			#ifdef PINOCCHIO_WITH_HPP_FCL
148		54	case ResidualModelTypes::ResidualModelPairCollision:
149		54	residual = boost::make_shared<crocoddyl::ResidualModelPairCollision>(
150			state, nu, geometry, 0,
151	✓✗	54	state->get_pinocchio()->frames[frame_index].parent);
152		54	break;
153			#endif // PINOCCHIO_WITH_HPP_FCL
154			default:
155			throw_pretty(__FILE__ ": Wrong ResidualModelTypes::Type given");
156			break;
157			}
158		1080	return residual;
159			}
160
161			boost::shared_ptr<crocoddyl::ResidualModelAbstract> create_random_residual(
162			StateModelTypes::Type state_type) {
163			static bool once = true;
164			if (once) {
165			srand((unsigned)time(NULL));
166			once = false;
167			}
168
169			ResidualModelFactory factory;
170			ResidualModelTypes::Type rand_type = static_cast<ResidualModelTypes::Type>(
171			rand() % ResidualModelTypes::NbResidualModelTypes);
172			return factory.create(rand_type, state_type);
173			}
174
175			} // namespace unittest
176			} // namespace crocoddyl