Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/srdf.cpp	Lines:	61	61	100.0 %
Date:	2024-01-23 21:41:47	Branches:	302	604	50.0 %


//
// Copyright (c) 2016-2021 CNRS INRIA
//

#include <iostream>

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/srdf.hpp"

#include <boost/test/unit_test.hpp>

using namespace pinocchio;
using namespace std;

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE(test_removeCollisionPairs)
{
  using namespace pinocchio::urdf;
  using namespace pinocchio::srdf;
  const string model_filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
  const string model_dir = PINOCCHIO_MODEL_DIR;
  const string srdf_filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/srdf/romeo.srdf");

  Model model;
  buildModel(model_filename, model);

  GeometryModel geom_model;
  vector<string> paths; paths.push_back(model_dir);
  buildGeom(model,model_filename,COLLISION,geom_model,paths);

  geom_model.addAllCollisionPairs();

  const size_t num_init_col_pairs = geom_model.collisionPairs.size();

  removeCollisionPairs(model,geom_model,srdf_filename,false);
  const size_t num_col_pairs = geom_model.collisionPairs.size();

  BOOST_CHECK(num_init_col_pairs > num_col_pairs);
}

BOOST_AUTO_TEST_CASE(readReferenceConfig)
{
  using namespace pinocchio::urdf;
  using namespace pinocchio::srdf;
  const string model_filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
  const string srdf_filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.srdf");

  Model model;
  buildModel(model_filename, model);

  loadReferenceConfigurations(model,srdf_filename,false);
  Eigen::VectorXd q = model.referenceConfigurations["half_sitting"];
  Eigen::VectorXd q2 = model.referenceConfigurations["half_sitting2"];
  BOOST_CHECK(q.size() == model.nq);
  BOOST_CHECK(!q.isZero());

  BOOST_CHECK(q2.size() == model.nq);
  BOOST_CHECK(!q2.isZero());


}

BOOST_AUTO_TEST_CASE(readReferenceConfig_stream)
{
  const string urdf =
    "<robot name='test'>"
    "<link name='base_link'/>"
    "<link name='child_link'/>"
    "<joint type='revolute' name='joint'>"
    "  <parent link='base_link'/>"
    "  <child link='child_link'/>"
    "  <limit effort='30' velocity='1.0' />"
    "</joint>"
    "</robot>";
  const string srdf =
    "<robot name='test'>"
    "<group_state name='reference' group='all'>"
    "     <joint name='joint'  value='0.0' />"
    "</group_state>"
    "</robot>";

  Model model;
  pinocchio::urdf::buildModelFromXML(urdf, model);

  std::istringstream iss (srdf);
  pinocchio::srdf::loadReferenceConfigurationsFromXML(model,iss,false);

  Eigen::VectorXd q = model.referenceConfigurations["reference"];
  Eigen::VectorXd qexpected(1); qexpected << 0;
  BOOST_CHECK(q.size() == model.nq);
  BOOST_CHECK(q.isApprox(qexpected));
}

BOOST_AUTO_TEST_CASE(test_continuous_joint)
{
  const string urdf =
  "<robot name='test'>"
  "<link name='base_link'/>"
  "<link name='child_link'/>"
  "<joint type='continuous' name='joint'>"
  "  <parent link='base_link'/>"
  "  <child link='child_link'/>"
  "  <limit effort='30' velocity='1.0' />"
  "</joint>"
  "</robot>";
  const string srdf =
  "<robot name='test'>"
  "<group_state name='reference' group='all'>"
  "     <joint name='joint'  value='0.0' />"
  "</group_state>"
  "</robot>";

  Model model;
  pinocchio::urdf::buildModelFromXML(urdf, model);

  std::istringstream iss (srdf);
  pinocchio::srdf::loadReferenceConfigurationsFromXML(model,iss,false);

  Eigen::VectorXd q = model.referenceConfigurations["reference"];
  Eigen::VectorXd qexpected(2); qexpected << 1,0;
  BOOST_CHECK(q.size() == model.nq);
  BOOST_CHECK(q.isApprox(qexpected));
}

BOOST_AUTO_TEST_CASE(readRotorParams)
{
  using namespace pinocchio::urdf;
  using namespace pinocchio::srdf;
  const string model_filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
  const string srdf_filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.srdf");

  Model model;
  buildModel(model_filename, model);

  loadRotorParameters(model,srdf_filename,false);

  BOOST_CHECK(model.rotorInertia(model.joints[model.getJointId("WAIST_P")].idx_v())==1.0);
  BOOST_CHECK(model.rotorGearRatio(model.joints[model.getJointId("WAIST_R")].idx_v())==1.0);
}

