AddAndAttachObjectInWorkingMemory.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    RobotSkillTemplates::MemoryXUtility
 * @author     Fabian Paus ( fabian dot paus at kit dot edu )
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "AddAndAttachObjectInWorkingMemory.h"
 
#include <SimoxUtility/math/pose/pose.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXCore/core/time/CycleUtil.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
 
#include <MemoryX/core/GridFileManager.h>
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
#include <MemoryX/libraries/motionmodels/MotionModelAttachedToOtherObject.h>
 
//#include <ArmarXCore/core/time/TimeUtil.h>
//#include <ArmarXCore/observers/variant/DatafieldRef.h>
 
using namespace armarx;
using namespace MemoryXUtility;
 
// DO NOT EDIT NEXT LINE
AddAndAttachObjectInWorkingMemory::SubClassRegistry
    AddAndAttachObjectInWorkingMemory::Registry(AddAndAttachObjectInWorkingMemory::GetName(),
                                                &AddAndAttachObjectInWorkingMemory::CreateInstance);
 
void
AddAndAttachObjectInWorkingMemory::onEnter()
{
}
 
void
AddAndAttachObjectInWorkingMemory::run()
{
    memoryx::ObjectInstanceMemorySegmentBasePrx instanceMemory =
        getWorkingMemory()->getObjectInstancesSegment();
    std::string objectClassName = in.getObjectClassName();
 
    Eigen::Matrix4f transformationOffset = in.getTransformationOffset()->toEigen();
 
    SharedRobotInterfacePrx sharedRobot = getRobotStateComponent()->getSynchronizedRobot();
    VirtualRobot::RobotPtr robot = RemoteRobot::createLocalClone(sharedRobot);
    VirtualRobot::RobotNodeSetPtr kinematicChain =
        robot->getRobotNodeSet(in.getAttachToTCPofKinematicChain());
    VirtualRobot::RobotNodePtr tcp = kinematicChain->getTCP();
    Eigen::Matrix4f tcpPose = tcp->getGlobalPose();
 
    RobotNameHelperPtr nameHelper = getRobotNameHelper();
    Eigen::Matrix4f objectPose = tcpPose * transformationOffset;
 
 
    std::string objectID = ""; //<-- The output value
    if (objectClassName.find("/") != std::string::npos)
    {
        ARMARX_INFO << "Attach object '" << objectClassName << "' to new ObjectMemory!";
 
        armarx::objpose::AttachObjectToRobotNodeInput input;
        armarx::ObjectID oID(objectClassName);
        input.objectID = toIce(oID);
        input.providerName = ""; // Optional
        input.frameName = "Hand R TCP"; // ToDo: get as argument
        input.agentName = "Armar6";
        input.poseInFrame = armarx::PosePtr(
            new armarx::Pose(transformationOffset *
                             simox::math::pose(Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ()))));
        armarx::objpose::AttachObjectToRobotNodeOutput result =
            getObjectPoseStorage()->attachObjectToRobotNode(input);
        if (!result.success)
        {
            ARMARX_WARNING << "Failed to attach object '" << input.objectID << "' to frame '"
                           << input.frameName << "' of agent '" << input.agentName << "'.";
        }
        objectID = oID.str();
    }
    else
    {
        ARMARX_INFO << "Attach object '" << objectClassName << "' to old WorkingMemory!";
 
        std::string memoryHandName = nameHelper->getArm(in.getSide()).getMemoryHandName();
        auto objClassBase =
            getPriorKnowledge()->getObjectClassesSegment()->getObjectClassByName(objectClassName);
        memoryx::ObjectClassPtr objectClass = memoryx::ObjectClassPtr::dynamicCast(objClassBase);
        if (objectClass)
        {
            memoryx::GridFileManagerPtr fileManager(
                new memoryx::GridFileManager(getPriorKnowledge()->getCommonStorage()));
            objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
            memoryx::EntityWrappers::SimoxObjectWrapperPtr simoxWrapper =
                objectClass->getWrapper<memoryx::EntityWrappers::SimoxObjectWrapper>();
            Eigen::Vector3f placeOrientationAngles = simoxWrapper->getPutdownOrientationRPY();
            Eigen::Matrix3f orientationVisu =
                (Eigen::AngleAxisf(placeOrientationAngles(0), Eigen::Vector3f::UnitX()) *
                 Eigen::AngleAxisf(placeOrientationAngles(1), Eigen::Vector3f::UnitY()) *
                 Eigen::AngleAxisf(placeOrientationAngles(2), Eigen::Vector3f::UnitZ()))
                    .toRotationMatrix();
            objectPose.block<3, 3>(0, 0) = objectPose.block<3, 3>(0, 0) * orientationVisu;
        }
        else
        {
            ARMARX_WARNING << "ObjectClass is not set!";
        }
 
        // Get channel for hand
        ChannelRefPtr handChannel;
        if (in.isHandChannelSet())
        {
            handChannel = in.getHandChannel();
        }
        else
        {
            ARMARX_INFO << "Using memory hand name: " << memoryHandName;
            std::string handChannelClass = memoryHandName;
 
            std::vector<ChannelRefBasePtr> objectInstanceList =
                getObjectMemoryObserver()->getObjectInstancesByClass(handChannelClass);
            ARMARX_INFO << "found " << objectInstanceList.size() << " instances for object "
                        << handChannelClass;
            if (objectInstanceList.size())
            {
                handChannel = ChannelRefPtr::dynamicCast(objectInstanceList.front());
            }
            else
            {
                ARMARX_WARNING << "No hand channel defined. Trying to request hand localization.";
 
                ARMARX_INFO << "Requesting localization for object class '" << handChannelClass;
 
                int cycleTimeInMS = static_cast<int>(1000.0f * 0.2f);
                int localizationPriority = 50;
                getObjectMemoryObserver()->requestObjectClassRepeated(
                    handChannelClass, cycleTimeInMS, localizationPriority);
 
                // Wait for localization
                IceUtil::Time startTime = TimeUtil::GetTime();
                bool timeout = false;
                CycleUtil c(10);
                while (!isRunningTaskStopped())
                {
                    c.waitForCycleDuration();
 
                    std::vector<ChannelRefBasePtr> objectInstanceList =
                        getObjectMemoryObserver()->getObjectInstancesByClass(handChannelClass);
                    ARMARX_INFO << deactivateSpam(1) << "Found " << objectInstanceList.size()
                                << " instances for object " << handChannelClass;
 
                    if (!objectInstanceList.empty())
                    {
                        handChannel = ChannelRefPtr::dynamicCast(objectInstanceList.front());
                        if (!handChannel || !handChannel->hasDatafield("pose"))
                        {
                            ARMARX_WARNING << "Unable to cast channel reference or datafield pose "
                                              "does not exists.";
                            emitFailure();
                            return;
                        }
                        else
                        {
                            break;
                        }
                    }
 
                    IceUtil::Time now = TimeUtil::GetTime();
                    double timeElapsed = (now - startTime).toSecondsDouble();
                    timeout = timeElapsed > in.getLocalizationTimeoutInSeconds();
                    if (timeout)
                    {
                        ARMARX_WARNING << "Could not localize hand '" << handChannelClass << "' in "
                                       << in.getLocalizationTimeoutInSeconds() << " seconds.";
                        emitFailure();
                        return;
                    }
                }
            }
        }
 
        memoryx::MotionModelAttachedToOtherObjectPtr motionModel(
            new memoryx::MotionModelAttachedToOtherObject(getRobotStateComponent(), handChannel));
        objectID =
            instanceMemory->addObjectInstance(objectClassName,
                                              objectClassName,
                                              new LinkedPose(objectPose, GlobalFrame, sharedRobot),
                                              motionModel);
    }
 
    if (objectID.empty())
    {
        ARMARX_ERROR << "The output value 'objectID' is empty. It should be either set from the "
                        "old or the new memory.";
    }
    out.setNewObjectID(objectID);
    emitSuccess();
}
 
//void AddAndAttachObjectInWorkingMemory::onBreak()
//{
//    // put your user code for the breaking point here
//    // execution time should be short (<100ms)
//}
 
void
AddAndAttachObjectInWorkingMemory::onExit()
{
    // put your user code for the exit point here
    // execution time should be short (<100ms)
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
AddAndAttachObjectInWorkingMemory::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new AddAndAttachObjectInWorkingMemory(stateData));
}