diff --git a/source/armarx/control/components/control_skill_provider/Component.cpp b/source/armarx/control/components/control_skill_provider/Component.cpp
index efea3043895e74ac3341cd822fa60bf0b6212ca3..de1c6ddaa38d1599bdc57d4bc3f493d6093affb1 100644
--- a/source/armarx/control/components/control_skill_provider/Component.cpp
+++ b/source/armarx/control/components/control_skill_provider/Component.cpp
@@ -11,30 +11,30 @@ namespace armarx::control::components::control_skill_provider
::armarx::PropertyDefinitionsPtr def =
new ::armarx::ComponentPropertyDefinitions(getConfigIdentifier());
- def->component(remote_.kinematicUnit);
+ def->component(remote.kinematicUnit);
- def->required(properties_.robotName, "RobotName", "Name of the robot.");
+ def->required(properties.robotName, "RobotName", "Name of the robot.");
return def;
}
- Component::Component()
- {
- addPlugin(remote_.virtualRobotReader);
- }
-
void
Component::onInitComponent()
{
- skills_.moveJoints = addSkill<skills::MoveJoints>();
}
void
Component::onConnectComponent()
{
- skills_.moveJoints->connect({.kinematicUnit = remote_.kinematicUnit,
- .virtualRobotReader = remote_.virtualRobotReader->get(),
- .robotName = properties_.robotName});
+ armarx::armem::robot_state::VirtualRobotReader robotStateReader;
+ robotStateReader.connect(memoryNameSystem());
+
+ {
+ skills::MoveJoints::Remote skillRemote{.kinematicUnit = remote.kinematicUnit,
+ .virtualRobotReader = robotStateReader,
+ .robotName = properties.robotName};
+ addSkillFactory<skills::MoveJoints>(skillRemote);
+ }
}
void
diff --git a/source/armarx/control/components/control_skill_provider/Component.h b/source/armarx/control/components/control_skill_provider/Component.h
index 14ac9199ac989a4c4d011dc0c8335c98cdcb7100..214b560dce35e393960dd44679f6d365c3ca1ad6 100644
--- a/source/armarx/control/components/control_skill_provider/Component.h
+++ b/source/armarx/control/components/control_skill_provider/Component.h
@@ -3,6 +3,7 @@
#include <ArmarXCore/core/Component.h>
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
#include <RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h>
@@ -18,12 +19,11 @@ namespace armarx::control::components::control_skill_provider
class Component :
virtual public ::armarx::Component,
virtual public ::armarx::control::components::control_skill_provider::ComponentInterface,
- virtual public ::armarx::SkillProviderComponentPluginUser
+ virtual public ::armarx::SkillProviderComponentPluginUser,
+ virtual public ::armarx::armem::client::PluginUser
{
public:
- Component();
-
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
@@ -52,19 +52,12 @@ namespace armarx::control::components::control_skill_provider
struct Remote
{
::armarx::KinematicUnitInterfacePrx kinematicUnit;
- ::armarx::armem::client::plugins::ReaderWriterPlugin<
- ::armarx::armem::robot_state::VirtualRobotReader>* virtualRobotReader;
- } remote_;
+ } remote;
struct Properties
{
std::string robotName;
- } properties_;
-
- struct Skills
- {
- skills::MoveJoints* moveJoints;
- } skills_;
+ } properties;
static const std::string defaultName;
};
diff --git a/source/armarx/control/skills/skills/MoveJoints.cpp b/source/armarx/control/skills/skills/MoveJoints.cpp
index f500221f7c21ec8c3d0abda8ae5ddbdc3e0e3ac3..186932cd1012dd9e01210973678df36b078a03b8 100644
--- a/source/armarx/control/skills/skills/MoveJoints.cpp
+++ b/source/armarx/control/skills/skills/MoveJoints.cpp
@@ -5,20 +5,21 @@
namespace armarx::control::skills::skills
{
- class AbortedException : public std::exception
- {
- public:
- ~AbortedException() override = default;
- };
-
- MoveJoints::MoveJoints() : ::armarx::skills::SpecializedSkill<ParamType>(GetSkillDescription())
+ MoveJoints::MoveJoints(const Remote& r) :
+ ::armarx::skills::SimpleSpecializedSkill<ParamType>(GetSkillDescription()), remote(r)
{
}
- armarx::skills::Skill::InitResult
- MoveJoints::init(const SpecializedInitInput& in)
+ ::armarx::skills::Skill::ExitResult
+ MoveJoints::exit(const SpecializedExitInput& in)
{
- ARMARX_CHECK(remote_.has_value()); // connect(Remote) has been called
+ std::vector<std::string> jointNames;
+ for (const auto& [jointName, _] : in.parameters.jointsTargetValues)
+ {
+ jointNames.push_back(jointName);
+ }
+
+ remote.kinematicUnit->releaseJoints(jointNames);
return {::armarx::skills::TerminatedSkillStatus::Succeeded};
}
@@ -27,22 +28,22 @@ namespace armarx::control::skills::skills
{
std::vector<std::string> jointNames;
- for (const auto& [jointName, _] : in.params.jointsTargetValues)
+ for (const auto& [jointName, _] : in.parameters.jointsTargetValues)
{
jointNames.push_back(jointName);
}
// get VirtualRobot
const VirtualRobot::RobotPtr virtualRobot =
- remote_->virtualRobotReader.getRobot(remote_->robotName);
+ remote.virtualRobotReader.getRobot(remote.robotName);
if (not virtualRobot)
{
- ARMARX_ERROR << "Could not get robot '" << remote_->robotName << "'";
+ ARMARX_ERROR << "Could not get robot '" << remote.robotName << "'";
return {::armarx::skills::TerminatedSkillStatus::Failed};
}
// check jointsTargetValues are valid (joints exist, target values are within limits)
- if (not checkJointsTargetValuesValid(in.params.jointsTargetValues, virtualRobot))
+ if (not checkJointsTargetValuesValid(in.parameters.jointsTargetValues, virtualRobot))
{
return {::armarx::skills::TerminatedSkillStatus::Failed};
}
@@ -51,7 +52,7 @@ namespace armarx::control::skills::skills
ARMARX_INFO << "Requesting joints";
try
{
- remote_->kinematicUnit->requestJoints(jointNames);
+ remote.kinematicUnit->requestJoints(jointNames);
}
catch (armarx::KinematicUnitUnavailable e)
{
@@ -61,35 +62,26 @@ namespace armarx::control::skills::skills
}
// move joints
- ARMARX_INFO << "Moving joints to " << in.params.jointsTargetValues;
+ ARMARX_INFO << "Moving joints to " << in.parameters.jointsTargetValues;
- switchControlMode(in.params.jointsTargetValues, ePositionControl);
+ switchControlMode(in.parameters.jointsTargetValues, ePositionControl);
- try
+ if (in.parameters.inSimulation)
{
- if (in.params.inSimulation)
- {
- // trajectories from joint value updates through the KinematicUnit are not simulated
- // -> simulate manually
- moveJointsSimulation(in.params.jointsTargetValues,
- in.params.simulationJointSpeed,
- in.params.simulationJointSpeedOverride,
- virtualRobot);
- }
- else
- {
- moveJoints(in.params.jointsTargetValues,
- in.params.accuracy,
- in.params.accuracyOverride,
- virtualRobot);
- }
+ // trajectories from joint value updates through the KinematicUnit are not simulated
+ // -> simulate manually
+ moveJointsSimulation(in.parameters.jointsTargetValues,
+ in.parameters.simulationJointSpeed,
+ in.parameters.simulationJointSpeedOverride,
+ virtualRobot);
}
- catch (const AbortedException&) // skill got aborted
+ else
{
- remote_->kinematicUnit->releaseJoints(jointNames);
- return {::armarx::skills::TerminatedSkillStatus::Aborted};
+ moveJoints(in.parameters.jointsTargetValues,
+ in.parameters.accuracy,
+ in.parameters.accuracyOverride,
+ virtualRobot);
}
- remote_->kinematicUnit->releaseJoints(jointNames);
return {::armarx::skills::TerminatedSkillStatus::Succeeded};
}
@@ -125,7 +117,7 @@ namespace armarx::control::skills::skills
{
modeMap[jointName] = controlMode;
}
- remote_->kinematicUnit->switchControlMode(modeMap);
+ remote.kinematicUnit->switchControlMode(modeMap);
}
void
@@ -134,7 +126,7 @@ namespace armarx::control::skills::skills
const NameValueMap& accuracyOverride,
const VirtualRobot::RobotPtr& virtualRobot)
{
- remote_->kinematicUnit->setJointAngles(jointsTargetValues);
+ remote.kinematicUnit->setJointAngles(jointsTargetValues);
// wait until target values are reached
ARMARX_INFO << "Waiting until target values are reached";
@@ -142,9 +134,30 @@ namespace armarx::control::skills::skills
const Clock clock;
const Duration sleepTime = Duration::MilliSeconds(20);
+ auto do_if_terminate_and_not_reached = [&]()
+ {
+ ARMARX_INFO << "Skill got aborted, stopping movement";
+ syncVirtualRobot(virtualRobot);
+
+ // stop movement
+ switchControlMode(jointsTargetValues, eVelocityControl);
+
+ NameValueMap targetJointVelocities;
+ for (auto& [jointName, _] : jointsTargetValues)
+ {
+ targetJointVelocities[jointName] = 0;
+ }
+ remote.kinematicUnit->setJointVelocities(targetJointVelocities);
+
+ throwIfSkillShouldTerminate(); // throws exception
+
+ ARMARX_INFO << "Target values reached";
+ };
+
bool reached = false;
- while (not checkWhetherSkillShouldStopASAP() and not reached)
+ while (not reached)
{
+ throwIfSkillShouldTerminate(do_if_terminate_and_not_reached);
syncVirtualRobot(virtualRobot);
reached = true;
@@ -166,25 +179,6 @@ namespace armarx::control::skills::skills
}
clock.waitFor(sleepTime);
}
-
- if (not reached) // skill got aborted
- {
- ARMARX_INFO << "Skill got aborted, stopping movement";
- syncVirtualRobot(virtualRobot);
-
- // stop movement
- switchControlMode(jointsTargetValues, eVelocityControl);
-
- NameValueMap targetJointVelocities;
- for (auto& [jointName, _] : jointsTargetValues)
- {
- targetJointVelocities[jointName] = 0;
- }
- remote_->kinematicUnit->setJointVelocities(targetJointVelocities);
-
- throw AbortedException();
- }
- ARMARX_INFO << "Target values reached";
}
void
@@ -222,8 +216,9 @@ namespace armarx::control::skills::skills
const armarx::DateTime start = armarx::DateTime::Now();
double t = 0;
- while (not checkWhetherSkillShouldStopASAP() and t < maxDuration)
+ while (t < maxDuration)
{
+ throwIfSkillShouldTerminate();
t = std::min((armarx::DateTime::Now() - start).toSecondsDouble(), maxDuration);
NameValueMap nextValues;
@@ -233,17 +228,10 @@ namespace armarx::control::skills::skills
nextValues[name] = value;
}
- remote_->kinematicUnit->setJointAngles(nextValues);
+ remote.kinematicUnit->setJointAngles(nextValues);
clock.waitFor(sleepTime);
}
-
- if (t < maxDuration)
- {
- ARMARX_INFO << "Skill got aborted";
- throw AbortedException();
- }
-
ARMARX_INFO << "Target values reached";
}
@@ -251,7 +239,7 @@ namespace armarx::control::skills::skills
MoveJoints::syncVirtualRobot(const VirtualRobot::RobotPtr& virtualRobot)
{
ARMARX_CHECK(
- remote_->virtualRobotReader.synchronizeRobot(*virtualRobot, armarx::DateTime::Now()))
+ remote.virtualRobotReader.synchronizeRobot(*virtualRobot, armarx::DateTime::Now()))
<< "Robot state synchronization failed";
}
diff --git a/source/armarx/control/skills/skills/MoveJoints.h b/source/armarx/control/skills/skills/MoveJoints.h
index 8fda53d29cd8ceb763ff64214d0f80dadc7b2b19..40752ce4433a75cd3aef6d32e7730a2db50f7e5f 100644
--- a/source/armarx/control/skills/skills/MoveJoints.h
+++ b/source/armarx/control/skills/skills/MoveJoints.h
@@ -3,7 +3,7 @@
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
-#include <RobotAPI/libraries/skills/provider/SpecializedSkill.h>
+#include <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h>
#include <armarx/control/skills/skills/aron/MoveJointsParams.aron.generated.h>
@@ -18,42 +18,33 @@ namespace armarx::control::skills::skills
* simulate trajectory when in simulation (otherwise trajectories from joint value updates through
* the KinematicUnit are not simulated).
*/
- class MoveJoints : public ::armarx::skills::SpecializedSkill<MoveJointsParams>
+ class MoveJoints : public ::armarx::skills::SimpleSpecializedSkill<MoveJointsParams>
{
- public:
- static ::armarx::skills::SkillDescription
- GetSkillDescription()
- {
- return {.skillName = "MoveJoints",
- .description =
- "Moves joints to specified target values using the KinematicUnit",
- .robots = {},
- .timeout = ::armarx::Duration::Minutes(1),
- .acceptedType = ParamType::ToAronType()};
- }
-
- MoveJoints();
-
-
public:
struct Remote
{
- ::armarx::KinematicUnitInterfacePrx& kinematicUnit;
- ::armarx::armem::robot_state::VirtualRobotReader& virtualRobotReader;
- const std::string& robotName;
+ ::armarx::KinematicUnitInterfacePrx kinematicUnit;
+ ::armarx::armem::robot_state::VirtualRobotReader virtualRobotReader;
+ std::string robotName;
};
- void
- connect(const Remote& remote)
+ static ::armarx::skills::SkillDescription
+ GetSkillDescription()
{
- this->remote_.emplace(remote);
+ return {armarx::skills::SkillID("MoveJoints"),
+ "Moves joints to specified target values using the KinematicUnit",
+ nullptr,
+ ::armarx::Duration::Minutes(1),
+ ParamType::ToAronType()};
}
+ MoveJoints(const Remote&);
+
private:
- ::armarx::skills::Skill::InitResult init(const SpecializedInitInput& in) override;
::armarx::skills::Skill::MainResult main(const SpecializedMainInput& in) override;
+ ::armarx::skills::Skill::ExitResult exit(const SpecializedExitInput& in) override;
bool checkJointsTargetValuesValid(const NameValueMap& jointsTargetValues,
const VirtualRobot::RobotPtr& virtualRobot);
@@ -74,7 +65,7 @@ namespace armarx::control::skills::skills
void syncVirtualRobot(const VirtualRobot::RobotPtr& virtualRobot);
private:
- std::optional<Remote> remote_;
+ Remote remote;
};
} // namespace armarx::control::skills::skills