DSObstacleAvoidance.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::DSObstacleAvoidance
 * @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/DSObstacleAvoidance/DSObstacleAvoidance.h>
 
 
// STD/STL
#include <cmath>
#include <fstream>
#include <limits>
#include <map>
#include <memory>
#include <mutex>
#include <string>
 
// Simox
#include <SimoxUtility/json.h>
#include <SimoxUtility/math/convert.h>
#include <VirtualRobot/Robot.h>
 
// Ice
#include <Ice/Current.h>
 
// DynamicObstacleAvoidance
#include <DynamicObstacleAvoidance/Modulation.hpp>
#include <DynamicObstacleAvoidance/Obstacle/Ellipsoid.hpp>
#include <DynamicObstacleAvoidance/Utils/Plotting/PlottingTools.hpp>
namespace doa = DynamicObstacleAvoidance;
 
// ArmarX
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
// RobotAPI
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
namespace
{
 
    std::shared_ptr<doa::Obstacle>
    create_doa_obstacle(const armarx::obstacledetection::Obstacle& obstacle)
    {
        Eigen::Vector3d pos(obstacle.posX, obstacle.posY, obstacle.posZ);
        Eigen::Quaterniond ori{Eigen::AngleAxisd(obstacle.yaw, Eigen::Vector3d::UnitZ())};
 
        std::shared_ptr<doa::Ellipsoid> obstacle_ptr =
            std::make_shared<doa::Ellipsoid>(obstacle.name,
                                             doa::State(pos / 1000, ori),
                                             Eigen::Array3d(obstacle.safetyMarginX / 1000,
                                                            obstacle.safetyMarginY / 1000,
                                                            obstacle.safetyMarginZ / 1000));
 
        obstacle_ptr->set_axis_lengths(
            Eigen::Array3d(obstacle.axisLengthX, obstacle.axisLengthY, obstacle.axisLengthZ) /
            1000);
        obstacle_ptr->set_reference_position(
            Eigen::Vector3d(obstacle.refPosX, obstacle.refPosY, obstacle.refPosZ) / 1000);
        obstacle_ptr->set_curvature_factor(
            Eigen::Array3d(obstacle.curvatureX, obstacle.curvatureY, obstacle.curvatureZ));
 
        return obstacle_ptr;
    }
 
} // namespace
 
namespace armarx::obstacledetection
{
 
    void
    from_json(const nlohmann::json& j, armarx::obstacledetection::Obstacle& obstacle)
    {
        j.at("name").get_to(obstacle.name);
        j.at("posX").get_to(obstacle.posX);
        j.at("posY").get_to(obstacle.posY);
        if (j.find("posZ") != j.end())
        {
            j.at("posZ").get_to(obstacle.posZ);
        }
        j.at("yaw").get_to(obstacle.yaw);
        j.at("axisLengthX").get_to(obstacle.axisLengthX);
        j.at("axisLengthY").get_to(obstacle.axisLengthY);
        if (j.find("axisLengthZ") != j.end())
        {
            j.at("axisLengthZ").get_to(obstacle.axisLengthZ);
        }
        j.at("refPosX").get_to(obstacle.refPosX);
        j.at("refPosY").get_to(obstacle.refPosY);
        if (j.find("refPosZ") != j.end())
        {
            j.at("refPosZ").get_to(obstacle.refPosZ);
        }
        if (j.find("safetyMarginX") != j.end())
        {
            j.at("safetyMarginX").get_to(obstacle.safetyMarginX);
        }
        if (j.find("safetyMarginY") != j.end())
        {
            j.at("safetyMarginY").get_to(obstacle.safetyMarginY);
        }
        if (j.find("safetyMarginZ") != j.end())
        {
            j.at("safetyMarginZ").get_to(obstacle.safetyMarginZ);
        }
        if (j.find("curvatureX") != j.end())
        {
            j.at("curvatureX").get_to(obstacle.curvatureX);
        }
        if (j.find("curvatureY") != j.end())
        {
            j.at("curvatureY").get_to(obstacle.curvatureY);
        }
        if (j.find("curvatureZ") != j.end())
        {
            j.at("curvatureZ").get_to(obstacle.curvatureZ);
        }
    }
 
} // namespace armarx::obstacledetection
 
