HandUnitDynamicSimulation.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    ArmarXCore::units
 * @author     Nikolaus Vahrenkamp (vahrenkamp at kit dot edu)
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "HandUnitDynamicSimulation.h"
 
#include <cmath>
 
#include <Eigen/Geometry>
 
#include <VirtualRobot/EndEffector/EndEffector.h>
#include <VirtualRobot/RobotConfig.h>
 
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h>
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
 
 
using namespace armarx;
 
void HandUnitDynamicSimulation::onInitHandUnit()
{
    ARMARX_INFO << "Init hand unit" << flush;
    simulatorPrxName = getProperty<std::string>("SimulatorProxyName").getValue();
    usingProxy(simulatorPrxName);
 
    kinematicUnitName = getProperty<std::string>("KinematicUnitName").getValue();
    usingProxy(kinematicUnitName);
 
    robotStateComponentName = getProperty<std::string>("RobotStateComponentName").getValue();
    usingProxy(robotStateComponentName);
 
    if (getProperty<bool>("UseLegacyWorkingMemory").getValue())
    {
        usingProxy("WorkingMemory");
    }
}
 
void HandUnitDynamicSimulation::onStartHandUnit()
{
    ARMARX_INFO << "Starting hand unit" << flush;
    simulatorPrx = getProxy<SimulatorInterfacePrx>(simulatorPrxName);
    ARMARX_INFO << "Using simulator proxy: " << simulatorPrxName << ":" << simulatorPrx << flush;
 
    kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
 
    workingMemoryPrx = getProxy<memoryx::WorkingMemoryInterfacePrx>("WorkingMemory", false, "", false);
    robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>(robotStateComponentName);
}
 
 
void HandUnitDynamicSimulation::onExitHandUnit()
{
 
}
 
 
/*
void HandUnitDynamicSimulation::open(const Ice::Current &c)
{
    ARMARX_INFO << "Open Hand: setting all hand joint value targets to zero" << flush;
    NameValueMap targetJointAngles;
 
    // set all joints to zero
 
    std::map<std::string, float>::iterator it = handJoints.begin();
    while (it != handJoints.end())
    {
        targetJointAngles[it->first] = 0.0f;
        it++;
    }
    ARMARX_DEBUG << "targetJointAngles:" << targetJointAngles << flush;
    simulatorPrx->begin_actuateRobotJointsPos(robotName,targetJointAngles);
}*/
 
 
PropertyDefinitionsPtr HandUnitDynamicSimulation::createPropertyDefinitions()
{
    auto defs = PropertyDefinitionsPtr(
               new HandUnitDynamicSimulationPropertyDefinitions(getConfigIdentifier()));
 
    defs->optional(objectPoseStorageName, "cmp.ObjectPoseStorageName",
                   "Name of the object pose storage (only used if necessary).");
 
    return defs;
}
 
 
void HandUnitDynamicSimulation::setObjectGrasped(const std::string& objectName, const Ice::Current&)
{
    ARMARX_INFO << "Object grasped " << objectName << flush;
    graspedObject = objectName;
 
    // inform simulator
    if (simulatorPrx)
    {
        ARMARX_INFO << "Simulator call objectGrasped: " << robotName << "," << tcpName << "," << objectName << flush;
        simulatorPrx->objectGrasped(robotName, tcpName, objectName);
    }
    else
    {
        ARMARX_WARNING << "No Simulator..." << flush;
    }
}
 
void HandUnitDynamicSimulation::setObjectReleased(const std::string& objectName, const Ice::Current&)
{
    ARMARX_INFO << "Object released " << objectName << flush;
    graspedObject = "";
 
    // inform simulator
    if (simulatorPrx)
    {
        simulatorPrx->objectReleased(robotName, tcpName, objectName);
    }
    else
    {
        ARMARX_WARNING << "No Simulator..." << flush;
    }
}
 
NameValueMap HandUnitDynamicSimulation::getCurrentJointValues(const Ice::Current& c)
{
    NameValueMap result;
 
    try
    {
        for (const auto& j : handJoints)
        {
            result[j.first] = simulatorPrx->getRobotJointAngle(robotName, j.first);
        }
    }
    catch (...)
    {
        ARMARX_WARNING << "Could not get joint angles fvrom simulator...";
    }
 
    return result;
}
 
