PlatformUnitDynamicSimulation.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2013-2016, 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    ArmarXSimulation
 * @author     Valerij Wittenbeck, Nikolaus Vahrenkamp
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "PlatformUnitDynamicSimulation.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include "RobotAPI/libraries/core/FramedPose.h"
#include "RobotAPI/libraries/core/Pose.h"
#include <Eigen/Geometry>
 
#include <cmath>
 
using namespace armarx;
 
void PlatformUnitDynamicSimulation::onInitPlatformUnit()
{
    linearVelocityMax = getProperty<float>("LinearVelocityMax").getValue();
    linearVelocityMin = getProperty<float>("LinearVelocityMin").getValue();
    linearAccelerationMax = getProperty<float>("LinearAccelerationMax").getValue();
    angularVelocityMax = getProperty<float>("AngularVelocityMax").getValue();
    angularAccelerationMax = getProperty<float>("AngularAccelerationMax").getValue();
    platformOrientationOffset = getProperty<float>("PlatformOrientationOffset").getValue();
 
 
    robotName = getProperty<std::string>("RobotName").getValue();
    platformName = getProperty<std::string>("PlatformName").getValue();
 
    simulatorPrxName = getProperty<std::string>("SimulatorProxyName").getValue();
 
    positionalAccuracy = 0.02f; //in m
    orientationalAccuracy = 0.1f; //in rad
 
    intervalMs = getProperty<int>("IntervalMs").getValue();
    ARMARX_VERBOSE << "Starting platform unit dynamic simulation with " << intervalMs << " ms interval";
    simulationTask = new PeriodicTask<PlatformUnitDynamicSimulation>(this, &PlatformUnitDynamicSimulation::simulationFunction, intervalMs, false, "PlatformUnitSimulation");
    simulationTask->setDelayWarningTolerance(100);
 
    if (!robotName.empty())
    {
        std::string robotTopicName = "Simulator_Robot_";
        robotTopicName += robotName;
        ARMARX_INFO << "Simulator Robot topic: " << robotTopicName;
        usingTopic(robotTopicName);
    }
    else
    {
        ARMARX_WARNING << "No robot loaded...";
    }
 
    usingProxy(simulatorPrxName);
 
    currentRotation = 0.0f;
    targetRotation = 0.0f;
}
 
void PlatformUnitDynamicSimulation::onStopPlatformUnit()
{
    ARMARX_INFO << "Stopping platform unit";
    std::unique_lock<std::mutex> lock(currentPoseMutex);
    simulationTask->stop();
    simulatorPrx = nullptr;
}
 
void PlatformUnitDynamicSimulation::onStartPlatformUnit()
{
    platformControlMode = eUnknown;
 
 
    ARMARX_INFO << "Using simulator proxy: " << simulatorPrxName << flush;
    simulatorPrx = getProxy<SimulatorInterfacePrx>(simulatorPrxName);
 
    {
        std::unique_lock<std::mutex> lock(currentPoseMutex);
        updateCurrentPose(simulatorPrx->getRobotPose(robotName));
    }
    targetPosition = currentPosition;
    targetRotation = currentRotation;
 
    simulationTask->start();
}
 
 
void PlatformUnitDynamicSimulation::onExitPlatformUnit()
{
    simulationTask->stop();
}
 
 
Eigen::Vector3f PlatformUnitDynamicSimulation::calculateLinearVelocity(Eigen::Vector3f distance, Eigen::Vector3f curV)
{
    ARMARX_DEBUG << "calc platform vel, dist:" << distance.transpose() << ", curVel:" << curV.transpose() << ", linearVelocityMax:" << linearVelocityMax << ", linearAccelerationMax:" << linearAccelerationMax << flush;
 
    if (distance.norm() < positionalAccuracy)
    {
        ARMARX_DEBUG << "calc platform vel end: ZERO" << flush;
        return Eigen::Vector3f::Zero();
    }
 
    Eigen::Vector3f v = distance;
    //make sure there is a minimum velocity so the platform doesn't slow down too much
    v = clampf(v.norm(), linearVelocityMin, linearVelocityMax) * v.normalized();
 
    if (!std::isfinite(v(0)) || !std::isfinite(v(1)) || !std::isfinite(v(2)))
    {
 
        ARMARX_ERROR << "NAN in platform calculation, v=" << v << ", distance=" << distance << ",distance.normalized()=" << distance.normalized() << flush;
        return Eigen::Vector3f::Zero();
    }
 
    //throttle speed if the change of current to desired is too large (> max. accel)
    Eigen::Vector3f a = (v - curV);
 
    if ((v - curV).norm() > 1e-5)
    {
        a = std::min<float>(a.norm(), linearAccelerationMax) * a.normalized();
 
        if (!std::isfinite(a(0)) || !std::isfinite(a(1)) || !std::isfinite(a(2)))
        {
 
            ARMARX_ERROR << "NAN in platform calculation, a=" << a << ", (v - curV)=" << (v - curV) << ",(v - curV).normalized()=" << (v - curV).normalized() << flush;
            return Eigen::Vector3f::Zero();
        }
    }
 
    v = a + curV;
    ARMARX_DEBUG << "calc platform vel end: " << v.transpose() << flush;
    return v;
}
 
