RobotUnitSimulation.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * 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::RobotUnitSimulation
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "RobotUnitSimulation.h"
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <chrono>
#include <set>
 
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/ForceTorqueSensor.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/util/OnScopeExit.h>
 
 
using namespace armarx;
 
void
RobotUnitSimulation::onInitRobotUnit()
{
    robot = cloneRobot();
    simulatorPrxName = getProperty<std::string>("SimulatorName").getValue();
    usingProxy(simulatorPrxName);
 
    synchronizedSimulation = getProperty<bool>("SynchronizedSimulation").getValue();
    simulatorRobotListenerInterfaceTopic =
        getProperty<std::string>("SimulatorRobotListenerInterfaceTopic").getValue();
    if (simulatorRobotListenerInterfaceTopic.empty())
    {
        simulatorRobotListenerInterfaceTopic = "Simulator_Robot_" + getRobotName();
    }
    simulatorForceTorqueListenerInterfaceTopic =
        getProperty<std::string>("SimulatorForceTorqueListenerInterfaceTopic").getValue();
    std::size_t controlIterationMsProp = getProperty<std::size_t>("ControlIterationMs").getValue();
    if (!controlIterationMsProp)
    {
        ARMARX_WARNING << "the controll iteration was set to 0ms. detting it to 1ms";
        controlIterationMsProp = 1;
    }
    controlIterationMs = IceUtil::Time::milliSeconds(controlIterationMsProp);
    usingTopic(simulatorRobotListenerInterfaceTopic);
    usingTopic(simulatorForceTorqueListenerInterfaceTopic);
 
    //ft mapping
    {
        const std::string mappingStr = getProperty<std::string>("ForceTorqueSensorMapping");
        std::vector<std::string> entries;
        if (mappingStr != "")
        {
            bool trimEntries = true;
            entries = Split(mappingStr, ",", trimEntries);
        }
        for (std::string entry : entries)
        {
            ARMARX_CHECK_EXPRESSION(!entry.empty())
                << "empty entry in ForceTorqueSensorMapping! entries:\n"
                << entries;
            bool trimFields = true;
            std::vector<std::string> fields = Split(entry, ":", trimFields);
            ARMARX_CHECK_EXPRESSION(fields.size() == 2 || fields.size() == 3)
                << "invalid entry in ForceTorqueSensorMapping! invalid entry:\n"
                << fields << "\nall entries:\n"
                << entries;
            for (auto& field : fields)
            {
                ARMARX_CHECK_EXPRESSION(!field.empty())
                    << "empty field in ForceTorqueSensorMapping entry! invalid entry:\n"
                    << fields << "\nall entries:\n"
                    << entries;
            }
            const VirtualRobot::ForceTorqueSensorPtr ftsensor =
                robot->getSensor<typename VirtualRobot::ForceTorqueSensor>(fields.at(0));
            if (!ftsensor)
            {
                ARMARX_WARNING << "the robot has not ft sensor of name '" << fields.at(0)
                               << "' this ForceTorqueSensorMapping entry has no effect: " << entry;
                continue;
            }
            ARMARX_CHECK_EXPRESSION(fields.size() == 2 || robot->hasRobotNode(fields.at(2)))
                << VAROUT(fields.size()) << " "
                << (fields.size() != 2 ? VAROUT(fields.at(2)) : std::string{});
            ARMARX_CHECK_EXPRESSION(!ftMappings.count(fields.at(0)));
            FTMappingData& mapping = ftMappings[fields.at(0)];
            mapping.originalReportFrame = ftsensor->getRobotNode()->getName();
            mapping.reportFrame = mapping.originalReportFrame;
            mapping.reportTransformation = Eigen::Matrix3f::Identity();
            mapping.sensorName = fields.at(1);
            if (fields.size() == 3)
            {
                mapping.reportFrame = fields.at(2);
                //get rotation from original to target
                const auto orig = robot->getRobotNode(mapping.originalReportFrame)->getGlobalPose();
                const auto targ = robot->getRobotNode(mapping.reportFrame)->getGlobalPose();
                mapping.reportTransformation =
                    (targ.block<3, 3>(0, 0).inverse() * orig.block<3, 3>(0, 0));
            }
        }
    }
 
    maxLinearPlatformVelocity = getProperty<float>("maxLinearPlatformVelocity").getValue();
    ARMARX_CHECK_POSITIVE(maxLinearPlatformVelocity);
 
    maxAngularPlatformVelocity = getProperty<float>("maxAngularPlatformVelocity").getValue();
    ARMARX_CHECK_POSITIVE(maxAngularPlatformVelocity);
 
    ARMARX_INFO << "using " << VAROUT(simulatorRobotListenerInterfaceTopic);
    ARMARX_INFO << "using " << VAROUT(simulatorForceTorqueListenerInterfaceTopic);
    ARMARX_INFO << "using " << VAROUT(controlIterationMs);
}
 
