ZeroTorque.cpp

Go to the documentation of this file.

#include "ZeroTorque.h"
#include <Ice/Exception.h>
 
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/interface/units/RobotUnit/NjointZeroTorqueController.h>
#include <RobotAPI/libraries/skills/core/Skill.h>
#include <RobotAPI/libraries/skills/core/SkillDescription.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
#include <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h>
#include <armarx/control/skills/aron/ZeroTorque.aron.generated.h>
 
namespace armarx::control::skills::skills
{
    armarx::skills::SkillDescription
    ZeroTorque::GetSkillDescription()
    {
        ParamType defaults;
        defaults.nodeSetName = "RightArm";
        return ::armarx::skills::SkillDescription{
            .skillId = {.skillName = "ZeroTorque"},
            .description = "Enables zero torque mode for the runtime",
            .rootProfileDefaults = defaults.toAron(),
            .timeout = ::armarx::Duration::Minutes(10),
            .parametersType = ParamType::ToAronType(),
        };
    }
 
    ZeroTorque::ZeroTorque(const config::ZeroTorque& r) :
        ::armarx::skills::SimpleSpecializedSkill<params::ZeroTorque>(GetSkillDescription()),
        config_(r)
    {
    }
 
    armarx::skills::Skill::MainResult
    ZeroTorque::main(const SpecializedMainInput& in)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(controller_);
 
        // Config
        const armarx::Duration maxDurationPerCleanUpStep = armarx::Duration::Seconds(5);
 
        // Auxiliary variables to avoid re-computation
        bool terminate = false;
        bool isControllerActive = false;
 
        // Information on history, to distinguish between a not-yet active and a no-longer active
        // controller, and to distinguish between self-determined and implicit termination
        bool hasControllerBeenActive = false;
        bool hasTerminationBeenRequested = false;
 
        // Request activation of controller
        ARMARX_INFO << "Requesting to start the zero-torque controller.";
        try
        {
            controller_->activateController();
        }
        catch (const Ice::Exception& e)
        {
            ARMARX_WARNING << "Failed to request the controller getting activated: " << e.what();
            return {.status = armarx::skills::TerminatedSkillStatus::Failed};
        }
 
