GCC Code Coverage Report

Directory:	./
File:	benchmark/factory/legged-robots.hpp
Date:	2025-06-03 08:14:12

	Total	Coverage
Lines:	63	0.0%
Functions:	1	0.0%
Branches:	143	0.0%

  
      Line
      Branch
      Exec
      Source
    
      ///////////////////////////////////////////////////////////////////////////////
    
      // BSD 3-Clause License
    
      //
    
      // Copyright (C) 2019-2025, University of Edinburgh, LAAS-CNRS,
    
      //                          Heriot-Watt University
    
      // Copyright note valid unless otherwise stated in individual files.
    
      // All rights reserved.
    
      ///////////////////////////////////////////////////////////////////////////////
    
      #ifndef CROCODDYL_LEGGED_ROBOTS_FACTORY_HPP_
    
      #define CROCODDYL_LEGGED_ROBOTS_FACTORY_HPP_
    
      #include <example-robot-data/path.hpp>
    
      #include <pinocchio/parsers/srdf.hpp>
    
      #include <pinocchio/parsers/urdf.hpp>
    
      #include "crocoddyl/core/costs/cost-sum.hpp"
    
      #include "crocoddyl/core/costs/residual.hpp"
    
      #include "crocoddyl/core/integrator/euler.hpp"
    
      #include "crocoddyl/core/residuals/control.hpp"
    
      #include "crocoddyl/multibody/actions/contact-fwddyn.hpp"
    
      #include "crocoddyl/multibody/actuations/floating-base.hpp"
    
      #include "crocoddyl/multibody/contacts/contact-3d.hpp"
    
      #include "crocoddyl/multibody/contacts/contact-6d.hpp"
    
      #include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
    
      #include "crocoddyl/multibody/residuals/com-position.hpp"
    
      #include "crocoddyl/multibody/residuals/frame-placement.hpp"
    
      #include "crocoddyl/multibody/residuals/frame-translation.hpp"
    
      #include "crocoddyl/multibody/residuals/state.hpp"
    
      #include "crocoddyl/multibody/states/multibody.hpp"
    
      #include "robot-ee-names.hpp"
    
      namespace crocoddyl {
    
      namespace benchmark {
    
      template <typename Scalar>
    
      ✗
      void build_contact_action_models(
    
          RobotEENames robotNames,
    
          std::shared_ptr<crocoddyl::ActionModelAbstractTpl<Scalar> >& runningModel,
    
          std::shared_ptr<crocoddyl::ActionModelAbstractTpl<Scalar> >&
    
              terminalModel) {
    
        typedef
    
            typename crocoddyl::DifferentialActionModelContactFwdDynamicsTpl<Scalar>
    
                DifferentialActionModelContactFwdDynamics;
    
        typedef typename crocoddyl::IntegratedActionModelEulerTpl<Scalar>
    
            IntegratedActionModelEuler;
    
        typedef typename crocoddyl::ActuationModelFloatingBaseTpl<Scalar>
    
            ActuationModelFloatingBase;
    
        typedef typename crocoddyl::CostModelSumTpl<Scalar> CostModelSum;
    
        typedef typename crocoddyl::ContactModelMultipleTpl<Scalar>
    
            ContactModelMultiple;
    
        typedef typename crocoddyl::CostModelAbstractTpl<Scalar> CostModelAbstract;
    
        typedef typename crocoddyl::ContactModelAbstractTpl<Scalar>
    
            ContactModelAbstract;
    
        typedef typename crocoddyl::CostModelResidualTpl<Scalar> CostModelResidual;
    
        typedef typename crocoddyl::ResidualModelFramePlacementTpl<Scalar>
    
            ResidualModelFramePlacement;
    
        typedef typename crocoddyl::ResidualModelCoMPositionTpl<Scalar>
    
            ResidualModelCoMPosition;
    
        typedef typename crocoddyl::ResidualModelStateTpl<Scalar> ResidualModelState;
    
        typedef typename crocoddyl::ResidualModelControlTpl<Scalar>
    
            ResidualModelControl;
    
        typedef typename crocoddyl::ContactModel6DTpl<Scalar> ContactModel6D;
    
        typedef typename crocoddyl::ContactModel3DTpl<Scalar> ContactModel3D;
    
        typedef typename crocoddyl::MathBaseTpl<Scalar>::Vector2s Vector2s;
    
        typedef typename crocoddyl::MathBaseTpl<Scalar>::Vector3s Vector3s;
    
        typedef typename crocoddyl::MathBaseTpl<Scalar>::VectorXs VectorXs;
    
        typedef typename crocoddyl::MathBaseTpl<Scalar>::Matrix3s Matrix3s;
    
      ✗
        pinocchio::ModelTpl<double> modeld;
    
      ✗
        pinocchio::urdf::buildModel(robotNames.urdf_path,
    
      ✗
                                    pinocchio::JointModelFreeFlyer(), modeld);
    
      ✗
        modeld.lowerPositionLimit.head<7>().array() = -1;
    
      ✗
        modeld.upperPositionLimit.head<7>().array() = 1.;
    
      ✗
        pinocchio::srdf::loadReferenceConfigurations(modeld, robotNames.srdf_path,
    
                                                     false);
    
      ✗
        pinocchio::ModelTpl<Scalar> model(modeld.cast<Scalar>());
    
      ✗
        std::shared_ptr<crocoddyl::StateMultibodyTpl<Scalar> > state =
    
            std::make_shared<crocoddyl::StateMultibodyTpl<Scalar> >(
    
                std::make_shared<pinocchio::ModelTpl<Scalar> >(model));
    
      ✗
        VectorXs default_state(model.nq + model.nv);
    
      ✗
        default_state << model.referenceConfigurations[robotNames.reference_conf],
    
      ✗
            VectorXs::Zero(model.nv);
    
      ✗
        std::shared_ptr<ActuationModelFloatingBase> actuation =
    
            std::make_shared<ActuationModelFloatingBase>(state);
    
      ✗
        std::shared_ptr<CostModelAbstract> comCost =
    
            std::make_shared<CostModelResidual>(
    
                state, std::make_shared<ResidualModelCoMPosition>(
    
      ✗
                           state, Vector3s::Zero(), actuation->get_nu()));
    
      ✗
        std::shared_ptr<CostModelAbstract> goalTrackingCost =
    
            std::make_shared<CostModelResidual>(
    
                state, std::make_shared<ResidualModelFramePlacement>(
    
      ✗
                           state, model.getFrameId(robotNames.ee_name),
    
      ✗
                           pinocchio::SE3Tpl<Scalar>(
    
                               Matrix3s::Identity(),
    
      ✗
                               Vector3s(Scalar(.0), Scalar(.0), Scalar(.4))),
    
      ✗
                           actuation->get_nu()));
    
      ✗
        std::shared_ptr<CostModelAbstract> xRegCost =
    
            std::make_shared<CostModelResidual>(
    
                state, std::make_shared<ResidualModelState>(state, default_state,
    
      ✗
                                                            actuation->get_nu()));
    
      ✗
        std::shared_ptr<CostModelAbstract> uRegCost =
    
            std::make_shared<CostModelResidual>(
    
                state,
    
      ✗
                std::make_shared<ResidualModelControl>(state, actuation->get_nu()));
    
        // Create a cost model per the running and terminal action model.
    
      ✗
        std::shared_ptr<CostModelSum> runningCostModel =
    
      ✗
            std::make_shared<CostModelSum>(state, actuation->get_nu());
    
      ✗
        std::shared_ptr<CostModelSum> terminalCostModel =
    
      ✗
            std::make_shared<CostModelSum>(state, actuation->get_nu());
    
        // Then let's added the running and terminal cost functions
    
      ✗
        runningCostModel->addCost("gripperPose", goalTrackingCost, Scalar(1));
    
        //   runningCostModel->addCost("comPos", comCost, Scalar(1e-7));
    
      ✗
        runningCostModel->addCost("xReg", xRegCost, Scalar(1e-4));
    
      ✗
        runningCostModel->addCost("uReg", uRegCost, Scalar(1e-4));
    
      ✗
        terminalCostModel->addCost("gripperPose", goalTrackingCost, Scalar(1));
    
      ✗
        std::shared_ptr<ContactModelMultiple> contact_models =
    
      ✗
            std::make_shared<ContactModelMultiple>(state, actuation->get_nu());
    
      ✗
        for (std::size_t i = 0; i < robotNames.contact_names.size(); ++i) {
    
      ✗
          switch (robotNames.contact_types[i]) {
    
      ✗
            case Contact3D: {
    
      ✗
              std::shared_ptr<ContactModelAbstract> support_contact =
    
                  std::make_shared<ContactModel3D>(
    
      ✗
                      state, model.getFrameId(robotNames.contact_names[i]),
    
      ✗
                      Eigen::Vector3d::Zero(), pinocchio::LOCAL_WORLD_ALIGNED,
    
      ✗
                      actuation->get_nu(), Vector2s(Scalar(0.), Scalar(50.)));
    
      ✗
              contact_models->addContact(
    
      ✗
                  model.frames[model.getFrameId(robotNames.contact_names[i])].name,
    
                  support_contact);
    
      ✗
              break;
    
      ✗
            }
    
      ✗
            case Contact6D: {
    
      ✗
              std::shared_ptr<ContactModelAbstract> support_contact =
    
                  std::make_shared<ContactModel6D>(
    
      ✗
                      state, model.getFrameId(robotNames.contact_names[i]),
    
                      pinocchio::SE3Tpl<Scalar>::Identity(),
    
      ✗
                      pinocchio::LOCAL_WORLD_ALIGNED, actuation->get_nu(),
    
      ✗
                      Vector2s(Scalar(0.), Scalar(50.)));
    
      ✗
              contact_models->addContact(
    
      ✗
                  model.frames[model.getFrameId(robotNames.contact_names[i])].name,
    
                  support_contact);
    
      ✗
              break;
    
      ✗
            }
    
      ✗
            default: {
    
      ✗
              break;
    
            }
    
          }
    
        }
    
        // Next, we need to create an action model for running and terminal nodes
    
      ✗
        std::shared_ptr<DifferentialActionModelContactFwdDynamics> runningDAM =
    
            std::make_shared<DifferentialActionModelContactFwdDynamics>(
    
                state, actuation, contact_models, runningCostModel);
    
      ✗
        std::shared_ptr<DifferentialActionModelContactFwdDynamics> terminalDAM =
    
            std::make_shared<DifferentialActionModelContactFwdDynamics>(
    
                state, actuation, contact_models, terminalCostModel);
    
      ✗
        runningModel =
    
      ✗
            std::make_shared<IntegratedActionModelEuler>(runningDAM, Scalar(5e-3));
    
      ✗
        terminalModel =
    
      ✗
            std::make_shared<IntegratedActionModelEuler>(terminalDAM, Scalar(5e-3));
    
      ✗
      }
    
      }  // namespace benchmark
    
      }  // namespace crocoddyl
    
      #endif  // CROCODDYL_BIPED_FACTORY_HPP_

