TCPControllerSubUnit.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotAPI::ArmarXObjects::TCPControllerSubUnit
 * @author     Stefan Reither ( stef dot reither at web dot de )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "TCPControllerSubUnit.h"
 
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
 
using namespace armarx;
 
void
TCPControllerSubUnit::update(const SensorAndControl& sc, const JointAndNJointControllers& c)
{
}
 
PropertyDefinitionsPtr
TCPControllerSubUnit::createPropertyDefinitions()
{
    return PropertyDefinitionsPtr(
        new TCPControllerSubUnitPropertyDefinitions(getConfigIdentifier()));
}
 
void
TCPControllerSubUnit::setup(RobotUnit* rUnit, VirtualRobot::RobotPtr robot)
{
    ARMARX_CHECK_EXPRESSION(robot);
    ARMARX_CHECK_EXPRESSION(!this->coordinateTransformationRobot);
    coordinateTransformationRobot = robot;
 
    ARMARX_CHECK_EXPRESSION(!robotUnit);
    ARMARX_CHECK_EXPRESSION(rUnit);
    robotUnit = rUnit;
}
 
void
TCPControllerSubUnit::setCycleTime(Ice::Int, const Ice::Current& c)
{
    ARMARX_WARNING << deactivateSpam()
                   << "Setting cycle time in RT-Controller does not have an effect";
}
 
void
TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName,
                                     const std::string& tcpName,
                                     const FramedDirectionBasePtr& translationVelocity,
                                     const FramedDirectionBasePtr& orientationVelocityRPY,
                                     const Ice::Current& c)
{
    std::unique_lock lock(dataMutex);
    ARMARX_CHECK_EXPRESSION(coordinateTransformationRobot->hasRobotNodeSet(nodeSetName))
        << "The robot does not have the node set: " + nodeSetName;
    std::string tcp;
    if (tcpName.empty())
    {
        tcp = coordinateTransformationRobot->getRobotNodeSet(nodeSetName)->getTCP()->getName();
    }
    else
    {
        ARMARX_CHECK_EXPRESSION(coordinateTransformationRobot->hasRobotNode(tcpName))
            << "The robot does not have the node: " + tcpName;
        tcp = tcpName;
    }
 
    robotUnit->updateVirtualRobot(coordinateTransformationRobot);
 
    float xVel = 0.f;
    float yVel = 0.f;
    float zVel = 0.f;
    float rollVel = 0.f;
    float pitchVel = 0.f;
    float yawVel = 0.f;
 
 
    FramedDirectionPtr globalTransVel = FramedDirectionPtr::dynamicCast(translationVelocity);
    if (globalTransVel)
    {
        globalTransVel->changeFrame(coordinateTransformationRobot,
                                    coordinateTransformationRobot->getRootNode()->getName());
        xVel = globalTransVel->x;
        yVel = globalTransVel->y;
        zVel = globalTransVel->z;
    }
 
    FramedDirectionPtr globalOriVel = FramedDirectionPtr::dynamicCast(orientationVelocityRPY);
    if (globalOriVel)
    {
        globalOriVel->changeFrame(coordinateTransformationRobot,
                                  coordinateTransformationRobot->getRootNode()->getName());
        rollVel = globalOriVel->x;
        pitchVel = globalOriVel->y;
        yawVel = globalOriVel->z;
    }
 
    CartesianSelectionMode::CartesianSelection mode = CartesianSelectionMode::eAll;
    bool noMode = false;
    if (globalTransVel && globalOriVel)
    {
        mode = CartesianSelectionMode::eAll;
    }
    else if (globalTransVel && !globalOriVel)
    {
        mode = CartesianSelectionMode::ePosition;
    }
    else if (!globalTransVel && globalOriVel)
    {
        mode = CartesianSelectionMode::eOrientation;
    }
    else
    {
        noMode = true;
    }
    ARMARX_DEBUG << "CartesianSelection-Mode: " << (int)mode;
    auto controllerName =
        this->getName() + "_" + tcp + "_" + nodeSetName + "_mode_" + std::to_string((int)mode);
    auto NJointControllers = robotUnit->getNJointControllerNames();
    NJointCartesianVelocityControllerWithRampPtr tcpController;
    bool nodeSetAlreadyControlled = false;
    for (auto& name : NJointControllers)
    {
        NJointControllerBasePtr controller;
        try
        {
            controller = robotUnit->getNJointControllerNotNull(name);
        }
        catch (...)
        {
            continue;
        }
 
        tcpController = NJointCartesianVelocityControllerWithRampPtr::dynamicCast(controller);
        if (!tcpController)
        {
            continue;
        }
        if (tcpController->getNodeSetName() == nodeSetName &&
            tcpController->getInstanceName() == controllerName)
        {
            nodeSetAlreadyControlled = true;
            break;
        }
    }
 
    if (!nodeSetAlreadyControlled)
    {
        NJointCartesianVelocityControllerWithRampConfigPtr config =
            new NJointCartesianVelocityControllerWithRampConfig(
                nodeSetName, tcp, mode, 500, 1, 2, 0.3f, 2);
        //        NJointCartesianVelocityControllerConfigPtr config = new NJointCartesianVelocityControllerConfig(nodeSetName, tcp, mode);
        NJointCartesianVelocityControllerWithRampPtr ctrl =
            NJointCartesianVelocityControllerWithRampPtr::dynamicCast(
                robotUnit->createNJointController("NJointCartesianVelocityControllerWithRamp",
                                                  controllerName,
                                                  config,
                                                  true,
                                                  true));
 
        tcpController = ctrl;
        tcpControllerNameMap[nodeSetName] = controllerName;
        tcpController->setKpJointLimitAvoidance(getProperty<float>("AvoidJointLimitsKp").getValue(),
                                                c);
    }
 
 
    // Only activate controller if at least either translationVelocity or orientaionVelocity is set
    if (!noMode)
    {
        ARMARX_CHECK_EXPRESSION(tcpController);
        //        ARMARX_CHECK_EXPRESSION(tcpController->getObjectScheduler());
        tcpController->setTargetVelocity(xVel, yVel, zVel, rollVel, pitchVel, yawVel, c);
        //        if (!tcpController->getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted, 5000))
        //        {
        //            ARMARX_ERROR << "NJointController was not initialized after 5000ms - bailing out!";
        //            return;
        //        }
        if (!tcpController->isControllerActive())
        {
            tcpController->activateController();
        }
    }
}
 
bool
TCPControllerSubUnit::isRequested(const Ice::Current&)
{
    std::unique_lock lock(dataMutex);
    for (auto& pair : tcpControllerNameMap)
    {
        auto ctrl = robotUnit->getNJointController(pair.second);
        if (ctrl && ctrl->isControllerActive())
        {
            return true;
        }
    }
    return false;
}
 
void
armarx::TCPControllerSubUnit::componentPropertiesUpdated(
    const std::set<std::string>& changedProperties)
{
    if (changedProperties.count("AvoidJointLimitsKp") && robotUnit)
    {
        float avoidJointLimitsKp = getProperty<float>("AvoidJointLimitsKp").getValue();
        auto activeNJointControllers =
            robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
        for (NJointControllerBasePtr controller : activeNJointControllers)
        {
            auto tcpController =
                NJointCartesianVelocityControllerWithRampPtr::dynamicCast(controller);
            if (tcpController)
            {
                tcpController->setKpJointLimitAvoidance(avoidJointLimitsKp, Ice::emptyCurrent);
            }
        }
    }
}