const std::string armarx::DSObstacleAvoidance::default_name = "DSObstacleAvoidance";
 
armarx::DSObstacleAvoidance::DSObstacleAvoidance()
{
    // DOA config.
    m_doa.cfg.only_2d = true;
    m_doa.cfg.aggregated = true;
    m_doa.cfg.agent_safety_margin = 0;
    m_doa.cfg.local_modulation = false;
    m_doa.cfg.repulsion = true;
    m_doa.cfg.planar_modulation = false;
    m_doa.cfg.critical_distance = 1.0;
    m_doa.cfg.weight_power = 2.0;
}
 
void
armarx::DSObstacleAvoidance::onInitComponent()
{
    ARMARX_DEBUG << "Initializing " << getName() << ".";
 
    m_buf.needs_env_update = false;
 
    m_doa.env.set_aggregated(m_doa.cfg.aggregated);
 
    if (not m_doa.load_obstacles_from_file.empty())
    {
        const std::filesystem::path scene_obstacles_path =
            ArmarXDataPath::getAbsolutePath(m_doa.load_obstacles_from_file);
        std::ifstream ifs(scene_obstacles_path);
        nlohmann::json j = nlohmann::json::parse(ifs);
 
        std::vector<obstacledetection::Obstacle> obstacles = j;
        for (obstacledetection::Obstacle& obstacle : obstacles)
        {
            if (m_doa.cfg.only_2d)
            {
                obstacle.posZ = 0;
                obstacle.axisLengthZ = 0;
                obstacle.refPosZ = 0;
                obstacle.safetyMarginZ = 0;
                obstacle.curvatureZ = 1;
            }
 
            sanity_check_obstacle(obstacle);
 
            m_doa.env.add_obstacle(::create_doa_obstacle(obstacle));
        }
 
        m_doa.env.update();
    }
 
    writeDebugPlots("doa_upstart");
 
    ARMARX_DEBUG << "Initialized " << getName() << ".";
}
 
void
armarx::DSObstacleAvoidance::onConnectComponent()
{
    ARMARX_DEBUG << "Connecting " << getName() << ".";
 
    if (m_vis.enabled)
    {
        m_vis.task = new PeriodicTask<DSObstacleAvoidance>(
            this, &DSObstacleAvoidance::run_visualization, m_vis.task_interval);
        m_vis.task->start();
    }
 
    if (m_watchdog.enabled)
    {
        m_watchdog.task = new PeriodicTask<DSObstacleAvoidance>(
            this, &DSObstacleAvoidance::run_watchdog, m_watchdog.task_interval);
        m_watchdog.task->start();
    }
 
    ARMARX_DEBUG << "Connected " << getName() << ".";
}
 
void
armarx::DSObstacleAvoidance::onDisconnectComponent()
{
    ARMARX_DEBUG << "Disconnecting " << getName() << ".";
 
    if (m_vis.enabled)
    {
        m_vis.task->stop();
    }
 
    if (m_watchdog.enabled)
    {
        m_watchdog.task->stop();
    }
 
    ARMARX_DEBUG << "Disconnected " << getName() << ".";
}
 
void
armarx::DSObstacleAvoidance::onExitComponent()
{
    ARMARX_DEBUG << "Exiting " << getName() << ".";
 
    ARMARX_DEBUG << "Exited " << getName() << ".";
}
 
std::string
armarx::DSObstacleAvoidance::getDefaultName() const
{
    return default_name;
}
 
armarx::dsobstacleavoidance::Config
armarx::DSObstacleAvoidance::getConfig(const Ice::Current&)
{
    return m_doa.cfg;
}
 
