KinematicUnitSimulation.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-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    ArmarXCore::units
 * @author     Christian Boege (boege at kit dot edu)
 * @date       2011
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "KinematicUnitSimulation.h"
 
#include <algorithm>
 
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/VirtualRobotException.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/util/algorithm.h>
 
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
 
using namespace armarx;
 
KinematicUnitSimulationPropertyDefinitions::KinematicUnitSimulationPropertyDefinitions(
    std::string prefix) :
    KinematicUnitPropertyDefinitions(prefix)
{
    defineOptionalProperty<int>(
        "IntervalMs", 10, "The time in milliseconds between two calls to the simulation method.");
    defineOptionalProperty<float>(
        "Noise", 0.0f, "Gaussian noise is added to the velocity. Value in Degree");
    defineOptionalProperty<bool>("UsePDControllerForJointControl",
                                 true,
                                 "If true a PD controller is also used in Position Mode instead of "
                                 "setting the joint angles instantly");
    defineOptionalProperty<float>("Kp", 3, "proportional gain of the PID position controller.");
    defineOptionalProperty<float>("Ki", 0.001f, "integral gain of the PID position controller.");
    defineOptionalProperty<float>("Kd", 0.0, "derivative gain of the PID position controller.");
}
 
void
KinematicUnitSimulation::onInitKinematicUnit()
{
    ARMARX_TRACE;
    ARMARX_INFO << "Init Simulation";
    usePDControllerForPosMode = getProperty<bool>("UsePDControllerForJointControl").getValue();
    for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin();
         it != robotNodes.end();
         it++)
    {
        std::string jointName = (*it)->getName();
        jointInfos[jointName].limitLo = (*it)->getJointLimitLo();
        jointInfos[jointName].limitHi = (*it)->getJointLimitHi();
 
        if ((*it)->isRotationalJoint())
        {
            if (jointInfos[jointName].limitHi - jointInfos[jointName].limitLo >= float(M_PI * 2))
            {
                jointInfos[jointName].reset();
            }
        }
 
        ARMARX_VERBOSE << jointName << " with limits: lo: " << jointInfos[jointName].limitLo
                       << " hi: " << jointInfos[jointName].limitHi;
    }
    ARMARX_TRACE;
    {
        // duplicate the loop because of locking
        std::unique_lock lock(jointStatesMutex);
 
        // initialize JoinStates
        for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin();
             it != robotNodes.end();
             it++)
        {
            std::string jointName = (*it)->getName();
            jointStates[jointName] = KinematicUnitSimulationJointState();
            jointStates[jointName].init();
            positionControlPIDController[jointName] =
                PIDControllerPtr(new PIDController(getProperty<float>("Kp").getValue(),
                                                   getProperty<float>("Ki").getValue(),
                                                   getProperty<float>("Kd").getValue()));
        }
    }
    ARMARX_TRACE;
    noise = getProperty<float>("Noise").getValue() / 180.f * M_PI;
    if (noise > 0)
    {
        // Aborts due to assertion when noise <= 0.
        distribution = std::normal_distribution<double>(0, noise);
    }
    intervalMs = getProperty<int>("IntervalMs").getValue();
    ARMARX_VERBOSE << "Starting kinematic unit simulation with " << intervalMs << " ms interval";
    simulationTask =
        new PeriodicTask<KinematicUnitSimulation>(this,
                                                  &KinematicUnitSimulation::simulationFunction,
                                                  intervalMs,
                                                  false,
                                                  "KinematicUnitSimulation");
}
 
void
KinematicUnitSimulation::onStartKinematicUnit()
{
    lastExecutionTime = TimeUtil::GetTime();
    simulationTask->start();
}
 
void
KinematicUnitSimulation::onExitKinematicUnit()
{
    simulationTask->stop();
}
 