void armarx::HandUnitDynamicSimulation::setShape(const std::string& shapeName, const Ice::Current&)
{
    std::string myShapeName = shapeName;
    ARMARX_INFO << "Setting shape " << myShapeName;
 
    if (!eef)
    {
        ARMARX_WARNING << "No EEF";
        return;
    }
 
 
    if (!eef->hasPreshape(myShapeName))
    {
        ARMARX_INFO << "Shape with name " << myShapeName << " not known in eef " << eef->getName() << ". Looking for partial match";
 
        bool foundMatch = false;
 
        for (const std::string& name : eef->getPreshapes())
        {
            if (name.find(myShapeName) != std::string::npos)
            {
                foundMatch = true;
                myShapeName = name;
                ARMARX_INFO << "Using matching shape: " << name;
                break;
            }
        }
 
        if (!foundMatch)
        {
            ARMARX_WARNING << "No match found for " << myShapeName << " in eef " << eef->getName() << " available shapes: " << eef->getPreshapes();
            return;
        }
    }
 
    VirtualRobot::RobotConfigPtr config = eef->getPreshape(myShapeName);
    std::map < std::string, float > jointAngles = config->getRobotNodeJointValueMap();
 
    NameControlModeMap controlModes;
 
    for (std::pair<std::string, float> pair : jointAngles)
    {
        controlModes.insert(std::make_pair(pair.first, ePositionControl));
    }
 
    kinematicUnitPrx->switchControlMode(controlModes);
    kinematicUnitPrx->setJointAngles(jointAngles);
}
 
 
void armarx::HandUnitDynamicSimulation::setShapeWithObjectInstance(
        const std::string& shapeName, const std::string& objectInstanceName, const Ice::Current& c)
{
    std::string myShapeName = shapeName;
    ARMARX_INFO << "Setting shape " << myShapeName << " while checking for collision with " << objectInstanceName;
 
    if (!eef)
    {
        ARMARX_WARNING << "No EEF";
        return;
    }
 
    if (!eef->hasPreshape(myShapeName))
    {
        ARMARX_INFO << "Shape with name " << myShapeName << " not known in eef " << eef->getName() << ". Looking for partial match";
 
        bool foundMatch = false;
        for (const std::string& name : eef->getPreshapes())
        {
            if (name.find(myShapeName) != std::string::npos)
            {
                foundMatch = true;
                myShapeName = name;
                ARMARX_INFO << "Using matching shape: " << name;
                break;
            }
        }
 
        if (!foundMatch)
        {
            ARMARX_WARNING << "No match found for " << myShapeName << " in eef " << eef->getName() << " available shapes: " << eef->getPreshapes();
            return;
        }
    }
 
    VirtualRobot::EndEffectorPtr endeffector = robot->getEndEffector(eef->getName());
    ARMARX_CHECK_EQUAL(endeffector.get(), eef.get());  // Is this really always the same??
 
    auto loadFromObjectPoseStorage = [this, &objectInstanceName]() -> VirtualRobot::ManipulationObjectPtr
    {
        const armarx::ObjectID objectID(objectInstanceName);
 
        armarx::objpose::ObjectPoseClient client;
        auto fetchObjectPose = [this, &objectID, &client]() -> std::optional<Eigen::Matrix4f>
        {
            getProxy(client.objectPoseStorage, objectPoseStorageName, false, "", false);
            if (client.objectPoseStorage)
            {
                const objpose::ObjectPoseMap objectPoses = client.fetchObjectPosesAsMap();
                if (auto it = objectPoses.find(objectID); it != objectPoses.end())
                {
                    return it->second.objectPoseGlobal;
                }
            }
            return std::nullopt;
        };
        if (auto objectPose = fetchObjectPose())
        {
            if (std::optional<armarx::ObjectInfo> info = client.getObjectFinder().findObject(objectID))
            {
                VirtualRobot::ManipulationObjectPtr object = client.getObjectFinder().loadManipulationObject(info);
                object->setGlobalPose(objectPose.value());
                return object;
            }
        }
        return nullptr;
    };
 
    auto loadFromWorkingMemory = [this, &objectInstanceName]() -> VirtualRobot::ManipulationObjectPtr
    {
        memoryx::ObjectInstancePtr objInstance =
                memoryx::ObjectInstancePtr::dynamicCast(workingMemoryPrx->getObjectInstancesSegment()->getObjectInstanceByName(objectInstanceName));
 
        VirtualRobot::RobotPtr robot = RemoteRobot::createLocalCloneFromFile(robotStateComponentPrx, VirtualRobot::RobotIO::eCollisionModel);
        RemoteRobot::synchronizeLocalClone(robot, robotStateComponentPrx);
 
        const std::string objectClassName = objInstance->getMostProbableClass();
        // Assure complete ontology tree is in object classes segment
        auto classes = workingMemoryPrx->getObjectClassesSegment()->addPriorClassWithSubclasses(objectClassName);
        if (classes.empty())
        {
            ARMARX_WARNING << "Class '" << objectClassName << "' not found ";
            return nullptr;
        }
 
        memoryx::ObjectClassPtr objclass = memoryx::ObjectClassPtr::dynamicCast(
                    workingMemoryPrx->getObjectClassesSegment()->getEntityByName(objectClassName)->ice_clone());
        ARMARX_CHECK_NOT_NULL(objclass);
 
        memoryx::GridFileManagerPtr fileManager(new memoryx::GridFileManager(workingMemoryPrx->getCommonStorage()));
        memoryx::EntityWrappers::SimoxObjectWrapperPtr simoxObject =
                objclass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
 
        objInstance->setPose(objInstance->getPose()->toGlobal(robot));
        simoxObject->updateFromEntity(objInstance);
 
        return simoxObject->getManipulationObject();
    };
 
    VirtualRobot::ManipulationObjectPtr manipulationObject = nullptr;
    if (not manipulationObject)
    {
        manipulationObject = loadFromObjectPoseStorage();
    }
    if (not manipulationObject and workingMemoryPrx)
    {
        manipulationObject = loadFromWorkingMemory();
    }
 
    if (manipulationObject)
    {
        endeffector->closeActors(manipulationObject);
 
        const std::map<std::string, float> jointAngles = endeffector->getConfiguration()->getRobotNodeJointValueMap();
 
        NameControlModeMap controlModes;
        for (const auto& [name, value] : jointAngles)
        {
            controlModes.emplace(name, ePositionControl);
        }
 
        kinematicUnitPrx->switchControlMode(controlModes);
        kinematicUnitPrx->setJointAngles(jointAngles);
    }
    else
    {
        ARMARX_WARNING << "Could not load object '" << objectInstanceName << "'. "
                       << "Cannot set shape '" << shapeName << "' with collision check. "
                       << "Setting shape '" << shapeName << "' without collision check instead.";
        setShape(shapeName, c);
        return;
    }
}
 
 
void armarx::HandUnitDynamicSimulation::setJointAngles(const NameValueMap& jointAngles, const Ice::Current&)
{
    NameControlModeMap controlModes;
 
    for (std::pair<std::string, float> pair : jointAngles)
    {
        controlModes.insert(std::make_pair(pair.first, ePositionControl));
    }
 
    kinematicUnitPrx->switchControlMode(controlModes);
    kinematicUnitPrx->setJointAngles(jointAngles);
    simulatorPrx->actuateRobotJointsPos(getRobotNameFromHandUnitName(), handUnitJointsToRobotJoints(jointAngles));
}
 