void
armarx::DSObstacleAvoidance::updateObstacle(const armarx::obstacledetection::Obstacle& obstacle,
                                            const Ice::Current&)
{
    ARMARX_DEBUG << "Adding/updating obstacle: " << obstacle.name << ".";
 
    std::unique_lock l{m_buf.mutex};
 
    sanity_check_obstacle(obstacle);
 
    buffer::update update;
    update.time = IceUtil::Time::now();
    update.name = obstacle.name;
    update.type = buffer::update_type::update;
 
    m_buf.update_log.push_back(update);
    m_buf.obstacles[obstacle.name] = obstacle;
    m_buf.needs_env_update = true;
 
    ARMARX_DEBUG << "Added/updated obstacle: " << obstacle.name << ".";
}
 
void
armarx::DSObstacleAvoidance::removeObstacle(const std::string& obstacle_name, const Ice::Current&)
{
    ARMARX_DEBUG << "Removing obstacle: " << obstacle_name << ".";
 
    std::unique_lock l{m_buf.mutex};
 
    buffer::update update;
    update.time = IceUtil::Time::now();
    update.name = obstacle_name;
    update.type = buffer::update_type::removal;
 
    m_buf.update_log.push_back(update);
    m_buf.needs_env_update = true;
 
    ARMARX_DEBUG << "Removed obstacle: " << obstacle_name << ".";
}
 
std::vector<armarx::obstacledetection::Obstacle>
armarx::DSObstacleAvoidance::getObstacles(const Ice::Current&)
{
    ARMARX_DEBUG << "Get Obstacles";
    std::shared_lock l{m_doa.mutex};
 
    std::vector<obstacledetection::Obstacle> obstacle_list;
 
    for (auto& [doa_name, doa_obstacle] : m_doa.env)
    {
        ARMARX_DEBUG << "Got an obtascle";
        std::shared_ptr<doa::Ellipsoid> doa_ellipsoid =
            std::dynamic_pointer_cast<doa::Ellipsoid>(doa_obstacle);
        obstacledetection::Obstacle obstacle;
 
        obstacle.name = doa_name;
        obstacle.posX = doa_ellipsoid->get_position()(0) * 1000;
        obstacle.posY = doa_ellipsoid->get_position()(1) * 1000;
        obstacle.posZ = doa_ellipsoid->get_position()(2) * 1000;
        obstacle.yaw = simox::math::mat3f_to_rpy(
                           doa_ellipsoid->get_orientation().toRotationMatrix().cast<float>())
                           .z();
        obstacle.axisLengthX = doa_ellipsoid->get_axis_lengths(0) * 1000;
        obstacle.axisLengthY = doa_ellipsoid->get_axis_lengths(1) * 1000;
        obstacle.axisLengthZ = doa_ellipsoid->get_axis_lengths(2) * 1000;
        obstacle.refPosX = doa_ellipsoid->get_reference_position()(0) * 1000;
        obstacle.refPosY = doa_ellipsoid->get_reference_position()(1) * 1000;
        obstacle.refPosZ = doa_ellipsoid->get_reference_position()(2) * 1000;
        obstacle.safetyMarginX = doa_ellipsoid->get_safety_margin()(0) * 1000;
        obstacle.safetyMarginY = doa_ellipsoid->get_safety_margin()(1) * 1000;
        obstacle.safetyMarginZ = doa_ellipsoid->get_safety_margin()(2) * 1000;
        obstacle.curvatureX = doa_ellipsoid->get_curvature_factor()(0);
        obstacle.curvatureY = doa_ellipsoid->get_curvature_factor()(1);
        obstacle.curvatureZ = doa_ellipsoid->get_curvature_factor()(2);
 
        ARMARX_DEBUG << "push back " << obstacle.name;
        obstacle_list.push_back(std::move(obstacle));
    }
 
    ARMARX_DEBUG << "Return obstacle list";
    return obstacle_list;
}
 