BOOST_AUTO_TEST_SUITE_END()


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2016-2021 CNRS INRIA
3			//
4
5			#include <iostream>
6
7			#include "pinocchio/multibody/model.hpp"
8			#include "pinocchio/parsers/urdf.hpp"
9			#include "pinocchio/parsers/srdf.hpp"
10
11			#include <boost/test/unit_test.hpp>
12
13			using namespace pinocchio;
14			using namespace std;
15
16			BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
17
18	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_removeCollisionPairs)
19			{
20			using namespace pinocchio::urdf;
21			using namespace pinocchio::srdf;
22	✓✗✓✗	6	const string model_filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
23	✓✗	4	const string model_dir = PINOCCHIO_MODEL_DIR;
24	✓✗✓✗	6	const string srdf_filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/srdf/romeo.srdf");
25
26	✓✗	4	Model model;
27	✓✗	2	buildModel(model_filename, model);
28
29	✓✗	4	GeometryModel geom_model;
30	✓✗	4	vector<string> paths; paths.push_back(model_dir);
31	✓✗	2	buildGeom(model,model_filename,COLLISION,geom_model,paths);
32
33	✓✗	2	geom_model.addAllCollisionPairs();
34
35		2	const size_t num_init_col_pairs = geom_model.collisionPairs.size();
36
37	✓✗	2	removeCollisionPairs(model,geom_model,srdf_filename,false);
38		2	const size_t num_col_pairs = geom_model.collisionPairs.size();
39
40	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(num_init_col_pairs > num_col_pairs);
41		2	}
42
43	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(readReferenceConfig)
44			{
45			using namespace pinocchio::urdf;
46			using namespace pinocchio::srdf;
47	✓✗✓✗	6	const string model_filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
48	✓✗✓✗	6	const string srdf_filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.srdf");
49
50	✓✗	4	Model model;
51	✓✗	2	buildModel(model_filename, model);
52
53	✓✗	2	loadReferenceConfigurations(model,srdf_filename,false);
54	✓✗✓✗ ✓✗	6	Eigen::VectorXd q = model.referenceConfigurations["half_sitting"];
55	✓✗✓✗ ✓✗	6	Eigen::VectorXd q2 = model.referenceConfigurations["half_sitting2"];
56	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(q.size() == model.nq);
57	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(!q.isZero());
58
59	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(q2.size() == model.nq);
60	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(!q2.isZero());
61
62
63		2	}
64
65	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(readReferenceConfig_stream)
66			{
67			const string urdf =
68			"<robot name='test'>"
69			"<link name='base_link'/>"
70			"<link name='child_link'/>"
71			"<joint type='revolute' name='joint'>"
72			" <parent link='base_link'/>"
73			" <child link='child_link'/>"
74			" <limit effort='30' velocity='1.0' />"
75			"</joint>"
76	✓✗	4	"</robot>";
77			const string srdf =
78			"<robot name='test'>"
79			"<group_state name='reference' group='all'>"
80			" <joint name='joint' value='0.0' />"
81			"</group_state>"
82	✓✗	4	"</robot>";
83
84	✓✗	4	Model model;
85	✓✗	2	pinocchio::urdf::buildModelFromXML(urdf, model);
86
87	✓✗	4	std::istringstream iss (srdf);
88	✓✗	2	pinocchio::srdf::loadReferenceConfigurationsFromXML(model,iss,false);
89
90	✓✗✓✗ ✓✗	6	Eigen::VectorXd q = model.referenceConfigurations["reference"];
91	✓✗✓✗	4	Eigen::VectorXd qexpected(1); qexpected << 0;
92	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(q.size() == model.nq);
93	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(q.isApprox(qexpected));
94		2	}
95
96	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_continuous_joint)
97			{
98			const string urdf =
99			"<robot name='test'>"
100			"<link name='base_link'/>"
101			"<link name='child_link'/>"
102			"<joint type='continuous' name='joint'>"
103			" <parent link='base_link'/>"
104			" <child link='child_link'/>"
105			" <limit effort='30' velocity='1.0' />"
106			"</joint>"
107	✓✗	4	"</robot>";
108			const string srdf =
109			"<robot name='test'>"
110			"<group_state name='reference' group='all'>"
111			" <joint name='joint' value='0.0' />"
112			"</group_state>"
113	✓✗	4	"</robot>";
114
115	✓✗	4	Model model;
116	✓✗	2	pinocchio::urdf::buildModelFromXML(urdf, model);
117
118	✓✗	4	std::istringstream iss (srdf);
119	✓✗	2	pinocchio::srdf::loadReferenceConfigurationsFromXML(model,iss,false);
120
121	✓✗✓✗ ✓✗	6	Eigen::VectorXd q = model.referenceConfigurations["reference"];
122	✓✗✓✗ ✓✗	4	Eigen::VectorXd qexpected(2); qexpected << 1,0;
123	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(q.size() == model.nq);
124	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(q.isApprox(qexpected));
125		2	}
126
127	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(readRotorParams)
128			{
129			using namespace pinocchio::urdf;
130			using namespace pinocchio::srdf;
131	✓✗✓✗	6	const string model_filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
132	✓✗✓✗	6	const string srdf_filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.srdf");
133
134	✓✗	4	Model model;
135	✓✗	2	buildModel(model_filename, model);
136
137	✓✗	2	loadRotorParameters(model,srdf_filename,false);
138
139	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(model.rotorInertia(model.joints[model.getJointId("WAIST_P")].idx_v())==1.0);
140	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(model.rotorGearRatio(model.joints[model.getJointId("WAIST_R")].idx_v())==1.0);
141		2	}
142
143			BOOST_AUTO_TEST_SUITE_END()