Merge branch 'ShapeHand-skill' into 'main'

Implement 'ShapeHand'-skill

See merge request !40

source/armarx/control/components/control_skill_provider/Component.cpp

@@ -4,6 +4,7 @@
 
 #include <armarx/control/skills/skills/MoveJointsToPosition.h>
 #include <armarx/control/skills/skills/MoveJointsWithVelocity.h>
+#include <armarx/control/skills/skills/ShapeHand.h>
 
 namespace armarx::control::components::control_skill_provider
 {
@@ -15,6 +16,8 @@ namespace armarx::control::components::control_skill_provider
             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.");
 
@@ -43,6 +46,17 @@ namespace armarx::control::components::control_skill_provider
                 addSkillFactory<armarx::control::skills::skills::MoveJointsWithVelocity>(
                     skillRemote);
             }
+            {
+                armarx::control::skills::skills::ShapeHand::Remote skillRemote{
+                    remote.leftHandUnit, remote.rightHandUnit};
+                addSkillFactory<armarx::control::skills::skills::ShapeHand>(skillRemote);
+            }
+            {
+                addSkillFactory<armarx::control::skills::skills::OpenHand>();
+            }
+            {
+                addSkillFactory<armarx::control::skills::skills::CloseHand>();
+            }
         }
     }
 

source/armarx/control/components/control_skill_provider/Component.h

@@ -2,6 +2,7 @@
 
 #include <ArmarXCore/core/Component.h>
 
+#include <RobotAPI/interface/units/HandUnitInterface.h>
 #include <RobotAPI/interface/units/KinematicUnitInterface.h>
 #include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
 #include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
@@ -48,6 +49,8 @@ namespace armarx::control::components::control_skill_provider
         struct Remote
         {
             ::armarx::KinematicUnitInterfacePrx kinematicUnit;
+            ::armarx::HandUnitInterfacePrx leftHandUnit;
+            ::armarx::HandUnitInterfacePrx rightHandUnit;
         } remote;
 
         struct Properties

source/armarx/control/skills/CMakeLists.txt