void
KinematicUnitSimulation::simulationFunction()
{
    // the time it took until this method was called again
    double timeDeltaInSeconds =
        (TimeUtil::GetTime() - lastExecutionTime).toMilliSecondsDouble() / 1000.0;
    lastExecutionTime = TimeUtil::GetTime();
 
    // name value maps for reporting
    std::lock_guard guard{sensorDataMutex};
    sensorAngles.clear();
    sensorVelocities.clear();
 
    bool aPosValueChanged = false;
    bool aVelValueChanged = false;
    auto signedMin = [](float newValue, float minAbsValue)
    { return std::copysign(std::min<float>(fabs(newValue), fabs(minAbsValue)), newValue); };
 
    {
 
        std::unique_lock lock(jointStatesMutex);
 
        for (KinematicUnitSimulationJointStates::iterator it = jointStates.begin();
             it != jointStates.end();
             it++)
        {
 
            float newAngle = 0.0f;
            float newVelocity = 0.0f;
            KinematicUnitSimulationJointInfo& jointInfo = jointInfos[it->first];
            // calculate joint positions if joint is in velocity control mode
            if (it->second.controlMode == eVelocityControl)
            {
                double change = (it->second.velocity * timeDeltaInSeconds);
                newVelocity = it->second.velocity;
                if (noise > 0)
                {
                    double randomNoiseValue = distribution(generator);
                    change *= 1 + randomNoiseValue;
                }
                newAngle = it->second.angle + change;
                // check if velocities have changed
                if (std::fabs(previousJointStates[it->first].velocity - newVelocity) > 0.00000001f)
                {
                    aVelValueChanged = true;
                }
            }
            else if (it->second.controlMode == ePositionControl)
            {
                if (!usePDControllerForPosMode)
                {
                    newAngle = it->second.angle;
                }
                else
                {
                    PIDControllerPtr pid = positionControlPIDController[it->first];
                    if (pid)
                    {
                        float velValue =
                            (it->second.velocity != 0.0f
                                 ? signedMin(pid->getControlValue(), it->second.velocity)
                                 : pid->getControlValue()); //restrain to max velocity
                        if (noise > 0)
                        {
                            velValue *= 1 + distribution(generator);
                        }
 
                        pid->update(it->second.angle);
                        newAngle = it->second.angle + velValue * timeDeltaInSeconds;
                        if (fabs(previousJointStates[it->first].velocityActual - velValue) >
                            0.00000001)
                        {
                            aVelValueChanged = true;
                        }
                        it->second.velocityActual = newVelocity = velValue;
                    }
                }
            }
 
            // constrain the angle
            if (!jointInfo.continuousJoint())
            {
                float maxAngle = (jointInfo.hasLimits()) ? jointInfo.limitHi : M_PI;
                float minAngle = (jointInfo.hasLimits()) ? jointInfo.limitLo : -M_PI;
 
                newAngle = std::max<float>(newAngle, minAngle);
                newAngle = std::min<float>(newAngle, maxAngle);
            }
 
            // Check if joint existed before
            KinematicUnitSimulationJointStates::iterator itPrev =
                previousJointStates.find(it->first);
 
            if (itPrev == previousJointStates.end())
            {
                aPosValueChanged = aVelValueChanged = true;
            }
 
            // check if the angle has changed
            if (fabs(previousJointStates[it->first].angle - newAngle) > 0.00000001)
            {
                aPosValueChanged = true;
                // set the new joint angle locally for the next simulation iteration
                it->second.angle = newAngle;
            }
 
 
            if (jointInfo.continuousJoint())
            {
                if (newAngle < 0)
                {
                    newAngle = fmod(newAngle - M_PI, 2 * M_PI) + M_PI;
                }
                else
                {
                    newAngle = fmod(newAngle + M_PI, 2 * M_PI) - M_PI;
                }
            }
 
            sensorAngles[it->first] = newAngle;
            sensorVelocities[it->first] = newVelocity;
        }
 
        previousJointStates = jointStates;
    }
    auto timestamp = TimeUtil::GetTime().toMicroSeconds();
    listenerPrx->reportJointAngles(sensorAngles, timestamp, aPosValueChanged);
    listenerPrx->reportJointVelocities(sensorVelocities, timestamp, aVelValueChanged);
}
 
void
KinematicUnitSimulation::switchControlMode(const NameControlModeMap& targetJointModes,
                                           const Ice::Current& c)
{
    bool aValueChanged = false;
    NameControlModeMap changedControlModes;
    {
        std::unique_lock lock(jointStatesMutex);
 
        for (NameControlModeMap::const_iterator it = targetJointModes.begin();
             it != targetJointModes.end();
             it++)
        {
            // target values
            std::string targetJointName = it->first;
            ControlMode targetControlMode = it->second;
 
            KinematicUnitSimulationJointStates::iterator iterJoints =
                jointStates.find(targetJointName);
 
            // check whether there is a joint with this name and set joint angle
            if (iterJoints != jointStates.end())
            {
                if (iterJoints->second.controlMode != targetControlMode)
                {
                    aValueChanged = true;
                }
 
                iterJoints->second.controlMode = targetControlMode;
                changedControlModes.insert(*it);
            }
            else
            {
                ARMARX_WARNING << "Could not find joint with name " << targetJointName;
            }
        }
    }
    // only report control modes which have been changed
    listenerPrx->reportControlModeChanged(
        changedControlModes, aValueChanged, TimeUtil::GetTime().toMicroSeconds());
}
 
