MoveJointsToPosition.h

Go to the documentation of this file.

#pragma once
 
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
#include <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h>
 
#include <armarx/control/skills/aron/MoveJointsToPositionParams.aron.generated.h>
#include <armarx/control/skills/constants/constants.h>
 
namespace armarx::control::skills::skills
{
 
    /**
   * @class MoveJoints
   * @brief MoveJoints-skill to move joints to specified target values using the KinematicUnit
   *
   * Sets joint values with the KinematicUnit and waits until target values are reached. Can
   * simulate trajectory when in simulation (otherwise trajectories from joint value updates through
   * the KinematicUnit are not simulated).
   */
    class MoveJointsToPosition :
        public ::armarx::skills::SimpleSpecializedSkill<arondto::MoveJointsToPositionParams>
    {
 
    public:
        struct Services
        {
            ::armarx::KinematicUnitInterfacePrx kinematicUnit;
            ::armarx::armem::robot_state::VirtualRobotReader virtualRobotReader;
            std::string robotName;
        };
 
        static ::armarx::skills::SkillDescription
        GetSkillDescription()
        {
            ParamType defaults;
            defaults.timeoutMS = 1000;
 
            return ::armarx::skills::SkillDescription{
                .skillId = {.skillName = constants::MOVE_JOINTS_TO_POSITION_SKILL_NAME},
                .description = "Moves joints to specified target values using the KinematicUnit",
                .rootProfileDefaults = defaults.toAron(),
                .timeout = ::armarx::Duration::Minutes(1),
                .parametersType = ParamType::ToAronType()};
        }
 
        MoveJointsToPosition(const Services&);
 
 
    private:
        using Base::exit;
        using Base::main;
 
        ::armarx::skills::Skill::MainResult main(const SpecializedMainInput& in) override;
        ::armarx::skills::Skill::ExitResult exit(const SpecializedExitInput& in) override;
 
        std::map<std::string, float>
        filterJointTargetValues(const NameValueMap& jointsTargetValues,
                                const std::vector<std::string>& disabledJoints) const;
 
        bool checkJointsTargetValuesValid(const NameValueMap& jointsTargetValues,
                                          const VirtualRobot::RobotPtr& virtualRobot);
 
        void switchControlMode(const NameValueMap& jointsTargetValues,
                               const armarx::ControlMode controlMode);
 
        void moveJoints(const NameValueMap& jointsTargetValues,
                        float accuracyDefault,
                        const NameValueMap& accuracyOverride,
                        float timeoutDefault,
                        NameValueMap timeoutOverride,
                        const VirtualRobot::RobotPtr& virtualRobot);
 
        void moveJointsSimulation(const NameValueMap& jointsTargetValues,
                                  float speedDefault,
                                  const NameValueMap& speedOverride,
                                  const VirtualRobot::RobotPtr& virtualRobot);
 
    private:
        Services srv_;
    };
 
} // namespace armarx::control::skills::skills