void
RobotUnitSimulation::onConnectRobotUnit()
{
    simulatorPrx = getProxy<SimulatorInterfacePrx>(simulatorPrxName);
    if (!rtThread.joinable())
    {
        rtThread = std::thread{[&] { rtTask(); }};
    }
}
 
void
RobotUnitSimulation::joinControlThread()
{
    shutdownRtThread = true;
    rtThread.join();
}
 
void
RobotUnitSimulation::rtTask()
{
    ARMARX_ON_SCOPE_EXIT
    {
        ARMARX_IMPORTANT << "exiting rt thread";
    };
    try
    {
        {
            const auto timing = -TimeUtil::GetTime(true);
            robotName = getRobotName();
            //device init
            {
                const auto timing = -TimeUtil::GetTime(true);
                //joints
                {
                    const auto timing = -TimeUtil::GetTime(true);
                    ARMARX_INFO << "adding devices for joints:";
                    for (const VirtualRobot::RobotNodePtr& node : *(robot->getRobotNodeSet(
                             getProperty<std::string>("RobotNodeSetName").getValue())))
                    {
                        if (node->isJoint())
                        {
                            JointSimulationDevicePtr jdev = std::make_shared<JointSimulationDevice>(
                                node->getName(), anglesCtrl, velocitiesCtrl, torquesCtrl);
                            jointDevs.add(jdev->getDeviceName(), jdev);
                            addSensorDevice(jdev);
                            addControlDevice(jdev);
                        }
                    }
                    ARMARX_INFO << "adding devices for joints done ("
                                << (timing + TimeUtil::GetTime(true)).toMicroSeconds() << " us)";
                }
                //platform
                {
                    const auto timing = -TimeUtil::GetTime(true);
                    ARMARX_INFO << "adding devices for the platform:";
 
                    if (getRobotPlatformName().empty())
                    {
                        ARMARX_INFO
                            << "No platform device will be created since platform name was given";
                    }
                    else
                    {
                        if (robot->hasRobotNode(getRobotPlatformName()))
                        {
                            platformDev =
                                std::make_shared<PlatformSimulationDevice>(getRobotPlatformName());
                            addSensorDevice(platformDev);
                            addControlDevice(platformDev);
                        }
                        else
                        {
                            ARMARX_WARNING << "No robot node with the name '" +
                                                  getRobotPlatformName() +
                                                  "' so no platform device will be created";
                        }
                    }
                    ARMARX_INFO << "adding devices for the platform done ("
                                << (timing + TimeUtil::GetTime(true)).toMicroSeconds() << " us)";
                }
                //force torque
                {
                    const auto timing = -TimeUtil::GetTime(true);
                    ARMARX_INFO << "adding devices for force torque sensors:";
                    for (const auto& ft : robot->getSensors<VirtualRobot::ForceTorqueSensor>())
                    {
                        ForceTorqueSimulationSensorDevicePtr ftdev =
                            std::make_shared<ForceTorqueSimulationSensorDevice>(
                                getMappedFTName(ft->getName()),
                                getMappedFTReportingFrame(ft->getName(),
                                                          ft->getRobotNode()->getName()),
                                getMappedFTReportingTransformation(ft->getName()));
                        addSensorDevice(ftdev);
                        forceTorqueDevs.add(ftdev->getDeviceName(), ftdev);
                    }
                    ARMARX_INFO << "adding devices for force torque done ("
                                << (timing + TimeUtil::GetTime(true)).toMicroSeconds() << " us)";
                }
                //global pose device
                {
                    // globalPoseDevice = std::make_shared<GlobalRobotPoseSimulationSensorDevice>();
                    // addSensorDevice(globalPoseDevice);
                }
                ARMARX_INFO << "transitioning to " << RobotUnitState::InitializingUnits
                            << std::flush;
                finishDeviceInitialization();
                ARMARX_INFO << "device init done ("
                            << (timing + TimeUtil::GetTime(true)).toMicroSeconds() << " us)";
                ARMARX_INFO << "transitioned to " << getRobotUnitState() << std::flush;
            }
            //unit init
            {
                const auto timing = -TimeUtil::GetTime(true);
                //resize my tripple buffers
                {
                    const auto timing = -TimeUtil::GetTime(true);
                    torquesTB.reinit(std::vector<float>(jointDevs.size(), 0));
                    anglesTB.reinit(std::vector<float>(jointDevs.size(), 0));
                    velocitiesTB.reinit(std::vector<float>(jointDevs.size(), 0));
                    forceTorqueTB.reinit(std::vector<FT>(forceTorqueDevs.size(), FT{}));
                    forceTorqueTimes.clear();
                    forceTorqueTimes.resize(forceTorqueDevs.size(), 0);
                    initializeDefaultUnits();
                    ARMARX_INFO << "resize buffers done ("
                                << (timing + TimeUtil::GetTime(true)).toMicroSeconds() << " us)";
                }
                ARMARX_INFO << "transitioning to " << RobotUnitState::InitializingControlThread
                            << std::flush;
                finishUnitInitialization();
                ARMARX_INFO << "transitioned to " << getRobotUnitState() << std::flush;
                ARMARX_INFO << "unit init done ("
                            << (timing + TimeUtil::GetTime(true)).toMicroSeconds() << " us)";
            }
            //get initial sensor values
            {
                const auto timing = -TimeUtil::GetTime(true);
                gotSensorData = false;
                ARMARX_INFO << "fetching initial robot state" << std::flush;
                rtUpdateSensors(true);
                ARMARX_INFO << "fetching initial robot done" << std::flush;
                if (platformDev)
                {
                    const float absolutePositionX = robPoseTB.getReadBuffer()(0, 3);
                    const float absolutePositionY = robPoseTB.getReadBuffer()(1, 3);
                    const float absolutePositionRotation =
                        VirtualRobot::MathTools::eigen4f2rpy(robPoseTB.getReadBuffer())(2);
 
                    platformDev->initAbsolutePositionX = absolutePositionX;
                    platformDev->initAbsolutePositionY = absolutePositionY;
                    platformDev->initAbsolutePositionRotation = absolutePositionRotation;
                }
                ARMARX_INFO << "get init sensor values done ("
                            << (timing + TimeUtil::GetTime(true)).toMicroSeconds() << " us)";
            }
            //enter RT
            {
                if (synchronizedSimulation)
                {
                    simulatorPrx->stop();
                    ARMARX_IMPORTANT << "using synchronized simulator execution" << std::flush;
                }
                ARMARX_INFO << "now transitioning to rt" << std::flush;
                {
                    const auto timing = -TimeUtil::GetTime(true);
                    finishControlThreadInitialization();
                    ARMARX_INFO << "init rt thread done ("
                                << (timing + TimeUtil::GetTime(true)).toMicroSeconds() << " us)";
                }
                ARMARX_INFO << "transitioned to " << getRobotUnitState() << std::flush;
            }
            ARMARX_INFO << "pre loop done (" << (timing + TimeUtil::GetTime(true)).toMicroSeconds()
                        << " us)";
        }
        //meassure time
        IceUtil::Time timeSinceLastIteration;
        IceUtil::Time currentIterationStart;
        IceUtil::Time lastIterationStart = TimeUtil::GetTime();
        //loop
        while (!shutdownRtThread)
        {
            rtGetRTThreadTimingsSensorDevice().rtMarkRtLoopStart();
            //time
            currentIterationStart = TimeUtil::GetTime();
            timeSinceLastIteration = currentIterationStart - lastIterationStart;
            lastIterationStart = currentIterationStart;
            //call functions
            rtSwitchControllerSetup(); // << switch controllers
            rtResetAllTargets();
            rtRunNJointControllers(sensorValuesTimestamp,
                                   timeSinceLastIteration); // << run NJointControllers
            rtHandleInvalidTargets(); // << deactivate broken NJointControllers
            rtRunJointControllers(sensorValuesTimestamp,
                                  timeSinceLastIteration); // << run JointControllers
            //communicate with the simulator
            {
                rtGetRTThreadTimingsSensorDevice().rtMarkRtBusSendReceiveStart();
                ARMARX_ON_SCOPE_EXIT
                {
                    rtGetRTThreadTimingsSensorDevice().rtMarkRtBusSendReceiveEnd();
                };
                rtSendCommands();
                //run sim
                if (synchronizedSimulation)
                {
                    gotSensorData = false;
                    simulatorPrx->step();
                }
                rtUpdateSensors(synchronizedSimulation);
            }
            rtUpdateSensorAndControlBuffer(
                sensorValuesTimestamp,
                timeSinceLastIteration); // << swap out ControllerTargets / SensorValues
            ++iterationCount;
            rtGetRTThreadTimingsSensorDevice().rtMarkRtLoopPreSleep();
            //sleep remainder
            //since the timeserver does not end the sleep on shutdown (and this would block shutdown),
            //we do busy waiting and poll the time
            const IceUtil::Time sleepUntil = currentIterationStart + controlIterationMs;
            IceUtil::Time currentTime = TimeUtil::GetTime();
            while (currentTime < sleepUntil)
            {
                if (shutdownRtThread)
                {
                    return;
                }
                if (currentTime < currentIterationStart)
                {
                    //this fixes sleeping for a long time
                    //in case the time server is reset
                    break;
                }
                std::this_thread::sleep_for(
                    std::chrono::microseconds{100}); //polling is done with 10kHz
                currentTime = TimeUtil::GetTime();
            }
        }
    }
    catch (Ice::Exception& e)
    {
        ARMARX_ERROR << "exception in rtTask!\nwhat:\n"
                     << e.what() << "\n\tname: " << e.ice_id() << "\n\tfile: " << e.ice_file()
                     << "\n\tline: " << e.ice_line() << "\n\tstack: " << e.ice_stackTrace()
                     << std::flush;
        throw;
    }
    catch (std::exception& e)
    {
        ARMARX_ERROR << "exception in rtTask!\nwhat:\n" << e.what() << std::flush;
        throw;
    }
    catch (...)
    {
        ARMARX_ERROR << "exception in rtTask!" << std::flush;
        throw;
    }
}
 
