Line data Source code
1 : #include "RobotController.hpp"
2 :
3 : #include "HLR/ObjectMoveStart.h"
4 : #include "Planners/InterpolatingAnglePlanner.hpp"
5 : #include "Planners/InterpolatingEuclideanPlanner.hpp"
6 :
7 : namespace HLR
8 : {
9 : namespace Kinematics
10 : {
11 0 : RobotController::RobotController(RobotDriverHandler& driver) : driver(driver)
12 : {
13 0 : this->driver.set_move_complete_handler(
14 0 : [this]() { this->position_reached(); });
15 :
16 0 : this->events.push(Event::move_to_ready);
17 0 : this->worker = std::thread(&RobotController::event_handler, this);
18 0 : }
19 :
20 0 : RobotController::~RobotController()
21 : {
22 0 : this->events.push(Event::terminate);
23 0 : this->worker.join();
24 0 : }
25 :
26 0 : RobotControllerState RobotController::get_state() const
27 : {
28 0 : std::lock_guard<std::mutex> l(this->state_m);
29 0 : return this->state;
30 : }
31 :
32 0 : void RobotController::pick_up_cup(std::uint32_t id)
33 : {
34 0 : std::lock_guard<std::mutex> l(this->state_m);
35 0 : if (this->state != RobotControllerState::idle)
36 : {
37 0 : throw std::logic_error("Robot is not currently idle.");
38 : }
39 :
40 : {
41 0 : std::lock_guard<std::recursive_mutex> l(this->objects_m);
42 0 : const auto it = this->objects.find(id);
43 0 : if (it == this->objects.end())
44 : {
45 : throw std::invalid_argument(
46 0 : "No object found with the specified id.");
47 : }
48 :
49 0 : if (!it->second.is_cup)
50 : {
51 : throw std::invalid_argument(
52 0 : "Specifed object exists but is not a cup.");
53 : }
54 0 : this->cup_position = it->second.position;
55 : }
56 0 : this->set_state(RobotControllerState::calculating_path);
57 0 : this->events.push(Event::pick_up_cup);
58 0 : }
59 :
60 0 : void RobotController::move_stop()
61 : {
62 0 : std::lock_guard<std::mutex> l(this->state_m);
63 0 : switch (this->state)
64 : {
65 0 : case RobotControllerState::moving_to_ready:
66 : case RobotControllerState::calculating_path:
67 : case RobotControllerState::giving_cup:
68 0 : this->set_state(RobotControllerState::stopped);
69 0 : this->events.push(Event::stop);
70 0 : break;
71 0 : default:
72 : throw std::logic_error(
73 0 : "Robot is not currently moving or movement is cancelling.");
74 : }
75 0 : }
76 :
77 0 : void RobotController::move_continue()
78 : {
79 0 : std::lock_guard<std::mutex> l(this->state_m);
80 0 : switch (this->state)
81 : {
82 0 : case RobotControllerState::stopped:
83 0 : this->set_state(this->prev_state);
84 0 : this->events.push(Event::resume);
85 0 : break;
86 0 : default:
87 0 : throw std::logic_error("Robot is not currently stopped.");
88 : }
89 0 : }
90 :
91 0 : void RobotController::move_cancel()
92 : {
93 0 : std::lock_guard<std::mutex> l(this->state_m);
94 : // Can't cancel a move to ready.
95 0 : if ((this->state == RobotControllerState::stopped
96 0 : && this->prev_state == RobotControllerState::moving_to_ready)
97 0 : || this->state == RobotControllerState::moving_to_ready)
98 : {
99 0 : throw std::logic_error("Can't cancel a move to ready.");
100 : }
101 :
102 0 : switch (this->state)
103 : {
104 0 : case RobotControllerState::stopped:
105 : case RobotControllerState::giving_cup:
106 : case RobotControllerState::calculating_path:
107 0 : this->set_state(RobotControllerState::cancelling);
108 0 : this->events.push(Event::cancel);
109 0 : break;
110 0 : default:
111 : throw std::logic_error(
112 0 : "Robot is not currently stopped or moving to a position.");
113 : }
114 0 : }
115 :
116 0 : void RobotController::remove_object(const EnvironmentObject& obj)
117 : {
118 0 : std::lock_guard<std::recursive_mutex> l(this->objects_m);
119 0 : const auto it = this->objects.find(obj.id);
120 0 : if (it != this->objects.end())
121 : {
122 0 : this->objects.erase(it);
123 : }
124 0 : }
125 :
126 0 : void RobotController::add_object(EnvironmentObject&& obj)
127 : {
128 0 : std::lock_guard<std::recursive_mutex> l(this->objects_m);
129 0 : this->remove_object(obj);
130 0 : const auto id = obj.id;
131 0 : this->objects.insert(std::make_pair(id, std::move(obj)));
132 0 : }
133 :
134 0 : void RobotController::set_state(RobotControllerState state)
135 : {
136 0 : this->prev_state = this->state;
137 0 : this->state = state;
138 0 : if (this->state_callback)
139 : {
140 0 : this->state_callback(state);
141 : }
142 0 : }
143 :
144 0 : void RobotController::set_error_callback(
145 : const std::function<void(ArmError)>& cb)
146 : {
147 0 : this->error_callback = cb;
148 0 : }
149 :
150 0 : void RobotController::set_state_change_callback(
151 : const std::function<void(RobotControllerState)>& cb)
152 : {
153 0 : this->state_callback = cb;
154 0 : }
155 :
156 0 : void RobotController::position_reached()
157 : {
158 0 : std::lock_guard<std::mutex> l(this->state_m);
159 0 : switch (this->state)
160 : {
161 0 : case RobotControllerState::moving_to_ready:
162 0 : this->set_state(RobotControllerState::idle);
163 0 : break;
164 0 : case RobotControllerState::cancelling:
165 : case RobotControllerState::giving_cup:
166 0 : this->set_state(RobotControllerState::moving_to_ready);
167 0 : this->events.push(Event::move_to_ready);
168 0 : break;
169 0 : default:
170 0 : return;
171 : }
172 : }
173 :
174 0 : void RobotController::event_handler()
175 : {
176 0 : bool term = false;
177 0 : while (!term)
178 : {
179 0 : auto ev = this->events.pop();
180 0 : switch (ev)
181 : {
182 0 : case Event::move_to_ready:
183 0 : this->move_to_ready_handler();
184 0 : break;
185 0 : case Event::pick_up_cup:
186 0 : this->pick_up_cup_handler();
187 0 : break;
188 0 : case Event::stop:
189 0 : this->stop_handler();
190 0 : break;
191 0 : case Event::resume:
192 0 : this->resume_handler();
193 0 : break;
194 0 : case Event::cancel:
195 0 : this->cancel_handler();
196 0 : break;
197 0 : case Event::terminate:
198 0 : term = true;
199 0 : break;
200 : }
201 : }
202 0 : }
203 :
204 0 : void RobotController::move_to_ready_handler()
205 : {
206 0 : const Planners::InterpolatingAnglePlanner planner(this->angle_step_size);
207 : auto path_opt =
208 0 : planner.get_path(this->driver.get_arm_state(), this->ready_pos);
209 0 : if (!path_opt.has_value())
210 : {
211 0 : if (this->error_callback)
212 : {
213 0 : this->error_callback(ArmError::no_path_found);
214 : }
215 0 : this->set_state(RobotControllerState::idle);
216 0 : return;
217 : }
218 :
219 0 : this->path = path_opt.value();
220 :
221 : try
222 : {
223 0 : this->driver.move(this->path, this->speed);
224 : }
225 0 : catch (const std::runtime_error& e)
226 : {
227 0 : if (this->error_callback)
228 : {
229 0 : this->error_callback(ArmError::driver_error);
230 : }
231 : }
232 : }
233 :
234 0 : void RobotController::pick_up_cup_handler()
235 : {
236 : // The different planners that are used.
237 : const Planners::InterpolatingAnglePlanner angle_planner(
238 0 : this->angle_step_size);
239 : const Planners::InterpolatingEuclideanPlanner euclidean_planner(
240 0 : this->euclidean_step_size);
241 :
242 : /**
243 : * The intermediate positions. The following positons are computed:
244 : * 1. Just prior to pickup up the cup.
245 : * 2. Move gripper around cup.
246 : * 3. Move gripper upwards to lock in cup.
247 : * 4. Move gripper with cup above the target position.
248 : * 5. Move gripper down so cup will stand on the target position.
249 : * 6. Move gripper back a little bit so the gripper no longer surrounds
250 : * the cup.
251 : */
252 0 : Matrix<double, 3, 1> ignore_y(this->cup_position);
253 0 : ignore_y[1][0] = 0;
254 0 : auto magnitude = ignore_y.get_magnitude();
255 0 : auto scalefactor = (magnitude - this->distance_from_cup) / magnitude;
256 :
257 0 : Matrix<double, 3, 1> pre_cup_position(this->cup_position);
258 0 : pre_cup_position[0][0] *= scalefactor;
259 0 : pre_cup_position[2][0] *= scalefactor;
260 :
261 0 : Matrix<double, 3, 1> lift_cup_postion(this->cup_position);
262 0 : lift_cup_postion[1][0] += lift_distance;
263 :
264 0 : Matrix<double, 3, 1> target_lift_position(this->target_position);
265 0 : target_lift_position[1][0] += lift_distance;
266 :
267 0 : ignore_y = this->target_position;
268 0 : ignore_y[1][0] = 0;
269 0 : magnitude = ignore_y.get_magnitude();
270 0 : scalefactor = (magnitude - this->distance_from_cup) / magnitude;
271 0 : Matrix<double, 3, 1> post_target_position(this->target_position);
272 0 : post_target_position[0][0] *= scalefactor;
273 0 : post_target_position[2][0] *= scalefactor;
274 :
275 : // All path parts.
276 : const auto to_pre_cup_opt =
277 0 : angle_planner.get_path(this->driver.get_arm_state(), pre_cup_position);
278 0 : if (!to_pre_cup_opt.has_value())
279 : {
280 0 : if (this->error_callback)
281 : {
282 0 : this->error_callback(ArmError::no_path_found);
283 : }
284 0 : this->set_state(RobotControllerState::idle);
285 0 : return;
286 : }
287 :
288 : const auto to_cup_opt = euclidean_planner.get_path(
289 0 : to_pre_cup_opt.value().back(), this->cup_position);
290 0 : if (!to_cup_opt.has_value())
291 : {
292 0 : if (this->error_callback)
293 : {
294 0 : this->error_callback(ArmError::no_path_found);
295 : }
296 0 : this->set_state(RobotControllerState::idle);
297 0 : return;
298 : }
299 :
300 : const auto lift_cup_opt =
301 0 : euclidean_planner.get_path(to_cup_opt.value().back(), lift_cup_postion);
302 0 : if (!lift_cup_opt.has_value())
303 : {
304 0 : if (this->error_callback)
305 : {
306 0 : this->error_callback(ArmError::no_path_found);
307 : }
308 0 : this->set_state(RobotControllerState::idle);
309 0 : return;
310 : }
311 :
312 : const auto target_lift_opt = angle_planner.get_path(
313 0 : lift_cup_opt.value().back(), target_lift_position);
314 0 : if (!target_lift_opt.has_value())
315 : {
316 0 : if (this->error_callback)
317 : {
318 0 : this->error_callback(ArmError::no_path_found);
319 : }
320 0 : this->set_state(RobotControllerState::idle);
321 0 : return;
322 : }
323 :
324 : const auto target_opt = euclidean_planner.get_path(
325 0 : target_lift_opt.value().back(), this->target_position);
326 0 : if (!target_opt.has_value())
327 : {
328 0 : if (this->error_callback)
329 : {
330 0 : this->error_callback(ArmError::no_path_found);
331 : }
332 0 : this->set_state(RobotControllerState::idle);
333 0 : return;
334 : }
335 :
336 : const auto post_target_opt = euclidean_planner.get_path(
337 0 : target_opt.value().back(), post_target_position);
338 0 : if (!post_target_opt.has_value())
339 : {
340 0 : if (this->error_callback)
341 : {
342 0 : this->error_callback(ArmError::no_path_found);
343 : }
344 0 : this->set_state(RobotControllerState::idle);
345 0 : return;
346 : }
347 :
348 : // Move all parts into single vector.
349 0 : this->path_index = 0;
350 0 : this->path.clear();
351 0 : this->path.reserve(
352 0 : to_pre_cup_opt.value().size() + to_cup_opt.value().size()
353 0 : + lift_cup_opt.value().size() + target_lift_opt.value().size()
354 0 : + target_opt.value().size() + post_target_opt.value().size());
355 0 : std::move(to_pre_cup_opt.value().begin(),
356 0 : to_pre_cup_opt.value().end(),
357 0 : std::back_inserter(this->path));
358 0 : std::move(to_cup_opt.value().begin(),
359 0 : to_cup_opt.value().end(),
360 0 : std::back_inserter(this->path));
361 0 : std::move(lift_cup_opt.value().begin(),
362 0 : lift_cup_opt.value().end(),
363 0 : std::back_inserter(this->path));
364 0 : std::move(target_lift_opt.value().begin(),
365 0 : target_lift_opt.value().end(),
366 0 : std::back_inserter(this->path));
367 0 : std::move(target_opt.value().begin(),
368 0 : target_opt.value().end(),
369 0 : std::back_inserter(this->path));
370 0 : std::move(post_target_opt.value().begin(),
371 0 : post_target_opt.value().end(),
372 0 : std::back_inserter(this->path));
373 :
374 : try
375 : {
376 0 : std::lock_guard<std::mutex> l(this->state_m);
377 0 : this->set_state(RobotControllerState::giving_cup);
378 0 : this->driver.move(this->path, this->speed);
379 : }
380 0 : catch (const std::runtime_error& e)
381 : {
382 0 : if (this->error_callback)
383 : {
384 0 : this->error_callback(ArmError::driver_error);
385 : }
386 : }
387 : }
388 :
389 0 : void RobotController::stop_handler()
390 : {
391 : try
392 : {
393 0 : this->driver.stop();
394 : }
395 0 : catch (const std::runtime_error& e)
396 : {
397 0 : if (this->error_callback)
398 : {
399 0 : this->error_callback(ArmError::driver_error);
400 : }
401 : }
402 0 : }
403 :
404 0 : void RobotController::resume_handler()
405 : {
406 0 : std::vector<Matrix<double, 5, 1>> remaining_path(this->path);
407 0 : remaining_path.erase(this->path.begin() + this->path_index,
408 0 : remaining_path.end());
409 : try
410 : {
411 0 : this->driver.move(remaining_path, this->speed);
412 : }
413 0 : catch (const std::runtime_error& e)
414 : {
415 0 : if (this->error_callback)
416 : {
417 0 : this->error_callback(ArmError::driver_error);
418 : }
419 : }
420 0 : }
421 :
422 0 : void RobotController::cancel_handler()
423 : {
424 0 : std::vector<Matrix<double, 5, 1>> remaining_path(this->path);
425 0 : remaining_path.erase(this->path.end() - this->path_index, this->path.end());
426 0 : std::reverse(remaining_path.begin(), remaining_path.end());
427 : try
428 : {
429 0 : this->driver.move(remaining_path, this->speed);
430 : }
431 0 : catch (const std::runtime_error& e)
432 : {
433 0 : if (this->error_callback)
434 : {
435 0 : this->error_callback(ArmError::driver_error);
436 : }
437 : }
438 0 : }
439 :
440 : } // namespace Kinematics
441 3 : } // namespace HLR
|
