MovePlatformForGrasp.cpp

Go to the documentation of this file.

#include "MovePlatformForGrasp.h"
 
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/skills/core/SkillProxy.h>
 
#include <RobotSkillTemplates/libraries/skill_platform_control/MovePlatformToPose.h>
 
#include "util/GraspObjectUtil.h"
 
namespace armarx::skills
{
    namespace
    {
        grasp_object::arondto::MovePlatformForGraspAcceptedType
        GetDefaultParameterization()
        {
            grasp_object::arondto::MovePlatformForGraspAcceptedType ret;
            ret.orientationalAccuracy = 0.01;
            ret.positionalAccuracy = 25;
            return ret;
        }
    } // namespace
 
    SkillDescription MovePlatformForGraspSkill::Description = skills::SkillDescription{
        .skillId = {.skillName = "MovePlatformForGrasp"},
        .description = "Move Platform in order to ease the grasp of an object",
        .rootProfileDefaults = GetDefaultParameterization().toAron(),
        .timeout = armarx::Duration::MilliSeconds(12000),
        .parametersType = grasp_object::arondto::MovePlatformForGraspAcceptedType::ToAronType(),
    };
 
    MovePlatformForGraspSkill::MovePlatformForGraspSkill(armem::client::MemoryNameSystem& mns,
                                                         armarx::viz::Client& arviz,
                                                         TwoArmGraspControlSkillContext& context) :
        TwoArmGraspControlSkill(mns, arviz, Description.skillId.skillName, context),
        SimpleSpecializedSkill<ArgType>(Description)
    {
    }
 
    Skill::MainResult
    MovePlatformForGraspSkill::main(const SpecializedMainInput& in)
    {
        armem::robot_state::VirtualRobotReader robotReader;
        armem::obj::instance::Reader objectReader;
 
        robotReader.connect(mns);
        objectReader.connect(mns);
 
        // //////////////////////////////
        // get robot
        // //////////////////////////////
        auto robot =
            robotReader.getSynchronizedRobot(in.parameters.robotName,
                                             armem::Time::Now(),
                                             VirtualRobot::RobotIO::RobotDescription::eStructure);
        if (!robot)
        {
            ARMARX_ERROR << "Lost robot.";
            return {TerminatedSkillStatus::Failed, nullptr};
        }
 
        // //////////////////////////////
        // get object pose
        // //////////////////////////////
        auto objInstance = objectReader.queryLatestObjectInstance(in.parameters.objectEntityId);
        if (!objInstance)
        {
            ARMARX_ERROR << "Lost object pose.";
            return {TerminatedSkillStatus::Failed, nullptr};
        }
 
        // //////////////////////////////
        // Exec move skill
        // //////////////////////////////
        auto targetPose = skills::grasp_control::util::GetGlobalPlatformPoseForObjectGrasp(
            {*objInstance, *robot, in.parameters.tcpName});
 
        skills::platform_control::arondto::MovePlatformToPoseAcceptedType params;
        params.robotName = in.parameters.robotName;
        params.orientationalAccuracy = in.parameters.orientationalAccuracy;
        params.positionalAccuracy = in.parameters.positionalAccuracy;
        params.pose = targetPose.platformGlobalPose;
 
        SkillProxy prx(manager, SkillID{
            .providerId = ProviderID{.providerName = context.platformControlSkillProvider},
            .skillName = MovePlatformToPose::Description.skillId.skillName});
        return {prx.executeSkill(in.executorName, params.toAron()).status, nullptr};
    }
} // namespace armarx::skills