NJointHolonomicPlatformRelativePositionController.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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    ArmarX
 * @author     Mirko Waechter( mirko.waechter at kit dot edu)
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "NJointHolonomicPlatformRelativePositionController.h"
 
#include <Eigen/Geometry>
 
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
 
namespace armarx
{
    NJointControllerRegistration<NJointHolonomicPlatformRelativePositionController>
        registrationNJointHolonomicPlatformRelativePositionController(
            "NJointHolonomicPlatformRelativePositionController");
 
    NJointHolonomicPlatformRelativePositionController::
        NJointHolonomicPlatformRelativePositionController(
            RobotUnit*,
            const NJointHolonomicPlatformRelativePositionControllerConfigPtr& cfg,
            const VirtualRobot::RobotPtr&) :
        pid(cfg->p, cfg->i, cfg->d, cfg->maxVelocity, cfg->maxAcceleration)
    {
        const SensorValueBase* sv = useSensorValue(cfg->platformName);
        this->sv = sv->asA<SensorValueHolonomicPlatform>();
        target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)
                     ->asA<ControlTargetHolonomicPlatformVelocity>();
        ARMARX_CHECK_EXPRESSION(target)
            << "The actuator '" << cfg->platformName << "' has no control mode "
            << ControlModes::HolonomicPlatformVelocity;
 
        pid.threadSafe = false;
 
        oriCtrl.maxV = cfg->maxRotationVelocity;
        oriCtrl.acceleration = cfg->maxRotationAcceleration;
        oriCtrl.deceleration = cfg->maxRotationAcceleration;
        oriCtrl.maxDt = 0.1;
        oriCtrl.pid->Kp = cfg->p;
        oriCtrl.positionPeriodLo = -M_PI;
        oriCtrl.positionPeriodHi = M_PI;
        pid.preallocate(2);
    }
 
    void
    NJointHolonomicPlatformRelativePositionController::rtRun(
        const IceUtil::Time&,
        const IceUtil::Time& timeSinceLastIteration)
    {
        currentPosition << sv->relativePositionX, sv->relativePositionY;
        currentOrientation = sv->relativePositionRotation;
        if (rtGetControlStruct().newTargetSet)
        {
            startPosition = currentPosition;
            startOrientation = currentOrientation;
            pid.reset();
            getWriterControlStruct().newTargetSet = false;
            writeControlStruct();
        }
 
        //position pid
        Eigen::Vector2f relativeCurrentPosition = currentPosition - startPosition;
        pid.update(timeSinceLastIteration.toSecondsDouble(),
                   relativeCurrentPosition,
                   rtGetControlStruct().target);
 
        float relativeOrientation = currentOrientation - startOrientation;
        //rotation pid
        // Revert the rotation by rotating by the negative angle
        Eigen::Vector2f localTargetVelocity =
            Eigen::Rotation2Df(-currentOrientation) *
            Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
        //ARMARX_RT_LOGF_INFO("global target vel x: %.2f y: %2.f, local target vel x: %.2f y: %2.f rotation: %2.f", pid.getControlValue()[0], pid.getControlValue()[1], localTargetVelocity[0], localTargetVelocity[1], sv->relativePositionRotation).deactivateSpam(0.1);
 
        target->velocityX = localTargetVelocity[0];
        target->velocityY = localTargetVelocity[1];
        //        target->velocityRotation = pid.getControlValue()[2] / rad2MMFactor;
        oriCtrl.dt = timeSinceLastIteration.toSecondsDouble();
        oriCtrl.accuracy = rtGetControlStruct().rotationAccuracy;
        oriCtrl.currentPosition = relativeOrientation;
        oriCtrl.targetPosition = rtGetControlStruct().targetOrientation;
        oriCtrl.currentV = sv->velocityRotation;
        target->velocityRotation = oriCtrl.run();
        Eigen::Vector2f posError = pid.target.head(2) - pid.processValue.head(2);
        if (posError.norm() < rtGetControlStruct().translationAccuracy)
        {
            target->velocityX = 0;
            target->velocityY = 0;
        }
        //float orientationError = std::abs(oriCtrl.currentPosition - oriCtrl.targetPosition);
        //        if (orientationError < rtGetControlStruct().rotationAccuracy)
        //        {
        //            target->velocityRotation = 0;
        //        }
        //        ARMARX_INFO << deactivateSpam(0.1) << VAROUT(oriCtrl.currentPosition) << VAROUT(orientationError);
        //        ARMARX_INFO << deactivateSpam(0.1) << VAROUT(target->velocityRotation) << VAROUT(sv->velocityRotation);
        //        ARMARX_RT_LOGF_INFO("current pose x: %.2f y: %2.f, error x: %.2f y: %2.f error: %2.f ori error: %2.f", currentPose[0], currentPose[1], posError[0], posError[1], posError.norm(), orientationError).deactivateSpam(0.1);
        //        ARMARX_RT_LOGF_INFO("new target vel: %2.f, %2.f current vel: %2.f, %2.f", target->velocityX, target->velocityY, sv->velocityX, sv->velocityY).deactivateSpam(0.1);
    }
 
    void
    NJointHolonomicPlatformRelativePositionController::rtPreActivateController()
    {
        startPosition[0] = sv->relativePositionX;
        startPosition[1] = sv->relativePositionY;
        startOrientation = sv->relativePositionRotation;
    }
 
    void
    NJointHolonomicPlatformRelativePositionController::setTarget(float x,
                                                                 float y,
                                                                 float yaw,
                                                                 float translationAccuracy,
                                                                 float rotationAccuracy)
    {
        LockGuardType guard{controlDataMutex};
        getWriterControlStruct().target << x, y;
        getWriterControlStruct().targetOrientation = yaw;
        getWriterControlStruct().translationAccuracy = translationAccuracy;
        getWriterControlStruct().rotationAccuracy = rotationAccuracy;
        getWriterControlStruct().newTargetSet = true;
        writeControlStruct();
        //        rtUpdateControlStruct();
    }
 
 
} // namespace armarx