KinematicUnitDynamicSimulation.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::KinematicUnitDynamicSimulation
 * @author     Nikolaus ( vahrenkamp at kit dot edu )
 * @date       2013
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "KinematicUnitDynamicSimulation.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>
 
using namespace armarx;
 
void
KinematicUnitDynamicSimulation::onInitKinematicUnit()
{
    ARMARX_VERBOSE << "Init with RNS " << robotNodeSetName << std::endl;
 
    for (auto& robotNode : robotNodes)
    {
        std::string jointName = robotNode->getName();
        ARMARX_VERBOSE << "* " << jointName << flush;
        nodeNames.insert(jointName);
    }
 
    if (robot)
    {
        robotTopicName = "Simulator_Robot_";
        robotName =
            hasProperty("RobotName") ? getProperty<std::string>("RobotName") : robot->getName();
        robotTopicName += robotName;
        ARMARX_INFO << "Simulator Robot topic: " << robotTopicName << std::endl;
        usingTopic(robotTopicName);
    }
    else
    {
        ARMARX_WARNING << "No robot loaded..." << std::endl;
    }
 
    mapVelToPosVel = getProperty<bool>("MapVelocityToPositionVelocity").getValue();
 
    simulatorPrxName = getProperty<std::string>("SimulatorName").getValue();
    usingProxy(simulatorPrxName);
}
 
void
KinematicUnitDynamicSimulation::onStartKinematicUnit()
{
    // subscribe topic in order to receive the simulator reports
    ARMARX_INFO << "Using simulator proxy: " << simulatorPrxName << flush;
    simulatorPrx = getProxy<SimulatorInterfacePrx>(simulatorPrxName);
 
    // periodic task setup
    cycleTime = 1000.f / getProperty<float>("ControlFrequency").getValue();
 
    if (execTask)
    {
        execTask->stop();
    }
 
    execTask = new PeriodicTask<KinematicUnitDynamicSimulation>(
        this,
        &KinematicUnitDynamicSimulation::periodicExec,
        cycleTime,
        false,
        "KinematicUnitDynamicSimulation");
    execTask->start();
    execTask->setDelayWarningTolerance(100);
}
 
void
KinematicUnitDynamicSimulation::onExitKinematicUnit()
{
    if (execTask)
    {
        execTask->stop();
    }
}
 
void
KinematicUnitDynamicSimulation::reportState(SimulatedRobotState const& state, const Ice::Current&)
{
    NameValueMap angleValues;
    NameValueMap velocitiyValues;
    NameValueMap torqueValues;
 
    {
        // Calculate results with lock, publish later without lock
        std::scoped_lock scoped_lock(accessMutex);
 
        // Joint angles
        if (state.jointAngles.size() > 0)
        {
            // check which joints are covered
            for (const auto& actualJointAngle : state.jointAngles)
            {
                if (nodeNames.find(actualJointAngle.first) != nodeNames.end())
                {
                    angleValues[actualJointAngle.first] = actualJointAngle.second;
                }
 
                currentJointValues[actualJointAngle.first] = actualJointAngle.second;
            }
        }
 
        // Joint velocities
        // check which jonts are covered
        for (const auto& actualVelocity : state.jointVelocities)
        {
            if (nodeNames.find(actualVelocity.first) != nodeNames.end())
            {
                velocitiyValues[actualVelocity.first] = actualVelocity.second;
            }
        }
 
        // Joint torques
        // check which jonts are covered
        for (const auto& actualJointTorque : state.jointTorques)
        {
            if (nodeNames.find(actualJointTorque.first) != nodeNames.end())
            {
                torqueValues[actualJointTorque.first] = actualJointTorque.second;
            }
        }
    }
 
    // FIXME: Use Ice batching to optimize
    if (angleValues.size() > 0)
    {
        listenerPrx->reportJointAngles(angleValues, state.timestampInMicroSeconds, true);
    }
    if (velocitiyValues.size() > 0)
    {
        listenerPrx->reportJointVelocities(velocitiyValues, state.timestampInMicroSeconds, true);
    }
    if (torqueValues.size() > 0)
    {
        listenerPrx->reportJointTorques(torqueValues, state.timestampInMicroSeconds, true);
    }
}
 
