diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
index 954ad2ab8cd1e1f0148a929defb7f111d8a82746..39457c47b02e6b8618a23c0b828de04bbccee7ef 100644
--- a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
+++ b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
@@ -26,6 +26,7 @@
// STD/STL
#include <algorithm>
+#include <limits>
// Eigen
#include <Eigen/Geometry>
@@ -76,7 +77,6 @@ armarx::ObstacleAvoidingPlatformUnit::onExitPlatformUnit()
ARMARX_DEBUG << "Exiting " << getName() << ".";
schedule_high_level_control_loop(control_mode::none);
- stopPlatform();
ARMARX_DEBUG << "Exited " << getName() << ".";
}
@@ -117,10 +117,12 @@ armarx::ObstacleAvoidingPlatformUnit::move(
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 = target_rot_vel;
+ m_control_data.target_rot_vel = periodic_clamp<float>(target_rot_vel, -M_PI, M_PI);
ARMARX_CHECK(m_control_data.target_vel.allFinite());
ARMARX_CHECK(std::isfinite(m_control_data.target_rot_vel));
@@ -144,8 +146,7 @@ armarx::ObstacleAvoidingPlatformUnit::moveRelative(
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);
+ const float agent_ori = mat4f_to_rpy(m_robot->getGlobalPose()).z();
lock.unlock();
// Use moveTo.
@@ -175,8 +176,7 @@ armarx::ObstacleAvoidingPlatformUnit::setMaxVelocities(
void
armarx::ObstacleAvoidingPlatformUnit::stopPlatform(const Ice::Current&)
{
- m_platform->move(0, 0, 0);
- m_platform->stopPlatform();
+ schedule_high_level_control_loop(control_mode::none);
}
@@ -194,6 +194,7 @@ armarx::ObstacleAvoidingPlatformUnit::schedule_high_level_control_loop(control_m
// 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 " << control_mode_to_string(m_control_loop.mode) << " control.";
const bool join = true;
m_control_loop.task->stop(join);
}
@@ -201,10 +202,12 @@ armarx::ObstacleAvoidingPlatformUnit::schedule_high_level_control_loop(control_m
// 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 " << control_mode_to_string(mode) << " control.";
m_control_loop.mode = mode;
m_control_loop.task = new RunningTask<ObstacleAvoidingPlatformUnit>(
this,
@@ -221,12 +224,9 @@ armarx::ObstacleAvoidingPlatformUnit::high_level_control_loop()
CycleUtil cu{m_control_loop.cycle_time};
while (not m_control_loop.task->isStopped())
{
- if (target_reached())
- {
- break;
- }
-
- run_control_loop();
+ const auto& [vel, rot_vel] = get_velocities();
+ m_platform->move(vel.x(), vel.y(), rot_vel);
+ visualize(vel);
cu.waitForCycleDuration();
}
}
@@ -240,94 +240,147 @@ armarx::ObstacleAvoidingPlatformUnit::high_level_control_loop()
ARMARX_ERROR << "Unknown error occured while running control loop.";
}
- clean_up();
- stopPlatform();
+ m_platform->move(0, 0, 0);
+ m_platform->stopPlatform();
m_control_loop.mode = control_mode::none;
}
-void
-armarx::ObstacleAvoidingPlatformUnit::run_control_loop()
+std::tuple<Eigen::Vector2f, float>
+armarx::ObstacleAvoidingPlatformUnit::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 pos and ori.
+ // Update agent and get target velocities.
+ update_agent();
+ const Eigen::Vector2f target_vel = get_target_velocity();
+ const float target_rot_vel = get_target_rotational_velocity();
+
+ // 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};
+ const Eigen::Vector2f raw_modulated_vel =
+ m_obstacle_avoidance->modulateVelocity(m_agent).head<2>();
+ const Eigen::Vector2f modulated_vel =
+ /*post_process_vel(target_vel, */norm_max(raw_modulated_vel, m_control_data.max_vel)/*)*/;
+
+ ARMARX_CHECK(modulated_vel.allFinite()) << "Velocity contains non-finite values.";
+ ARMARX_CHECK(std::isfinite(target_rot_vel)) << "Rotation velocity is non-finite.";
+
+ const auto r = Eigen::Rotation2D(m_control_data.agent_ori).toRotationMatrix().inverse();
+ return {r * modulated_vel, target_rot_vel};
+}
+
+
+Eigen::Vector2f
+armarx::ObstacleAvoidingPlatformUnit::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::ObstacleAvoidingPlatformUnit::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::ObstacleAvoidingPlatformUnit::update_agent()
+{
+ 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);
- // Calculate target velocity and target angular velocity if in position control mode.
+ ARMARX_CHECK_GREATER(m_control_data.target_ori, -M_PI);
+ ARMARX_CHECK_LESS(m_control_data.target_ori, M_PI);
+ ARMARX_CHECK_GREATER(m_control_data.agent_ori, -M_PI);
+ ARMARX_CHECK_LESS(m_control_data.agent_ori, M_PI);
+
+ // In position control mode, also compute target distances.
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;
+ ARMARX_CHECK_GREATER(m_control_data.target_angular_dist, -M_PI);
+ ARMARX_CHECK_LESS(m_control_data.target_angular_dist, M_PI);
- // 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_DEBUG << "Distance to target: " << m_control_data.target_dist << " mm and "
+ << m_control_data.target_angular_dist << " rad.";
}
+}
- 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)
+bool
+armarx::ObstacleAvoidingPlatformUnit::target_position_reached()
+const
+{
+ if (m_control_loop.mode == control_mode::position)
{
- visualize(modulated_vel);
+ return m_control_data.target_dist < m_control_data.pos_reached_threshold;
}
- // 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);
+ // Cannot reach any target in non-position mode.
+ return false;
}
bool
-armarx::ObstacleAvoidingPlatformUnit::target_pos_reached()
+armarx::ObstacleAvoidingPlatformUnit::target_orientation_reached()
+const
{
if (m_control_loop.mode == control_mode::position)
{
- return m_control_data.target_dist < m_control_data.pos_reached_threshold;
+ return std::fabs(m_control_data.target_angular_dist) < m_control_data.ori_reached_threshold;
}
// Cannot reach any target in non-position mode.
@@ -337,23 +390,11 @@ armarx::ObstacleAvoidingPlatformUnit::target_pos_reached()
bool
armarx::ObstacleAvoidingPlatformUnit::target_reached()
+const
{
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 target_position_reached() and target_orientation_reached();
}
return false;
@@ -424,11 +465,17 @@ const
const float max_buffer_size_dist = 1500;
const float max_buffer_size = m_control_data.amount_smoothing;
+ // In velocity control mode, don't smooth that much.
+ if (m_control_loop.mode == control_mode::velocity)
+ {
+ return min_buffer_size;
+ }
+
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);
+ (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.
@@ -476,123 +523,135 @@ const
void
-armarx::ObstacleAvoidingPlatformUnit::clean_up()
+armarx::ObstacleAvoidingPlatformUnit::visualize(const Eigen::Vector2f& local_modulated_vel_2d)
{
- // Clear buffers.
- m_control_data.vel_history.clear();
- m_viz.path.clear();
+ using namespace armarx::viz;
- // Clear ArViz.
- for (const std::string& layer_name : {"progress", "velocities", "agent"})
+ if (not m_viz.enabled)
{
- viz::Layer layer = arviz.layer(layer_name);
- arviz.commit(layer);
+ return;
}
-}
-
-
-void
-armarx::ObstacleAvoidingPlatformUnit::visualize(const Eigen::Vector2f& modulated_vel_2d)
-{
- using namespace armarx::viz;
+ const auto r = Eigen::Rotation2D(m_control_data.agent_ori).toRotationMatrix();
+ const Eigen::Vector2f modulated_vel_2d = r * local_modulated_vel_2d;
const Eigen::Vector3f modulated_vel{modulated_vel_2d.x(), modulated_vel_2d.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;
+
+ // If no start is set, use current agent pos.
+ if (not m_viz.start.allFinite())
+ {
+ m_viz.start = m_agent.pos;
+ }
+
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)
+ if (not target_reached())
{
- Layer layer = arviz.layer("progress");
-
// Start.
- layer.add(Sphere{"start"}
- .position(m_viz.start)
- .color(Color::cyan(255, 64))
- .radius(40));
+ 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)
{
- layer.add(Sphere{"path_" + std::to_string(i + 1)}
- .position(m_viz.path[i])
- .color(Color::magenta(255, 128))
- .radius(20));
+ 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};
- 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);
+ 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();
+ m_viz.start = Eigen::Vector3f{std::numeric_limits<float>::infinity(),
+ std::numeric_limits<float>::infinity(),
+ std::numeric_limits<float>::infinity()};
}
}
// Velocities.
+ Layer l_vels = arviz.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));
+ l_vels.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));
+ l_vels.add(Arrow{"modulated"}
+ .fromTo(from2, from2 + modulated_vel * 5)
+ .color(Color::cyan(255, 128))
+ .width(25));
}
-
- arviz.commit(layer);
}
// Agent.
+ Layer l_agnt = arviz.layer("agent");
{
- Layer layer = arviz.layer("agent");
-
- layer.add(Sphere{"agent"}
- .position(m_agent.pos)
- .color(Color::red(255, 128))
- .radius(50));
+ l_agnt.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));
+ l_agnt.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({l_prog, l_vels, l_agnt});
+}
+
- arviz.commit(layer);
+std::string
+armarx::ObstacleAvoidingPlatformUnit::control_mode_to_string(control_mode mode)
+{
+ switch (mode)
+ {
+ case control_mode::position:
+ return "position";
+ case control_mode::velocity:
+ return "velocity";
+ case control_mode::none:
+ default:
+ return "none";
}
}
diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h
index e8ed4cb3f7be2fcf47f10e4648b2a4e92ccbd356..3ba35451a3555c52a8a0207025e359c8b19399f5 100644
--- a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h
+++ b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h
@@ -27,6 +27,7 @@
// STD/STL
#include <deque>
#include <string>
+#include <tuple>
#include <mutex>
#include <vector>
@@ -192,14 +193,31 @@ namespace armarx
void
high_level_control_loop();
+ std::tuple<Eigen::Vector2f, float>
+ get_velocities();
+
void
- run_control_loop();
+ update_agent();
+
+ Eigen::Vector2f
+ get_target_velocity()
+ const;
+
+ float
+ get_target_rotational_velocity()
+ const;
bool
- target_pos_reached();
+ target_position_reached()
+ const;
bool
- target_reached();
+ target_orientation_reached()
+ const;
+
+ bool
+ target_reached()
+ const;
Eigen::Vector2f
post_process_vel(const Eigen::Vector2f& target_vel, const Eigen::Vector2f& modulated_vel);
@@ -219,8 +237,8 @@ namespace armarx
void
visualize(const Eigen::Vector2f& modulated_vel);
- void
- clean_up();
+ std::string
+ control_mode_to_string(control_mode);
public:
@@ -235,7 +253,7 @@ namespace armarx
ObstacleAvoidingPlatformUnit::control_loop m_control_loop;
ObstacleAvoidingPlatformUnit::control_data m_control_data;
- PIDController m_rot_pid_controller{1.0, 0.00009, 0.0};
+ mutable PIDController m_rot_pid_controller{1.0, 0.00009, 0.0};
obstacleavoidance::Agent m_agent;