diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index fac11b677edbf7fc26cd2538e453981601e94f13..9d9342ab21b51d11fd188a70c5e4cf6066afff24 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -43,10 +43,18 @@
<Proxy include="RobotAPI/interface/units/PlatformUnitInterface.h"
humanName="Platform Unit (obstacle avoiding)"
typeName="PlatformUnitInterfacePrx"
- memberName="obstacleAvoidingPlatformUnit"
- getterName="getObstacleAvoidingPlatformUnit"
- propertyName="ObstacleAvoidingPlatformUnitName"
+ memberName="platformUnit1"
+ getterName="getPlatformUnit1"
+ propertyName="PlatformUnitName1"
propertyDefaultValue="ObstacleAvoidingPlatformUnit"
+ propertyIsOptional="true" />
+ <Proxy include="RobotAPI/interface/units/PlatformUnitInterface.h"
+ humanName="Platform Unit (obstacle aware)"
+ typeName="PlatformUnitInterfacePrx"
+ memberName="platformUnit2"
+ getterName="getPlatformUnit2"
+ propertyName="PlatformUnitName2"
+ propertyDefaultValue="ObstacleAwarePlatformUnit"
propertyIsOptional="true" />
<Proxy include="RobotAPI/interface/observers/PlatformUnitObserverInterface.h"
humanName="Platform Unit Observer"
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Path.cpp b/source/RobotAPI/components/ArViz/Client/elements/Path.cpp
index 0a88d089382eebb915104c24ee41339a18dd2f29..cf5e9899e943dd4076c2dd21ca9fe982b3c1a9bc 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Path.cpp
+++ b/source/RobotAPI/components/ArViz/Client/elements/Path.cpp
@@ -1,27 +1,21 @@
#include "Path.h"
+#include <iterator>
+
+#include <ArmarXCore/interface/core/BasicVectorTypes.h>
+#include <ArmarXCore/interface/core/BasicVectorTypesHelpers.h>
+
namespace armarx::viz
{
Path& Path::clear()
{
data_->points.clear();
- return *this;
- }
-
- Path& Path::lineColor(Color color)
- {
- data_->lineColor = color;
return *this;
}
- Path& Path::lineColorGlasbeyLUT(std::size_t id, int alpha)
- {
- return lineColor(Color::fromRGBA(simox::color::GlasbeyLUT::at(id, alpha)));
- }
-
- Path& Path::lineWidth(float w)
+ Path& Path::width(float w)
{
data_->lineWidth = w;
@@ -33,17 +27,21 @@ namespace armarx::viz
auto& points = data_->points;
points.clear();
points.reserve(ps.size());
- for (auto& p : ps)
+
+ std::transform(ps.begin(),
+ ps.end(),
+ std::back_inserter(points),
+ [](const auto & e)
{
- points.push_back(armarx::Vector3f{p.x(), p.y(), p.z()});
- }
+ return ToBasicVectorType(e);
+ });
return *this;
}
Path& Path::addPoint(Eigen::Vector3f p)
{
- data_->points.push_back(armarx::Vector3f{p.x(), p.y(), p.z()});
+ data_->points.emplace_back(ToBasicVectorType(p));
return *this;
}
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Path.h b/source/RobotAPI/components/ArViz/Client/elements/Path.h
index 50eec6abe454bbf56bc45053c4d35f5e41ca5bc6..7cfbecbd973a194cef0e0e350a5c7c3c1aedbc13 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Path.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/Path.h
@@ -35,17 +35,7 @@ namespace armarx::viz
Path& clear();
- Path& lineColor(Color color);
-
- template<class...Ts>
- Path& lineColor(Ts&& ...ts)
- {
- return lineColor({std::forward<Ts>(ts)...});
- }
-
- Path& lineColorGlasbeyLUT(std::size_t id, int alpha = 255);
-
- Path& lineWidth(float w);
+ Path& width(float w);
Path& points(std::vector<Eigen::Vector3f> const& ps);
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp
index 3018a3af51e25222b97c0c60ed06fea65a8c5822..a0190abd75a8438a2371c728a580837d68c3eecc 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp
@@ -1,9 +1,12 @@
#include "VisualizationPath.h"
+#include <algorithm>
+
#include <Inventor/SbVec3f.h>
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoDrawStyle.h>
#include <Inventor/nodes/SoLineSet.h>
+#include <Inventor/nodes/SoPointSet.h>
namespace armarx::viz::coin
{
@@ -27,19 +30,36 @@ namespace armarx::viz::coin
node->addChild(coordinate3);
node->addChild(lineSep);
+
+ // points (use the following, if the points should be drawn in a different color)
+
+ // pclMat = new SoMaterial;
+
+ // SoMaterialBinding* pclMatBind = new SoMaterialBinding;
+ // pclMatBind->value = SoMaterialBinding::PER_PART;
+
+ pclCoords = new SoCoordinate3;
+ pclStyle = new SoDrawStyle;
+
+ // node->addChild(pclMat);
+ // node->addChild(pclMatBind);
+ node->addChild(pclCoords);
+ node->addChild(pclStyle);
+ node->addChild(new SoPointSet);
}
bool VisualizationPath::update(ElementType const& element)
{
// set position
- coordinate3->point.setValuesPointer(element.points.size(), reinterpret_cast<const float*>(element.points.data()));
+ coordinate3->point.setValuesPointer(element.points.size(),
+ reinterpret_cast<const float*>(element.points.data()));
// set color
- const auto lineColor = element.lineColor;
+ const auto lineColor = element.color;
constexpr float toUnit = 1.0F / 255.0F;
- const auto color = SbVec3f(lineColor.r, lineColor.g, lineColor.b) * toUnit;
+ const auto color = SbVec3f(lineColor.r, lineColor.g, lineColor.b) * toUnit;
const float transparency = 1.0F - static_cast<float>(lineColor.a) * toUnit;
lineMaterial->diffuseColor.setValue(color);
@@ -58,6 +78,20 @@ namespace armarx::viz::coin
const int pointSize = static_cast<int>(element.points.size());
lineSet->numVertices.set1Value(0, pointSize);
+ // points at each node
+ // const std::vector<SbVec3f> colorData(element.points.size(), color);
+
+ // pclMat->diffuseColor.setValuesPointer(colorData.size(),
+ // reinterpret_cast<const float*>(colorData.data()));
+ // pclMat->ambientColor.setValuesPointer(colorData.size(),
+ // reinterpret_cast<const float*>(colorData.data()));
+ // pclMat->transparency = transparency;
+
+ pclCoords->point.setValuesPointer(element.points.size(),
+ reinterpret_cast<const float*>(element.points.data()));
+
+ pclStyle->pointSize = element.lineWidth + 5;
+
return true;
}
} // namespace armarx::viz::coin
\ No newline at end of file
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationPath.h b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.h
index e1e446bbe66bc49a6d0c4bdf1999c7f0615f06ec..487ee9df58baeb6b91e91de6fdb3be873511a0af 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationPath.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.h
@@ -39,9 +39,17 @@ namespace armarx::viz::coin
bool update(ElementType const& element);
+ /// lines
SoCoordinate3* coordinate3;
SoDrawStyle* lineStyle;
SoLineSet* lineSet;
SoMaterial* lineMaterial;
+
+
+ /// points
+ // SoMaterial* pclMat;
+ SoCoordinate3* pclCoords;
+ SoDrawStyle* pclStyle;
+
};
} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
index e712e30ccb2978fdc7caa0210b919773b7dce46b..41fc3e85f7a6a7250c47fe58c0730973a16480be 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
@@ -22,6 +22,8 @@
#include "DynamicObstacleManager.h"
+#include <VirtualRobot/MathTools.h>
+
// STD/STL
#include <string>
#include <vector>
@@ -118,6 +120,7 @@ namespace armarx
void DynamicObstacleManager::add_decayable_line_segment(const Eigen::Vector2f& line_start, const Eigen::Vector2f& line_end, const Ice::Current&)
{
+ TIMING_START(add_decayable_line_segment);
const Eigen::Vector2f difference_vec = line_end - line_start;
const float length = difference_vec.norm();
const Eigen::Vector2f center_vec = line_start + 0.5 * difference_vec;
@@ -136,10 +139,10 @@ namespace armarx
return;
}
-
{
std::shared_lock<std::shared_mutex> l(m_managed_obstacles_mutex);
+ TIMING_START(add_decayable_line_segment_mutex);
// First check own obstacles
for (ManagedObstaclePtr& managed_obstacle : m_managed_obstacles)
{
@@ -155,13 +158,15 @@ namespace armarx
managed_obstacle->line_matches.push_back(line_start);
managed_obstacle->line_matches.push_back(line_end);
managed_obstacle->m_last_matched = IceUtil::Time::now();
+ TIMING_END(add_decayable_line_segment_mutex);
return;
}
}
+ TIMING_END(add_decayable_line_segment_mutex);
}
// Second check the obstacles from the obstacle avoidance
- for (const auto& published_obstacle : m_obstacle_detection->getObstacles())
+ /*for (const auto& published_obstacle : m_obstacle_detection->getObstacles())
{
float coverage = ManagedObstacle::line_segment_ellipsis_coverage(
{published_obstacle.posX, published_obstacle.posY}, published_obstacle.axisLengthX, published_obstacle.axisLengthY, published_obstacle.yaw,
@@ -172,7 +177,7 @@ namespace armarx
ARMARX_DEBUG << "Found the obstacle in the static obstacle list! Matching name: " << published_obstacle.name;
return;
}
- }
+ }*/
ARMARX_DEBUG << " No matching obstacle found. Create new obstacle and add to list";
ManagedObstaclePtr new_managed_obstacle(new ManagedObstacle());
@@ -198,6 +203,15 @@ namespace armarx
std::lock_guard<std::shared_mutex> l(m_managed_obstacles_mutex);
m_managed_obstacles.push_back(new_managed_obstacle);
}
+ TIMING_END(add_decayable_line_segment);
+ }
+
+ void DynamicObstacleManager::add_decayable_line_segments(const dynamicobstaclemanager::LineSegments& lines, const Ice::Current& c)
+ {
+ for (const auto& line : lines)
+ {
+ add_decayable_line_segment(line.lineStart, line.lineEnd, c);
+ }
}
@@ -218,7 +232,7 @@ namespace armarx
std::lock_guard l(m_managed_obstacles_mutex);
ARMARX_DEBUG << "Remove Obstacle " << name << " from obstacle map and from obstacledetection";
- for (ManagedObstaclePtr& managed_obstacle : m_managed_obstacles)
+ for (ManagedObstaclePtr managed_obstacle : m_managed_obstacles)
{
if (managed_obstacle->m_obstacle.name == name)
{
@@ -240,6 +254,36 @@ namespace armarx
}
+ float
+ DynamicObstacleManager::distanceToObstacle(const Eigen::Vector2f& agentPosition, float safetyRadius, const Eigen::Vector2f& goal, const Ice::Current&)
+ {
+ std::shared_lock<std::shared_mutex> l{m_managed_obstacles_mutex};
+
+ const float sample_step = 5; // in [mm], sample step size towards goal.
+ const float distance_to_goal = (goal - agentPosition).norm() + safetyRadius;
+ float current_distance = safetyRadius;
+
+ while (current_distance < distance_to_goal)
+ {
+ for (const auto man_obstacle : m_managed_obstacles)
+ {
+ Eigen::Vector2f sample = agentPosition + ((goal - agentPosition).normalized() * current_distance);
+ obstacledetection::Obstacle obstacle = man_obstacle->m_obstacle;
+ Eigen::Vector2f obstacle_origin{obstacle.posX, obstacle.posY};
+
+ if (ManagedObstacle::point_ellipsis_coverage(obstacle_origin, obstacle.axisLengthX, obstacle.axisLengthY, obstacle.yaw, sample))
+ {
+ return current_distance - safetyRadius;
+ }
+ }
+
+ current_distance += sample_step;
+ }
+
+ return std::numeric_limits<float>::infinity();
+ }
+
+
void DynamicObstacleManager::directly_update_obstacle(const std::string& name, const Eigen::Vector2f& obs_pos, float e_rx, float e_ry, float e_yaw, const Ice::Current&)
{
obstacledetection::Obstacle new_unmanaged_obstacle;
@@ -283,7 +327,6 @@ namespace armarx
}
}
-
// Update obstacle avoidance
int checked_obstacles = 0;
bool updated = false;
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
index 1629b7efe3102dccf85521ac0fac6379e09225cc..5f2d8065a307706ea16789930c12f5165d6044eb 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
@@ -69,10 +69,12 @@ namespace armarx
void add_decayable_obstacle(const Eigen::Vector2f&, float, float, float, const Ice::Current& = Ice::Current()) override;
void add_decayable_line_segment(const Eigen::Vector2f&, const Eigen::Vector2f&, const Ice::Current& = Ice::Current()) override;
+ void add_decayable_line_segments(const dynamicobstaclemanager::LineSegments& lines, const Ice::Current& = Ice::Current()) override;
void remove_all_decayable_obstacles(const Ice::Current& = Ice::Current()) override;
void directly_update_obstacle(const std::string& name, const Eigen::Vector2f&, float, float, float, const Ice::Current& = Ice::Current()) override;
void remove_obstacle(const std::string& name, const Ice::Current& = Ice::Current()) override;
void wait_unitl_obstacles_are_ready(const Ice::Current& = Ice::Current()) override;
+ float distanceToObstacle(const Eigen::Vector2f& agentPosition, float safetyRadius, const Eigen::Vector2f& goal, const Ice::Current& = Ice::Current()) override;
protected:
void onInitComponent() override;
diff --git a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
index fe21cc4e46558b3a70102592ee77ef56715421f8..f718a45434ec538fbd5d2541fb9413568f3e79f0 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
@@ -94,10 +94,25 @@ namespace armarx
float ManagedObstacle::line_segment_ellipsis_coverage(const Eigen::Vector2f e_origin, const float e_rx, const float e_ry, const float e_yaw, const Eigen::Vector2f line_seg_start, const Eigen::Vector2f line_seg_end)
{
// Again we discretize the line into n points and we check the coverage of those points
- const unsigned int SAMPLES = 40;
const Eigen::Vector2f difference_vec = line_seg_end - line_seg_start;
const Eigen::Vector2f difference_vec_normed = difference_vec.normalized();
const float distance = difference_vec.norm();
+ const unsigned int SAMPLES = std::max(distance / 10.0, 40.0);
+
+ const Vector2f difference_start_origin = e_origin - line_seg_start;
+ const Vector2f difference_end_origin = e_origin - line_seg_end;
+
+ if (difference_start_origin.norm() > std::max(e_rx, e_ry) && distance < std::max(e_rx, e_ry))
+ {
+ return 0.0;
+ }
+
+ if (difference_end_origin.norm() > std::max(e_rx, e_ry) && distance < std::max(e_rx, e_ry))
+ {
+ return 0.0;
+ }
+
+
const float step_size = distance / SAMPLES;
const Eigen::Vector2f dir = difference_vec_normed * step_size;
diff --git a/source/RobotAPI/components/units/CMakeLists.txt b/source/RobotAPI/components/units/CMakeLists.txt
index 51d1930b83753c195ff7558154092ea025466e4c..3fa53590170e0ff9d288f7f130adae9814ee7446 100644
--- a/source/RobotAPI/components/units/CMakeLists.txt
+++ b/source/RobotAPI/components/units/CMakeLists.txt
@@ -75,6 +75,7 @@ set(LIB_FILES
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
add_subdirectory(ObstacleAvoidingPlatformUnit)
+add_subdirectory(ObstacleAwarePlatformUnit)
add_subdirectory(relays)
add_subdirectory(RobotUnit)
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..524b650bf5a3767a92a457ab97b26ffcdd903a40
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt
@@ -0,0 +1,10 @@
+armarx_add_component(
+ COMPONENT_NAME ObstacleAwarePlatformUnit
+ HEADERS ObstacleAwarePlatformUnit.h
+ SOURCES ObstacleAwarePlatformUnit.cpp
+ COMPONENT_LIBS ArmarXCoreComponentPlugins
+ RobotAPICore
+ RobotAPIComponentPlugins
+ RobotUnit
+)
+armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2acb4a23b423df17ca7f80ffc76251634bcb933f
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
@@ -0,0 +1,728 @@
+/*
+ * 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::ObstacleAwarePlatformUnit
+ * @author Christian R. G. Dreher <c.dreher@kit.edu>
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#include <RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h>
+
+
+// STD/STL
+#include <algorithm>
+#include <cmath>
+#include <limits>
+#include <numeric>
+
+// Eigen
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+// Simox
+#include <SimoxUtility/math.h>
+
+// ArmarX
+#include <ArmarXCore/observers/variant/Variant.h>
+
+
+namespace
+{
+
+ void
+ invalidate(float& v)
+ {
+ v = std::numeric_limits<float>::infinity();
+ }
+
+
+ void
+ invalidate(Eigen::Vector2f& v)
+ {
+ v.x() = std::numeric_limits<float>::infinity();
+ v.y() = std::numeric_limits<float>::infinity();
+ }
+
+
+ void
+ invalidate(Eigen::Vector3f& v)
+ {
+ v.x() = std::numeric_limits<float>::infinity();
+ v.y() = std::numeric_limits<float>::infinity();
+ v.z() = std::numeric_limits<float>::infinity();
+ }
+
+ template<typename T>
+ void invalidate(std::deque<T>& d)
+ {
+ d.clear();
+ }
+
+
+ std::string
+ to_string(armarx::ObstacleAwarePlatformUnit::control_mode mode)
+ {
+ switch (mode)
+ {
+ case armarx::ObstacleAwarePlatformUnit::control_mode::position:
+ return "position";
+ case armarx::ObstacleAwarePlatformUnit::control_mode::velocity:
+ return "velocity";
+ case armarx::ObstacleAwarePlatformUnit::control_mode::none:
+ default:
+ return "unset";
+ }
+ }
+
+}
+
+
+const std::string
+armarx::ObstacleAwarePlatformUnit::default_name = "ObstacleAwarePlatformUnit";
+
+
+armarx::ObstacleAwarePlatformUnit::ObstacleAwarePlatformUnit() = default;
+
+
+armarx::ObstacleAwarePlatformUnit::~ObstacleAwarePlatformUnit() = default;
+
+
+void
+armarx::ObstacleAwarePlatformUnit::onInitPlatformUnit()
+{
+ ARMARX_DEBUG << "Initializing " << getName() << ".";
+
+ ARMARX_DEBUG << "Initialized " << getName() << ".";
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::onStartPlatformUnit()
+{
+ ARMARX_DEBUG << "Starting " << getName() << ".";
+
+ if (!hasRobot("robot"))
+ {
+ m_robot = addRobot("robot", VirtualRobot::RobotIO::eStructure);
+ }
+
+ invalidate(m_control_data.target_vel);
+ invalidate(m_control_data.target_rot_vel);
+ invalidate(m_control_data.target_pos);
+ invalidate(m_control_data.target_ori);
+ invalidate(m_viz.start);
+
+ ARMARX_DEBUG << "Started " << getName() << ".";
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::onExitPlatformUnit()
+{
+ ARMARX_DEBUG << "Exiting " << getName() << ".";
+
+ schedule_high_level_control_loop(control_mode::none);
+
+ ARMARX_DEBUG << "Exited " << getName() << ".";
+}
+
+
+std::string
+armarx::ObstacleAwarePlatformUnit::getDefaultName()
+const
+{
+ return default_name;
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::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&)
+{
+ using namespace simox::math;
+
+ 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 = periodic_clamp<float>(target_ori, -M_PI, M_PI);
+ m_control_data.pos_reached_threshold = pos_reached_threshold;
+ m_control_data.ori_reached_threshold = ori_reached_threshold;
+
+ invalidate(m_control_data.target_vel);
+ invalidate(m_control_data.target_rot_vel);
+
+ schedule_high_level_control_loop(control_mode::position);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::move(
+ const float target_vel_x,
+ const float target_vel_y,
+ const float target_rot_vel,
+ const Ice::Current&)
+{
+ using namespace simox::math;
+
+ 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 = periodic_clamp<float>(target_rot_vel, -M_PI, M_PI);
+
+ invalidate(m_control_data.target_pos);
+ invalidate(m_control_data.target_ori);
+
+ 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::ObstacleAwarePlatformUnit::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 = mat4f_to_rpy(m_robot->getGlobalPose()).z();
+ 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::ObstacleAwarePlatformUnit::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::ObstacleAwarePlatformUnit::stopPlatform(const Ice::Current&)
+{
+ schedule_high_level_control_loop(control_mode::none);
+ m_platform->stopPlatform();
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::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)
+ {
+ ARMARX_VERBOSE << "Stopping " << ::to_string(m_control_loop.mode) << " control.";
+ 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)
+ {
+ ARMARX_VERBOSE << "Going into stand-by.";
+ return;
+ }
+
+ // Start next control loop.
+ ARMARX_VERBOSE << "Starting " << ::to_string(mode) << " control.";
+ m_control_loop.mode = mode;
+ m_control_loop.task = new RunningTask<ObstacleAwarePlatformUnit>(
+ this,
+ &ObstacleAwarePlatformUnit::high_level_control_loop);
+ m_control_loop.task->start();
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::high_level_control_loop()
+{
+ const control_mode mode = m_control_loop.mode;
+ ARMARX_VERBOSE << "Started " << ::to_string(mode) << " control.";
+
+ try
+ {
+ CycleUtil cu{m_control_loop.cycle_time};
+ while (not m_control_loop.task->isStopped())
+ {
+ const velocities vels = get_velocities();
+
+ // Debug.
+ StringVariantBaseMap m;
+ m["err_dist"] = new Variant{vels.err_dist};
+ m["err_angular_dist"] = new Variant{vels.err_angular_dist};
+
+ m["target_global_x"] = new Variant{vels.target_global.x()};
+ m["target_global_y"] = new Variant{vels.target_global.y()};
+ m["target_global_abs"] = new Variant{vels.target_global.norm()};
+
+ m["target_local_x"] = new Variant{vels.target_local.x()};
+ m["target_local_y"] = new Variant{vels.target_local.y()};
+ m["target_local_abs"] = new Variant(vels.target_local.norm());
+ m["target_rot"] = new Variant{vels.target_rot};
+
+ m["modulated_global_x"] = new Variant{vels.modulated_global.x()};
+ m["modulated_global_y"] = new Variant{vels.modulated_global.y()};
+ m["modulated_global_abs"] = new Variant{vels.modulated_global.norm()};
+
+ m["modulated_local_x"] = new Variant{vels.modulated_local.x()};
+ m["modulated_local_y"] = new Variant{vels.modulated_local.y()};
+ m["modulated_local_abs"] = new Variant{vels.modulated_local.norm()};
+
+ setDebugObserverChannel("ObstacleAvoidingPlatformCtrl", m);
+
+ m_platform->move(vels.modulated_local.x(), vels.modulated_local.y(), vels.target_rot);
+ visualize(vels);
+ 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.";
+ }
+
+ m_platform->move(0, 0, 0);
+ m_platform->stopPlatform();
+ m_control_loop.mode = control_mode::none;
+
+ visualize();
+
+ ARMARX_VERBOSE << "Stopped " << ::to_string(mode) << " control.";
+}
+
+
+armarx::ObstacleAwarePlatformUnit::velocities
+armarx::ObstacleAwarePlatformUnit::get_velocities()
+{
+ using namespace simox::math;
+
+ // Acquire control_data mutex to ensure data remains consistent.
+ std::scoped_lock l{m_control_data.mutex};
+ // Update agent and get target velocities.
+ update_agent_dependent_values();
+ const Eigen::Vector2f target_vel = get_target_velocity();
+ const float target_rot_vel = get_target_rotational_velocity();
+
+ const Eigen::Vector2f extents = Eigen::Affine3f(m_robot->getRobotNode("PlatformCornerFrontLeft")->getPoseInRootFrame()).translation().head<2>();
+
+ // Apply obstacle avoidance and apply post processing to get the modulated velocity.
+ const Eigen::Vector2f modulated_vel = [this, &target_vel, &extents]
+ {
+ const VirtualRobot::Circle circle = VirtualRobot::projectedBoundingCircle(*m_robot);
+ ARMARX_DEBUG << "Circle approximation: " << circle.radius;
+ m_viz.boundingCircle = circle;
+ const float distance = m_obsman->distanceToObstacle(m_control_data.agent_pos /*circle.center*/, circle.radius, m_control_data.target_pos);
+
+ ARMARX_DEBUG << "Distance to obstacle: " << distance;
+
+ // https://www.wolframalpha.com/input/?i=line+through+%281000%2C+0%29+and+%282000%2C+1%29
+ float modifier = std::clamp((distance / 1000) - 1., 0.0, 1.0);
+
+ const Eigen::Vector2f vel = modifier * target_vel;
+ return vel.norm() > 20 ? vel : Eigen::Vector2f{0, 0};
+ }();
+
+ ARMARX_CHECK(modulated_vel.allFinite()) << "Velocity contains non-finite values.";
+ ARMARX_CHECK(std::isfinite(target_rot_vel)) << "Rotation velocity is non-finite.";
+
+ ARMARX_DEBUG << "Target velocity: " << target_vel.transpose()
+ << "; norm: " << target_vel.norm() << "; " << target_rot_vel;
+ ARMARX_DEBUG << "Modulated velocity: " << modulated_vel.transpose() << modulated_vel.norm();
+
+ const auto r = Eigen::Rotation2D(m_control_data.agent_ori).toRotationMatrix().inverse();
+
+ velocities vels;
+ vels.target_local = r * target_vel;
+ vels.target_global = target_vel;
+ vels.modulated_local = r * modulated_vel;
+ vels.modulated_global = modulated_vel;
+ vels.target_rot = target_rot_vel;
+ vels.err_dist = m_control_data.target_dist;
+ vels.err_angular_dist = m_control_data.target_angular_dist;
+
+ return vels;
+}
+
+
+Eigen::Vector2f
+armarx::ObstacleAwarePlatformUnit::get_target_velocity()
+const
+{
+ using namespace simox::math;
+
+ Eigen::Vector2f uncapped_target_vel = Eigen::Vector2f::Zero();
+
+ if (m_control_loop.mode == control_mode::position /*and not target_position_reached()*/)
+ {
+ uncapped_target_vel =
+ (m_control_data.target_pos - m_control_data.agent_pos) * m_control_data.kp;
+ }
+ else if (m_control_loop.mode == control_mode::velocity)
+ {
+ uncapped_target_vel = m_control_data.target_vel;
+ }
+
+ ARMARX_CHECK(uncapped_target_vel.allFinite());
+
+ return norm_max(uncapped_target_vel, m_control_data.max_vel);
+}
+
+
+float
+armarx::ObstacleAwarePlatformUnit::get_target_rotational_velocity()
+const
+{
+ using namespace simox::math;
+
+ float uncapped_target_rot_vel = 0;
+
+ if (m_control_loop.mode == control_mode::position /*and not target_orientation_reached()*/)
+ {
+ m_rot_pid_controller.update(m_control_data.target_angular_dist, 0);
+ uncapped_target_rot_vel = -m_rot_pid_controller.getControlValue();
+ }
+ else if (m_control_loop.mode == control_mode::velocity)
+ {
+ uncapped_target_rot_vel = m_control_data.target_rot_vel;
+ }
+
+ ARMARX_CHECK(std::isfinite(uncapped_target_rot_vel));
+
+ return std::copysign(std::min(std::fabs(uncapped_target_rot_vel), m_control_data.max_rot_vel),
+ uncapped_target_rot_vel);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::update_agent_dependent_values()
+{
+ using namespace simox::math;
+
+ 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);
+
+ ARMARX_CHECK_GREATER(m_control_data.agent_ori, -M_PI);
+ ARMARX_CHECK_LESS(m_control_data.agent_ori, M_PI);
+
+ // Update distances in position mode.
+ if (m_control_loop.mode == control_mode::position)
+ {
+ ARMARX_CHECK_GREATER(m_control_data.target_ori, -M_PI);
+ ARMARX_CHECK_LESS(m_control_data.target_ori, M_PI);
+ ARMARX_CHECK(m_control_data.target_pos.allFinite());
+ ARMARX_CHECK(std::isfinite(m_control_data.target_ori));
+
+ 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_CHECK_GREATER(m_control_data.target_angular_dist, -M_PI);
+ ARMARX_CHECK_LESS(m_control_data.target_angular_dist, M_PI);
+
+ ARMARX_DEBUG << "Distance to target: " << m_control_data.target_dist << " mm and "
+ << m_control_data.target_angular_dist << " rad.";
+ }
+ // Otherwise invalidate them.
+ else
+ {
+ invalidate(m_control_data.target_dist);
+ invalidate(m_control_data.target_angular_dist);
+ }
+}
+
+
+bool
+armarx::ObstacleAwarePlatformUnit::target_position_reached()
+const
+{
+ 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::ObstacleAwarePlatformUnit::target_orientation_reached()
+const
+{
+ if (m_control_loop.mode == control_mode::position)
+ {
+ return std::fabs(m_control_data.target_angular_dist) < m_control_data.ori_reached_threshold;
+ }
+
+ // Cannot reach any target in non-position mode.
+ return false;
+}
+
+
+bool
+armarx::ObstacleAwarePlatformUnit::target_reached()
+const
+{
+ if (m_control_loop.mode == control_mode::position)
+ {
+ return target_position_reached() and target_orientation_reached();
+ }
+
+ return false;
+}
+
+
+bool
+armarx::ObstacleAwarePlatformUnit::is_near_target(const Eigen::Vector2f& control_velocity)
+const noexcept
+{
+ if (m_control_data.target_dist < m_control_data.pos_near_threshold)
+ {
+ const Eigen::Vector2f target_direction = m_control_data.target_pos - m_control_data.agent_pos;
+ const Eigen::Vector2f control_direction = control_velocity / control_velocity.norm();
+
+ const float sim = simox::math::cosine_similarity(target_direction, control_direction);
+
+ // if almost pointing into same direction
+ if (sim > cos(M_PI_4f32))
+ {
+ return true;
+ }
+ }
+
+ return false;
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::visualize()
+{
+ const Eigen::Vector2f zero = Eigen::Vector2f::Zero();
+ velocities vels;
+ vels.target_local = zero;
+ vels.target_global = zero;
+ vels.modulated_local = zero;
+ vels.modulated_global = zero;
+ vels.target_rot = 0;
+
+ visualize(vels);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::visualize(const velocities& vels)
+{
+ using namespace armarx::viz;
+
+ if (not m_viz.enabled)
+ {
+ return;
+ }
+
+ Eigen::Vector3f agent_pos{m_control_data.agent_pos.x(), m_control_data.agent_pos.y(), 0};
+
+ // Progress. Only draw in position control mode.
+ Layer l_prog = arviz.layer("progress");
+ if (m_control_loop.mode == control_mode::position)
+ {
+ const float min_keypoint_dist = 50;
+
+ {
+ l_prog.add(Cylinder{"boundingCylinder"}
+ .position(agent_pos)
+ .color(Color::cyan(255, 64))
+ .pose(simox::math::pos_rpy_to_mat4f(agent_pos, -M_PI_2, 0, 0))
+ .radius(m_viz.boundingCircle.radius)
+ .height(2000));
+ }
+
+ // If no start is set, use current agent pos.
+ if (not m_viz.start.allFinite())
+ {
+ m_viz.start = agent_pos;
+ }
+
+ const Eigen::Vector3f& last_keypoint_pos =
+ m_viz.path.size() >= 1 ? m_viz.path.back() : m_viz.start;
+
+ // Keep a certain distance between path keypoints.
+ if ((last_keypoint_pos - agent_pos).norm() > min_keypoint_dist)
+ {
+ m_viz.path.push_back(agent_pos);
+ }
+
+ // Draw path.
+ if (not target_reached())
+ {
+ // Start.
+ l_prog.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)
+ {
+ l_prog.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};
+ l_prog.add(Arrow{"goal"}
+ .fromTo(target + Eigen::Vector3f{0, 0, 220},
+ target + Eigen::Vector3f{0, 0, 40})
+ .color(Color::red(255, 128))
+ .width(20));
+ }
+ else
+ {
+ m_viz.path.clear();
+ invalidate(m_viz.start);
+ }
+ }
+
+ // Velocities.
+ Layer l_vels = arviz.layer("velocities");
+ if (m_control_loop.mode != control_mode::none)
+ {
+ const float min_velocity = 15;
+ const Eigen::Vector3f from1{agent_pos + Eigen::Vector3f{0, 0, 2000}};
+ const Eigen::Vector3f from2 = from1 + Eigen::Vector3f{0, 0, 200};
+ const Eigen::Vector3f original{vels.target_global.x(), vels.target_global.y(), 0};
+ const Eigen::Vector3f modulated{vels.modulated_global.x(), vels.modulated_global.y(), 0};
+
+ if (original.norm() > min_velocity)
+ {
+ l_vels.add(Arrow{"original"}
+ .fromTo(from1, from1 + original * 5)
+ .color(Color::green(255, 128))
+ .width(25));
+ }
+
+ if (modulated.norm() > min_velocity)
+ {
+ l_vels.add(Arrow{"modulated"}
+ .fromTo(from2, from2 + modulated * 5)
+ .color(Color::cyan(255, 128))
+ .width(25));
+ }
+ }
+
+ // Agent.
+ Layer l_agnt = arviz.layer("agent");
+ if (m_control_loop.mode != control_mode::none)
+ {
+ l_agnt.add(Sphere{"agent"}
+ .position(agent_pos)
+ .color(Color::red(255, 128))
+ .radius(50));
+
+ // Agent safety margin.
+ if (m_control_data.agent_safety_margin > 0)
+ {
+ l_agnt.add(Cylinder{"agent_safety_margin"}
+ .pose(simox::math::pos_rpy_to_mat4f(agent_pos, -M_PI_2, 0, 0))
+ .color(Color::red(255, 64))
+ .radius(m_control_data.agent_safety_margin)
+ .height(2));
+ }
+ }
+
+ arviz.commit({l_prog, l_vels, l_agnt});
+}
+
+
+armarx::PropertyDefinitionsPtr
+armarx::ObstacleAwarePlatformUnit::createPropertyDefinitions()
+{
+ PropertyDefinitionsPtr def = PlatformUnit::createPropertyDefinitions();
+
+ def->component(m_platform, "Platform");
+ def->component(m_obsman, "ObstacleAvoidingPlatformUnit");
+
+ // Settings.
+ def->optional(m_control_data.min_vel_near_target, "min_vel_near_target", "Velocity in [mm/s] "
+ "the robot should at least set when near the target");
+ def->optional(m_control_data.min_vel_general, "min_vel_general", "Velocity in [mm/s] the robot "
+ "should at least set on general");
+ def->optional(m_control_data.pos_near_threshold, "pos_near_threshold", "Distance in [mm] after "
+ "which the robot is considered to be near the target for min velocity, "
+ "smoothing, etc.");
+
+ // 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", "Control loop cycle time.");
+
+ // Obstacle avoidance parameter.
+ def->optional(m_control_data.agent_safety_margin, "doa.agent_safety_margin");
+
+ return def;
+}
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..274b67ea153eec20473b67fa4f45b8425c7b174d
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h
@@ -0,0 +1,259 @@
+/*
+ * 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::ObstacleAwarePlatformUnit
+ * @author Christian R. G. Dreher <c.dreher@kit.edu>
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+
+// STD/STL
+#include <deque>
+#include <string>
+#include <tuple>
+#include <mutex>
+#include <vector>
+
+// Eigen
+#include <Eigen/Core>
+
+// Ice
+#include <IceUtil/Time.h>
+
+// Simox
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Safety.h>
+
+// ArmarX
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+#include <ArmarXCore/util/tasks.h>
+#include <ArmarXCore/util/time.h>
+
+// RobotAPI
+#include <RobotAPI/components/units/PlatformUnit.h>
+#include <RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.h>
+#include <RobotAPI/libraries/core/PIDController.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
+
+
+namespace armarx
+{
+
+ class ObstacleAwarePlatformUnit :
+ virtual public PlatformUnit,
+ virtual public RobotStateComponentPluginUser,
+ virtual public ArVizComponentPluginUser,
+ virtual public DebugObserverComponentPluginUser
+ {
+
+ public:
+
+ enum class control_mode
+ {
+ position,
+ velocity,
+ none
+ };
+
+ private:
+
+ struct control_loop
+ {
+ std::mutex mutex;
+ control_mode mode = control_mode::none;
+ RunningTask<ObstacleAwarePlatformUnit>::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;
+ double agent_safety_margin = 0;
+ float min_vel_near_target = 50;
+ float min_vel_general = 100;
+ float max_vel = 200;
+ float max_rot_vel = 0.3;
+ float pos_near_threshold = 250;
+ float pos_reached_threshold = 8;
+ float ori_reached_threshold = 0.1;
+ float kp = 3.5;
+ };
+
+ struct visualization
+ {
+ Eigen::Vector3f start;
+ std::vector<Eigen::Vector3f> path;
+ bool enabled = true;
+ VirtualRobot::Circle boundingCircle;
+ };
+
+ struct velocities
+ {
+ Eigen::Vector2f target_local;
+ Eigen::Vector2f modulated_local;
+ Eigen::Vector2f target_global;
+ Eigen::Vector2f modulated_global;
+ float target_rot;
+ float err_dist;
+ float err_angular_dist;
+ };
+
+ public:
+
+ ObstacleAwarePlatformUnit();
+
+ ~ObstacleAwarePlatformUnit()
+ override;
+
+ std::string
+ getDefaultName()
+ const override;
+
+ 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;
+
+ void
+ move(
+ float target_vel_x,
+ float target_vel_y,
+ float target_rot_vel,
+ const Ice::Current& = Ice::Current{})
+ override;
+
+ 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;
+
+ void
+ setMaxVelocities(
+ float max_pos_vel,
+ float max_rot_vel,
+ const Ice::Current& = Ice::Current{})
+ override;
+
+ void
+ stopPlatform(const Ice::Current& = Ice::Current{})
+ override;
+
+ protected:
+
+ void
+ onInitPlatformUnit()
+ override;
+
+ void
+ onStartPlatformUnit()
+ override;
+
+ void
+ onExitPlatformUnit()
+ override;
+
+ PropertyDefinitionsPtr
+ createPropertyDefinitions()
+ override;
+
+ private:
+
+ void
+ schedule_high_level_control_loop(control_mode mode);
+
+ void
+ high_level_control_loop();
+
+ velocities
+ get_velocities();
+
+ void
+ update_agent_dependent_values();
+
+ Eigen::Vector2f
+ get_target_velocity()
+ const;
+
+ float
+ get_target_rotational_velocity()
+ const;
+
+ bool
+ target_position_reached()
+ const;
+
+ bool
+ target_orientation_reached()
+ const;
+
+ bool
+ target_reached()
+ const;
+
+ void
+ visualize();
+
+ void
+ visualize(const velocities& vels);
+
+ bool
+ is_near_target(const Eigen::Vector2f& control_velocity)
+ const
+ noexcept;
+
+ public:
+
+ static const std::string default_name;
+
+ private:
+
+ PlatformUnitInterfacePrx m_platform;
+ VirtualRobot::RobotPtr m_robot;
+ DynamicObstacleManagerInterfacePrx m_obsman;
+
+ ObstacleAwarePlatformUnit::control_loop m_control_loop;
+ ObstacleAwarePlatformUnit::control_data m_control_data;
+
+ mutable PIDController m_rot_pid_controller{1.0, 0.00009, 0.0};
+
+ visualization m_viz;
+
+ };
+
+}
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/main.cpp b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f168208f6734ea228aa848dcf0c5d0976370bae7
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/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::ObstacleAwarePlatformUnit
+ * @author Christian R. G. Dreher <c.dreher@kit.edu>
+ * @date 2021
+ * @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/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h>
+
+
+int main(int argc, char* argv[])
+{
+ using namespace armarx;
+ const std::string name = ObstacleAwarePlatformUnit::default_name;
+ return runSimpleComponentApp<ObstacleAwarePlatformUnit>(argc, argv, name);
+}
diff --git a/source/RobotAPI/interface/ArViz/Elements.ice b/source/RobotAPI/interface/ArViz/Elements.ice
index 1c00dd4e630208a79852ea3d18d96da8a501f7d0..bebbb3f414be2184f51fdb8518cdcd368da33ebc 100644
--- a/source/RobotAPI/interface/ArViz/Elements.ice
+++ b/source/RobotAPI/interface/ArViz/Elements.ice
@@ -104,7 +104,6 @@ module data
{
Vector3fSeq points;
- Color lineColor;
float lineWidth = 10.0f;
};
diff --git a/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice b/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
index d21a3c55b7574a459f0be3e2577870cf7640f5e3..5ff2649fae12cb906f5821f73ba1fb1fd21647fe 100644
--- a/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
+++ b/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
@@ -30,6 +30,18 @@
module armarx
{
+ module dynamicobstaclemanager
+ {
+ struct LineSegment
+ {
+ Eigen::Vector2f lineStart;
+ Eigen::Vector2f lineEnd;
+ };
+
+ sequence<LineSegment> LineSegments;
+ }
+
+
interface DynamicObstacleManagerInterface
{
void
@@ -38,6 +50,9 @@ module armarx
void
add_decayable_line_segment(Eigen::Vector2f line_start, Eigen::Vector2f line_end);
+ void
+ add_decayable_line_segments(dynamicobstaclemanager::LineSegments lines);
+
void
remove_all_decayable_obstacles();
@@ -48,6 +63,8 @@ module armarx
remove_obstacle(string name);
void wait_unitl_obstacles_are_ready();
+
+ float distanceToObstacle(Eigen::Vector2f agentPosition, float safetyRadius, Eigen::Vector2f goal);
};
};