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