MoveJointsToNamedConfiguration.cpp

Go to the documentation of this file.

#include "MoveJointsToNamedConfiguration.h"
 
#include <set>
 
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
#include <RobotAPI/libraries/skills/provider/SpecializedSkillProxy.h>
 
#include <armarx/control/skills/aron/MoveJointsToPositionParams.aron.generated.h>
#include <armarx/control/skills/constants/constants.h>
#include <armarx/control/skills/skills/param_conversions.h>
 
namespace armarx::control::skills::skills
{
 
    MoveJointsToNamedConfiguration::MoveJointsToNamedConfiguration(const Services& remote) :
        SimpleSpecializedSkill(GetSkillDescription()), remote(remote)
    {
    }
 
    armarx::skills::SkillDescription
    MoveJointsToNamedConfiguration::GetSkillDescription()
    {
        ParamType defaultParams;
        // - arms
        defaultParams.enableLeftArm = true;
        defaultParams.enableRightArm = true;
        // - hands
        defaultParams.enableLeftHand = false;
        defaultParams.enableRightHand = false;
        // - torso
        defaultParams.enableTorso = true;
        // - head (-> should be controlled by GazeController)
        defaultParams.enableHead = false;
 
        std::stringstream desc;
        desc << "Move all joints to a named configuration of the robot model."
             << "\n\n"
             << "The `enable*` parameters can be used to enable/disable movement for different "
                "body parts of the robot.";
 
        return armarx::skills::SkillDescription{
            .skillId = {.skillName = constants::MOVE_JOINTS_TO_NAMED_CONFIGURATION_SKILL_NAME},
            .description = desc.str(),
            .rootProfileDefaults = defaultParams.toAron(),
            .timeout = ::armarx::Duration::Seconds(30),
            .parametersType = ParamType::ToAronType(),
        };
    }
 
    ::armarx::skills::Skill::MainResult
    MoveJointsToNamedConfiguration::main(const SpecializedMainInput& in)
    {
        const VirtualRobot::RobotPtr robot =
            remote.virtualRobotReader.getRobotWaiting(remote.robotName);
        if (not robot)
        {
            ARMARX_ERROR << "Could not get robot '" << remote.robotName << "'";
            return MakeFailedResult();
        }
 
        const auto configuration = robot->getConfiguration(in.parameters.configuration);
        if (not configuration)
        {
            ARMARX_ERROR << "Robot '" << remote.robotName << "' does not have configuration '"
                         << in.parameters.configuration << "'.";
            return MakeFailedResult();
        }
 
        // for every enabled robot part, the nodes of the corresponding RobotNodeSet are enabled
        std::set<std::string> enabledJoints;
        const auto addJoints = [&](const std::string& nodeSetName)
        {
            if (not robot->hasRobotNodeSet(nodeSetName))
            {
                ARMARX_ERROR << "Robot '" << remote.robotName << "' does not have a RobotNodeSet '"
                             << in.parameters.configuration << "'.";
            }
            else
            {
                const auto robotNodes = robot->getRobotNodeSet(nodeSetName)->getNodeNames();
                enabledJoints.insert(robotNodes.cbegin(), robotNodes.cend());
            }
        };
 
        if (in.parameters.enableLeftArm)
        {
            addJoints("LeftArm");
        }
        if (in.parameters.enableRightArm)
        {
            addJoints("RightArm");
        }
        if (in.parameters.enableLeftHand)
        {
            addJoints("LeftHand");
        }
        if (in.parameters.enableRightHand)
        {
            addJoints("RightHand");
        }
        if (in.parameters.enableTorso)
        {
            addJoints("Torso");
        }
        if (in.parameters.enableHead)
        {
            addJoints("Head");
        }
 
 
        {
            ::armarx::skills::SkillID skillId{
                .providerId = getSkillId().providerId,
                .skillName = constants::MOVE_JOINTS_TO_POSITION_SKILL_NAME,
            };
 
            using Parameters = arondto::MoveJointsToPositionParams;
 
            auto status =
                callSubskill<Parameters>(skillId,
                                         [&](Parameters& params)
                                         {
                                             copy_params(in.parameters, params);
 
                                             // apply configuration for all enabled joints
                                             for (const auto& [node, value] : *configuration)
                                             {
                                                 if (enabledJoints.count(node))
                                                 {
                                                     params.jointsTargetValues[node] = value;
                                                 }
                                             }
                                         });
 
            if (armarx::skills::skillExecutionFailed(status))
            {
                return MakeFailedResult();
            }
 
            return MakeSucceededResult();
        }
    }
 
} // namespace armarx::control::skills::skills