Eigen::Vector3f
armarx::DSObstacleAvoidance::modulateVelocity(const obstacleavoidance::Agent& agent,
                                              const Ice::Current&)
{
 
    ARMARX_DEBUG << "Modulate velocity";
    std::shared_lock l{m_doa.mutex};
 
    // Create and initialize agent.
    doa::Agent doa_agent{agent.safety_margin};
    {
        Eigen::Vector3d agent_global_position = agent.pos.cast<double>() / 1000;
        Eigen::Vector3d agent_velocity = agent.desired_vel.cast<double>() / 1000;
 
        if (m_doa.cfg.only_2d)
        {
            agent_global_position(2) = 0;
            agent_velocity(2) = 0;
        }
 
        doa_agent.set_position(agent_global_position);
        doa_agent.set_linear_velocity(agent_velocity);
    }
 
    // Modulate velocity given the agent, the environment, and additional parameters.
    Eigen::Vector3d modulated_vel = doa::Modulation::modulate_velocity(doa_agent,
                                                                       m_doa.env,
                                                                       m_doa.cfg.local_modulation,
                                                                       m_doa.cfg.repulsion,
                                                                       m_doa.cfg.planar_modulation,
                                                                       m_doa.cfg.critical_distance,
                                                                       m_doa.cfg.weight_power);
 
    // Convert output back to mm.
    modulated_vel *= 1000;
 
    // Set Z-component to 0 in 2D mode.
    if (m_doa.cfg.only_2d)
    {
        modulated_vel(2) = 0;
    }
 
    ARMARX_DEBUG << deactivateSpam(0.3) << "Target vel:  " << modulated_vel;
 
    return modulated_vel.cast<float>();
}
 
void
armarx::DSObstacleAvoidance::updateEnvironment(const Ice::Current&)
{
    ARMARX_VERBOSE << "Updating environment.";
    std::lock_guard l2{m_vis.mutex};
    std::lock_guard l{m_doa.mutex};
 
    // Make working copies of the updates and free them again.
    std::vector<buffer::update> update_log;
    std::map<std::string, obstacledetection::Obstacle> obstacles;
    {
        std::lock_guard l{m_buf.mutex};
 
        if (not m_buf.needs_env_update)
        {
            ARMARX_DEBUG << "Nothing to do.";
            return;
        }
 
        update_log = m_buf.update_log;
        obstacles = m_buf.obstacles;
        m_buf.update_log.clear();
        m_buf.obstacles.clear();
        m_buf.needs_env_update = false;
        m_buf.last_env_update = IceUtil::Time::now();
    }
 
    // Update environment.
    armarx::core::time::StopWatch sw;
    for (const buffer::update& update : update_log)
    {
        doa::Environment::iterator it = m_doa.env.find(update.name);
 
        switch (update.type)
        {
            case buffer::update_type::removal:
            {
                ARMARX_DEBUG << "Removing obstacle " << update.name << ".";
                if (it != m_doa.env.end())
                {
                    m_doa.env.erase(update.name);
                }
                else
                {
                    ARMARX_WARNING << "Tried to remove obstacle " << update.name << ", but this "
                                   << "obstacle is unknown to the environment.";
                }
            }
            break;
            case buffer::update_type::update:
            {
                if (it != m_doa.env.end())
                {
                    ARMARX_DEBUG << "Updating obstacle " << update.name << " with new values.";
                    m_doa.env.erase(update.name);
                }
                else
                {
                    ARMARX_DEBUG << "Adding obstacle " << update.name << ".";
                }
 
                const obstacledetection::Obstacle& obstacle = obstacles.at(update.name);
                m_doa.env.add_obstacle(::create_doa_obstacle(obstacle));
            }
            break;
        }
    }
    ARMARX_DEBUG << "Calling update on environment now.";
    m_doa.env.update();
 
    {
        m_vis.needs_update = true;
    }
 
    ARMARX_VERBOSE << "Updating environment with " << update_log.size() << " updates took "
                   << sw.stop() << ".";
}
 
