From 518cd5adebb85b4ef2ac0d83487b7223791d3477 Mon Sep 17 00:00:00 2001
From: "Christian R. G. Dreher" <c.dreher@kit.edu>
Date: Thu, 30 Apr 2020 15:43:50 +0200
Subject: [PATCH] Implement an obstacle avoiding platform controller component.
---
.../DSObstacleAvoidance/CMakeLists.txt | 5 +-
.../DSObstacleAvoidance/arviz_agent.h | 184 ------
.../RobotAPI/components/units/CMakeLists.txt | 1 +
.../CMakeLists.txt | 13 +
.../ObstacleAvoidingPlatformUnit.cpp | 619 ++++++++++++++++++
.../ObstacleAvoidingPlatformUnit.h | 246 +++++++
.../ObstacleAvoidingPlatformUnit/main.cpp | 40 ++
7 files changed, 920 insertions(+), 188 deletions(-)
delete mode 100644 source/RobotAPI/components/DSObstacleAvoidance/arviz_agent.h
create mode 100644 source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/CMakeLists.txt
create mode 100644 source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
create mode 100644 source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h
create mode 100644 source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/main.cpp
diff --git a/source/RobotAPI/components/DSObstacleAvoidance/CMakeLists.txt b/source/RobotAPI/components/DSObstacleAvoidance/CMakeLists.txt
index a32de2577..ff53d0508 100644
--- a/source/RobotAPI/components/DSObstacleAvoidance/CMakeLists.txt
+++ b/source/RobotAPI/components/DSObstacleAvoidance/CMakeLists.txt
@@ -23,10 +23,7 @@ set(COMPONENT_LIBS
)
set(SOURCES DSObstacleAvoidance.cpp)
-set(HEADERS
- DSObstacleAvoidance.h
- arviz_agent.h
-)
+set(HEADERS DSObstacleAvoidance.h)
armarx_add_component("${SOURCES}" "${HEADERS}")
armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/components/DSObstacleAvoidance/arviz_agent.h b/source/RobotAPI/components/DSObstacleAvoidance/arviz_agent.h
deleted file mode 100644
index ddc8ad896..000000000
--- a/source/RobotAPI/components/DSObstacleAvoidance/arviz_agent.h
+++ /dev/null
@@ -1,184 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package Armar6Skills::ArmarXObjects::ObstacleAvoidance
- * @author Christian R. G. Dreher <c.dreher@kit.edu>
- * @date 2020
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-
-#pragma once
-
-
-// STD/STL
-#include <string>
-
-// Eigen
-#include <Eigen/Geometry>
-
-// Simox
-#include <SimoxUtility/math/convert/pos_rpy_to_mat4f.h>
-
-// ArmarX
-#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
-#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/components/ArViz/Client/Client.h>
-
-// RobotAPI
-#include <RobotAPI/interface/components/DSObstacleAvoidanceInterface.h>
-
-
-namespace armarx::viz
-{
-
- class obstacle_avoidance
- {
-
- public:
-
- obstacle_avoidance(const Eigen::Vector2f& start, const Eigen::Vector2f& goal) :
- m_start{start(0), start(1), 0},
- m_goal{goal(0), goal(1), 0}
- {
- ;
- }
-
- obstacle_avoidance(const Eigen::Vector3f& start, const Eigen::Vector3f& goal) :
- m_start{start},
- m_goal{goal}
- {
- ;
- }
-
- void update(
- const obstacleavoidance::Agent& agent,
- const Eigen::Vector2f& agent_modulated_vel)
- {
- update(agent, Eigen::Vector3f{agent_modulated_vel(0), agent_modulated_vel(1), 0});
- }
-
- void update(
- const obstacleavoidance::Agent& agent,
- const Eigen::Vector3f& agent_modulated_vel)
- {
- const float min_keypoint_dist = 50;
- const Eigen::Vector3f& last_keypoint_pos = m_path.size() >= 1 ? m_path.back() : m_start;
-
- // Keep a certain distance between path keypoints.
- if ((last_keypoint_pos - agent.pos).norm() > min_keypoint_dist)
- {
- m_path.push_back(agent.pos);
- m_redraw_path = true;
- }
-
- m_agent_desired_vel = agent.desired_vel;
- m_agent_modulated_vel = agent_modulated_vel;
- m_agent_safety_margin = agent.safety_margin;
- m_agent_pos = agent.pos;
- }
-
- bool viz_progress(armarx::viz::Layer& layer)
- {
- using namespace armarx::viz;
-
- bool is_redrawn = false;
-
- if (m_redraw_path)
- {
- // Start.
- layer.add(Sphere{"start"}
- .position(m_start)
- .color(Color::cyan(255, 64))
- .radius(40));
-
- // Path.
- for (unsigned i = 0; i < m_path.size(); ++i)
- {
- layer.add(Sphere{"path_" + std::to_string(i + 1)}
- .position(m_path[i])
- .color(Color::magenta(255, 128))
- .radius(20));
- }
-
- // Goal.
- layer.add(Arrow{"goal"}
- .fromTo(m_goal + Eigen::Vector3f{0, 0, 220},
- m_goal + Eigen::Vector3f{0, 0, 40})
- .color(Color::red(255, 128))
- .width(20));
-
- is_redrawn = true;
- m_redraw_path = false;
- }
-
- return is_redrawn;
- }
-
- void viz_velocities(armarx::viz::Layer& layer)
- {
- using namespace armarx::viz;
-
- const Eigen::Vector3f from1{m_agent_pos + Eigen::Vector3f{0, 0, 2000}};
- const Eigen::Vector3f from2 = from1 + Eigen::Vector3f{0, 0, 200};
-
- layer.add(Arrow{"desired_vel"}
- .fromTo(from1, from1 + m_agent_desired_vel * 5)
- .color(Color::green(255, 128))
- .width(25));
-
- layer.add(Arrow{"modulated_vel"}
- .fromTo(from2, from2 + m_agent_modulated_vel * 5)
- .color(Color::cyan(255, 128))
- .width(25));
- }
-
- void viz_agent(armarx::viz::Layer& layer)
- {
- using namespace armarx::viz;
-
- // Agent.
- layer.add(Sphere{"agent"}
- .position(m_agent_pos)
- .color(Color::red(255, 128))
- .radius(50));
-
- // Agent safety margin.
- if (m_agent_safety_margin > 0)
- {
- layer.add(Cylinder{"agent_safety_margin"}
- .pose(simox::math::pos_rpy_to_mat4f(m_agent_pos, -M_PI_2, 0, 0))
- .color(Color::red(255, 64))
- .radius(m_agent_safety_margin)
- .height(2));
- }
- }
-
- private:
-
- bool m_redraw_path = true;
-
- Eigen::Vector3f m_start;
- Eigen::Vector3f m_goal;
- Eigen::Vector3f m_agent_desired_vel;
- Eigen::Vector3f m_agent_modulated_vel;
- Eigen::Vector3f m_agent_pos;
- float m_agent_safety_margin;
- std::vector<Eigen::Vector3f> m_path;
-
- };
-
-}
diff --git a/source/RobotAPI/components/units/CMakeLists.txt b/source/RobotAPI/components/units/CMakeLists.txt
index ce1deee81..430741e73 100644
--- a/source/RobotAPI/components/units/CMakeLists.txt
+++ b/source/RobotAPI/components/units/CMakeLists.txt
@@ -74,5 +74,6 @@ set(LIB_FILES
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+add_subdirectory(ObstacleAvoidingPlatformUnit)
add_subdirectory(relays)
add_subdirectory(RobotUnit)
diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/CMakeLists.txt b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/CMakeLists.txt
new file mode 100644
index 000000000..eace3006c
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/CMakeLists.txt
@@ -0,0 +1,13 @@
+armarx_component_set_name("ObstacleAvoidingPlatformUnit")
+
+set(COMPONENT_LIBS
+ RobotAPICore
+ RobotAPIComponentPlugins
+ RobotUnit
+)
+
+set(SOURCES ObstacleAvoidingPlatformUnit.cpp)
+set(HEADERS ObstacleAvoidingPlatformUnit.h)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
new file mode 100644
index 000000000..954ad2ab8
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
@@ -0,0 +1,619 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::ObstacleAvoidingPlatformUnit
+ * @author Christian R. G. Dreher <c.dreher@kit.edu>
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#include <RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h>
+
+
+// STD/STL
+#include <algorithm>
+
+// Eigen
+#include <Eigen/Geometry>
+
+// Simox
+#include <SimoxUtility/math.h>
+
+
+const std::string
+armarx::ObstacleAvoidingPlatformUnit::default_name = "ObstacleAvoidingPlatformUnit";
+
+
+armarx::ObstacleAvoidingPlatformUnit::ObstacleAvoidingPlatformUnit()
+{
+ m_agent.safety_margin = 0;
+}
+
+
+armarx::ObstacleAvoidingPlatformUnit::~ObstacleAvoidingPlatformUnit()
+{
+ ;
+}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnit::onInitPlatformUnit()
+{
+ ARMARX_DEBUG << "Initializing " << getName() << ".";
+
+ ARMARX_DEBUG << "Initialized " << getName() << ".";
+}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnit::onStartPlatformUnit()
+{
+ ARMARX_DEBUG << "Starting " << getName() << ".";
+
+ m_robot = addRobot("robot", VirtualRobot::RobotIO::eStructure);
+
+ ARMARX_DEBUG << "Started " << getName() << ".";
+}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnit::onExitPlatformUnit()
+{
+ ARMARX_DEBUG << "Exiting " << getName() << ".";
+
+ schedule_high_level_control_loop(control_mode::none);
+ stopPlatform();
+
+ ARMARX_DEBUG << "Exited " << getName() << ".";
+}
+
+
+std::string
+armarx::ObstacleAvoidingPlatformUnit::getDefaultName()
+const
+{
+ return default_name;
+}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnit::moveTo(
+ const float target_pos_x,
+ const float target_pos_y,
+ const float target_ori,
+ const float pos_reached_threshold,
+ const float ori_reached_threshold,
+ const Ice::Current&)
+{
+ std::scoped_lock l{m_control_data.mutex};
+
+ m_control_data.target_pos = Eigen::Vector2f{target_pos_x, target_pos_y};
+ m_control_data.target_ori = target_ori;
+ m_control_data.pos_reached_threshold = pos_reached_threshold;
+ m_control_data.ori_reached_threshold = ori_reached_threshold;
+
+ schedule_high_level_control_loop(control_mode::position);
+}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnit::move(
+ const float target_vel_x,
+ const float target_vel_y,
+ const float target_rot_vel,
+ const Ice::Current&)
+{
+ std::scoped_lock l{m_control_data.mutex};
+
+ m_control_data.target_vel = Eigen::Vector2f{target_vel_x, target_vel_y};
+ m_control_data.target_rot_vel = target_rot_vel;
+
+ ARMARX_CHECK(m_control_data.target_vel.allFinite());
+ ARMARX_CHECK(std::isfinite(m_control_data.target_rot_vel));
+
+ schedule_high_level_control_loop(control_mode::velocity);
+}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnit::moveRelative(
+ const float target_pos_delta_x,
+ const float target_pos_delta_y,
+ const float target_delta_ori,
+ const float pos_reached_threshold,
+ const float ori_reached_threshold,
+ const Ice::Current& current)
+{
+ using namespace simox::math;
+
+ // Transform relative to global.
+ std::unique_lock lock{m_control_data.mutex};
+ synchronizeLocalClone(m_robot);
+ const Eigen::Vector2f agent_pos = m_robot->getGlobalPosition().head<2>();
+ const float agent_ori =
+ periodic_clamp<float>(mat4f_to_rpy(m_robot->getGlobalPose()).z(), -M_PI, M_PI);
+ lock.unlock();
+
+ // Use moveTo.
+ moveTo(
+ agent_pos.x() + target_pos_delta_x,
+ agent_pos.y() + target_pos_delta_y,
+ agent_ori + target_delta_ori,
+ pos_reached_threshold,
+ ori_reached_threshold,
+ current);
+}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnit::setMaxVelocities(
+ const float max_pos_vel,
+ const float max_rot_vel,
+ const Ice::Current&)
+{
+ std::scoped_lock l{m_control_data.mutex};
+ m_control_data.max_vel = max_pos_vel;
+ m_control_data.max_rot_vel = max_rot_vel;
+ m_platform->setMaxVelocities(max_pos_vel, max_rot_vel);
+}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnit::stopPlatform(const Ice::Current&)
+{
+ m_platform->move(0, 0, 0);
+ m_platform->stopPlatform();
+}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnit::schedule_high_level_control_loop(control_mode mode)
+{
+ std::scoped_lock l{m_control_loop.mutex};
+
+ // If the control mode didn't change, there's nothing to do.
+ if (m_control_loop.mode == mode)
+ {
+ return;
+ }
+
+ // If a control loop is runnung, stop it and wait for termination.
+ if (m_control_loop.mode != mode and m_control_loop.task)
+ {
+ const bool join = true;
+ m_control_loop.task->stop(join);
+ }
+
+ // If the new control mode is none, no new control loop needs to be created.
+ if (mode == control_mode::none)
+ {
+ return;
+ }
+
+ // Start next control loop.
+ m_control_loop.mode = mode;
+ m_control_loop.task = new RunningTask<ObstacleAvoidingPlatformUnit>(
+ this,
+ &ObstacleAvoidingPlatformUnit::high_level_control_loop);
+ m_control_loop.task->start();
+}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnit::high_level_control_loop()
+{
+ try
+ {
+ CycleUtil cu{m_control_loop.cycle_time};
+ while (not m_control_loop.task->isStopped())
+ {
+ if (target_reached())
+ {
+ break;
+ }
+
+ run_control_loop();
+ cu.waitForCycleDuration();
+ }
+ }
+ catch (const std::exception& e)
+ {
+ ARMARX_ERROR << "Error occured while running control loop.\n"
+ << e.what();
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "Unknown error occured while running control loop.";
+ }
+
+ clean_up();
+ stopPlatform();
+ m_control_loop.mode = control_mode::none;
+}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnit::run_control_loop()
+{
+ using namespace simox::math;
+
+ // Acquire control_data mutex to ensure data remains consistent.
+ std::scoped_lock l{m_control_data.mutex};
+
+ // Update agent pos and ori.
+ synchronizeLocalClone(m_robot);
+ m_control_data.agent_pos = m_robot->getGlobalPosition().head<2>();
+ m_control_data.agent_ori =
+ periodic_clamp<float>(mat4f_to_rpy(m_robot->getGlobalPose()).z(), -M_PI, M_PI);
+
+ // Calculate target velocity and target angular velocity if in position control mode.
+ if (m_control_loop.mode == control_mode::position)
+ {
+ ARMARX_CHECK(m_control_data.target_pos.allFinite());
+ ARMARX_CHECK(std::isfinite(m_control_data.target_ori));
+
+ // Determine distance and angular distance to target position and orientation.
+ m_control_data.target_dist =
+ (m_control_data.target_pos - m_control_data.agent_pos).norm();
+ m_control_data.target_angular_dist =
+ periodic_clamp<float>(m_control_data.target_ori - m_control_data.agent_ori,
+ -M_PI, M_PI);
+ ARMARX_DEBUG << "Distance to target: " << m_control_data.target_dist << " mm and "
+ << m_control_data.target_angular_dist << " rad.";
+
+ // Calculate target velocity.
+ m_control_data.target_vel =
+ (m_control_data.target_pos - m_control_data.agent_pos) * m_control_data.kp;
+
+ // Calculate target rotational velocity.
+ m_rot_pid_controller.update(m_control_data.target_angular_dist, 0);
+ m_control_data.target_rot_vel = m_rot_pid_controller.getControlValue();
+ }
+
+ ARMARX_CHECK(m_control_data.target_vel.allFinite());
+ ARMARX_CHECK(std::isfinite(m_control_data.target_rot_vel));
+
+ // Cap velocities if they exceed maximum values.
+ const Eigen::Vector2f target_vel =
+ norm_max(m_control_data.target_vel, m_control_data.max_vel);
+ const float target_rot_vel = std::copysign(
+ std::min(std::fabs(m_control_data.target_rot_vel), m_control_data.max_rot_vel),
+ m_control_data.target_rot_vel);
+
+ // Apply obstacle avoidance and apply post processing.
+ m_agent.pos = Eigen::Vector3f{m_control_data.agent_pos.x(), m_control_data.agent_pos.y(), 0};
+ m_agent.desired_vel = Eigen::Vector3f{target_vel.x(), target_vel.y(), 0};
+ ARMARX_CHECK(m_agent.pos.allFinite());
+ ARMARX_CHECK(m_agent.desired_vel.allFinite());
+ const Eigen::Vector2f raw_modulated_vel =
+ m_obstacle_avoidance->modulateVelocity(m_agent).head<2>();
+ ARMARX_CHECK(raw_modulated_vel.allFinite());
+ const Eigen::Vector2f modulated_vel =
+ target_pos_reached() ?
+ Eigen::Vector2f::Zero() :
+ post_process_vel(target_vel, norm_max(raw_modulated_vel, m_control_data.max_vel));
+ ARMARX_CHECK(modulated_vel.allFinite());
+
+ if (m_viz.enabled)
+ {
+ visualize(modulated_vel);
+ }
+
+ // Move platform now.
+ const Eigen::Vector2f local_vel =
+ Eigen::Rotation2D(-m_control_data.agent_ori).toRotationMatrix() * modulated_vel;
+ ARMARX_CHECK(local_vel.allFinite()) << "Velocity contains non-finite values.";
+ ARMARX_CHECK(std::isfinite(target_rot_vel)) << "Rotation velocity is non-finite.";
+ m_platform->move(local_vel.x(), local_vel.y(), target_rot_vel);
+}
+
+
+bool
+armarx::ObstacleAvoidingPlatformUnit::target_pos_reached()
+{
+ if (m_control_loop.mode == control_mode::position)
+ {
+ return m_control_data.target_dist < m_control_data.pos_reached_threshold;
+ }
+
+ // Cannot reach any target in non-position mode.
+ return false;
+}
+
+
+bool
+armarx::ObstacleAvoidingPlatformUnit::target_reached()
+{
+ if (m_control_loop.mode == control_mode::position)
+ {
+ // Determine whether the goal position was reached.
+ if (target_pos_reached())
+ {
+ ARMARX_VERBOSE << "Goal position reached.";
+
+ // Determine whether goal orientation was reached.
+ if (std::fabs(m_control_data.target_angular_dist) <
+ m_control_data.ori_reached_threshold)
+ {
+ ARMARX_VERBOSE << "Goal orientation reached.";
+
+ return true;
+ }
+ }
+ }
+
+ return false;
+}
+
+
+Eigen::Vector2f
+armarx::ObstacleAvoidingPlatformUnit::post_process_vel(
+ const Eigen::Vector2f& target_vel,
+ const Eigen::Vector2f& modulated_vel)
+{
+ ARMARX_CHECK(target_vel.allFinite());
+ ARMARX_CHECK(modulated_vel.allFinite());
+
+ // Update buffer.
+ m_control_data.vel_history.push_front(std::make_tuple(target_vel, modulated_vel));
+ const unsigned max_real_buffer_size =
+ std::max(m_control_data.amount_smoothing, m_control_data.amount_max_vel);
+ if (m_control_data.vel_history.size() > max_real_buffer_size)
+ {
+ m_control_data.vel_history.resize(max_real_buffer_size);
+ }
+
+ // Smooth by calculating mean over the last `m_control_data.amount_smoothing` elements.
+ const Eigen::Vector2f mean_modulated_vel = calculate_mean_modulated_vel();
+ ARMARX_CHECK(mean_modulated_vel.allFinite());
+
+ // Determine max velocity by considering the coherency between the last
+ // `m_control_data.amount_max_vel` pairs of desired and modulated velocities.
+ const float max_vel = calculate_adaptive_max_vel();
+ ARMARX_CHECK(std::isfinite(max_vel));
+
+ return simox::math::norm_max(mean_modulated_vel, max_vel);
+}
+
+
+Eigen::Vector2f
+armarx::ObstacleAvoidingPlatformUnit::calculate_mean_modulated_vel()
+const
+{
+ const unsigned adaptive_buffer_size = calculate_adaptive_smoothing_buffer_size();
+
+ std::vector<Eigen::Vector2f> last_modulated_vels;
+ const unsigned max_i =
+ std::min<unsigned>(m_control_data.vel_history.size(), adaptive_buffer_size);
+
+ ARMARX_CHECK_GREATER(max_i, 0);
+
+ for (unsigned i = 0; i < max_i; ++i)
+ {
+ const Eigen::Vector2f modulated_vel = std::get<1>(m_control_data.vel_history.at(i));
+ ARMARX_CHECK(modulated_vel.allFinite());
+ last_modulated_vels.push_back(std::move(modulated_vel));
+ }
+
+ return simox::math::mean(last_modulated_vels);
+}
+
+
+unsigned
+armarx::ObstacleAvoidingPlatformUnit::calculate_adaptive_smoothing_buffer_size()
+const
+{
+ // buffer_size(min_buffer_size_dist) = min_buffer_size
+ const float min_buffer_size_dist = 200;
+ const float min_buffer_size = 3;
+ // buffer_size(max_buffer_size_dist) = max_buffer_size
+ const float max_buffer_size_dist = 1500;
+ const float max_buffer_size = m_control_data.amount_smoothing;
+
+ ARMARX_CHECK_LESS(min_buffer_size, max_buffer_size);
+
+ // Given the two points, calculate m and b in: f(x) = m * x + b
+ const float m = (max_buffer_size - min_buffer_size) /
+ (max_buffer_size_dist - min_buffer_size_dist);
+ const float b = min_buffer_size - (m * min_buffer_size_dist);
+
+ // Calculate buffer size and clamp value if need be.
+ return static_cast<unsigned>(std::clamp(m * m_control_data.target_dist + b,
+ min_buffer_size, max_buffer_size));
+}
+
+
+float
+armarx::ObstacleAvoidingPlatformUnit::calculate_adaptive_max_vel()
+const
+{
+ using namespace simox::math;
+
+ std::vector<float> angular_similarities;
+ const unsigned max_i =
+ std::min<unsigned>(m_control_data.vel_history.size(), m_control_data.amount_max_vel);
+
+ for (unsigned int i = 0; i < max_i; ++i)
+ {
+ auto& [desired_vel, modulated_vel] = m_control_data.vel_history.at(i);
+ float angular_sim = -1;
+
+ if (desired_vel.isZero() and modulated_vel.isZero())
+ {
+ angular_sim = 1;
+ }
+ else if (desired_vel.isZero() xor modulated_vel.isZero())
+ {
+ angular_sim = -1;
+ }
+ else
+ {
+ angular_sim = angular_similarity(desired_vel, modulated_vel);
+ }
+
+ angular_similarities.push_back(angular_sim);
+ }
+
+ const float mean_angular_similarity = mean(angular_similarities);
+ const float max_vel_factor = std::pow(mean_angular_similarity, 2);
+
+ return max_vel_factor * m_control_data.max_vel;
+}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnit::clean_up()
+{
+ // Clear buffers.
+ m_control_data.vel_history.clear();
+ m_viz.path.clear();
+
+ // Clear ArViz.
+ for (const std::string& layer_name : {"progress", "velocities", "agent"})
+ {
+ viz::Layer layer = arviz.layer(layer_name);
+ arviz.commit(layer);
+ }
+}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnit::visualize(const Eigen::Vector2f& modulated_vel_2d)
+{
+ using namespace armarx::viz;
+
+ const Eigen::Vector3f modulated_vel{modulated_vel_2d.x(), modulated_vel_2d.y(), 0};
+
+ // Progress. Only draw in position control mode.
+ if (m_control_loop.mode == control_mode::position)
+ {
+ const float min_keypoint_dist = 50;
+ const Eigen::Vector3f& last_keypoint_pos =
+ m_viz.path.size() >= 1 ? m_viz.path.back() : m_viz.start;
+ bool redraw_path = false;
+
+ // Keep a certain distance between path keypoints.
+ if ((last_keypoint_pos - m_agent.pos).norm() > min_keypoint_dist)
+ {
+ m_viz.path.push_back(m_agent.pos);
+ redraw_path = true;
+ }
+
+ // Draw path.
+ if (redraw_path)
+ {
+ Layer layer = arviz.layer("progress");
+
+ // Start.
+ layer.add(Sphere{"start"}
+ .position(m_viz.start)
+ .color(Color::cyan(255, 64))
+ .radius(40));
+
+ // Path.
+ for (unsigned i = 0; i < m_viz.path.size(); ++i)
+ {
+ layer.add(Sphere{"path_" + std::to_string(i + 1)}
+ .position(m_viz.path[i])
+ .color(Color::magenta(255, 128))
+ .radius(20));
+ }
+
+ // Goal.
+ const Eigen::Vector3f target{m_control_data.target_pos.x(),
+ m_control_data.target_pos.y(),
+ 0};
+ layer.add(Arrow{"goal"}
+ .fromTo(target + Eigen::Vector3f{0, 0, 220},
+ target + Eigen::Vector3f{0, 0, 40})
+ .color(Color::red(255, 128))
+ .width(20));
+
+ arviz.commit(layer);
+ }
+ }
+
+ // Velocities.
+ {
+ Layer layer = arviz.layer("velocities");
+
+ const Eigen::Vector3f from1{m_agent.pos + Eigen::Vector3f{0, 0, 2000}};
+ const Eigen::Vector3f from2 = from1 + Eigen::Vector3f{0, 0, 200};
+
+ if (not m_agent.desired_vel.isZero())
+ {
+ layer.add(Arrow{"original"}
+ .fromTo(from1, from1 + m_agent.desired_vel * 5)
+ .color(Color::green(255, 128))
+ .width(25));
+ }
+
+ if (not modulated_vel.isZero())
+ {
+ layer.add(Arrow{"modulated"}
+ .fromTo(from2, from2 + modulated_vel * 5)
+ .color(Color::cyan(255, 128))
+ .width(25));
+ }
+
+ arviz.commit(layer);
+ }
+
+ // Agent.
+ {
+ Layer layer = arviz.layer("agent");
+
+ layer.add(Sphere{"agent"}
+ .position(m_agent.pos)
+ .color(Color::red(255, 128))
+ .radius(50));
+
+ // Agent safety margin.
+ if (m_agent.safety_margin > 0)
+ {
+ layer.add(Cylinder{"agent_safety_margin"}
+ .pose(simox::math::pos_rpy_to_mat4f(m_agent.pos, -M_PI_2, 0, 0))
+ .color(Color::red(255, 64))
+ .radius(m_agent.safety_margin)
+ .height(2));
+ }
+
+ arviz.commit(layer);
+ }
+}
+
+
+armarx::PropertyDefinitionsPtr
+armarx::ObstacleAvoidingPlatformUnit::createPropertyDefinitions()
+{
+ PropertyDefinitionsPtr def{new PlatformUnitPropertyDefinitions{getConfigIdentifier()}};
+
+ def->component(m_platform, "Platform");
+ def->component(m_obstacle_avoidance, "PlatformObstacleAvoidance");
+
+ // Control parameters.
+ def->optional(m_control_data.kp, "control.pos.kp");
+ def->optional(m_rot_pid_controller.Kp, "control.rot.kp");
+ def->optional(m_rot_pid_controller.Ki, "control.rot.ki");
+ def->optional(m_rot_pid_controller.Kd, "control.rot.kd");
+ def->optional(m_control_loop.cycle_time, "control.pose.cycle_time");
+
+ // Obstacle avoidance parameter.
+ def->optional(m_agent.safety_margin, "doa.agent_safety_margin");
+
+ return def;
+}
diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h
new file mode 100644
index 000000000..e8ed4cb3f
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h
@@ -0,0 +1,246 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::ObstacleAvoidingPlatformUnit
+ * @author Christian R. G. Dreher <c.dreher@kit.edu>
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+
+// STD/STL
+#include <deque>
+#include <string>
+#include <mutex>
+#include <vector>
+
+// Eigen
+#include <Eigen/Core>
+
+// Ice
+#include <IceUtil/Time.h>
+
+// Simox
+#include <VirtualRobot/Nodes/RobotNode.h>
+
+// ArmarX
+#include <ArmarXCore/util/tasks.h>
+#include <ArmarXCore/util/time.h>
+
+// RobotAPI
+#include <RobotAPI/components/units/PlatformUnit.h>
+#include <RobotAPI/interface/components/ObstacleAvoidanceInterface.h>
+#include <RobotAPI/libraries/core/PIDController.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
+
+
+namespace armarx
+{
+
+ class ObstacleAvoidingPlatformUnit :
+ virtual public PlatformUnit,
+ virtual public RobotStateComponentPluginUser,
+ virtual public ArVizComponentPluginUser
+ {
+
+ private:
+
+ enum class control_mode
+ {
+ position,
+ velocity,
+ none
+ };
+
+ struct control_loop
+ {
+ std::mutex mutex;
+ control_mode mode = control_mode::none;
+ RunningTask<ObstacleAvoidingPlatformUnit>::pointer_type task;
+ IceUtil::Time cycle_time = IceUtil::Time::milliSeconds(10);
+ };
+
+ struct control_data
+ {
+ std::mutex mutex;
+ Eigen::Vector2f target_pos;
+ float target_ori;
+ Eigen::Vector2f target_vel;
+ float target_rot_vel;
+ float target_dist;
+ float target_angular_dist;
+ Eigen::Vector2f agent_pos;
+ float agent_ori;
+ float max_vel = 200;
+ float max_rot_vel = 0.3;
+ float pos_reached_threshold = 8;
+ float ori_reached_threshold = 0.1;
+ float kp = 3.5;
+ std::deque<std::tuple<Eigen::Vector2f, Eigen::Vector2f>> vel_history;
+ unsigned amount_smoothing = 50;
+ unsigned amount_max_vel = 3;
+ };
+
+ struct visualization
+ {
+ Eigen::Vector3f start;
+ std::vector<Eigen::Vector3f> path;
+ bool enabled = true;
+ };
+
+ public:
+
+ ObstacleAvoidingPlatformUnit();
+
+ virtual
+ ~ObstacleAvoidingPlatformUnit()
+ override;
+
+ virtual
+ std::string
+ getDefaultName()
+ const override;
+
+ 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;
+
+ virtual
+ void
+ move(
+ float target_vel_x,
+ float target_vel_y,
+ float target_rot_vel,
+ const Ice::Current& = Ice::Current{})
+ override;
+
+ 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;
+
+ virtual
+ void
+ setMaxVelocities(
+ float max_pos_vel,
+ float max_rot_vel,
+ const Ice::Current& = Ice::Current{})
+ override;
+
+ virtual
+ void
+ stopPlatform(const Ice::Current& = Ice::Current{})
+ override;
+
+ protected:
+
+ virtual
+ void
+ onInitPlatformUnit()
+ override;
+
+ virtual
+ void
+ onStartPlatformUnit()
+ override;
+
+ virtual
+ void
+ onExitPlatformUnit()
+ override;
+
+ virtual
+ PropertyDefinitionsPtr
+ createPropertyDefinitions()
+ override;
+
+ private:
+
+ void
+ schedule_high_level_control_loop(control_mode mode);
+
+ void
+ high_level_control_loop();
+
+ void
+ run_control_loop();
+
+ bool
+ target_pos_reached();
+
+ bool
+ target_reached();
+
+ Eigen::Vector2f
+ post_process_vel(const Eigen::Vector2f& target_vel, const Eigen::Vector2f& modulated_vel);
+
+ Eigen::Vector2f
+ calculate_mean_modulated_vel()
+ const;
+
+ unsigned
+ calculate_adaptive_smoothing_buffer_size()
+ const;
+
+ float
+ calculate_adaptive_max_vel()
+ const;
+
+ void
+ visualize(const Eigen::Vector2f& modulated_vel);
+
+ void
+ clean_up();
+
+ public:
+
+ static const std::string default_name;
+
+ private:
+
+ PlatformUnitInterfacePrx m_platform;
+ ObstacleAvoidanceInterfacePrx m_obstacle_avoidance;
+ VirtualRobot::RobotPtr m_robot;
+
+ ObstacleAvoidingPlatformUnit::control_loop m_control_loop;
+ ObstacleAvoidingPlatformUnit::control_data m_control_data;
+
+ PIDController m_rot_pid_controller{1.0, 0.00009, 0.0};
+
+ obstacleavoidance::Agent m_agent;
+
+ visualization m_viz;
+
+ };
+
+}
diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/main.cpp b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/main.cpp
new file mode 100644
index 000000000..8dd9c74ae
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/main.cpp
@@ -0,0 +1,40 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::ObstacleAvoidingPlatformUnit
+ * @author Christian R. G. Dreher <c.dreher@kit.edu>
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+// STD/STL
+#include <string>
+
+// ArmarX
+#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/Component.h>
+
+// RobotAPI
+#include <RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h>
+
+
+int main(int argc, char* argv[])
+{
+ using namespace armarx;
+ const std::string name = ObstacleAvoidingPlatformUnit::default_name;
+ return runSimpleComponentApp<ObstacleAvoidingPlatformUnit>(argc, argv, name);
+}
--
GitLab