KinematicSubUnit.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    RobotAPI::ArmarXObjects::KinematicSubUnit
 * @author     Raphael ( raphael dot grimm at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "KinematicSubUnit.h"
 
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXCore/core/util/algorithm.h>
 
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.h>
 
armarx::KinematicSubUnit::KinematicSubUnit() : reportSkipper(20.0f)
{
}
 
void
armarx::KinematicSubUnit::setupData(
    std::string relRobFile,
    VirtualRobot::RobotPtr rob,
    std::map<std::string, ActuatorData>&& newDevs,
    std::vector<std::set<std::string>> controlDeviceHardwareControlModeGrps,
    std::set<std::string> controlDeviceHardwareControlModeGrpsMerged,
    RobotUnit* newRobotUnit)
{
    std::lock_guard<std::mutex> guard{dataMutex};
    ARMARX_CHECK_EXPRESSION(getState() == eManagedIceObjectCreated);
 
    ARMARX_CHECK_EXPRESSION(!robot);
    ARMARX_CHECK_EXPRESSION(rob);
    robot = rob;
    robot->setUpdateCollisionModel(false);
    robot->setUpdateVisualization(false);
    robot->setThreadsafe(false);
 
    ARMARX_CHECK_EXPRESSION(!robotUnit);
    ARMARX_CHECK_EXPRESSION(newRobotUnit);
    robotUnit = newRobotUnit;
 
    ARMARX_CHECK_EXPRESSION(relativeRobotFile.empty());
    ARMARX_CHECK_EXPRESSION(!relRobFile.empty());
    relativeRobotFile = relRobFile;
 
    devs = std::move(newDevs);
    controlDeviceHardwareControlModeGroups = controlDeviceHardwareControlModeGrps;
    controlDeviceHardwareControlModeGroupsMerged = controlDeviceHardwareControlModeGrpsMerged;
 
    auto nodes = robot->getRobotNodes();
    for (auto& node : nodes)
    {
        if (node->isJoint() && !devs.count(node->getName()))
        {
            sensorLessJoints.push_back(node);
        }
    }
}
 
void
armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc,
                                 const armarx::JointAndNJointControllers& c)
{
    if (!getProxy())
    {
        //this unit is not initialized yet
        ARMARX_IMPORTANT << deactivateSpam(1) << "not initialized yet";
        return;
    }
    if (!listenerPrx)
    {
        ARMARX_IMPORTANT << deactivateSpam(1) << "listener is not set";
        return;
    }
    std::lock_guard<std::mutex> guard{dataMutex};
    bool ctrlModesAValueChanged = false;
    bool angAValueChanged = false;
    bool velAValueChanged = false;
    bool accAValueChanged = false;
    bool torAValueChanged = false;
    bool motorCurrentAValueChanged = false;
    bool motorTemperaturesAValueChanged = false;
    bool statusesAvalueChanged = false;
 
    long timestamp = sc.sensorValuesTimestamp.toMicroSeconds();
    std::set<NJointControllerBase*> nJointCtrls{c.nJointControllers.begin(),
                                                c.nJointControllers.end()};
    std::vector<std::string> actuatorsWithoutSensor;
    actuatorsWithoutSensor.reserve(devs.size());
    for (const auto& name2actuatorData : devs)
    {
        const ActuatorData& actuatorData = name2actuatorData.second;
        const auto& name = actuatorData.name;
        //mode
        {
            ControlMode c = eUnknown;
            if (actuatorData.ctrlPos && nJointCtrls.count(actuatorData.ctrlPos.get()))
            {
                c = ePositionControl;
            }
            if (actuatorData.ctrlVel && nJointCtrls.count(actuatorData.ctrlVel.get()))
            {
                ARMARX_CHECK_EXPRESSION(eUnknown == c);
                c = eVelocityControl;
            }
            if (actuatorData.ctrlTor && nJointCtrls.count(actuatorData.ctrlTor.get()))
            {
                ARMARX_CHECK_EXPRESSION(eUnknown == c);
                c = eTorqueControl;
            }
            ctrlModesAValueChanged =
                ctrlModesAValueChanged || !ctrlModes.count(name) || (ctrlModes.at(name) != c);
            ctrlModes[name] = c;
        }
        if (actuatorData.sensorDeviceIndex < sc.sensors.size())
        {
            const SensorValueBase* s = sc.sensors.at(actuatorData.sensorDeviceIndex).get();
            UpdateNameValueMap<float,
                               SensorValue1DoFActuatorPosition,
                               &SensorValue1DoFActuatorPosition::position>(
                ang, name, s, angAValueChanged);
            UpdateNameValueMap<float,
                               SensorValue1DoFActuatorVelocity,
                               &SensorValue1DoFActuatorVelocity::velocity>(
                vel, name, s, velAValueChanged);
            UpdateNameValueMap<float,
                               SensorValue1DoFActuatorAcceleration,
                               &SensorValue1DoFActuatorAcceleration::acceleration>(
                acc, name, s, accAValueChanged);
            UpdateNameValueMap<float,
                               SensorValue1DoFActuatorTorque,
                               &SensorValue1DoFActuatorTorque::torque>(
                tor, name, s, torAValueChanged);
            UpdateNameValueMap<float,
                               SensorValue1DoFActuatorCurrent,
                               &SensorValue1DoFActuatorCurrent::motorCurrent>(
                motorCurrents, name, s, motorCurrentAValueChanged);
            UpdateNameValueMap<float,
                               SensorValue1DoFActuatorMotorTemperature,
                               &SensorValue1DoFActuatorMotorTemperature::motorTemperature>(
                motorTemperatures, name, s, motorTemperaturesAValueChanged);
            UpdateNameValueMap<JointStatus,
                               SensorValue1DoFActuatorStatus,
                               &SensorValue1DoFActuatorStatus::status>(
                statuses, name, s, statusesAvalueChanged);
        }
        else
        {
            actuatorsWithoutSensor.emplace_back(name);
        }
    }
    if (!actuatorsWithoutSensor.empty())
    {
        ARMARX_WARNING << deactivateSpam(5) << "these actuators have no sensor!:\n"
                       << actuatorsWithoutSensor;
    }
 
    // update the joint values of linked joints
    robot->setJointValues(ang);
    auto nodes = robot->getRobotNodes();
    for (auto& node : nodes)
    {
        node->updatePose(false);
    }
    for (auto& node : sensorLessJoints)
    {
        ang[node->getName()] = node->getJointValue();
    }
 
 
    ARMARX_DEBUG << deactivateSpam(30) << "reporting updated data:\n"
                 << ctrlModes.size()
                 << " \tcontrol modes       (updated = " << ctrlModesAValueChanged << ")\n"
                 << ang.size() << " \tjoint angles        (updated = " << angAValueChanged << ")\n"
                 << vel.size() << " \tjoint velocities    (updated = " << velAValueChanged << ")\n"
                 << acc.size() << " \tjoint accelerations (updated = " << accAValueChanged << ")\n"
                 << tor.size() << " \tjoint torques       (updated = " << torAValueChanged << ")\n"
                 << motorCurrents.size()
                 << " \tmotor currents (updated = " << motorCurrentAValueChanged << ")\n"
                 << motorTemperatures.size()
                 << " \tmotor temperatures (updated = " << motorTemperaturesAValueChanged << ")\n";
    auto prx = listenerPrx->ice_batchOneway();
    prx->reportJointAngles(ang, timestamp, angAValueChanged);
    prx->reportJointVelocities(vel, timestamp, velAValueChanged);
    prx->reportJointTorques(tor, timestamp, torAValueChanged);
    if (reportSkipper.checkFrequency("Meta")) // only report the following data with low frequency
    {
        prx->reportJointAccelerations(acc, timestamp, accAValueChanged);
        if (!motorCurrents.empty())
        {
            prx->reportJointCurrents(motorCurrents, timestamp, motorCurrentAValueChanged);
        }
        if (!motorTemperatures.empty())
        {
            prx->reportJointMotorTemperatures(
                motorTemperatures, timestamp, motorTemperaturesAValueChanged);
        }
        if (!statuses.empty())
        {
            prx->reportJointStatuses(statuses, timestamp, statusesAvalueChanged);
        }
        prx->reportControlModeChanged(ctrlModes, timestamp, ctrlModesAValueChanged);
    }
    prx->ice_flushBatchRequests();
}
 
