GCC Code Coverage Report

Directory:	./
File:	unittest/factory/residual.cpp
Date:	2025-06-03 08:14:12

	Total	Coverage
Lines:	95	0.0%
Functions:	5	0.0%
Branches:	79	0.0%

  
      Line
      Branch
      Exec
      Source
    
      ///////////////////////////////////////////////////////////////////////////////
    
      // BSD 3-Clause License
    
      //
    
      // Copyright (C) 2021-2025, University of Edinburgh, LAAS-CNRS,
    
      //                          Heriot-Watt University
    
      // Copyright note valid unless otherwise stated in individual files.
    
      // All rights reserved.
    
      ///////////////////////////////////////////////////////////////////////////////
    
      #include "residual.hpp"
    
      #include "crocoddyl/core/residuals/control.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"
    
      #ifdef CROCODDYL_WITH_PAIR_COLLISION
    
      #include "crocoddyl/multibody/residuals/pair-collision.hpp"
    
      #endif  // CROCODDYL_WITH_PAIR_COLLISION
    
      #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
    
      #ifdef CROCODDYL_WITH_PAIR_COLLISION
    
          case ResidualModelTypes::ResidualModelPairCollision:
    
            os << "ResidualModelPairCollision";
    
            break;
    
      #endif  // CROCODDYL_WITH_PAIR_COLLISION
    
      #endif  // PINOCCHIO_WITH_HPP_FCL
    
      ✗
          case ResidualModelTypes::NbResidualModelTypes:
    
      ✗
            os << "NbResidualModelTypes";
    
      ✗
            break;
    
      ✗
          default:
    
      ✗
            break;
    
        }
    
      ✗
        return os;
    
      }
    
      ✗
      ResidualModelFactory::ResidualModelFactory() {}
    
      ✗
      ResidualModelFactory::~ResidualModelFactory() {}
    
      ✗
      std::shared_ptr<crocoddyl::ResidualModelAbstract> ResidualModelFactory::create(
    
          ResidualModelTypes::Type residual_type, StateModelTypes::Type state_type,
    
          std::size_t nu) const {
    
      ✗
        StateModelFactory state_factory;
    
      ✗
        std::shared_ptr<crocoddyl::ResidualModelAbstract> residual;
    
        std::shared_ptr<crocoddyl::StateMultibody> state =
    
            std::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
    
      #ifdef CROCODDYL_WITH_PAIR_COLLISION
    
        pinocchio::SE3 frame_SE3_obstacle = pinocchio::SE3::Random();
    
        std::shared_ptr<pinocchio::GeometryModel> geometry =
    
            std::make_shared<pinocchio::GeometryModel>(pinocchio::GeometryModel());
    
        pinocchio::GeomIndex ig_frame =
    
            geometry->addGeometryObject(pinocchio::GeometryObject(
    
                "frame", frame_index,
    
                state->get_pinocchio()->frames[frame_index].parentJoint,
    
                std::make_shared<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")]
    
                    .parentJoint,
    
                std::make_shared<hpp::fcl::Sphere>(0), frame_SE3_obstacle));
    
        geometry->addCollisionPair(pinocchio::CollisionPair(ig_frame, ig_obs));
    
      #endif  // CROCODDYL_WITH_PAIR_COLLISION
    
      #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 = std::make_shared<crocoddyl::ResidualModelState>(
    
      ✗
                state, state->rand(), nu);
    
      ✗
            break;
    
      ✗
          case ResidualModelTypes::ResidualModelControl:
    
      ✗
            residual = std::make_shared<crocoddyl::ResidualModelControl>(
    
      ✗
                state, Eigen::VectorXd::Random(nu));
    
      ✗
            break;
    
      ✗
          case ResidualModelTypes::ResidualModelCoMPosition:
    
      ✗
            residual = std::make_shared<crocoddyl::ResidualModelCoMPosition>(
    
      ✗
                state, Eigen::Vector3d::Random(), nu);
    
      ✗
            break;
    
      ✗
          case ResidualModelTypes::ResidualModelCentroidalMomentum:
    
      ✗
            residual = std::make_shared<crocoddyl::ResidualModelCentroidalMomentum>(
    
      ✗
                state, Vector6d::Random(), nu);
    
      ✗
            break;
    
      ✗
          case ResidualModelTypes::ResidualModelFramePlacement:
    
      ✗
            residual = std::make_shared<crocoddyl::ResidualModelFramePlacement>(
    
      ✗
                state, frame_index, frame_SE3, nu);
    
      ✗
            break;
    
      ✗
          case ResidualModelTypes::ResidualModelFrameRotation:
    
      ✗
            residual = std::make_shared<crocoddyl::ResidualModelFrameRotation>(
    
      ✗
                state, frame_index, frame_SE3.rotation(), nu);
    
      ✗
            break;
    
      ✗
          case ResidualModelTypes::ResidualModelFrameTranslation:
    
      ✗
            residual = std::make_shared<crocoddyl::ResidualModelFrameTranslation>(
    
      ✗
                state, frame_index, frame_SE3.translation(), nu);
    
      ✗
            break;
    
      ✗
          case ResidualModelTypes::ResidualModelFrameVelocity:
    
      ✗
            residual = std::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 =
    
      ✗
                std::make_shared<crocoddyl::ResidualModelControlGrav>(state, nu);
    
      ✗
            break;
    
      #ifdef PINOCCHIO_WITH_HPP_FCL
    
      #ifdef CROCODDYL_WITH_PAIR_COLLISION
    
          case ResidualModelTypes::ResidualModelPairCollision:
    
            residual = std::make_shared<crocoddyl::ResidualModelPairCollision>(
    
                state, nu, geometry, 0,
    
                state->get_pinocchio()->frames[frame_index].parentJoint);
    
            break;
    
      #endif  // CROCODDYL_WITH_PAIR_COLLISION
    
      #endif  // PINOCCHIO_WITH_HPP_FCL
    
      ✗
          default:
    
      ✗
            throw_pretty(__FILE__ ": Wrong ResidualModelTypes::Type given");
    
            break;
    
        }
    
      ✗
        return residual;
    
      ✗
      }
    
      ✗
      std::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

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