void
RobotUnitSimulation::reportState(const SimulatedRobotState& state, const Ice::Current&)
{
    IceUtil::Time timestamp = IceUtil::Time::microSeconds(state.timestampInMicroSeconds);
 
    if (TimeUtil::HasTimeServer())
    {
        simulationDataTimestampInMicroSeconds = state.timestampInMicroSeconds;
    }
    else
    {
        simulationDataTimestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
    }
 
    fillTB(anglesTB, state.jointAngles, "JointAngles");
    fillTB(velocitiesTB, state.jointVelocities, "JointVelocities");
    fillTB(torquesTB, state.jointTorques, "JointTorques");
 
    for (ForceTorqueData const& ftData : state.forceTorqueValues)
    {
        updateForceTorque(ftData, timestamp);
    }
 
    // Pose
    if (skipReport())
    {
        ARMARX_DEBUG << deactivateSpam() << "Skipped all sensor values for robot pose";
    }
    else
    {
        auto g = robPoseTB.guard();
        ARMARX_CHECK_NOT_NULL(state.pose)
            << "Robot Pose is not allowed to be NULL. Maybe no state was reported from Simulator!";
        robPoseTB.getWriteBuffer() = PosePtr::dynamicCast(state.pose)->toEigen();
        robPoseTB.write();
        ARMARX_DEBUG << deactivateSpam() << "Got sensor values for robot pose";
    }
 
    if (skipReport())
    {
        ARMARX_DEBUG << deactivateSpam() << "Skipped all sensor values for robot vel";
        return;
    }
    else
    {
        auto& trans = state.linearVelocity;
        auto& rotat = state.angularVelocity;
 
        auto g = robVelTB.guard();
        robVelTB.getWriteBuffer().lin(0) = trans->x;
        robVelTB.getWriteBuffer().lin(1) = trans->y;
        robVelTB.getWriteBuffer().lin(2) = trans->z;
        robVelTB.getWriteBuffer().lin =
            robPoseTB.getReadBuffer().block<3, 3>(0, 0).transpose() * robVelTB.getWriteBuffer().lin;
        robVelTB.getWriteBuffer().ang(0) = rotat->x;
        robVelTB.getWriteBuffer().ang(1) = rotat->y;
        robVelTB.getWriteBuffer().ang(2) = rotat->z;
        robVelTB.getWriteBuffer().ang =
            robPoseTB.getReadBuffer().block<3, 3>(0, 0).transpose() * robVelTB.getWriteBuffer().ang;
        robVelTB.write();
        ARMARX_DEBUG << deactivateSpam() << "Got sensor values for robot vel";
    }
 
    gotSensorData = true;
}
 
