Component.cpp

Go to the documentation of this file.

#include "Component.h"
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <armarx/control/pointing/skills/PointAt.h>
#include <armarx/control/skills/skills/ExecuteTrajectory.h>
#include <armarx/control/skills/skills/GuidingWithCollAvoidance.h>
#include <armarx/control/skills/skills/MoveJointsToNamedConfiguration.h>
#include <armarx/control/skills/skills/MoveJointsToPosition.h>
#include <armarx/control/skills/skills/MoveJointsWithVelocity.h>
#include <armarx/control/skills/skills/RelaxHand.h>
#include <armarx/control/skills/skills/ShapeHand.h>
#include <armarx/control/skills/skills/ZeroTorque.h>
#include <armarx/control/skills/skills/meta/HomePose.h>
#include <armarx/control/skills/skills/meta/ZeroPose.h>
 
namespace armarx::control::components::control_skill_provider
{
 
    ::armarx::PropertyDefinitionsPtr
    Component::createPropertyDefinitions()
    {
        ::armarx::PropertyDefinitionsPtr def =
            new ::armarx::ComponentPropertyDefinitions(getConfigIdentifier());
 
        def->component(remote.kinematicUnit);
        def->component(remote.leftHandUnit, "LeftHandUnit", "LeftHandUnit");
        def->component(remote.rightHandUnit, "RightHandUnit", "RightHandUnit");
 
        def->required(properties.robotName, "RobotName", "Name of the robot.");
 
        return def;
    }
 
    void
    Component::onInitComponent()
    {
        {
            armarx::control::skills::config::ZeroTorque skillConfig;
            skillConfig.robotName = properties.robotName;
            skillBlueprints.zeroTorque = std::experimental::make_observer(
                addCustomSkillFactory<armarx::skills::SkillWithContextBlueprint<
                    armarx::control::skills::skills::ZeroTorque>>(skillConfig));
        }
    }
 
    void
    Component::onConnectComponent()
    {
        armarx::armem::robot_state::VirtualRobotReader robotStateReader;
        robotStateReader.connect(memoryNameSystem());
        ::armarx::control::client::ComponentPlugin* controlComponentPluginUser =
            &this->getControlComponentPlugin();
        {
            {
                armarx::control::skills::skills::ExecuteTrajectory::Services srv{
                    .controlComponentPluginUser = controlComponentPluginUser};
                addSkillFactory<armarx::control::skills::skills::ExecuteTrajectory>(srv);
            }
            {
                armarx::control::skills::skills::GuidingWithCollAvoidance::Services srv{
                    .controlComponentPluginUser = controlComponentPluginUser};
                addSkillFactory<armarx::control::skills::skills::GuidingWithCollAvoidance>(srv);
            }
            {
                armarx::control::skills::skills::MoveJointsToPosition::Services srv{
                    remote.kinematicUnit, robotStateReader, properties.robotName};
                addSkillFactory<armarx::control::skills::skills::MoveJointsToPosition>(srv);
            }
            {
                armarx::control::skills::skills::MoveJointsWithVelocity::Services srv{
                    remote.kinematicUnit};
                addSkillFactory<armarx::control::skills::skills::MoveJointsWithVelocity>(srv);
            }
            {
                armarx::control::skills::skills::MoveJointsToNamedConfiguration::Services srv{
                    robotStateReader, properties.robotName};
                addSkillFactory<armarx::control::skills::skills::MoveJointsToNamedConfiguration>(
                    srv);
            }
            {
                armarx::control::skills::HandUnitServices srv{remote.leftHandUnit,
                                                              remote.rightHandUnit};
                addSkillFactory<armarx::control::skills::skills::RelaxHand>(srv);
            }
            {
                armarx::control::skills::HandUnitServices srv{remote.leftHandUnit,
                                                              remote.rightHandUnit};
                addSkillFactory<armarx::control::skills::skills::ShapeHand>(srv);
            }
            {
                addSkillFactory<armarx::control::skills::skills::OpenHand>();
            }
            {
                addSkillFactory<armarx::control::skills::skills::CloseHand>();
            }
            {
                armarx::control::skills::skills::ZeroTorque::Context context{
                    .localRobot_ =
                        remote.robotReaderPlugin->get().getRobotWaiting(properties.robotName),
                    .robotUnitPlugin_ = remote.robotUnitPlugin};
                skillBlueprints.zeroTorque->connectTo(context);
            }
            {
                armarx::control::pointing::skills::PointAt::Remote remote{
                    properties.robotName, arviz, memoryNameSystem(), getTrajectoryPlayer()};
                addSkillFactory<armarx::control::pointing::skills::PointAt>(remote);
            }
            // meta skills
            {
                addSkillFactory<armarx::control::skills::skills::meta::HomePose>();
                addSkillFactory<armarx::control::skills::skills::meta::ZeroPose>();
            }
        }
    }
 
    void
    Component::onDisconnectComponent()
    {
    }
 
    void
    Component::onExitComponent()
    {
    }
 
    const std::string Component::defaultName = skills::constants::CONTROL_SKILL_PROVIDER_NAME;
 
    std::string
    Component::getDefaultName() const
    {
        return Component::defaultName;
    }
 
    std::string
    Component::GetDefaultName()
    {
        return Component::defaultName;
    }
 
    Component::Component()
    {
        addPlugin(remote.robotUnitPlugin);
        addPlugin(remote.robotReaderPlugin);
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
 
} // namespace armarx::control::components::control_skill_provider