void
KinematicUnitSimulation::setJointAngles(const NameValueMap& targetJointAngles,
                                        const Ice::Current& c)
{
 
    std::unique_lock lock(jointStatesMutex);
    // name value maps for reporting
    NameValueMap actualJointAngles;
    bool aValueChanged = false;
 
    for (NameValueMap::const_iterator it = targetJointAngles.begin(); it != targetJointAngles.end();
         it++)
    {
        // target values
        std::string targetJointName = it->first;
        float targetJointAngle = it->second;
 
        // check whether there is a joint with this name and set joint angle
        KinematicUnitSimulationJointStates::iterator iterJoints = jointStates.find(targetJointName);
 
        if (iterJoints != jointStates.end())
        {
            if (fabs(iterJoints->second.angle - targetJointAngle) > 0)
            {
                aValueChanged = true;
            }
            KinematicUnitSimulationJointInfo& jointInfo = jointInfos[targetJointName];
            if (jointInfo.hasLimits() && !jointInfo.continuousJoint())
            {
                targetJointAngle = std::max<float>(targetJointAngle, jointInfo.limitLo);
                targetJointAngle = std::min<float>(targetJointAngle, jointInfo.limitHi);
            }
 
            if (!usePDControllerForPosMode)
            {
                iterJoints->second.angle = targetJointAngle;
                actualJointAngles[targetJointName] = iterJoints->second.angle;
            }
            else
            {
                PIDControllerPtr pid = positionControlPIDController[it->first];
                if (pid)
                {
                    pid->reset();
                    if (jointInfo.continuousJoint())
                    {
                        float delta = targetJointAngle - iterJoints->second.angle;
                        float signedSmallestDelta = atan2(sin(delta), cos(delta));
                        targetJointAngle = iterJoints->second.angle + signedSmallestDelta;
                    }
 
                    pid->setTarget(targetJointAngle);
                }
            }
        }
        else
        {
            ARMARX_WARNING << deactivateSpam(1) << "Joint '" << targetJointName << "' not found!";
        }
    }
 
    if (aValueChanged)
    {
        listenerPrx->reportJointAngles(
            actualJointAngles, aValueChanged, TimeUtil::GetTime().toMicroSeconds());
    }
}
 
void
KinematicUnitSimulation::setJointVelocities(const NameValueMap& targetJointVelocities,
                                            const Ice::Current&)
{
    std::unique_lock lock(jointStatesMutex);
    NameValueMap actualJointVelocities;
    bool aValueChanged = false;
 
    for (NameValueMap::const_iterator it = targetJointVelocities.begin();
         it != targetJointVelocities.end();
         it++)
    {
        // target values
        std::string targetJointName = it->first;
        float targetJointVelocity = it->second;
 
        KinematicUnitSimulationJointStates::iterator iterJoints = jointStates.find(targetJointName);
 
        // check whether there is a joint with this name and set joint angle
        if (iterJoints != jointStates.end())
        {
            if (fabs(iterJoints->second.velocity - targetJointVelocity) > 0)
            {
                aValueChanged = true;
            }
 
            iterJoints->second.velocity = targetJointVelocity;
            actualJointVelocities[targetJointName] = iterJoints->second.velocity;
        }
    }
 
    if (aValueChanged)
    {
        listenerPrx->reportJointVelocities(
            actualJointVelocities, aValueChanged, TimeUtil::GetTime().toMicroSeconds());
    }
}
 
