LookAtObject.cpp

Go to the documentation of this file.

#include "LookAtObject.h"
 
#include <vector>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <range/v3/algorithm/max_element.hpp>
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/transform.hpp>
 
#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
#include <SimoxUtility/shapes/OrientedBox.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/interface/core/UserException.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/simox.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/skills/core/ProviderID.h>
#include <RobotAPI/libraries/skills/core/Skill.h>
#include <RobotAPI/libraries/skills/core/SkillDescription.h>
#include <RobotAPI/libraries/skills/core/SkillID.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
#include <RobotAPI/libraries/skills/provider/SpecializedSkillProxy.h>
 
#include <armarx/view_selection/gaze_targets/aron/AttentionType.aron.generated.h>
#include <armarx/view_selection/gaze_targets/aron/GazeTarget.aron.generated.h>
#include <armarx/view_selection/skills/aron/LookAt.aron.generated.h>
#include <armarx/view_selection/skills/aron/LookAtObject.aron.generated.h>
#include <armarx/view_selection/skills/constants.h>
#include <armarx/view_selection/skills/skill_ids.h>
 
namespace armarx::view_selection::skills
{
    LookAtObject::LookAtObject(const Services& srv) : Base(DefaultSkillDescription()), srv_(srv)
    {
    }
 
    ::armarx::skills::Skill::InitResult
    LookAtObject::init(const Base::SpecializedInitInput& in)
    {
        return ::armarx::skills::Skill::InitResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    ::armarx::skills::Skill::MainResult
    LookAtObject::main(const Base::SpecializedMainInput& in)
    {
 
        ObjectID objectID;
        fromAron(in.parameters.object, objectID);
 
        ARMARX_INFO << "Querying object " << QUOTED(objectID);
 
        const auto result = srv_.objectInstanceReader->queryLatestObjectInstance(objectID);
 
        if (not result)
        {
            ARMARX_INFO << "Memory query did not provide any observation for " << objectID;
            return MakeFailedResult();
        }
 
        // make sure that the instance name is set.
        objectID = result->objectID;
 
        const simox::OrientedBoxf oobb = [&]()
        {
            if (result->localOOBB.has_value())
            {
                return result->localOOBB.value();
            }
 
            // fallback: load from file system
            {
                auto finder = ObjectFinder();
                auto objectClassInfoOpt = finder.findObject(objectID);
                ARMARX_CHECK_NOT_NULL(objectClassInfoOpt);
 
                const auto& objectClassInfo = objectClassInfoOpt.value();
 
                const auto oobbOpt = objectClassInfo.loadOOBB();
                ARMARX_CHECK_NOT_NULL(oobbOpt);
 
                const simox::OrientedBoxf& oobb = oobbOpt.value();
                return oobb;
            }
        }();
 
 
        const armarx::objpose::ObjectPose& objectObservation = result.value();
 
 
        const Eigen::Vector3f global_P_obj = [&]() -> Eigen::Vector3f
        {
            const Eigen::Isometry3f globalObjectPose{objectObservation.objectPoseGlobal};
 
            ARMARX_INFO << "Object global position " << globalObjectPose.translation();
 
            using armarx::view_selection::skills::arondto::PointOnObject;
            switch (in.parameters.pointOnObject.value)
            {
                case PointOnObject::Center:
                {
                    const Eigen::Vector3f globalObjectCenter =
                        globalObjectPose * (oobb.center() + in.parameters.pointOnObjectOffset);
 
                    return globalObjectCenter;
                    break;
                }
                case PointOnObject::BoundingBoxTopCenter:
                {
                    // All 8 permutations
                    const std::vector<Eigen::Vector3f> faceCenterMultipliers{
                        Eigen::Vector3f::UnitX(),
                        -Eigen::Vector3f::UnitX(),
                        Eigen::Vector3f::UnitY(),
                        -Eigen::Vector3f::UnitY(),
                        Eigen::Vector3f::UnitZ(),
                        -Eigen::Vector3f::UnitZ(),
                    };
 
                    const auto applyTransformation =
                        [&globalObjectPose,
                         &oobb,
                         pointOnObjectOffset = in.parameters.pointOnObjectOffset](
                            const Eigen::Vector3f& faceMultiplier) -> Eigen::Vector3f
                    {
                        const Eigen::Vector3f shift =
                            Eigen::Vector3f(oobb.dimensions() / 2) +
                            Eigen::Vector3f{Eigen::Vector3f(oobb.dimensions() / 2).array() *
                                            faceMultiplier.array()};
 
 
                        return Eigen::Vector3f{globalObjectPose * oobb.from_box_frame(shift) +
                                               pointOnObjectOffset};
 
 
                        // const Eigen::Isometry3f globalObjectBB =
                        //     globalObjectPose * Eigen::Isometry3f{oobb.transformation()};
 
 
                        // return globalObjectBB * shift;
                    };
 
                    const std::vector<Eigen::Vector3f> globalObjectSurfaceCenters =
                        faceCenterMultipliers | ranges::views::transform(applyTransformation) |
                        ranges::to_vector;
 
                    const std::vector<float> height =
                        globalObjectSurfaceCenters |
                        ranges::views::transform([](const Eigen::Vector3f& v) { return v.z(); }) |
                        ranges::to_vector;
 
                    const auto it = ranges::max_element(height);
 
                    const Eigen::Vector3f& highestSurfaceCenter =
                        globalObjectSurfaceCenters.at(std::distance(height.begin(), it));
 
                    return highestSurfaceCenter;
 
                    break;
                }
                default:
                    throw armarx::InvalidArgumentException("should not get here");
            }
        }();
 
        ARMARX_INFO << "Looking at global position " << global_P_obj;
 
        // check whether the position is valid
        if (in.parameters.permissibleRegionGlobalFrame.has_value())
        {
            ARMARX_INFO
                << "Permissible region provided. Will check whether object position is valid.";
 
            ARMARX_CHECK(
                srv_.virtualRobotReader.synchronizeRobot(*srv_.robot, armarx::Clock::Now()));
 
            const Eigen::Isometry3f global_T_root{srv_.robot->getGlobalPose()};
            const Eigen::Vector3f root_P_obj = global_T_root.inverse() * global_P_obj;
 
            ::simox::OrientedBoxf bb;
            fromAron(in.parameters.permissibleRegionGlobalFrame.value(), bb);
 
            ARMARX_INFO << VAROUT(root_P_obj);
            ARMARX_INFO << VAROUT(bb.center());
            // ARMARX_INFO << VAROUT(bb.extents());
 
            if (bb.contains(root_P_obj))
            {
                ARMARX_INFO << "Object position is inside permissible region.";
                // We are good to go. We can continue.
            }
            else
            {
                ARMARX_INFO << "Object position is outside permissible region.";
                return MakeFailedResult();
            }
        }
        // check whether the position is valid
        else if (in.parameters.permissibleRegionRobotFrame.has_value())
        {
            ARMARX_INFO
                << "Permissible region provided. Will check whether object position is valid.";
 
            ARMARX_CHECK(
                srv_.virtualRobotReader.synchronizeRobot(*srv_.robot, armarx::Clock::Now()));
 
            const Eigen::Isometry3f global_T_root{srv_.robot->getGlobalPose()};
            const Eigen::Vector3f root_P_obj = global_T_root.inverse() * global_P_obj;
 
            ::simox::AxisAlignedBoundingBox bb;
            fromAron(in.parameters.permissibleRegionRobotFrame.value(), bb);
 
            ARMARX_INFO << VAROUT(root_P_obj);
            ARMARX_INFO << VAROUT(bb.center());
            ARMARX_INFO << VAROUT(bb.extents());
 
            if (bb.is_inside(root_P_obj))
            {
                ARMARX_INFO << "Object position is inside permissible region.";
                // We are good to go. We can continue.
            }
            else
            {
                ARMARX_INFO << "Object position is outside permissible region.";
                return MakeFailedResult();
            }
        }
 
        // generate view target
        const armarx::view_selection::gaze_targets::arondto::GazeTarget gazeTarget = [&]()
        {
            armarx::view_selection::gaze_targets::arondto::GazeTarget gazeTarget;
            gazeTarget.name = "LookAtObject_" + objectID.str();
 
            gazeTarget.position.header.agent = ""; // not needed
            gazeTarget.position.header.frame = armarx::GlobalFrame; // not needed
 
            gazeTarget.position.position = global_P_obj;
 
            gazeTarget.duration =
                armarx::core::time::Duration::SecondsDouble(in.parameters.durationSeconds);
 
            gazeTarget.priority.attentionType =
                gaze_targets::arondto::AttentionType::ImplEnum::TaskDriven;
            gazeTarget.priority.priority = in.parameters.priority;
 
            gazeTarget.creationTimestamp = Clock::Now();
            gazeTarget.keepInQueue = false;
 
            return gazeTarget;
        }();
 
        // execute subskill
        {
            using Parameters = armarx::view_selection::skills::arondto::LookAtParams;
 
            // prepare skill id
            const ::armarx::skills::SkillID skillId{
                .providerId =
                    ::armarx::skills::ProviderID{.providerName = armarx::view_selection::skills::
                                                     constants::ViewSelectionSkillProviderName},
                .skillName = armarx::view_selection::skills::constants::skill_names::LookAt};
 
 
            // call subskill using skill id and a parameter-update function that returns the aron parameters
            auto update = callSubskill<Parameters>(skillId,
                                                   [&gazeTarget](Parameters& subSkillParams)
                                                   { subSkillParams.target = gazeTarget; });
 
            if (armarx::skills::skillExecutionFailed(update))
            {
                return MakeFailedResult();
            }
        }
 
        return MakeSucceededResult();
    }
 
    void
    LookAtObject::onStopRequested()
    {
    }
 
    ::armarx::skills::SkillDescription
    LookAtObject::DefaultSkillDescription()
    {
        Params defaults;
        defaults.priority = 100;
        //defaults.maxObservationAgeSeconds = 15 * 60; // quater of an hour
        defaults.durationSeconds = 20;
        defaults.pointOnObject = armarx::view_selection::skills::arondto::PointOnObject::Center;
        defaults.pointOnObjectOffset = Eigen::Vector3f::Zero();
        defaults.permissibleRegionGlobalFrame = std::nullopt;
        defaults.permissibleRegionRobotFrame = std::nullopt;
 
        return armarx::skills::SkillDescription{
            .skillId = skill_ids::LookAtObject,
            .description = "Look at the location where an instance of the object class has most "
                           "recently been seen.",
            .rootProfileDefaults = defaults.toAron(),
            .timeout = armarx::Duration::Minutes(5),
            .parametersType = Params::ToAronType(),
            .resultType = Result::ToAronType(),
        };
    }
 
} // namespace armarx::view_selection::skills