Smoothing.cpp

Go to the documentation of this file.

#include "Smoothing.h"
 
#include <iomanip>
#include <ios>
#include <vector>
 
#include <Eigen/Geometry>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include <armarx/navigation/algorithms/orientation_aware/Costmap3D.h>
#include <armarx/navigation/algorithms/orientation_aware/smoothing/Costmap3DWrapper.h>
#include <armarx/navigation/algorithms/orientation_aware/smoothing/residuals.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/Trajectory.h>
 
namespace armarx::navigation::algorithms::orientation_aware::smoothing
{
    Smoothing::Smoothing(const core::GlobalTrajectory& trajectory,
                         const Costmap3D& costmap3d,
                         const Params& params) :
        trajectory(trajectory), costmap3d(costmap3d), params(params)
    {
    }
 
    // convert back to global trajectory
    armarx::navigation::core::GlobalTrajectory
    toGlobalTrajectory(const std::vector<CenterPoint>& traj)
    {
        armarx::navigation::core::GlobalTrajectory gtraj;
        for (const auto& p : traj)
        {
            armarx::navigation::core::GlobalTrajectoryPoint gp;
            gp.waypoint.pose =
                Eigen::Translation3f{Eigen::Vector3f(p.x, p.y, 0.0)} *
                Eigen::AngleAxisf{static_cast<float>(p.theta), Eigen::Vector3f::UnitZ()};
            gp.velocity = 200;
            gtraj.mutablePoints().push_back(gp);
        }
        return gtraj;
    }
 
    // convert global trajectory to trajectory for smoothing
    std::vector<CenterPoint>
    toCenterTrajectory(const armarx::navigation::core::GlobalTrajectory& gtraj)
    {
        std::vector<CenterPoint> traj;
        for (const auto& gp : gtraj.points())
        {
            CenterPoint p;
            p.x = gp.waypoint.pose.translation().x();
            p.y = gp.waypoint.pose.translation().y();
            p.theta = gp.waypoint.pose.rotation().eulerAngles(0, 1, 2)[2];
            p.v = gp.velocity;
            traj.push_back(p);
        }
        return traj;
    }
 
    Smoothing::OptimizationResult
    Smoothing::optimize()
    {
        Costmap3DWrapper costmapWrapper{costmap3d};
 
        // Convert trajectory (currently only a shortcut - not the correct trajectory is used)
        std::vector<CenterPoint> traj;
        std::vector<CenterPoint> traj_orig;
        {
            traj = toCenterTrajectory(trajectory);
            traj_orig = traj;
        }
 
        optimizeTrajectoryCeres(traj, costmapWrapper, traj_orig, params);
 
        for (int i = 0; i < static_cast<int>(traj.size()); ++i)
        {
            ARMARX_DEBUG << std::fixed << std::setprecision(3) << "i=" << i << " : x=" << traj[i].x
                         << " y=" << traj[i].y << " theta=" << traj[i].theta << " v=" << traj[i].v;
        }
 
        return {.trajectory = toGlobalTrajectory(traj)};
    }
 
    std::vector<core::GlobalTrajectoryPoint>
    Smoothing::checkCenterTrajectory(core::GlobalTrajectory& centerTrajectory) const
    {
        std::vector<core::GlobalTrajectoryPoint> collisionPoints;
        for (const auto& p : centerTrajectory.points())
        {
            bool inCollision = costmap3d.isInCollision(
                conv::to2D(p.waypoint.pose.translation()),
                costmap3d.closestRotationFromDegrees(
                    costmap3d.rotationDegrees(conv::to2D(p.waypoint.pose))));
            if (inCollision)
            {
                ARMARX_INFO << "Collision detected at position "
                            << VAROUT(conv::to2D(p.waypoint.pose).translation());
                collisionPoints.push_back(p);
            }
        }
        ARMARX_INFO << "Trajectory is "
                    << (collisionPoints.empty() ? "collision-free." : "in collision!");
        return collisionPoints;
    }
 
 
} // namespace armarx::navigation::algorithms::orientation_aware::smoothing