IMUSimulation.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::ArmarXObjects::IMUSimulation
 * @author     Markus Grotz ( markus dot grotz at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "IMUSimulation.h"
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
using namespace armarx;
 
IMUSimulationPropertyDefinitions::IMUSimulationPropertyDefinitions(std::string prefix) :
    InertialMeasurementUnitPropertyDefinitions(prefix)
{
    defineOptionalProperty<std::string>("nodeName", "HeadIMU", "");
    defineOptionalProperty<std::string>(
        "SimulatorName", "Simulator", "Name of the simulator component that should be used");
    defineOptionalProperty<std::string>("RobotStateComponentName",
                                        "RobotStateComponent",
                                        "Name of the robot state component that should be used");
}
 
void
IMUSimulation::onInitIMU()
{
    usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
 
    nodeName = getProperty<std::string>("nodeName").getValue();
 
    sensorTask =
        new PeriodicTask<IMUSimulation>(this, &IMUSimulation::frameAcquisitionTaskLoop, 50);
 
    usingProxy(getProperty<std::string>("SimulatorName").getValue());
}
 
void
IMUSimulation::onStartIMU()
{
    robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(
        getProperty<std::string>("RobotStateComponentName").getValue());
 
    simulator =
        getProxy<SimulatorInterfacePrx>(getProperty<std::string>("SimulatorName").getValue());
 
    timestamp = 0;
 
    sensorTask->start();
}
 
void
IMUSimulation::onExitIMU()
{
    sensorTask->stop();
}
 
void
IMUSimulation::frameAcquisitionTaskLoop()
{
    TimestampVariantPtr now = TimestampVariant::nowPtr();
 
    auto robotLinearVelocityAsync =
        simulator->begin_getRobotLinearVelocity(robotStateComponent->getRobotName(), nodeName);
    auto robotAngularVelocityAsync =
        simulator->begin_getRobotAngularVelocity(robotStateComponent->getRobotName(), nodeName);
 
    SharedRobotNodeInterfacePrx robotNode =
        robotStateComponent->getSynchronizedRobot()->getRobotNode(nodeName);
    FramedPoseBasePtr poseInRootFrame = robotNode->getGlobalPose();
 
    Eigen::Quaternionf currentOrientation =
        QuaternionPtr::dynamicCast(poseInRootFrame->orientation)->toEigenQuaternion();
    Eigen::Vector3f currentPosition =
        Vector3Ptr::dynamicCast(poseInRootFrame->position)->toEigen() / 1000.0;
 
    float deltaTime = (now->timestamp - timestamp) * std::pow(10, -6);
 
    Vector3BasePtr robotLinearVelocity =
        simulator->end_getRobotLinearVelocity(robotLinearVelocityAsync);
    Vector3BasePtr robotAngularVelocity =
        simulator->end_getRobotAngularVelocity(robotAngularVelocityAsync);
 
 
    // convert to SI units
    Eigen::Vector3f currentLinearVelocity =
        orientation.toRotationMatrix().transpose() *
        Vector3Ptr::dynamicCast(robotLinearVelocity)->toEigen() / 1000.0f;
    Eigen::Vector3f angularVelocity = orientation.toRotationMatrix().transpose() *
                                      Vector3Ptr::dynamicCast(robotAngularVelocity)->toEigen();
 
 
    if (timestamp)
    {
        Eigen::Vector3f acceleration = (currentLinearVelocity - linearVelocity) / deltaTime;
 
        IMUData data;
 
        data.acceleration.push_back(acceleration(0));
        data.acceleration.push_back(acceleration(1));
        data.acceleration.push_back(acceleration(2));
 
        data.gyroscopeRotation.push_back(angularVelocity(0));
        data.gyroscopeRotation.push_back(angularVelocity(1));
        data.gyroscopeRotation.push_back(angularVelocity(2));
 
        data.magneticRotation.push_back(0.0);
        data.magneticRotation.push_back(0.0);
        data.magneticRotation.push_back(0.0);
 
        data.orientationQuaternion.push_back(currentOrientation.w());
        data.orientationQuaternion.push_back(currentOrientation.x());
        data.orientationQuaternion.push_back(currentOrientation.y());
        data.orientationQuaternion.push_back(currentOrientation.z());
 
        IMUTopicPrx->reportSensorValues(getName(), "name", data, now);
    }
 
    orientation = currentOrientation;
    position = currentPosition;
    timestamp = now->timestamp;
    linearVelocity = currentLinearVelocity;
}
 
std::string
IMUSimulation::GetDefaultName()
{
    return "IMUSimulation";
}
 
std::string
IMUSimulation::getDefaultName() const
{
    return GetDefaultName();
}
 
armarx::PropertyDefinitionsPtr
IMUSimulation::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new IMUSimulationPropertyDefinitions(getConfigIdentifier()));
}
 
ARMARX_REGISTER_COMPONENT_EXECUTABLE(::armarx::IMUSimulation, ::armarx::IMUSimulation::GetDefaultName());