Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	tests/tools/test_robot_utils.cpp	Lines:	79	92	85.9 %
Date:	2023-03-13 12:09:37	Branches:	143	318	45.0 %


/*
 * Copyright 2019
 *
 * LAAS-CNRS
 *
 * François Bailly
 */

/* -------------------------------------------------------------------------- */
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
#include <dynamic-graph/factory.h>

#include <iostream>
#include <sot/core/debug.hh>
#include <sot/core/robot-utils.hh>

using namespace std;
using namespace dynamicgraph;
using namespace dynamicgraph::sot;

std::string localName("robot_test");
RobotUtilShrPtr robot_util;
int main(void) {
  robot_util = createRobotUtil(localName);
  /*Test set and get joint_limits_for_id */
  const double upper_lim(1);
  const double lower_lim(2);
  robot_util->set_joint_limits_for_id(1, lower_lim, upper_lim);
  if (robot_util->get_joint_limits_from_id(1).upper == upper_lim &&
      robot_util->get_joint_limits_from_id(1).lower == lower_lim) {
    std::cout << "joint_limits_for_id works !" << std::endl;
  } else {
    std::cout << "ERROR: joint_limits_for_id does not work !" << std::endl;
  }
  if (robot_util->cp_get_joint_limits_from_id(1).upper == upper_lim &&
      robot_util->cp_get_joint_limits_from_id(1).lower == lower_lim) {
    std::cout << "cp_get_joint_limits_for_id works !" << std::endl;
  } else {
    std::cout << "ERROR: cp_get_joint_limits_for_id does not work !"
              << std::endl;
  }

  /*Test set and get name_to_id */
  const std::string joint_name("test_joint");
  const Index joint_id(10);
  robot_util->set_name_to_id(joint_name, joint_id);
  if (robot_util->get_id_from_name(joint_name) == joint_id &&
      robot_util->get_name_from_id(joint_id) == joint_name) {
    std::cout << "name_to_id works !" << std::endl;
  } else {
    std::cout << "ERROR: name_to_id does not work !" << std::endl;
  }

  /*Test create_id_to_name_map */
  robot_util->create_id_to_name_map();

  /*Test set urdf_to_sot */

  dynamicgraph::Vector urdf_to_sot(3);
  urdf_to_sot << 0, 2, 1;
  robot_util->set_urdf_to_sot(urdf_to_sot);
  if (urdf_to_sot == robot_util->m_dgv_urdf_to_sot) {
    std::cout << "urdf_to_sot works !" << std::endl;
  } else {
    std::cout << "ERROR: urdf_to_sot does not work !" << std::endl;
  }
  /*Test joints_urdf_to_sot and joints_sot_to_urdf */
  dynamicgraph::Vector q_urdf(3);
  q_urdf << 10, 20, 30;
  dynamicgraph::Vector q_sot(3);
  dynamicgraph::Vector q_test_urdf(3);
  robot_util->joints_urdf_to_sot(q_urdf, q_sot);
  robot_util->joints_sot_to_urdf(q_sot, q_test_urdf);
  if (q_urdf == q_test_urdf) {
    std::cout << "joints_urdf_to_sot and joints_sot_to_urdf work !"
              << std::endl;
  } else {
    std::cout << "ERROR: joints_urdf_to_sot or joints_sot_to_urdf "
                 "do not work !"
              << std::endl;
  }

  /*Test velocity_sot_to_urdf and velocity_urdf_to_sot */
  dynamicgraph::Vector q2_urdf(10);
  dynamicgraph::Vector v_urdf(9);
  dynamicgraph::Vector v_sot(9);
  robot_util->velocity_urdf_to_sot(q2_urdf, v_urdf, v_sot);
  robot_util->velocity_sot_to_urdf(q2_urdf, v_sot, v_urdf);
  std::cout << "velocity_sot_to_urdf and velocity_urdf_to_sot work !"
            << std::endl;

  /*Test base_urdf_to_sot and base_sot_to_urdf */
  dynamicgraph::Vector base_q_urdf(7);
  dynamicgraph::Vector base_q_sot(6);
  robot_util->base_urdf_to_sot(base_q_urdf, base_q_sot);
  robot_util->base_sot_to_urdf(base_q_sot, base_q_urdf);
  std::cout << "base_urdf_to_sot and base_sot_to_urdf work !" << std::endl;

  /*Test config_urdf_to_sot and config_sot_to_urdf */
  dynamicgraph::Vector q2_sot(9);
  robot_util->config_urdf_to_sot(q2_urdf, q2_sot);
  robot_util->config_sot_to_urdf(q2_sot, q2_urdf);
  std::cout << "config_urdf_to_sot and config_sot_to_urdf work !" << std::endl;

  robot_util->display(std::cout);
  robot_util->sendMsg("test", MSG_TYPE_ERROR_STREAM);

  /*Test set_name_to_force_id of forceutil */
  const std::string rf("rf");
  const std::string lf("lf");
  const std::string lh("lh");
  const std::string rh("rh");

  robot_util->m_force_util.set_name_to_force_id(rf, 0);
  robot_util->m_force_util.set_name_to_force_id(lf, 1);
  robot_util->m_force_util.set_name_to_force_id(lh, 2);
  robot_util->m_force_util.set_name_to_force_id(rh, 3);

  dynamicgraph::Vector lf_lim(6);
  lf_lim << 1, 2, 3, 4, 5, 6;
  dynamicgraph::Vector uf_lim(6);
  uf_lim << 10, 20, 30, 40, 50, 60;
  robot_util->m_force_util.set_force_id_to_limits(1, lf_lim, uf_lim);
  if (robot_util->m_force_util.get_id_from_name(rf) == 0 &&
      robot_util->m_force_util.get_id_from_name(lf) == 1 &&
      robot_util->m_force_util.get_id_from_name(lh) == 2 &&
      robot_util->m_force_util.get_id_from_name(rh) == 3) {
    std::cout << "force_util set and get id_from_name work !" << std::endl;
  } else {
    std::cout << "ERROR: force_util set and get id_from_name do not work !"
              << std::endl;
  }
  if (robot_util->m_force_util.get_name_from_id(0) == rf &&
      robot_util->m_force_util.get_name_from_id(1) == lf &&
      robot_util->m_force_util.get_name_from_id(2) == lh &&
      robot_util->m_force_util.get_name_from_id(3) == rh) {
    std::cout << "force_util get name_from_id works !" << std::endl;
  } else {
    std::cout << "ERROR: force_util get name_from_id does not work !"
              << std::endl;
  }
  if (robot_util->m_force_util.get_limits_from_id(1).upper == uf_lim &&
      robot_util->m_force_util.get_limits_from_id(1).lower == lf_lim) {
    std::cout << "force_util set and get id to limits work !" << std::endl;
  } else {
    std::cout << "ERROR: force_util set and get id to "
                 "limits works do not work !"
              << std::endl;
  }
  robot_util->m_force_util.display(std::cout);
  robot_util->m_foot_util.display(std::cout);
  return 0;
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Copyright 2019
3			*
4			* LAAS-CNRS
5			*
6			* François Bailly
7			*/
8
9			/* -------------------------------------------------------------------------- */
10			/* --- INCLUDES ------------------------------------------------------------- */
11			/* -------------------------------------------------------------------------- */
12			#include <dynamic-graph/factory.h>
13
14			#include <iostream>
15			#include <sot/core/debug.hh>
16			#include <sot/core/robot-utils.hh>
17
18			using namespace std;
19			using namespace dynamicgraph;
20			using namespace dynamicgraph::sot;
21
22			std::string localName("robot_test");
23			RobotUtilShrPtr robot_util;
24		1	int main(void) {
25	✓✗	1	robot_util = createRobotUtil(localName);
26			/Test set and get joint_limits_for_id /
27		1	const double upper_lim(1);
28		1	const double lower_lim(2);
29	✓✗	1	robot_util->set_joint_limits_for_id(1, lower_lim, upper_lim);
30	✓✗✓✗ ✓✗	2	if (robot_util->get_joint_limits_from_id(1).upper == upper_lim &&
31	✓✗✓✗	1	robot_util->get_joint_limits_from_id(1).lower == lower_lim) {
32	✓✗✓✗	1	std::cout << "joint_limits_for_id works !" << std::endl;
33			} else {
34			std::cout << "ERROR: joint_limits_for_id does not work !" << std::endl;
35			}
36	✓✗✓✗ ✓✗	2	if (robot_util->cp_get_joint_limits_from_id(1).upper == upper_lim &&
37	✓✗✓✗	1	robot_util->cp_get_joint_limits_from_id(1).lower == lower_lim) {
38	✓✗✓✗	1	std::cout << "cp_get_joint_limits_for_id works !" << std::endl;
39			} else {
40			std::cout << "ERROR: cp_get_joint_limits_for_id does not work !"
41			<< std::endl;
42			}
43
44			/Test set and get name_to_id /
45	✓✗	2	const std::string joint_name("test_joint");
46		1	const Index joint_id(10);
47	✓✗	1	robot_util->set_name_to_id(joint_name, joint_id);
48	✓✗✓✗ ✓✗✓✗	2	if (robot_util->get_id_from_name(joint_name) == joint_id &&
49	✓✗	1	robot_util->get_name_from_id(joint_id) == joint_name) {
50	✓✗✓✗	1	std::cout << "name_to_id works !" << std::endl;
51			} else {
52			std::cout << "ERROR: name_to_id does not work !" << std::endl;
53			}
54
55			/Test create_id_to_name_map /
56	✓✗	1	robot_util->create_id_to_name_map();
57
58			/Test set urdf_to_sot /
59
60	✓✗	2	dynamicgraph::Vector urdf_to_sot(3);
61	✓✗✓✗ ✓✗	1	urdf_to_sot << 0, 2, 1;
62	✓✗	1	robot_util->set_urdf_to_sot(urdf_to_sot);
63	✓✗✓✗	1	if (urdf_to_sot == robot_util->m_dgv_urdf_to_sot) {
64	✓✗✓✗	1	std::cout << "urdf_to_sot works !" << std::endl;
65			} else {
66			std::cout << "ERROR: urdf_to_sot does not work !" << std::endl;
67			}
68			/Test joints_urdf_to_sot and joints_sot_to_urdf /
69	✓✗	2	dynamicgraph::Vector q_urdf(3);
70	✓✗✓✗ ✓✗	1	q_urdf << 10, 20, 30;
71	✓✗	2	dynamicgraph::Vector q_sot(3);
72	✓✗	2	dynamicgraph::Vector q_test_urdf(3);
73	✓✗✓✗ ✓✗	1	robot_util->joints_urdf_to_sot(q_urdf, q_sot);
74	✓✗✓✗ ✓✗	1	robot_util->joints_sot_to_urdf(q_sot, q_test_urdf);
75	✓✗✓✗	1	if (q_urdf == q_test_urdf) {
76	✓✗	1	std::cout << "joints_urdf_to_sot and joints_sot_to_urdf work !"
77	✓✗	1	<< std::endl;
78			} else {
79			std::cout << "ERROR: joints_urdf_to_sot or joints_sot_to_urdf "
80			"do not work !"
81			<< std::endl;
82			}
83
84			/Test velocity_sot_to_urdf and velocity_urdf_to_sot /
85	✓✗	2	dynamicgraph::Vector q2_urdf(10);
86	✓✗	2	dynamicgraph::Vector v_urdf(9);
87	✓✗	2	dynamicgraph::Vector v_sot(9);
88	✓✗✓✗ ✓✗✓✗	1	robot_util->velocity_urdf_to_sot(q2_urdf, v_urdf, v_sot);
89	✓✗✓✗ ✓✗✓✗	1	robot_util->velocity_sot_to_urdf(q2_urdf, v_sot, v_urdf);
90	✓✗	1	std::cout << "velocity_sot_to_urdf and velocity_urdf_to_sot work !"
91	✓✗	1	<< std::endl;
92
93			/Test base_urdf_to_sot and base_sot_to_urdf /
94	✓✗	2	dynamicgraph::Vector base_q_urdf(7);
95	✓✗	2	dynamicgraph::Vector base_q_sot(6);
96	✓✗✓✗ ✓✗	1	robot_util->base_urdf_to_sot(base_q_urdf, base_q_sot);
97	✓✗✓✗ ✓✗	1	robot_util->base_sot_to_urdf(base_q_sot, base_q_urdf);
98	✓✗✓✗	1	std::cout << "base_urdf_to_sot and base_sot_to_urdf work !" << std::endl;
99
100			/Test config_urdf_to_sot and config_sot_to_urdf /
101	✓✗	2	dynamicgraph::Vector q2_sot(9);
102	✓✗✓✗ ✓✗	1	robot_util->config_urdf_to_sot(q2_urdf, q2_sot);
103	✓✗✓✗ ✓✗	1	robot_util->config_sot_to_urdf(q2_sot, q2_urdf);
104	✓✗✓✗	1	std::cout << "config_urdf_to_sot and config_sot_to_urdf work !" << std::endl;
105
106	✓✗	1	robot_util->display(std::cout);
107	✓✗✓✗ ✓✗	1	robot_util->sendMsg("test", MSG_TYPE_ERROR_STREAM);
108
109			/Test set_name_to_force_id of forceutil /
110	✓✗	2	const std::string rf("rf");
111	✓✗	2	const std::string lf("lf");
112	✓✗	2	const std::string lh("lh");
113	✓✗	2	const std::string rh("rh");
114
115	✓✗	1	robot_util->m_force_util.set_name_to_force_id(rf, 0);
116	✓✗	1	robot_util->m_force_util.set_name_to_force_id(lf, 1);
117	✓✗	1	robot_util->m_force_util.set_name_to_force_id(lh, 2);
118	✓✗	1	robot_util->m_force_util.set_name_to_force_id(rh, 3);
119
120	✓✗	2	dynamicgraph::Vector lf_lim(6);
121	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	1	lf_lim << 1, 2, 3, 4, 5, 6;
122	✓✗	1	dynamicgraph::Vector uf_lim(6);
123	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	1	uf_lim << 10, 20, 30, 40, 50, 60;
124	✓✗	1	robot_util->m_force_util.set_force_id_to_limits(1, lf_lim, uf_lim);
125	✓✗	1	if (robot_util->m_force_util.get_id_from_name(rf) == 0 &&
126	✓✗✓✗	1	robot_util->m_force_util.get_id_from_name(lf) == 1 &&
127	✓✗✓✗ ✓✗✓✗	3	robot_util->m_force_util.get_id_from_name(lh) == 2 &&
128	✓✗✓✗	1	robot_util->m_force_util.get_id_from_name(rh) == 3) {
129	✓✗✓✗	1	std::cout << "force_util set and get id_from_name work !" << std::endl;
130			} else {
131			std::cout << "ERROR: force_util set and get id_from_name do not work !"
132			<< std::endl;
133			}
134	✓✗✓✗	2	if (robot_util->m_force_util.get_name_from_id(0) == rf &&
135	✓✗✓✗	2	robot_util->m_force_util.get_name_from_id(1) == lf &&
136	✓✗✓✗ ✓✗✓✗	3	robot_util->m_force_util.get_name_from_id(2) == lh &&
137	✓✗	1	robot_util->m_force_util.get_name_from_id(3) == rh) {
138	✓✗✓✗	1	std::cout << "force_util get name_from_id works !" << std::endl;
139			} else {
140			std::cout << "ERROR: force_util get name_from_id does not work !"
141			<< std::endl;
142			}
143	✓✗✓✗ ✓✗✓✗	2	if (robot_util->m_force_util.get_limits_from_id(1).upper == uf_lim &&
144	✓✗✓✗ ✓✗	1	robot_util->m_force_util.get_limits_from_id(1).lower == lf_lim) {
145	✓✗✓✗	1	std::cout << "force_util set and get id to limits work !" << std::endl;
146			} else {
147			std::cout << "ERROR: force_util set and get id to "
148			"limits works do not work !"
149			<< std::endl;
150			}
151	✓✗	1	robot_util->m_force_util.display(std::cout);
152	✓✗	1	robot_util->m_foot_util.display(std::cout);
153		1	return 0;
154			}