Component.cpp

Go to the documentation of this file.

#include "Component.h"
 
#include <experimental/memory>
#include <string>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
#include <RobotAPI/libraries/skills/provider/blueprints/SkillWithContextBlueprint.h>
 
#include <armarx/control/client/ComponentPlugin.h>
#include <armarx/control/skills/aron/ZeroTorque.aron.generated.h>
#include <armarx/control/skills/constants/constants.h>
#include <armarx/control/skills/skills/ExecuteTrajectory.h>
#include <armarx/control/skills/skills/GuidingWithCollAvoidance.h>
#include <armarx/control/skills/skills/HandUnitServices.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/SwitchCoordinationMode.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.");
        def->optional(properties.homePoseConfiguration,
                      "homePoseConfiguration",
                      "Named configuration used as `Home` pose.");
        def->required(properties.disableWaitUntilFinished,
                      "disableWaitUntilFinished",
                      "This property disables the 'waitUntilFinished' functionality of the "
                      "'ShapeHands' skill when set to true. This is needed when no finger joint "
                      "encoder are available (e.g. when using the Vincent hand).");
 
        return def;
    }
 
    void
    Component::onInitComponent()
    {
        {
        }
        usingProxy("RobotStateMemory");
    }
 
    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::SwitchCoordinationMode::Services srv{
                    .controlComponentPluginUser = controlComponentPluginUser};
                addSkillFactory<armarx::control::skills::skills::SwitchCoordinationMode>(srv);
            }
            {
                armarx::control::skills::skills::GuidingWithCollAvoidance::Services srv{
                    .controlComponentPluginUser = controlComponentPluginUser};
                addSkillFactory<armarx::control::skills::skills::GuidingWithCollAvoidance>(srv);
            }
            {
                armarx::control::skills::skills::MoveJointsToPosition::Services srv{
                    .kinematicUnit = remote.kinematicUnit,
                    .virtualRobotReader = robotStateReader,
                    .robotName = 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{
                    .virtualRobotReader = robotStateReader, .robotName = properties.robotName};
                addSkillFactory<armarx::control::skills::skills::MoveJointsToNamedConfiguration>(
                    srv);
            }
            {
                armarx::control::skills::HandUnitServices srv{
                    .leftHandUnit = remote.leftHandUnit, .rightHandUnit = remote.rightHandUnit};
                addSkillFactory<armarx::control::skills::skills::RelaxHand>(srv);
            }
            {
                armarx::control::skills::HandUnitServices srv{
                    .leftHandUnit = remote.leftHandUnit, .rightHandUnit = remote.rightHandUnit};
                armarx::control::skills::skills::ShapeHand::Properties p{
                    .disableWaitUntilFinished = properties.disableWaitUntilFinished};
                addSkillFactory<armarx::control::skills::skills::ShapeHand>(srv, p);
            }
            {
                addSkillFactory<armarx::control::skills::skills::OpenHand>();
            }
            {
                addSkillFactory<armarx::control::skills::skills::CloseHand>();
            }
            {
                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));
                armarx::control::skills::skills::ZeroTorque::Context context{
                    .virtualRobotReader =
                        std::experimental::make_observer(&remote.robotReaderPlugin->get()),
                    .robotUnitPlugin_ = remote.robotUnitPlugin};
                skillBlueprints.zeroTorque->connectTo(context);
            }
 
            // This skill depends on SimoxControl. Hence, it should be moved to another skill provider
            // {
            //     armarx::control::pointing::skills::PointAt::Remote remote{
            //         properties.robotName, arviz, memoryNameSystem(), getTrajectoryPlayer()};
            //     addSkillFactory<armarx::control::pointing::skills::PointAt>(remote);
            // }
 
            {
                addSkillFactory<armarx::control::skills::skills::meta::HomePose>(
                    properties.homePoseConfiguration);
            }
 
            // meta skills
            {
                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