float PlatformUnitDynamicSimulation::calculateAngularVelocity(float source, float target, float curV)
{
    ARMARX_DEBUG << "calc platform ang vel, from:" << source << " to:" << target << ", curVel:" << curV << flush;
    const float M_2PI = 2 * M_PI;
    float cw = fmod(source - target + M_2PI, M_2PI); //ensure that angle is in [0,2PI]
    float ccw = fmod(target - source + M_2PI, M_2PI);
    float minDist = (cw < ccw) ? -cw : ccw;
 
    if (fabs(minDist) < orientationalAccuracy)
    {
        ARMARX_DEBUG << "calc platform ang vel end: ZERO" << flush;
        return 0.f;
    }
 
    float v = (minDist < 0) ? -angularVelocityMax : angularVelocityMax;
    float a = (v - curV);
    a = clampf(a, -angularAccelerationMax, angularAccelerationMax);
    v = a + curV;
    ARMARX_DEBUG << "calc platform ang vel end:" << v << flush;
    return v;
}
 
void PlatformUnitDynamicSimulation::stopPlatform(const Ice::Current& current)
{
    moveRelative(0, 0, 0, 500, 500, current);
}
 
static Eigen::Vector2f transformToLocalVelocity(float currentRotation, float velX, float velY)
{
    // Revert the rotation by rotating by the negative angle
    return Eigen::Rotation2Df(-currentRotation) * Eigen::Vector2f(velX, velY);
}
 
void PlatformUnitDynamicSimulation::simulationFunction()
{
    Eigen::Vector3f relTargetPosition;
    Eigen::Vector3f curLinVel;
    float curAngVel = 0.f;
    {
        std::unique_lock<std::mutex> lock(currentPoseMutex);
 
        if (!simulatorPrx)
        {
            return;
        }
 
        if (platformControlMode == ePositionControl)
        {
 
 
            ARMARX_DEBUG << "Platform sim step" << flush;
 
            SimulatedRobotState state = simulatorPrx->getRobotState(robotName);
 
            if (!state.hasRobot)
            {
                ARMARX_ERROR << deactivateSpam(5) << "No robot with name " << robotName;
                return;
            }
 
            Vector3Ptr p = Vector3Ptr::dynamicCast(state.pose->position);
            relTargetPosition << (targetPosition.x - p->x) / 1000.f, (targetPosition.y - p->y) / 1000.f, 0;
 
            Vector3BasePtr v = state.linearVelocity;
            Vector3BasePtr o = state.angularVelocity;
            currentVelocity.x = v->x;
            currentVelocity.y = v->y;
            currentRotationVelocity = o->z;
 
            // The currentVelocity is in the global frame, we need to report the velocity in the platform local frame
            Eigen::Vector2f localVelocity = transformToLocalVelocity(currentRotation, currentVelocity.x, currentVelocity.y);
            listenerPrx->begin_reportPlatformVelocity(localVelocity.x(), localVelocity.y(), currentRotationVelocity);
 
            curLinVel = Eigen::Vector3f(currentVelocity.x / 1000.f, currentVelocity.y / 1000.f, 0.f);
            curAngVel = currentRotationVelocity;
 
            const Eigen::Vector3f desiredLinVel = calculateLinearVelocity(relTargetPosition, curLinVel);
            const float desiredAngVel = calculateAngularVelocity(currentRotation, targetRotation, curAngVel);
 
            simulatorPrx->begin_setRobotLinearVelocity(robotName, platformName, new Vector3(desiredLinVel));
            simulatorPrx->begin_setRobotAngularVelocity(robotName, platformName, new Vector3(Eigen::Vector3f(0.f, 0.f, desiredAngVel)));
            ARMARX_DEBUG << "Platform sim step end" << flush;
        }
    }
}
 
