MoveJointsToPosition.cpp

Go to the documentation of this file.

#include "MoveJointsToPosition.h"
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
 
#include <ArmarXCore/core/util/StringHelpers.h>
#include <RobotAPI/libraries/SimpleTrajectory/Trajectory.h>
 
namespace armarx::control::skills::skills
{
 
    MoveJointsToPosition::MoveJointsToPosition(const Services& srv) :
        ::armarx::skills::SimpleSpecializedSkill<ParamType>(GetSkillDescription()), srv_(srv)
    {
    }
 
    std::map<std::string, float>
    MoveJointsToPosition::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
    MoveJointsToPosition::exit(const SpecializedExitInput& in)
    {
        auto filtered = this->filterJointTargetValues(in.parameters.jointsTargetValues,
                                                      in.parameters.disabledJoints);
 
        auto jointNames = simox::alg::get_keys(filtered);
 
        srv_.kinematicUnit->releaseJoints(jointNames);
        return {::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    ::armarx::skills::Skill::MainResult
    MoveJointsToPosition::main(const SpecializedMainInput& in)
    {
        auto filtered = this->filterJointTargetValues(in.parameters.jointsTargetValues,
                                                      in.parameters.disabledJoints);
 
        auto jointNames = simox::alg::get_keys(filtered);
 
        // get VirtualRobot
        const VirtualRobot::RobotPtr virtualRobot =
            srv_.virtualRobotReader.getRobot(srv_.robotName);
        if (not virtualRobot)
        {
            ARMARX_ERROR << "Could not get robot '" << srv_.robotName << "'";
            return MakeFailedResult();
        }
 
        // check jointsTargetValues are valid (joints exist, target values are within limits)
        if (not checkJointsTargetValuesValid(in.parameters.jointsTargetValues, virtualRobot))
        {
            return MakeFailedResult();
        }
 
        // 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 to " << filtered;
 
        switchControlMode(filtered, ePositionControl);
 
        if (in.parameters.inSimulation)
        {
            // trajectories from joint value updates through the KinematicUnit are not simulated
            // -> simulate manually
            moveJointsSimulation(filtered,
                                 in.parameters.simulationJointSpeed,
                                 in.parameters.simulationJointSpeedOverride,
                                 virtualRobot);
        }
        else
        {
            NameValueMap test = in.parameters.timeoutOverrideMS;
 
            moveJoints(filtered,
                       in.parameters.accuracy,
                       in.parameters.accuracyOverride,
                       in.parameters.timeoutMS,
                       in.parameters.timeoutOverrideMS,
                       virtualRobot);
        }
        return MakeSucceededResult();
    }
 
    bool
    MoveJointsToPosition::checkJointsTargetValuesValid(const NameValueMap& jointsTargetValues,
                                                       const VirtualRobot::RobotPtr& virtualRobot)
    {
        for (const auto& [jointName, targetValue] : jointsTargetValues)
        {
            if (not virtualRobot->hasRobotNode(jointName))
            {
                ARMARX_ERROR << "Robot has no joint '" << jointName << "'";
                return false;
            }
            const VirtualRobot::RobotNodePtr robotNode = virtualRobot->getRobotNode(jointName);
            if (not robotNode->checkJointLimits(targetValue))
            {
                ARMARX_ERROR << "Target value '" << targetValue << "' for joint '" << jointName
                             << "' out of limits (" << robotNode->getJointLimitLo() << " - "
                             << robotNode->getJointLimitHi() << ")";
                return false;
            }
        }
        return true;
    }
 
    void
    MoveJointsToPosition::switchControlMode(const NameValueMap& jointsTargetValues,
                                            const ControlMode controlMode)
    {
        armarx::NameControlModeMap modeMap;
        for (const auto& [jointName, _] : jointsTargetValues)
        {
            modeMap[jointName] = controlMode;
        }
        srv_.kinematicUnit->switchControlMode(modeMap);
    }
 
    void
    MoveJointsToPosition::moveJoints(const NameValueMap& jointsTargetValues,
                                     const float accuracyDefault,
                                     const NameValueMap& accuracyOverride,
                                     const float timeoutDefault,
                                     NameValueMap timeoutOverride,
                                     const VirtualRobot::RobotPtr& virtualRobot)
    {
        srv_.kinematicUnit->setJointAngles(jointsTargetValues);
 
        // wait until target values are reached
        ARMARX_INFO << "Waiting until target values are reached";
 
        const Clock clock;
        const Duration sleepTime = Duration::MilliSeconds(50);
 
        auto do_if_terminate_and_not_reached = [&]()
        {
            ARMARX_INFO << "Skill got aborted, stopping movement";
            ARMARX_CHECK(srv_.virtualRobotReader.synchronizeRobotJoints(*virtualRobot,
                                                                        armarx::DateTime::Now()))
                << "Robot state synchronization failed";
 
            // stop movement
            switchControlMode(jointsTargetValues, eVelocityControl);
 
            NameValueMap targetJointVelocities;
            for (auto& [jointName, _] : jointsTargetValues)
            {
                targetJointVelocities[jointName] = 0;
            }
            srv_.kinematicUnit->setJointVelocities(targetJointVelocities);
 
            throwIfSkillShouldTerminate(); // throws exception
 
            ARMARX_INFO << "Target values reached";
        };
 
        bool reachedOrTimedOut = false;
 
 
        // fill map with default value, if joint has no specific timeout
        for (const auto& [jointName, _] : jointsTargetValues)
        {
            if (timeoutOverride.find(jointName) == timeoutOverride.end())
            {
                timeoutOverride[jointName] = timeoutDefault;
            }
        }
 
        // store if joint has reached its timeout
        std::map<std::string, bool> timeoutedJoints;
        for (const auto& [jointName, _] : jointsTargetValues)
        {
            timeoutedJoints[jointName] = false;
        }
 
        // hold last joint angle
        NameValueMap pastJointAngles = srv_.kinematicUnit->getJointAngles();
 
        // store the last time where joint moved significantly
        std::map<std::string, armarx::DateTime> lastMotion;
        for (const auto& [jointName, _] : jointsTargetValues)
        {
            lastMotion[jointName] = DateTime::Now();
        }
 
        while (not reachedOrTimedOut)
        {
            throwIfSkillShouldTerminate(do_if_terminate_and_not_reached);
            ARMARX_CHECK(srv_.virtualRobotReader.synchronizeRobotJoints(*virtualRobot,
                                                                        armarx::DateTime::Now()))
                << "Robot state synchronization failed";
 
            reachedOrTimedOut = true;
            for (const auto& [jointName, targetValue] : jointsTargetValues)
            {
                if (not timeoutedJoints[jointName])
                {
 
                    const float currentValue =
                        virtualRobot->getRobotNode(jointName)->getJointValue();
                    const float accuracy = accuracyOverride.count(jointName) > 0
                                               ? accuracyOverride.at(jointName)
                                               : accuracyDefault;
 
                    ARMARX_INFO << deactivateSpam(1) << "Delta for joint '" << jointName
                                << "' is: " << std::abs(targetValue - currentValue);
 
                    // check if joint still moves significantly
                    if (std::abs(currentValue - pastJointAngles[jointName]) > accuracy)
                    {
                        // update last joint angles and timestamp
                        pastJointAngles[jointName] = currentValue;
                        lastMotion[jointName] = DateTime::Now();
                    }
 
                    // check if joint reaches its timeout
                    if ((timeoutOverride[jointName] >= 0) &&
                        ((DateTime::Now() - lastMotion[jointName]).toMilliSecondsDouble() >=
                         timeoutOverride[jointName]))
                    {
                        timeoutedJoints[jointName] = true;
                        ARMARX_INFO << "Joint " << jointName << " has reached its timeout.";
                    }
 
                    if (std::abs(targetValue - currentValue) > accuracy)
                    {
                        ARMARX_INFO
                            << deactivateSpam(1) << "Delta for joint '" << jointName
                            << "' is higer than accuracy: " << std::abs(targetValue - currentValue)
                            << " > " << accuracy << "(accuracy)";
                        reachedOrTimedOut = false;
 
                        break;
                    }
                }
            }
            clock.waitFor(sleepTime);
        }
    }
 
    void
    MoveJointsToPosition::moveJointsSimulation(const NameValueMap& jointsTargetValues,
                                               const float speedDefault,
                                               const NameValueMap& speedOverride,
                                               const VirtualRobot::RobotPtr& virtualRobot)
    {
        ARMARX_CHECK(
            srv_.virtualRobotReader.synchronizeRobotJoints(*virtualRobot, armarx::DateTime::Now()))
            << "Robot state synchronization failed";
 
        //build trajectory
        armarx::trajectory::Trajectory trajectory;
        double maxDuration = 0;
 
        for (const auto& [jointName, targetValue] : jointsTargetValues)
        {
            const float speed =
                speedOverride.count(jointName) > 0 ? speedOverride.at(jointName) : speedDefault;
 
            const float currentValue = virtualRobot->getRobotNode(jointName)->getJointValue();
            const double duration = std::abs(targetValue - currentValue) / speed;
 
            ARMARX_DEBUG << VAROUT(jointName) << VAROUT(duration);
 
            armarx::trajectory::VariantTrack& track = trajectory.addTrack(jointName);
            track[0] = currentValue;
            track[duration] = targetValue;
 
            maxDuration = std::max(maxDuration, duration);
        }
 
        ARMARX_INFO << "Simulating trajectory (duration: " << maxDuration << "s)";
 
        // run trajectory
        const Clock clock;
        const Duration sleepTime = Duration::MilliSeconds(10);
 
        const armarx::DateTime start = armarx::DateTime::Now();
        double t = 0;
        while (t < maxDuration)
        {
            throwIfSkillShouldTerminate();
            t = std::min((armarx::DateTime::Now() - start).toSecondsDouble(), maxDuration);
 
            NameValueMap nextValues;
            for (const auto& [name, _] : jointsTargetValues)
            {
                const float value = std::get<float>(trajectory[name].at(t));
                nextValues[name] = value;
            }
 
            srv_.kinematicUnit->setJointAngles(nextValues);
 
            clock.waitFor(sleepTime);
        }
        ARMARX_INFO << "Target values reached";
    }
 
} // namespace armarx::control::skills::skills