void
RobotUnitSimulation::updateForceTorque(const ForceTorqueData& ftData, IceUtil::Time timestamp)
{
    auto& sensorName = ftData.sensorName;
    auto& nodeName = ftData.nodeName;
 
    const std::string sensname =
        ftMappings.count(sensorName) ? ftMappings.at(sensorName).sensorName : sensorName;
    if (ftMappings.count(sensorName) && ftMappings.at(sensorName).originalReportFrame != nodeName)
    {
        ARMARX_ERROR << deactivateSpam(0.25) << "Sensor values for sensor '" << sensorName << "' ('"
                     << sensname << "') are reported in frame '" << nodeName << "' instead of '"
                     << ftMappings.at(sensorName).originalReportFrame
                     << "' as defined during setup! (this value is skipped!)";
        return;
    }
    if (skipReport())
    {
        ARMARX_DEBUG << deactivateSpam(10, sensname)
                     << "Skipped all sensor values for force torque of sensor " << sensname
                     << " (node = " << nodeName << ")";
        return;
    }
    auto g = forceTorqueTB.guard();
    if (!forceTorqueDevs.has(sensname))
    {
        ARMARX_WARNING << deactivateSpam(10, sensname) << "no ftsensor with name " << sensname
                       << "\nftsensors:\n"
                       << forceTorqueDevs.keys();
        return;
    }
    auto i = forceTorqueDevs.index(sensname);
    ARMARX_CHECK_EXPRESSION(forceTorqueTB.getWriteBuffer().size() > i)
        << forceTorqueTB.getWriteBuffer().size() << " > " << i;
    auto& force = ftData.force;
    auto& torque = ftData.torque;
    ForceTorqueSimulationSensorDevice& ftdev = *forceTorqueDevs.at(i);
    ftdev.fx.update(force->x);
    ftdev.fy.update(force->y);
    ftdev.fz.update(force->z);
    ftdev.tx.update(torque->x);
    ftdev.ty.update(torque->y);
    ftdev.tz.update(torque->z);
 
    Eigen::Vector3f filteredTorque;
    Eigen::Vector3f filteredForce;
    filteredForce(0) = ftdev.fx.getRawValue();
    filteredForce(1) = ftdev.fy.getRawValue();
    filteredForce(2) = ftdev.fz.getRawValue();
    filteredTorque(0) = ftdev.tx.getRawValue();
    filteredTorque(1) = ftdev.ty.getRawValue();
    filteredTorque(2) = ftdev.tz.getRawValue();
 
    forceTorqueTB.getWriteBuffer().at(i).force = ftdev.reportingTransformation * filteredForce;
    forceTorqueTB.getWriteBuffer().at(i).torque = ftdev.reportingTransformation * filteredTorque;
    forceTorqueTB.write();
    forceTorqueTimes.at(i) = timestamp.toMicroSeconds() * 1000; // Nanoseconds
    ARMARX_DEBUG << deactivateSpam(10, sensname)
                 << "Got new sensor values for force torque of sensor " << sensname
                 << " (node = " << nodeName << ")";
}
 