Line	Exec	Source
1		///////////////////////////////////////////////////////////////////////////////
2		// BSD 3-Clause License
3		//
4		// Copyright (C) 2019-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		#ifndef CROCODDYL_LEGGED_ROBOTS_FACTORY_HPP_
11		#define CROCODDYL_LEGGED_ROBOTS_FACTORY_HPP_
12
13		#include <example-robot-data/path.hpp>
14		#include <pinocchio/parsers/srdf.hpp>
15		#include <pinocchio/parsers/urdf.hpp>
16
17		#include "crocoddyl/core/costs/cost-sum.hpp"
18		#include "crocoddyl/core/costs/residual.hpp"
19		#include "crocoddyl/core/integrator/euler.hpp"
20		#include "crocoddyl/core/residuals/control.hpp"
21		#include "crocoddyl/multibody/actions/contact-fwddyn.hpp"
22		#include "crocoddyl/multibody/actuations/floating-base.hpp"
23		#include "crocoddyl/multibody/contacts/contact-3d.hpp"
24		#include "crocoddyl/multibody/contacts/contact-6d.hpp"
25		#include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
26		#include "crocoddyl/multibody/residuals/com-position.hpp"
27		#include "crocoddyl/multibody/residuals/frame-placement.hpp"
28		#include "crocoddyl/multibody/residuals/frame-translation.hpp"
29		#include "crocoddyl/multibody/residuals/state.hpp"
30		#include "crocoddyl/multibody/states/multibody.hpp"
31		#include "robot-ee-names.hpp"
32
33		namespace crocoddyl {
34		namespace benchmark {
35
36		template <typename Scalar>
37	✗	void build_contact_action_models(
38		RobotEENames robotNames,
39		std::shared_ptr<crocoddyl::ActionModelAbstractTpl<Scalar> >& runningModel,
40		std::shared_ptr<crocoddyl::ActionModelAbstractTpl<Scalar> >&
41		terminalModel) {
42		typedef
43		typename crocoddyl::DifferentialActionModelContactFwdDynamicsTpl<Scalar>
44		DifferentialActionModelContactFwdDynamics;
45		typedef typename crocoddyl::IntegratedActionModelEulerTpl<Scalar>
46		IntegratedActionModelEuler;
47		typedef typename crocoddyl::ActuationModelFloatingBaseTpl<Scalar>
48		ActuationModelFloatingBase;
49		typedef typename crocoddyl::CostModelSumTpl<Scalar> CostModelSum;
50		typedef typename crocoddyl::ContactModelMultipleTpl<Scalar>
51		ContactModelMultiple;
52		typedef typename crocoddyl::CostModelAbstractTpl<Scalar> CostModelAbstract;
53		typedef typename crocoddyl::ContactModelAbstractTpl<Scalar>
54		ContactModelAbstract;
55		typedef typename crocoddyl::CostModelResidualTpl<Scalar> CostModelResidual;
56		typedef typename crocoddyl::ResidualModelFramePlacementTpl<Scalar>
57		ResidualModelFramePlacement;
58		typedef typename crocoddyl::ResidualModelCoMPositionTpl<Scalar>
59		ResidualModelCoMPosition;
60		typedef typename crocoddyl::ResidualModelStateTpl<Scalar> ResidualModelState;
61		typedef typename crocoddyl::ResidualModelControlTpl<Scalar>
62		ResidualModelControl;
63		typedef typename crocoddyl::ContactModel6DTpl<Scalar> ContactModel6D;
64		typedef typename crocoddyl::ContactModel3DTpl<Scalar> ContactModel3D;
65		typedef typename crocoddyl::MathBaseTpl<Scalar>::Vector2s Vector2s;
66		typedef typename crocoddyl::MathBaseTpl<Scalar>::Vector3s Vector3s;
67		typedef typename crocoddyl::MathBaseTpl<Scalar>::VectorXs VectorXs;
68		typedef typename crocoddyl::MathBaseTpl<Scalar>::Matrix3s Matrix3s;
69
70	✗	pinocchio::ModelTpl<double> modeld;
71	✗	pinocchio::urdf::buildModel(robotNames.urdf_path,
72	✗	pinocchio::JointModelFreeFlyer(), modeld);
73	✗	modeld.lowerPositionLimit.head<7>().array() = -1;
74	✗	modeld.upperPositionLimit.head<7>().array() = 1.;
75	✗	pinocchio::srdf::loadReferenceConfigurations(modeld, robotNames.srdf_path,
76		false);
77
78	✗	pinocchio::ModelTpl<Scalar> model(modeld.cast<Scalar>());
79	✗	std::shared_ptr<crocoddyl::StateMultibodyTpl<Scalar> > state =
80		std::make_shared<crocoddyl::StateMultibodyTpl<Scalar> >(
81		std::make_shared<pinocchio::ModelTpl<Scalar> >(model));
82
83	✗	VectorXs default_state(model.nq + model.nv);
84	✗	default_state << model.referenceConfigurations[robotNames.reference_conf],
85	✗	VectorXs::Zero(model.nv);
86
87	✗	std::shared_ptr<ActuationModelFloatingBase> actuation =
88		std::make_shared<ActuationModelFloatingBase>(state);
89
90	✗	std::shared_ptr<CostModelAbstract> comCost =
91		std::make_shared<CostModelResidual>(
92		state, std::make_shared<ResidualModelCoMPosition>(
93	✗	state, Vector3s::Zero(), actuation->get_nu()));
94	✗	std::shared_ptr<CostModelAbstract> goalTrackingCost =
95		std::make_shared<CostModelResidual>(
96		state, std::make_shared<ResidualModelFramePlacement>(
97	✗	state, model.getFrameId(robotNames.ee_name),
98	✗	pinocchio::SE3Tpl<Scalar>(
99		Matrix3s::Identity(),
100	✗	Vector3s(Scalar(.0), Scalar(.0), Scalar(.4))),
101	✗	actuation->get_nu()));
102	✗	std::shared_ptr<CostModelAbstract> xRegCost =
103		std::make_shared<CostModelResidual>(
104		state, std::make_shared<ResidualModelState>(state, default_state,
105	✗	actuation->get_nu()));
106	✗	std::shared_ptr<CostModelAbstract> uRegCost =
107		std::make_shared<CostModelResidual>(
108		state,
109	✗	std::make_shared<ResidualModelControl>(state, actuation->get_nu()));
110
111		// Create a cost model per the running and terminal action model.
112	✗	std::shared_ptr<CostModelSum> runningCostModel =
113	✗	std::make_shared<CostModelSum>(state, actuation->get_nu());
114	✗	std::shared_ptr<CostModelSum> terminalCostModel =
115	✗	std::make_shared<CostModelSum>(state, actuation->get_nu());
116
117		// Then let's added the running and terminal cost functions
118	✗	runningCostModel->addCost("gripperPose", goalTrackingCost, Scalar(1));
119		// runningCostModel->addCost("comPos", comCost, Scalar(1e-7));
120	✗	runningCostModel->addCost("xReg", xRegCost, Scalar(1e-4));
121	✗	runningCostModel->addCost("uReg", uRegCost, Scalar(1e-4));
122	✗	terminalCostModel->addCost("gripperPose", goalTrackingCost, Scalar(1));
123
124	✗	std::shared_ptr<ContactModelMultiple> contact_models =
125	✗	std::make_shared<ContactModelMultiple>(state, actuation->get_nu());
126
127	✗	for (std::size_t i = 0; i < robotNames.contact_names.size(); ++i) {
128	✗	switch (robotNames.contact_types[i]) {
129	✗	case Contact3D: {
130	✗	std::shared_ptr<ContactModelAbstract> support_contact =
131		std::make_shared<ContactModel3D>(
132	✗	state, model.getFrameId(robotNames.contact_names[i]),
133	✗	Eigen::Vector3d::Zero(), pinocchio::LOCAL_WORLD_ALIGNED,
134	✗	actuation->get_nu(), Vector2s(Scalar(0.), Scalar(50.)));
135	✗	contact_models->addContact(
136	✗	model.frames[model.getFrameId(robotNames.contact_names[i])].name,
137		support_contact);
138	✗	break;
139	✗	}
140	✗	case Contact6D: {
141	✗	std::shared_ptr<ContactModelAbstract> support_contact =
142		std::make_shared<ContactModel6D>(
143	✗	state, model.getFrameId(robotNames.contact_names[i]),
144		pinocchio::SE3Tpl<Scalar>::Identity(),
145	✗	pinocchio::LOCAL_WORLD_ALIGNED, actuation->get_nu(),
146	✗	Vector2s(Scalar(0.), Scalar(50.)));
147	✗	contact_models->addContact(
148	✗	model.frames[model.getFrameId(robotNames.contact_names[i])].name,
149		support_contact);
150	✗	break;
151	✗	}
152	✗	default: {
153	✗	break;
154		}
155		}
156		}
157
158		// Next, we need to create an action model for running and terminal nodes
159	✗	std::shared_ptr<DifferentialActionModelContactFwdDynamics> runningDAM =
160		std::make_shared<DifferentialActionModelContactFwdDynamics>(
161		state, actuation, contact_models, runningCostModel);
162	✗	std::shared_ptr<DifferentialActionModelContactFwdDynamics> terminalDAM =
163		std::make_shared<DifferentialActionModelContactFwdDynamics>(
164		state, actuation, contact_models, terminalCostModel);
165
166	✗	runningModel =
167	✗	std::make_shared<IntegratedActionModelEuler>(runningDAM, Scalar(5e-3));
168	✗	terminalModel =
169	✗	std::make_shared<IntegratedActionModelEuler>(terminalDAM, Scalar(5e-3));
170	✗	}
171
172		} // namespace benchmark
173		} // namespace crocoddyl
174
175		#endif // CROCODDYL_BIPED_FACTORY_HPP_
176