GCC Code Coverage Report

Directory: ./
File: tests/tools/test_robot_utils.cpp
Date: 2024-12-13 12:22:33
Exec Total Coverage
Lines: 80 93 86.0%
Branches: 143 318 45.0%
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/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 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/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 robot_util->set_joint_limits_for_id(1, lower_lim, upper_lim);
30
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 2 if (robot_util->get_joint_limits_from_id(1).upper == upper_lim &&
31
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 robot_util->get_joint_limits_from_id(1).lower == lower_lim) {
32
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 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
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 2 if (robot_util->cp_get_joint_limits_from_id(1).upper == upper_lim &&
37
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 robot_util->cp_get_joint_limits_from_id(1).lower == lower_lim) {
38
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 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
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 const std::string joint_name("test_joint");
46 1 const Index joint_id(10);
47
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 robot_util->set_name_to_id(joint_name, joint_id);
48
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 2 if (robot_util->get_id_from_name(joint_name) == joint_id &&
49
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 robot_util->get_name_from_id(joint_id) == joint_name) {
50
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 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/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 robot_util->create_id_to_name_map();
57
58 /*Test set urdf_to_sot */
59
60
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 dynamicgraph::Vector urdf_to_sot(3);
61
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 urdf_to_sot << 0, 2, 1;
62
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 robot_util->set_urdf_to_sot(urdf_to_sot);
63
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 if (urdf_to_sot == robot_util->m_dgv_urdf_to_sot) {
64
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 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
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 dynamicgraph::Vector q_urdf(3);
70
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 q_urdf << 10, 20, 30;
71
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 dynamicgraph::Vector q_sot(3);
72
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 dynamicgraph::Vector q_test_urdf(3);
73
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 robot_util->joints_urdf_to_sot(q_urdf, q_sot);
74
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 robot_util->joints_sot_to_urdf(q_sot, q_test_urdf);
75
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 if (q_urdf == q_test_urdf) {
76
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::cout << "joints_urdf_to_sot and joints_sot_to_urdf work !"
77
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 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
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 dynamicgraph::Vector q2_urdf(10);
86
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 dynamicgraph::Vector v_urdf(9);
87
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 dynamicgraph::Vector v_sot(9);
88
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 robot_util->velocity_urdf_to_sot(q2_urdf, v_urdf, v_sot);
89
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 robot_util->velocity_sot_to_urdf(q2_urdf, v_sot, v_urdf);
90
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::cout << "velocity_sot_to_urdf and velocity_urdf_to_sot work !"
91
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 << std::endl;
92
93 /*Test base_urdf_to_sot and base_sot_to_urdf */
94
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 dynamicgraph::Vector base_q_urdf(7);
95
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 dynamicgraph::Vector base_q_sot(6);
96
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 robot_util->base_urdf_to_sot(base_q_urdf, base_q_sot);
97
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 robot_util->base_sot_to_urdf(base_q_sot, base_q_urdf);
98
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 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
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 dynamicgraph::Vector q2_sot(9);
102
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 robot_util->config_urdf_to_sot(q2_urdf, q2_sot);
103
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 robot_util->config_sot_to_urdf(q2_sot, q2_urdf);
104
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 std::cout << "config_urdf_to_sot and config_sot_to_urdf work !" << std::endl;
105
106
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 robot_util->display(std::cout);
107
3/6
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 robot_util->sendMsg("test", MSG_TYPE_ERROR_STREAM);
108
109 /*Test set_name_to_force_id of forceutil */
110
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 const std::string rf("rf");
111
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 const std::string lf("lf");
112
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 const std::string lh("lh");
113
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 const std::string rh("rh");
114
115
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 robot_util->m_force_util.set_name_to_force_id(rf, 0);
116
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 robot_util->m_force_util.set_name_to_force_id(lf, 1);
117
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 robot_util->m_force_util.set_name_to_force_id(lh, 2);
118
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 robot_util->m_force_util.set_name_to_force_id(rh, 3);
119
120
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 dynamicgraph::Vector lf_lim(6);
121
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 1 lf_lim << 1, 2, 3, 4, 5, 6;
122
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 dynamicgraph::Vector uf_lim(6);
123
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 1 uf_lim << 10, 20, 30, 40, 50, 60;
124
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 robot_util->m_force_util.set_force_id_to_limits(1, lf_lim, uf_lim);
125
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 if (robot_util->m_force_util.get_id_from_name(rf) == 0 &&
126
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 robot_util->m_force_util.get_id_from_name(lf) == 1 &&
127
4/8
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 3 robot_util->m_force_util.get_id_from_name(lh) == 2 &&
128
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 robot_util->m_force_util.get_id_from_name(rh) == 3) {
129
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 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/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 if (robot_util->m_force_util.get_name_from_id(0) == rf &&
135
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 robot_util->m_force_util.get_name_from_id(1) == lf &&
136
4/8
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 3 robot_util->m_force_util.get_name_from_id(2) == lh &&
137
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 robot_util->m_force_util.get_name_from_id(3) == rh) {
138
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 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
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 2 if (robot_util->m_force_util.get_limits_from_id(1).upper == uf_lim &&
144
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 robot_util->m_force_util.get_limits_from_id(1).lower == lf_lim) {
145
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 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/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 robot_util->m_force_util.display(std::cout);
152
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 robot_util->m_foot_util.display(std::cout);
153 1 return 0;
154 1 }
155