armarx::NameValueMap HandUnitDynamicSimulation::handUnitJointsToRobotJoints(const NameValueMap& joints)
{
    ARMARX_VERBOSE << getRobotNameFromHandUnitName();
    ARMARX_VERBOSE << getHandSideFromHandUnitName();
    ARMARX_VERBOSE << joints;
    std::map<std::string, std::vector<std::string>> conversion_dict;
    conversion_dict["Fingers"] = {"Index", "Middle", "Ring", "Pinky"};
    conversion_dict["Thumb"] = {"Thumb"};
    auto robot_joints = armarx::NameValueMap();
    std::string side = getHandSideFromHandUnitName();
    for (const auto& joint : joints)
    {
        if (conversion_dict.find(joint.first) != conversion_dict.end())
        {
            auto correspondences = conversion_dict.at(joint.first);
            for (const auto& correspondence : correspondences)
            {
                for (int i = 1; i < 4; i++)
                {
                    std::stringstream joint_id;
                    joint_id << correspondence << " " << side << " " << i << " Joint";
                    robot_joints[joint_id.str()] = joint.second * M_PI_2;
                }
            }
        } else
        {
            ARMARX_WARNING << "No corresponding finger joint found for " << joint.first;
        }
    }
    return robot_joints;
}
 
std::string HandUnitDynamicSimulation::getRobotNameFromHandUnitName()
{
    return armarx::split(robotName, " ").front();
}
 
std::string HandUnitDynamicSimulation::getHandSideFromHandUnitName()
{
    auto side = armarx::split(robotName, " ").back();
    std::stringstream stream;
    stream << side.front();
    return stream.str();
}