DSObstacleAvoidance.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::DSObstacleAvoidance
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 <cmath>
29 #include <fstream>
30 #include <limits>
31 #include <string>
32 #include <map>
33 #include <memory>
34 #include <mutex>
35 
36 // Simox
37 #include <VirtualRobot/Robot.h>
38 #include <SimoxUtility/json.h>
39 #include <SimoxUtility/math/convert.h>
40 
41 // Ice
42 #include <Ice/Current.h>
43 
44 // DynamicObstacleAvoidance
45 #include <DynamicObstacleAvoidance/Modulation.hpp>
46 #include <DynamicObstacleAvoidance/Obstacle/Ellipsoid.hpp>
47 #include <DynamicObstacleAvoidance/Utils/Plotting/PlottingTools.hpp>
48 namespace doa = DynamicObstacleAvoidance;
49 
50 // ArmarX
53 
54 // RobotAPI
56 
57 
58 namespace
59 {
60 
61  std::shared_ptr<doa::Obstacle>
62  create_doa_obstacle(const armarx::obstacledetection::Obstacle& obstacle)
63  {
64  Eigen::Vector3d pos(obstacle.posX, obstacle.posY, obstacle.posZ);
65  Eigen::Quaterniond ori{Eigen::AngleAxisd(obstacle.yaw, Eigen::Vector3d::UnitZ())};
66 
67  std::shared_ptr<doa::Ellipsoid> obstacle_ptr =
68  std::make_shared<doa::Ellipsoid>(obstacle.name,
69  doa::State(pos / 1000, ori),
70  Eigen::Array3d(obstacle.safetyMarginX / 1000,
71  obstacle.safetyMarginY / 1000,
72  obstacle.safetyMarginZ / 1000));
73 
74  obstacle_ptr->set_axis_lengths(Eigen::Array3d(obstacle.axisLengthX,
75  obstacle.axisLengthY,
76  obstacle.axisLengthZ) / 1000);
77  obstacle_ptr->set_reference_position(Eigen::Vector3d(obstacle.refPosX,
78  obstacle.refPosY,
79  obstacle.refPosZ) / 1000);
80  obstacle_ptr->set_curvature_factor(Eigen::Array3d(obstacle.curvatureX,
81  obstacle.curvatureY,
82  obstacle.curvatureZ));
83 
84  return obstacle_ptr;
85  }
86 
87 }
88 
89 
91 {
92 
93  void
94  from_json(const nlohmann::json& j, armarx::obstacledetection::Obstacle& obstacle)
95  {
96  j.at("name").get_to(obstacle.name);
97  j.at("posX").get_to(obstacle.posX);
98  j.at("posY").get_to(obstacle.posY);
99  if (j.find("posZ") != j.end())
100  {
101  j.at("posZ").get_to(obstacle.posZ);
102  }
103  j.at("yaw").get_to(obstacle.yaw);
104  j.at("axisLengthX").get_to(obstacle.axisLengthX);
105  j.at("axisLengthY").get_to(obstacle.axisLengthY);
106  if (j.find("axisLengthZ") != j.end())
107  {
108  j.at("axisLengthZ").get_to(obstacle.axisLengthZ);
109  }
110  j.at("refPosX").get_to(obstacle.refPosX);
111  j.at("refPosY").get_to(obstacle.refPosY);
112  if (j.find("refPosZ") != j.end())
113  {
114  j.at("refPosZ").get_to(obstacle.refPosZ);
115  }
116  if (j.find("safetyMarginX") != j.end())
117  {
118  j.at("safetyMarginX").get_to(obstacle.safetyMarginX);
119  }
120  if (j.find("safetyMarginY") != j.end())
121  {
122  j.at("safetyMarginY").get_to(obstacle.safetyMarginY);
123  }
124  if (j.find("safetyMarginZ") != j.end())
125  {
126  j.at("safetyMarginZ").get_to(obstacle.safetyMarginZ);
127  }
128  if (j.find("curvatureX") != j.end())
129  {
130  j.at("curvatureX").get_to(obstacle.curvatureX);
131  }
132  if (j.find("curvatureY") != j.end())
133  {
134  j.at("curvatureY").get_to(obstacle.curvatureY);
135  }
136  if (j.find("curvatureZ") != j.end())
137  {
138  j.at("curvatureZ").get_to(obstacle.curvatureZ);
139  }
140  }
141 
142 }
143 
144 
145 const std::string
146 armarx::DSObstacleAvoidance::default_name = "DSObstacleAvoidance";
147 
148 
150 {
151  // DOA config.
152  m_doa.cfg.only_2d = true;
153  m_doa.cfg.aggregated = true;
154  m_doa.cfg.agent_safety_margin = 0;
155  m_doa.cfg.local_modulation = false;
156  m_doa.cfg.repulsion = true;
157  m_doa.cfg.planar_modulation = false;
158  m_doa.cfg.critical_distance = 1.0;
159  m_doa.cfg.weight_power = 2.0;
160 }
161 
162 
163 void
165 {
166  ARMARX_DEBUG << "Initializing " << getName() << ".";
167 
168  m_buf.needs_env_update = false;
169 
170  m_doa.env.set_aggregated(m_doa.cfg.aggregated);
171 
172  if (not m_doa.load_obstacles_from_file.empty())
173  {
174  const std::filesystem::path scene_obstacles_path =
175  ArmarXDataPath::getAbsolutePath(m_doa.load_obstacles_from_file);
176  std::ifstream ifs(scene_obstacles_path);
177  nlohmann::json j = nlohmann::json::parse(ifs);
178 
179  std::vector<obstacledetection::Obstacle> obstacles = j;
180  for (obstacledetection::Obstacle& obstacle : obstacles)
181  {
182  if (m_doa.cfg.only_2d)
183  {
184  obstacle.posZ = 0;
185  obstacle.axisLengthZ = 0;
186  obstacle.refPosZ = 0;
187  obstacle.safetyMarginZ = 0;
188  obstacle.curvatureZ = 1;
189  }
190 
191  sanity_check_obstacle(obstacle);
192 
193  m_doa.env.add_obstacle(::create_doa_obstacle(obstacle));
194  }
195 
196  m_doa.env.update();
197  }
198 
199  writeDebugPlots("doa_upstart");
200 
201  ARMARX_DEBUG << "Initialized " << getName() << ".";
202 }
203 
204 
205 void
207 {
208  ARMARX_DEBUG << "Connecting " << getName() << ".";
209 
210  if (m_vis.enabled)
211  {
212  m_vis.task = new PeriodicTask<DSObstacleAvoidance>(
213  this,
214  &DSObstacleAvoidance::run_visualization, m_vis.task_interval);
215  m_vis.task->start();
216  }
217 
218  if (m_watchdog.enabled)
219  {
220  m_watchdog.task = new PeriodicTask<DSObstacleAvoidance>(
221  this,
222  &DSObstacleAvoidance::run_watchdog, m_watchdog.task_interval);
223  m_watchdog.task->start();
224  }
225 
226  ARMARX_DEBUG << "Connected " << getName() << ".";
227 }
228 
229 
230 void
232 {
233  ARMARX_DEBUG << "Disconnecting " << getName() << ".";
234 
235  if (m_vis.enabled)
236  {
237  m_vis.task->stop();
238  }
239 
240  if (m_watchdog.enabled)
241  {
242  m_watchdog.task->stop();
243  }
244 
245  ARMARX_DEBUG << "Disconnected " << getName() << ".";
246 }
247 
248 
249 void
251 {
252  ARMARX_DEBUG << "Exiting " << getName() << ".";
253 
254  ARMARX_DEBUG << "Exited " << getName() << ".";
255 }
256 
257 
258 std::string
260 const
261 {
262  return default_name;
263 }
264 
265 
266 armarx::dsobstacleavoidance::Config
268 {
269  return m_doa.cfg;
270 }
271 
272 
273 void
275  const armarx::obstacledetection::Obstacle& obstacle,
276  const Ice::Current&)
277 {
278  ARMARX_DEBUG << "Adding/updating obstacle: " << obstacle.name << ".";
279 
280  std::unique_lock l{m_buf.mutex};
281 
282  sanity_check_obstacle(obstacle);
283 
285  update.time = IceUtil::Time::now();
286  update.name = obstacle.name;
288 
289  m_buf.update_log.push_back(update);
290  m_buf.obstacles[obstacle.name] = obstacle;
291  m_buf.needs_env_update = true;
292 
293  ARMARX_DEBUG << "Added/updated obstacle: " << obstacle.name << ".";
294 }
295 
296 
297 void
298 armarx::DSObstacleAvoidance::removeObstacle(const std::string& obstacle_name, const Ice::Current&)
299 {
300  ARMARX_DEBUG << "Removing obstacle: " << obstacle_name << ".";
301 
302  std::unique_lock l{m_buf.mutex};
303 
305  update.time = IceUtil::Time::now();
306  update.name = obstacle_name;
307  update.type = buffer::update_type::removal;
308 
309  m_buf.update_log.push_back(update);
310  m_buf.needs_env_update = true;
311 
312  ARMARX_DEBUG << "Removed obstacle: " << obstacle_name << ".";
313 }
314 
315 
316 std::vector<armarx::obstacledetection::Obstacle>
318 {
319  ARMARX_DEBUG << "Get Obstacles";
320  std::shared_lock l{m_doa.mutex};
321 
322  std::vector<obstacledetection::Obstacle> obstacle_list;
323 
324  for (auto& [doa_name, doa_obstacle] : m_doa.env)
325  {
326  ARMARX_DEBUG << "Got an obtascle";
327  std::shared_ptr<doa::Ellipsoid> doa_ellipsoid = std::dynamic_pointer_cast<doa::Ellipsoid>(doa_obstacle);
328  obstacledetection::Obstacle obstacle;
329 
330  obstacle.name = doa_name;
331  obstacle.posX = doa_ellipsoid->get_position()(0) * 1000;
332  obstacle.posY = doa_ellipsoid->get_position()(1) * 1000;
333  obstacle.posZ = doa_ellipsoid->get_position()(2) * 1000;
334  obstacle.yaw = simox::math::mat3f_to_rpy(doa_ellipsoid->get_orientation().toRotationMatrix().cast<float>()).z();
335  obstacle.axisLengthX = doa_ellipsoid->get_axis_lengths(0) * 1000;
336  obstacle.axisLengthY = doa_ellipsoid->get_axis_lengths(1) * 1000;
337  obstacle.axisLengthZ = doa_ellipsoid->get_axis_lengths(2) * 1000;
338  obstacle.refPosX = doa_ellipsoid->get_reference_position()(0) * 1000;
339  obstacle.refPosY = doa_ellipsoid->get_reference_position()(1) * 1000;
340  obstacle.refPosZ = doa_ellipsoid->get_reference_position()(2) * 1000;
341  obstacle.safetyMarginX = doa_ellipsoid->get_safety_margin()(0) * 1000;
342  obstacle.safetyMarginY = doa_ellipsoid->get_safety_margin()(1) * 1000;
343  obstacle.safetyMarginZ = doa_ellipsoid->get_safety_margin()(2) * 1000;
344  obstacle.curvatureX = doa_ellipsoid->get_curvature_factor()(0);
345  obstacle.curvatureY = doa_ellipsoid->get_curvature_factor()(1);
346  obstacle.curvatureZ = doa_ellipsoid->get_curvature_factor()(2);
347 
348  ARMARX_DEBUG << "push back " << obstacle.name;
349  obstacle_list.push_back(std::move(obstacle));
350  }
351 
352  ARMARX_DEBUG << "Return obstacle list";
353  return obstacle_list;
354 }
355 
356 
357 Eigen::Vector3f
359  const obstacleavoidance::Agent& agent,
360  const Ice::Current&)
361 {
362 
363  ARMARX_DEBUG << "Modulate velocity";
364  std::shared_lock l{m_doa.mutex};
365 
366  // Create and initialize agent.
367  doa::Agent doa_agent{agent.safety_margin};
368  {
369  Eigen::Vector3d agent_global_position = agent.pos.cast<double>() / 1000;
370  Eigen::Vector3d agent_velocity = agent.desired_vel.cast<double>() / 1000;
371 
372  if (m_doa.cfg.only_2d)
373  {
374  agent_global_position(2) = 0;
375  agent_velocity(2) = 0;
376  }
377 
378  doa_agent.set_position(agent_global_position);
379  doa_agent.set_linear_velocity(agent_velocity);
380  }
381 
382  // Modulate velocity given the agent, the environment, and additional parameters.
383  Eigen::Vector3d modulated_vel = doa::Modulation::modulate_velocity(
384  doa_agent,
385  m_doa.env,
386  m_doa.cfg.local_modulation,
387  m_doa.cfg.repulsion,
388  m_doa.cfg.planar_modulation,
389  m_doa.cfg.critical_distance,
390  m_doa.cfg.weight_power);
391 
392  // Convert output back to mm.
393  modulated_vel *= 1000;
394 
395  // Set Z-component to 0 in 2D mode.
396  if (m_doa.cfg.only_2d)
397  {
398  modulated_vel(2) = 0;
399  }
400 
401  ARMARX_DEBUG << deactivateSpam(0.3) << "Target vel: " << modulated_vel;
402 
403  return modulated_vel.cast<float>();
404 }
405 
406 
407 void
409 {
410  ARMARX_VERBOSE << "Updating environment.";
411  std::lock_guard l2{m_vis.mutex};
412  std::lock_guard l{m_doa.mutex};
413 
414  // Make working copies of the updates and free them again.
415  std::vector<buffer::update> update_log;
416  std::map<std::string, obstacledetection::Obstacle> obstacles;
417  {
418  std::lock_guard l{m_buf.mutex};
419 
420  if (not m_buf.needs_env_update)
421  {
422  ARMARX_DEBUG << "Nothing to do.";
423  return;
424  }
425 
426  update_log = m_buf.update_log;
427  obstacles = m_buf.obstacles;
428  m_buf.update_log.clear();
429  m_buf.obstacles.clear();
430  m_buf.needs_env_update = false;
431  m_buf.last_env_update = IceUtil::Time::now();
432  }
433 
434  // Update environment.
436  for (const buffer::update& update : update_log)
437  {
438  doa::Environment::iterator it = m_doa.env.find(update.name);
439 
440  switch (update.type)
441  {
442  case buffer::update_type::removal:
443  {
444  ARMARX_DEBUG << "Removing obstacle " << update.name << ".";
445  if (it != m_doa.env.end())
446  {
447  m_doa.env.erase(update.name);
448  }
449  else
450  {
451  ARMARX_WARNING << "Tried to remove obstacle " << update.name << ", but this "
452  << "obstacle is unknown to the environment.";
453  }
454  }
455  break;
457  {
458  if (it != m_doa.env.end())
459  {
460  ARMARX_DEBUG << "Updating obstacle " << update.name << " with new values.";
461  m_doa.env.erase(update.name);
462  }
463  else
464  {
465  ARMARX_DEBUG << "Adding obstacle " << update.name << ".";
466  }
467 
468  const obstacledetection::Obstacle& obstacle = obstacles.at(update.name);
469  m_doa.env.add_obstacle(::create_doa_obstacle(obstacle));
470  }
471  break;
472  }
473  }
474  ARMARX_DEBUG << "Calling update on environment now.";
475  m_doa.env.update();
476 
477  {
478  m_vis.needs_update = true;
479  }
480 
481  ARMARX_VERBOSE << "Updating environment with " << update_log.size() << " updates took "
482  << sw.stop() << ".";
483 }
484 
485 
486 void
487 armarx::DSObstacleAvoidance::writeDebugPlots(const std::string& filename, const Ice::Current&)
488 {
489  ARMARX_DEBUG << "Write debug plots";
490  std::shared_lock l{m_doa.mutex};
491 
492  const std::string filename_annotated = getName() + "_" + filename;
493  ARMARX_DEBUG << "Writing debug plots to `/tmp/" << filename_annotated << ".png`.";
494 
495  const bool show = false;
496  try
497  {
498  doa::PlottingTools::plot_configuration(m_doa.env.get_obstacle_list(), filename_annotated,
499  show, not m_doa.cfg.only_2d);
500  }
501  catch (...)
502  {
503  ARMARX_WARNING << "Tried to write debug plots but failed, probably because "
504  << "`python3-matplotlib` are not installed.";
505  }
506 }
507 
508 
509 void
510 armarx::DSObstacleAvoidance::sanity_check_obstacle(
511  const armarx::obstacledetection::Obstacle& obstacle)
512 const
513 {
514  ARMARX_DEBUG << "Sanity checking obstacle `" << obstacle.name << "`.";
515 
516  const std::string curvature_error = "Curvature must be greater than or equal to 1.";
517  const std::string z_comps_set_error = "2D mode activated. Z-component values must not be set.";
518 
519  ARMARX_CHECK_GREATER_EQUAL(obstacle.curvatureX, 1) << curvature_error;
520  ARMARX_CHECK_GREATER_EQUAL(obstacle.curvatureY, 1) << curvature_error;
521  ARMARX_CHECK_GREATER_EQUAL(obstacle.curvatureZ, 1) << curvature_error;
522 
523  if (m_doa.cfg.only_2d)
524  {
525  ARMARX_DEBUG << "Additionally sanity checking obstacle `" << obstacle.name
526  << "` for 2D compliance.";
527 
528  ARMARX_CHECK_EQUAL(obstacle.posZ, 0) << z_comps_set_error;
529  ARMARX_CHECK_EQUAL(obstacle.refPosZ, 0) << z_comps_set_error;
530  ARMARX_CHECK_EQUAL(obstacle.axisLengthZ, 0) << z_comps_set_error;
531  ARMARX_CHECK_EQUAL(obstacle.safetyMarginZ, 0) << z_comps_set_error;
532  }
533 }
534 
535 
536 void
537 armarx::DSObstacleAvoidance::run_visualization()
538 {
539  //ARMARX_DEBUG << "Run visualization";
540  using namespace armarx::viz;
541 
542  std::lock_guard l{m_vis.mutex};
543  if (not m_vis.needs_update)
544  {
545  return;
546  }
547 
548  Layer layer_obs = arviz.layer("obstacles");
549  Layer layer_sms = arviz.layer("safety_margins");
550  Layer layer_rps = arviz.layer("reference_points");
551  Layer layer_bbs = arviz.layer("bounding_boxes");
552 
553  for (const obstacledetection::Obstacle& obstacle : getObstacles())
554  {
555  Color color = Color::orange(255, 128);
556  Color color_m = Color::orange(255, 64);
557 
558  if (obstacle.name == "human")
559  {
560  color = Color::red(255, 128);
561  color_m = Color::red(255, 64);
562  }
563  else if (not m_doa.cfg.only_2d)
564  {
565  color = Color::blue(255, 128);
566  color_m = Color::blue(255, 64);
567  }
568 
569  const double safetyMarginZ = m_doa.cfg.only_2d ? 1 : obstacle.safetyMarginZ;
570  const double posZ = m_doa.cfg.only_2d ? 1 : obstacle.posZ;
571  const double refPosZ = m_doa.cfg.only_2d ? 1 : obstacle.refPosZ;
572  const double axisLengthZ = m_doa.cfg.only_2d ? 1 : obstacle.axisLengthZ;
573 
574  const Eigen::Matrix4f pose = simox::math::pos_rpy_to_mat4f(
575  obstacle.posX, obstacle.posY, posZ,
576  0, 0, obstacle.yaw);
577  const Eigen::Vector3f dim(obstacle.axisLengthX, obstacle.axisLengthY, axisLengthZ);
578  const Eigen::Vector3f sm(obstacle.safetyMarginX, obstacle.safetyMarginY, safetyMarginZ);
579  const Eigen::Vector3f curv(obstacle.curvatureX, obstacle.curvatureY, obstacle.curvatureZ);
580 
581  if (m_doa.cfg.only_2d)
582  {
583 
584  layer_obs.add(Cylindroid{obstacle.name}
585  .pose(pose)
586  .axisLengths(dim.head<2>())
587  .height(dim(2))
588  .curvature(curv.head<2>())
589  .color(color));
590 
591  layer_sms.add(Cylindroid{obstacle.name + "_sm"}
592  .pose(pose)
593  .axisLengths((dim + sm).head<2>())
594  .height(dim(2) + sm(2))
595  .curvature(curv.head<2>())
596  .color(color_m));
597  }
598  else
599  {
600  layer_obs.add(Ellipsoid{obstacle.name}
601  .pose(pose)
602  .axisLengths(dim)
603  .curvature(curv)
604  .color(color));
605 
606  layer_sms.add(Ellipsoid{obstacle.name + "_sm"}
607  .pose(pose)
608  .axisLengths(dim + sm)
609  .curvature(curv)
610  .color(color_m));
611  }
612 
613  layer_rps.add(Sphere{obstacle.name + "_rp"}
614  .position(Eigen::Vector3f(obstacle.refPosX, obstacle.refPosY, refPosZ))
615  .radius(35)
616  .color(Color::green()));
617 
618  layer_bbs.add(Box{obstacle.name + "_bb"}
619  .pose(pose)
620  .size(dim)
621  .color(color));
622  }
623 
624  m_vis.needs_update = false;
625 
626  arviz.commit({layer_obs, layer_sms, layer_rps});
627  //arviz.commit(layer_bbs); // Do not render bounding boxes by default.
628 }
629 
630 
631 void
632 armarx::DSObstacleAvoidance::run_watchdog()
633 {
634  bool needs_env_update;
635  bool was_recently_updated;
636  {
637  std::unique_lock l{m_buf.mutex};
638  needs_env_update = m_buf.needs_env_update;
639  was_recently_updated = IceUtil::Time::now() - m_buf.last_env_update <= m_watchdog.threshold;
640  }
641 
642  if (needs_env_update and not was_recently_updated)
643  {
644  ARMARX_WARNING << "Environment has not been updated for over "
645  << m_watchdog.threshold << ". Have you forgotten to call "
646  << "`updateEnvironment()` after adding, updating or removing obstacles? "
647  << "Forcing update now.";
648 
649  updateEnvironment();
650  }
651 }
652 
653 
656 {
657  PropertyDefinitionsPtr def{new ComponentPropertyDefinitions{getConfigIdentifier()}};
658 
659  def->optional(m_vis.enabled, "visualize", "Enable/disable visualization.");
660  def->optional(m_watchdog.enabled, "udpate_watchdog", "Run environment update watchdog.");
661  def->optional(m_doa.load_obstacles_from_file, "load_obstacles_from",
662  "Path to JSON file to load initial obstacles from.");
663 
664  // "doa" namespace for properties that directly configure the library.
665  def->optional(m_doa.cfg.only_2d, "doa.only_2d", "Only consider 2D.");
666  def->optional(m_doa.cfg.aggregated, "doa.aggregated", "Aggregated environment.");
667  def->optional(m_doa.cfg.agent_safety_margin, "doa.agent_safety_margin", "Agent safety margin.");
668  def->optional(m_doa.cfg.local_modulation, "doa.local_modulation", "Local modulation on/off.");
669  def->optional(m_doa.cfg.repulsion, "doa.repulsion", "Repulsion on/off.");
670  def->optional(m_doa.cfg.planar_modulation, "doa.planar_modulation",
671  "Planar modulation on/off.");
672  def->optional(m_doa.cfg.critical_distance, "doa.critical_distance", "Critical distance.");
673  def->optional(m_doa.cfg.weight_power, "doa.weight_power", "Weight power");
674 
675  return def;
676 }
armarx::DSObstacleAvoidance::removeObstacle
void removeObstacle(const std::string &obstacle_name, const Ice::Current &=Ice::emptyCurrent) override
Definition: DSObstacleAvoidance.cpp:298
armarx::DSObstacleAvoidance::getObstacles
std::vector< obstacledetection::Obstacle > getObstacles(const Ice::Current &=Ice::emptyCurrent) override
Definition: DSObstacleAvoidance.cpp:317
armarx::DSObstacleAvoidance::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: DSObstacleAvoidance.cpp:655
RemoteRobot.h
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
armarx::DSObstacleAvoidance::buffer::update
Definition: DSObstacleAvoidance.h:199
DSObstacleAvoidance.h
armarx::DSObstacleAvoidance::onDisconnectComponent
void onDisconnectComponent() override
Definition: DSObstacleAvoidance.cpp:231
armarx::DSObstacleAvoidance::DSObstacleAvoidance
DSObstacleAvoidance()
Definition: DSObstacleAvoidance.cpp:149
armarx::viz::Color::blue
static Color blue(int b=255, int a=255)
Definition: Color.h:89
armarx::obstacledetection
Definition: DSObstacleAvoidance.cpp:90
armarx::viz::Layer::add
void add(ElementT const &element)
Definition: Layer.h:29
armarx::viz::Sphere
Definition: Elements.h:134
armarx::viz::ElementOps::position
DerivedT & position(float x, float y, float z)
Definition: ElementOps.h:127
armarx::core::time::StopWatch::stop
Duration stop()
Stops the timer and returns the measured duration.
Definition: StopWatch.cpp:43
armarx::DSObstacleAvoidance::getDefaultName
std::string getDefaultName() const override
Definition: DSObstacleAvoidance.cpp:259
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:72
armarx::DSObstacleAvoidance::default_name
static const std::string default_name
Definition: DSObstacleAvoidance.h:161
armarx::DSObstacleAvoidance::modulateVelocity
Eigen::Vector3f modulateVelocity(const obstacleavoidance::Agent &agent, const Ice::Current &=Ice::emptyCurrent) override
Definition: DSObstacleAvoidance.cpp:358
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
armarx::viz::Sphere::radius
Sphere & radius(float r)
Definition: Elements.h:138
KITProsthesis::ProsthesisState::State
State
Definition: KITProstheticHandInterface.ice:32
armarx::DSObstacleAvoidance::updateEnvironment
void updateEnvironment(const Ice::Current &=Ice::emptyCurrent) override
Definition: DSObstacleAvoidance.cpp:408
armarx::viz::Color
Definition: Color.h:13
armarx::viz::Cylindroid
Definition: Elements.h:98
armarx::viz::Ellipsoid
Definition: Elements.h:147
armarx::viz::Box
Definition: Elements.h:51
filename
std::string filename
Definition: VisualizationRobot.cpp:83
armarx::DSObstacleAvoidance::updateObstacle
void updateObstacle(const obstacledetection::Obstacle &obstacle, const Ice::Current &=Ice::emptyCurrent) override
Definition: DSObstacleAvoidance.cpp:274
armarx::DSObstacleAvoidance::onConnectComponent
void onConnectComponent() override
Definition: DSObstacleAvoidance.cpp:206
armarx::viz::Color::red
static Color red(int r=255, int a=255)
Definition: Color.h:79
ARMARX_CHECK_GREATER_EQUAL
#define ARMARX_CHECK_GREATER_EQUAL(lhs, rhs)
This macro evaluates whether lhs is greater or equal (>=) rhs and if it turns out to be false it will...
Definition: ExpressionException.h:123
armarx::armem::server::ltm::util::mongodb::detail::update
bool update(mongocxx::collection &coll, const nlohmann::json &query, const nlohmann::json &update)
Definition: mongodb.cpp:67
Component.h
armarx::viz::Cylindroid::height
Cylindroid & height(float height)
Definition: Elements.h:107
armarx::DSObstacleAvoidance::writeDebugPlots
void writeDebugPlots(const std::string &filename, const Ice::Current &=Ice::emptyCurrent) override
Definition: DSObstacleAvoidance.cpp:487
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:70
armarx::viz::ElementOps::pose
DerivedT & pose(Eigen::Matrix4f const &pose)
Definition: ElementOps.h:159
armarx::PropertyDefinitionContainer::optional
decltype(auto) optional(PropertyType &setter, const std::string &name, const std::string &description="", PropertyDefinitionBase::PropertyConstness constness=PropertyDefinitionBase::eConstant)
Definition: PropertyDefinitionContainer.h:84
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::viz::ElementOps::color
DerivedT & color(Color color)
Definition: ElementOps.h:195
armarx::viz::Color::orange
static Color orange(int o=255, int a=255)
2 Red + 1 Green
Definition: Color.h:119
armarx::DSObstacleAvoidance::onInitComponent
void onInitComponent() override
Definition: DSObstacleAvoidance.cpp:164
Eigen::Quaterniond
Quaternion< double, 0 > Quaterniond
Definition: EigenForwardDeclarations.h:62
armarx::core::time::StopWatch
Measures the passed time between the construction or calling reset() and stop().
Definition: StopWatch.h:42
armarx::ArmarXDataPath::getAbsolutePath
static bool getAbsolutePath(const std::string &relativeFilename, std::string &storeAbsoluteFilename, const std::vector< std::string > &additionalSearchPaths={}, bool verbose=true)
Definition: ArmarXDataPath.cpp:111
armarx::PeriodicTask
Definition: ArmarXManager.h:70
armarx::viz::Color::green
static Color green(int g=255, int a=255)
Definition: Color.h:84
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::DSObstacleAvoidance::onExitComponent
void onExitComponent() override
Definition: DSObstacleAvoidance.cpp:250
ArmarXDataPath.h
armarx::viz::Layer
Definition: Layer.h:12
armarx::obstacledetection::from_json
void from_json(const nlohmann::json &j, armarx::obstacledetection::Obstacle &obstacle)
Definition: DSObstacleAvoidance.cpp:94
armarx::viz
This file is part of ArmarX.
Definition: ArVizStorage.cpp:416
armarx::DSObstacleAvoidance::getConfig
dsobstacleavoidance::Config getConfig(const Ice::Current &=Ice::emptyCurrent) override
Definition: DSObstacleAvoidance.cpp:267