ObstacleAwarePlatformUnit.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::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);
 
    m_rot_pid_controller.reset();
 
    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(
    float max_pos_vel,
    float max_rot_vel,
    const Ice::Current&)
{
    // ensure positiveness
    max_pos_vel = std::max(max_pos_vel, 0.F);
    max_rot_vel = std::max(max_rot_vel, 0.F);
 
    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()};
 
//            m["max_rot_vel"] = new Variant{m_control_data.max_rot_vel};
//            m["max_lin_vel"] = new Variant{m_control_data.max_vel};
 
//            m["rot_desired"] = new Variant{m_control_data.target_ori};
//            m["rot_measured"] = new Variant{m_control_data.agent_ori};
 
//            m["pos_x_desired"] = new Variant{m_control_data.target_pos.x()};
//            m["pos_x_measured"] = new Variant{m_control_data.agent_pos.y()};
 
//            m["pos_y_desired"] = new Variant{m_control_data.target_pos.x()};
//            m["pos_y_measured"] = new Variant{m_control_data.agent_pos.y()};
 
            setDebugObserverChannel("ObstacleAwarePlatformCtrl", 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.agent_ori, m_control_data.target_ori);
        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::clamp(uncapped_target_rot_vel, -m_control_data.max_rot_vel, m_control_data.max_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_4))
        {
            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) * Eigen::Isometry3f(Eigen::Translation3f(0, -1000, 0)).matrix())
                       .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));
        }
 
        // TODO rotation arrow
    }
 
    // 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;
}