ObstacleAvoidingPlatformUnit.cpp

Go to the documentation of this file.

/*
 * 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>
#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::ObstacleAvoidingPlatformUnit::control_mode mode)
    {
        switch (mode)
        {
            case armarx::ObstacleAvoidingPlatformUnit::control_mode::position:
                return "position";
            case armarx::ObstacleAvoidingPlatformUnit::control_mode::velocity:
                return "velocity";
            case armarx::ObstacleAvoidingPlatformUnit::control_mode::none:
            default:
                return "unset";
        }
    }
 
}
 
 
const std::string
armarx::ObstacleAvoidingPlatformUnit::default_name = "ObstacleAvoidingPlatformUnit";
 
 
armarx::ObstacleAvoidingPlatformUnit::ObstacleAvoidingPlatformUnit() = default;
 
 
armarx::ObstacleAvoidingPlatformUnit::~ObstacleAvoidingPlatformUnit() = default;
 
 
void
armarx::ObstacleAvoidingPlatformUnit::onInitPlatformUnit()
{
    ARMARX_DEBUG << "Initializing " << getName() << ".";
 
    ARMARX_DEBUG << "Initialized " << getName() << ".";
}
 
 
void
armarx::ObstacleAvoidingPlatformUnit::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);
    invalidate(m_control_data.vel_history);
 
    ARMARX_DEBUG << "Started " << getName() << ".";
}
 
 
void
armarx::ObstacleAvoidingPlatformUnit::onExitPlatformUnit()
{
    ARMARX_DEBUG << "Exiting " << getName() << ".";
 
    schedule_high_level_control_loop(control_mode::none);
 
    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&)
{
    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;
 
    // 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);
 
    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&)
{
    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::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 = 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::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&)
{
    schedule_high_level_control_loop(control_mode::none);
    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)
    {
        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<ObstacleAvoidingPlatformUnit>(
        this,
        &ObstacleAvoidingPlatformUnit::high_level_control_loop);
    m_control_loop.task->start();
}
 
 
void
armarx::ObstacleAvoidingPlatformUnit::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.";
    }
 
    // 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;
 
    visualize();
 
    ARMARX_VERBOSE << "Stopped " << ::to_string(mode) << " control.";
}
 
 
armarx::ObstacleAvoidingPlatformUnit::velocities
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 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();
 
    // Apply obstacle avoidance and apply post processing to get the modulated velocity.
    const Eigen::Vector2f modulated_vel = [this, &target_vel]
    {
        obstacleavoidance::Agent agent;
        agent.safety_margin = m_control_data.agent_safety_margin;
        agent.pos = Eigen::Vector3f{m_control_data.agent_pos.x(), m_control_data.agent_pos.y(), 0};
        agent.desired_vel = Eigen::Vector3f{target_vel.x(), target_vel.y(), 0};
 
        const Eigen::Vector2f raw = m_obstacle_avoidance->modulateVelocity(agent).head<2>();
        return post_process_vel(target_vel, norm_max(raw, 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.";
 
    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::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_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::ObstacleAvoidingPlatformUnit::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::ObstacleAvoidingPlatformUnit::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::ObstacleAvoidingPlatformUnit::target_reached()
const
{
    if (m_control_loop.mode == control_mode::position)
    {
        return target_position_reached() and target_orientation_reached();
    }
 
    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();
 
    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.
    // 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_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_4))
        {
            return true;
        }
    }
 
    return false;
}
 
 
Eigen::Vector2f
armarx::ObstacleAvoidingPlatformUnit::calculate_mean_modulated_vel()
const
{
    const unsigned adaptive_buffer_size = calculate_adaptive_smoothing_buffer_size();
    const unsigned elements =
        std::min<unsigned>(m_control_data.vel_history.size(), adaptive_buffer_size);
    auto end = m_control_data.vel_history.begin();
    std::advance(end, elements);
 
    const auto transform =
        [elements](
            const Eigen::Vector2f & vel,
            const std::tuple<Eigen::Vector2f, Eigen::Vector2f>& vels)
        -> Eigen::Vector2f
    {
        return vel + std::get<1>(vels) * (1. / elements);
    };
 
    return std::accumulate(m_control_data.vel_history.begin(),
                           end,
                           Eigen::Vector2f{0, 0},
                           transform);
}
 
 
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);
 
    if (m_control_loop.mode == control_mode::position)
    {
        // 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));
    }
    else
    {
        // In velocity control mode, don't smooth that much.
        return min_buffer_size;
    }
}
 
 
float
armarx::ObstacleAvoidingPlatformUnit::calculate_adaptive_max_vel()
const
{
    using namespace simox::math;
 
    if (m_control_loop.mode == control_mode::position and m_control_data.adaptive_max_vel_exp > 0)
    {
        std::vector<float> angular_similarities;
 
        const unsigned elements =
            std::min<unsigned>(m_control_data.vel_history.size(), m_control_data.amount_max_vel);
        auto end = m_control_data.vel_history.begin();
        std::advance(end, elements);
 
        const auto transform =
            [elements](const std::tuple<Eigen::Vector2f, Eigen::Vector2f>& vels) -> float
        {
            const auto& [desired_vel, modulated_vel] = vels;
 
            if (desired_vel.isZero() and modulated_vel.isZero())
            {
                return 1;
            }
            else if (desired_vel.isZero() xor modulated_vel.isZero())
            {
                return -1;
            }
 
            return angular_similarity(desired_vel, modulated_vel) / elements;
        };
 
        // TODO: GCC 9+: Use transform_reduce instead of transform + accumulate.
        //        const float mean_angular_similarity = std::transform_reduce(
        //                m_control_data.vel_history.begin(), end,
        //                0,
        //                std::plus<float>(),
        //                transform);
 
        std::transform(m_control_data.vel_history.begin(),
                       end,
                       std::back_inserter(angular_similarities),
                       transform);
 
        const float mean_angular_similarity = std::accumulate(angular_similarities.begin(),
                                              angular_similarities.end(),
                                              0.f,
                                              std::plus<float>());
        const float max_vel_factor = std::pow(mean_angular_similarity,
                                              m_control_data.adaptive_max_vel_exp);
 
        return max_vel_factor * m_control_data.max_vel;
    }
    else
    {
        return m_control_data.max_vel;
    }
}
 
 
void
armarx::ObstacleAvoidingPlatformUnit::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::ObstacleAvoidingPlatformUnit::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;
 
        // 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::ObstacleAvoidingPlatformUnit::createPropertyDefinitions()
{
    PropertyDefinitionsPtr def = PlatformUnit::createPropertyDefinitions();
 
    def->component(m_platform, "Platform");
    def->component(m_obstacle_avoidance, "PlatformObstacleAvoidance");
 
    // Settings.
    def->optional(m_control_data.adaptive_max_vel_exp, "adaptive_max_vel_exponent",
                  "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");
    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;
}