void
RobotUnitSimulation::fillTB(TripleBufferWithGuardAndTime<std::vector<float>>& b,
                            const NameValueMap& nv,
                            const std::string name) const
{
    if (skipReport())
    {
        ARMARX_DEBUG << deactivateSpam(10, name) << "Skipped all sensor values for " << name;
        return;
    }
    auto g = b.guard();
    std::stringstream ignored;
    bool someWereIgnored = false;
    for (const auto& a : nv)
    {
        if (jointDevs.has(a.first))
        {
            b.getWriteBuffer().at(jointDevs.index(a.first)) = a.second;
        }
        else
        {
            ignored << a.first << " -> " << a.second << "\n";
            someWereIgnored = true;
        }
    }
    b.write();
    ARMARX_DEBUG << deactivateSpam(10, name) << "Got new  sensor values for " << name;
    if (someWereIgnored)
    {
        ARMARX_VERBOSE << deactivateSpam(10, name) << "Ignored sensor values for " << name << ":\n"
                       << ignored.str();
    }
}
 
void
RobotUnitSimulation::rtSendCommands()
{
    try
    {
        auto prx = simulatorPrx->ice_batchOneway();
        //send commands
        if (!velocitiesCtrl.empty())
        {
            prx->actuateRobotJointsVel(robotName, velocitiesCtrl);
        }
        if (!anglesCtrl.empty())
        {
            prx->actuateRobotJointsPos(robotName, anglesCtrl);
        }
        if (!torquesCtrl.empty())
        {
            prx->actuateRobotJointsTorque(robotName, torquesCtrl);
        }
 
        if (platformDev)
        {
            Eigen::Vector3f translationVel;
            translationVel(0) = std::clamp(platformDev->target.velocityX,
                                           -maxLinearPlatformVelocity,
                                           maxLinearPlatformVelocity);
            translationVel(1) = std::clamp(platformDev->target.velocityY,
                                           -maxLinearPlatformVelocity,
                                           maxLinearPlatformVelocity);
            translationVel(2) = 0;
            translationVel /= 1000;
 
            Eigen::Vector3f rotationVel(0.f,
                                        0.f,
                                        std::clamp(platformDev->target.velocityRotation,
                                                   -maxAngularPlatformVelocity,
                                                   maxAngularPlatformVelocity));
 
            prx->setRobotLinearVelocityRobotRootFrame(
                robotName, platformDev->getDeviceName(), new Vector3(translationVel));
            prx->setRobotAngularVelocityRobotRootFrame(
                robotName, platformDev->getDeviceName(), new Vector3(rotationVel));
            ARMARX_DEBUG << deactivateSpam(10)
                         << "setting platform vel ang: " << rotationVel.transpose();
            ARMARX_DEBUG << deactivateSpam(10)
                         << "setting platform vel lin: " << translationVel.transpose();
        }
        prx->ice_flushBatchRequests();
    }
    catch (Ice::ConnectionRefusedException&)
    {
        if (!shutdownRtThread)
        {
            //ignore this when shutting down. this probably happened when the simulator
            //was shut down while the RobotUnitSimulation was still running
            ARMARX_WARNING
                << "Ice::ConnectionRefusedException when sending commands to the simulator";
        }
    }
    catch (Ice::NotRegisteredException&)
    {
        if (!shutdownRtThread)
        {
            //ignore this when shutting down. this probably happened when the simulator
            //was shut down while the RobotUnitSimulation was still running
            ARMARX_WARNING << "Ice::NotRegisteredException when sending commands to the simulator";
        }
    }
}
 
