diff --git a/data/RobotAPI/obstacle_avoidance/2d_scene_obstacles.json b/data/RobotAPI/obstacle_avoidance/2d_scene_obstacles.json
index 2f86cf315a8612ceade3f1ddefecd3c1d9ee8b0e..caa034c2e9f9d21d6322f8e48c0483aa337eba42 100644
--- a/data/RobotAPI/obstacle_avoidance/2d_scene_obstacles.json
+++ b/data/RobotAPI/obstacle_avoidance/2d_scene_obstacles.json
@@ -77,11 +77,11 @@
"safetyMarginY": 400
},{
"name": "table_corner",
- "posX": 3100,
- "posY": 5216,
+ "posX": 3300,
+ "posY": 4916,
"yaw": 0,
"axisLengthX": 200,
- "axisLengthY": 1000,
+ "axisLengthY": 500,
"refPosX": 3400,
"refPosY": 5216,
"safetyMarginX": 500,
diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
index 950edbf74b1bdc1516614b7dfc8ac5c9a9598845..dcf0ebd846beb390d2bb1c29e511e14555e1cb42 100644
--- a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
+++ b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
@@ -26,15 +26,20 @@
// 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
{
@@ -62,6 +67,12 @@ namespace
v.z() = std::numeric_limits<float>::infinity();
}
+ template<typename T>
+ void invalidate(std::deque<T>& d)
+ {
+ d.clear();
+ }
+
std::string
to_string(armarx::ObstacleAvoidingPlatformUnit::control_mode mode)
@@ -85,16 +96,10 @@ const std::string
armarx::ObstacleAvoidingPlatformUnit::default_name = "ObstacleAvoidingPlatformUnit";
-armarx::ObstacleAvoidingPlatformUnit::ObstacleAvoidingPlatformUnit()
-{
- ;
-}
+armarx::ObstacleAvoidingPlatformUnit::ObstacleAvoidingPlatformUnit() = default;
-armarx::ObstacleAvoidingPlatformUnit::~ObstacleAvoidingPlatformUnit()
-{
- ;
-}
+armarx::ObstacleAvoidingPlatformUnit::~ObstacleAvoidingPlatformUnit() = default;
void
@@ -121,6 +126,7 @@ armarx::ObstacleAvoidingPlatformUnit::onStartPlatformUnit()
invalidate(m_control_data.target_pos);
invalidate(m_control_data.target_ori);
invalidate(m_viz.start);
+ invalidate(m_control_data.vel_history);
ARMARX_DEBUG << "Started " << getName() << ".";
}
@@ -154,13 +160,18 @@ armarx::ObstacleAvoidingPlatformUnit::moveTo(
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 = target_ori;
+ 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;
+ // clear the buffer to prevent any movement into arbitrary directions
+ invalidate(m_control_data.vel_history);
+
invalidate(m_control_data.target_vel);
invalidate(m_control_data.target_rot_vel);
@@ -295,15 +306,24 @@ armarx::ObstacleAvoidingPlatformUnit::high_level_control_loop()
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);
@@ -321,6 +341,9 @@ armarx::ObstacleAvoidingPlatformUnit::high_level_control_loop()
ARMARX_ERROR << "Unknown error occured while running control loop.";
}
+ // clear the buffer such that zero-velocity is set without delay
+ invalidate(m_control_data.vel_history);
+
m_platform->move(0, 0, 0);
m_platform->stopPlatform();
m_control_loop.mode = control_mode::none;
@@ -529,14 +552,47 @@ armarx::ObstacleAvoidingPlatformUnit::post_process_vel(
// Smooth by calculating mean over the last `m_control_data.amount_smoothing` elements.
const Eigen::Vector2f mean_modulated_vel = calculate_mean_modulated_vel();
+ const bool is_near_target = this->is_near_target(mean_modulated_vel);
+
+ // Min velocity is determined through distance to target.
+ float min_vel = is_near_target ? m_control_data.min_vel_near_target : m_control_data.min_vel_general;
+ if (target_vel.norm() < min_vel)
+ {
+ min_vel = target_vel.norm();
+ }
+
// 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();
+ // If the robot is almost at the goal position and no collision needs to be avoided
+ // (aka moving into the direction of the target position), we can prevent overshooting by
+ // clipping the velocity. The reference velocity is computed by a standard P-controller.
+ const float max_vel = is_near_target ? target_vel.norm() : calculate_adaptive_max_vel();
ARMARX_CHECK(mean_modulated_vel.allFinite());
+ ARMARX_CHECK(std::isfinite(min_vel));
ARMARX_CHECK(std::isfinite(max_vel));
- return simox::math::norm_max(mean_modulated_vel, max_vel);
+ return simox::math::norm_clamp(mean_modulated_vel, min_vel, max_vel);
+}
+
+
+bool armarx::ObstacleAvoidingPlatformUnit::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;
}
@@ -586,6 +642,9 @@ const
(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));
@@ -805,6 +864,13 @@ armarx::ObstacleAvoidingPlatformUnit::createPropertyDefinitions()
"Adaptive max vel exponent. This throttles the max_vel adaptively "
"depending on the angle between target velocity and modulated velocity. "
"0 = disable.");
+ 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");
diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h
index 2fa54737a94c647df0d3234873d194e567d40c07..1535fdc74c809fd43518095997a51e64aa31cc23 100644
--- a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h
+++ b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h
@@ -94,8 +94,11 @@ namespace armarx
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;
@@ -257,6 +260,11 @@ namespace armarx
void
visualize(const velocities& vels);
+ bool
+ is_near_target(const Eigen::Vector2f& control_velocity)
+ const
+ noexcept;
+
public:
static const std::string default_name;
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp
index 03ed255120ccf8a5084ea824f7d8e2d790c1c485..0cbf7e730f56186618f92c0a74be2b5dc5ad0f5a 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp
@@ -24,26 +24,28 @@
#include <RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h>
-// STD/STL
-#include <limits>
-
// Simox
#include <SimoxUtility/math.h>
+armarx::ObstacleAvoidingPlatformUnitHelper::ObstacleAvoidingPlatformUnitHelper(
+ armarx::PlatformUnitInterfacePrx platform_unit,
+ VirtualRobot::RobotPtr robot) :
+ ObstacleAvoidingPlatformUnitHelper{platform_unit, robot, Config{}}
+{
+ // pass
+}
+
+
armarx::ObstacleAvoidingPlatformUnitHelper::ObstacleAvoidingPlatformUnitHelper(
armarx::PlatformUnitInterfacePrx platform_unit,
VirtualRobot::RobotPtr robot,
- const float pos_reached_threshold,
- const float ori_reached_threshold) :
+ const Config& cfg) :
m_platform_unit{platform_unit},
m_robot{robot},
- m_pos_reached_threshold{pos_reached_threshold},
- m_ori_reached_threshold{ori_reached_threshold}
+ m_cfg{cfg}
{
- const float inf = std::numeric_limits<float>::infinity();
- m_target.pos = Eigen::Vector2f{inf, inf};
- m_target.ori = inf;
+ // pass
}
@@ -58,36 +60,119 @@ armarx::ObstacleAvoidingPlatformUnitHelper::setTarget(
const Eigen::Vector2f& target_pos,
const float target_ori)
{
- m_target.pos = target_pos;
- m_target.ori = target_ori;
+ setTarget(Target{target_pos, target_ori});
+}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnitHelper::setTarget(const Target& target)
+{
+ m_waypoints.clear();
+ m_waypoints.push_back(target);
+ m_current_waypoint_index = 0;
+ m_waypoint_changed = true;
+}
+
+
+armarx::ObstacleAvoidingPlatformUnitHelper::Target
+armarx::ObstacleAvoidingPlatformUnitHelper::getCurrentTarget()
+const
+{
+ return m_waypoints.at(m_current_waypoint_index);
}
void
armarx::ObstacleAvoidingPlatformUnitHelper::update()
{
- m_platform_unit->moveTo(m_target.pos.x(), m_target.pos.y(), m_target.ori,
- m_pos_reached_threshold, m_ori_reached_threshold);
+ // Skip to next waypoint if in proximity.
+ if (isCurrentTargetNear() and not isLastWaypoint())
+ {
+ m_current_waypoint_index++;
+ m_waypoint_changed = true;
+ }
+
+ // Only call `moveTo` on platform unit if the target actually changed, as `update` is designed
+ // to be called within a high-frequency loop. Prevents constantly invalidating the buffers in
+ // platform unit.
+ if (m_waypoint_changed)
+ {
+ // Use reached-thresholds regardless of whether this is the final target or just a waypoint.
+ // The near-thresholds are more unconstrained as the reached-thresholds, so this helper will
+ // change the position target before the unit will actually reach it, preventing slow downs
+ // or stops.
+ const float pos_thresh = m_cfg.pos_reached_threshold;
+ const float ori_thresh = m_cfg.ori_reached_threshold;
+
+ // Control.
+ const Target target = getCurrentTarget();
+ m_platform_unit->moveTo(target.pos.x(), target.pos.y(), target.ori, pos_thresh, ori_thresh);
+
+ m_waypoint_changed = false;
+ }
+}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnitHelper::addWaypoint(const Eigen::Vector2f& waypoint_pos, float waypoint_ori)
+{
+ addWaypoint(Target{waypoint_pos, waypoint_ori});
+}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnitHelper::setWaypoints(const std::vector<Target>& waypoints)
+{
+ m_waypoints = waypoints;
+ m_current_waypoint_index = 0;
+ m_waypoint_changed = true;
+}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnitHelper::addWaypoint(const Target& waypoint)
+{
+ m_waypoints.push_back(waypoint);
}
bool
-armarx::ObstacleAvoidingPlatformUnitHelper::isFinalTargetReached()
+armarx::ObstacleAvoidingPlatformUnitHelper::isLastWaypoint()
const
{
- using namespace simox::math;
+ return m_current_waypoint_index + 1 >= m_waypoints.size();
+}
- // Determine agent position.
- 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);
- // Determine distance and angular distance to goal position and orientation.
- const float target_dist = (m_target.pos - agent_pos).norm();
- const float target_angular_dist = periodic_clamp<float>(m_target.ori - agent_ori, -M_PI, M_PI);
+bool
+armarx::ObstacleAvoidingPlatformUnitHelper::isCurrentTargetNear()
+const
+{
+ return getPositionError() < m_cfg.pos_near_threshold and getOrientationError() < m_cfg.ori_near_threshold;
+}
+
- return target_dist < m_pos_reached_threshold and
- std::fabs(target_angular_dist) < m_ori_reached_threshold;
+bool
+armarx::ObstacleAvoidingPlatformUnitHelper::isCurrentTargetReached()
+const
+{
+ return getPositionError() < m_cfg.pos_reached_threshold and getOrientationError() < m_cfg.ori_reached_threshold;
+}
+
+
+bool
+armarx::ObstacleAvoidingPlatformUnitHelper::isFinalTargetNear()
+const
+{
+ return isLastWaypoint() and isCurrentTargetNear();
+}
+
+
+bool
+armarx::ObstacleAvoidingPlatformUnitHelper::isFinalTargetReached()
+const
+{
+ return isLastWaypoint() and isCurrentTargetReached();
}
@@ -96,3 +181,24 @@ armarx::ObstacleAvoidingPlatformUnitHelper::setMaxVelocities(float max_vel, floa
{
m_platform_unit->setMaxVelocities(max_vel, max_angular_vel);
}
+
+
+float
+armarx::ObstacleAvoidingPlatformUnitHelper::getPositionError()
+const
+{
+ const Eigen::Vector2f agent_pos = m_robot->getGlobalPosition().head<2>();
+ return (getCurrentTarget().pos - agent_pos).norm();
+}
+
+
+float
+armarx::ObstacleAvoidingPlatformUnitHelper::getOrientationError()
+const
+{
+ using namespace simox::math;
+
+ const float agent_ori =
+ periodic_clamp<float>(mat4f_to_rpy(m_robot->getGlobalPose()).z(), -M_PI, M_PI);
+ return std::fabs(periodic_clamp<float>(getCurrentTarget().ori - agent_ori, -M_PI, M_PI));
+}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h
index 12d7100f997eeb818f6cf0aff989e42a92059df5..85d7543c956369817a0bb4df0c3dbda9fd22a576 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h
@@ -42,11 +42,30 @@ namespace armarx
public:
+ struct Config
+ {
+ float pos_reached_threshold = 10; // [mm]
+ float ori_reached_threshold = 0.1; // [rad]
+ float pos_near_threshold = 50; // [mm]
+ float ori_near_threshold = 0.2; // [rad]
+ };
+
+ struct Target
+ {
+ Eigen::Vector2f pos;
+ float ori;
+ };
+
+ public:
+
+ ObstacleAvoidingPlatformUnitHelper(
+ armarx::PlatformUnitInterfacePrx platform_unit,
+ VirtualRobot::RobotPtr robot);
+
ObstacleAvoidingPlatformUnitHelper(
armarx::PlatformUnitInterfacePrx platform_unit,
VirtualRobot::RobotPtr robot,
- float pos_reached_threshold,
- float ori_reached_threshold);
+ const Config& cfg);
virtual
~ObstacleAvoidingPlatformUnitHelper();
@@ -54,9 +73,40 @@ namespace armarx
void
setTarget(const Eigen::Vector2f& target_pos, float target_ori);
+ void
+ setTarget(const Target& target);
+
+ Target
+ getCurrentTarget() const;
+
void
update();
+ void
+ setWaypoints(const std::vector<Target>& waypoints);
+
+ void
+ addWaypoint(const Eigen::Vector2f& waypoint_pos, float waypoint_ori);
+
+ void
+ addWaypoint(const Target& waypoint);
+
+ bool
+ isLastWaypoint()
+ const;
+
+ bool
+ isCurrentTargetNear()
+ const;
+
+ bool
+ isCurrentTargetReached()
+ const;
+
+ bool
+ isFinalTargetNear()
+ const;
+
bool
isFinalTargetReached()
const;
@@ -64,22 +114,25 @@ namespace armarx
void
setMaxVelocities(float max_vel, float max_angular_vel);
- private:
+ float
+ getPositionError()
+ const;
- struct target
- {
- Eigen::Vector2f pos;
- float ori;
- };
+ float
+ getOrientationError()
+ const;
+
+ private:
armarx::PlatformUnitInterfacePrx m_platform_unit;
VirtualRobot::RobotPtr m_robot;
- target m_target;
+ std::vector<Target> m_waypoints;
+ unsigned int m_current_waypoint_index = 0;
+ bool m_waypoint_changed = false;
- float m_pos_reached_threshold;
- float m_ori_reached_threshold;
+ Config m_cfg;
};