Directory: | ./ |
---|---|
File: | tests/tools/test_robot_utils.cpp |
Date: | 2024-08-13 12:13:25 |
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 |