NameControlModeMap
KinematicUnitDynamicSimulation::getControlModes(const Ice::Current& c)
{
    NameControlModeMap result;
 
    for (const auto& nodeName : nodeNames)
    {
        auto c = currentControlModes.find(nodeName);
 
        // if not specified, we assume position control
        if (c == currentControlModes.end())
        {
            result[nodeName] = ePositionControl;
        }
        else
        {
            result[nodeName] = c->second;
        }
    }
 
    return result;
}
 
void
KinematicUnitDynamicSimulation::switchControlMode(const NameControlModeMap& targetJointModes,
                                                  const Ice::Current& c)
{
    std::scoped_lock scoped_lock(accessMutex);
 
    //currentControlModes = targetJointModes;
    NameControlModeMap::const_iterator itAng = targetJointModes.begin();
 
    while (itAng != targetJointModes.end())
    {
        currentControlModes[itAng->first] = itAng->second;
        itAng++;
    }
 
    listenerPrx->reportControlModeChanged(
        targetJointModes, IceUtil::Time::now().toMicroSeconds(), true);
}
 
void
KinematicUnitDynamicSimulation::setJointAngles(const NameValueMap& targetJointAngles,
                                               const Ice::Current& c)
{
    std::scoped_lock scoped_lock(accessMutex);
 
 
    // store current joint angles
    NameValueMap::const_iterator itAng = targetJointAngles.begin();
 
    while (itAng != targetJointAngles.end())
    {
        currentJointTargets[itAng->first] = itAng->second;
        itAng++;
    }
}
 
void
KinematicUnitDynamicSimulation::setJointVelocities(const NameValueMap& targetJointVelocities,
                                                   const Ice::Current& c)
{
    std::scoped_lock scoped_lock(accessMutex);
    NameValueMap::const_iterator itVel = targetJointVelocities.begin();
 
    while (itVel != targetJointVelocities.end())
    {
        currentJointVelTargets[itVel->first] = itVel->second;
        itVel++;
    }
}
 
void
KinematicUnitDynamicSimulation::setJointTorques(const NameValueMap& targetJointTorques,
                                                const Ice::Current& c)
{
    std::scoped_lock scoped_lock(accessMutex);
 
    // store current joint torques
    NameValueMap::const_iterator itAng = targetJointTorques.begin();
 
    while (itAng != targetJointTorques.end())
    {
        currentJointTorqueTargets[itAng->first] = itAng->second;
        itAng++;
    }
}
 
void
KinematicUnitDynamicSimulation::setJointAccelerations(const NameValueMap& targetJointAccelerations,
                                                      const Ice::Current& c)
{
}
 
void
KinematicUnitDynamicSimulation::setJointDecelerations(const NameValueMap& targetJointDecelerations,
                                                      const Ice::Current& c)
{
}
 
void
KinematicUnitDynamicSimulation::requestJoints(const Ice::StringSeq& joints, const Ice::Current& c)
{
    std::scoped_lock scoped_lock(accessMutex);
 
    // if one of the joints belongs to this unit, lock access to this unit for other components except for the requesting one
    for (const auto& joint : joints)
    {
        if (nodeNames.find(joint) != nodeNames.end())
        {
            KinematicUnit::request(c);
            return;
        }
    }
}
 
void
KinematicUnitDynamicSimulation::releaseJoints(const Ice::StringSeq& joints, const Ice::Current& c)
{
    std::scoped_lock scoped_lock(accessMutex);
 
    // if one of the joints belongs to this unit, unlock access
    for (const auto& joint : joints)
    {
        if (nodeNames.find(joint) != nodeNames.end())
        {
            KinematicUnit::release(c);
            return;
        }
    }
}
 