void
armarx::DSObstacleAvoidance::writeDebugPlots(const std::string& filename, const Ice::Current&)
{
    ARMARX_DEBUG << "Write debug plots";
    std::shared_lock l{m_doa.mutex};
 
    const std::string filename_annotated = getName() + "_" + filename;
    ARMARX_DEBUG << "Writing debug plots to `/tmp/" << filename_annotated << ".png`.";
 
    const bool show = false;
    try
    {
        doa::PlottingTools::plot_configuration(
            m_doa.env.get_obstacle_list(), filename_annotated, show, not m_doa.cfg.only_2d);
    }
    catch (...)
    {
        ARMARX_WARNING << "Tried to write debug plots but failed, probably because "
                       << "`python3-matplotlib` are not installed.";
    }
}
 
void
armarx::DSObstacleAvoidance::sanity_check_obstacle(
    const armarx::obstacledetection::Obstacle& obstacle) const
{
    ARMARX_DEBUG << "Sanity checking obstacle `" << obstacle.name << "`.";
 
    const std::string curvature_error = "Curvature must be greater than or equal to 1.";
    const std::string z_comps_set_error = "2D mode activated. Z-component values must not be set.";
 
    ARMARX_CHECK_GREATER_EQUAL(obstacle.curvatureX, 1) << curvature_error;
    ARMARX_CHECK_GREATER_EQUAL(obstacle.curvatureY, 1) << curvature_error;
    ARMARX_CHECK_GREATER_EQUAL(obstacle.curvatureZ, 1) << curvature_error;
 
    if (m_doa.cfg.only_2d)
    {
        ARMARX_DEBUG << "Additionally sanity checking obstacle `" << obstacle.name
                     << "` for 2D compliance.";
 
        ARMARX_CHECK_EQUAL(obstacle.posZ, 0) << z_comps_set_error;
        ARMARX_CHECK_EQUAL(obstacle.refPosZ, 0) << z_comps_set_error;
        ARMARX_CHECK_EQUAL(obstacle.axisLengthZ, 0) << z_comps_set_error;
        ARMARX_CHECK_EQUAL(obstacle.safetyMarginZ, 0) << z_comps_set_error;
    }
}
 
