RobotPoseUnitDynamicSimulation.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::units
 * @author     Nikolaus Vahrenkamp
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "RobotPoseUnitDynamicSimulation.h"
 
#include <ArmarXCore/core/system/Synchronization.h>
 
#include <Eigen/Geometry>
 
#include <cmath>
 
#include <VirtualRobot/MathTools.h>
 
using namespace armarx;
 
void RobotPoseUnitDynamicSimulation::onInitRobotPoseUnit()
{
    //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();
 
    robotName = getProperty<std::string>("RobotName").getValue();
    //robotRootNodeName = getProperty<std::string>("RobotRootNodeName").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 RobotPose unit dynamic simulation with " << intervalMs << " ms interval";
    simulationTask = new PeriodicTask<RobotPoseUnitDynamicSimulation>(this, &RobotPoseUnitDynamicSimulation::simulationFunction, intervalMs, false, "RobotPoseUnitSimulation");
    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);
 
    Eigen::Matrix4f m;
    m.setIdentity();
    currentPose = Pose(m);
    targetPose = Pose(m);
}
 
void RobotPoseUnitDynamicSimulation::onStopRobotPoseUnit()
{
    ARMARX_INFO << "Stopping RobotPose unit";
    std::unique_lock lock(currentPoseMutex);
    simulationTask->stop();
    simulatorPrx = nullptr;
}
 
void RobotPoseUnitDynamicSimulation::onStartRobotPoseUnit()
{
    RobotPoseControlMode = eUnknown;
 
    ARMARX_INFO << "Using simulator proxy: " << simulatorPrxName;
    simulatorPrx = getProxy<SimulatorInterfacePrx>(simulatorPrxName);
    PoseBasePtr p = simulatorPrx->getRobotPose(robotName);
    setCurrentPose(p);
    targetPose = currentPose;
 
    simulationTask->start();
}
 
 
void RobotPoseUnitDynamicSimulation::onExitRobotPoseUnit()
{
    simulationTask->stop();
}
 
void RobotPoseUnitDynamicSimulation::stopMovement(const Ice::Current& current)
{
    Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
    PosePtr p(new Pose(m));
 
    moveRelative(p, 500, 500, current);
}
 
 
void RobotPoseUnitDynamicSimulation::simulationFunction()
{
    // todo: implement velocity control....
 
    {
        std::unique_lock lock(currentPoseMutex);
 
        if (!simulatorPrx)
        {
            return;
        }
    }
}
 
void RobotPoseUnitDynamicSimulation::moveTo(const PoseBasePtr& targetPose, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c)
{
    std::unique_lock lock(currentPoseMutex);
    RobotPoseControlMode = ePositionControl;
    ARMARX_DEBUG << "RobotPose move to" << flush;
    this->targetPose = *(PosePtr::dynamicCast(targetPose));
    this->positionalAccuracy = positionalAccuracy * .001f;
    this->orientationalAccuracy = orientationalAccuracy;
    listenerPrx->begin_reportNewTargetPose(targetPose);
    ARMARX_LOG << deactivateSpam(5) << "New Target: " << targetPose->position->x << " " << targetPose->position->y << " " << targetPose->position->z;
    if (simulatorPrx)
    {
        //simulatorPrx->begin_setRobotPose(robotName, targetPose);
        simulatorPrx->setRobotPose(robotName, targetPose);
    }
}
 
Eigen::Vector3f RobotPoseUnitDynamicSimulation::getRPY(PoseBasePtr pose)
{
    Eigen::Quaternionf q(QuaternionPtr::dynamicCast(pose->orientation)->toEigen());
    Eigen::Matrix4f m = VirtualRobot::MathTools::quat2eigen4f(q.x(), q.y(), q.z(), q.w());
    Eigen::Vector3f rpy;
    VirtualRobot::MathTools::eigen4f2rpy(m, rpy);
    return rpy;
}
 
void RobotPoseUnitDynamicSimulation::move(const PoseBasePtr& targetVelocity, const Ice::Current& c)
{
    std::unique_lock lock(currentPoseMutex);
    ARMARX_INFO << deactivateSpam(1) << "NYI";
}
 
void RobotPoseUnitDynamicSimulation::moveRelative(const PoseBasePtr& relativeTarget, float positionalAccuracy, float orientationalAccuracy, const Ice::Current& c)
{
    PosePtr p;
    {
        std::unique_lock lock(currentPoseMutex);
        RobotPoseControlMode = ePositionControl;
        Eigen::Matrix4f pose;
        Eigen::Matrix4f cur = currentPose.toEigen();
        Eigen::Matrix4f rel = PosePtr::dynamicCast(relativeTarget)->toEigen();
        pose = cur * rel;
        p = PosePtr(new Pose(pose));
        ARMARX_DEBUG << "RobotPose move relative" << flush;
    }
    moveTo(p, positionalAccuracy, orientationalAccuracy);
}
 
void RobotPoseUnitDynamicSimulation::setMaxVelocities(float linearVelocity, float angularVelocity, const Ice::Current& c)
{
    //linearVelocityMax = linearVelocity;
    //angularVelocityMax = angularVelocity;
}
 
 
void RobotPoseUnitDynamicSimulation::updateCurrentPose(const PoseBasePtr& newPose)
{
    ARMARX_DEBUG << "Update new RobotPose pose" << flush;
 
    if (!newPose)
    {
        return;
    }
 
    currentPose = *PosePtr::dynamicCast(newPose);
}
 
void RobotPoseUnitDynamicSimulation::reportState(SimulatedRobotState const& state, const Ice::Current&)
{
    setCurrentPose(state.pose);
}
 
void RobotPoseUnitDynamicSimulation::setCurrentPose(const PoseBasePtr& newPose)
{
    std::unique_lock lock(currentPoseMutex);
    updateCurrentPose(newPose);
    try
    {
        PosePtr p(new Pose(currentPose));
        listenerPrx->begin_reportRobotPose(p);
    }
    catch (...)
    {
    }
}
 
PropertyDefinitionsPtr RobotPoseUnitDynamicSimulation::createPropertyDefinitions()
{
    return PropertyDefinitionsPtr(
               new RobotPoseUnitDynamicSimulationPropertyDefinitions(getConfigIdentifier()));
}