void
KinematicUnitDynamicSimulation::periodicExec()
{
    if (!simulatorPrx)
    {
        return;
    }
 
    float loopTime = (float)execTask->getInterval() / 1000.0f; // in s
    float factorLoopTime = 1.1f;
 
    {
        std::scoped_lock lock(accessMutex);
 
        NameValueMap applyPosMap;
        NameValueMap applyVelMap;
        NameValueMap applyPosVelMap_pos;
        NameValueMap applyPosVelMap_vel;
        NameValueMap applyTorMap;
        NameControlModeMap::const_iterator itMode = currentControlModes.begin();
 
        while (itMode != currentControlModes.end())
        {
            switch (itMode->second)
            {
                case ePositionControl:
                    if (currentJointTargets.find(itMode->first) != currentJointTargets.end())
                    {
                        applyPosMap[itMode->first] =
                            currentJointTargets.find(itMode->first)->second;
                    }
 
                    break;
 
                case eVelocityControl:
                {
                    auto it = currentJointVelTargets.find(itMode->first);
                    if (it != currentJointVelTargets.end())
                    {
                        if (mapVelToPosVel)
                        {
                            // compute pos
                            if (currentJointValues.find(itMode->first) != currentJointValues.end())
                            {
                                float posTarget = currentJointValues[itMode->first] +
                                                  it->second * loopTime * factorLoopTime;
                                applyPosVelMap_pos[itMode->first] = posTarget;
                                applyPosVelMap_vel[itMode->first] = it->second;
                            }
                        }
                        else
                        {
                            // just use velocity
                            applyVelMap[itMode->first] =
                                currentJointVelTargets.find(itMode->first)->second;
                        }
                    }
                }
 
                break;
 
                case eTorqueControl:
                    if (currentJointTorqueTargets.find(itMode->first) !=
                        currentJointTorqueTargets.end())
                    {
                        applyTorMap[itMode->first] =
                            currentJointTorqueTargets.find(itMode->first)->second;
                    }
 
                    break;
 
                case ePositionVelocityControl:
                    if (currentJointVelTargets.find(itMode->first) !=
                            currentJointVelTargets.end() &&
                        currentJointTargets.find(itMode->first) != currentJointTargets.end())
                    {
                        applyPosVelMap_pos[itMode->first] =
                            currentJointTargets.find(itMode->first)->second;
                        applyPosVelMap_vel[itMode->first] =
                            currentJointVelTargets.find(itMode->first)->second;
                    }
 
                    break;
 
                case eUnknown:
                case eDisabled:
                default:
                    // do nothing
                    ;
            }
 
            itMode++;
        }
        auto batchPrx = simulatorPrx->ice_batchOneway();
        if (applyPosMap.size() > 0)
        {
            batchPrx->actuateRobotJointsPos(robotName, applyPosMap);
        }
 
        if (applyVelMap.size() > 0)
        {
            batchPrx->actuateRobotJointsVel(robotName, applyVelMap);
        }
 
        if (applyTorMap.size() > 0)
        {
            batchPrx->actuateRobotJointsTorque(robotName, applyTorMap);
        }
 
        if (applyPosVelMap_pos.size() > 0)
        {
            batchPrx->actuateRobotJoints(robotName, applyPosVelMap_pos, applyPosVelMap_vel);
        }
        batchPrx->ice_flushBatchRequests();
 
        // command only once
        if (!mapVelToPosVel) // dont clear this in pos-velo mode because it is needed to calculate next position target
        {
            currentJointVelTargets.clear();
        }
        currentJointTargets.clear();
        currentJointTorqueTargets.clear();
    }
}
 
armarx::DebugInfo
armarx::KinematicUnitDynamicSimulation::getDebugInfo(const Ice::Current& c) const
{
    const ::armarx::SimulatedRobotState robotState = simulatorPrx->getRobotState(robotName);
 
    ::armarx::NameStatusMap jointStatus;
    for (const auto& [jointName, _] : currentControlModes)
    {
        jointStatus[jointName] = {
            .operation = eOnline,
            .error = eOk,
            .enabled = true,
            .emergencyStop = false // TODO: implement
        };
    }
 
    return {.jointModes = currentControlModes,
            .jointAngles = robotState.jointAngles,
            .jointVelocities = robotState.jointVelocities,
            .jointAccelerations = {}, // not available
            .jointTorques = robotState.jointTorques,
            .jointCurrents = {}, // not available
            .jointMotorTemperatures = {}, // not available
            .jointStatus = jointStatus};
}