void
armarx::DSObstacleAvoidance::run_visualization()
{
    //ARMARX_DEBUG << "Run visualization";
    using namespace armarx::viz;
 
    std::lock_guard l{m_vis.mutex};
    if (not m_vis.needs_update)
    {
        return;
    }
 
    Layer layer_obs = arviz.layer("obstacles");
    Layer layer_sms = arviz.layer("safety_margins");
    Layer layer_rps = arviz.layer("reference_points");
    Layer layer_bbs = arviz.layer("bounding_boxes");
 
    for (const obstacledetection::Obstacle& obstacle : getObstacles())
    {
        Color color = Color::orange(255, 128);
        Color color_m = Color::orange(255, 64);
 
        if (obstacle.name == "human")
        {
            color = Color::red(255, 128);
            color_m = Color::red(255, 64);
        }
        else if (not m_doa.cfg.only_2d)
        {
            color = Color::blue(255, 128);
            color_m = Color::blue(255, 64);
        }
 
        const double safetyMarginZ = m_doa.cfg.only_2d ? 1 : obstacle.safetyMarginZ;
        const double posZ = m_doa.cfg.only_2d ? 1 : obstacle.posZ;
        const double refPosZ = m_doa.cfg.only_2d ? 1 : obstacle.refPosZ;
        const double axisLengthZ = m_doa.cfg.only_2d ? 1 : obstacle.axisLengthZ;
 
        const Eigen::Matrix4f pose =
            simox::math::pos_rpy_to_mat4f(obstacle.posX, obstacle.posY, posZ, 0, 0, obstacle.yaw);
        const Eigen::Vector3f dim(obstacle.axisLengthX, obstacle.axisLengthY, axisLengthZ);
        const Eigen::Vector3f sm(obstacle.safetyMarginX, obstacle.safetyMarginY, safetyMarginZ);
        const Eigen::Vector3f curv(obstacle.curvatureX, obstacle.curvatureY, obstacle.curvatureZ);
 
        if (m_doa.cfg.only_2d)
        {
 
            layer_obs.add(Cylindroid{obstacle.name}
                              .pose(pose)
                              .axisLengths(dim.head<2>())
                              .height(dim(2))
                              .curvature(curv.head<2>())
                              .color(color));
 
            layer_sms.add(Cylindroid{obstacle.name + "_sm"}
                              .pose(pose)
                              .axisLengths((dim + sm).head<2>())
                              .height(dim(2) + sm(2))
                              .curvature(curv.head<2>())
                              .color(color_m));
        }
        else
        {
            layer_obs.add(
                Ellipsoid{obstacle.name}.pose(pose).axisLengths(dim).curvature(curv).color(color));
 
            layer_sms.add(Ellipsoid{obstacle.name + "_sm"}
                              .pose(pose)
                              .axisLengths(dim + sm)
                              .curvature(curv)
                              .color(color_m));
        }
 
        layer_rps.add(Sphere{obstacle.name + "_rp"}
                          .position(Eigen::Vector3f(obstacle.refPosX, obstacle.refPosY, refPosZ))
                          .radius(35)
                          .color(Color::green()));
 
        layer_bbs.add(Box{obstacle.name + "_bb"}.pose(pose).size(dim).color(color));
    }
 
    m_vis.needs_update = false;
 
    arviz.commit({layer_obs, layer_sms, layer_rps});
    //arviz.commit(layer_bbs);  // Do not render bounding boxes by default.
}
 
void
armarx::DSObstacleAvoidance::run_watchdog()
{
    bool needs_env_update;
    bool was_recently_updated;
    {
        std::unique_lock l{m_buf.mutex};
        needs_env_update = m_buf.needs_env_update;
        was_recently_updated = IceUtil::Time::now() - m_buf.last_env_update <= m_watchdog.threshold;
    }
 
    if (needs_env_update and not was_recently_updated)
    {
        ARMARX_WARNING << "Environment has not been updated for over " << m_watchdog.threshold
                       << ".  Have you forgotten to call "
                       << "`updateEnvironment()` after adding, updating or removing obstacles?  "
                       << "Forcing update now.";
 
        updateEnvironment();
    }
}
 
armarx::PropertyDefinitionsPtr
armarx::DSObstacleAvoidance::createPropertyDefinitions()
{
    PropertyDefinitionsPtr def{new ComponentPropertyDefinitions{getConfigIdentifier()}};
 
    def->optional(m_vis.enabled, "visualize", "Enable/disable visualization.");
    def->optional(m_watchdog.enabled, "udpate_watchdog", "Run environment update watchdog.");
    def->optional(m_doa.load_obstacles_from_file,
                  "load_obstacles_from",
                  "Path to JSON file to load initial obstacles from.");
 
    // "doa" namespace for properties that directly configure the library.
    def->optional(m_doa.cfg.only_2d, "doa.only_2d", "Only consider 2D.");
    def->optional(m_doa.cfg.aggregated, "doa.aggregated", "Aggregated environment.");
    def->optional(m_doa.cfg.agent_safety_margin, "doa.agent_safety_margin", "Agent safety margin.");
    def->optional(m_doa.cfg.local_modulation, "doa.local_modulation", "Local modulation on/off.");
    def->optional(m_doa.cfg.repulsion, "doa.repulsion", "Repulsion on/off.");
    def->optional(
        m_doa.cfg.planar_modulation, "doa.planar_modulation", "Planar modulation on/off.");
    def->optional(m_doa.cfg.critical_distance, "doa.critical_distance", "Critical distance.");
    def->optional(m_doa.cfg.weight_power, "doa.weight_power", "Weight power");
 
    return def;
}