void PlatformUnitDynamicSimulation::moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c)
{
    std::unique_lock<std::mutex> lock(currentPoseMutex);
    platformControlMode = ePositionControl;
    ARMARX_DEBUG << "Platform move to" << flush;
    targetPosition.x = targetPlatformPositionX;
    targetPosition.y = targetPlatformPositionY;
    targetRotation = targetPlatformRotation;
    this->positionalAccuracy = positionalAccuracy * .001f;
    this->orientationalAccuracy = orientationalAccuracy;
    // listenerPrx->begin_reportNewTargetPose(targetPosition.x, targetPosition.y, targetRotation);
    ARMARX_INFO << "New Target: " << targetPosition.x << " " << targetPosition.y << " " << targetRotation << flush;
}
 
void PlatformUnitDynamicSimulation::move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation, const Ice::Current& c)
{
    std::unique_lock<std::mutex> lock(currentPoseMutex);
    targetPlatformVelocityRotation *= 5;
 
    Eigen::Vector3f translationVel;
    platformControlMode = eVelocityControl;
    translationVel << targetPlatformVelocityX, targetPlatformVelocityY, 0;
    translationVel *= .003;
    //    ARMARX_INFO << deactivateSpam(1) << "New Platformvelocity: " << translationVel << "\nrot vel: " << targetPlatformVelocityRotation;
 
    simulatorPrx->setRobotLinearVelocityRobotRootFrame(robotName, platformName, new Vector3(translationVel));
    simulatorPrx->setRobotAngularVelocityRobotRootFrame(robotName, platformName, new Vector3(Eigen::Vector3f(0.f, 0.f, targetPlatformVelocityRotation)));
}
 
void PlatformUnitDynamicSimulation::moveRelative(float targetPlatformOffsetX, float targetPlatformOffsetY, float targetPlatformOffsetRotation, float positionalAccuracy, float orientationalAccuracy, const Ice::Current& c)
{
    float targetPositionX, targetPositionY, targetRotation;
    {
        std::unique_lock<std::mutex> lock(currentPoseMutex);
        platformControlMode = ePositionControl;
        ARMARX_DEBUG << "Platform move relative" << flush;
        targetPositionX = currentPosition.x + targetPlatformOffsetX;
        targetPositionY = currentPosition.y + targetPlatformOffsetY;
        targetRotation = currentRotation + targetPlatformOffsetRotation;
    }
    moveTo(targetPositionX, targetPositionY, targetRotation, positionalAccuracy, orientationalAccuracy);
}
 
void PlatformUnitDynamicSimulation::setMaxVelocities(float linearVelocity, float angularVelocity, const Ice::Current& c)
{
    linearVelocityMax = linearVelocity;
    angularVelocityMax = angularVelocity;
}
 
 
void PlatformUnitDynamicSimulation::updateCurrentPose(const PoseBasePtr& newPose)
{
    ARMARX_DEBUG << "Update new platform pose" << flush;
 
    if (!newPose)
    {
        return;
    }
 
    Vector3Ptr newPosition = Vector3Ptr::dynamicCast(newPose->position);
 
    if (newPosition)
    {
        currentPosition = Vector3(newPosition->toEigen());
    }
 
    QuaternionPtr newOrientation = QuaternionPtr::dynamicCast(newPose->orientation);
 
    if (newOrientation)
    {
        //axis is assumed to be z (fixed)
        Eigen::Matrix3f rotMat = newOrientation->toEigen();
        currentRotation =  copysign(std::acos(rotMat(0, 0)), std::asin(rotMat(1, 0)));
        //for some really odd reason, currentRotation can be nan when it should be 0
        currentRotation = std::isfinite(currentRotation) ? currentRotation : 0.0f;
 
        currentRotation += platformOrientationOffset;
    }
}
 