void
KinematicUnitSimulation::setJointTorques(const NameValueMap& targetJointTorques,
                                         const Ice::Current&)
{
    std::unique_lock lock(jointStatesMutex);
    NameValueMap actualJointTorques;
    bool aValueChanged = false;
 
    for (NameValueMap::const_iterator it = targetJointTorques.begin();
         it != targetJointTorques.end();
         it++)
    {
        // target values
        std::string targetJointName = it->first;
        float targetJointTorque = it->second;
 
        KinematicUnitSimulationJointStates::iterator iterJoints = jointStates.find(targetJointName);
 
        // check whether there is a joint with this name and set joint angle
        if (iterJoints != jointStates.end())
        {
            if (fabs(iterJoints->second.torque - targetJointTorque) > 0)
            {
                aValueChanged = true;
            }
 
            iterJoints->second.torque = targetJointTorque;
            actualJointTorques[targetJointName] = iterJoints->second.torque;
        }
    }
 
    if (aValueChanged)
    {
        listenerPrx->reportJointTorques(
            actualJointTorques, aValueChanged, TimeUtil::GetTime().toMicroSeconds());
    }
}
 
NameControlModeMap
KinematicUnitSimulation::getControlModes(const Ice::Current&)
{
    NameControlModeMap result;
 
    for (KinematicUnitSimulationJointStates::iterator it = jointStates.begin();
         it != jointStates.end();
         it++)
    {
        result[it->first] = it->second.controlMode;
    }
 
    return result;
}
 
void
KinematicUnitSimulation::setJointAccelerations(const NameValueMap& targetJointAccelerations,
                                               const Ice::Current&)
{
    (void)targetJointAccelerations;
}
 
void
KinematicUnitSimulation::setJointDecelerations(const NameValueMap& targetJointDecelerations,
                                               const Ice::Current&)
{
    (void)targetJointDecelerations;
}
 
void
KinematicUnitSimulation::stop(const Ice::Current& c)
{
 
    std::unique_lock lock(jointStatesMutex);
 
    for (KinematicUnitSimulationJointStates::iterator it = jointStates.begin();
         it != jointStates.end();
         it++)
    {
        it->second.velocity = 0;
    }
 
    SensorActorUnit::stop(c);
}
 
PropertyDefinitionsPtr
KinematicUnitSimulation::createPropertyDefinitions()
{
    return PropertyDefinitionsPtr(
        new KinematicUnitSimulationPropertyDefinitions(getConfigIdentifier()));
}
 
NameValueMap
KinematicUnitSimulation::getJointAngles(const Ice::Current& c) const
{
    std::lock_guard<std::mutex> guard{sensorDataMutex};
    return sensorAngles;
}
 
NameValueMap
KinematicUnitSimulation::getJointVelocities(const Ice::Current& c) const
{
    std::lock_guard<std::mutex> guard{sensorDataMutex};
    return sensorVelocities;
}
 
Ice::StringSeq
KinematicUnitSimulation::getJoints(const Ice::Current& c) const
{
    std::lock_guard<std::mutex> guard{sensorDataMutex};
    return getMapKeys(sensorAngles);
}
 
DebugInfo
KinematicUnitSimulation::getDebugInfo(const Ice::Current& c) const
{
    std::lock_guard g{jointStatesMutex};
 
    DebugInfo debugInfo;
    for (const auto& [jointName, info] : jointStates)
    {
        debugInfo.jointAngles[jointName] = info.angle;
        debugInfo.jointVelocities[jointName] = info.velocity;
 
        debugInfo.jointMotorTemperatures[jointName] = info.temperature;
 
        debugInfo.jointTorques[jointName] = info.torque;
        debugInfo.jointModes[jointName] = info.controlMode;
    };
 
    return debugInfo;
}
 
bool
mapEntryEqualsString(std::pair<std::string, KinematicUnitSimulationJointState> iter,
                     std::string compareString)
{
    return (iter.first == compareString);
}
 
void
KinematicUnitSimulation::requestJoints(const Ice::StringSeq& joints, const Ice::Current& c)
{
    // if one of the joints belongs to this unit, lock access to this unit for other components except for the requesting one
    KinematicUnitSimulationJointStates::const_iterator it = std::find_first_of(
        jointStates.begin(), jointStates.end(), joints.begin(), joints.end(), mapEntryEqualsString);
 
    if (jointStates.end() != it)
    {
        KinematicUnit::request(c);
    }
}
 
void
KinematicUnitSimulation::releaseJoints(const Ice::StringSeq& joints, const Ice::Current& c)
{
    // if one of the joints belongs to this unit, unlock access
    KinematicUnitSimulationJointStates::const_iterator it = std::find_first_of(
        jointStates.begin(), jointStates.end(), joints.begin(), joints.end(), mapEntryEqualsString);
 
    if (jointStates.end() != it)
    {
        KinematicUnit::release(c);
    }
}