ObstacleAvoidingPlatformUnit.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package RobotAPI::ArmarXObjects::ObstacleAvoidingPlatformUnit
17  * @author Christian R. G. Dreher <c.dreher@kit.edu>
18  * @date 2020
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 
25 
26 
27 // STD/STL
28 #include <algorithm>
29 #include <cmath>
30 #include <limits>
31 #include <numeric>
32 
33 // Eigen
34 #include <Eigen/Core>
35 #include <Eigen/Geometry>
36 
37 // Simox
38 #include <SimoxUtility/math.h>
39 
40 // ArmarX
42 
43 
44 namespace
45 {
46 
47  void
48  invalidate(float& v)
49  {
50  v = std::numeric_limits<float>::infinity();
51  }
52 
53 
54  void
55  invalidate(Eigen::Vector2f& v)
56  {
57  v.x() = std::numeric_limits<float>::infinity();
58  v.y() = std::numeric_limits<float>::infinity();
59  }
60 
61 
62  void
63  invalidate(Eigen::Vector3f& v)
64  {
65  v.x() = std::numeric_limits<float>::infinity();
66  v.y() = std::numeric_limits<float>::infinity();
67  v.z() = std::numeric_limits<float>::infinity();
68  }
69 
70  template<typename T>
71  void invalidate(std::deque<T>& d)
72  {
73  d.clear();
74  }
75 
76 
77  std::string
79  {
80  switch (mode)
81  {
83  return "position";
85  return "velocity";
87  default:
88  return "unset";
89  }
90  }
91 
92 }
93 
94 
95 const std::string
96 armarx::ObstacleAvoidingPlatformUnit::default_name = "ObstacleAvoidingPlatformUnit";
97 
98 
100 
101 
103 
104 
105 void
107 {
108  ARMARX_DEBUG << "Initializing " << getName() << ".";
109 
110  ARMARX_DEBUG << "Initialized " << getName() << ".";
111 }
112 
113 
114 void
116 {
117  ARMARX_DEBUG << "Starting " << getName() << ".";
118 
119  if (!hasRobot("robot"))
120  {
121  m_robot = addRobot("robot", VirtualRobot::RobotIO::eStructure);
122  }
123 
124  invalidate(m_control_data.target_vel);
125  invalidate(m_control_data.target_rot_vel);
126  invalidate(m_control_data.target_pos);
127  invalidate(m_control_data.target_ori);
128  invalidate(m_viz.start);
129  invalidate(m_control_data.vel_history);
130 
131  ARMARX_DEBUG << "Started " << getName() << ".";
132 }
133 
134 
135 void
137 {
138  ARMARX_DEBUG << "Exiting " << getName() << ".";
139 
140  schedule_high_level_control_loop(control_mode::none);
141 
142  ARMARX_DEBUG << "Exited " << getName() << ".";
143 }
144 
145 
146 std::string
148 const
149 {
150  return default_name;
151 }
152 
153 
154 void
156  const float target_pos_x,
157  const float target_pos_y,
158  const float target_ori,
159  const float pos_reached_threshold,
160  const float ori_reached_threshold,
161  const Ice::Current&)
162 {
163  using namespace simox::math;
164 
165  std::scoped_lock l{m_control_data.mutex};
166 
167  m_control_data.target_pos = Eigen::Vector2f{target_pos_x, target_pos_y};
168  m_control_data.target_ori = periodic_clamp<float>(target_ori, -M_PI, M_PI);
169  m_control_data.pos_reached_threshold = pos_reached_threshold;
170  m_control_data.ori_reached_threshold = ori_reached_threshold;
171 
172  // clear the buffer to prevent any movement into arbitrary directions
173  invalidate(m_control_data.vel_history);
174 
175  invalidate(m_control_data.target_vel);
176  invalidate(m_control_data.target_rot_vel);
177 
178  schedule_high_level_control_loop(control_mode::position);
179 }
180 
181 
182 void
184  const float target_vel_x,
185  const float target_vel_y,
186  const float target_rot_vel,
187  const Ice::Current&)
188 {
189  using namespace simox::math;
190 
191  std::scoped_lock l{m_control_data.mutex};
192 
193  m_control_data.target_vel = Eigen::Vector2f{target_vel_x, target_vel_y};
194  m_control_data.target_rot_vel = periodic_clamp<float>(target_rot_vel, -M_PI, M_PI);
195 
196  invalidate(m_control_data.target_pos);
197  invalidate(m_control_data.target_ori);
198 
199  ARMARX_CHECK(m_control_data.target_vel.allFinite());
200  ARMARX_CHECK(std::isfinite(m_control_data.target_rot_vel));
201 
202  schedule_high_level_control_loop(control_mode::velocity);
203 }
204 
205 
206 void
208  const float target_pos_delta_x,
209  const float target_pos_delta_y,
210  const float target_delta_ori,
211  const float pos_reached_threshold,
212  const float ori_reached_threshold,
213  const Ice::Current& current)
214 {
215  using namespace simox::math;
216 
217  // Transform relative to global.
218  std::unique_lock lock{m_control_data.mutex};
219  synchronizeLocalClone(m_robot);
220  const Eigen::Vector2f agent_pos = m_robot->getGlobalPosition().head<2>();
221  const float agent_ori = mat4f_to_rpy(m_robot->getGlobalPose()).z();
222  lock.unlock();
223 
224  // Use moveTo.
225  moveTo(
226  agent_pos.x() + target_pos_delta_x,
227  agent_pos.y() + target_pos_delta_y,
228  agent_ori + target_delta_ori,
229  pos_reached_threshold,
230  ori_reached_threshold,
231  current);
232 }
233 
234 
235 void
237  const float max_pos_vel,
238  const float max_rot_vel,
239  const Ice::Current&)
240 {
241  std::scoped_lock l{m_control_data.mutex};
242  m_control_data.max_vel = max_pos_vel;
243  m_control_data.max_rot_vel = max_rot_vel;
244  m_platform->setMaxVelocities(max_pos_vel, max_rot_vel);
245 }
246 
247 
248 void
250 {
251  schedule_high_level_control_loop(control_mode::none);
252  m_platform->stopPlatform();
253 }
254 
255 
256 void
257 armarx::ObstacleAvoidingPlatformUnit::schedule_high_level_control_loop(control_mode mode)
258 {
259  std::scoped_lock l{m_control_loop.mutex};
260 
261  // If the control mode didn't change, there's nothing to do.
262  if (m_control_loop.mode == mode)
263  {
264  return;
265  }
266 
267  // If a control loop is runnung, stop it and wait for termination.
268  if (m_control_loop.mode != mode and m_control_loop.task)
269  {
270  ARMARX_VERBOSE << "Stopping " << ::to_string(m_control_loop.mode) << " control.";
271  const bool join = true;
272  m_control_loop.task->stop(join);
273  }
274 
275  // If the new control mode is none, no new control loop needs to be created.
276  if (mode == control_mode::none)
277  {
278  ARMARX_VERBOSE << "Going into stand-by.";
279  return;
280  }
281 
282  // Start next control loop.
283  ARMARX_VERBOSE << "Starting " << ::to_string(mode) << " control.";
284  m_control_loop.mode = mode;
285  m_control_loop.task = new RunningTask<ObstacleAvoidingPlatformUnit>(
286  this,
287  &ObstacleAvoidingPlatformUnit::high_level_control_loop);
288  m_control_loop.task->start();
289 }
290 
291 
292 void
293 armarx::ObstacleAvoidingPlatformUnit::high_level_control_loop()
294 {
295  const control_mode mode = m_control_loop.mode;
296  ARMARX_VERBOSE << "Started " << ::to_string(mode) << " control.";
297 
298  try
299  {
300  CycleUtil cu{m_control_loop.cycle_time};
301  while (not m_control_loop.task->isStopped())
302  {
303  const velocities vels = get_velocities();
304 
305  // Debug.
307  m["err_dist"] = new Variant{vels.err_dist};
308  m["err_angular_dist"] = new Variant{vels.err_angular_dist};
309 
310  m["target_global_x"] = new Variant{vels.target_global.x()};
311  m["target_global_y"] = new Variant{vels.target_global.y()};
312  m["target_global_abs"] = new Variant{vels.target_global.norm()};
313 
314  m["target_local_x"] = new Variant{vels.target_local.x()};
315  m["target_local_y"] = new Variant{vels.target_local.y()};
316  m["target_local_abs"] = new Variant(vels.target_local.norm());
317  m["target_rot"] = new Variant{vels.target_rot};
318 
319  m["modulated_global_x"] = new Variant{vels.modulated_global.x()};
320  m["modulated_global_y"] = new Variant{vels.modulated_global.y()};
321  m["modulated_global_abs"] = new Variant{vels.modulated_global.norm()};
322 
323  m["modulated_local_x"] = new Variant{vels.modulated_local.x()};
324  m["modulated_local_y"] = new Variant{vels.modulated_local.y()};
325  m["modulated_local_abs"] = new Variant{vels.modulated_local.norm()};
326 
327  setDebugObserverChannel("ObstacleAvoidingPlatformCtrl", m);
328 
329  m_platform->move(vels.modulated_local.x(), vels.modulated_local.y(), vels.target_rot);
330  visualize(vels);
331  cu.waitForCycleDuration();
332  }
333  }
334  catch (const std::exception& e)
335  {
336  ARMARX_ERROR << "Error occured while running control loop.\n"
337  << e.what();
338  }
339  catch (...)
340  {
341  ARMARX_ERROR << "Unknown error occured while running control loop.";
342  }
343 
344  // clear the buffer such that zero-velocity is set without delay
345  invalidate(m_control_data.vel_history);
346 
347  m_platform->move(0, 0, 0);
348  m_platform->stopPlatform();
349  m_control_loop.mode = control_mode::none;
350 
351  visualize();
352 
353  ARMARX_VERBOSE << "Stopped " << ::to_string(mode) << " control.";
354 }
355 
356 
357 armarx::ObstacleAvoidingPlatformUnit::velocities
358 armarx::ObstacleAvoidingPlatformUnit::get_velocities()
359 {
360  using namespace simox::math;
361 
362  // Acquire control_data mutex to ensure data remains consistent.
363  std::scoped_lock l{m_control_data.mutex};
364 
365  // Update agent and get target velocities.
366  update_agent_dependent_values();
367  const Eigen::Vector2f target_vel = get_target_velocity();
368  const float target_rot_vel = get_target_rotational_velocity();
369 
370  // Apply obstacle avoidance and apply post processing to get the modulated velocity.
371  const Eigen::Vector2f modulated_vel = [this, &target_vel]
372  {
373  obstacleavoidance::Agent agent;
374  agent.safety_margin = m_control_data.agent_safety_margin;
375  agent.pos = Eigen::Vector3f{m_control_data.agent_pos.x(), m_control_data.agent_pos.y(), 0};
376  agent.desired_vel = Eigen::Vector3f{target_vel.x(), target_vel.y(), 0};
377 
378  const Eigen::Vector2f raw = m_obstacle_avoidance->modulateVelocity(agent).head<2>();
379  return post_process_vel(target_vel, norm_max(raw, m_control_data.max_vel));
380  }();
381 
382  ARMARX_CHECK(modulated_vel.allFinite()) << "Velocity contains non-finite values.";
383  ARMARX_CHECK(std::isfinite(target_rot_vel)) << "Rotation velocity is non-finite.";
384 
385  ARMARX_DEBUG << "Target velocity: " << target_vel.transpose() << "; norm: " << target_vel.norm() << "; " << target_rot_vel;
386  ARMARX_DEBUG << "Modulated velocity: " << modulated_vel.transpose() << modulated_vel.norm();
387 
388  const auto r = Eigen::Rotation2D(m_control_data.agent_ori).toRotationMatrix().inverse();
389 
390  velocities vels;
391  vels.target_local = r * target_vel;
392  vels.target_global = target_vel;
393  vels.modulated_local = r * modulated_vel;
394  vels.modulated_global = modulated_vel;
395  vels.target_rot = target_rot_vel;
396  vels.err_dist = m_control_data.target_dist;
397  vels.err_angular_dist = m_control_data.target_angular_dist;
398 
399  return vels;
400 }
401 
402 
403 Eigen::Vector2f
404 armarx::ObstacleAvoidingPlatformUnit::get_target_velocity()
405 const
406 {
407  using namespace simox::math;
408 
409  Eigen::Vector2f uncapped_target_vel = Eigen::Vector2f::Zero();
410 
411  if (m_control_loop.mode == control_mode::position /*and not target_position_reached()*/)
412  {
413  uncapped_target_vel =
414  (m_control_data.target_pos - m_control_data.agent_pos) * m_control_data.kp;
415  }
416  else if (m_control_loop.mode == control_mode::velocity)
417  {
418  uncapped_target_vel = m_control_data.target_vel;
419  }
420 
421  ARMARX_CHECK(uncapped_target_vel.allFinite());
422 
423  return norm_max(uncapped_target_vel, m_control_data.max_vel);
424 }
425 
426 
427 float
428 armarx::ObstacleAvoidingPlatformUnit::get_target_rotational_velocity()
429 const
430 {
431  using namespace simox::math;
432 
433  float uncapped_target_rot_vel = 0;
434 
435  if (m_control_loop.mode == control_mode::position /*and not target_orientation_reached()*/)
436  {
437  m_rot_pid_controller.update(m_control_data.target_angular_dist, 0);
438  uncapped_target_rot_vel = -m_rot_pid_controller.getControlValue();
439  }
440  else if (m_control_loop.mode == control_mode::velocity)
441  {
442  uncapped_target_rot_vel = m_control_data.target_rot_vel;
443  }
444 
445  ARMARX_CHECK(std::isfinite(uncapped_target_rot_vel));
446 
447  return std::copysign(std::min(std::fabs(uncapped_target_rot_vel), m_control_data.max_rot_vel),
448  uncapped_target_rot_vel);
449 }
450 
451 
452 void
453 armarx::ObstacleAvoidingPlatformUnit::update_agent_dependent_values()
454 {
455  using namespace simox::math;
456 
457  synchronizeLocalClone(m_robot);
458  m_control_data.agent_pos = m_robot->getGlobalPosition().head<2>();
459  m_control_data.agent_ori =
460  periodic_clamp<float>(mat4f_to_rpy(m_robot->getGlobalPose()).z(), -M_PI, M_PI);
461 
462  ARMARX_CHECK_GREATER(m_control_data.agent_ori, -M_PI);
463  ARMARX_CHECK_LESS(m_control_data.agent_ori, M_PI);
464 
465  // Update distances in position mode.
466  if (m_control_loop.mode == control_mode::position)
467  {
468  ARMARX_CHECK_GREATER(m_control_data.target_ori, -M_PI);
469  ARMARX_CHECK_LESS(m_control_data.target_ori, M_PI);
470  ARMARX_CHECK(m_control_data.target_pos.allFinite());
471  ARMARX_CHECK(std::isfinite(m_control_data.target_ori));
472 
473  m_control_data.target_dist =
474  (m_control_data.target_pos - m_control_data.agent_pos).norm();
475  m_control_data.target_angular_dist =
476  periodic_clamp<float>(m_control_data.target_ori - m_control_data.agent_ori,
477  -M_PI, M_PI);
478 
479  ARMARX_CHECK_GREATER(m_control_data.target_angular_dist, -M_PI);
480  ARMARX_CHECK_LESS(m_control_data.target_angular_dist, M_PI);
481 
482  ARMARX_DEBUG << "Distance to target: " << m_control_data.target_dist << " mm and "
483  << m_control_data.target_angular_dist << " rad.";
484  }
485  // Otherwise invalidate them.
486  else
487  {
488  invalidate(m_control_data.target_dist);
489  invalidate(m_control_data.target_angular_dist);
490  }
491 }
492 
493 
494 bool
495 armarx::ObstacleAvoidingPlatformUnit::target_position_reached()
496 const
497 {
498  if (m_control_loop.mode == control_mode::position)
499  {
500  return m_control_data.target_dist < m_control_data.pos_reached_threshold;
501  }
502 
503  // Cannot reach any target in non-position mode.
504  return false;
505 }
506 
507 
508 bool
509 armarx::ObstacleAvoidingPlatformUnit::target_orientation_reached()
510 const
511 {
512  if (m_control_loop.mode == control_mode::position)
513  {
514  return std::fabs(m_control_data.target_angular_dist) < m_control_data.ori_reached_threshold;
515  }
516 
517  // Cannot reach any target in non-position mode.
518  return false;
519 }
520 
521 
522 bool
523 armarx::ObstacleAvoidingPlatformUnit::target_reached()
524 const
525 {
526  if (m_control_loop.mode == control_mode::position)
527  {
528  return target_position_reached() and target_orientation_reached();
529  }
530 
531  return false;
532 }
533 
534 
535 Eigen::Vector2f
536 armarx::ObstacleAvoidingPlatformUnit::post_process_vel(
537  const Eigen::Vector2f& target_vel,
538  const Eigen::Vector2f& modulated_vel)
539 {
540  ARMARX_CHECK(target_vel.allFinite());
541  ARMARX_CHECK(modulated_vel.allFinite());
542 
543  // Update buffer.
544  m_control_data.vel_history.push_front(std::make_tuple(target_vel, modulated_vel));
545  const unsigned max_real_buffer_size =
546  std::max(m_control_data.amount_smoothing, m_control_data.amount_max_vel);
547  if (m_control_data.vel_history.size() > max_real_buffer_size)
548  {
549  m_control_data.vel_history.resize(max_real_buffer_size);
550  }
551 
552  // Smooth by calculating mean over the last `m_control_data.amount_smoothing` elements.
553  const Eigen::Vector2f mean_modulated_vel = calculate_mean_modulated_vel();
554 
555  const bool is_near_target = this->is_near_target(mean_modulated_vel);
556 
557  // Min velocity is determined through distance to target.
558  float min_vel = is_near_target ? m_control_data.min_vel_near_target : m_control_data.min_vel_general;
559  if (target_vel.norm() < min_vel)
560  {
561  min_vel = target_vel.norm();
562  }
563 
564  // Determine max velocity by considering the coherency between the last
565  // `m_control_data.amount_max_vel` pairs of desired and modulated velocities.
566  // If the robot is almost at the goal position and no collision needs to be avoided
567  // (aka moving into the direction of the target position), we can prevent overshooting by
568  // clipping the velocity. The reference velocity is computed by a standard P-controller.
569  const float max_vel = is_near_target ? target_vel.norm() : calculate_adaptive_max_vel();
570 
571  ARMARX_CHECK(mean_modulated_vel.allFinite());
572  ARMARX_CHECK(std::isfinite(min_vel));
573  ARMARX_CHECK(std::isfinite(max_vel));
574 
575  return simox::math::norm_clamp(mean_modulated_vel, min_vel, max_vel);
576 }
577 
578 
579 bool armarx::ObstacleAvoidingPlatformUnit::is_near_target(const Eigen::Vector2f& control_velocity) const noexcept
580 {
581  if (m_control_data.target_dist < m_control_data.pos_near_threshold)
582  {
583  const Eigen::Vector2f target_direction = m_control_data.target_pos - m_control_data.agent_pos;
584  const Eigen::Vector2f control_direction = control_velocity / control_velocity.norm();
585 
586  const float sim = simox::math::cosine_similarity(target_direction, control_direction);
587 
588  // if almost pointing into same direction
589  if (sim > cos(M_PI_4))
590  {
591  return true;
592  }
593  }
594 
595  return false;
596 }
597 
598 
599 Eigen::Vector2f
600 armarx::ObstacleAvoidingPlatformUnit::calculate_mean_modulated_vel()
601 const
602 {
603  const unsigned adaptive_buffer_size = calculate_adaptive_smoothing_buffer_size();
604  const unsigned elements =
605  std::min<unsigned>(m_control_data.vel_history.size(), adaptive_buffer_size);
606  auto end = m_control_data.vel_history.begin();
607  std::advance(end, elements);
608 
609  const auto transform =
610  [elements](
611  const Eigen::Vector2f & vel,
612  const std::tuple<Eigen::Vector2f, Eigen::Vector2f>& vels)
613  -> Eigen::Vector2f
614  {
615  return vel + std::get<1>(vels) * (1. / elements);
616  };
617 
618  return std::accumulate(m_control_data.vel_history.begin(),
619  end,
620  Eigen::Vector2f{0, 0},
621  transform);
622 }
623 
624 
625 unsigned
626 armarx::ObstacleAvoidingPlatformUnit::calculate_adaptive_smoothing_buffer_size()
627 const
628 {
629  // buffer_size(min_buffer_size_dist) = min_buffer_size
630  const float min_buffer_size_dist = 200;
631  const float min_buffer_size = 3;
632  // buffer_size(max_buffer_size_dist) = max_buffer_size
633  const float max_buffer_size_dist = 1500;
634  const float max_buffer_size = m_control_data.amount_smoothing;
635 
636  ARMARX_CHECK_LESS(min_buffer_size, max_buffer_size);
637 
638  if (m_control_loop.mode == control_mode::position)
639  {
640  // Given the two points, calculate m and b in: f(x) = m * x + b
641  const float m = (max_buffer_size - min_buffer_size) /
642  (max_buffer_size_dist - min_buffer_size_dist);
643  const float b = min_buffer_size - (m * min_buffer_size_dist);
644 
645 
646 
647 
648  // Calculate buffer size and clamp value if need be.
649  return static_cast<unsigned>(std::clamp(m * m_control_data.target_dist + b,
650  min_buffer_size, max_buffer_size));
651  }
652  else
653  {
654  // In velocity control mode, don't smooth that much.
655  return min_buffer_size;
656  }
657 }
658 
659 
660 float
661 armarx::ObstacleAvoidingPlatformUnit::calculate_adaptive_max_vel()
662 const
663 {
664  using namespace simox::math;
665 
666  if (m_control_loop.mode == control_mode::position and m_control_data.adaptive_max_vel_exp > 0)
667  {
668  std::vector<float> angular_similarities;
669 
670  const unsigned elements =
671  std::min<unsigned>(m_control_data.vel_history.size(), m_control_data.amount_max_vel);
672  auto end = m_control_data.vel_history.begin();
673  std::advance(end, elements);
674 
675  const auto transform =
676  [elements](const std::tuple<Eigen::Vector2f, Eigen::Vector2f>& vels) -> float
677  {
678  const auto& [desired_vel, modulated_vel] = vels;
679 
680  if (desired_vel.isZero() and modulated_vel.isZero())
681  {
682  return 1;
683  }
684  else if (desired_vel.isZero() xor modulated_vel.isZero())
685  {
686  return -1;
687  }
688 
689  return angular_similarity(desired_vel, modulated_vel) / elements;
690  };
691 
692  // TODO: GCC 9+: Use transform_reduce instead of transform + accumulate.
693  // const float mean_angular_similarity = std::transform_reduce(
694  // m_control_data.vel_history.begin(), end,
695  // 0,
696  // std::plus<float>(),
697  // transform);
698 
699  std::transform(m_control_data.vel_history.begin(),
700  end,
701  std::back_inserter(angular_similarities),
702  transform);
703 
704  const float mean_angular_similarity = std::accumulate(angular_similarities.begin(),
705  angular_similarities.end(),
706  0.f,
707  std::plus<float>());
708  const float max_vel_factor = std::pow(mean_angular_similarity,
709  m_control_data.adaptive_max_vel_exp);
710 
711  return max_vel_factor * m_control_data.max_vel;
712  }
713  else
714  {
715  return m_control_data.max_vel;
716  }
717 }
718 
719 
720 void
721 armarx::ObstacleAvoidingPlatformUnit::visualize()
722 {
723  const Eigen::Vector2f zero = Eigen::Vector2f::Zero();
724  velocities vels;
725  vels.target_local = zero;
726  vels.target_global = zero;
727  vels.modulated_local = zero;
728  vels.modulated_global = zero;
729  vels.target_rot = 0;
730 
731  visualize(vels);
732 }
733 
734 
735 void
736 armarx::ObstacleAvoidingPlatformUnit::visualize(const velocities& vels)
737 {
738  using namespace armarx::viz;
739 
740  if (not m_viz.enabled)
741  {
742  return;
743  }
744 
745  Eigen::Vector3f agent_pos{m_control_data.agent_pos.x(), m_control_data.agent_pos.y(), 0};
746 
747  // Progress. Only draw in position control mode.
748  Layer l_prog = arviz.layer("progress");
749  if (m_control_loop.mode == control_mode::position)
750  {
751  const float min_keypoint_dist = 50;
752 
753  // If no start is set, use current agent pos.
754  if (not m_viz.start.allFinite())
755  {
756  m_viz.start = agent_pos;
757  }
758 
759  const Eigen::Vector3f& last_keypoint_pos =
760  m_viz.path.size() >= 1 ? m_viz.path.back() : m_viz.start;
761 
762  // Keep a certain distance between path keypoints.
763  if ((last_keypoint_pos - agent_pos).norm() > min_keypoint_dist)
764  {
765  m_viz.path.push_back(agent_pos);
766  }
767 
768  // Draw path.
769  if (not target_reached())
770  {
771  // Start.
772  l_prog.add(Sphere{"start"}
773  .position(m_viz.start)
774  .color(Color::cyan(255, 64))
775  .radius(40));
776 
777  // Path.
778  for (unsigned i = 0; i < m_viz.path.size(); ++i)
779  {
780  l_prog.add(Sphere{"path_" + std::to_string(i + 1)}
781  .position(m_viz.path[i])
782  .color(Color::magenta(255, 128))
783  .radius(20));
784  }
785 
786  // Goal.
787  const Eigen::Vector3f target{m_control_data.target_pos.x(),
788  m_control_data.target_pos.y(),
789  0};
790  l_prog.add(Arrow{"goal"}
791  .fromTo(target + Eigen::Vector3f{0, 0, 220},
792  target + Eigen::Vector3f{0, 0, 40})
793  .color(Color::red(255, 128))
794  .width(20));
795  }
796  else
797  {
798  m_viz.path.clear();
799  invalidate(m_viz.start);
800  }
801  }
802 
803  // Velocities.
804  Layer l_vels = arviz.layer("velocities");
805  if (m_control_loop.mode != control_mode::none)
806  {
807  const float min_velocity = 15;
808  const Eigen::Vector3f from1{agent_pos + Eigen::Vector3f{0, 0, 2000}};
809  const Eigen::Vector3f from2 = from1 + Eigen::Vector3f{0, 0, 200};
810  const Eigen::Vector3f original{vels.target_global.x(), vels.target_global.y(), 0};
811  const Eigen::Vector3f modulated{vels.modulated_global.x(), vels.modulated_global.y(), 0};
812 
813  if (original.norm() > min_velocity)
814  {
815  l_vels.add(Arrow{"original"}
816  .fromTo(from1, from1 + original * 5)
817  .color(Color::green(255, 128))
818  .width(25));
819  }
820 
821  if (modulated.norm() > min_velocity)
822  {
823  l_vels.add(Arrow{"modulated"}
824  .fromTo(from2, from2 + modulated * 5)
825  .color(Color::cyan(255, 128))
826  .width(25));
827  }
828  }
829 
830  // Agent.
831  Layer l_agnt = arviz.layer("agent");
832  if (m_control_loop.mode != control_mode::none)
833  {
834  l_agnt.add(Sphere{"agent"}
835  .position(agent_pos)
836  .color(Color::red(255, 128))
837  .radius(50));
838 
839  // Agent safety margin.
840  if (m_control_data.agent_safety_margin > 0)
841  {
842  l_agnt.add(Cylinder{"agent_safety_margin"}
843  .pose(simox::math::pos_rpy_to_mat4f(agent_pos, -M_PI_2, 0, 0))
844  .color(Color::red(255, 64))
845  .radius(m_control_data.agent_safety_margin)
846  .height(2));
847  }
848  }
849 
850  arviz.commit({l_prog, l_vels, l_agnt});
851 }
852 
853 
856 {
858 
859  def->component(m_platform, "Platform");
860  def->component(m_obstacle_avoidance, "PlatformObstacleAvoidance");
861 
862  // Settings.
863  def->optional(m_control_data.adaptive_max_vel_exp, "adaptive_max_vel_exponent",
864  "Adaptive max vel exponent. This throttles the max_vel adaptively "
865  "depending on the angle between target velocity and modulated velocity. "
866  "0 = disable.");
867  def->optional(m_control_data.min_vel_near_target, "min_vel_near_target", "Velocity in [mm/s] "
868  "the robot should at least set when near the target");
869  def->optional(m_control_data.min_vel_general, "min_vel_general", "Velocity in [mm/s] the robot "
870  "should at least set on general");
871  def->optional(m_control_data.pos_near_threshold, "pos_near_threshold", "Distance in [mm] after "
872  "which the robot is considered to be near the target for min velocity, "
873  "smoothing, etc.");
874 
875  // Control parameters.
876  def->optional(m_control_data.kp, "control.pos.kp");
877  def->optional(m_rot_pid_controller.Kp, "control.rot.kp");
878  def->optional(m_rot_pid_controller.Ki, "control.rot.ki");
879  def->optional(m_rot_pid_controller.Kd, "control.rot.kd");
880  def->optional(m_control_loop.cycle_time, "control.pose.cycle_time", "Control loop cycle time.");
881 
882  // Obstacle avoidance parameter.
883  def->optional(m_control_data.agent_safety_margin, "doa.agent_safety_margin");
884 
885  return def;
886 }
armarx::ObstacleAvoidingPlatformUnit::control_mode
control_mode
Definition: ObstacleAvoidingPlatformUnit.h:68
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
Eigen
Definition: Elements.h:36
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:111
armarx::ObstacleAvoidingPlatformUnit::onInitPlatformUnit
virtual void onInitPlatformUnit() override
Definition: ObstacleAvoidingPlatformUnit.cpp:106
armarx::ObstacleAvoidingPlatformUnit::getDefaultName
virtual std::string getDefaultName() const override
Retrieve default name of component.
Definition: ObstacleAvoidingPlatformUnit.cpp:147
armarx::ObstacleAvoidingPlatformUnit::moveRelative
virtual void moveRelative(float target_pos_delta_x, float target_pos_delta_y, float target_delta_ori, float pos_reached_threshold, float ori_reached_threshold, const Ice::Current &=Ice::Current{}) override
Definition: ObstacleAvoidingPlatformUnit.cpp:207
armarx::ObstacleAvoidingPlatformUnit::move
virtual void move(float target_vel_x, float target_vel_y, float target_rot_vel, const Ice::Current &=Ice::Current{}) override
Definition: ObstacleAvoidingPlatformUnit.cpp:183
armarx::ObstacleAvoidingPlatformUnit::control_mode::none
@ none
boost::target
Vertex target(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:688
ARMARX_CHECK_GREATER
#define ARMARX_CHECK_GREATER(lhs, rhs)
This macro evaluates whether lhs is greater (>) than rhs and if it turns out to be false it will thro...
Definition: ExpressionException.h:116
ObstacleAvoidingPlatformUnit.h
armarx::viz::Arrow
Definition: Elements.h:198
ARMARX_CHECK_LESS
#define ARMARX_CHECK_LESS(lhs, rhs)
This macro evaluates whether lhs is less (<) than rhs and if it turns out to be false it will throw a...
Definition: ExpressionException.h:102
armarx::viz::Cylinder::height
Cylinder & height(float h)
Definition: Elements.h:85
armarx::viz::Layer::add
void add(ElementT const &element)
Definition: Layer.h:29
armarx::viz::Sphere
Definition: Elements.h:134
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
clamp
double clamp(double x, double a, double b)
Definition: point.hpp:125
armarx::ObstacleAvoidingPlatformUnit::onExitPlatformUnit
virtual void onExitPlatformUnit() override
Definition: ObstacleAvoidingPlatformUnit.cpp:136
armarx::viz::ElementOps::position
DerivedT & position(float x, float y, float z)
Definition: ElementOps.h:127
std::isfinite
bool isfinite(const std::vector< T, Ts... > &v)
Definition: algorithm.h:327
armarx::ObstacleAvoidingPlatformUnit::onStartPlatformUnit
virtual void onStartPlatformUnit() override
Definition: ObstacleAvoidingPlatformUnit.cpp:115
M_PI
#define M_PI
Definition: MathTools.h:17
armarx::ObstacleAvoidingPlatformUnit::control_mode::position
@ position
armarx::viz::Arrow::width
Arrow & width(float w)
Definition: Elements.h:211
armarx::ObstacleAvoidingPlatformUnit::setMaxVelocities
virtual void setMaxVelocities(float max_pos_vel, float max_rot_vel, const Ice::Current &=Ice::Current{}) override
Definition: ObstacleAvoidingPlatformUnit.cpp:236
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
armarx::viz::Cylinder
Definition: Elements.h:74
armarx::viz::Sphere::radius
Sphere & radius(float r)
Definition: Elements.h:138
armarx::ObstacleAvoidingPlatformUnit::default_name
static const std::string default_name
Definition: ObstacleAvoidingPlatformUnit.h:270
max
T max(T t1, T t2)
Definition: gdiam.h:48
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::ObstacleAvoidingPlatformUnit::~ObstacleAvoidingPlatformUnit
virtual ~ObstacleAvoidingPlatformUnit() override
armarx::viz::Color::red
static Color red(int r=255, int a=255)
Definition: Color.h:79
armarx::viz::Color::cyan
static Color cyan(int c=255, int a=255)
Green + Blue.
Definition: Color.h:98
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:40
armarx::viz::Arrow::fromTo
Arrow & fromTo(const Eigen::Vector3f &from, const Eigen::Vector3f &to)
Definition: Elements.h:218
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
armarx::navigation::algorithms::visualize
void visualize(const algorithms::Costmap &costmap, viz::Layer &layer, const std::string &name, const float zOffset)
Definition: visualization.cpp:20
armarx::ObstacleAvoidingPlatformUnit::ObstacleAvoidingPlatformUnit
ObstacleAvoidingPlatformUnit()
armarx::viz::Color::magenta
static Color magenta(int m=255, int a=255)
Red + Blue.
Definition: Color.h:110
armarx::viz::ElementOps::pose
DerivedT & pose(Eigen::Matrix4f const &pose)
Definition: ElementOps.h:159
armarx::ObstacleAvoidingPlatformUnit::moveTo
virtual void moveTo(float target_pos_x, float target_pos_y, float target_ori, float pos_reached_threshold, float ori_reached_threshold, const Ice::Current &=Ice::Current{}) override
Definition: ObstacleAvoidingPlatformUnit.cpp:155
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::transform
auto transform(const Container< InputT, Alloc > &in, OutputT(*func)(InputT const &)) -> Container< OutputT, typename std::allocator_traits< Alloc >::template rebind_alloc< OutputT > >
Convenience function (with less typing) to transform a container of type InputT into the same contain...
Definition: algorithm.h:315
armarx::viz::ElementOps::color
DerivedT & color(Color color)
Definition: ElementOps.h:195
armarx::ObstacleAvoidingPlatformUnit::createPropertyDefinitions
virtual PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: ObstacleAvoidingPlatformUnit.cpp:855
return
return() armarx_component_set_name(ArMarkerLocalizerOpenCV) armarx_build_if(OpenCV_FOUND "OpenCV not available") set(COMPONENT_LIBS ArmarXCoreInterfaces ArmarXCore ArmarXGuiComponentPlugins RobotAPICore RobotAPIComponentPlugins VisionXInterfaces VisionXCore VisionXTools opencv_core opencv_imgcodecs opencv_imgproc) set(SOURCES ArMarkerLocalizerOpenCV.cpp) set(HEADERS ArMarkerLocalizerOpenCV.h) armarx_add_component("$
Definition: CMakeLists.txt:1
armarx::ManagedIceObject::getName
std::string getName() const
Retrieve name of object.
Definition: ManagedIceObject.cpp:107
armarx::viz::Layer::size
std::size_t size() const noexcept
Definition: Layer.h:48
armarx::ObstacleAvoidingPlatformUnit::control_mode::velocity
@ velocity
armarx::viz::Color::green
static Color green(int g=255, int a=255)
Definition: Color.h:84
min
T min(T t1, T t2)
Definition: gdiam.h:42
armarx::ObstacleAvoidingPlatformUnit::stopPlatform
virtual void stopPlatform(const Ice::Current &=Ice::Current{}) override
Definition: ObstacleAvoidingPlatformUnit.cpp:249
armarx::viz::Layer
Definition: Layer.h:12
Variant.h
armarx::PlatformUnit::createPropertyDefinitions
PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: PlatformUnit.cpp:35
armarx::viz
This file is part of ArmarX.
Definition: ArVizStorage.cpp:370
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
norm
double norm(const Point &a)
Definition: point.hpp:94