void
armarx::KinematicSubUnit::requestJoints(const Ice::StringSeq&, const Ice::Current&)
{
    ARMARX_WARNING << "NYI";
}
 
void
armarx::KinematicSubUnit::releaseJoints(const Ice::StringSeq&, const Ice::Current&)
{
    ARMARX_WARNING << "NYI";
}
 
void
armarx::KinematicSubUnit::setJointAngles(const armarx::NameValueMap& angles, const Ice::Current&)
{
    std::lock_guard<std::mutex> guard{dataMutex};
    for (const auto& n2v : angles)
    {
        if (devs.count(n2v.first))
        {
            if (devs.at(n2v.first).ctrlPos)
            {
                devs.at(n2v.first).ctrlPos->set(n2v.second);
            }
        }
    }
}
 
void
armarx::KinematicSubUnit::setJointVelocities(const armarx::NameValueMap& velocities,
                                             const Ice::Current&)
{
    std::lock_guard<std::mutex> guard{dataMutex};
    for (const auto& n2v : velocities)
    {
        if (devs.count(n2v.first))
        {
            if (devs.at(n2v.first).ctrlVel)
            {
                devs.at(n2v.first).ctrlVel->set(n2v.second);
            }
        }
    }
}
 
void
armarx::KinematicSubUnit::setJointTorques(const armarx::NameValueMap& torques, const Ice::Current&)
{
    std::lock_guard<std::mutex> guard{dataMutex};
    for (const auto& n2v : torques)
    {
        if (devs.count(n2v.first))
        {
            if (devs.at(n2v.first).ctrlTor)
            {
                devs.at(n2v.first).ctrlTor->set(n2v.second);
            }
        }
    }
}
 
