diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp
index 03ed255120ccf8a5084ea824f7d8e2d790c1c485..595bdf0ddd20cd91ee5452ea168fcb670821a17a 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,113 @@ 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;
+ }
+
+ if (m_waypoint_changed)
+ {
+ // Get thresholds depending on waypoint or final target (last waypoint).
+ 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 +175,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;
};