PlaceObject.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2014-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    RobotSkillTemplates::PlaceObjectGroup
 * @author     David ( david dot schiebener at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "PlaceObject.h"
 
#include <VirtualRobot/Grasping/Grasp.h>
#include <VirtualRobot/Grasping/GraspSet.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
#include <MemoryX/libraries/motionmodels/MotionModelStaticObject.h>
 
 
using namespace armarx;
using namespace PlaceObjectGroup;
 
// DO NOT EDIT NEXT LINE
PlaceObject::SubClassRegistry PlaceObject::Registry(PlaceObject::GetName(),
                                                    &PlaceObject::CreateInstance);
 
PlaceObject::PlaceObject(const XMLStateConstructorParams& stateData) :
    XMLStateTemplate<PlaceObject>(stateData), PlaceObjectGeneratedBase<PlaceObject>(stateData)
{
}
 
void
PlaceObject::onEnter()
{
    const float ZoffsetForPreAndPostPlacePoses = 120;
    const float xRetreatOffset = in.getXRetreatOffset();
    const float offsetForRetreatPoseY = 20; //100;
 
    PlaceObjectGroupStatechartContextBase* context =
        getContext<PlaceObjectGroupStatechartContextBase>();
 
    std::string handName = in.getHandChannelRef()->getDataField("className")->getString();
 
    local.setHandName(handName);
    //local.setHandShapeOpenName("open");
 
    std::string objectName =
        in.getObjectClassName(); //in.getObjectChannelRef()->getDataField("className")->getString();
    memoryx::ObjectClassBasePtr objectClassBase =
        context->getPriorKnowledge()->getObjectClassesSegment()->getObjectClassByName(objectName);
    Eigen::Matrix3f orientation = Eigen::Matrix3f::Identity();
    if (in.isPlaceOrientationSet()) // use custom orientation
    {
        FramedOrientationPtr ori = in.getPlaceOrientation();
        orientation =
            ori->toRootEigen(getRobotStateComponent()->getSynchronizedRobot()).block<3, 3>(0, 0);
    }
    Eigen::Matrix4f transformationFromTCPToObject = Eigen::Matrix4f::Identity();
 
    if (objectClassBase)
    {
        memoryx::ObjectClassPtr objectClass = memoryx::ObjectClassPtr::dynamicCast(objectClassBase);
        memoryx::GridFileManagerPtr fileManager(
            new memoryx::GridFileManager(context->getPriorKnowledge()->getCommonStorage()));
        objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
        memoryx::EntityWrappers::SimoxObjectWrapperPtr simoxWrapper =
            objectClass->getWrapper<memoryx::EntityWrappers::SimoxObjectWrapper>();
        Eigen::Vector3f placeOrientationAngles = simoxWrapper->getPutdownOrientationRPY();
 
        ARMARX_VERBOSE << VAROUT(placeOrientationAngles);
 
        /*placeOrientationAngles(0) = -1.5708;
        placeOrientationAngles(1) = 0;
        placeOrientationAngles(2) = 0;*/
        if (!in.isPlaceOrientationSet())
        {
            ARMARX_INFO << "Using placing orientation of the object (RPY): "
                        << placeOrientationAngles;
            orientation = Eigen::AngleAxisf(placeOrientationAngles(0), Eigen::Vector3f::UnitX()) *
                          Eigen::AngleAxisf(placeOrientationAngles(1), Eigen::Vector3f::UnitY()) *
                          Eigen::AngleAxisf(placeOrientationAngles(2), Eigen::Vector3f::UnitZ());
        }
        else
        {
            ARMARX_INFO << "Using placing orientation from input: " << orientation;
        }
 
        VirtualRobot::GraspSetPtr graspSet =
            simoxWrapper->getManipulationObject()->getGraspSet(in.getGraspSetName());
 
        if (graspSet)
        {
            VirtualRobot::GraspPtr grasp = graspSet->getGrasp(in.getGraspName());
 
            if (grasp)
            {
                transformationFromTCPToObject = grasp->getTransformation();
                ARMARX_VERBOSE << "Found grasp " << in.getGraspName() << ", trafo:\n"
                               << VAROUT(transformationFromTCPToObject);
            }
            else
            {
                ARMARX_WARNING << "Grasp " << in.getGraspName() << " not found in grasp set";
            }
        }
        else
        {
            ARMARX_WARNING << "Grasp set " << in.getGraspSetName() << " not found";
        }
    }
    else
    {
        ARMARX_WARNING << "Object class with name " << objectName
                       << " not found in the classes segment of PriorKnowledge";
    }
 
    Eigen::Matrix4f placePoseObjectLocal = Eigen::Matrix4f::Identity();
    placePoseObjectLocal.block<3, 3>(0, 0) = orientation;
    FramedPositionPtr placePoseTemp = in.getPlaceTargetPosition();
    placePoseTemp->changeFrame(context->getRobot(), context->getRobot()->getRootNode()->getName());
    placePoseObjectLocal.block<3, 1>(0, 3) = placePoseTemp->toEigen();
    armarx::FramedPosePtr placeObjectPoseGobal(
        new FramedPose(placePoseObjectLocal,
                       context->getRobot()->getRootNode()->getName(),
                       context->getRobotStateComponent()->getSynchronizedRobot()->getName()));
    ARMARX_IMPORTANT << "Placing pose of the object: " << *placeObjectPoseGobal;
    placeObjectPoseGobal->changeToGlobal(context->getRobotStateComponent()->getSynchronizedRobot());
    //context->getDebugDrawerTopic()->setPoseVisu("PlacingPoses", "PlaceTargetObject", placeObjectPoseGobal);
    getEntityDrawerTopic()->setObjectVisu(
        "PlaceObject", objectName, objectClassBase, placeObjectPoseGobal);
    getEntityDrawerTopic()->updateObjectColor("PlaceObject", objectName, DrawColor{0, 0, 0, 0.3});
 
    // just for debugging
    //    if (objectClassBase)
    //    {
    //        memoryx::ObjectClassPtr objectClass = memoryx::ObjectClassPtr::dynamicCast(objectClassBase);
    //        memoryx::GridFileManagerPtr fileManager(new memoryx::GridFileManager(context->getPriorKnowledge()->getCommonStorage()));
    //        objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
    //        memoryx::EntityWrappers::SimoxObjectWrapperPtr simoxWrapper = objectClass->getWrapper<memoryx::EntityWrappers::SimoxObjectWrapper>();
    //        Eigen::Vector3f placeOrientationAnglesVisu = simoxWrapper->getPutdownOrientationRPY();
    //        Eigen::Matrix3f orientationVisu;
    //        orientationVisu = Eigen::AngleAxisf(placeOrientationAnglesVisu(0), Eigen::Vector3f::UnitX())
    //                          * Eigen::AngleAxisf(placeOrientationAnglesVisu(1), Eigen::Vector3f::UnitY())
    //                          * Eigen::AngleAxisf(placeOrientationAnglesVisu(2), Eigen::Vector3f::UnitZ());
    //        Eigen::Matrix4f placePoseVisu = placeObjectPoseGobal->toEigen();
    //        placePoseVisu.block<3, 3>(0, 0) = orientationVisu;
    //        armarx::FramedPosePtr placePoseVisuFramed(new FramedPose(placePoseVisu, context->getRobot()->getRootNode()->getName(), context->getRobotStateComponent()->getSynchronizedRobot()->getName()));
    //        getEntityDrawerTopic()->setObjectVisu("PlaceObjectVisu", objectName, objectClassBase, placePoseVisuFramed);
    //        getEntityDrawerTopic()->updateObjectColor("PlaceObjectVisu", objectName, DrawColor {0.5, 0.5, 0, 0.3});
    //        // more debugging
    //        VirtualRobot::MathTools::rpy2eigen4f(placeOrientationAnglesVisu(0), placeOrientationAnglesVisu(1), placeOrientationAnglesVisu(2), placePoseVisu);
    //        placePoseVisu.block<3, 1>(0, 3) = placeObjectPoseGobal->toEigen().block<3, 1>(0, 3);
    //        placePoseVisuFramed = new FramedPose(placePoseVisu, context->getRobot()->getRootNode()->getName(), context->getRobotStateComponent()->getSynchronizedRobot()->getName());
    //        getEntityDrawerTopic()->setObjectVisu("PlaceObjectVisu2", objectName, objectClassBase, placePoseVisuFramed);
    //        getEntityDrawerTopic()->updateObjectColor("PlaceObjectVisu2", objectName, DrawColor {0.0, 0.5, 0.5, 0.3});
    //    }
 
    Eigen::Matrix4f placePoseTcpLocal =
        placePoseObjectLocal * transformationFromTCPToObject.inverse();
    ARMARX_IMPORTANT << "TCP target for putdown: " << placePoseTcpLocal;
    local.setObjectFinalPose(placeObjectPoseGobal);
    armarx::FramedPose placePoseTcpFramed(
        placePoseTcpLocal,
        context->getRobot()->getRootNode()->getName(),
        context->getRobotStateComponent()->getSynchronizedRobot()->getName());
    local.setPlacePose(placePoseTcpFramed);
    armarx::FramedPosePtr placePoseGobal(new FramedPose(placePoseTcpFramed));
    placePoseGobal->changeToGlobal(context->getRobotStateComponent()->getSynchronizedRobot());
    context->getDebugDrawerTopic()->setPoseVisu("PlacingPoses", "PlaceTargetHand", placePoseGobal);
 
    // add z offset
    placePoseTcpLocal(2, 3) += ZoffsetForPreAndPostPlacePoses;
    armarx::FramedPose prePlacePose(
        placePoseTcpLocal,
        context->getRobot()->getRootNode()->getName(),
        context->getRobotStateComponent()->getSynchronizedRobot()->getName());
    local.setPrePlacePose(prePlacePose);
    armarx::FramedPosePtr prePlacePoseGobal(new FramedPose(prePlacePose));
    prePlacePoseGobal->changeToGlobal(context->getRobotStateComponent()->getSynchronizedRobot());
    context->getDebugDrawerTopic()->setPoseVisu(
        "PlacingPoses", "PrePlaceTarget", prePlacePoseGobal);
 
    bool useLeftHand = false;
 
    if (handName.find("left") != std::string::npos || handName.find("Left") != std::string::npos)
    {
        useLeftHand = true;
        ARMARX_VERBOSE << "Using left hand";
    }
    else
    {
        ARMARX_VERBOSE << "Using right hand";
    }
 
    // add x offset
    if (useLeftHand)
    {
        placePoseTcpLocal(0, 3) -= xRetreatOffset;
    }
    else
    {
        placePoseTcpLocal(0, 3) += xRetreatOffset;
    }
 
    // add y offset
    placePoseTcpLocal(1, 3) -= offsetForRetreatPoseY;
 
    armarx::FramedPose retreatPose(
        placePoseTcpLocal,
        context->getRobot()->getRootNode()->getName(),
        context->getRobotStateComponent()->getSynchronizedRobot()->getName());
    local.setRetreatPose(retreatPose);
    armarx::FramedPosePtr retreatPoseGobal(new FramedPose(retreatPose));
    retreatPoseGobal->changeToGlobal(context->getRobotStateComponent()->getSynchronizedRobot());
    //context->getDebugDrawerTopic()->setPoseVisu("PlacingPoses", "RetreatTarget", retreatPoseGobal);
 
    ARMARX_VERBOSE << "Pre-place pose: " << prePlacePose;
    ARMARX_VERBOSE << "Place pose: " << placePoseTcpLocal;
    ARMARX_VERBOSE << "Retreat pose: " << retreatPose;
 
    Eigen::Matrix3f ori = placePoseTcpLocal.block<3, 3>(0, 0);
    Eigen::Vector3f placeOrientationRPY = ori.eulerAngles(0, 1, 2);
    ARMARX_VERBOSE << "Place orientation angles: " << placeOrientationRPY;
    ori.transposeInPlace();
    placeOrientationRPY = ori.eulerAngles(0, 1, 2);
    ARMARX_VERBOSE << "Place orientation inverse angles: " << placeOrientationRPY;
 
 
    ARMARX_VERBOSE << "Requesting TCP control unit";
 
    ARMARX_VERBOSE << "Requested TCP control unit";
 
    std::string speechOutput = "I will now put down the " + objectName;
    ARMARX_INFO << "Speech output: '" << speechOutput << "'";
    context->getTextToSpeech()->reportText(speechOutput);
 
    local.setDecelerationTimeForStopping(1000);
}
 
void
PlaceObject::onExit()
{
    std::string objectName = in.getObjectClassName();
    getEntityDrawerTopic()->removeObjectVisu("PlaceObject", objectName);
    getDebugDrawerTopic()->removeLayer("PlacingPoses");
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
PlaceObject::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new PlaceObject(stateData));
}