void
armarx::KinematicSubUnit::switchControlMode(const armarx::NameControlModeMap& ncm,
                                            const Ice::Current&)
{
    std::map<std::string, NJointControllerBasePtr> toActivate;
    {
        std::lock_guard<std::mutex> guard{dataMutex};
        for (const auto& n2c : ncm)
        {
            const std::string& name = n2c.first;
            ControlMode mode = n2c.second;
            if (!devs.count(name))
            {
                ARMARX_WARNING << "requested mode '"
                               << KinematicUnitHelper::ControlModeToString(mode)
                               << "' for nonexistent device '" << name
                               << "'. (ignoring this device)";
                continue;
            }
            NJointKinematicUnitPassThroughControllerPtr ctrl =
                NJointKinematicUnitPassThroughControllerPtr::dynamicCast(
                    devs.at(name).getController(mode));
            if (!ctrl)
            {
                ARMARX_WARNING << "requested unsupported mode '"
                               << KinematicUnitHelper::ControlModeToString(mode) << "' for device '"
                               << name << "'. (ignoring this device)";
                continue;
            }
            if (ctrl->isControllerActive())
            {
                continue;
            }
            ctrl->reset(); // reset immediately instead of preActivate() to avoid race condition
            toActivate[name] = std::move(ctrl);
        }
        const auto printToActivate = ARMARX_STREAM_PRINTER
        {
            for (const auto& elem : toActivate)
            {
                out << "    '" << elem.first << "' -> '" << elem.second->getInstanceName() << "'\n";
            }
        };
        ARMARX_DEBUG << "switching control modes requests these NJointControllers (without "
                        "consideration of ControlDeviceHardwareControlModeGroups):\n"
                     << printToActivate;
        for (const auto& n2NJointCtrl : toActivate)
        {
            const auto& name = n2NJointCtrl.first;
            ARMARX_DEBUG << "checking group of '" << name << "'";
            const NJointControllerBasePtr& nJointCtrl = n2NJointCtrl.second;
            if (!controlDeviceHardwareControlModeGroupsMerged.count(name))
            {
                continue;
            }
            //is in a group! (find the correct group)
            const std::set<std::string>* group = nullptr;
            for (const auto& grp : controlDeviceHardwareControlModeGroups)
            {
                if (grp.count(name))
                {
                    group = &grp;
                    break;
                }
            }
            ARMARX_CHECK_EXPRESSION(group);
            const auto jointCtrl = nJointCtrl->getControlDevicesUsedJointController().at(name);
            const auto hwMode = jointCtrl->getHardwareControlMode();
            //check against all other elems of the group
            for (const auto& other : *group)
            {
                if (name == other)
                {
                    continue;
                }
                if (!devs.count(other))
                {
                    continue;
                }
                //this device may have a controller switch
                if (toActivate.count(other))
                {
                    const auto otherJointCtrl =
                        toActivate.at(other)->getControlDevicesUsedJointController().at(other);
                    const auto otherHwMode = otherJointCtrl->getHardwareControlMode();
                    if (otherHwMode != hwMode)
                    {
                        std::stringstream strstr;
                        strstr << "The hardware control mode for two control devices with "
                                  "requested control mode changein the same group does not match!\n"
                               << "Device 1: '" << name << "' mode "
                               << "'" << jointCtrl->getControlMode() << "' hw mode '" << hwMode
                               << "'\n"
                               << "Device 2: '" << other << "' mode "
                               << "'" << otherJointCtrl->getControlMode() << "' hw mode '"
                               << otherHwMode << "'\n";
                        ARMARX_ERROR << strstr.str();
                        throw InvalidArgumentException{strstr.str()};
                    }
                }
                else
                {
                    //get the current active
                    const auto otherNJointCtrl = devs.at(other).getActiveController();
                    const auto otherJointCtrl =
                        otherNJointCtrl
                            ? otherNJointCtrl->getControlDevicesUsedJointController().at(other)
                            : nullptr;
                    const auto otherHwMode =
                        otherJointCtrl ? otherJointCtrl->getHardwareControlMode() : "";
                    if (hwMode != otherHwMode)
                    {
                        toActivate[other] = std::move(devs.at(other).getController(ncm.at(name)));
                        ARMARX_CHECK_EXPRESSION(toActivate.at(other));
                        ARMARX_CHECK_EXPRESSION(toActivate.at(other)
                                                    ->getControlDevicesUsedJointController()
                                                    .at(other)
                                                    ->getHardwareControlMode() == hwMode);
                        ARMARX_VERBOSE << "activating '" << nJointCtrl->getInstanceName()
                                       << "' caused activation of '"
                                       << toActivate.at(name)->getInstanceName() << "'";
                    }
                }
            }
            ARMARX_DEBUG << "checking group of '" << name << "'...done!";
        }
        ARMARX_DEBUG << "switching control modes requests these NJointControllers (with "
                        "consideration of ControlDeviceHardwareControlModeGroups):\n"
                     << printToActivate;
        //now check if groups are satisfied
        ARMARX_CHECK_EXPRESSION(robotUnit);
    }
    if (!toActivate.empty())
    {
        robotUnit->activateNJointControllers(getMapValues(toActivate));
    }
}
 
