NJointHolonomicPlatformUnitVelocityPassThroughController.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::NJointHolonomicPlatformUnitVelocityPassThroughController
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "NJointHolonomicPlatformUnitVelocityPassThroughController.h"
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
 
namespace armarx
{
    NJointHolonomicPlatformUnitVelocityPassThroughController::
        NJointHolonomicPlatformUnitVelocityPassThroughController(
            RobotUnit* prov,
            NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr cfg,
            const VirtualRobot::RobotPtr&) :
        maxCommandDelay(IceUtil::Time::milliSeconds(500))
    {
        target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)
                     ->asA<ControlTargetHolonomicPlatformVelocity>();
        ARMARX_CHECK_EXPRESSION(target)
            << "The actuator " << cfg->platformName << " has no control mode "
            << ControlModes::HolonomicPlatformVelocity;
 
        initialSettings.velocityX = cfg->initialVelocityX;
        initialSettings.velocityY = cfg->initialVelocityY;
        initialSettings.velocityRotation = cfg->initialVelocityRotation;
        reinitTripleBuffer(initialSettings);
    }
 
    void
    NJointHolonomicPlatformUnitVelocityPassThroughController::rtRun(
        const IceUtil::Time& sensorValuesTimestamp,
        const IceUtil::Time&)
    {
        auto commandAge = sensorValuesTimestamp - rtGetControlStruct().commandTimestamp;
 
        if (commandAge > maxCommandDelay && // command must be recent
            (rtGetControlStruct().velocityX != 0.0f || rtGetControlStruct().velocityY != 0.0f ||
             rtGetControlStruct().velocityRotation !=
                 0.0f)) // only throw error if any command is not zero
        {
            throw LocalException(
                "Platform target velocity was not set for a too long time: delay: ")
                << commandAge.toSecondsDouble()
                << " s, max allowed delay: " << maxCommandDelay.toSecondsDouble() << " s";
        }
        else
        {
            target->velocityX = rtGetControlStruct().velocityX;
            target->velocityY = rtGetControlStruct().velocityY;
            target->velocityRotation = rtGetControlStruct().velocityRotation;
        }
    }
 
    void
    NJointHolonomicPlatformUnitVelocityPassThroughController::setVelocites(float velocityX,
                                                                           float velocityY,
                                                                           float velocityRotation)
    {
        LockGuardType guard{controlDataMutex};
        getWriterControlStruct().velocityX = velocityX;
        getWriterControlStruct().velocityY = velocityY;
        getWriterControlStruct().velocityRotation = velocityRotation;
        getWriterControlStruct().commandTimestamp = IceUtil::Time::now();
        writeControlStruct();
    }
 
    IceUtil::Time
    NJointHolonomicPlatformUnitVelocityPassThroughController::getMaxCommandDelay() const
    {
        return maxCommandDelay;
    }
 
    void
    NJointHolonomicPlatformUnitVelocityPassThroughController::setMaxCommandDelay(
        const IceUtil::Time& value)
    {
        maxCommandDelay = value;
    }
 
    NJointControllerRegistration<NJointHolonomicPlatformUnitVelocityPassThroughController>
        registrationNJointHolonomicPlatformUnitVelocityPassThroughController(
            "NJointHolonomicPlatformUnitVelocityPassThroughController");
} // namespace armarx