bool
RobotUnitSimulation::skipReport() const
{
    static const std::set<RobotUnitState> reportAcceptingStates{
        RobotUnitState::InitializingControlThread, RobotUnitState::Running};
    return !reportAcceptingStates.count(getRobotUnitState());
}
 
void
RobotUnitSimulation::rtPollRobotState()
{
    if (!isPolling.exchange(true))
    {
        std::thread{[&]
                    {
                        try
                        {
                            const auto name = getRobotName();
                            auto rootname = robot->getRootNode()->getName();
                            SimulatedRobotState state = simulatorPrx->getRobotState(robotName);
 
                            reportState(state);
                        }
                        catch (Ice::Exception& e)
                        {
                            ARMARX_ERROR << "exception in polling!\nwhat:\n"
                                         << e.what() << "\n\tname: " << e.ice_id()
                                         << "\n\tfile: " << e.ice_file()
                                         << "\n\tline: " << e.ice_line()
                                         << "\n\tstack: " << e.ice_stackTrace() << std::flush;
                            throw;
                        }
                        catch (std::exception& e)
                        {
                            ARMARX_ERROR << "exception in polling!\nwhat:\n"
                                         << e.what() << std::flush;
                            throw;
                        }
                        catch (...)
                        {
                            ARMARX_ERROR << "exception in polling!" << std::flush;
                            throw;
                        }
                        isPolling = false;
                    }}
            .detach();
    }
}
 
