GraspProviderExample.cpp

Go to the documentation of this file.

#include "GraspProviderExample.h"
 
#include <SimoxUtility/color/cmaps.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/Metronome.h>
 
#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/core/Pose.h>
 
namespace armarx
{
    GraspProviderExamplePropertyDefinitions::GraspProviderExamplePropertyDefinitions(
        std::string prefix) :
        armarx::ComponentPropertyDefinitions(prefix)
    {
    }
 
    armarx::PropertyDefinitionsPtr
    GraspProviderExample::createPropertyDefinitions()
    {
        ARMARX_IMPORTANT << "Prperty defs";
        armarx::PropertyDefinitionsPtr defs =
            new GraspProviderExamplePropertyDefinitions(getConfigIdentifier());
 
        defs->topic(debugObserver);
 
        defs->optional(memoryName, "mem.MemoryName", "Name of the memory to use.");
 
        return defs;
    }
 
    std::string
    GraspProviderExample::getDefaultName() const
    {
        return "GraspProviderExample";
    }
 
    GraspProviderExample::GraspProviderExample()
    {
    }
 
    void
    GraspProviderExample::onInitComponent()
    {
        ARMARX_IMPORTANT << "Init";
    }
 
    void
    GraspProviderExample::onConnectComponent()
    {
        writer.connect(memoryNameSystem());
        reader.connect(memoryNameSystem());
        task = new RunningTask<GraspProviderExample>(this, &GraspProviderExample::run);
        task->start();
    }
 
    void
    GraspProviderExample::onDisconnectComponent()
    {
        task->stop();
    }
 
    void
    GraspProviderExample::onExitComponent()
    {
    }
 
    void
    GraspProviderExample::run()
    {
        ARMARX_IMPORTANT << "Running example.";
 
        Metronome m(Duration::MilliSeconds(1000));
        int i = 0;
 
 
        while (!task->isStopped() && i++ < 100)
        {
            // initialize all necessary fields of a grasp candidate and use writer to commit it to memory
            grasping::GraspCandidate candidate = makeDummyGraspCandidate();
            candidate.groupNr = i; //non-necessary field, but used to commit different candidates
 
            writer.commitGraspCandidate(candidate, armem::Time::Now(), "candidateProvider");
 
            // initialize all necessary fields of a bimanual grasp candidate and use writer to commit it to memory
            grasping::BimanualGraspCandidate bimanualCandidate = makeDummyBimanualGraspCandidate();
            bimanualCandidate.groupNr =
                i; //non-necessary field, but used to commit different candidates
 
            writer.commitBimanualGraspCandidate(
                bimanualCandidate, armem::Time::Now(), "bimanualProvider");
 
 
            //test for writing Seqs, candidates from the same object appear as instances of the same snapshot
            grasping::GraspCandidateSeq candidatesToWrite;
            candidatesToWrite.push_back(new grasping::GraspCandidate(candidate));
            candidate.side = "Left";
            candidatesToWrite.push_back(new grasping::GraspCandidate(candidate));
 
 
            writer.commitGraspCandidateSeq(
                candidatesToWrite, armem::Time::Now(), "candidateProvider");
 
            // test reader and debug by logging the group number of the candidate
 
            std::map<std::string, grasping::GraspCandidatePtr> candidates;
 
            try
            {
                candidates = reader.queryLatestGraspCandidates();
            }
            catch (armem::error::QueryFailed& e)
            {
                ARMARX_ERROR << e.makeMsg(memoryName);
            }
 
 
            for (auto& [id, ca] : candidates)
            {
                ARMARX_INFO << "candidate with ID " << id << " has group number " << ca->groupNr;
            }
 
            std::map<std::string, grasping::BimanualGraspCandidatePtr> bimanualCandidates;
 
            try
            {
                bimanualCandidates = reader.queryLatestBimanualGraspCandidates();
            }
            catch (armem::error::QueryFailed& e)
            {
                ARMARX_ERROR << e.makeMsg(memoryName);
            }
 
            for (auto& [id, ca] : bimanualCandidates)
            {
                ARMARX_INFO << "bimanual candidate with ID " << id << " has group number "
                            << ca->groupNr;
            }
 
            m.waitForNextTick();
        }
    }
 
    grasping::GraspCandidate
    GraspProviderExample::makeDummyGraspCandidate()
    {
        armarx::grasping::GraspCandidate candidate = armarx::grasping::GraspCandidate();
 
        candidate.approachVector = Vector3BasePtr(toIce(zeroVector));
        candidate.graspPose = PoseBasePtr(toIce(identityMatrix));
        candidate.providerName = "Example";
        candidate.side = "Right";
        candidate.robotPose = PoseBasePtr(toIce(identityMatrix));
        // tcpPose is optional, however it is needed to visualize with arviz
        candidate.tcpPoseInHandRoot = PoseBasePtr(toIce(identityMatrix));
        // source Info is also not necessary, but reference object name is used as entity name
        // "UnknownObject" if none is provided
        candidate.sourceInfo = new grasping::GraspCandidateSourceInfo();
        candidate.sourceInfo->referenceObjectName = "Box";
        candidate.sourceInfo->bbox = new grasping::BoundingBox();
        candidate.sourceInfo->bbox->center = Vector3BasePtr(toIce(zeroVector));
        candidate.sourceInfo->bbox->ha1 = Vector3BasePtr(toIce(zeroVector));
        candidate.sourceInfo->bbox->ha2 = Vector3BasePtr(toIce(zeroVector));
        candidate.sourceInfo->bbox->ha3 = Vector3BasePtr(toIce(zeroVector));
        candidate.sourceInfo->referenceObjectPose = PoseBasePtr(toIce(identityMatrix));
 
        return candidate;
    }
 
    grasping::BimanualGraspCandidate
    GraspProviderExample::makeDummyBimanualGraspCandidate()
    {
        armarx::grasping::BimanualGraspCandidate bimanualCandidate =
            armarx::grasping::BimanualGraspCandidate();
 
        bimanualCandidate.approachVectorLeft = Vector3BasePtr(toIce(zeroVector));
        bimanualCandidate.approachVectorRight = Vector3BasePtr(toIce(zeroVector));
        bimanualCandidate.graspPoseLeft = PoseBasePtr(toIce(Eigen::Matrix4f()));
        bimanualCandidate.graspPoseRight = PoseBasePtr(toIce(Eigen::Matrix4f()));
        bimanualCandidate.providerName = "BimanualExample";
        bimanualCandidate.robotPose = PoseBasePtr(toIce(identityMatrix));
        bimanualCandidate.tcpPoseInHandRootRight = PoseBasePtr(toIce(identityMatrix));
        bimanualCandidate.tcpPoseInHandRootLeft = PoseBasePtr(toIce(identityMatrix));
        bimanualCandidate.inwardsVectorLeft = Vector3BasePtr(toIce(zeroVector));
        bimanualCandidate.inwardsVectorRight = Vector3BasePtr(toIce(zeroVector));
        return bimanualCandidate;
    }
 
} // namespace armarx