        // Let the controller run (or complete its start up phase), until it shall terminate
        /**
         * Termination conditions:
         * - Explicit request for the skill to terminate
         * - The controller is no longer active, e.g., due to having been kicked out by another
         *   controller
         */
        do
        {
            // Fetch information on current status and conclude whether to terminate
            hasControllerBeenActive = isControllerActive;
            isControllerActive = controller_->isControllerActive();
            hasTerminationBeenRequested = shouldSkillTerminate();
            terminate =
                hasTerminationBeenRequested or (hasControllerBeenActive and not isControllerActive);
 
            // Inform exactly once about the controller having gotten active
            if (not hasControllerBeenActive and isControllerActive)
            {
                ARMARX_INFO << "Controller is now active.";
            }
 
            // Avoid waiting and respective logging if termination is about to happen
            if (not terminate)
            {
                ARMARX_VERBOSE << deactivateSpam(10) << "ZeroTorque controller is active. "
                               << "Waiting for termination request or controller getting inactive.";
                armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(10));
            }
        } while (not terminate);
        ARMARX_INFO << "Terminating the zero-torque controller. Current state:\n"
                    << "explicit termination request: " << hasTerminationBeenRequested << "\n"
                    << "controller has been active: " << hasControllerBeenActive << "\n"
                    << "controller is (still) active: " << isControllerActive;
 
        // Clean up, but only as far as it has not already been done
        if (armarx::NJointZeroTorqueControllerInterfacePrx::uncheckedCast(controller_))
        {
            // Controller is still present, at least needs to be deleted
 
            if (controller_->isControllerActive())
            {
                // Controller also needs to be deactivated
                try
                {
                    ARMARX_INFO << "About to deactivate the controller";
                    controller_->deactivateController();
                }
                catch (const Ice::Exception& e)
                {
                    ARMARX_WARNING << "Failed to request the controller getting deactivated: "
                                   << e.what();
                    return {.status = armarx::skills::TerminatedSkillStatus::Failed};
                }
 
                armarx::DateTime deactivationStartTime = armarx::DateTime::Now();
                while (controller_->isControllerActive())
                {
                    ARMARX_INFO << deactivateSpam(1)
                                << "Waiting for the controller to get deactivated.";
                    armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(10));
 
                    if (armarx::DateTime::Now() - deactivationStartTime > maxDurationPerCleanUpStep)
                    {
                        ARMARX_WARNING << "Trying to deactivate the controller timed out: "
                                       << armarx::DateTime::Now() - deactivationStartTime;
                        return {.status = armarx::skills::TerminatedSkillStatus::Failed};
                    }
                }
            }
 
            // The controller should still be existent. However, skipp its deletion if this is not
            // the case anymore.
 
            // if (context_.robotUnitPlugin_->getRobotUnit()->getNJointControllerStatus(controllerName_))
            // if (context_.robotUnitPlugin_->getRobotUnit()->getNJointController(controllerName_)->isDeletable())
            // TODO: find a way to check if deletion is still needed; the options above produced
            //       false-positives
            if (false)
            {
                try
                {
                    ARMARX_INFO << "About to delete the controller";
                    controller_->deleteController();
                }
                catch (const Ice::Exception& e)
                {
                    ARMARX_WARNING << "Failed to request the controller getting deleted: "
                                   << e.what();
                    return {.status = armarx::skills::TerminatedSkillStatus::Failed};
                }
                armarx::DateTime deactivationStartTime = armarx::DateTime::Now();
                while (armarx::NJointZeroTorqueControllerInterfacePrx::uncheckedCast(controller_))
                {
                    ARMARX_INFO << deactivateSpam(1)
                                << "Waiting for the controller to get deleted.";
                    armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(10));
 
                    if (armarx::DateTime::Now() - deactivationStartTime > maxDurationPerCleanUpStep)
                    {
                        ARMARX_WARNING << "Trying to delete the controller timed out: "
                                       << armarx::DateTime::Now() - deactivationStartTime;
                        return {.status = armarx::skills::TerminatedSkillStatus::Failed};
                    }
                }
            }
            else
            {
                //ARMARX_INFO << "Controller has already been deleted in the meanwhile.";
                ARMARX_INFO << "Performing an additional explicit deletion of the controller is "
                               "not attempted at the moment.";
            }
        }
 
        // Exit with respective status
        if (hasTerminationBeenRequested)
        {
            if (hasControllerBeenActive)
            {
                return {.status = armarx::skills::TerminatedSkillStatus::Succeeded};
            }
            else
            {
                ARMARX_INFO << "ZeroTorque controller has successfully been cleaned up, "
                               "but has never been active.";
                return {.status = armarx::skills::TerminatedSkillStatus::Failed};
            }
        }
        else
        {
            ARMARX_INFO << "ZeroTorque controller has successfully been cleaned up, "
                           "but was terminated due to having gone inactive instead of "
                           "due to an explicit request.";
            return {.status = armarx::skills::TerminatedSkillStatus::Failed};
        }
    }
 
    void
    ZeroTorque::connectTo(const ZeroTorque::Context& context)
    {
        context_ = context;
    }
 
    armarx::skills::Skill::InitResult
    ZeroTorque::init(
        const armarx::skills::SimpleSpecializedSkill<params::ZeroTorque>::SpecializedInitInput& in)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(context_.virtualRobotReader);
        auto robot = context_.virtualRobotReader->getRobot(config_.robotName);
        ARMARX_CHECK_NOT_NULL(robot) << config_.robotName;
 
        ARMARX_CHECK(context_.virtualRobotReader->synchronizeRobot(*robot, armarx::Clock::Now()));
 
        ARMARX_TRACE;
        NJointZeroTorqueControllerConfigPtr config(new NJointZeroTorqueControllerConfig());
        config->maxTorque = 0;
        controllerName_ = "ControlSkills_ZeroTorque_" + in.parameters.nodeSetName;
 
        ARMARX_CHECK_EXPRESSION(robot->hasRobotNodeSet(in.parameters.nodeSetName))
            << "RNS " << in.parameters.nodeSetName << " is unknown";
        auto nodeSet = robot->getRobotNodeSet(in.parameters.nodeSetName);
        for (auto& name : nodeSet->getNodeNames())
        {
            config->jointNames.push_back(name);
        }
        ARMARX_CHECK_GREATER(config->jointNames.size(), 0) << "RNS is empty!";
        controller_ = armarx::NJointZeroTorqueControllerInterfacePrx::checkedCast(
            context_.robotUnitPlugin_->getRobotUnit()->getNJointController(controllerName_));
        if (not controller_)
        {
            controller_ = armarx::NJointZeroTorqueControllerInterfacePrx::checkedCast(
                context_.robotUnitPlugin_->createNJointController(
                    armarx::RobotUnitControllerNames::NJointZeroTorqueController,
                    controllerName_,
                    config));
        }
        return {.status = armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    armarx::skills::Skill::ExitResult
    ZeroTorque::exit(
        const armarx::skills::SimpleSpecializedSkill<params::ZeroTorque>::SpecializedExitInput& in)
    {
        ARMARX_TRACE;
        context_.robotUnitPlugin_->getRobotUnit()->deactivateAndDeleteNJointController(
            controllerName_);
        //        controller_->deactivateAndDeleteController();
        return {.status = armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
} // namespace armarx::control::skills::skills