@@ -2,6 +2,7 @@ armarx_add_aron_library(skills_aron
     ARON_FILES
         skills/aron/MoveJointsToPositionParams.xml
         skills/aron/MoveJointsWithVelocityParams.xml
+        skills/aron/ShapeHandParams.xml
 )
 
 armarx_add_library(skills
@@ -9,11 +10,13 @@ armarx_add_library(skills
         skills.cpp
         skills/MoveJointsToPosition.cpp
         skills/MoveJointsWithVelocity.cpp
+        skills/ShapeHand.cpp
 
     HEADERS
         skills.h
         skills/MoveJointsToPosition.h
         skills/MoveJointsWithVelocity.h
+        skills/ShapeHand.h
 
     DEPENDENCIES_PUBLIC
 
@@ -24,6 +27,7 @@ armarx_add_library(skills
         # RobotAPI
         RobotAPISkills
         armem_robot_state
+        RobotAPIUnits
         RobotAPISimpleTrajectory
 
         # armarx_control

source/armarx/control/skills/skills/ShapeHand.cpp

+#include "ShapeHand.h"
+
+#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
+
+namespace armarx::control::skills::skills
+{
+    ShapeHand::ShapeHand(const Remote& r) :
+        ::armarx::skills::SpecializedSkill<ShapeHandParams>(GetSkillDescription()), remote(r)
+    {
+    }
+
+    ::armarx::skills::Skill::MainResult
+    ShapeHand::main()
+    {
+        // verify exactly one of the parameters 'shapeName' and 'jointAngles' is set
+        if ((not getParameters().shapeName) and (not getParameters().jointAngles))
+        {
+            ARMARX_ERROR << "Exactly one of the parameters 'shapeName' and 'jointAngles' must be "
+                            "set. However, neither has been set.";
+            return MakeFailedResult();
+        }
+        else if (getParameters().shapeName and getParameters().jointAngles)
+        {
+            ARMARX_ERROR << "Exactly one of the parameters 'shapeName' and 'jointAngles' must be "
+                            "set. However, both have been set.";
+            return MakeFailedResult();
+        }
+
+        // set shape/joinAngles
+
+        auto& handUnit = getHandUnit(getParameters().hand);
+
+        if (getParameters().shapeName)
+        {
+            const auto& shape = getParameters().shapeName.value();
+
+            // check that the shape is defined
+            const auto defShapes =
+                ::armarx::SingleTypeVariantListPtr::dynamicCast(handUnit->getShapeNames())
+                    ->toStdVector<std::string>();
+
+            if (not std::any_of(defShapes.begin(),
+                                defShapes.end(),
+                                [&shape](const auto& defShape) { return shape == defShape; }))
+            {
+                ARMARX_ERROR << "Shape with name '" << shape
+                             << "' not defined. Defined shapes: " << defShapes;
+                return MakeFailedResult();
+            }
+
+            // set shape
+            ARMARX_INFO << "Setting shape of hand '" << handUnit->getHandName() << "' to '" << shape
+                        << "'.";
+
+            handUnit->setShape(shape);
+
+            if (getParameters().waitUntilFinished)
+            {
+                return waitUntilFinished(handUnit->getShapeJointValues(shape));
+            }
+        }
+
+        else // getParameters().jointAngles
+        {
+            const auto jointAngles = getParameters().jointAngles.value();
+
+            ARMARX_INFO << "Setting joint angles of hand '" << handUnit->getHandName()
+                        << "' to: " << jointAngles;
+
+            handUnit->setJointAngles(jointAngles);
+
+            if (getParameters().waitUntilFinished)
+            {
+                return waitUntilFinished(jointAngles);
+            }
+        }
+        return MakeSucceededResult();
+    }
+
+    armarx::skills::Skill::MainResult
+    ShapeHand::waitUntilFinished(const NameValueMap& targetAngles)
+    {
+        auto& handUnit = getHandUnit(getParameters().hand);
+        const auto movementTimeout = getParameters().waitUntilFinished->movementTimeout;
+
+        ARMARX_INFO << "Waiting until target position is reached"
+                    << (movementTimeout ? " or movement times out" : "");
+
+        struct
+        {
+            NameValueMap angles;
+            ::armarx::DateTime time;
+        } pastJointAngles{handUnit->getCurrentJointValues(), DateTime::Now()};
+
+        const Clock clock;
+        const auto sleepTime = Duration::MilliSeconds(20);
+
+        bool reached = false;
+        bool movementTimedOut = false;
+
+        while (not shouldSkillTerminate() and not reached and not movementTimedOut)
+        {
+            const auto currentAngles = handUnit->getCurrentJointValues();
+
+            // check if target reached
+            reached = true;
+            for (const auto& [jointName, targetAngle] : targetAngles)
+            {
+                if (std::abs(targetAngle - currentAngles.at(jointName)) > getAccuracy(jointName))
+                {
+                    reached = false;
+                    ARMARX_VERBOSE
+                        << deactivateSpam(1) << "Delta for joint '" << jointName
+                        << "' is: " << std::abs(targetAngle - currentAngles.at(jointName)) << " > "
+                        << getAccuracy(jointName) << "(accuracy)";
+                    break;
+                }
+            }
+
+            // check if movement timed out
+            if (movementTimeout and (DateTime::Now() - pastJointAngles.time).toSecondsDouble() >=
+                                        movementTimeout.value())
+            {
+                movementTimedOut = true;
+                for (const auto& [jointName, targetAngle] : targetAngles)
+                {
+                    if (std::abs(currentAngles.at(jointName) -
+                                 pastJointAngles.angles.at(jointName)) > getAccuracy(jointName))
+                    {
+                        movementTimedOut = false;
+                        ARMARX_VERBOSE
+                            << deactivateSpam(1) << "Joint '" << jointName << "' still moving: "
+                            << std::abs(currentAngles.at(jointName) -
+                                        pastJointAngles.angles.at(jointName))
+                            << " > " << getAccuracy(jointName) << "(accuracy)"
+                            << "in the last " << movementTimeout.value() << "s (movement timeout)";
+                        break;
+                    }
+                }
+                pastJointAngles = {currentAngles, DateTime::Now()};
+            }
+
+            clock.waitFor(sleepTime);
+        }
+
+        if (reached)
+        {
+            ARMARX_INFO << "Target position reached!";
+        }
+        else if (movementTimedOut)
+        {
+            ARMARX_INFO << "Movement timed out! Joints stopped moving. Not waiting anymore until "
+                           "target postion is reached";
+        }
+        else if (shouldSkillTerminate())
+        {
+            ARMARX_IMPORTANT
+                << "Skill aborted. Not waiting anymore until target position is reached";
+            return MakeAbortedResult();
+        }
+
+        return MakeSucceededResult();
+    }
+
+    HandUnitInterfacePrx&
+    ShapeHand::getHandUnit(const Hand hand)
+    {
+        if (hand == ::armarx::control::skills::skills::Hand::Left)
+        {
+            return remote.leftHandUnit;
+        }
+        else
+        {
+            return remote.rightHandUnit;
+        }
+    }
+
+    float
+    ShapeHand::getAccuracy(const std::string& jointName)
+    {
+        const auto accuracy = getParameters().waitUntilFinished->accuracy;
+        const auto& accuracyOverride = getParameters().waitUntilFinished->accuracyOverride;
+
+        return accuracyOverride.count(jointName) > 0 ? accuracyOverride.at(jointName) : accuracy;
+    }
+
+    OpenHand::OpenHand() :
+        armarx::skills::SpecializedSkill<OpenCloseHandParams>(GetSkillDescription())
+    {
+    }
+
+    ::armarx::skills::Skill::MainResult
+    OpenHand::main()
+    {
+        // call ShapeHand-skill
+        ::armarx::skills::SkillID id;
+        id.skillName = "ShapeHand";
+        id.providerId = getSkillId().providerId;
+
+        ShapeHandParams params;
+        params.hand = getParameters().hand;
+        params.shapeName = "Open";
+        params.waitUntilFinished = getParameters().waitUntilFinished;
+
+        return {(*callSubskill(::armarx::skills::SkillProxy(manager, id), params.toAron())).status};
+    }
+
+    CloseHand::CloseHand() :
+        armarx::skills::SpecializedSkill<OpenCloseHandParams>(GetSkillDescription())
+    {
+    }
+
+    ::armarx::skills::Skill::MainResult
+    CloseHand::main()
+    {
+        // call ShapeHand-skill
+        ::armarx::skills::SkillID id;
+        id.skillName = "ShapeHand";
+        id.providerId = getSkillId().providerId;
+
+        ShapeHandParams params;
+        params.hand = getParameters().hand;
+        params.shapeName = "Close";
+        params.waitUntilFinished = getParameters().waitUntilFinished;
+
+        return {(*callSubskill(::armarx::skills::SkillProxy(manager, id), params.toAron())).status};
+    }
+} // namespace armarx::control::skills::skills

source/armarx/control/skills/skills/ShapeHand.h

+#pragma once
+
+#include <RobotAPI/interface/units/HandUnitInterface.h>
+#include <RobotAPI/libraries/skills/provider/SpecializedSkill.h>
+
+#include <armarx/control/skills/skills/aron/ShapeHandParams.aron.generated.h>
+
+namespace armarx::control::skills::skills
+{
+
+    class ShapeHand : public ::armarx::skills::SpecializedSkill<ShapeHandParams>
+    {
+
+    public:
+        static ::armarx::skills::SkillDescription
+        GetSkillDescription()
+        {
+            return ::armarx::skills::SkillDescription{
+                .skillId = {.skillName = "ShapeHand"},
+                .description =
+                    "Shapes the hand according to a predefined shape or passed joint angles",
+                .timeout = ::armarx::Duration::Minutes(1),
+                .parametersType = ParamType::ToAronType()};
+        }
+
+        struct Remote
+        {
+            ::armarx::HandUnitInterfacePrx& leftHandUnit;
+            ::armarx::HandUnitInterfacePrx& rightHandUnit;
+        };
+
+        ShapeHand(const Remote&);
+
+    private:
+        ::armarx::skills::Skill::MainResult main() override;
+        Skill::MainResult waitUntilFinished(const NameValueMap& targetAngles);
+
+    private:
+        Remote remote;
+        ::armarx::HandUnitInterfacePrx& getHandUnit(const Hand);
+
+        float getAccuracy(const std::string& jointName);
+    };
+
+    class OpenHand : public ::armarx::skills::SpecializedSkill<OpenCloseHandParams>
+    {
+    public:
+        static ::armarx::skills::SkillDescription
+        GetSkillDescription()
+        {
+            return ::armarx::skills::SkillDescription{.skillId = {.skillName = "OpenHand"},
+                                                      .description = "Opens the hand",
+                                                      .timeout = ::armarx::Duration::Minutes(1),
+                                                      .parametersType = ParamType::ToAronType()};
+        }
+
+        OpenHand();
+
+    private:
+        ::armarx::skills::Skill::MainResult main() override;
+    };
+
+    class CloseHand : public ::armarx::skills::SpecializedSkill<OpenCloseHandParams>
+    {
+    public:
+        static ::armarx::skills::SkillDescription
+        GetSkillDescription()
+        {
+            return ::armarx::skills::SkillDescription{.skillId = {.skillName = "CloseHand"},
+                                                      .description = "Closes the hand",
+                                                      .timeout = ::armarx::Duration::Minutes(1),
+                                                      .parametersType = ParamType::ToAronType()};
+        }
+
+        CloseHand();
+
+    private:
+        ::armarx::skills::Skill::MainResult main() override;
+    };
+
+} // namespace armarx::control::skills::skills

source/armarx/control/skills/skills/aron/ShapeHandParams.xml

+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+    <GenerateTypes>
+
+        <IntEnum name="::armarx::control::skills::skills::Hand">
+            <EnumValue key="Left" value="0"/>
+            <EnumValue key="Right" value="1"/>
+        </IntEnum>
+
+        <!-- Parameters to define when the skill should terminate (if waiting) -->
+        <Object name="::armarx::control::skills::skills::WaitUntilFinishedParams">
+
+            <!-- Each joint must be within this accuracy around the target angle for the skill
+                 to terminate -->
+            <ObjectChild key="accuracy">
+                <float />
+            </ObjectChild>
+
+            <!-- joint name -> accuracy -->
+            <ObjectChild key="accuracyOverride">
+                <Dict>
+                    <float />
+                </Dict>
+            </ObjectChild>
+
+            <!-- If set the skill also terminates when no joint moves more than 'accuracy' in
+                'movementTimeout' seconds (e.g. when joint is blocked) -->
+            <ObjectChild key="movementTimeout">
+                <float optional="true" />
+            </ObjectChild>
+
+        </Object>
+
+
+        <!-- 'ShapeHand'-skill parameters -->
+        <Object name="::armarx::control::skills::skills::ShapeHandParams">
+
+            <ObjectChild key="hand">
+                <::armarx::control::skills::skills::Hand />
+            </ObjectChild>
+
+            <!-- Must provide exactly one of the parameters 'shapeName' and 'jointAngles' -->
+
+            <!-- name of predefined hand shape -->
+            <ObjectChild key="shapeName">
+                <string optional="true" />
+            </ObjectChild>
+
+            <!-- joint name -> target value -->
+            <ObjectChild key="jointAngles">
+                <Dict optional="true">
+                    <float />
+                </Dict>
+            </ObjectChild>
+
+            <!-- If set, the skill blocks until target angles are reached / angles don't change
+                 anymore. Otherwise, the skill terminates instantly. When in simulation,
+                 'simulateMovement' must be set in order for this to work. -->
+            <ObjectChild key="waitUntilFinished">
+                 <::armarx::control::skills::skills::WaitUntilFinishedParams optional="true" />
+            </ObjectChild>
+
+        </Object>
+
+
+        <!-- 'OpenHand'/'CloseHand'-skills parameters -->
+        <Object name="::armarx::control::skills::skills::OpenCloseHandParams">
+
+            <ObjectChild key="hand">
+                <::armarx::control::skills::skills::Hand />
+            </ObjectChild>
+
+            <!-- If set, the skill blocks until target angles are reached / angles don't change
+                 anymore. Otherwise, the skill terminates instantly. When in simulation,
+                 'simulateMovement' must be set in order for this to work. -->
+            <ObjectChild key="waitUntilFinished">
+                 <::armarx::control::skills::skills::WaitUntilFinishedParams optional="true" />
+            </ObjectChild>
+
+        </Object>
+
+    </GenerateTypes>
+</AronTypeDefinition>