void
RobotUnitSimulation::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
{
    RobotUnit::componentPropertiesUpdated(changedProperties);
    if (changedProperties.count("SynchronizedSimulation"))
    {
        synchronizedSimulation = getProperty<bool>("SynchronizedSimulation").getValue();
    }
    if (areDevicesReady())
    {
        if (synchronizedSimulation)
        {
            simulatorPrx->stop();
            ARMARX_IMPORTANT << "using synchronized simulator execution" << std::flush;
        }
        else
        {
            simulatorPrx->start();
            ARMARX_IMPORTANT << "using synchronized simulator execution" << std::flush;
        }
    }
}
 
void
RobotUnitSimulation::rtUpdateSensors(bool wait)
{
    //wait for all new data
    if (wait)
    {
        std::mutex dummy;
        std::unique_lock<std::mutex> dummlock{dummy};
        std::size_t fails = 0;
        while (!gotSensorData)
        {
            ARMARX_IMPORTANT << deactivateSpam()
                             << "waiting for up to date sensor values (iteration " << iterationCount
                             << ") ";
            cvGotSensorData.wait_for(dummlock, std::chrono::milliseconds{10});
            ++fails;
            if (!(fails % 10))
            {
                rtPollRobotState();
            }
        }
    }
 
    // This should be the timestamp from the time where the data was queried
    // We have to use armarx::rtNow() now because of changes in the framework to assess the timestamp in
    // a realtime-conform way (using MONOTONIC_RAW clock, which is the time difference since last boot).
    // This probably breaks simulation speeds faster than realtime, but this feature probably was broken
    // before anyways.  See https://git.h2t.iar.kit.edu/sw/armarx/armarx-simulation/-/issues/26.
    const IceUtil::Time now = armarx::rtNow();
    //const IceUtil::Time now = IceUtil::Time::microSeconds(simulationDataTimestampInMicroSeconds);
    const IceUtil::Time timeSinceLastIteration = now - sensorValuesTimestamp;
    const float dt = static_cast<float>(timeSinceLastIteration.toSecondsDouble());
    sensorValuesTimestamp = now;
    torquesTB.read();
    anglesTB.read();
    velocitiesTB.read();
    robPoseTB.read();
    robVelTB.read();
    forceTorqueTB.read();
    //update joint sensors
    for (std::size_t i = 0; i < jointDevs.size(); ++i)
    {
        JointSimulationDevice& jdev = *jointDevs.at(i);
        const float deltav = velocitiesTB.getReadBuffer().at(i) - jdev.sensorValue.velocity;
        jdev.sensorValue.acceleration = deltav / dt;
        jdev.sensorValue.position = anglesTB.getReadBuffer().at(i);
        jdev.sensorValue.torque = torquesTB.getReadBuffer().at(i);
        jdev.sensorValue.velocity = velocitiesTB.getReadBuffer().at(i);
    }
 
    //ft
    for (std::size_t i = 0; i < forceTorqueDevs.size(); ++i)
    {
        ForceTorqueSimulationSensorDevice& ft = *forceTorqueDevs.at(i);
        ft.sensorValue.force = forceTorqueTB.getReadBuffer().at(i).force;
        ft.sensorValue.torque = forceTorqueTB.getReadBuffer().at(i).torque;
    }
    //platform
    if (platformDev)
    {
        auto& s = platformDev->sensorValue;
 
        const float absolutePositionX = robPoseTB.getReadBuffer()(0, 3);
        const float absolutePositionY = robPoseTB.getReadBuffer()(1, 3);
        const float absolutePositionRotation =
            VirtualRobot::MathTools::eigen4f2rpy(robPoseTB.getReadBuffer())(2);
 
        Eigen::Vector2f relativePositionGlobalFrame;
        relativePositionGlobalFrame << absolutePositionX - platformDev->initAbsolutePositionX,
            absolutePositionY - platformDev->initAbsolutePositionY;
        s.relativePositionRotation =
            absolutePositionRotation - platformDev->initAbsolutePositionRotation;
        // Revert the rotation by rotating by the negative angle
        Eigen::Vector2f relativePosition =
            Eigen::Rotation2Df(-platformDev->initAbsolutePositionRotation) *
            relativePositionGlobalFrame;
        s.relativePositionX = relativePosition(0);
        s.relativePositionY = relativePosition(1);
 
        const RobVel& v = robVelTB.getReadBuffer();
        s.setVelocitiesAndDeriveAcceleration(v.lin(0) / 3.f, v.lin(1) / 3.f, v.ang(2) / 5.0f, dt);
    }
    //globalpose
    // globalPoseDevice->sensor.pose = robPoseTB.getReadBuffer();
 
    rtSetRobotGlobalPose(robPoseTB.getReadBuffer(), false);
    //this call should not do anything in this case, since sensors are updated above
    rtReadSensorDeviceValues(sensorValuesTimestamp, timeSinceLastIteration);
}
 
ARMARX_REGISTER_COMPONENT_EXECUTABLE(::armarx::RobotUnitSimulation, ::armarx::RobotUnitSimulation::GetDefaultName());