void
armarx::KinematicSubUnit::setJointAccelerations(const armarx::NameValueMap&, const Ice::Current&)
{
    ARMARX_WARNING << "NYI";
}
 
void
armarx::KinematicSubUnit::setJointDecelerations(const armarx::NameValueMap&, const Ice::Current&)
{
    ARMARX_WARNING << "NYI";
}
 
armarx::NameControlModeMap
armarx::KinematicSubUnit::getControlModes(const Ice::Current&)
{
    std::lock_guard<std::mutex> guard{dataMutex};
    return ctrlModes;
}
 
armarx::NameValueMap
armarx::KinematicSubUnit::getJointAngles(const Ice::Current& c) const
{
    std::lock_guard<std::mutex> guard{dataMutex};
    return ang;
}
 
armarx::NameValueMap
armarx::KinematicSubUnit::getJointVelocities(const Ice::Current& c) const
{
    std::lock_guard<std::mutex> guard{dataMutex};
    return vel;
}
 
Ice::StringSeq
armarx::KinematicSubUnit::getJoints(const Ice::Current& c) const
{
    std::lock_guard<std::mutex> guard{dataMutex};
    return getMapKeys(ang);
}
 
armarx::DebugInfo
armarx::KinematicSubUnit::getDebugInfo(const Ice::Current& c) const
{
    std::lock_guard<std::mutex> guard{dataMutex};
 
    armarx::DebugInfo debugInfo{.jointModes = ctrlModes,
                                .jointAngles = ang,
                                .jointVelocities = vel,
                                .jointAccelerations = acc,
                                .jointTorques = tor,
                                .jointCurrents = motorCurrents,
                                .jointMotorTemperatures = motorTemperatures,
                                .jointStatus = statuses};
 
    return debugInfo;
}
 
armarx::NJointControllerPtr
armarx::KinematicSubUnit::ActuatorData::getController(armarx::ControlMode c) const
{
    switch (c)
    {
        case ePositionVelocityControl:
            return ctrlPos;
        case ePositionControl:
            return ctrlPos;
        case eVelocityControl:
            return ctrlVel;
        case eTorqueControl:
            return ctrlTor;
        default:
            return nullptr;
    }
}
 
armarx::NJointControllerPtr
armarx::KinematicSubUnit::ActuatorData::getActiveController() const
{
    if (ctrlPos->isControllerActive())
    {
        return ctrlPos;
    }
    if (ctrlVel->isControllerActive())
    {
        return ctrlVel;
    }
    if (ctrlTor->isControllerActive())
    {
        return ctrlTor;
    }
    return nullptr;
}