void PlatformUnitDynamicSimulation::updateCurrentVelocity(const Vector3BasePtr& translationVel, const Vector3BasePtr& rotationVel)
{
    ARMARX_DEBUG << "Update new platform vel" << flush;
 
    if (translationVel)
    {
        currentVelocity = *Vector3Ptr::dynamicCast(translationVel);
        ARMARX_DEBUG << "currentVelocity: " << currentVelocity.x << "," << currentVelocity.y << "," << currentVelocity.z;
    }
    if (rotationVel)
    {
        ARMARX_DEBUG << "currentRotationVelocity (only z is used): " << rotationVel->x << "," << rotationVel->x << "," << rotationVel->z;
        currentRotationVelocity = rotationVel->z;
    }
}
 
void PlatformUnitDynamicSimulation::reportState(SimulatedRobotState const& state, const Ice::Current&)
{
    std::unique_lock<std::mutex> lock(currentPoseMutex);
 
    // Pose
    updateCurrentPose(state.pose);
 
    ::armarx::TransformStamped transformStamped;
    
    FrameHeader header;
    header.parentFrame = GlobalFrame;
    header.frame = "root";
    header.agent = robotName;
    header.timestampInMicroSeconds = state.timestampInMicroSeconds;
 
    TransformStamped globalRobotPose;
    globalRobotPose.header = header;
    globalRobotPose.transform = armarx::fromIce(state.pose);
 
    globalPosePrx->reportGlobalRobotPose(globalRobotPose);
 
    // Velocity
    updateCurrentVelocity(state.linearVelocity, state.angularVelocity);
 
    if (platformControlMode != ePositionControl)
    {
        // The currentVelocity is in the global frame, we need to report the velocity in the platform local frame
        Eigen::Vector2f localVelocity = transformToLocalVelocity(currentRotation, currentVelocity.x, currentVelocity.y);
        listenerPrx->reportPlatformVelocity(localVelocity.x(), localVelocity.y(), currentRotationVelocity);
    }
}
 
PropertyDefinitionsPtr PlatformUnitDynamicSimulation::createPropertyDefinitions()
{
    auto def = PlatformUnit::createPropertyDefinitions();
 
    def->defineOptionalProperty<int>("IntervalMs", 100, "The time in milliseconds between two calls to the simulation method.");
 
    //assures that the platform still gets some amount of velocity when near target
    def->defineOptionalProperty<float>("LinearVelocityMin", 0.5, "Minimum Linear velocity of the platform (unit: m/sec).");
    def->defineOptionalProperty<float>("LinearVelocityMax", 1.5, "Maximum Linear velocity of the platform (unit: m/sec).");
    def->defineOptionalProperty<float>("LinearAccelerationMax", 0.9, "Maximum Linear Acceleration (unit: m/sec^2");
    def->defineOptionalProperty<float>("AngularVelocityMax", 0.8, "Maximum Angular velocity of the platform (unit: rad/sec).");
    def->defineOptionalProperty<float>("AngularAccelerationMax", 1.9, "Maximum Angular Acceleration (unit: rad/sec^2");
    def->defineOptionalProperty<float>("PlatformOrientationOffset", 0, "Rotation offset of the platform (unit: rad).");
    def->defineOptionalProperty<std::string>("RobotName", "Armar3", "Name of the Robot to use.");
    def->defineOptionalProperty<std::string>("PlatformName", "Platform", "Name of the Platform node to use.");
    def->defineOptionalProperty<std::string>("SimulatorProxyName", "Simulator", "Name of the simulator proxy to use.");
 
    return def;
}