SimulatedHuman.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/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2022
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "SimulatedHuman.h"
 
#include <algorithm>
#include <optional>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h>
#include <armarx/navigation/algorithms/util.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/StaticScene.h>
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/global_planning/SPFA.h>
#include <armarx/navigation/human/types.h>
 
namespace armarx::navigation::human::simulation
{
 
 
    human::Human
    SimulatedHuman::update()
    {
        switch (state_)
        {
            case State::Idle:
                ARMARX_DEBUG << "State:Idle";
                initialize();
                state_ = State::Walking;
                break;
            case State::Walking:
            {
                ARMARX_DEBUG << "State:Walking";
 
                step();
 
                if (goalReached())
                {
                    ARMARX_INFO << "Human reached goal";
                    state_ = State::GoalReached;
                }
 
                break;
            }
            case State::GoalReached:
                ARMARX_DEBUG << "State:GoalReached";
 
                // TODO(fabian.reister): simulate "perform action at goal": wait a while until movement is started again.
                state_ = State::Idle;
                break;
        }
 
        return human_;
    }
 
    bool
    SimulatedHuman::goalReached() const
    {
        const Eigen::Vector2f goal =
            conv::to2D(globalTrajectory_.points().back().waypoint.pose.translation());
 
        return ((goal - human_.pose.translation()).norm() < params_.goalDistanceThreshold);
    }
 
    void
    SimulatedHuman::reinitialize(const core::Pose2D& pose)
    {
        human_.pose = pose;
 
        if (goalReached())
        {
            ARMARX_VERBOSE << "Goal reached";
            state_ = State::Idle;
        }
 
        initialize();
        update();
    }
 
    void
    SimulatedHuman::initialize()
    {
        global_planning::SPFA::Params params;
        params.linearVelocity = this->params_.maxLinearVelocity;
        params.algo.obstacleDistanceWeight = 1.1F;
 
        core::Scene scene;
 
        scene.staticScene.emplace(core::StaticScene{nullptr, {}, {}, std::nullopt, {}});
        scene.staticScene->distanceToObstaclesCostmap.emplace(distanceField_);
 
        global_planning::SPFA planner(params, scene);
 
        const core::Pose2D start = human_.pose;
 
        if (state_ == State::Idle) // human reached goal and needs a new one
        {
            while (true)
            {
                const auto sampledPose = algorithms::sampleValidPositionInMap(distanceField_);
                ARMARX_CHECK(sampledPose.has_value());
                const core::Pose2D& goal = sampledPose.value();
 
 
                const auto plan = planner.plan(conv::to3D(start), conv::to3D(goal));
 
                // check if plan could be created. otherwise try another goal
                if (plan.has_value())
                {
                    globalTrajectory_ = plan->trajectory;
 
                    human_ = human::Human{.pose = start,
                                          .linearVelocity = Eigen::Vector2f::Zero(),
                                          .detectionTime = Clock::Now()};
                    return;
                }
            }
        }
        else if (state_ == State::Walking) // human is walking and replans to existing goal
        {
            const auto goal = conv::to2D(globalTrajectory_.poses().back());
            const auto plan = planner.plan(conv::to3D(start), conv::to3D(goal));
 
            // check if plan could be created. otherwise try another goal
            if (plan.has_value())
            {
                globalTrajectory_ = plan->trajectory;
 
                human_ = human::Human{.pose = start,
                                      .linearVelocity = Eigen::Vector2f::Zero(),
                                      .detectionTime = Clock::Now()};
                return;
            }
        }
    }
 
    void
    SimulatedHuman::step()
    {
        const auto updateTime = Clock::Now();
 
        const auto duration = updateTime - human_.detectionTime;
 
        // move according to old state
        human_.pose.translation() += duration.toSecondsDouble() * human_.linearVelocity;
 
        const auto projection =
            globalTrajectory_.getProjection(conv::to3D(human_.pose.translation()),
                                            core::VelocityInterpolation::LinearInterpolation);
 
        human_.pose = conv::to2D(projection.projection.waypoint.pose);
 
        const auto wpBefore = projection.wayPointBefore.waypoint.pose.translation();
        const auto wpAfter = projection.wayPointAfter.waypoint.pose.translation();
 
        human_.linearVelocity =
            (wpAfter - wpBefore).head<2>().normalized() * projection.projection.velocity;
 
        human_.linearVelocity =
            human_.linearVelocity.normalized() * std::clamp(human_.linearVelocity.norm(),
                                                            params_.minLinearVelocity,
                                                            params_.maxLinearVelocity);
 
        human_.detectionTime = updateTime;
    }
 
    SimulatedHuman::SimulatedHuman(const algorithms::Costmap& distanceField, const Params& params) :
        distanceField_(distanceField), params_(params)
    {
        const auto sampledPose = algorithms::sampleValidPositionInMap(distanceField_);
        ARMARX_CHECK(sampledPose.has_value());
        human_.pose = sampledPose.value();
 
        update(); // should bring the robot into walking state
    }
} // namespace armarx::navigation::human::simulation