MoveJointsWithVelocity.cpp

Go to the documentation of this file.

#include "MoveJointsWithVelocity.h"
 
#include <ArmarXCore/core/time/Clock.h>
 
#include <RobotAPI/libraries/SimpleTrajectory/Trajectory.h>
 
namespace armarx::control::skills::skills
{
 
    MoveJointsWithVelocity::MoveJointsWithVelocity(const Services& srv) :
        ::armarx::skills::SimpleSpecializedSkill<ParamType>(GetSkillDescription()), srv_(srv)
    {
    }
 
    std::map<std::string, float>
    MoveJointsWithVelocity::filterJointTargetValues(
        const NameValueMap& jointsTargetValues,
        const std::vector<std::string>& disabledJoints) const
    {
        std::map<std::string, float> filtered;
        for (const auto& [jointName, v] : jointsTargetValues)
        {
            if (std::find(disabledJoints.begin(), disabledJoints.end(), jointName) !=
                disabledJoints.end())
            {
                ARMARX_INFO << "Disable joint " << jointName
                            << " because it was explictly disabled in skill params.";
                continue;
            }
            filtered[jointName] = v;
        }
        return filtered;
    }
 
    ::armarx::skills::Skill::ExitResult
    MoveJointsWithVelocity::exit(const SpecializedExitInput& in)
    {
        auto filtered = this->filterJointTargetValues(in.parameters.jointsTargetVelocities,
                                                      in.parameters.disabledJoints);
 
        auto jointNames = simox::alg::get_keys(filtered);
 
        for (auto& [k, v] : filtered)
        {
            filtered[k] = 0.0;
        }
 
        srv_.kinematicUnit->setJointVelocities(filtered);
 
        srv_.kinematicUnit->releaseJoints(jointNames);
        return {::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    ::armarx::skills::Skill::MainResult
    MoveJointsWithVelocity::main(const SpecializedMainInput& in)
    {
        auto filtered = this->filterJointTargetValues(in.parameters.jointsTargetVelocities,
                                                      in.parameters.disabledJoints);
 
        auto jointNames = simox::alg::get_keys(filtered);
 
        // request joints
        ARMARX_INFO << "Requesting joints";
        try
        {
            srv_.kinematicUnit->requestJoints(jointNames);
        }
        catch (const armarx::KinematicUnitUnavailable& e)
        {
            ARMARX_IMPORTANT << "Failed to request joints. Terminating. Joints unavailable:"
                             << e.nodeOwners;
            return MakeFailedResult();
        }
 
        // move joints
        ARMARX_INFO << "Moving joints with vel " << filtered;
 
        switchControlMode(filtered, eVelocityControl);
 
        moveJoints(filtered, in.parameters.duration);
        return MakeSucceededResult();
    }
 
    void
    MoveJointsWithVelocity::switchControlMode(const NameValueMap& jointsTargetValues,
                                              const ControlMode controlMode)
    {
        armarx::NameControlModeMap modeMap;
        for (const auto& [jointName, _] : jointsTargetValues)
        {
            modeMap[jointName] = controlMode;
        }
        srv_.kinematicUnit->switchControlMode(modeMap);
    }
 
    void
    MoveJointsWithVelocity::moveJoints(const NameValueMap& jointsTargetVelocities,
                                       const armarx::core::time::Duration& dur)
    {
        srv_.kinematicUnit->setJointVelocities(jointsTargetVelocities);
 
        const Clock clock;
        const Duration sleepTime = Duration::MilliSeconds(50);
 
        auto started = armarx::core::time::DateTime::Now();
        while (armarx::core::time::DateTime::Now() - started < dur)
        {
            throwIfSkillShouldTerminate();
            clock.waitFor(sleepTime);
        }
    }
 
} // namespace armarx::control::skills::skills