From c4c2a21de699e7de77cd8f8e324c70311a8881bd Mon Sep 17 00:00:00 2001
From: Jianfeng Gao <jianfeng.gao@kit.edu>
Date: Mon, 29 Jul 2024 19:16:37 +0200
Subject: [PATCH 01/22] add guard carry with skill
---
.../Armar6BoardSupportDemo.scgxml | 1 +
.../Armar6BoardSupportDemo/CMakeLists.txt | 4 +
.../GuardCarryingSkill.cpp | 827 ++++++++++++++++++
.../GuardCarryingSkill.h | 53 ++
.../GuardCarryingSkill.xml | 167 ++++
.../Armar6BoardSupportDemo/RemoveGuardV2.xml | 502 ++++++-----
6 files changed, 1315 insertions(+), 239 deletions(-)
create mode 100644 source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.cpp
create mode 100644 source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.h
create mode 100644 source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.xml
diff --git a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/Armar6BoardSupportDemo.scgxml b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/Armar6BoardSupportDemo.scgxml
index c09288f4f..e4f468012 100644
--- a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/Armar6BoardSupportDemo.scgxml
+++ b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/Armar6BoardSupportDemo.scgxml
@@ -142,6 +142,7 @@ ArmarX.Armar6BoardSupportDemoRemoteStateOfferer.OpenPoseEstimationName = OpenPos
<State filename="GoToGuardLowering.xml" visibility="public"/>
<State filename="GoToPickUp.xml"/>
<State filename="GuardCarrying.xml"/>
+ <State filename="GuardCarryingSkill.xml" visibility="public"/>
<State filename="GuardCarryingWithFailureDetection.xml"/>
<State filename="GuardCarryingWithObstacleAvoidance.xml"/>
<State filename="GuardInHandManipulate.xml"/>
diff --git a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/CMakeLists.txt b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/CMakeLists.txt
index a77f3ce9c..324a02c87 100644
--- a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/CMakeLists.txt
+++ b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/CMakeLists.txt
@@ -8,11 +8,15 @@ armarx_add_statechart_group(Armar6BoardSupportDemo
ArmarXCoreStatechart
ArmarXCoreObservers
RobotAPICore
+ RobotAPISkillsCore
+ RobotAPISkills
RobotComponentsInterfaces
ArViz
MemoryXCore
MemoryXMemoryTypes
armarx_control::deprecated_njoint_mp_controller_interface
+ armarx_control::skills_platform_follower_controller
+ armarx_control::platform_follower_aron
armar6_rt::interfaces
armar6_skills::interfaces
devices_ethercat::hand_armar6_v2_interfaces
diff --git a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.cpp b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.cpp
new file mode 100644
index 000000000..fa64581ae
--- /dev/null
+++ b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.cpp
@@ -0,0 +1,827 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package skills::Armar6BoardSupportDemo
+ * @author [Author Name] ( [Author Email] )
+ * @date 2024
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "GuardCarryingSkill.h"
+
+#include <RobotAPI/interface/skills/SkillManagerInterface.h>
+#include <RobotAPI/libraries/armem_gui/MemoryViewer.h>
+#include <RobotAPI/libraries/skills/core/ProviderID.h>
+#include <RobotAPI/libraries/skills/core/SkillExecutionID.h>
+#include <RobotAPI/libraries/skills/core/SkillExecutionRequest.h>
+#include <ArmarXCore/core/time/Frequency.h>
+#include <ArmarXCore/core/time/Metronome.h>
+
+#include <armarx/control/skills/skills/platform_follower_controller/aron/FollowerParams.aron.generated.h>
+#include <armarx/control/njoint_controller/platform/platform_follower_controller/aron/PlatformFollowerControllerConfig.aron.generated.h>
+
+#include <armar6/skills/statecharts/Armar6BoardSupportDemo/Armar6BoardSupportDemoStatechartContext.generated.h>
+
+///// copied
+#include <devices/ethercat/hand/armar6_v2/njoint_controller/ShapeInterface.h>
+
+#include <RobotAPI/libraries/core/math/MathUtils.h>
+#include <RobotAPI/libraries/core/PIDController.h>
+
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/MathTools.h>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+#include <RobotAPI/components/DebugDrawer/DebugDrawerHelper.h>
+#include <RobotAPI/interface/components/ObstacleAvoidance/ObstacleDetectionInterface.h>
+#include <RobotAPI/interface/components/ObstacleAvoidance/ObstacleAvoidanceInterface.h>
+#include <RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/components/ArViz/Client/ScopedClient.h>
+
+#include "Helpers.h"
+
+
+namespace armarx::Armar6BoardSupportDemo
+{
+ // DO NOT EDIT NEXT LINE
+ GuardCarryingSkill::SubClassRegistry GuardCarryingSkill::Registry(GuardCarryingSkill::GetName(), &GuardCarryingSkill::CreateInstance);
+
+ float signedMinFunction(float newValue, float minAbsValue)
+ {
+ return std::copysign(std::min<float>(fabs(newValue), minAbsValue), newValue);
+ }
+
+ void GuardCarryingSkill::onEnter()
+ {
+ // put your user code for the enter-point here
+ // execution time should be short (<100ms)
+ }
+
+ void GuardCarryingSkill::run()
+ {
+ ARMARX_INFO << "GuardCarryingSkill is running";
+ const std::string providerName = "bimanual_transport_skill_provider";
+ const std::string skillName = "Follower";
+ const armarx::skills::ProviderID providerId = {.providerName = providerName};
+
+ const armarx::skills::SkillID skillId{.providerId = providerId, .skillName = skillName};
+
+ auto followerParams = armarx::control::skills::skills::platform_follower_controller::arondto::FollowerParams();
+ followerParams.jointConfigFileName = "";
+
+ auto platformControlConfigParams = armarx::control::njoint_controller::platform::platform_follower_controller::arondto::Params();
+ platformControlConfigParams.dAngle = 0;
+ platformControlConfigParams.dPos = 0;
+ platformControlConfigParams.filterTimeConstant = 0;
+ platformControlConfigParams.kAngle = 8;
+ platformControlConfigParams.kPos = 12;
+ platformControlConfigParams.maxAngleVel = 0;
+ platformControlConfigParams.maxPosVel = 0;
+ platformControlConfigParams.minAngleTolerance = 0;
+ platformControlConfigParams.minPosTolerance = 0;
+ platformControlConfigParams.nodeSetNameLeft = "LeftArm";
+ platformControlConfigParams.nodeSetNameRight = "RightArm";
+
+ followerParams.platformControlConfig = platformControlConfigParams;
+
+ armarx::skills::SkillExecutionRequest skillExecutionRequest{
+ .skillId = skillId,
+ .executorName = "GuardCarryingSkillStatechart",
+ .parameters = followerParams.toAron()
+ };
+
+ ARMARX_CHECK(skillId.isFullySpecified()); // sanity check
+
+ armarx::skills::manager::dti::SkillManagerInterfacePrx skillMemory =
+ StateBase::getContext<armarx::Armar6BoardSupportDemo::Armar6BoardSupportDemoStatechartContext>()
+ ->getProxy<armarx::skills::manager::dti::SkillManagerInterfacePrx>("SkillMemory");
+
+ armarx::skills::manager::dto::SkillExecutionID skillExecutionId;
+ try
+ {
+ ARMARX_INFO << "Invoking control skill";
+ skillExecutionId = skillMemory->executeSkillAsync(skillExecutionRequest.toManagerIce());
+ }
+ catch (Ice::Exception const& e)
+ {
+ ARMARX_ERROR << "Unhandeled Ice exception while executing skill '" << skillId.skillName
+ << "'. Aborting..."
+ << "Execution failed of skill " << skillId.skillName << e.what();
+ emitFailure();
+ return;
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "Unhandled error while executing skill '" << skillId.skillName
+ << "'. Aborting...";
+ emitFailure();
+ return;
+ }
+
+ // ARMARX_INFO << "Control skill has been invoked; starting loop";
+ // armarx::Metronome metronome(armarx::Frequency::Hertz(30));
+ // while (!isRunningTaskStopped()) // stop run function if returning true
+ // {
+ // metronome.waitForNextTick();
+ // }
+
+
+ /// copied
+ getTextToSpeech()->reportText(in.getTextBeforeThreeSeconds());
+ viz::ScopedClient arviz(viz::Client::createFromTopic("GuardCarryingWithObstacleAvoidance", getArvizTopic()));
+
+
+ NJointControllerInterfacePrx controller = getRobotUnit()->getNJointController("LeftController");
+ if (controller)
+ {
+ getRobotUnit()->deactivateAndDeleteNJointController("LeftController");
+ TimeUtil::SleepMS(10);
+ }
+
+ controller = getRobotUnit()->getNJointController("RightController");
+ if (controller)
+ {
+ getRobotUnit()->deactivateAndDeleteNJointController("RightController");
+ TimeUtil::SleepMS(10);
+ }
+
+
+ std::map<std::string, ::armarx::MatrixFloatPtr> restrictPolygonMap = in.getRestrictPolygon();
+ std::map<std::string, ::armarx::Vector3Ptr> platFormTargetMap = in.getPlatformTargetPose();
+
+ Eigen::MatrixXf polygon;
+ Eigen::Vector3f platformTargetPose;
+ if (in.getIsGuardPlacement())
+ {
+ polygon = restrictPolygonMap.at("GuardPlacement")->toEigen();
+ platformTargetPose = platFormTargetMap.at("GuardPlacement")->toEigen();
+ }
+ else
+ {
+ polygon = restrictPolygonMap.at("GuardLifting")->toEigen();
+ platformTargetPose = platFormTargetMap.at("GuardLifting")->toEigen();
+ }
+
+ std::vector<Eigen::Vector2f> points;
+ for (int i = 0; i < polygon.rows(); ++i)
+ {
+ // Polygon is relative to target pose (but in global frame).
+ Eigen::Vector2f point = platformTargetPose.head<2>() + polygon.row(i).transpose();
+ points.push_back(point);
+ }
+ {
+ viz::Layer layer = arviz.layer("Goal Polygon");
+ viz::Polygon goalPolygon("Goal Polygon");
+ for (const Eigen::Vector2f& point : points)
+ {
+ goalPolygon.addPoint(Eigen::Vector3f(point.x(), point.y(), +1));
+ }
+ goalPolygon.lineColor(simox::Color::green()).lineWidth(5);
+ goalPolygon.color(simox::Color::green(255, 64));
+ layer.add(goalPolygon);
+ arviz.commit(layer);
+ }
+
+ VirtualRobot::MathTools::ConvexHull2DPtr convPolygon = VirtualRobot::MathTools::createConvexHull2D(points);
+
+ devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx leftHandcontroller;
+ if (!getRobotUnit()->getNJointController("Hand_L_EEF_NJointKITHandV2ShapeController"))
+ {
+ leftHandcontroller = StateBase::getContext<Armar6BoardSupportDemoStatechartContext>()->getProxy<devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx>("Hand_L_EEF_NJointKITHandV2ShapeController");
+ }
+ else
+ {
+ leftHandcontroller = devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx::checkedCast(getRobotUnit()->getNJointController("Hand_L_EEF_NJointKITHandV2ShapeController"));
+ }
+ leftHandcontroller->activateController();
+
+ devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx rightHandcontroller;
+ if (!getRobotUnit()->getNJointController("Hand_R_EEF_NJointKITHandV2ShapeController"))
+ {
+ rightHandcontroller = StateBase::getContext<Armar6BoardSupportDemoStatechartContext>()->getProxy<devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx>("Hand_R_EEF_NJointKITHandV2ShapeController");
+ }
+ else
+ {
+ rightHandcontroller = devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx::checkedCast(getRobotUnit()->getNJointController("Hand_R_EEF_NJointKITHandV2ShapeController"));
+ }
+ rightHandcontroller->activateController();
+
+ leftHandcontroller->setTargetsWithPwm(0, 2, 1, 1);
+ rightHandcontroller->setTargetsWithPwm(0, 2, 1, 1);
+
+
+ VirtualRobot::RobotPtr localRobot = getLocalRobot();
+ RemoteRobot::synchronizeLocalClone(localRobot, getRobotStateComponent());
+
+ Eigen::Matrix4f desiredPoseLeft;
+ if (in.isDesiredPoseLeftSet())
+ {
+ desiredPoseLeft = in.getDesiredPoseLeft()->toEigen();
+ }
+ else
+ {
+ desiredPoseLeft = localRobot->getRobotNodeSet("LeftArm")->getTCP()->getPoseInRootFrame();
+ out.setDesiredPoseLeft(localRobot->getRobotNodeSet("LeftArm")->getTCP()->getPoseInRootFrame());
+ }
+
+
+ Eigen::Matrix4f desiredPoseRight;
+ if (in.isDesiredPoseRightSet())
+ {
+ desiredPoseRight = in.getDesiredPoseRight()->toEigen();
+ }
+ else
+ {
+ desiredPoseRight = localRobot->getRobotNodeSet("RightArm")->getTCP()->getPoseInRootFrame();
+ out.setDesiredPoseRight(localRobot->getRobotNodeSet("RightArm")->getTCP()->getPoseInRootFrame());
+ }
+
+ const Eigen::Vector3f targetRight = desiredPoseRight.topRightCorner<3, 1>();
+ const Eigen::Vector3f targetLeft = desiredPoseLeft.topRightCorner<3, 1>();
+
+
+// const std::vector<float> Kpos = in.getKpos();
+// const std::vector<float> Kori = in.getKori();
+// const std::vector<float> Dpos = in.getDpos();
+// const std::vector<float> Dori = in.getDori();
+
+ const std::string nodeSetNameLeft = "LeftArm";
+ const std::string nodeSetNameRight = "RightArm";
+ VirtualRobot::RobotNodePtr tcpRight = localRobot->getRobotNodeSet(nodeSetNameRight)->getTCP();
+ VirtualRobot::RobotNodePtr tcpLeft = localRobot->getRobotNodeSet(nodeSetNameLeft)->getTCP();
+
+// std::vector<float> desiredJointPositionLeft;
+// if (in.isDesiredLeftJointValuesSet())
+// {
+// desiredJointPositionLeft = in.getDesiredLeftJointValues();
+// }
+// else
+// {
+// desiredJointPositionLeft = localRobot->getRobotNodeSet(nodeSetNameLeft)->getJointValues();
+// }
+
+// std::vector<float> desiredJointPositionRight;
+// if (in.isDesiredRightJointValuesSet())
+// {
+// desiredJointPositionRight = in.getDesiredRightJointValues();
+// }
+// else
+// {
+// desiredJointPositionRight = localRobot->getRobotNodeSet(nodeSetNameRight)->getJointValues();
+// }
+
+// std::vector<float> knull = in.getKnull();
+// std::vector<float> dnull = in.getDnull();
+
+// NJointTaskSpaceImpedanceControlConfigPtr leftConfig = new NJointTaskSpaceImpedanceControlConfig;
+// leftConfig->nodeSetName = nodeSetNameLeft;
+// leftConfig->desiredPosition = simox::math::mat4f_to_pos(desiredPoseLeft);
+// leftConfig->desiredOrientation = simox::math::mat4f_to_quat(desiredPoseLeft);
+// leftConfig->Kpos = Eigen::Map<const Eigen::Vector3f>(Kpos.data(), Kpos.size());
+// leftConfig->Kori = Eigen::Map<const Eigen::Vector3f>(Kori.data(), Kori.size());
+// leftConfig->Dpos = Eigen::Map<const Eigen::Vector3f>(Dpos.data(), Dpos.size());
+// leftConfig->Dori = Eigen::Map<const Eigen::Vector3f>(Dori.data(), Dori.size());
+// leftConfig->desiredJointPositions = Eigen::Map<const Eigen::VectorXf>(desiredJointPositionLeft.data(), desiredJointPositionLeft.size());
+// leftConfig->Knull = Eigen::Map<const Eigen::VectorXf>(knull.data(), knull.size());
+// leftConfig->Dnull = Eigen::Map<const Eigen::VectorXf>(dnull.data(), dnull.size());
+// leftConfig->torqueLimit = in.getTorqueLimit();
+
+// NJointTaskSpaceImpedanceControlConfigPtr rightConfig = new NJointTaskSpaceImpedanceControlConfig;
+// rightConfig->nodeSetName = nodeSetNameRight;
+// rightConfig->desiredPosition = simox::math::mat4f_to_pos(desiredPoseRight);
+// rightConfig->desiredOrientation = simox::math::mat4f_to_quat(desiredPoseRight);
+// rightConfig->Kpos = Eigen::Map<const Eigen::Vector3f>(Kpos.data(), Kpos.size());
+// rightConfig->Kori = Eigen::Map<const Eigen::Vector3f>(Kori.data(), Kori.size());
+// rightConfig->Dpos = Eigen::Map<const Eigen::Vector3f>(Dpos.data(), Dpos.size());
+// rightConfig->Dori = Eigen::Map<const Eigen::Vector3f>(Dori.data(), Dori.size());
+// rightConfig->desiredJointPositions = Eigen::Map<const Eigen::VectorXf>(desiredJointPositionRight.data(), desiredJointPositionRight.size());
+// rightConfig->Knull = Eigen::Map<const Eigen::VectorXf>(knull.data(), knull.size());
+// rightConfig->Dnull = Eigen::Map<const Eigen::VectorXf>(dnull.data(), dnull.size());
+// rightConfig->torqueLimit = in.getTorqueLimit();
+
+// NJointTaskSpaceImpedanceControlInterfacePrx leftController
+// = NJointTaskSpaceImpedanceControlInterfacePrx::checkedCast(getRobotUnit()->createNJointController("NJointTaskSpaceImpedanceController", "LeftController", leftConfig));
+// NJointTaskSpaceImpedanceControlInterfacePrx rightController
+// = NJointTaskSpaceImpedanceControlInterfacePrx::checkedCast(getRobotUnit()->createNJointController("NJointTaskSpaceImpedanceController", "RightController", rightConfig));
+
+
+// leftController->activateController();
+// rightController->activateController();
+
+
+ /* ---------------- platform ------------------- */
+
+ float Kplatform = in.getKplatform();
+ float Dplatform = in.getDplatform();
+
+ float maxVelocity = in.getMaxVelocity();
+
+ float KplatformAngle = in.getKplatformAngle();
+ float DplatformAngle = in.getDplatformAngle();
+ float maxAngularVelocity = in.getMaxAngularVelocity();
+
+ Eigen::Vector3f filteredLinearVel;
+ filteredLinearVel.setZero();
+ float filteredAngularVel = 0;
+
+ // currentTCPLinearVelocity_filtered = (1 - filterFactor) * currentTCPLinearVelocity_filtered + filterFactor * currentTCPLinearVelocity;
+
+ IceUtil::Time last = TimeUtil::GetTime();
+ float filterTimeConstant = 0.01; //0.05;
+
+ MultiDimPIDController pidTrans(in.getRestrictTranslationPID()->x, in.getRestrictTranslationPID()->y, in.getRestrictTranslationPID()->z);
+ PIDController pidRot(in.getRestrictRotationPID()->x, in.getRestrictRotationPID()->y, in.getRestrictRotationPID()->z);
+
+ DatafieldRefPtr platformVelX = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformVelocity", "velocityX"));
+ DatafieldRefPtr platformVelY = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformVelocity", "velocityY"));
+ DatafieldRefPtr platformRotVel = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformVelocity", "velocityRotation"));
+
+ DatafieldRefPtr platformPosX = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformPose", "positionX"));
+ DatafieldRefPtr platformPosY = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformPose", "positionY"));
+ DatafieldRefPtr platformRotAng = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformPose", "rotation"));
+
+ DatafieldRefPtr forceDfLeft = DatafieldRefPtr::dynamicCast(getForceTorqueObserver()->getForceDatafield("FT L_ArmL_FT"));
+ DatafieldRefPtr forceDfRight = DatafieldRefPtr::dynamicCast(getForceTorqueObserver()->getForceDatafield("FT R_ArmR_FT"));
+ Eigen::Vector3f initialForceLeft;
+ initialForceLeft.setZero();
+ Eigen::Vector3f initialForceRight;
+ initialForceRight.setZero();
+
+ usleep(100000);
+ // auto dd = getDebugDrawerTopic();
+
+
+// //**** use obstacle avoidance component *****//
+// ObstacleDetectionInterfacePrx obstacle_detection = getObstacleDetection();
+// ObstacleAvoidanceInterfacePrx obstacle_avoidance = getObstacleAvoidance();
+// // ObstacleAvoidanceInterfacePrx obstacle_avoidance = getObstacleAvoidance2D();
+// // obstacleavoidance::Config obstacle_avoidance_cfg = obstacle_avoidance->getConfig();
+
+// // clear_drawings(dd);
+
+ CycleUtil c(10);
+ // Eigen::Vector3f oldVisuPos = Eigen::Vector3f::Zero();
+ // int visuSphereCounter = 0;
+
+
+ /* ----------------- thumb feedback ------------------- */
+
+ DatafieldRefPtr thumbDFLeft = DatafieldRefPtr::dynamicCast(getKinematicUnitObserver()->getDatafieldRefByName("jointangles", "LeftHandThumb"));
+ DatafieldRefPtr thumbDFRight = DatafieldRefPtr::dynamicCast(getKinematicUnitObserver()->getDatafieldRefByName("jointangles", "RightHandThumb"));
+
+ IceUtil::Time start = TimeUtil::GetTime();
+ Eigen::Vector3f initialPositionRight;
+ Eigen::Vector3f initialPositionLeft;
+ Eigen::Vector3f initialDirection;
+ Eigen::Vector3f filteredPlatformTargetVelocity = Eigen::Vector3f::Zero();
+
+ Eigen::Vector3f dimensions;
+ dimensions << 805, 1205, 20;
+ bool isFirstLoop = true;
+ bool speechOutputAfterWait = true;
+ float minPosiTolerance = in.getMinPosiTolerance();
+ bool isImpedanceParameterReset = false;
+
+ obstacledetection::Obstacle obs3d;
+// //bool isObs3d = false;
+// if (in.isObs3DPositionSet())
+// {
+// obstacledetection::ObstacleList obstacleList = obstacle_detection->getObstacles();
+
+// for (size_t i = 0; i < obstacleList.size(); ++i)
+// {
+// obstacledetection::Obstacle obs = obstacleList.at(i);
+// if (obs.name == "obs3d")
+// {
+// ARMARX_INFO << "Removing obstacle 3d";
+// obstacle_detection->removeObstacle("obs3d");
+// obs3d = obs;
+// //isObs3d = true;
+// }
+// }
+
+// }
+
+ while (!isRunningTaskStopped())
+ {
+ RemoteRobot::synchronizeLocalClone(localRobot, getRobotStateComponent());
+
+ if (in.getIsConsiderFailure())
+ {
+ if (thumbDFLeft->getDataField()->getFloat() > in.getLeftThumbThreshold() || thumbDFRight->getDataField()->getFloat() > in.getRightThumbThreshold())
+ {
+ getPlatformUnit()->move(0, 0, 0);
+ emitLoseGuard();
+ return;
+ }
+ }
+ IceUtil::Time now = TimeUtil::GetTime();
+
+ /* Debug Drawer */
+ // if (in.isShowGuardSet() && in.getShowGuard())
+ // {
+ // std::string layerName = "guard_layer";
+ // std::string boxName = "guard";
+ // DrawColor color = {0.5, 0.5, 0.5, 1};
+ // }
+
+ /* Control Platform */
+ double timeDuration = (now - start).toSecondsDouble();
+ if (timeDuration < in.getMinExecutionTime() || isFirstLoop)
+ {
+ std::vector<float> currPoseLeft = Helpers::pose2vec(localRobot->getRobotNodeSet("LeftArm")->getTCP()->getPoseInRootFrame());
+ std::vector<float> currPoseRight = Helpers::pose2vec(localRobot->getRobotNodeSet("RightArm")->getTCP()->getPoseInRootFrame());
+
+ initialPositionRight << currPoseRight.at(0), currPoseRight.at(1), currPoseRight.at(2);
+ initialPositionLeft << currPoseLeft.at(0), currPoseLeft.at(1), currPoseLeft.at(2);
+ initialDirection = initialPositionRight - initialPositionLeft;
+ initialDirection = initialDirection / initialDirection.norm();
+
+ {
+ FramedDirectionPtr forcePtr = forceDfLeft->getDataField()->get<FramedDirection>();
+ initialForceLeft = forcePtr->toRootEigen(localRobot);
+ }
+ {
+ FramedDirectionPtr forcePtr = forceDfRight->getDataField()->get<FramedDirection>();
+ initialForceRight = forcePtr->toRootEigen(localRobot);
+ }
+
+ isFirstLoop = false;
+
+ continue;
+ }
+ if (speechOutputAfterWait)
+ {
+ getTextToSpeech()->reportText(in.getTextAfterWaitForZeroTorque());
+ speechOutputAfterWait = false;
+
+ const Eigen::Vector3f desiredPositionRightVec = desiredPoseRight.topRightCorner<3, 1>();
+ const Eigen::Vector3f desiredPositionLeftVec = desiredPoseLeft.topRightCorner<3, 1>();
+ const Eigen::Vector3f positionRightError = initialPositionRight - desiredPositionRightVec;
+ const Eigen::Vector3f positionLeftError = initialPositionLeft - desiredPositionLeftVec;
+ ARMARX_INFO << "differece Pose: " << positionLeftError.norm() << ", " << positionRightError.norm();
+ }
+
+ const Eigen::Vector3f currentPosition(
+ platformPosX->getDataField()->getFloat(),
+ platformPosY->getDataField()->getFloat(),
+ platformRotAng->getDataField()->getFloat()
+ );
+
+ ARMARX_DEBUG << "currentGuardPosition: " << currentPosition[0] << " ," << currentPosition[1];
+ bool platformInPolygon = in.getIsAutoDriveEnabled() && VirtualRobot::MathTools::isInside(currentPosition.head(2), convPolygon);
+ platformInPolygon = platformInPolygon && fabs(platformTargetPose(2) - currentPosition(2)) < in.getRestrictAngle() * M_PI / 180.0;
+
+ // RemoteRobot::synchronizeLocalClone(localRobot, getRobotStateComponent());
+ Eigen::Vector3f currentPositionRight = tcpRight->getPositionInRootFrame();
+ Eigen::Vector3f currentPositionLeft = tcpLeft->getPositionInRootFrame();
+
+ bool automaticDriveCondition = in.getIsAutoDriveEnabled() && ((targetLeft - currentPositionLeft).norm() < in.getHandOutofRangeThreshold() || (targetRight - currentPositionRight).norm() < in.getHandOutofRangeThreshold());
+ bool textReported = false;
+
+ // 3D obstacle avoidance
+
+// bool isObs3DReached = false;
+// if (in.isObs3DPositionSet())
+// {
+// Eigen::Vector3f currentGuardLocalPosi = (currentPositionLeft + currentPositionRight) / 2;
+// currentGuardLocalPosi(1) += 0.6;
+// Eigen::Vector3f currentGuardGlobalPosi = localRobot->toGlobalCoordinateSystemVec(currentGuardLocalPosi);
+// currentGuardGlobalPosi(2) = 0;
+// Eigen::Vector3f currentObsPosi = in.getObs3DPosition()->toEigen();
+
+// float distToObs = (currentGuardGlobalPosi - currentObsPosi).norm();
+// float heightOffset = 0;
+
+// float distToReact = in.getObs3DDistToReact();
+
+// if (distToObs < 0.5 * distToReact)
+// {
+// isObs3DReached = true;
+// }
+// else
+// {
+// isObs3DReached = false;
+// }
+
+// if (distToObs < distToReact)
+// {
+
+// float changeFactor = cos(distToObs / distToReact * M_PI / 2);
+// heightOffset = in.getObs3DHeightAmplitude() * changeFactor;
+// std::vector<float> currentKpos = Kpos;
+// currentKpos[0] = Kpos[0] - in.getObs3DMaxiReducedStiffness()->x * changeFactor;
+// // currentKpos[1] = Kpos[1] + in.getObs3DMaxiReducedStiffness()->y * changeFactor;
+
+// leftController->setPosition(
+// {
+// desiredPoseLeft(0, 3),
+// desiredPoseLeft(1, 3),
+// desiredPoseLeft(2, 3) + heightOffset
+// });
+// rightController->setPosition(
+// {
+// desiredPoseRight(0, 3),
+// desiredPoseRight(1, 3),
+// desiredPoseRight(2, 3) + heightOffset
+// });
+// leftController->setImpedanceParameters("Kpos", currentKpos);
+// rightController->setImpedanceParameters("Kpos", currentKpos);
+
+// }
+// else
+// {
+// leftController->setImpedanceParameters("Kpos", Kpos);
+// rightController->setImpedanceParameters("Kpos", Kpos);
+// }
+// }
+
+
+ if (platformInPolygon && automaticDriveCondition)
+ {
+ if (!isImpedanceParameterReset)
+ {
+ if (!textReported)
+ {
+ getTextToSpeech()->reportText(in.getHandOutofRangeSpeechOutput());
+ textReported = true;
+ }
+
+ /// TODO this is not regarded anymore, may affect the demo
+// leftController->setImpedanceParameters("Kpos", in.getRestrictKpos());
+// leftController->setImpedanceParameters("Dpos", in.getRestrictDpos());
+// rightController->setImpedanceParameters("Kpos", in.getRestrictKpos());
+// rightController->setImpedanceParameters("Dpos", in.getRestrictDpos());
+ minPosiTolerance = in.getHandOutofRangeThreshold();
+ isImpedanceParameterReset = true;
+ }
+ float posx = platformPosX->getDataField()->getFloat();
+ float posy = platformPosY->getDataField()->getFloat();
+ float ori = platformRotAng->getDataField()->getFloat();
+
+ Eigen::Vector3f pos {posx, posy, 0.0f};
+
+ if (ori > M_PI)
+ {
+ ori = - 2 * M_PI + ori;
+ }
+
+ float angleDelta = platformTargetPose(2) - ori;
+ ARMARX_INFO << deactivateSpam(1) << VAROUT(angleDelta);
+
+ // transform alpha to [-pi, pi)
+ while (angleDelta < -M_PI)
+ {
+ angleDelta += 2 * M_PI;
+ }
+
+ while (angleDelta >= M_PI)
+ {
+ angleDelta -= 2 * M_PI;
+ }
+
+ Eigen::Vector3f target {platformTargetPose(0), platformTargetPose(1), 0.0f};
+ pidTrans.update(pos, target);
+ pidRot.update(angleDelta, 0);
+
+ Eigen::Vector2f newVel(pidTrans.getControlValue()[0], pidTrans.getControlValue()[1]);
+
+
+ float newVelocityRot = -signedMinFunction(pidRot.getControlValue(), in.getRestrictMaxAngularVelocity());
+ Eigen::Vector3f globalVel {newVel[0], newVel[1], 0};
+ Eigen::Matrix3f m;
+ m = Eigen::AngleAxisf(ori, Eigen::Vector3f::UnitZ());
+ Eigen::Vector3f localVel = m.inverse() * globalVel;
+ if (localVel.norm() > in.getRestrictMaxVelocity())
+ {
+ localVel *= in.getRestrictMaxVelocity() / localVel.norm();
+ }
+
+ if (std::isnan(localVel[0]) || std::isnan(localVel[1]) || std::isnan(newVelocityRot))
+ {
+ throw LocalException("A target velocity is NaN!");
+ }
+
+ getPlatformUnit()->move(localVel[0], localVel[1], newVelocityRot);
+
+
+ if (fabs(pidTrans.previousError) < in.getPositionalAccuracy() && fabs(pidRot.previousError) < in.getOrientationalAccuracy())
+ {
+ break;
+ }
+ else
+ {
+ ARMARX_INFO << deactivateSpam(1) << "error: " << pidTrans.previousError << " x velocity: " << newVel[0] << " y velocity: " << newVel[1];
+ ARMARX_INFO << deactivateSpam(1) << "rot error: " << pidRot.previousError << " rot velocity: " << newVelocityRot;
+ }
+
+ usleep(100000);
+ }
+ else
+ {
+ if (!platformInPolygon && isImpedanceParameterReset)
+ {
+// leftController->setImpedanceParameters("Kpos", in.getKpos());
+// leftController->setImpedanceParameters("Dpos", in.getDpos());
+// rightController->setImpedanceParameters("Kpos", in.getKpos());
+// rightController->setImpedanceParameters("Dpos", in.getDpos());
+ minPosiTolerance = in.getMinPosiTolerance();
+ isImpedanceParameterReset = false;
+ }
+
+ Eigen::Vector3f positionDeltaRight = currentPositionRight - initialPositionRight;
+ Eigen::Vector3f positionDeltaLeft = currentPositionLeft - initialPositionLeft;
+ Eigen::Vector3f positionDelta = (positionDeltaLeft + positionDeltaRight) / 2;
+ Eigen::Vector3f currentVel;
+ currentVel << platformVelX->getDataField()->getFloat(), platformVelY->getDataField()->getFloat(), 0;
+ Eigen::Vector3f velocity;
+ if (positionDelta.norm() < minPosiTolerance)
+ {
+ velocity.setZero();
+ }
+ else
+ {
+ positionDelta = positionDelta - minPosiTolerance * positionDelta / positionDelta.norm();
+ velocity = positionDelta * Kplatform - currentVel * Dplatform;
+ }
+
+
+ double deltaT = (now - last).toSecondsDouble();
+ last = now;
+
+ if (deltaT == 0)
+ {
+ continue;
+ }
+ // ARMARX_INFO << "deltaT: " << deltaT;
+ float Tstar = 1 / ((filterTimeConstant / deltaT) + 1);
+ filteredLinearVel = Tstar * (velocity - filteredLinearVel) + filteredLinearVel;
+
+ if (!std::isfinite(filteredLinearVel(0)))
+ {
+ filteredLinearVel(0) = 0;
+ }
+ if (!std::isfinite(filteredLinearVel(1)))
+ {
+ filteredLinearVel(1) = 0;
+ }
+
+ velocity = math::MathUtils::LimitTo(filteredLinearVel, maxVelocity); // in mm
+
+ // modulated velocity for obstacle avoidance
+
+ //const Eigen::Vector2f agent_pos = localRobot->getGlobalPosition().head<2>();
+
+ float ori = platformRotAng->getDataField()->getFloat();
+ if (ori > M_PI)
+ {
+ ori = - 2 * M_PI + ori;
+ }
+
+ Eigen::Matrix3f m;
+ m = Eigen::AngleAxisf(ori, Eigen::Vector3f::UnitZ());
+ Eigen::Vector3f globalVel = m * velocity;
+
+ obstacleavoidance::Agent agent;
+ agent.safety_margin = 0;
+ agent.pos = localRobot->getGlobalPosition();
+ agent.desired_vel = globalVel;
+ Eigen::Vector3f modulated_vel = obstacle_avoidance->modulateVelocity(agent);
+ modulated_vel[2] = 0;
+ Eigen::Vector3f localVel = m.inverse() * modulated_vel;
+ localVel[2] = 0;
+ if (localVel.norm() > in.getMaxVelocity())
+ {
+ localVel *= in.getMaxVelocity() / localVel.norm();
+ }
+
+ //
+ float angularVelocity ;
+
+ Eigen::Vector3f currentDirection = currentPositionRight - currentPositionLeft;
+ currentDirection = currentDirection / currentDirection.norm();
+
+ float cosAlpha = currentDirection.dot(initialDirection);
+
+ float alpha = 0;
+
+ Eigen::Vector3f directionDiff = currentDirection - initialDirection;
+ if (directionDiff(1) < 0)
+ {
+ alpha = -acos(cosAlpha);
+ }
+ else
+ {
+ alpha = acos(cosAlpha);
+ }
+
+
+ float minAngleTolerance = in.getMinAngleTolerance();
+ if (fabs(alpha) < minAngleTolerance)
+ {
+ angularVelocity = 0;
+ }
+ else
+ {
+ if (alpha > 0)
+ {
+ alpha -= minAngleTolerance;
+ }
+ else
+ {
+ alpha += minAngleTolerance;
+ }
+ angularVelocity = KplatformAngle * alpha - DplatformAngle * platformRotVel;
+ }
+
+ filteredAngularVel = Tstar * (angularVelocity - filteredAngularVel) + filteredAngularVel;
+ if (!std::isfinite(filteredAngularVel))
+ {
+ filteredAngularVel = 0;
+ }
+
+ angularVelocity = math::MathUtils::LimitTo(filteredAngularVel, maxAngularVelocity);
+
+ getDebugObserver()->setDebugChannel("FollowMeTorque",
+ {
+ { "vx", new Variant(velocity(0)) },
+ { "vy", new Variant(velocity(1)) },
+ { "vangle", new Variant(angularVelocity) },
+ });
+ localVel(2) = angularVelocity;
+
+ if (std::isnan(localVel[0]) || std::isnan(localVel[1]) || std::isnan(angularVelocity))
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "Some velocity target is NaN - setting to zero";
+ localVel.setZero();
+ }
+
+ float ratio = 0.7;
+ filteredPlatformTargetVelocity = filteredPlatformTargetVelocity * ratio + (1.0 - ratio) * localVel;
+ float oriVel = filteredPlatformTargetVelocity(2);
+
+// if (isObs3DReached)
+// {
+// oriVel = 0;
+// }
+
+ getPlatformUnit()->move(filteredPlatformTargetVelocity(0), filteredPlatformTargetVelocity(1), oriVel);
+
+
+ // debug drawer ....
+ // {
+ // if ((oldVisuPos - localRobot->getGlobalPosition()).norm() > 50)
+ // {
+ // draw_agent_path(dd, agent_pos, visuSphereCounter++);
+ // oldVisuPos = localRobot->getGlobalPosition();
+ // }
+ // draw_agent_safety_margin(dd, agent_pos, ori, obstacle_avoidance_cfg.agent_safety_margin);
+ // draw_agent_desired_velocity(dd, agent_pos, globalVel.head(2));
+ // draw_agent_modulated_velocity(dd, agent_pos, modulated_vel.head(2));
+ // }
+
+ }
+
+ c.waitForCycleDuration();
+ }
+
+ ARMARX_INFO << "running task has been requested to stop";
+ skillMemory->abortSkillAsync(skillExecutionId);
+ emitSuccess();
+ return;
+ }
+
+ //void GuardCarryingSkill::onBreak()
+ //{
+ // // put your user code for the breaking point here
+ // // execution time should be short (<100ms)
+ //}
+
+ void GuardCarryingSkill::onExit()
+ {
+ // put your user code for the exit point here
+ // execution time should be short (<100ms)
+ }
+
+
+ // DO NOT EDIT NEXT FUNCTION
+ XMLStateFactoryBasePtr GuardCarryingSkill::CreateInstance(XMLStateConstructorParams stateData)
+ {
+ return XMLStateFactoryBasePtr(new GuardCarryingSkill(stateData));
+ }
+}
diff --git a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.h b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.h
new file mode 100644
index 000000000..f20024a5c
--- /dev/null
+++ b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.h
@@ -0,0 +1,53 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package skills::Armar6BoardSupportDemo
+ * @author [Author Name] ( [Author Email] )
+ * @date 2024
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <mutex>
+
+#include <armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.generated.h>
+
+namespace armarx::Armar6BoardSupportDemo
+{
+ class GuardCarryingSkill :
+ public GuardCarryingSkillGeneratedBase < GuardCarryingSkill >
+ {
+ public:
+ GuardCarryingSkill(const XMLStateConstructorParams& stateData):
+ XMLStateTemplate < GuardCarryingSkill > (stateData), GuardCarryingSkillGeneratedBase < GuardCarryingSkill > (stateData)
+ {
+ }
+
+ // inherited from StateBase
+ void onEnter() override;
+ void run() override;
+ // void onBreak() override;
+ void onExit() override;
+
+ // static functions for AbstractFactory Method
+ static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+ static SubClassRegistry Registry;
+
+ // DO NOT INSERT ANY CLASS MEMBERS,
+ // use stateparameters instead,
+ // if classmember are neccessary nonetheless, reset them in onEnter
+ };
+}
diff --git a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.xml b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.xml
new file mode 100644
index 000000000..4232348da
--- /dev/null
+++ b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.xml
@@ -0,0 +1,167 @@
+<?xml version="1.0" encoding="utf-8"?>
+<State version="1.2" name="GuardCarryingSkill" uuid="30629B08-477C-4EC0-8150-549910532B42" width="800" height="391.529" type="Normal State">
+ <InputParameters>
+ <Parameter name="DesiredLeftJointValues" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="yes">
+ <DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.037332}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.303279}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.250224}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.510265}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1.826300}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":-0.171072}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.355409}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.220867}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="0.0373319983482361\n0.303279012441635\n0.250223994255066\n0.510264992713928\n1.82630002498627\n-0.171072006225586\n0.355408996343613\n0.220866993069649"/>
+ </Parameter>
+ <Parameter name="DesiredPoseLeft" type="::armarx::PoseBase" docType="Pose" optional="yes"/>
+ <Parameter name="DesiredPoseRight" type="::armarx::PoseBase" docType="Pose" optional="yes"/>
+ <Parameter name="DesiredRightJointValues" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="yes">
+ <DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":-0.014552}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":-0.222375}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.269340}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":-0.464008}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1.874799}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.169847}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":-0.419656}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.031448}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="-0.0145519999787211\n-0.222375005483627\n0.269340008497238\n-0.464008003473282\n1.87479901313782\n0.169846996665001\n-0.419656008481979\n0.0314479991793633"/>
+ </Parameter>
+ <Parameter name="Dnull" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="no">
+ <DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="0\n0\n0\n0\n0\n0\n0\n0"/>
+ </Parameter>
+ <Parameter name="Dori" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="no">
+ <DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":50}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":50}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":50}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="50\n50\n50"/>
+ </Parameter>
+ <Parameter name="Dplatform" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}}' docValue="0"/>
+ </Parameter>
+ <Parameter name="DplatformAngle" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}}' docValue="0"/>
+ </Parameter>
+ <Parameter name="Dpos" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="no">
+ <DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":5}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":5}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":20}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="5\n5\n20"/>
+ </Parameter>
+ <Parameter name="HandOutofRangeSpeechOutput" type="::armarx::StringVariantData" docType="string" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Please follow me."}}' docValue="Please follow me."/>
+ </Parameter>
+ <Parameter name="HandOutofRangeThreshold" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":200}}' docValue="200"/>
+ </Parameter>
+ <Parameter name="IsAutoDriveEnabled" type="::armarx::BoolVariantData" docType="bool" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":true}}' docValue="True"/>
+ </Parameter>
+ <Parameter name="IsConsiderFailure" type="::armarx::BoolVariantData" docType="bool" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":false}}' docValue="False"/>
+ </Parameter>
+ <Parameter name="IsGuardPlacement" type="::armarx::BoolVariantData" docType="bool" optional="no">
+ <DefaultValue profile="Armar6Real" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":false}}' docValue="False"/>
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":true}}' docValue="True"/>
+ </Parameter>
+ <Parameter name="IsLocalModulationEnabled" type="::armarx::BoolVariantData" docType="bool" optional="no">
+ <DefaultValue profile="Armar6Real" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":true}}' docValue="True"/>
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":false}}' docValue="False"/>
+ </Parameter>
+ <Parameter name="Knull" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="no">
+ <DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":5}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":5}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":5}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":5}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":5}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="5\n0\n5\n5\n1\n5\n5\n0"/>
+ </Parameter>
+ <Parameter name="Kori" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="no">
+ <DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":200}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":200}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":200}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="200\n200\n200"/>
+ </Parameter>
+ <Parameter name="Kplatform" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":12}}' docValue="12"/>
+ </Parameter>
+ <Parameter name="KplatformAngle" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":2}}' docValue="2"/>
+ </Parameter>
+ <Parameter name="Kpos" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="no">
+ <DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":50}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":50}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1500}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="50\n50\n1500"/>
+ </Parameter>
+ <Parameter name="LeftThumbThreshold" type="::armarx::FloatVariantData" docType="float" optional="yes"/>
+ <Parameter name="MaxAngularVelocity" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1}}' docValue="1"/>
+ </Parameter>
+ <Parameter name="MaxVelocity" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":500}}' docValue="500"/>
+ </Parameter>
+ <Parameter name="MinAngleTolerance" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.05}}' docValue="0.0500000007450581"/>
+ </Parameter>
+ <Parameter name="MinExecutionTime" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":3}}' docValue="3"/>
+ </Parameter>
+ <Parameter name="MinPosiTolerance" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":30}}' docValue="30"/>
+ </Parameter>
+ <Parameter name="Obs3DDistToReact" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1500}}' docValue="1500"/>
+ </Parameter>
+ <Parameter name="Obs3DHeightAmplitude" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue profile="Armar6Real" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":250}}' docValue="250"/>
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":300}}' docValue="300"/>
+ </Parameter>
+ <Parameter name="Obs3DMaxiReducedStiffness" type="::armarx::Vector3Base" docType="Vector3" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::Vector3Base","value":{"x":70,"y":0,"z":0}}}' docValue="70\n0\n0"/>
+ </Parameter>
+ <Parameter name="Obs3DPosition" type="::armarx::Vector3Base" docType="Vector3" optional="yes"/>
+ <Parameter name="OrientationalAccuracy" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.1}}' docValue="0.100000001490116"/>
+ </Parameter>
+ <Parameter name="PlatformTargetPose" type="::armarx::StringValueMapBase(::armarx::Vector3Base)" docType="Map(Vector3)" optional="no"/>
+ <Parameter name="PositionalAccuracy" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":30}}' docValue="30"/>
+ </Parameter>
+ <Parameter name="ReGraspingForceThreshold" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":20}}' docValue="20"/>
+ </Parameter>
+ <Parameter name="ReactionTime" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.75}}' docValue="0.75"/>
+ <DefaultValue profile="Armar6Real" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.25}}' docValue="0.25"/>
+ </Parameter>
+ <Parameter name="RestrictAngle" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":45}}' docValue="45"/>
+ </Parameter>
+ <Parameter name="RestrictDpos" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="no">
+ <DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":20}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="1\n1\n20"/>
+ </Parameter>
+ <Parameter name="RestrictKpos" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="no">
+ <DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":20}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":20}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1500}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="20\n20\n1500"/>
+ </Parameter>
+ <Parameter name="RestrictMaxAngularVelocity" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue profile="Armar6Real" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.2}}' docValue="0.200000002980232"/>
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.3}}' docValue="0.300000011920929"/>
+ </Parameter>
+ <Parameter name="RestrictMaxVelocity" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":300}}' docValue="300"/>
+ </Parameter>
+ <Parameter name="RestrictPolygon" type="::armarx::StringValueMapBase(::armarx::MatrixFloatBase)" docType="Map(MatrixFloat)" optional="no">
+ <DefaultValue value='{"map":{"GuardPlacement":{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::MatrixFloatBase","value":{"cols":2,"rowContent":["-400, -700","400, -700","400, 700","-400, 700"],"rows":4}}},"GuardLifting":{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::MatrixFloatBase","value":{"cols":2,"rowContent":["-150.0, -350","150.0, -350","150, 350","-150, 350"],"rows":4}}}},"type":"::armarx::StringValueMapBase"}' docValue="GuardLifting: -150 -350\n 150 -350\n 150 350\n-150 350\nGuardPlacement: -400 -700\n 400 -700\n 400 700\n-400 700\n"/>
+ </Parameter>
+ <Parameter name="RestrictRotationPID" type="::armarx::Vector3Base" docType="Vector3" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::Vector3Base","value":{"x":1.0,"y":0.00009,"z":0.0}}}' docValue="1\n9e-05\n0"/>
+ </Parameter>
+ <Parameter name="RestrictTranslationPID" type="::armarx::Vector3Base" docType="Vector3" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::Vector3Base","value":{"x":2.0,"y":0.0004,"z":0.0}}}' docValue="2\n0.0004\n0"/>
+ </Parameter>
+ <Parameter name="RightThumbThreshold" type="::armarx::FloatVariantData" docType="float" optional="yes"/>
+ <Parameter name="RobotSafetyMargin" type="::armarx::DoubleVariantData" docType="double" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::DoubleVariantData","value":0.6}}' docValue="0.6"/>
+ </Parameter>
+ <Parameter name="SafetyMargin" type="::armarx::DoubleVariantData" docType="double" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::DoubleVariantData","value":0}}' docValue="0"/>
+ </Parameter>
+ <Parameter name="ShowGuard" type="::armarx::BoolVariantData" docType="bool" optional="yes"/>
+ <Parameter name="TextAfterWaitForZeroTorque" type="::armarx::StringVariantData" docType="string" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Okay."}}' docValue="Okay."/>
+ </Parameter>
+ <Parameter name="TextBeforeThreeSeconds" type="::armarx::StringVariantData" docType="string" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Please wait a moment"}}' docValue="Please wait a moment"/>
+ </Parameter>
+ <Parameter name="TextRegraspingLeft" type="::armarx::StringVariantData" docType="string" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Let me get a better grasp to help you carry the board."}}' docValue="Let me get a better grasp to help you carry the board."/>
+ </Parameter>
+ <Parameter name="TextRegraspingRight" type="::armarx::StringVariantData" docType="string" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Let me get a better grasp to help you carry the board."}}' docValue="Let me get a better grasp to help you carry the board."/>
+ </Parameter>
+ <Parameter name="TorqueLimit" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":20}}' docValue="20"/>
+ </Parameter>
+ </InputParameters>
+ <OutputParameters>
+ <Parameter name="DesiredPoseLeft" type="::armarx::PoseBase" docType="Pose" optional="yes"/>
+ <Parameter name="DesiredPoseRight" type="::armarx::PoseBase" docType="Pose" optional="yes"/>
+ </OutputParameters>
+ <LocalParameters/>
+ <Substates/>
+ <Events>
+ <Event name="Failure">
+ <Description>Event for statechart-internal failures or optionally user-code failures</Description>
+ </Event>
+ <Event name="Success"/>
+ <Event name="LoseGuard"/>
+ </Events>
+ <Transitions/>
+</State>
+
diff --git a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/RemoveGuardV2.xml b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/RemoveGuardV2.xml
index ef4b7494f..c1dcc6b67 100644
--- a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/RemoveGuardV2.xml
+++ b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/RemoveGuardV2.xml
@@ -1,5 +1,5 @@
<?xml version="1.0" encoding="utf-8"?>
-<State version="1.2" name="RemoveGuardV2" uuid="BD03CA34-6FD6-491D-A962-D61F66D9E995" width="3517.67" height="1075.14" type="Normal State">
+<State version="1.2" name="RemoveGuardV2" uuid="BD03CA34-6FD6-491D-A962-D61F66D9E995" width="3580.28" height="1084.89" type="Normal State">
<InputParameters>
<Parameter name="DesiredNullspaceAngleLeft" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="yes">
<DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.157}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":-0.076}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.542}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.511}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1.681}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":-0.589}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.379}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.149}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="0.157000005245209\n-0.0759999975562096\n0.541999995708466\n0.510999977588654\n1.68099999427795\n-0.58899998664856\n0.379000008106232\n0.149000003933907"/>
@@ -70,22 +70,22 @@
<Parameter name="RightThumbThreshold" type="::armarx::FloatVariantData" docType="float" optional="no"/>
</LocalParameters>
<Substates>
- <LocalState name="CarryAndRegrasp" refuuid="181CF11B-48EF-440B-953A-A7C204A16995" left="1835.33" top="633.5" boundingSquareSize="99.6636"/>
- <RemoteState name="CloseBothHands" refuuid="4BFA225C-8FA0-4214-89F3-3D717C2585CF" proxyName="Armar6LowLevelHandControlGroupRemoteStateOfferer" left="2084.5" top="615.5" boundingSquareSize="99.6636"/>
- <LocalState name="DummyState" refuuid="66424431-C806-4E7D-B288-2AC1341D6F48" left="1110.83" top="485.889" boundingSquareSize="99.6636"/>
- <LocalState name="DummyState_2" refuuid="66424431-C806-4E7D-B288-2AC1341D6F48" left="1597.67" top="563.833" boundingSquareSize="99.6636"/>
- <EndState name="Failure" event="Failure" left="3387.67" top="504.5" boundingSquareSize="99.6636"/>
- <LocalState name="GoToDefaultPose" refuuid="D5FC47DA-A2DE-4DD1-B389-E8D9B9417932" left="861.667" top="407.778" boundingSquareSize="99.6636"/>
- <LocalState name="GoToGuardLowering" refuuid="8F02B4E0-5B56-4789-9771-D68B6FB0B961" left="612.5" top="326.833" boundingSquareSize="99.6636"/>
- <RemoteState name="GoToLandmarkWithObstacleAvoidance" refuuid="19478C7D-BC94-4BA1-8C85-33EC9EECCC74" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="3086.47" top="859.5" boundingSquareSize="152.058"/>
- <RemoteState name="GoToLandmarkWithObstacleAvoidance_2" refuuid="19478C7D-BC94-4BA1-8C85-33EC9EECCC74" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="567.367" top="436.868" boundingSquareSize="100"/>
- <RemoteState name="GuardLowering" refuuid="A9221FE2-D60C-4009-B516-1440CC1E7932" proxyName="DSControllerGroupRemoteStateOfferer" left="1348.5" top="483.444" boundingSquareSize="99.6636"/>
- <RemoteState name="LookStraight" refuuid="2D4453C8-0CEB-4773-9493-ADD75AD31346" proxyName="Armar6HeadGroupRemoteStateOfferer" left="114.167" top="174.667" boundingSquareSize="99.6636"/>
- <RemoteState name="NavigationPose" refuuid="E7374741-A6DF-4E3F-B05D-68A1EB44D6A5" proxyName="Armar6PosesRemoteStateOfferer" left="363.333" top="238.333" boundingSquareSize="99.6636"/>
- <RemoteState name="OpenBothHands" refuuid="88173D79-82C3-4FE1-ACB6-7296D8AA85CF" proxyName="Armar6LowLevelHandControlGroupRemoteStateOfferer" left="2582.83" top="690.889" boundingSquareSize="99.6636"/>
- <LocalState name="PutDownGuard" refuuid="4D5C0ED4-9B27-4647-B07A-F298263A49D1" left="2333.67" top="633.556" boundingSquareSize="99.6636"/>
- <LocalState name="RaiseAndGoToDefaultPose" refuuid="FC7E4DC8-A4D1-4FA6-9924-82CED5D2BD95" left="2832.56" top="784.333" boundingSquareSize="104.779"/>
- <EndState name="Success" event="Success" left="3387.67" top="871.222" boundingSquareSize="99.6636"/>
+ <LocalState name="CarryAndRegrasp" refuuid="181CF11B-48EF-440B-953A-A7C204A16995" left="1897.94" top="530" boundingSquareSize="99.6636"/>
+ <RemoteState name="CloseBothHands" refuuid="4BFA225C-8FA0-4214-89F3-3D717C2585CF" proxyName="Armar6LowLevelHandControlGroupRemoteStateOfferer" left="2147.11" top="512" boundingSquareSize="99.6636"/>
+ <LocalState name="DummyState" refuuid="66424431-C806-4E7D-B288-2AC1341D6F48" left="1173.44" top="386.222" boundingSquareSize="99.6636"/>
+ <LocalState name="DummyState_2" refuuid="66424431-C806-4E7D-B288-2AC1341D6F48" left="1660.28" top="478.222" boundingSquareSize="99.6636"/>
+ <EndState name="Failure" event="Failure" left="3450.28" top="538.722" boundingSquareSize="99.6636"/>
+ <LocalState name="GoToDefaultPose" refuuid="D5FC47DA-A2DE-4DD1-B389-E8D9B9417932" left="924.278" top="324.722" boundingSquareSize="99.6636"/>
+ <LocalState name="GoToGuardLowering" refuuid="8F02B4E0-5B56-4789-9771-D68B6FB0B961" left="643.806" top="275.722" boundingSquareSize="99.6636"/>
+ <RemoteState name="GoToLandmarkWithObstacleAvoidance" refuuid="19478C7D-BC94-4BA1-8C85-33EC9EECCC74" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="3149.08" top="778.389" boundingSquareSize="152.058"/>
+ <RemoteState name="GoToLandmarkWithObstacleAvoidance_2" refuuid="19478C7D-BC94-4BA1-8C85-33EC9EECCC74" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="612.806" top="575.722" boundingSquareSize="162.279"/>
+ <RemoteState name="GuardLowering" refuuid="A9221FE2-D60C-4009-B516-1440CC1E7932" proxyName="DSControllerGroupRemoteStateOfferer" left="1411.11" top="385.056" boundingSquareSize="99.6636"/>
+ <RemoteState name="LookStraight" refuuid="2D4453C8-0CEB-4773-9493-ADD75AD31346" proxyName="Armar6HeadGroupRemoteStateOfferer" left="114.167" top="280.722" boundingSquareSize="99.6636"/>
+ <RemoteState name="NavigationPose" refuuid="E7374741-A6DF-4E3F-B05D-68A1EB44D6A5" proxyName="Armar6PosesRemoteStateOfferer" left="363.333" top="566.722" boundingSquareSize="99.6636"/>
+ <RemoteState name="OpenBothHands" refuuid="88173D79-82C3-4FE1-ACB6-7296D8AA85CF" proxyName="Armar6LowLevelHandControlGroupRemoteStateOfferer" left="2645.44" top="610.389" boundingSquareSize="99.6636"/>
+ <LocalState name="PutDownGuard" refuuid="4D5C0ED4-9B27-4647-B07A-F298263A49D1" left="2396.28" top="552.778" boundingSquareSize="99.6636"/>
+ <LocalState name="RaiseAndGoToDefaultPose" refuuid="FC7E4DC8-A4D1-4FA6-9924-82CED5D2BD95" left="2895.17" top="703.111" boundingSquareSize="104.779"/>
+ <EndState name="Success" event="Success" left="3450.28" top="879.889" boundingSquareSize="99.6636"/>
</Substates>
<Events>
<Event name="Failure">
@@ -97,10 +97,10 @@
<ParameterMapping sourceType="Parent" from="IsGuardPlacement" to="IsGuardPlacement"/>
</ParameterMappings>
<SupportPoints>
- <SupportPoint posX="67.1067" posY="216.667"/>
- <SupportPoint posX="76.5839" posY="216.667"/>
- <SupportPoint posX="88.7241" posY="216.667"/>
- <SupportPoint posX="101.186" posY="216.667"/>
+ <SupportPoint posX="67.1067" posY="322.722"/>
+ <SupportPoint posX="76.5839" posY="322.722"/>
+ <SupportPoint posX="88.7241" posY="322.722"/>
+ <SupportPoint posX="101.186" posY="322.722"/>
</SupportPoints>
</StartState>
<Transitions>
@@ -109,22 +109,22 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="961.628" posY="443.626"/>
- <SupportPoint posX="1011.82" posY="436.138"/>
- <SupportPoint posX="1091.48" posY="426.222"/>
- <SupportPoint posX="1160.83" posY="426.222"/>
- <SupportPoint posX="1160.83" posY="426.222"/>
- <SupportPoint posX="1160.83" posY="426.222"/>
- <SupportPoint posX="1766.5" posY="426.222"/>
- <SupportPoint posX="2096.55" posY="426.222"/>
- <SupportPoint posX="2178.2" posY="391.722"/>
- <SupportPoint posX="2508.25" posY="391.722"/>
- <SupportPoint posX="2508.25" posY="391.722"/>
- <SupportPoint posX="2508.25" posY="391.722"/>
- <SupportPoint posX="3162.47" posY="391.722"/>
- <SupportPoint posX="3250.38" posY="391.722"/>
- <SupportPoint posX="3336.76" posY="451.548"/>
- <SupportPoint posX="3388.77" posY="495.823"/>
+ <SupportPoint posX="1024.47" posY="355.561"/>
+ <SupportPoint posX="1074.51" posY="343.601"/>
+ <SupportPoint posX="1153.74" posY="327.833"/>
+ <SupportPoint posX="1223.44" posY="327.833"/>
+ <SupportPoint posX="1223.44" posY="327.833"/>
+ <SupportPoint posX="1223.44" posY="327.833"/>
+ <SupportPoint posX="2072.53" posY="327.833"/>
+ <SupportPoint posX="2294.09" posY="327.833"/>
+ <SupportPoint posX="2349.29" posY="311.222"/>
+ <SupportPoint posX="2570.86" posY="311.222"/>
+ <SupportPoint posX="2570.86" posY="311.222"/>
+ <SupportPoint posX="2570.86" posY="311.222"/>
+ <SupportPoint posX="3225.08" posY="311.222"/>
+ <SupportPoint posX="3348.13" posY="311.222"/>
+ <SupportPoint posX="3437.45" posY="450.206"/>
+ <SupportPoint posX="3477.32" posY="526.681"/>
</SupportPoints>
</Transition>
<Transition from="GoToDefaultPose" to="DummyState" eventName="Success">
@@ -134,13 +134,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="961.768" posY="480.489"/>
- <SupportPoint posX="969.243" posY="484.08"/>
- <SupportPoint posX="976.961" posY="487.428"/>
- <SupportPoint posX="984.5" posY="490.111"/>
- <SupportPoint posX="1021.16" posY="503.17"/>
- <SupportPoint posX="1063.9" posY="511.999"/>
- <SupportPoint posX="1098.12" posY="517.609"/>
+ <SupportPoint posX="1024.21" posY="385.308"/>
+ <SupportPoint posX="1031.84" posY="387.608"/>
+ <SupportPoint posX="1039.66" posY="389.818"/>
+ <SupportPoint posX="1047.11" posY="391.722"/>
+ <SupportPoint posX="1084.33" posY="401.203"/>
+ <SupportPoint posX="1126.55" posY="409.547"/>
+ <SupportPoint posX="1160.35" posY="415.655"/>
</SupportPoints>
</Transition>
<Transition from="GuardLowering" to="Failure" eventName="Failure">
@@ -148,19 +148,22 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1448.46" posY="519.883"/>
- <SupportPoint posX="1498.68" posY="512.025"/>
- <SupportPoint posX="1578.28" posY="501.611"/>
- <SupportPoint posX="1647.67" posY="501.611"/>
- <SupportPoint posX="1647.67" posY="501.611"/>
- <SupportPoint posX="1647.67" posY="501.611"/>
- <SupportPoint posX="2009.92" posY="501.611"/>
- <SupportPoint posX="2612.26" posY="501.611"/>
- <SupportPoint posX="2772.62" posY="406.838"/>
- <SupportPoint posX="3365" posY="515.667"/>
- <SupportPoint posX="3368.45" posY="516.293"/>
- <SupportPoint posX="3371.9" posY="517.072"/>
- <SupportPoint posX="3375.35" posY="517.954"/>
+ <SupportPoint posX="1511.2" posY="427.206"/>
+ <SupportPoint posX="1561.54" posY="424.587"/>
+ <SupportPoint posX="1641.28" posY="421.111"/>
+ <SupportPoint posX="1710.28" posY="421.111"/>
+ <SupportPoint posX="1710.28" posY="421.111"/>
+ <SupportPoint posX="1710.28" posY="421.111"/>
+ <SupportPoint posX="2072.53" posY="421.111"/>
+ <SupportPoint posX="2619.16" posY="421.111"/>
+ <SupportPoint posX="2756.52" posY="432.356"/>
+ <SupportPoint posX="3301.11" posY="479.889"/>
+ <SupportPoint posX="3357.59" posY="484.808"/>
+ <SupportPoint posX="3377.27" posY="469.168"/>
+ <SupportPoint posX="3427.61" posY="495.222"/>
+ <SupportPoint posX="3442.82" posY="503.093"/>
+ <SupportPoint posX="3456.36" posY="515.488"/>
+ <SupportPoint posX="3467.61" posY="528.227"/>
</SupportPoints>
</Transition>
<Transition from="GuardLowering" to="DummyState_2" eventName="Success">
@@ -170,13 +173,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1448.72" posY="556.07"/>
- <SupportPoint posX="1456.13" posY="559.558"/>
- <SupportPoint posX="1463.79" posY="562.829"/>
- <SupportPoint posX="1471.33" posY="565.5"/>
- <SupportPoint posX="1508.13" posY="578.61"/>
- <SupportPoint posX="1550.81" posY="588.027"/>
- <SupportPoint posX="1585.06" posY="594.263"/>
+ <SupportPoint posX="1510.94" posY="472.235"/>
+ <SupportPoint posX="1518.36" posY="477.103"/>
+ <SupportPoint posX="1526.15" posY="481.537"/>
+ <SupportPoint posX="1533.94" posY="485"/>
+ <SupportPoint posX="1569.72" posY="500.806"/>
+ <SupportPoint posX="1612.78" posY="509.099"/>
+ <SupportPoint posX="1647.41" posY="513.443"/>
</SupportPoints>
</Transition>
<Transition from="CarryAndRegrasp" to="Failure" eventName="Failure">
@@ -184,19 +187,19 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1892.36" posY="681.254"/>
- <SupportPoint posX="1914.98" posY="751.608"/>
- <SupportPoint posX="1992.28" posY="953.946"/>
- <SupportPoint posX="2134.5" posY="953.946"/>
- <SupportPoint posX="2134.5" posY="953.946"/>
- <SupportPoint posX="2134.5" posY="953.946"/>
- <SupportPoint posX="2508.25" posY="953.946"/>
- <SupportPoint posX="2889.28" posY="953.946"/>
- <SupportPoint posX="3081.33" posY="1177.72"/>
- <SupportPoint posX="3365" posY="923.279"/>
- <SupportPoint posX="3377.27" posY="912.251"/>
- <SupportPoint posX="3413.56" posY="694.492"/>
- <SupportPoint posX="3429.78" posY="593.33"/>
+ <SupportPoint posX="1953.44" posY="577.894"/>
+ <SupportPoint posX="1972.35" posY="654.063"/>
+ <SupportPoint posX="2042.88" posY="886.222"/>
+ <SupportPoint posX="2197.11" posY="886.222"/>
+ <SupportPoint posX="2197.11" posY="886.222"/>
+ <SupportPoint posX="2197.11" posY="886.222"/>
+ <SupportPoint posX="2570.86" posY="886.222"/>
+ <SupportPoint posX="2761.76" posY="886.222"/>
+ <SupportPoint posX="3296.64" posY="958.716"/>
+ <SupportPoint posX="3427.61" posY="819.778"/>
+ <SupportPoint posX="3454.06" posY="791.692"/>
+ <SupportPoint posX="3478.34" posY="688.409"/>
+ <SupportPoint posX="3491.12" posY="625.556"/>
</SupportPoints>
</Transition>
<Transition from="CarryAndRegrasp" to="CloseBothHands" eventName="Success">
@@ -217,10 +220,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1935.81" posY="657.5"/>
- <SupportPoint posX="1974.91" posY="657.5"/>
- <SupportPoint posX="2029.34" posY="657.5"/>
- <SupportPoint posX="2071.38" posY="657.5"/>
+ <SupportPoint posX="1998.42" posY="554"/>
+ <SupportPoint posX="2037.52" posY="554"/>
+ <SupportPoint posX="2091.95" posY="554"/>
+ <SupportPoint posX="2133.99" posY="554"/>
</SupportPoints>
</Transition>
<Transition from="PutDownGuard" to="Failure" eventName="Failure">
@@ -228,16 +231,19 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2433.76" posY="662.253"/>
- <SupportPoint posX="2441.29" posY="660.988"/>
- <SupportPoint posX="2449.09" posY="659.787"/>
- <SupportPoint posX="2456.5" posY="658.778"/>
- <SupportPoint posX="2858.49" posY="604.102"/>
- <SupportPoint posX="2970.29" posY="664.106"/>
- <SupportPoint posX="3365" posY="570.611"/>
- <SupportPoint posX="3368.45" posY="569.806"/>
- <SupportPoint posX="3371.9" posY="568.873"/>
- <SupportPoint posX="3375.35" posY="567.851"/>
+ <SupportPoint posX="2496.37" posY="580.961"/>
+ <SupportPoint posX="2503.91" posY="579.888"/>
+ <SupportPoint posX="2511.7" posY="578.929"/>
+ <SupportPoint posX="2519.11" posY="578.278"/>
+ <SupportPoint posX="2875.61" posY="546.602"/>
+ <SupportPoint posX="2966.33" posY="565.359"/>
+ <SupportPoint posX="3324.11" posY="561.667"/>
+ <SupportPoint posX="3370.11" posY="561.194"/>
+ <SupportPoint posX="3381.87" posY="557.284"/>
+ <SupportPoint posX="3427.61" posY="561.667"/>
+ <SupportPoint posX="3430.81" posY="561.973"/>
+ <SupportPoint posX="3434.13" posY="562.369"/>
+ <SupportPoint posX="3437.58" posY="562.817"/>
</SupportPoints>
</Transition>
<Transition from="PutDownGuard" to="OpenBothHands" eventName="Success">
@@ -256,13 +262,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2433.88" posY="691.233"/>
- <SupportPoint posX="2441.42" posY="693.84"/>
- <SupportPoint posX="2449.09" posY="696.319"/>
- <SupportPoint posX="2456.5" posY="698.389"/>
- <SupportPoint posX="2493.68" posY="708.79"/>
- <SupportPoint posX="2535.98" posY="717.185"/>
- <SupportPoint posX="2569.97" posY="723.101"/>
+ <SupportPoint posX="2496.37" posY="610.452"/>
+ <SupportPoint posX="2503.91" posY="613.161"/>
+ <SupportPoint posX="2511.7" posY="615.742"/>
+ <SupportPoint posX="2519.11" posY="617.889"/>
+ <SupportPoint posX="2556.17" posY="628.571"/>
+ <SupportPoint posX="2598.59" posY="636.979"/>
+ <SupportPoint posX="2632.45" posY="642.818"/>
</SupportPoints>
</Transition>
<Transition from="RaiseAndGoToDefaultPose" to="Failure" eventName="Failure">
@@ -270,16 +276,16 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2937.07" posY="813.274"/>
- <SupportPoint posX="3021.15" posY="798.707"/>
- <SupportPoint posX="3182.02" posY="770.328"/>
- <SupportPoint posX="3238.5" posY="757.167"/>
- <SupportPoint posX="3295.36" posY="743.916"/>
- <SupportPoint posX="3321.43" posY="760.348"/>
- <SupportPoint posX="3365" posY="721.389"/>
- <SupportPoint posX="3402.06" posY="688.243"/>
- <SupportPoint posX="3420.58" posY="633.503"/>
- <SupportPoint posX="3429.66" posY="593.292"/>
+ <SupportPoint posX="2999.56" posY="733.336"/>
+ <SupportPoint posX="3096.28" posY="715.281"/>
+ <SupportPoint posX="3293.83" posY="678.392"/>
+ <SupportPoint posX="3301.11" posY="676.667"/>
+ <SupportPoint posX="3357.97" posY="663.314"/>
+ <SupportPoint posX="3376.37" posY="669.038"/>
+ <SupportPoint posX="3427.61" posY="640.889"/>
+ <SupportPoint posX="3437.19" posY="635.599"/>
+ <SupportPoint posX="3446.65" posY="628.827"/>
+ <SupportPoint posX="3455.34" posY="621.646"/>
</SupportPoints>
</Transition>
<Transition from="RaiseAndGoToDefaultPose" to="GoToLandmarkWithObstacleAvoidance" eventName="Success">
@@ -292,13 +298,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2936.94" posY="842.152"/>
- <SupportPoint posX="2944.61" posY="844.707"/>
- <SupportPoint posX="2952.41" posY="847.135"/>
- <SupportPoint posX="2959.94" posY="849.167"/>
- <SupportPoint posX="2996.74" posY="859.121"/>
- <SupportPoint posX="3037.89" posY="867.426"/>
- <SupportPoint posX="3073.54" posY="873.738"/>
+ <SupportPoint posX="2999.68" posY="761.728"/>
+ <SupportPoint posX="3007.35" posY="764.194"/>
+ <SupportPoint posX="3015.14" posY="766.584"/>
+ <SupportPoint posX="3022.56" posY="768.667"/>
+ <SupportPoint posX="3059.36" posY="778.927"/>
+ <SupportPoint posX="3100.37" posY="788.357"/>
+ <SupportPoint posX="3135.77" posY="795.883"/>
</SupportPoints>
</Transition>
<Transition from="NavigationPose" to="Failure" eventName="Failure">
@@ -306,25 +312,25 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="463.409" posY="266.257"/>
- <SupportPoint posX="513.69" posY="262.513"/>
- <SupportPoint posX="593.411" posY="257.556"/>
- <SupportPoint posX="662.5" posY="257.556"/>
- <SupportPoint posX="662.5" posY="257.556"/>
- <SupportPoint posX="662.5" posY="257.556"/>
- <SupportPoint posX="1766.5" posY="257.556"/>
- <SupportPoint posX="1985.51" posY="257.556"/>
- <SupportPoint posX="2040.07" posY="240.944"/>
- <SupportPoint posX="2259.08" posY="240.944"/>
- <SupportPoint posX="2259.08" posY="240.944"/>
- <SupportPoint posX="2259.08" posY="240.944"/>
- <SupportPoint posX="3162.47" posY="240.944"/>
- <SupportPoint posX="3264.57" posY="240.944"/>
- <SupportPoint posX="3297.66" posY="272.748"/>
- <SupportPoint posX="3365" posY="349.556"/>
- <SupportPoint posX="3400.65" posY="390.176"/>
- <SupportPoint posX="3419.56" posY="449.951"/>
- <SupportPoint posX="3429.14" posY="492.181"/>
+ <SupportPoint posX="418.01" posY="630.948"/>
+ <SupportPoint posX="434.685" posY="732.161"/>
+ <SupportPoint posX="500.938" posY="1037"/>
+ <SupportPoint posX="693.806" posY="1037"/>
+ <SupportPoint posX="693.806" posY="1037"/>
+ <SupportPoint posX="693.806" posY="1037"/>
+ <SupportPoint posX="1829.11" posY="1037"/>
+ <SupportPoint posX="2269.56" posY="1037"/>
+ <SupportPoint posX="2379.58" posY="1054.89"/>
+ <SupportPoint posX="2820.03" posY="1054.89"/>
+ <SupportPoint posX="2820.03" posY="1054.89"/>
+ <SupportPoint posX="2820.03" posY="1054.89"/>
+ <SupportPoint posX="3225.08" posY="1054.89"/>
+ <SupportPoint posX="3320.92" posY="1054.89"/>
+ <SupportPoint posX="3365.89" posY="1054.15"/>
+ <SupportPoint posX="3427.61" posY="980.778"/>
+ <SupportPoint posX="3430.42" posY="977.369"/>
+ <SupportPoint posX="3473.1" posY="733.451"/>
+ <SupportPoint posX="3491.76" posY="625.811"/>
</SupportPoints>
</Transition>
<Transition from="NavigationPose" to="GoToLandmarkWithObstacleAvoidance_2" eventName="Success">
@@ -337,7 +343,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="500.121" posY="381.328"/>
+ <SupportPoint posX="463.473" posY="598.722"/>
+ <SupportPoint posX="501.053" posY="598.722"/>
+ <SupportPoint posX="553.582" posY="598.722"/>
+ <SupportPoint posX="598.994" posY="598.722"/>
</SupportPoints>
</Transition>
<Transition from="GoToGuardLowering" to="Failure" eventName="Failure">
@@ -345,25 +354,25 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="712.614" posY="350.833"/>
- <SupportPoint posX="762.921" posY="350.833"/>
- <SupportPoint posX="842.667" posY="350.833"/>
- <SupportPoint posX="911.667" posY="350.833"/>
- <SupportPoint posX="911.667" posY="350.833"/>
- <SupportPoint posX="911.667" posY="350.833"/>
- <SupportPoint posX="1766.5" posY="350.833"/>
- <SupportPoint posX="1986.02" posY="350.833"/>
- <SupportPoint posX="2039.56" posY="316.333"/>
- <SupportPoint posX="2259.08" posY="316.333"/>
- <SupportPoint posX="2259.08" posY="316.333"/>
- <SupportPoint posX="2259.08" posY="316.333"/>
- <SupportPoint posX="3162.47" posY="316.333"/>
- <SupportPoint posX="3263.29" posY="316.333"/>
- <SupportPoint posX="3291.53" posY="349.568"/>
- <SupportPoint posX="3365" posY="418.556"/>
- <SupportPoint posX="3387.11" posY="439.243"/>
- <SupportPoint posX="3404.61" posY="468.121"/>
- <SupportPoint posX="3416.88" posY="492.794"/>
+ <SupportPoint posX="744.048" posY="286.05"/>
+ <SupportPoint posX="799.836" posY="272.084"/>
+ <SupportPoint posX="892.807" posY="252.444"/>
+ <SupportPoint posX="974.278" posY="252.444"/>
+ <SupportPoint posX="974.278" posY="252.444"/>
+ <SupportPoint posX="974.278" posY="252.444"/>
+ <SupportPoint posX="1585.69" posY="252.444"/>
+ <SupportPoint posX="1912.93" posY="252.444"/>
+ <SupportPoint posX="1994.46" posY="235.833"/>
+ <SupportPoint posX="2321.69" posY="235.833"/>
+ <SupportPoint posX="2321.69" posY="235.833"/>
+ <SupportPoint posX="2321.69" posY="235.833"/>
+ <SupportPoint posX="3225.08" posY="235.833"/>
+ <SupportPoint posX="3248.47" posY="235.833"/>
+ <SupportPoint posX="3401.16" posY="332.165"/>
+ <SupportPoint posX="3427.61" posY="366.167"/>
+ <SupportPoint posX="3464.41" posY="413.304"/>
+ <SupportPoint posX="3483.32" posY="480.298"/>
+ <SupportPoint posX="3492.52" posY="525.748"/>
</SupportPoints>
</Transition>
<Transition from="GoToGuardLowering" to="GoToDefaultPose" eventName="Success">
@@ -375,13 +384,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="684.478" posY="375.558"/>
- <SupportPoint posX="697.869" posY="389.128"/>
- <SupportPoint posX="716.077" posY="405.101"/>
- <SupportPoint posX="735.333" posY="414.722"/>
- <SupportPoint posX="770.562" posY="432.343"/>
- <SupportPoint posX="813.802" posY="441.594"/>
- <SupportPoint posX="848.634" posY="446.437"/>
+ <SupportPoint posX="743.946" posY="312.053"/>
+ <SupportPoint posX="790.763" posY="323.578"/>
+ <SupportPoint posX="860.824" posY="340.816"/>
+ <SupportPoint posX="911.373" posY="353.248"/>
</SupportPoints>
</Transition>
<Transition from="DummyState" to="GuardLowering" eventName="Failure">
@@ -391,10 +397,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1211.24" posY="526.438"/>
- <SupportPoint posX="1247.44" posY="526.822"/>
- <SupportPoint posX="1296.49" posY="527.346"/>
- <SupportPoint posX="1335.25" posY="527.767"/>
+ <SupportPoint posX="1273.85" posY="427.04"/>
+ <SupportPoint posX="1310.08" posY="427.615"/>
+ <SupportPoint posX="1359.14" posY="428.407"/>
+ <SupportPoint posX="1397.86" posY="429.033"/>
</SupportPoints>
</Transition>
<Transition from="DummyState_2" to="CarryAndRegrasp" eventName="Failure">
@@ -411,10 +417,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1698.01" posY="615.218"/>
- <SupportPoint posX="1734.43" posY="623.422"/>
- <SupportPoint posX="1783.75" posY="634.564"/>
- <SupportPoint posX="1822.59" posY="643.342"/>
+ <SupportPoint posX="1760.62" posY="525.812"/>
+ <SupportPoint posX="1797.04" posY="531.281"/>
+ <SupportPoint posX="1846.36" posY="538.705"/>
+ <SupportPoint posX="1885.21" posY="544.57"/>
</SupportPoints>
</Transition>
<Transition from="CloseBothHands" to="Failure" eventName="Failure">
@@ -422,13 +428,19 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2184.59" posY="626.629"/>
- <SupportPoint posX="2192" posY="623.153"/>
- <SupportPoint posX="2199.67" posY="620.074"/>
- <SupportPoint posX="2207.33" posY="617.889"/>
- <SupportPoint posX="2429.92" posY="553.949"/>
- <SupportPoint posX="3155.96" posY="544.238"/>
- <SupportPoint posX="3374.84" posY="542.768"/>
+ <SupportPoint posX="2247.07" posY="527.933"/>
+ <SupportPoint posX="2254.61" posY="524.994"/>
+ <SupportPoint posX="2262.41" posY="522.464"/>
+ <SupportPoint posX="2269.94" posY="520.778"/>
+ <SupportPoint posX="2586.58" posY="450.628"/>
+ <SupportPoint posX="2675.38" posY="508.894"/>
+ <SupportPoint posX="2999.56" posY="515.667"/>
+ <SupportPoint posX="3189.94" posY="519.641"/>
+ <SupportPoint posX="3244.76" posY="480.336"/>
+ <SupportPoint posX="3427.61" posY="533.556"/>
+ <SupportPoint posX="3431.32" posY="534.642"/>
+ <SupportPoint posX="3435.15" posY="535.958"/>
+ <SupportPoint posX="3438.86" posY="537.453"/>
</SupportPoints>
</Transition>
<Transition from="CloseBothHands" to="PutDownGuard" eventName="Success">
@@ -443,10 +455,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2184.97" posY="660.349"/>
- <SupportPoint posX="2224.07" posY="662.547"/>
- <SupportPoint posX="2278.51" posY="665.627"/>
- <SupportPoint posX="2320.54" posY="668.003"/>
+ <SupportPoint posX="2247.58" posY="561.245"/>
+ <SupportPoint posX="2286.68" posY="566.854"/>
+ <SupportPoint posX="2341.12" posY="574.687"/>
+ <SupportPoint posX="2383.16" posY="580.718"/>
</SupportPoints>
</Transition>
<Transition from="OpenBothHands" to="Failure" eventName="Failure">
@@ -454,13 +466,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2683.05" posY="732.288"/>
- <SupportPoint posX="2829.1" posY="729.707"/>
- <SupportPoint posX="3250" posY="716.214"/>
- <SupportPoint posX="3365" posY="646"/>
- <SupportPoint posX="3385.83" posY="633.273"/>
- <SupportPoint posX="3402.31" posY="612.075"/>
- <SupportPoint posX="3414.32" posY="592.167"/>
+ <SupportPoint posX="2745.79" posY="647.597"/>
+ <SupportPoint posX="2886.22" posY="634.219"/>
+ <SupportPoint posX="3282.33" posY="596.499"/>
+ <SupportPoint posX="3437.19" posY="581.741"/>
</SupportPoints>
</Transition>
<Transition from="OpenBothHands" to="RaiseAndGoToDefaultPose" eventName="Success">
@@ -473,13 +482,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2677.43" posY="774.659"/>
- <SupportPoint posX="2686.37" posY="781.202"/>
- <SupportPoint posX="2695.96" posY="787.258"/>
- <SupportPoint posX="2705.67" posY="791.667"/>
- <SupportPoint posX="2741.19" posY="807.716"/>
- <SupportPoint posX="2784.12" posY="815.421"/>
- <SupportPoint posX="2819.01" posY="819.088"/>
+ <SupportPoint posX="2740.17" posY="694.108"/>
+ <SupportPoint posX="2748.98" posY="700.651"/>
+ <SupportPoint posX="2758.57" posY="706.72"/>
+ <SupportPoint posX="2768.28" posY="711.167"/>
+ <SupportPoint posX="2803.8" posY="727.356"/>
+ <SupportPoint posX="2846.73" posY="735.355"/>
+ <SupportPoint posX="2881.62" posY="739.303"/>
</SupportPoints>
</Transition>
<Transition from="LookStraight" to="Failure" eventName="Failure">
@@ -487,25 +496,25 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="214.473" posY="205.563"/>
- <SupportPoint posX="264.6" posY="195.481"/>
- <SupportPoint posX="343.899" posY="182.167"/>
- <SupportPoint posX="413.333" posY="182.167"/>
- <SupportPoint posX="413.333" posY="182.167"/>
- <SupportPoint posX="413.333" posY="182.167"/>
- <SupportPoint posX="1647.67" posY="182.167"/>
- <SupportPoint posX="1808.79" posY="182.167"/>
- <SupportPoint posX="1848.79" posY="165.556"/>
- <SupportPoint posX="2009.92" posY="165.556"/>
- <SupportPoint posX="2009.92" posY="165.556"/>
- <SupportPoint posX="2009.92" posY="165.556"/>
- <SupportPoint posX="3162.47" posY="165.556"/>
- <SupportPoint posX="3265.21" posY="165.556"/>
- <SupportPoint posX="3301.37" posY="196.12"/>
- <SupportPoint posX="3365" posY="276.722"/>
- <SupportPoint posX="3391.32" posY="309.996"/>
- <SupportPoint posX="3416.11" posY="424.536"/>
- <SupportPoint posX="3428.89" posY="491.9"/>
+ <SupportPoint posX="204.481" posY="280.977"/>
+ <SupportPoint posX="250.557" posY="237.942"/>
+ <SupportPoint posX="330.406" posY="177.056"/>
+ <SupportPoint posX="413.333" posY="177.056"/>
+ <SupportPoint posX="413.333" posY="177.056"/>
+ <SupportPoint posX="413.333" posY="177.056"/>
+ <SupportPoint posX="1710.28" posY="177.056"/>
+ <SupportPoint posX="1982.06" posY="177.056"/>
+ <SupportPoint posX="2049.91" posY="160.444"/>
+ <SupportPoint posX="2321.69" posY="160.444"/>
+ <SupportPoint posX="2321.69" posY="160.444"/>
+ <SupportPoint posX="2321.69" posY="160.444"/>
+ <SupportPoint posX="3225.08" posY="160.444"/>
+ <SupportPoint posX="3259.46" posY="160.444"/>
+ <SupportPoint posX="3408.32" posY="271.522"/>
+ <SupportPoint posX="3427.61" posY="299.722"/>
+ <SupportPoint posX="3474.89" posY="368.786"/>
+ <SupportPoint posX="3491.5" posY="466.804"/>
+ <SupportPoint posX="3497.25" posY="525.544"/>
</SupportPoints>
</Transition>
<Transition from="LookStraight" to="NavigationPose" eventName="Success">
@@ -513,13 +522,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="214.166" posY="238.708"/>
- <SupportPoint posX="221.718" posY="241.494"/>
- <SupportPoint posX="229.487" posY="244.062"/>
- <SupportPoint posX="237" posY="246.056"/>
- <SupportPoint posX="273.902" posY="255.882"/>
- <SupportPoint posX="316.273" posY="261.874"/>
- <SupportPoint posX="350.237" posY="265.452"/>
+ <SupportPoint posX="202.014" posY="364.646"/>
+ <SupportPoint posX="249.075" posY="416.779"/>
+ <SupportPoint posX="328.936" posY="505.227"/>
+ <SupportPoint posX="375.831" posY="557.182"/>
</SupportPoints>
</Transition>
<Transition from="GoToLandmarkWithObstacleAvoidance" to="Failure" eventName="Failure">
@@ -527,13 +533,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="3238.5" posY="875.093"/>
- <SupportPoint posX="3288.21" posY="865.484"/>
- <SupportPoint posX="3347.11" posY="850.764"/>
- <SupportPoint posX="3365" posY="832.556"/>
- <SupportPoint posX="3397.97" posY="798.976"/>
- <SupportPoint posX="3420.71" posY="667.594"/>
- <SupportPoint posX="3431.32" posY="593.701"/>
+ <SupportPoint posX="3301.24" posY="813.568"/>
+ <SupportPoint posX="3342.89" posY="809.837"/>
+ <SupportPoint posX="3392.98" posY="798.605"/>
+ <SupportPoint posX="3427.61" posY="768.667"/>
+ <SupportPoint posX="3469.27" posY="732.557"/>
+ <SupportPoint posX="3487.16" posY="670.035"/>
+ <SupportPoint posX="3494.82" posY="625.964"/>
</SupportPoints>
</Transition>
<Transition from="GoToLandmarkWithObstacleAvoidance" to="Success" eventName="Success">
@@ -541,16 +547,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="3238.88" posY="908.162"/>
- <SupportPoint posX="3246.55" posY="909.631"/>
- <SupportPoint posX="3254.09" posY="910.883"/>
- <SupportPoint posX="3261.5" posY="911.778"/>
- <SupportPoint posX="3307.12" posY="917.298"/>
- <SupportPoint posX="3319" posY="912.583"/>
- <SupportPoint posX="3365" posY="911.778"/>
- <SupportPoint posX="3368.19" posY="911.727"/>
- <SupportPoint posX="3371.52" posY="911.65"/>
- <SupportPoint posX="3374.84" posY="911.573"/>
+ <SupportPoint posX="3294.34" posY="847.927"/>
+ <SupportPoint posX="3304.31" posY="852.425"/>
+ <SupportPoint posX="3314.4" posY="856.808"/>
+ <SupportPoint posX="3324.11" posY="860.667"/>
+ <SupportPoint posX="3361.17" posY="875.348"/>
+ <SupportPoint posX="3403.84" posY="889.021"/>
+ <SupportPoint posX="3437.83" posY="899.192"/>
</SupportPoints>
</Transition>
<Transition from="GoToLandmarkWithObstacleAvoidance_2" to="Failure" eventName="Failure">
@@ -558,7 +561,25 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2020.86" posY="726.369"/>
+ <SupportPoint posX="700.131" posY="621.978"/>
+ <SupportPoint posX="722.773" posY="699.616"/>
+ <SupportPoint posX="807.553" posY="945"/>
+ <SupportPoint posX="974.278" posY="945"/>
+ <SupportPoint posX="974.278" posY="945"/>
+ <SupportPoint posX="974.278" posY="945"/>
+ <SupportPoint posX="1829.11" posY="945"/>
+ <SupportPoint posX="2269.82" posY="945"/>
+ <SupportPoint posX="2379.32" posY="979.5"/>
+ <SupportPoint posX="2820.03" posY="979.5"/>
+ <SupportPoint posX="2820.03" posY="979.5"/>
+ <SupportPoint posX="2820.03" posY="979.5"/>
+ <SupportPoint posX="3225.08" posY="979.5"/>
+ <SupportPoint posX="3321.17" posY="979.5"/>
+ <SupportPoint posX="3364.49" posY="976.55"/>
+ <SupportPoint posX="3427.61" posY="904.111"/>
+ <SupportPoint posX="3463.39" posY="863.018"/>
+ <SupportPoint posX="3485.75" posY="707.333"/>
+ <SupportPoint posX="3495.21" posY="625.964"/>
</SupportPoints>
</Transition>
<Transition from="GoToLandmarkWithObstacleAvoidance_2" to="GoToDefaultPose" eventName="Success">
@@ -572,7 +593,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="763.726" posY="429.244"/>
+ <SupportPoint posX="722.236" posY="575.403"/>
+ <SupportPoint posX="767.009" posY="538.692"/>
+ <SupportPoint posX="854.882" posY="466.626"/>
+ <SupportPoint posX="914.107" posY="418.057"/>
</SupportPoints>
</Transition>
</Transitions>
--
GitLab
From 44c7023d74ba265fdfb5b9d6cefb2a997c8f3309 Mon Sep 17 00:00:00 2001
From: Jianfeng Gao <jianfeng.gao@kit.edu>
Date: Mon, 29 Jul 2024 19:22:06 +0200
Subject: [PATCH 02/22] working state of carry motion
---
.../GuardCarryingSkill.cpp | 1180 ++++++++---------
1 file changed, 590 insertions(+), 590 deletions(-)
diff --git a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.cpp b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.cpp
index fa64581ae..eb5c4a138 100644
--- a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.cpp
+++ b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.cpp
@@ -114,691 +114,691 @@ namespace armarx::Armar6BoardSupportDemo
armarx::skills::manager::dto::SkillExecutionID skillExecutionId;
try
{
- ARMARX_INFO << "Invoking control skill";
- skillExecutionId = skillMemory->executeSkillAsync(skillExecutionRequest.toManagerIce());
+ ARMARX_INFO << "Invoking control skill";
+ skillExecutionId = skillMemory->executeSkillAsync(skillExecutionRequest.toManagerIce());
}
catch (Ice::Exception const& e)
{
- ARMARX_ERROR << "Unhandeled Ice exception while executing skill '" << skillId.skillName
- << "'. Aborting..."
- << "Execution failed of skill " << skillId.skillName << e.what();
- emitFailure();
- return;
+ ARMARX_ERROR << "Unhandeled Ice exception while executing skill '" << skillId.skillName
+ << "'. Aborting..."
+ << "Execution failed of skill " << skillId.skillName << e.what();
+ emitFailure();
+ return;
}
catch (...)
{
- ARMARX_ERROR << "Unhandled error while executing skill '" << skillId.skillName
- << "'. Aborting...";
- emitFailure();
- return;
+ ARMARX_ERROR << "Unhandled error while executing skill '" << skillId.skillName
+ << "'. Aborting...";
+ emitFailure();
+ return;
}
- // ARMARX_INFO << "Control skill has been invoked; starting loop";
- // armarx::Metronome metronome(armarx::Frequency::Hertz(30));
- // while (!isRunningTaskStopped()) // stop run function if returning true
- // {
- // metronome.waitForNextTick();
- // }
-
-
- /// copied
- getTextToSpeech()->reportText(in.getTextBeforeThreeSeconds());
- viz::ScopedClient arviz(viz::Client::createFromTopic("GuardCarryingWithObstacleAvoidance", getArvizTopic()));
-
-
- NJointControllerInterfacePrx controller = getRobotUnit()->getNJointController("LeftController");
- if (controller)
+ ARMARX_INFO << "Control skill has been invoked; starting loop";
+ armarx::Metronome metronome(armarx::Frequency::Hertz(30));
+ while (!isRunningTaskStopped()) // stop run function if returning true
{
- getRobotUnit()->deactivateAndDeleteNJointController("LeftController");
- TimeUtil::SleepMS(10);
- }
-
- controller = getRobotUnit()->getNJointController("RightController");
- if (controller)
- {
- getRobotUnit()->deactivateAndDeleteNJointController("RightController");
- TimeUtil::SleepMS(10);
+ metronome.waitForNextTick();
}
- std::map<std::string, ::armarx::MatrixFloatPtr> restrictPolygonMap = in.getRestrictPolygon();
- std::map<std::string, ::armarx::Vector3Ptr> platFormTargetMap = in.getPlatformTargetPose();
-
- Eigen::MatrixXf polygon;
- Eigen::Vector3f platformTargetPose;
- if (in.getIsGuardPlacement())
- {
- polygon = restrictPolygonMap.at("GuardPlacement")->toEigen();
- platformTargetPose = platFormTargetMap.at("GuardPlacement")->toEigen();
- }
- else
- {
- polygon = restrictPolygonMap.at("GuardLifting")->toEigen();
- platformTargetPose = platFormTargetMap.at("GuardLifting")->toEigen();
- }
-
- std::vector<Eigen::Vector2f> points;
- for (int i = 0; i < polygon.rows(); ++i)
- {
- // Polygon is relative to target pose (but in global frame).
- Eigen::Vector2f point = platformTargetPose.head<2>() + polygon.row(i).transpose();
- points.push_back(point);
- }
- {
- viz::Layer layer = arviz.layer("Goal Polygon");
- viz::Polygon goalPolygon("Goal Polygon");
- for (const Eigen::Vector2f& point : points)
- {
- goalPolygon.addPoint(Eigen::Vector3f(point.x(), point.y(), +1));
- }
- goalPolygon.lineColor(simox::Color::green()).lineWidth(5);
- goalPolygon.color(simox::Color::green(255, 64));
- layer.add(goalPolygon);
- arviz.commit(layer);
- }
+// /// copied
+// getTextToSpeech()->reportText(in.getTextBeforeThreeSeconds());
+// viz::ScopedClient arviz(viz::Client::createFromTopic("GuardCarryingWithObstacleAvoidance", getArvizTopic()));
- VirtualRobot::MathTools::ConvexHull2DPtr convPolygon = VirtualRobot::MathTools::createConvexHull2D(points);
- devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx leftHandcontroller;
- if (!getRobotUnit()->getNJointController("Hand_L_EEF_NJointKITHandV2ShapeController"))
- {
- leftHandcontroller = StateBase::getContext<Armar6BoardSupportDemoStatechartContext>()->getProxy<devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx>("Hand_L_EEF_NJointKITHandV2ShapeController");
- }
- else
- {
- leftHandcontroller = devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx::checkedCast(getRobotUnit()->getNJointController("Hand_L_EEF_NJointKITHandV2ShapeController"));
- }
- leftHandcontroller->activateController();
+// NJointControllerInterfacePrx controller = getRobotUnit()->getNJointController("LeftController");
+// if (controller)
+// {
+// getRobotUnit()->deactivateAndDeleteNJointController("LeftController");
+// TimeUtil::SleepMS(10);
+// }
- devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx rightHandcontroller;
- if (!getRobotUnit()->getNJointController("Hand_R_EEF_NJointKITHandV2ShapeController"))
- {
- rightHandcontroller = StateBase::getContext<Armar6BoardSupportDemoStatechartContext>()->getProxy<devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx>("Hand_R_EEF_NJointKITHandV2ShapeController");
- }
- else
- {
- rightHandcontroller = devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx::checkedCast(getRobotUnit()->getNJointController("Hand_R_EEF_NJointKITHandV2ShapeController"));
- }
- rightHandcontroller->activateController();
+// controller = getRobotUnit()->getNJointController("RightController");
+// if (controller)
+// {
+// getRobotUnit()->deactivateAndDeleteNJointController("RightController");
+// TimeUtil::SleepMS(10);
+// }
- leftHandcontroller->setTargetsWithPwm(0, 2, 1, 1);
- rightHandcontroller->setTargetsWithPwm(0, 2, 1, 1);
+// std::map<std::string, ::armarx::MatrixFloatPtr> restrictPolygonMap = in.getRestrictPolygon();
+// std::map<std::string, ::armarx::Vector3Ptr> platFormTargetMap = in.getPlatformTargetPose();
- VirtualRobot::RobotPtr localRobot = getLocalRobot();
- RemoteRobot::synchronizeLocalClone(localRobot, getRobotStateComponent());
+// Eigen::MatrixXf polygon;
+// Eigen::Vector3f platformTargetPose;
+// if (in.getIsGuardPlacement())
+// {
+// polygon = restrictPolygonMap.at("GuardPlacement")->toEigen();
+// platformTargetPose = platFormTargetMap.at("GuardPlacement")->toEigen();
+// }
+// else
+// {
+// polygon = restrictPolygonMap.at("GuardLifting")->toEigen();
+// platformTargetPose = platFormTargetMap.at("GuardLifting")->toEigen();
+// }
- Eigen::Matrix4f desiredPoseLeft;
- if (in.isDesiredPoseLeftSet())
- {
- desiredPoseLeft = in.getDesiredPoseLeft()->toEigen();
- }
- else
- {
- desiredPoseLeft = localRobot->getRobotNodeSet("LeftArm")->getTCP()->getPoseInRootFrame();
- out.setDesiredPoseLeft(localRobot->getRobotNodeSet("LeftArm")->getTCP()->getPoseInRootFrame());
- }
+// std::vector<Eigen::Vector2f> points;
+// for (int i = 0; i < polygon.rows(); ++i)
+// {
+// // Polygon is relative to target pose (but in global frame).
+// Eigen::Vector2f point = platformTargetPose.head<2>() + polygon.row(i).transpose();
+// points.push_back(point);
+// }
+// {
+// viz::Layer layer = arviz.layer("Goal Polygon");
+// viz::Polygon goalPolygon("Goal Polygon");
+// for (const Eigen::Vector2f& point : points)
+// {
+// goalPolygon.addPoint(Eigen::Vector3f(point.x(), point.y(), +1));
+// }
+// goalPolygon.lineColor(simox::Color::green()).lineWidth(5);
+// goalPolygon.color(simox::Color::green(255, 64));
+// layer.add(goalPolygon);
+// arviz.commit(layer);
+// }
+// VirtualRobot::MathTools::ConvexHull2DPtr convPolygon = VirtualRobot::MathTools::createConvexHull2D(points);
- Eigen::Matrix4f desiredPoseRight;
- if (in.isDesiredPoseRightSet())
- {
- desiredPoseRight = in.getDesiredPoseRight()->toEigen();
- }
- else
- {
- desiredPoseRight = localRobot->getRobotNodeSet("RightArm")->getTCP()->getPoseInRootFrame();
- out.setDesiredPoseRight(localRobot->getRobotNodeSet("RightArm")->getTCP()->getPoseInRootFrame());
- }
+// devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx leftHandcontroller;
+// if (!getRobotUnit()->getNJointController("Hand_L_EEF_NJointKITHandV2ShapeController"))
+// {
+// leftHandcontroller = StateBase::getContext<Armar6BoardSupportDemoStatechartContext>()->getProxy<devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx>("Hand_L_EEF_NJointKITHandV2ShapeController");
+// }
+// else
+// {
+// leftHandcontroller = devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx::checkedCast(getRobotUnit()->getNJointController("Hand_L_EEF_NJointKITHandV2ShapeController"));
+// }
+// leftHandcontroller->activateController();
- const Eigen::Vector3f targetRight = desiredPoseRight.topRightCorner<3, 1>();
- const Eigen::Vector3f targetLeft = desiredPoseLeft.topRightCorner<3, 1>();
+// devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx rightHandcontroller;
+// if (!getRobotUnit()->getNJointController("Hand_R_EEF_NJointKITHandV2ShapeController"))
+// {
+// rightHandcontroller = StateBase::getContext<Armar6BoardSupportDemoStatechartContext>()->getProxy<devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx>("Hand_R_EEF_NJointKITHandV2ShapeController");
+// }
+// else
+// {
+// rightHandcontroller = devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx::checkedCast(getRobotUnit()->getNJointController("Hand_R_EEF_NJointKITHandV2ShapeController"));
+// }
+// rightHandcontroller->activateController();
+// leftHandcontroller->setTargetsWithPwm(0, 2, 1, 1);
+// rightHandcontroller->setTargetsWithPwm(0, 2, 1, 1);
-// const std::vector<float> Kpos = in.getKpos();
-// const std::vector<float> Kori = in.getKori();
-// const std::vector<float> Dpos = in.getDpos();
-// const std::vector<float> Dori = in.getDori();
- const std::string nodeSetNameLeft = "LeftArm";
- const std::string nodeSetNameRight = "RightArm";
- VirtualRobot::RobotNodePtr tcpRight = localRobot->getRobotNodeSet(nodeSetNameRight)->getTCP();
- VirtualRobot::RobotNodePtr tcpLeft = localRobot->getRobotNodeSet(nodeSetNameLeft)->getTCP();
+// VirtualRobot::RobotPtr localRobot = getLocalRobot();
+// RemoteRobot::synchronizeLocalClone(localRobot, getRobotStateComponent());
-// std::vector<float> desiredJointPositionLeft;
-// if (in.isDesiredLeftJointValuesSet())
+// Eigen::Matrix4f desiredPoseLeft;
+// if (in.isDesiredPoseLeftSet())
// {
-// desiredJointPositionLeft = in.getDesiredLeftJointValues();
+// desiredPoseLeft = in.getDesiredPoseLeft()->toEigen();
// }
// else
// {
-// desiredJointPositionLeft = localRobot->getRobotNodeSet(nodeSetNameLeft)->getJointValues();
+// desiredPoseLeft = localRobot->getRobotNodeSet("LeftArm")->getTCP()->getPoseInRootFrame();
+// out.setDesiredPoseLeft(localRobot->getRobotNodeSet("LeftArm")->getTCP()->getPoseInRootFrame());
// }
-// std::vector<float> desiredJointPositionRight;
-// if (in.isDesiredRightJointValuesSet())
+
+// Eigen::Matrix4f desiredPoseRight;
+// if (in.isDesiredPoseRightSet())
// {
-// desiredJointPositionRight = in.getDesiredRightJointValues();
+// desiredPoseRight = in.getDesiredPoseRight()->toEigen();
// }
// else
// {
-// desiredJointPositionRight = localRobot->getRobotNodeSet(nodeSetNameRight)->getJointValues();
+// desiredPoseRight = localRobot->getRobotNodeSet("RightArm")->getTCP()->getPoseInRootFrame();
+// out.setDesiredPoseRight(localRobot->getRobotNodeSet("RightArm")->getTCP()->getPoseInRootFrame());
// }
-// std::vector<float> knull = in.getKnull();
-// std::vector<float> dnull = in.getDnull();
+// const Eigen::Vector3f targetRight = desiredPoseRight.topRightCorner<3, 1>();
+// const Eigen::Vector3f targetLeft = desiredPoseLeft.topRightCorner<3, 1>();
+
+
+//// const std::vector<float> Kpos = in.getKpos();
+//// const std::vector<float> Kori = in.getKori();
+//// const std::vector<float> Dpos = in.getDpos();
+//// const std::vector<float> Dori = in.getDori();
+
+// const std::string nodeSetNameLeft = "LeftArm";
+// const std::string nodeSetNameRight = "RightArm";
+// VirtualRobot::RobotNodePtr tcpRight = localRobot->getRobotNodeSet(nodeSetNameRight)->getTCP();
+// VirtualRobot::RobotNodePtr tcpLeft = localRobot->getRobotNodeSet(nodeSetNameLeft)->getTCP();
+
+//// std::vector<float> desiredJointPositionLeft;
+//// if (in.isDesiredLeftJointValuesSet())
+//// {
+//// desiredJointPositionLeft = in.getDesiredLeftJointValues();
+//// }
+//// else
+//// {
+//// desiredJointPositionLeft = localRobot->getRobotNodeSet(nodeSetNameLeft)->getJointValues();
+//// }
+
+//// std::vector<float> desiredJointPositionRight;
+//// if (in.isDesiredRightJointValuesSet())
+//// {
+//// desiredJointPositionRight = in.getDesiredRightJointValues();
+//// }
+//// else
+//// {
+//// desiredJointPositionRight = localRobot->getRobotNodeSet(nodeSetNameRight)->getJointValues();
+//// }
-// NJointTaskSpaceImpedanceControlConfigPtr leftConfig = new NJointTaskSpaceImpedanceControlConfig;
-// leftConfig->nodeSetName = nodeSetNameLeft;
-// leftConfig->desiredPosition = simox::math::mat4f_to_pos(desiredPoseLeft);
-// leftConfig->desiredOrientation = simox::math::mat4f_to_quat(desiredPoseLeft);
-// leftConfig->Kpos = Eigen::Map<const Eigen::Vector3f>(Kpos.data(), Kpos.size());
-// leftConfig->Kori = Eigen::Map<const Eigen::Vector3f>(Kori.data(), Kori.size());
-// leftConfig->Dpos = Eigen::Map<const Eigen::Vector3f>(Dpos.data(), Dpos.size());
-// leftConfig->Dori = Eigen::Map<const Eigen::Vector3f>(Dori.data(), Dori.size());
-// leftConfig->desiredJointPositions = Eigen::Map<const Eigen::VectorXf>(desiredJointPositionLeft.data(), desiredJointPositionLeft.size());
-// leftConfig->Knull = Eigen::Map<const Eigen::VectorXf>(knull.data(), knull.size());
-// leftConfig->Dnull = Eigen::Map<const Eigen::VectorXf>(dnull.data(), dnull.size());
-// leftConfig->torqueLimit = in.getTorqueLimit();
+//// std::vector<float> knull = in.getKnull();
+//// std::vector<float> dnull = in.getDnull();
+
+//// NJointTaskSpaceImpedanceControlConfigPtr leftConfig = new NJointTaskSpaceImpedanceControlConfig;
+//// leftConfig->nodeSetName = nodeSetNameLeft;
+//// leftConfig->desiredPosition = simox::math::mat4f_to_pos(desiredPoseLeft);
+//// leftConfig->desiredOrientation = simox::math::mat4f_to_quat(desiredPoseLeft);
+//// leftConfig->Kpos = Eigen::Map<const Eigen::Vector3f>(Kpos.data(), Kpos.size());
+//// leftConfig->Kori = Eigen::Map<const Eigen::Vector3f>(Kori.data(), Kori.size());
+//// leftConfig->Dpos = Eigen::Map<const Eigen::Vector3f>(Dpos.data(), Dpos.size());
+//// leftConfig->Dori = Eigen::Map<const Eigen::Vector3f>(Dori.data(), Dori.size());
+//// leftConfig->desiredJointPositions = Eigen::Map<const Eigen::VectorXf>(desiredJointPositionLeft.data(), desiredJointPositionLeft.size());
+//// leftConfig->Knull = Eigen::Map<const Eigen::VectorXf>(knull.data(), knull.size());
+//// leftConfig->Dnull = Eigen::Map<const Eigen::VectorXf>(dnull.data(), dnull.size());
+//// leftConfig->torqueLimit = in.getTorqueLimit();
+
+//// NJointTaskSpaceImpedanceControlConfigPtr rightConfig = new NJointTaskSpaceImpedanceControlConfig;
+//// rightConfig->nodeSetName = nodeSetNameRight;
+//// rightConfig->desiredPosition = simox::math::mat4f_to_pos(desiredPoseRight);
+//// rightConfig->desiredOrientation = simox::math::mat4f_to_quat(desiredPoseRight);
+//// rightConfig->Kpos = Eigen::Map<const Eigen::Vector3f>(Kpos.data(), Kpos.size());
+//// rightConfig->Kori = Eigen::Map<const Eigen::Vector3f>(Kori.data(), Kori.size());
+//// rightConfig->Dpos = Eigen::Map<const Eigen::Vector3f>(Dpos.data(), Dpos.size());
+//// rightConfig->Dori = Eigen::Map<const Eigen::Vector3f>(Dori.data(), Dori.size());
+//// rightConfig->desiredJointPositions = Eigen::Map<const Eigen::VectorXf>(desiredJointPositionRight.data(), desiredJointPositionRight.size());
+//// rightConfig->Knull = Eigen::Map<const Eigen::VectorXf>(knull.data(), knull.size());
+//// rightConfig->Dnull = Eigen::Map<const Eigen::VectorXf>(dnull.data(), dnull.size());
+//// rightConfig->torqueLimit = in.getTorqueLimit();
+
+//// NJointTaskSpaceImpedanceControlInterfacePrx leftController
+//// = NJointTaskSpaceImpedanceControlInterfacePrx::checkedCast(getRobotUnit()->createNJointController("NJointTaskSpaceImpedanceController", "LeftController", leftConfig));
+//// NJointTaskSpaceImpedanceControlInterfacePrx rightController
+//// = NJointTaskSpaceImpedanceControlInterfacePrx::checkedCast(getRobotUnit()->createNJointController("NJointTaskSpaceImpedanceController", "RightController", rightConfig));
+
+
+//// leftController->activateController();
+//// rightController->activateController();
+
+
+// /* ---------------- platform ------------------- */
+
+// float Kplatform = in.getKplatform();
+// float Dplatform = in.getDplatform();
+
+// float maxVelocity = in.getMaxVelocity();
+
+// float KplatformAngle = in.getKplatformAngle();
+// float DplatformAngle = in.getDplatformAngle();
+// float maxAngularVelocity = in.getMaxAngularVelocity();
+
+// Eigen::Vector3f filteredLinearVel;
+// filteredLinearVel.setZero();
+// float filteredAngularVel = 0;
+
+// // currentTCPLinearVelocity_filtered = (1 - filterFactor) * currentTCPLinearVelocity_filtered + filterFactor * currentTCPLinearVelocity;
+
+// IceUtil::Time last = TimeUtil::GetTime();
+// float filterTimeConstant = 0.01; //0.05;
+
+// MultiDimPIDController pidTrans(in.getRestrictTranslationPID()->x, in.getRestrictTranslationPID()->y, in.getRestrictTranslationPID()->z);
+// PIDController pidRot(in.getRestrictRotationPID()->x, in.getRestrictRotationPID()->y, in.getRestrictRotationPID()->z);
+
+// DatafieldRefPtr platformVelX = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformVelocity", "velocityX"));
+// DatafieldRefPtr platformVelY = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformVelocity", "velocityY"));
+// DatafieldRefPtr platformRotVel = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformVelocity", "velocityRotation"));
+
+// DatafieldRefPtr platformPosX = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformPose", "positionX"));
+// DatafieldRefPtr platformPosY = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformPose", "positionY"));
+// DatafieldRefPtr platformRotAng = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformPose", "rotation"));
+
+// DatafieldRefPtr forceDfLeft = DatafieldRefPtr::dynamicCast(getForceTorqueObserver()->getForceDatafield("FT L_ArmL_FT"));
+// DatafieldRefPtr forceDfRight = DatafieldRefPtr::dynamicCast(getForceTorqueObserver()->getForceDatafield("FT R_ArmR_FT"));
+// Eigen::Vector3f initialForceLeft;
+// initialForceLeft.setZero();
+// Eigen::Vector3f initialForceRight;
+// initialForceRight.setZero();
+
+// usleep(100000);
+// // auto dd = getDebugDrawerTopic();
+
+
+//// //**** use obstacle avoidance component *****//
+//// ObstacleDetectionInterfacePrx obstacle_detection = getObstacleDetection();
+//// ObstacleAvoidanceInterfacePrx obstacle_avoidance = getObstacleAvoidance();
+//// // ObstacleAvoidanceInterfacePrx obstacle_avoidance = getObstacleAvoidance2D();
+//// // obstacleavoidance::Config obstacle_avoidance_cfg = obstacle_avoidance->getConfig();
+
+//// // clear_drawings(dd);
+
+// CycleUtil c(10);
+// // Eigen::Vector3f oldVisuPos = Eigen::Vector3f::Zero();
+// // int visuSphereCounter = 0;
+
+
+// /* ----------------- thumb feedback ------------------- */
+
+// DatafieldRefPtr thumbDFLeft = DatafieldRefPtr::dynamicCast(getKinematicUnitObserver()->getDatafieldRefByName("jointangles", "LeftHandThumb"));
+// DatafieldRefPtr thumbDFRight = DatafieldRefPtr::dynamicCast(getKinematicUnitObserver()->getDatafieldRefByName("jointangles", "RightHandThumb"));
+
+// IceUtil::Time start = TimeUtil::GetTime();
+// Eigen::Vector3f initialPositionRight;
+// Eigen::Vector3f initialPositionLeft;
+// Eigen::Vector3f initialDirection;
+// Eigen::Vector3f filteredPlatformTargetVelocity = Eigen::Vector3f::Zero();
+
+// Eigen::Vector3f dimensions;
+// dimensions << 805, 1205, 20;
+// bool isFirstLoop = true;
+// bool speechOutputAfterWait = true;
+// float minPosiTolerance = in.getMinPosiTolerance();
+// bool isImpedanceParameterReset = false;
-// NJointTaskSpaceImpedanceControlConfigPtr rightConfig = new NJointTaskSpaceImpedanceControlConfig;
-// rightConfig->nodeSetName = nodeSetNameRight;
-// rightConfig->desiredPosition = simox::math::mat4f_to_pos(desiredPoseRight);
-// rightConfig->desiredOrientation = simox::math::mat4f_to_quat(desiredPoseRight);
-// rightConfig->Kpos = Eigen::Map<const Eigen::Vector3f>(Kpos.data(), Kpos.size());
-// rightConfig->Kori = Eigen::Map<const Eigen::Vector3f>(Kori.data(), Kori.size());
-// rightConfig->Dpos = Eigen::Map<const Eigen::Vector3f>(Dpos.data(), Dpos.size());
-// rightConfig->Dori = Eigen::Map<const Eigen::Vector3f>(Dori.data(), Dori.size());
-// rightConfig->desiredJointPositions = Eigen::Map<const Eigen::VectorXf>(desiredJointPositionRight.data(), desiredJointPositionRight.size());
-// rightConfig->Knull = Eigen::Map<const Eigen::VectorXf>(knull.data(), knull.size());
-// rightConfig->Dnull = Eigen::Map<const Eigen::VectorXf>(dnull.data(), dnull.size());
-// rightConfig->torqueLimit = in.getTorqueLimit();
+// obstacledetection::Obstacle obs3d;
+//// //bool isObs3d = false;
+//// if (in.isObs3DPositionSet())
+//// {
+//// obstacledetection::ObstacleList obstacleList = obstacle_detection->getObstacles();
-// NJointTaskSpaceImpedanceControlInterfacePrx leftController
-// = NJointTaskSpaceImpedanceControlInterfacePrx::checkedCast(getRobotUnit()->createNJointController("NJointTaskSpaceImpedanceController", "LeftController", leftConfig));
-// NJointTaskSpaceImpedanceControlInterfacePrx rightController
-// = NJointTaskSpaceImpedanceControlInterfacePrx::checkedCast(getRobotUnit()->createNJointController("NJointTaskSpaceImpedanceController", "RightController", rightConfig));
+//// for (size_t i = 0; i < obstacleList.size(); ++i)
+//// {
+//// obstacledetection::Obstacle obs = obstacleList.at(i);
+//// if (obs.name == "obs3d")
+//// {
+//// ARMARX_INFO << "Removing obstacle 3d";
+//// obstacle_detection->removeObstacle("obs3d");
+//// obs3d = obs;
+//// //isObs3d = true;
+//// }
+//// }
+//// }
-// leftController->activateController();
-// rightController->activateController();
+// while (!isRunningTaskStopped())
+// {
+// RemoteRobot::synchronizeLocalClone(localRobot, getRobotStateComponent());
+// if (in.getIsConsiderFailure())
+// {
+// if (thumbDFLeft->getDataField()->getFloat() > in.getLeftThumbThreshold() || thumbDFRight->getDataField()->getFloat() > in.getRightThumbThreshold())
+// {
+// getPlatformUnit()->move(0, 0, 0);
+// emitLoseGuard();
+// return;
+// }
+// }
+// IceUtil::Time now = TimeUtil::GetTime();
+
+// /* Debug Drawer */
+// // if (in.isShowGuardSet() && in.getShowGuard())
+// // {
+// // std::string layerName = "guard_layer";
+// // std::string boxName = "guard";
+// // DrawColor color = {0.5, 0.5, 0.5, 1};
+// // }
+
+// /* Control Platform */
+// double timeDuration = (now - start).toSecondsDouble();
+// if (timeDuration < in.getMinExecutionTime() || isFirstLoop)
+// {
+// std::vector<float> currPoseLeft = Helpers::pose2vec(localRobot->getRobotNodeSet("LeftArm")->getTCP()->getPoseInRootFrame());
+// std::vector<float> currPoseRight = Helpers::pose2vec(localRobot->getRobotNodeSet("RightArm")->getTCP()->getPoseInRootFrame());
- /* ---------------- platform ------------------- */
+// initialPositionRight << currPoseRight.at(0), currPoseRight.at(1), currPoseRight.at(2);
+// initialPositionLeft << currPoseLeft.at(0), currPoseLeft.at(1), currPoseLeft.at(2);
+// initialDirection = initialPositionRight - initialPositionLeft;
+// initialDirection = initialDirection / initialDirection.norm();
- float Kplatform = in.getKplatform();
- float Dplatform = in.getDplatform();
+// {
+// FramedDirectionPtr forcePtr = forceDfLeft->getDataField()->get<FramedDirection>();
+// initialForceLeft = forcePtr->toRootEigen(localRobot);
+// }
+// {
+// FramedDirectionPtr forcePtr = forceDfRight->getDataField()->get<FramedDirection>();
+// initialForceRight = forcePtr->toRootEigen(localRobot);
+// }
- float maxVelocity = in.getMaxVelocity();
+// isFirstLoop = false;
- float KplatformAngle = in.getKplatformAngle();
- float DplatformAngle = in.getDplatformAngle();
- float maxAngularVelocity = in.getMaxAngularVelocity();
+// continue;
+// }
+// if (speechOutputAfterWait)
+// {
+// getTextToSpeech()->reportText(in.getTextAfterWaitForZeroTorque());
+// speechOutputAfterWait = false;
+
+// const Eigen::Vector3f desiredPositionRightVec = desiredPoseRight.topRightCorner<3, 1>();
+// const Eigen::Vector3f desiredPositionLeftVec = desiredPoseLeft.topRightCorner<3, 1>();
+// const Eigen::Vector3f positionRightError = initialPositionRight - desiredPositionRightVec;
+// const Eigen::Vector3f positionLeftError = initialPositionLeft - desiredPositionLeftVec;
+// ARMARX_INFO << "differece Pose: " << positionLeftError.norm() << ", " << positionRightError.norm();
+// }
- Eigen::Vector3f filteredLinearVel;
- filteredLinearVel.setZero();
- float filteredAngularVel = 0;
+// const Eigen::Vector3f currentPosition(
+// platformPosX->getDataField()->getFloat(),
+// platformPosY->getDataField()->getFloat(),
+// platformRotAng->getDataField()->getFloat()
+// );
+
+// ARMARX_DEBUG << "currentGuardPosition: " << currentPosition[0] << " ," << currentPosition[1];
+// bool platformInPolygon = in.getIsAutoDriveEnabled() && VirtualRobot::MathTools::isInside(currentPosition.head(2), convPolygon);
+// platformInPolygon = platformInPolygon && fabs(platformTargetPose(2) - currentPosition(2)) < in.getRestrictAngle() * M_PI / 180.0;
+
+// // RemoteRobot::synchronizeLocalClone(localRobot, getRobotStateComponent());
+// Eigen::Vector3f currentPositionRight = tcpRight->getPositionInRootFrame();
+// Eigen::Vector3f currentPositionLeft = tcpLeft->getPositionInRootFrame();
+
+// bool automaticDriveCondition = in.getIsAutoDriveEnabled() && ((targetLeft - currentPositionLeft).norm() < in.getHandOutofRangeThreshold() || (targetRight - currentPositionRight).norm() < in.getHandOutofRangeThreshold());
+// bool textReported = false;
+
+// // 3D obstacle avoidance
+
+//// bool isObs3DReached = false;
+//// if (in.isObs3DPositionSet())
+//// {
+//// Eigen::Vector3f currentGuardLocalPosi = (currentPositionLeft + currentPositionRight) / 2;
+//// currentGuardLocalPosi(1) += 0.6;
+//// Eigen::Vector3f currentGuardGlobalPosi = localRobot->toGlobalCoordinateSystemVec(currentGuardLocalPosi);
+//// currentGuardGlobalPosi(2) = 0;
+//// Eigen::Vector3f currentObsPosi = in.getObs3DPosition()->toEigen();
+
+//// float distToObs = (currentGuardGlobalPosi - currentObsPosi).norm();
+//// float heightOffset = 0;
+
+//// float distToReact = in.getObs3DDistToReact();
+
+//// if (distToObs < 0.5 * distToReact)
+//// {
+//// isObs3DReached = true;
+//// }
+//// else
+//// {
+//// isObs3DReached = false;
+//// }
+
+//// if (distToObs < distToReact)
+//// {
+
+//// float changeFactor = cos(distToObs / distToReact * M_PI / 2);
+//// heightOffset = in.getObs3DHeightAmplitude() * changeFactor;
+//// std::vector<float> currentKpos = Kpos;
+//// currentKpos[0] = Kpos[0] - in.getObs3DMaxiReducedStiffness()->x * changeFactor;
+//// // currentKpos[1] = Kpos[1] + in.getObs3DMaxiReducedStiffness()->y * changeFactor;
+
+//// leftController->setPosition(
+//// {
+//// desiredPoseLeft(0, 3),
+//// desiredPoseLeft(1, 3),
+//// desiredPoseLeft(2, 3) + heightOffset
+//// });
+//// rightController->setPosition(
+//// {
+//// desiredPoseRight(0, 3),
+//// desiredPoseRight(1, 3),
+//// desiredPoseRight(2, 3) + heightOffset
+//// });
+//// leftController->setImpedanceParameters("Kpos", currentKpos);
+//// rightController->setImpedanceParameters("Kpos", currentKpos);
+
+//// }
+//// else
+//// {
+//// leftController->setImpedanceParameters("Kpos", Kpos);
+//// rightController->setImpedanceParameters("Kpos", Kpos);
+//// }
+//// }
+
+
+// if (platformInPolygon && automaticDriveCondition)
+// {
+// if (!isImpedanceParameterReset)
+// {
+// if (!textReported)
+// {
+// getTextToSpeech()->reportText(in.getHandOutofRangeSpeechOutput());
+// textReported = true;
+// }
+
+// /// TODO this is not regarded anymore, may affect the demo
+//// leftController->setImpedanceParameters("Kpos", in.getRestrictKpos());
+//// leftController->setImpedanceParameters("Dpos", in.getRestrictDpos());
+//// rightController->setImpedanceParameters("Kpos", in.getRestrictKpos());
+//// rightController->setImpedanceParameters("Dpos", in.getRestrictDpos());
+// minPosiTolerance = in.getHandOutofRangeThreshold();
+// isImpedanceParameterReset = true;
+// }
+// float posx = platformPosX->getDataField()->getFloat();
+// float posy = platformPosY->getDataField()->getFloat();
+// float ori = platformRotAng->getDataField()->getFloat();
- // currentTCPLinearVelocity_filtered = (1 - filterFactor) * currentTCPLinearVelocity_filtered + filterFactor * currentTCPLinearVelocity;
+// Eigen::Vector3f pos {posx, posy, 0.0f};
- IceUtil::Time last = TimeUtil::GetTime();
- float filterTimeConstant = 0.01; //0.05;
+// if (ori > M_PI)
+// {
+// ori = - 2 * M_PI + ori;
+// }
- MultiDimPIDController pidTrans(in.getRestrictTranslationPID()->x, in.getRestrictTranslationPID()->y, in.getRestrictTranslationPID()->z);
- PIDController pidRot(in.getRestrictRotationPID()->x, in.getRestrictRotationPID()->y, in.getRestrictRotationPID()->z);
+// float angleDelta = platformTargetPose(2) - ori;
+// ARMARX_INFO << deactivateSpam(1) << VAROUT(angleDelta);
- DatafieldRefPtr platformVelX = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformVelocity", "velocityX"));
- DatafieldRefPtr platformVelY = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformVelocity", "velocityY"));
- DatafieldRefPtr platformRotVel = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformVelocity", "velocityRotation"));
+// // transform alpha to [-pi, pi)
+// while (angleDelta < -M_PI)
+// {
+// angleDelta += 2 * M_PI;
+// }
- DatafieldRefPtr platformPosX = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformPose", "positionX"));
- DatafieldRefPtr platformPosY = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformPose", "positionY"));
- DatafieldRefPtr platformRotAng = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformPose", "rotation"));
+// while (angleDelta >= M_PI)
+// {
+// angleDelta -= 2 * M_PI;
+// }
- DatafieldRefPtr forceDfLeft = DatafieldRefPtr::dynamicCast(getForceTorqueObserver()->getForceDatafield("FT L_ArmL_FT"));
- DatafieldRefPtr forceDfRight = DatafieldRefPtr::dynamicCast(getForceTorqueObserver()->getForceDatafield("FT R_ArmR_FT"));
- Eigen::Vector3f initialForceLeft;
- initialForceLeft.setZero();
- Eigen::Vector3f initialForceRight;
- initialForceRight.setZero();
+// Eigen::Vector3f target {platformTargetPose(0), platformTargetPose(1), 0.0f};
+// pidTrans.update(pos, target);
+// pidRot.update(angleDelta, 0);
- usleep(100000);
- // auto dd = getDebugDrawerTopic();
+// Eigen::Vector2f newVel(pidTrans.getControlValue()[0], pidTrans.getControlValue()[1]);
-// //**** use obstacle avoidance component *****//
-// ObstacleDetectionInterfacePrx obstacle_detection = getObstacleDetection();
-// ObstacleAvoidanceInterfacePrx obstacle_avoidance = getObstacleAvoidance();
-// // ObstacleAvoidanceInterfacePrx obstacle_avoidance = getObstacleAvoidance2D();
-// // obstacleavoidance::Config obstacle_avoidance_cfg = obstacle_avoidance->getConfig();
+// float newVelocityRot = -signedMinFunction(pidRot.getControlValue(), in.getRestrictMaxAngularVelocity());
+// Eigen::Vector3f globalVel {newVel[0], newVel[1], 0};
+// Eigen::Matrix3f m;
+// m = Eigen::AngleAxisf(ori, Eigen::Vector3f::UnitZ());
+// Eigen::Vector3f localVel = m.inverse() * globalVel;
+// if (localVel.norm() > in.getRestrictMaxVelocity())
+// {
+// localVel *= in.getRestrictMaxVelocity() / localVel.norm();
+// }
-// // clear_drawings(dd);
+// if (std::isnan(localVel[0]) || std::isnan(localVel[1]) || std::isnan(newVelocityRot))
+// {
+// throw LocalException("A target velocity is NaN!");
+// }
- CycleUtil c(10);
- // Eigen::Vector3f oldVisuPos = Eigen::Vector3f::Zero();
- // int visuSphereCounter = 0;
+// getPlatformUnit()->move(localVel[0], localVel[1], newVelocityRot);
- /* ----------------- thumb feedback ------------------- */
+// if (fabs(pidTrans.previousError) < in.getPositionalAccuracy() && fabs(pidRot.previousError) < in.getOrientationalAccuracy())
+// {
+// break;
+// }
+// else
+// {
+// ARMARX_INFO << deactivateSpam(1) << "error: " << pidTrans.previousError << " x velocity: " << newVel[0] << " y velocity: " << newVel[1];
+// ARMARX_INFO << deactivateSpam(1) << "rot error: " << pidRot.previousError << " rot velocity: " << newVelocityRot;
+// }
- DatafieldRefPtr thumbDFLeft = DatafieldRefPtr::dynamicCast(getKinematicUnitObserver()->getDatafieldRefByName("jointangles", "LeftHandThumb"));
- DatafieldRefPtr thumbDFRight = DatafieldRefPtr::dynamicCast(getKinematicUnitObserver()->getDatafieldRefByName("jointangles", "RightHandThumb"));
+// usleep(100000);
+// }
+// else
+// {
+// if (!platformInPolygon && isImpedanceParameterReset)
+// {
+//// leftController->setImpedanceParameters("Kpos", in.getKpos());
+//// leftController->setImpedanceParameters("Dpos", in.getDpos());
+//// rightController->setImpedanceParameters("Kpos", in.getKpos());
+//// rightController->setImpedanceParameters("Dpos", in.getDpos());
+// minPosiTolerance = in.getMinPosiTolerance();
+// isImpedanceParameterReset = false;
+// }
- IceUtil::Time start = TimeUtil::GetTime();
- Eigen::Vector3f initialPositionRight;
- Eigen::Vector3f initialPositionLeft;
- Eigen::Vector3f initialDirection;
- Eigen::Vector3f filteredPlatformTargetVelocity = Eigen::Vector3f::Zero();
+// Eigen::Vector3f positionDeltaRight = currentPositionRight - initialPositionRight;
+// Eigen::Vector3f positionDeltaLeft = currentPositionLeft - initialPositionLeft;
+// Eigen::Vector3f positionDelta = (positionDeltaLeft + positionDeltaRight) / 2;
+// Eigen::Vector3f currentVel;
+// currentVel << platformVelX->getDataField()->getFloat(), platformVelY->getDataField()->getFloat(), 0;
+// Eigen::Vector3f velocity;
+// if (positionDelta.norm() < minPosiTolerance)
+// {
+// velocity.setZero();
+// }
+// else
+// {
+// positionDelta = positionDelta - minPosiTolerance * positionDelta / positionDelta.norm();
+// velocity = positionDelta * Kplatform - currentVel * Dplatform;
+// }
- Eigen::Vector3f dimensions;
- dimensions << 805, 1205, 20;
- bool isFirstLoop = true;
- bool speechOutputAfterWait = true;
- float minPosiTolerance = in.getMinPosiTolerance();
- bool isImpedanceParameterReset = false;
- obstacledetection::Obstacle obs3d;
-// //bool isObs3d = false;
-// if (in.isObs3DPositionSet())
-// {
-// obstacledetection::ObstacleList obstacleList = obstacle_detection->getObstacles();
+// double deltaT = (now - last).toSecondsDouble();
+// last = now;
-// for (size_t i = 0; i < obstacleList.size(); ++i)
-// {
-// obstacledetection::Obstacle obs = obstacleList.at(i);
-// if (obs.name == "obs3d")
+// if (deltaT == 0)
// {
-// ARMARX_INFO << "Removing obstacle 3d";
-// obstacle_detection->removeObstacle("obs3d");
-// obs3d = obs;
-// //isObs3d = true;
+// continue;
// }
-// }
+// // ARMARX_INFO << "deltaT: " << deltaT;
+// float Tstar = 1 / ((filterTimeConstant / deltaT) + 1);
+// filteredLinearVel = Tstar * (velocity - filteredLinearVel) + filteredLinearVel;
-// }
+// if (!std::isfinite(filteredLinearVel(0)))
+// {
+// filteredLinearVel(0) = 0;
+// }
+// if (!std::isfinite(filteredLinearVel(1)))
+// {
+// filteredLinearVel(1) = 0;
+// }
- while (!isRunningTaskStopped())
- {
- RemoteRobot::synchronizeLocalClone(localRobot, getRobotStateComponent());
-
- if (in.getIsConsiderFailure())
- {
- if (thumbDFLeft->getDataField()->getFloat() > in.getLeftThumbThreshold() || thumbDFRight->getDataField()->getFloat() > in.getRightThumbThreshold())
- {
- getPlatformUnit()->move(0, 0, 0);
- emitLoseGuard();
- return;
- }
- }
- IceUtil::Time now = TimeUtil::GetTime();
-
- /* Debug Drawer */
- // if (in.isShowGuardSet() && in.getShowGuard())
- // {
- // std::string layerName = "guard_layer";
- // std::string boxName = "guard";
- // DrawColor color = {0.5, 0.5, 0.5, 1};
- // }
-
- /* Control Platform */
- double timeDuration = (now - start).toSecondsDouble();
- if (timeDuration < in.getMinExecutionTime() || isFirstLoop)
- {
- std::vector<float> currPoseLeft = Helpers::pose2vec(localRobot->getRobotNodeSet("LeftArm")->getTCP()->getPoseInRootFrame());
- std::vector<float> currPoseRight = Helpers::pose2vec(localRobot->getRobotNodeSet("RightArm")->getTCP()->getPoseInRootFrame());
-
- initialPositionRight << currPoseRight.at(0), currPoseRight.at(1), currPoseRight.at(2);
- initialPositionLeft << currPoseLeft.at(0), currPoseLeft.at(1), currPoseLeft.at(2);
- initialDirection = initialPositionRight - initialPositionLeft;
- initialDirection = initialDirection / initialDirection.norm();
-
- {
- FramedDirectionPtr forcePtr = forceDfLeft->getDataField()->get<FramedDirection>();
- initialForceLeft = forcePtr->toRootEigen(localRobot);
- }
- {
- FramedDirectionPtr forcePtr = forceDfRight->getDataField()->get<FramedDirection>();
- initialForceRight = forcePtr->toRootEigen(localRobot);
- }
-
- isFirstLoop = false;
-
- continue;
- }
- if (speechOutputAfterWait)
- {
- getTextToSpeech()->reportText(in.getTextAfterWaitForZeroTorque());
- speechOutputAfterWait = false;
-
- const Eigen::Vector3f desiredPositionRightVec = desiredPoseRight.topRightCorner<3, 1>();
- const Eigen::Vector3f desiredPositionLeftVec = desiredPoseLeft.topRightCorner<3, 1>();
- const Eigen::Vector3f positionRightError = initialPositionRight - desiredPositionRightVec;
- const Eigen::Vector3f positionLeftError = initialPositionLeft - desiredPositionLeftVec;
- ARMARX_INFO << "differece Pose: " << positionLeftError.norm() << ", " << positionRightError.norm();
- }
-
- const Eigen::Vector3f currentPosition(
- platformPosX->getDataField()->getFloat(),
- platformPosY->getDataField()->getFloat(),
- platformRotAng->getDataField()->getFloat()
- );
-
- ARMARX_DEBUG << "currentGuardPosition: " << currentPosition[0] << " ," << currentPosition[1];
- bool platformInPolygon = in.getIsAutoDriveEnabled() && VirtualRobot::MathTools::isInside(currentPosition.head(2), convPolygon);
- platformInPolygon = platformInPolygon && fabs(platformTargetPose(2) - currentPosition(2)) < in.getRestrictAngle() * M_PI / 180.0;
-
- // RemoteRobot::synchronizeLocalClone(localRobot, getRobotStateComponent());
- Eigen::Vector3f currentPositionRight = tcpRight->getPositionInRootFrame();
- Eigen::Vector3f currentPositionLeft = tcpLeft->getPositionInRootFrame();
-
- bool automaticDriveCondition = in.getIsAutoDriveEnabled() && ((targetLeft - currentPositionLeft).norm() < in.getHandOutofRangeThreshold() || (targetRight - currentPositionRight).norm() < in.getHandOutofRangeThreshold());
- bool textReported = false;
-
- // 3D obstacle avoidance
-
-// bool isObs3DReached = false;
-// if (in.isObs3DPositionSet())
-// {
-// Eigen::Vector3f currentGuardLocalPosi = (currentPositionLeft + currentPositionRight) / 2;
-// currentGuardLocalPosi(1) += 0.6;
-// Eigen::Vector3f currentGuardGlobalPosi = localRobot->toGlobalCoordinateSystemVec(currentGuardLocalPosi);
-// currentGuardGlobalPosi(2) = 0;
-// Eigen::Vector3f currentObsPosi = in.getObs3DPosition()->toEigen();
+// velocity = math::MathUtils::LimitTo(filteredLinearVel, maxVelocity); // in mm
-// float distToObs = (currentGuardGlobalPosi - currentObsPosi).norm();
-// float heightOffset = 0;
+// // modulated velocity for obstacle avoidance
-// float distToReact = in.getObs3DDistToReact();
+// //const Eigen::Vector2f agent_pos = localRobot->getGlobalPosition().head<2>();
-// if (distToObs < 0.5 * distToReact)
+// float ori = platformRotAng->getDataField()->getFloat();
+// if (ori > M_PI)
// {
-// isObs3DReached = true;
+// ori = - 2 * M_PI + ori;
// }
-// else
+
+// Eigen::Matrix3f m;
+// m = Eigen::AngleAxisf(ori, Eigen::Vector3f::UnitZ());
+// Eigen::Vector3f globalVel = m * velocity;
+
+// obstacleavoidance::Agent agent;
+// agent.safety_margin = 0;
+// agent.pos = localRobot->getGlobalPosition();
+// agent.desired_vel = globalVel;
+// Eigen::Vector3f modulated_vel = obstacle_avoidance->modulateVelocity(agent);
+// modulated_vel[2] = 0;
+// Eigen::Vector3f localVel = m.inverse() * modulated_vel;
+// localVel[2] = 0;
+// if (localVel.norm() > in.getMaxVelocity())
// {
-// isObs3DReached = false;
+// localVel *= in.getMaxVelocity() / localVel.norm();
// }
-// if (distToObs < distToReact)
+// //
+// float angularVelocity ;
+
+// Eigen::Vector3f currentDirection = currentPositionRight - currentPositionLeft;
+// currentDirection = currentDirection / currentDirection.norm();
+
+// float cosAlpha = currentDirection.dot(initialDirection);
+
+// float alpha = 0;
+
+// Eigen::Vector3f directionDiff = currentDirection - initialDirection;
+// if (directionDiff(1) < 0)
+// {
+// alpha = -acos(cosAlpha);
+// }
+// else
// {
+// alpha = acos(cosAlpha);
+// }
-// float changeFactor = cos(distToObs / distToReact * M_PI / 2);
-// heightOffset = in.getObs3DHeightAmplitude() * changeFactor;
-// std::vector<float> currentKpos = Kpos;
-// currentKpos[0] = Kpos[0] - in.getObs3DMaxiReducedStiffness()->x * changeFactor;
-// // currentKpos[1] = Kpos[1] + in.getObs3DMaxiReducedStiffness()->y * changeFactor;
-
-// leftController->setPosition(
-// {
-// desiredPoseLeft(0, 3),
-// desiredPoseLeft(1, 3),
-// desiredPoseLeft(2, 3) + heightOffset
-// });
-// rightController->setPosition(
-// {
-// desiredPoseRight(0, 3),
-// desiredPoseRight(1, 3),
-// desiredPoseRight(2, 3) + heightOffset
-// });
-// leftController->setImpedanceParameters("Kpos", currentKpos);
-// rightController->setImpedanceParameters("Kpos", currentKpos);
+// float minAngleTolerance = in.getMinAngleTolerance();
+// if (fabs(alpha) < minAngleTolerance)
+// {
+// angularVelocity = 0;
// }
// else
// {
-// leftController->setImpedanceParameters("Kpos", Kpos);
-// rightController->setImpedanceParameters("Kpos", Kpos);
+// if (alpha > 0)
+// {
+// alpha -= minAngleTolerance;
+// }
+// else
+// {
+// alpha += minAngleTolerance;
+// }
+// angularVelocity = KplatformAngle * alpha - DplatformAngle * platformRotVel;
+// }
+
+// filteredAngularVel = Tstar * (angularVelocity - filteredAngularVel) + filteredAngularVel;
+// if (!std::isfinite(filteredAngularVel))
+// {
+// filteredAngularVel = 0;
// }
-// }
+// angularVelocity = math::MathUtils::LimitTo(filteredAngularVel, maxAngularVelocity);
- if (platformInPolygon && automaticDriveCondition)
- {
- if (!isImpedanceParameterReset)
- {
- if (!textReported)
- {
- getTextToSpeech()->reportText(in.getHandOutofRangeSpeechOutput());
- textReported = true;
- }
-
- /// TODO this is not regarded anymore, may affect the demo
-// leftController->setImpedanceParameters("Kpos", in.getRestrictKpos());
-// leftController->setImpedanceParameters("Dpos", in.getRestrictDpos());
-// rightController->setImpedanceParameters("Kpos", in.getRestrictKpos());
-// rightController->setImpedanceParameters("Dpos", in.getRestrictDpos());
- minPosiTolerance = in.getHandOutofRangeThreshold();
- isImpedanceParameterReset = true;
- }
- float posx = platformPosX->getDataField()->getFloat();
- float posy = platformPosY->getDataField()->getFloat();
- float ori = platformRotAng->getDataField()->getFloat();
-
- Eigen::Vector3f pos {posx, posy, 0.0f};
-
- if (ori > M_PI)
- {
- ori = - 2 * M_PI + ori;
- }
-
- float angleDelta = platformTargetPose(2) - ori;
- ARMARX_INFO << deactivateSpam(1) << VAROUT(angleDelta);
-
- // transform alpha to [-pi, pi)
- while (angleDelta < -M_PI)
- {
- angleDelta += 2 * M_PI;
- }
-
- while (angleDelta >= M_PI)
- {
- angleDelta -= 2 * M_PI;
- }
-
- Eigen::Vector3f target {platformTargetPose(0), platformTargetPose(1), 0.0f};
- pidTrans.update(pos, target);
- pidRot.update(angleDelta, 0);
-
- Eigen::Vector2f newVel(pidTrans.getControlValue()[0], pidTrans.getControlValue()[1]);
-
-
- float newVelocityRot = -signedMinFunction(pidRot.getControlValue(), in.getRestrictMaxAngularVelocity());
- Eigen::Vector3f globalVel {newVel[0], newVel[1], 0};
- Eigen::Matrix3f m;
- m = Eigen::AngleAxisf(ori, Eigen::Vector3f::UnitZ());
- Eigen::Vector3f localVel = m.inverse() * globalVel;
- if (localVel.norm() > in.getRestrictMaxVelocity())
- {
- localVel *= in.getRestrictMaxVelocity() / localVel.norm();
- }
-
- if (std::isnan(localVel[0]) || std::isnan(localVel[1]) || std::isnan(newVelocityRot))
- {
- throw LocalException("A target velocity is NaN!");
- }
-
- getPlatformUnit()->move(localVel[0], localVel[1], newVelocityRot);
-
-
- if (fabs(pidTrans.previousError) < in.getPositionalAccuracy() && fabs(pidRot.previousError) < in.getOrientationalAccuracy())
- {
- break;
- }
- else
- {
- ARMARX_INFO << deactivateSpam(1) << "error: " << pidTrans.previousError << " x velocity: " << newVel[0] << " y velocity: " << newVel[1];
- ARMARX_INFO << deactivateSpam(1) << "rot error: " << pidRot.previousError << " rot velocity: " << newVelocityRot;
- }
-
- usleep(100000);
- }
- else
- {
- if (!platformInPolygon && isImpedanceParameterReset)
- {
-// leftController->setImpedanceParameters("Kpos", in.getKpos());
-// leftController->setImpedanceParameters("Dpos", in.getDpos());
-// rightController->setImpedanceParameters("Kpos", in.getKpos());
-// rightController->setImpedanceParameters("Dpos", in.getDpos());
- minPosiTolerance = in.getMinPosiTolerance();
- isImpedanceParameterReset = false;
- }
-
- Eigen::Vector3f positionDeltaRight = currentPositionRight - initialPositionRight;
- Eigen::Vector3f positionDeltaLeft = currentPositionLeft - initialPositionLeft;
- Eigen::Vector3f positionDelta = (positionDeltaLeft + positionDeltaRight) / 2;
- Eigen::Vector3f currentVel;
- currentVel << platformVelX->getDataField()->getFloat(), platformVelY->getDataField()->getFloat(), 0;
- Eigen::Vector3f velocity;
- if (positionDelta.norm() < minPosiTolerance)
- {
- velocity.setZero();
- }
- else
- {
- positionDelta = positionDelta - minPosiTolerance * positionDelta / positionDelta.norm();
- velocity = positionDelta * Kplatform - currentVel * Dplatform;
- }
-
-
- double deltaT = (now - last).toSecondsDouble();
- last = now;
-
- if (deltaT == 0)
- {
- continue;
- }
- // ARMARX_INFO << "deltaT: " << deltaT;
- float Tstar = 1 / ((filterTimeConstant / deltaT) + 1);
- filteredLinearVel = Tstar * (velocity - filteredLinearVel) + filteredLinearVel;
-
- if (!std::isfinite(filteredLinearVel(0)))
- {
- filteredLinearVel(0) = 0;
- }
- if (!std::isfinite(filteredLinearVel(1)))
- {
- filteredLinearVel(1) = 0;
- }
-
- velocity = math::MathUtils::LimitTo(filteredLinearVel, maxVelocity); // in mm
-
- // modulated velocity for obstacle avoidance
-
- //const Eigen::Vector2f agent_pos = localRobot->getGlobalPosition().head<2>();
-
- float ori = platformRotAng->getDataField()->getFloat();
- if (ori > M_PI)
- {
- ori = - 2 * M_PI + ori;
- }
-
- Eigen::Matrix3f m;
- m = Eigen::AngleAxisf(ori, Eigen::Vector3f::UnitZ());
- Eigen::Vector3f globalVel = m * velocity;
-
- obstacleavoidance::Agent agent;
- agent.safety_margin = 0;
- agent.pos = localRobot->getGlobalPosition();
- agent.desired_vel = globalVel;
- Eigen::Vector3f modulated_vel = obstacle_avoidance->modulateVelocity(agent);
- modulated_vel[2] = 0;
- Eigen::Vector3f localVel = m.inverse() * modulated_vel;
- localVel[2] = 0;
- if (localVel.norm() > in.getMaxVelocity())
- {
- localVel *= in.getMaxVelocity() / localVel.norm();
- }
-
- //
- float angularVelocity ;
-
- Eigen::Vector3f currentDirection = currentPositionRight - currentPositionLeft;
- currentDirection = currentDirection / currentDirection.norm();
-
- float cosAlpha = currentDirection.dot(initialDirection);
-
- float alpha = 0;
-
- Eigen::Vector3f directionDiff = currentDirection - initialDirection;
- if (directionDiff(1) < 0)
- {
- alpha = -acos(cosAlpha);
- }
- else
- {
- alpha = acos(cosAlpha);
- }
-
-
- float minAngleTolerance = in.getMinAngleTolerance();
- if (fabs(alpha) < minAngleTolerance)
- {
- angularVelocity = 0;
- }
- else
- {
- if (alpha > 0)
- {
- alpha -= minAngleTolerance;
- }
- else
- {
- alpha += minAngleTolerance;
- }
- angularVelocity = KplatformAngle * alpha - DplatformAngle * platformRotVel;
- }
-
- filteredAngularVel = Tstar * (angularVelocity - filteredAngularVel) + filteredAngularVel;
- if (!std::isfinite(filteredAngularVel))
- {
- filteredAngularVel = 0;
- }
-
- angularVelocity = math::MathUtils::LimitTo(filteredAngularVel, maxAngularVelocity);
-
- getDebugObserver()->setDebugChannel("FollowMeTorque",
- {
- { "vx", new Variant(velocity(0)) },
- { "vy", new Variant(velocity(1)) },
- { "vangle", new Variant(angularVelocity) },
- });
- localVel(2) = angularVelocity;
-
- if (std::isnan(localVel[0]) || std::isnan(localVel[1]) || std::isnan(angularVelocity))
- {
- ARMARX_WARNING << deactivateSpam(1) << "Some velocity target is NaN - setting to zero";
- localVel.setZero();
- }
-
- float ratio = 0.7;
- filteredPlatformTargetVelocity = filteredPlatformTargetVelocity * ratio + (1.0 - ratio) * localVel;
- float oriVel = filteredPlatformTargetVelocity(2);
-
-// if (isObs3DReached)
+// getDebugObserver()->setDebugChannel("FollowMeTorque",
+// {
+// { "vx", new Variant(velocity(0)) },
+// { "vy", new Variant(velocity(1)) },
+// { "vangle", new Variant(angularVelocity) },
+// });
+// localVel(2) = angularVelocity;
+
+// if (std::isnan(localVel[0]) || std::isnan(localVel[1]) || std::isnan(angularVelocity))
// {
-// oriVel = 0;
+// ARMARX_WARNING << deactivateSpam(1) << "Some velocity target is NaN - setting to zero";
+// localVel.setZero();
// }
- getPlatformUnit()->move(filteredPlatformTargetVelocity(0), filteredPlatformTargetVelocity(1), oriVel);
+// float ratio = 0.7;
+// filteredPlatformTargetVelocity = filteredPlatformTargetVelocity * ratio + (1.0 - ratio) * localVel;
+// float oriVel = filteredPlatformTargetVelocity(2);
+//// if (isObs3DReached)
+//// {
+//// oriVel = 0;
+//// }
- // debug drawer ....
- // {
- // if ((oldVisuPos - localRobot->getGlobalPosition()).norm() > 50)
- // {
- // draw_agent_path(dd, agent_pos, visuSphereCounter++);
- // oldVisuPos = localRobot->getGlobalPosition();
- // }
- // draw_agent_safety_margin(dd, agent_pos, ori, obstacle_avoidance_cfg.agent_safety_margin);
- // draw_agent_desired_velocity(dd, agent_pos, globalVel.head(2));
- // draw_agent_modulated_velocity(dd, agent_pos, modulated_vel.head(2));
- // }
+// getPlatformUnit()->move(filteredPlatformTargetVelocity(0), filteredPlatformTargetVelocity(1), oriVel);
- }
- c.waitForCycleDuration();
- }
+// // debug drawer ....
+// // {
+// // if ((oldVisuPos - localRobot->getGlobalPosition()).norm() > 50)
+// // {
+// // draw_agent_path(dd, agent_pos, visuSphereCounter++);
+// // oldVisuPos = localRobot->getGlobalPosition();
+// // }
+// // draw_agent_safety_margin(dd, agent_pos, ori, obstacle_avoidance_cfg.agent_safety_margin);
+// // draw_agent_desired_velocity(dd, agent_pos, globalVel.head(2));
+// // draw_agent_modulated_velocity(dd, agent_pos, modulated_vel.head(2));
+// // }
+
+// }
+
+// c.waitForCycleDuration();
+// }
ARMARX_INFO << "running task has been requested to stop";
skillMemory->abortSkillAsync(skillExecutionId);
--
GitLab
From 48f9b7815145fd00a2144dfa173a5db85635e056 Mon Sep 17 00:00:00 2001
From: Jianfeng Gao <jianfeng.gao@kit.edu>
Date: Tue, 30 Jul 2024 11:52:12 +0200
Subject: [PATCH 03/22] add navigation skill to guardcarrying skill, simplify
statechart implementation
---
.../Armar6BoardSupportDemo/CMakeLists.txt | 2 +
.../GuardCarryingSkill.cpp | 1481 +++++++++--------
.../GuardCarryingSkill.xml | 215 +--
3 files changed, 873 insertions(+), 825 deletions(-)
diff --git a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/CMakeLists.txt b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/CMakeLists.txt
index 324a02c87..d89960194 100644
--- a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/CMakeLists.txt
+++ b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/CMakeLists.txt
@@ -17,6 +17,8 @@ armarx_add_statechart_group(Armar6BoardSupportDemo
armarx_control::deprecated_njoint_mp_controller_interface
armarx_control::skills_platform_follower_controller
armarx_control::platform_follower_aron
+ armarx_navigation::skills_aron
+ armarx_navigation::skills
armar6_rt::interfaces
armar6_skills::interfaces
devices_ethercat::hand_armar6_v2_interfaces
diff --git a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.cpp b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.cpp
index eb5c4a138..60bb05799 100644
--- a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.cpp
+++ b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.cpp
@@ -33,6 +33,9 @@
#include <armarx/control/skills/skills/platform_follower_controller/aron/FollowerParams.aron.generated.h>
#include <armarx/control/njoint_controller/platform/platform_follower_controller/aron/PlatformFollowerControllerConfig.aron.generated.h>
+#include <armarx/navigation/skills/constants.h>
+#include <armarx/navigation/skills/aron/NavigateTo.aron.generated.h>
+
#include <armar6/skills/statecharts/Armar6BoardSupportDemo/Armar6BoardSupportDemoStatechartContext.generated.h>
///// copied
@@ -74,7 +77,7 @@ namespace armarx::Armar6BoardSupportDemo
void GuardCarryingSkill::run()
{
- ARMARX_INFO << "GuardCarryingSkill is running";
+ /// -------------------------------- adding skills -----------------------------------------
const std::string providerName = "bimanual_transport_skill_provider";
const std::string skillName = "Follower";
const armarx::skills::ProviderID providerId = {.providerName = providerName};
@@ -111,7 +114,8 @@ namespace armarx::Armar6BoardSupportDemo
StateBase::getContext<armarx::Armar6BoardSupportDemo::Armar6BoardSupportDemoStatechartContext>()
->getProxy<armarx::skills::manager::dti::SkillManagerInterfacePrx>("SkillMemory");
- armarx::skills::manager::dto::SkillExecutionID skillExecutionId;
+ std::optional<armarx::skills::manager::dto::SkillExecutionID> skillExecutionId =
+ std::nullopt;
try
{
ARMARX_INFO << "Invoking control skill";
@@ -133,677 +137,824 @@ namespace armarx::Armar6BoardSupportDemo
return;
}
- ARMARX_INFO << "Control skill has been invoked; starting loop";
+ /// ------------------------------- adding navigateTo skill --------------------------------
+
+ // prepare invoking navigateTo skill
+ // (skill ID)
+ const armarx::skills::SkillID navigateToSkillId{
+ .providerId = armarx::skills::ProviderID{
+ .providerName = armarx::navigation::skills::constants::NavigationSkillProviderName
+ },
+ .skillName = armarx::navigation::skills::constants::skill_names::NavigateTo
+ };
+ ARMARX_CHECK(navigateToSkillId.isFullySpecified());
+
+ auto navigateToSubskill = [&](
+ const Eigen::Vector3f& platformTargetPose
+ ){
+ /*
+ * convert platform target pose
+ */
+ Eigen::Vector3f targetPosition = Eigen::Vector3f{platformTargetPose(0), platformTargetPose(1), 0};
+ Eigen::Matrix3f targetOrientation = Eigen::AngleAxisf(platformTargetPose(2), Eigen::Vector3f::UnitZ()).matrix();
+ Eigen::Matrix4f targetPose = armarx::Pose(targetOrientation, targetPosition).toEigen();
+
+ /*
+ * (parameters)
+ */
+ auto navigateToParams = armarx::navigation::skills::arondto::NavigateToParams();
+ // converts from column major to row major
+ armarx::aron::eigen::toAron(navigateToParams.targetPose, targetPose);
+ navigateToParams.velocityLimitLinear = in.getRestrictMaxVelocity();
+ navigateToParams.velocityLimitAngular = in.getRestrictMaxAngularVelocity();
+
+ /*
+ * execution request
+ */
+ armarx::skills::SkillExecutionRequest navigateToSkillExecutionRequest{
+ .skillId = navigateToSkillId,
+ .executorName = "GuardCarryingSkillStatechart",
+ .parameters = navigateToParams.toAron()
+ };
+
+ /*
+ * invoke skill synchronously
+ */
+ armarx::skills::manager::dto::SkillStatusUpdate navigateToSkillStatusUpdate;
+ try
+ {
+ ARMARX_INFO << "Invoking navigateTo skill";
+ navigateToSkillStatusUpdate = skillMemory->executeSkill(
+ navigateToSkillExecutionRequest.toManagerIce()
+ );
+ }
+ catch (Ice::Exception const& e)
+ {
+ ARMARX_ERROR << "Unhandeled Ice exception while executing skill "
+ << "'" << navigateToSkillId.skillName << "'. "
+ << "Aborting..."
+ << "Execution failed of skill " << navigateToSkillId.skillName << e.what();
+ emitFailure();
+ return;
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "Unhandled error while executing skill "
+ << "'" << navigateToSkillId.skillName << "'. "
+ << "Aborting...";
+ emitFailure();
+ return;
+ }
+ };
+
+ /// -------------------------------- adding polygon visualization --------------------------
+ getTextToSpeech()->reportText(in.getTextBeforeThreeSeconds());
+ viz::ScopedClient arviz(viz::Client::createFromTopic("GuardCarryingWithObstacleAvoidance", getArvizTopic()));
+
+ /// visualizing the target polygons
+ std::map<std::string, ::armarx::MatrixFloatPtr> restrictPolygonMap = in.getRestrictPolygon();
+ std::map<std::string, ::armarx::Vector3Ptr> platFormTargetMap = in.getPlatformTargetPose();
+
+ Eigen::MatrixXf polygon;
+ Eigen::Vector3f platformTargetPose;
+ if (in.getIsGuardPlacement())
+ {
+ polygon = restrictPolygonMap.at("GuardPlacement")->toEigen();
+ platformTargetPose = platFormTargetMap.at("GuardPlacement")->toEigen();
+ }
+ else
+ {
+ polygon = restrictPolygonMap.at("GuardLifting")->toEigen();
+ platformTargetPose = platFormTargetMap.at("GuardLifting")->toEigen();
+ }
+
+ std::vector<Eigen::Vector2f> points;
+ for (int i = 0; i < polygon.rows(); ++i)
+ {
+ // Polygon is relative to target pose (but in global frame).
+ Eigen::Vector2f point = platformTargetPose.head<2>() + polygon.row(i).transpose();
+ points.push_back(point);
+ }
+ {
+ viz::Layer layer = arviz.layer("Goal Polygon");
+ viz::Polygon goalPolygon("Goal Polygon");
+ for (const Eigen::Vector2f& point : points)
+ {
+ goalPolygon.addPoint(Eigen::Vector3f(point.x(), point.y(), +1));
+ }
+ goalPolygon.lineColor(simox::Color::green()).lineWidth(5);
+ goalPolygon.color(simox::Color::green(255, 64));
+ layer.add(goalPolygon);
+ arviz.commit(layer);
+ }
+
+ VirtualRobot::MathTools::ConvexHull2DPtr convPolygon = VirtualRobot::MathTools::createConvexHull2D(points);
+
+
+ /// -------------------------------- set hand target ---------------------------------------
+ devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx leftHandcontroller;
+ if (!getRobotUnit()->getNJointController("Hand_L_EEF_NJointKITHandV2ShapeController"))
+ {
+ leftHandcontroller = StateBase::getContext<Armar6BoardSupportDemoStatechartContext>()->getProxy<devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx>("Hand_L_EEF_NJointKITHandV2ShapeController");
+ }
+ else
+ {
+ leftHandcontroller = devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx::checkedCast(getRobotUnit()->getNJointController("Hand_L_EEF_NJointKITHandV2ShapeController"));
+ }
+ leftHandcontroller->activateController();
+
+ devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx rightHandcontroller;
+ if (!getRobotUnit()->getNJointController("Hand_R_EEF_NJointKITHandV2ShapeController"))
+ {
+ rightHandcontroller = StateBase::getContext<Armar6BoardSupportDemoStatechartContext>()->getProxy<devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx>("Hand_R_EEF_NJointKITHandV2ShapeController");
+ }
+ else
+ {
+ rightHandcontroller = devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx::checkedCast(getRobotUnit()->getNJointController("Hand_R_EEF_NJointKITHandV2ShapeController"));
+ }
+ rightHandcontroller->activateController();
+
+ leftHandcontroller->setTargetsWithPwm(0, 2, 1, 1);
+ rightHandcontroller->setTargetsWithPwm(0, 2, 1, 1);
+
+
+ /// -------------------------------- get tcp pose ------------------------------------------
+ /// obtain the current tcp poses to be used by later on states
+ VirtualRobot::RobotPtr localRobot = getLocalRobot();
+ RemoteRobot::synchronizeLocalClone(localRobot, getRobotStateComponent());
+
+ const std::string nodeSetNameLeft = "LeftArm";
+ const std::string nodeSetNameRight = "RightArm";
+ VirtualRobot::RobotNodePtr tcpRight = localRobot->getRobotNodeSet(nodeSetNameRight)->getTCP();
+ VirtualRobot::RobotNodePtr tcpLeft = localRobot->getRobotNodeSet(nodeSetNameLeft)->getTCP();
+
+ Eigen::Matrix4f desiredPoseLeft;
+ if (in.isDesiredPoseLeftSet())
+ {
+ desiredPoseLeft = in.getDesiredPoseLeft()->toEigen();
+ }
+ else
+ {
+ desiredPoseLeft = tcpLeft->getPoseInRootFrame();
+ out.setDesiredPoseLeft(desiredPoseLeft);
+ }
+
+
+ Eigen::Matrix4f desiredPoseRight;
+ if (in.isDesiredPoseRightSet())
+ {
+ desiredPoseRight = in.getDesiredPoseRight()->toEigen();
+ }
+ else
+ {
+ desiredPoseRight = tcpRight->getPoseInRootFrame();
+ out.setDesiredPoseRight(desiredPoseRight);
+ }
+
+ const Eigen::Vector3f targetRight = desiredPoseRight.topRightCorner<3, 1>();
+ const Eigen::Vector3f targetLeft = desiredPoseLeft.topRightCorner<3, 1>();
+
+ /// -------------------------------- get platform position/orientation ---------------------
+ DatafieldRefPtr platformPosX = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformPose", "positionX"));
+ DatafieldRefPtr platformPosY = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformPose", "positionY"));
+ DatafieldRefPtr platformRotAng = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformPose", "rotation"));
+
+ /// -------------------------------- thumb feedback ----------------------------------------
+ DatafieldRefPtr thumbDFLeft = DatafieldRefPtr::dynamicCast(getKinematicUnitObserver()->getDatafieldRefByName("jointangles", "LeftHandThumb"));
+ DatafieldRefPtr thumbDFRight = DatafieldRefPtr::dynamicCast(getKinematicUnitObserver()->getDatafieldRefByName("jointangles", "RightHandThumb"));
+
+ /// ----------------------------------- main loop ------------------------------------------
+ bool speechOutputAfterWait = true;
+ bool enterPolygonTextReported = false;
+
+ ARMARX_DEBUG << "Control skill has been invoked; starting loop";
armarx::Metronome metronome(armarx::Frequency::Hertz(30));
while (!isRunningTaskStopped()) // stop run function if returning true
{
+ RemoteRobot::synchronizeLocalClone(localRobot, getRobotStateComponent());
+
+ if (in.getIsConsiderFailure())
+ {
+ if (
+ thumbDFLeft->getDataField()->getFloat() > in.getLeftThumbThreshold() ||
+ thumbDFRight->getDataField()->getFloat() > in.getRightThumbThreshold()
+ )
+ {
+ // Thumb is closed too far, i.e., there is no guard in the hand anymore;
+ // transition to error recovery state
+ getPlatformUnit()->move(0, 0, 0);
+ emitLoseGuard();
+ return;
+ }
+ }
+ if (speechOutputAfterWait)
+ {
+ getTextToSpeech()->reportText(in.getTextAfterWaitForZeroTorque());
+ speechOutputAfterWait = false;
+ }
+
+ const Eigen::Vector3f currentPosition(
+ platformPosX->getDataField()->getFloat(),
+ platformPosY->getDataField()->getFloat(),
+ platformRotAng->getDataField()->getFloat()
+ );
+
+ ARMARX_DEBUG << "currentGuardPosition: " << currentPosition[0] << " ," << currentPosition[1];
+ bool platformInPolygon = in.getIsAutoDriveEnabled() && VirtualRobot::MathTools::isInside(currentPosition.head(2), convPolygon);
+ platformInPolygon = platformInPolygon && fabs(platformTargetPose(2) - currentPosition(2)) < in.getRestrictAngle() * M_PI / 180.0;
+
+ Eigen::Vector3f currentPositionRight = tcpRight->getPositionInRootFrame();
+ Eigen::Vector3f currentPositionLeft = tcpLeft->getPositionInRootFrame();
+
+
+ ARMARX_CHECK(in.getIsAutoDriveEnabled())
+ << "Current implementation relies on auto drive being enabled to terminate "
+ << "properly when reaching the target.";
+ bool automaticDriveCondition = in.getIsAutoDriveEnabled() &&
+ (
+ (targetLeft - currentPositionLeft).norm() < in.getHandOutofRangeThreshold() ||
+ (targetRight - currentPositionRight).norm() < in.getHandOutofRangeThreshold()
+ );
+
+ if (platformInPolygon && automaticDriveCondition)
+ {
+ if (!enterPolygonTextReported)
+ {
+ getTextToSpeech()->reportText(in.getHandOutofRangeSpeechOutput());
+ enterPolygonTextReported = true;
+ }
+
+ if(skillExecutionId.has_value())
+ {
+ skillMemory->abortSkill(skillExecutionId.value());
+ skillExecutionId = std::nullopt;
+ }
+ navigateToSubskill(platformTargetPose);
+ break;
+ }
metronome.waitForNextTick();
}
-
-// /// copied
-// getTextToSpeech()->reportText(in.getTextBeforeThreeSeconds());
-// viz::ScopedClient arviz(viz::Client::createFromTopic("GuardCarryingWithObstacleAvoidance", getArvizTopic()));
-
-
-// NJointControllerInterfacePrx controller = getRobotUnit()->getNJointController("LeftController");
-// if (controller)
-// {
-// getRobotUnit()->deactivateAndDeleteNJointController("LeftController");
-// TimeUtil::SleepMS(10);
-// }
-
-// controller = getRobotUnit()->getNJointController("RightController");
-// if (controller)
-// {
-// getRobotUnit()->deactivateAndDeleteNJointController("RightController");
-// TimeUtil::SleepMS(10);
-// }
-
-
-// std::map<std::string, ::armarx::MatrixFloatPtr> restrictPolygonMap = in.getRestrictPolygon();
-// std::map<std::string, ::armarx::Vector3Ptr> platFormTargetMap = in.getPlatformTargetPose();
-
-// Eigen::MatrixXf polygon;
-// Eigen::Vector3f platformTargetPose;
-// if (in.getIsGuardPlacement())
-// {
-// polygon = restrictPolygonMap.at("GuardPlacement")->toEigen();
-// platformTargetPose = platFormTargetMap.at("GuardPlacement")->toEigen();
-// }
-// else
-// {
-// polygon = restrictPolygonMap.at("GuardLifting")->toEigen();
-// platformTargetPose = platFormTargetMap.at("GuardLifting")->toEigen();
-// }
-
-// std::vector<Eigen::Vector2f> points;
-// for (int i = 0; i < polygon.rows(); ++i)
-// {
-// // Polygon is relative to target pose (but in global frame).
-// Eigen::Vector2f point = platformTargetPose.head<2>() + polygon.row(i).transpose();
-// points.push_back(point);
-// }
-// {
-// viz::Layer layer = arviz.layer("Goal Polygon");
-// viz::Polygon goalPolygon("Goal Polygon");
-// for (const Eigen::Vector2f& point : points)
-// {
-// goalPolygon.addPoint(Eigen::Vector3f(point.x(), point.y(), +1));
-// }
-// goalPolygon.lineColor(simox::Color::green()).lineWidth(5);
-// goalPolygon.color(simox::Color::green(255, 64));
-// layer.add(goalPolygon);
-// arviz.commit(layer);
-// }
-
-// VirtualRobot::MathTools::ConvexHull2DPtr convPolygon = VirtualRobot::MathTools::createConvexHull2D(points);
-
-// devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx leftHandcontroller;
-// if (!getRobotUnit()->getNJointController("Hand_L_EEF_NJointKITHandV2ShapeController"))
-// {
-// leftHandcontroller = StateBase::getContext<Armar6BoardSupportDemoStatechartContext>()->getProxy<devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx>("Hand_L_EEF_NJointKITHandV2ShapeController");
-// }
-// else
-// {
-// leftHandcontroller = devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx::checkedCast(getRobotUnit()->getNJointController("Hand_L_EEF_NJointKITHandV2ShapeController"));
-// }
-// leftHandcontroller->activateController();
-
-// devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx rightHandcontroller;
-// if (!getRobotUnit()->getNJointController("Hand_R_EEF_NJointKITHandV2ShapeController"))
-// {
-// rightHandcontroller = StateBase::getContext<Armar6BoardSupportDemoStatechartContext>()->getProxy<devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx>("Hand_R_EEF_NJointKITHandV2ShapeController");
-// }
-// else
-// {
-// rightHandcontroller = devices::ethercat::hand::armar6_v2::ShapeControllerInterfacePrx::checkedCast(getRobotUnit()->getNJointController("Hand_R_EEF_NJointKITHandV2ShapeController"));
-// }
-// rightHandcontroller->activateController();
-
-// leftHandcontroller->setTargetsWithPwm(0, 2, 1, 1);
-// rightHandcontroller->setTargetsWithPwm(0, 2, 1, 1);
-
-
-// VirtualRobot::RobotPtr localRobot = getLocalRobot();
-// RemoteRobot::synchronizeLocalClone(localRobot, getRobotStateComponent());
-
-// Eigen::Matrix4f desiredPoseLeft;
-// if (in.isDesiredPoseLeftSet())
-// {
-// desiredPoseLeft = in.getDesiredPoseLeft()->toEigen();
-// }
-// else
-// {
-// desiredPoseLeft = localRobot->getRobotNodeSet("LeftArm")->getTCP()->getPoseInRootFrame();
-// out.setDesiredPoseLeft(localRobot->getRobotNodeSet("LeftArm")->getTCP()->getPoseInRootFrame());
-// }
-
-
-// Eigen::Matrix4f desiredPoseRight;
-// if (in.isDesiredPoseRightSet())
-// {
-// desiredPoseRight = in.getDesiredPoseRight()->toEigen();
-// }
-// else
-// {
-// desiredPoseRight = localRobot->getRobotNodeSet("RightArm")->getTCP()->getPoseInRootFrame();
-// out.setDesiredPoseRight(localRobot->getRobotNodeSet("RightArm")->getTCP()->getPoseInRootFrame());
-// }
-
-// const Eigen::Vector3f targetRight = desiredPoseRight.topRightCorner<3, 1>();
-// const Eigen::Vector3f targetLeft = desiredPoseLeft.topRightCorner<3, 1>();
-
-
-//// const std::vector<float> Kpos = in.getKpos();
-//// const std::vector<float> Kori = in.getKori();
-//// const std::vector<float> Dpos = in.getDpos();
-//// const std::vector<float> Dori = in.getDori();
-
-// const std::string nodeSetNameLeft = "LeftArm";
-// const std::string nodeSetNameRight = "RightArm";
-// VirtualRobot::RobotNodePtr tcpRight = localRobot->getRobotNodeSet(nodeSetNameRight)->getTCP();
-// VirtualRobot::RobotNodePtr tcpLeft = localRobot->getRobotNodeSet(nodeSetNameLeft)->getTCP();
-
-//// std::vector<float> desiredJointPositionLeft;
-//// if (in.isDesiredLeftJointValuesSet())
-//// {
-//// desiredJointPositionLeft = in.getDesiredLeftJointValues();
-//// }
-//// else
-//// {
-//// desiredJointPositionLeft = localRobot->getRobotNodeSet(nodeSetNameLeft)->getJointValues();
-//// }
-
-//// std::vector<float> desiredJointPositionRight;
-//// if (in.isDesiredRightJointValuesSet())
-//// {
-//// desiredJointPositionRight = in.getDesiredRightJointValues();
-//// }
-//// else
-//// {
-//// desiredJointPositionRight = localRobot->getRobotNodeSet(nodeSetNameRight)->getJointValues();
-//// }
-
-//// std::vector<float> knull = in.getKnull();
-//// std::vector<float> dnull = in.getDnull();
-
-//// NJointTaskSpaceImpedanceControlConfigPtr leftConfig = new NJointTaskSpaceImpedanceControlConfig;
-//// leftConfig->nodeSetName = nodeSetNameLeft;
-//// leftConfig->desiredPosition = simox::math::mat4f_to_pos(desiredPoseLeft);
-//// leftConfig->desiredOrientation = simox::math::mat4f_to_quat(desiredPoseLeft);
-//// leftConfig->Kpos = Eigen::Map<const Eigen::Vector3f>(Kpos.data(), Kpos.size());
-//// leftConfig->Kori = Eigen::Map<const Eigen::Vector3f>(Kori.data(), Kori.size());
-//// leftConfig->Dpos = Eigen::Map<const Eigen::Vector3f>(Dpos.data(), Dpos.size());
-//// leftConfig->Dori = Eigen::Map<const Eigen::Vector3f>(Dori.data(), Dori.size());
-//// leftConfig->desiredJointPositions = Eigen::Map<const Eigen::VectorXf>(desiredJointPositionLeft.data(), desiredJointPositionLeft.size());
-//// leftConfig->Knull = Eigen::Map<const Eigen::VectorXf>(knull.data(), knull.size());
-//// leftConfig->Dnull = Eigen::Map<const Eigen::VectorXf>(dnull.data(), dnull.size());
-//// leftConfig->torqueLimit = in.getTorqueLimit();
-
-//// NJointTaskSpaceImpedanceControlConfigPtr rightConfig = new NJointTaskSpaceImpedanceControlConfig;
-//// rightConfig->nodeSetName = nodeSetNameRight;
-//// rightConfig->desiredPosition = simox::math::mat4f_to_pos(desiredPoseRight);
-//// rightConfig->desiredOrientation = simox::math::mat4f_to_quat(desiredPoseRight);
-//// rightConfig->Kpos = Eigen::Map<const Eigen::Vector3f>(Kpos.data(), Kpos.size());
-//// rightConfig->Kori = Eigen::Map<const Eigen::Vector3f>(Kori.data(), Kori.size());
-//// rightConfig->Dpos = Eigen::Map<const Eigen::Vector3f>(Dpos.data(), Dpos.size());
-//// rightConfig->Dori = Eigen::Map<const Eigen::Vector3f>(Dori.data(), Dori.size());
-//// rightConfig->desiredJointPositions = Eigen::Map<const Eigen::VectorXf>(desiredJointPositionRight.data(), desiredJointPositionRight.size());
-//// rightConfig->Knull = Eigen::Map<const Eigen::VectorXf>(knull.data(), knull.size());
-//// rightConfig->Dnull = Eigen::Map<const Eigen::VectorXf>(dnull.data(), dnull.size());
-//// rightConfig->torqueLimit = in.getTorqueLimit();
-
-//// NJointTaskSpaceImpedanceControlInterfacePrx leftController
-//// = NJointTaskSpaceImpedanceControlInterfacePrx::checkedCast(getRobotUnit()->createNJointController("NJointTaskSpaceImpedanceController", "LeftController", leftConfig));
-//// NJointTaskSpaceImpedanceControlInterfacePrx rightController
-//// = NJointTaskSpaceImpedanceControlInterfacePrx::checkedCast(getRobotUnit()->createNJointController("NJointTaskSpaceImpedanceController", "RightController", rightConfig));
-
-
-//// leftController->activateController();
-//// rightController->activateController();
-
-
-// /* ---------------- platform ------------------- */
-
-// float Kplatform = in.getKplatform();
-// float Dplatform = in.getDplatform();
-
-// float maxVelocity = in.getMaxVelocity();
-
-// float KplatformAngle = in.getKplatformAngle();
-// float DplatformAngle = in.getDplatformAngle();
-// float maxAngularVelocity = in.getMaxAngularVelocity();
-
-// Eigen::Vector3f filteredLinearVel;
-// filteredLinearVel.setZero();
-// float filteredAngularVel = 0;
-
-// // currentTCPLinearVelocity_filtered = (1 - filterFactor) * currentTCPLinearVelocity_filtered + filterFactor * currentTCPLinearVelocity;
-
-// IceUtil::Time last = TimeUtil::GetTime();
-// float filterTimeConstant = 0.01; //0.05;
-
-// MultiDimPIDController pidTrans(in.getRestrictTranslationPID()->x, in.getRestrictTranslationPID()->y, in.getRestrictTranslationPID()->z);
-// PIDController pidRot(in.getRestrictRotationPID()->x, in.getRestrictRotationPID()->y, in.getRestrictRotationPID()->z);
-
-// DatafieldRefPtr platformVelX = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformVelocity", "velocityX"));
-// DatafieldRefPtr platformVelY = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformVelocity", "velocityY"));
-// DatafieldRefPtr platformRotVel = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformVelocity", "velocityRotation"));
-
-// DatafieldRefPtr platformPosX = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformPose", "positionX"));
-// DatafieldRefPtr platformPosY = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformPose", "positionY"));
-// DatafieldRefPtr platformRotAng = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformPose", "rotation"));
-
-// DatafieldRefPtr forceDfLeft = DatafieldRefPtr::dynamicCast(getForceTorqueObserver()->getForceDatafield("FT L_ArmL_FT"));
-// DatafieldRefPtr forceDfRight = DatafieldRefPtr::dynamicCast(getForceTorqueObserver()->getForceDatafield("FT R_ArmR_FT"));
-// Eigen::Vector3f initialForceLeft;
-// initialForceLeft.setZero();
-// Eigen::Vector3f initialForceRight;
-// initialForceRight.setZero();
-
-// usleep(100000);
-// // auto dd = getDebugDrawerTopic();
-
-
-//// //**** use obstacle avoidance component *****//
-//// ObstacleDetectionInterfacePrx obstacle_detection = getObstacleDetection();
-//// ObstacleAvoidanceInterfacePrx obstacle_avoidance = getObstacleAvoidance();
-//// // ObstacleAvoidanceInterfacePrx obstacle_avoidance = getObstacleAvoidance2D();
-//// // obstacleavoidance::Config obstacle_avoidance_cfg = obstacle_avoidance->getConfig();
-
-//// // clear_drawings(dd);
-
-// CycleUtil c(10);
-// // Eigen::Vector3f oldVisuPos = Eigen::Vector3f::Zero();
-// // int visuSphereCounter = 0;
-
-
-// /* ----------------- thumb feedback ------------------- */
-
-// DatafieldRefPtr thumbDFLeft = DatafieldRefPtr::dynamicCast(getKinematicUnitObserver()->getDatafieldRefByName("jointangles", "LeftHandThumb"));
-// DatafieldRefPtr thumbDFRight = DatafieldRefPtr::dynamicCast(getKinematicUnitObserver()->getDatafieldRefByName("jointangles", "RightHandThumb"));
-
-// IceUtil::Time start = TimeUtil::GetTime();
-// Eigen::Vector3f initialPositionRight;
-// Eigen::Vector3f initialPositionLeft;
-// Eigen::Vector3f initialDirection;
-// Eigen::Vector3f filteredPlatformTargetVelocity = Eigen::Vector3f::Zero();
-
-// Eigen::Vector3f dimensions;
-// dimensions << 805, 1205, 20;
-// bool isFirstLoop = true;
-// bool speechOutputAfterWait = true;
-// float minPosiTolerance = in.getMinPosiTolerance();
-// bool isImpedanceParameterReset = false;
-
-// obstacledetection::Obstacle obs3d;
-//// //bool isObs3d = false;
-//// if (in.isObs3DPositionSet())
-//// {
-//// obstacledetection::ObstacleList obstacleList = obstacle_detection->getObstacles();
-
-//// for (size_t i = 0; i < obstacleList.size(); ++i)
-//// {
-//// obstacledetection::Obstacle obs = obstacleList.at(i);
-//// if (obs.name == "obs3d")
-//// {
-//// ARMARX_INFO << "Removing obstacle 3d";
-//// obstacle_detection->removeObstacle("obs3d");
-//// obs3d = obs;
-//// //isObs3d = true;
-//// }
-//// }
-
-//// }
-
-// while (!isRunningTaskStopped())
-// {
-// RemoteRobot::synchronizeLocalClone(localRobot, getRobotStateComponent());
-
-// if (in.getIsConsiderFailure())
-// {
-// if (thumbDFLeft->getDataField()->getFloat() > in.getLeftThumbThreshold() || thumbDFRight->getDataField()->getFloat() > in.getRightThumbThreshold())
-// {
-// getPlatformUnit()->move(0, 0, 0);
-// emitLoseGuard();
-// return;
-// }
-// }
-// IceUtil::Time now = TimeUtil::GetTime();
-
-// /* Debug Drawer */
-// // if (in.isShowGuardSet() && in.getShowGuard())
-// // {
-// // std::string layerName = "guard_layer";
-// // std::string boxName = "guard";
-// // DrawColor color = {0.5, 0.5, 0.5, 1};
-// // }
-
-// /* Control Platform */
-// double timeDuration = (now - start).toSecondsDouble();
-// if (timeDuration < in.getMinExecutionTime() || isFirstLoop)
-// {
-// std::vector<float> currPoseLeft = Helpers::pose2vec(localRobot->getRobotNodeSet("LeftArm")->getTCP()->getPoseInRootFrame());
-// std::vector<float> currPoseRight = Helpers::pose2vec(localRobot->getRobotNodeSet("RightArm")->getTCP()->getPoseInRootFrame());
-
-// initialPositionRight << currPoseRight.at(0), currPoseRight.at(1), currPoseRight.at(2);
-// initialPositionLeft << currPoseLeft.at(0), currPoseLeft.at(1), currPoseLeft.at(2);
-// initialDirection = initialPositionRight - initialPositionLeft;
-// initialDirection = initialDirection / initialDirection.norm();
-
-// {
-// FramedDirectionPtr forcePtr = forceDfLeft->getDataField()->get<FramedDirection>();
-// initialForceLeft = forcePtr->toRootEigen(localRobot);
-// }
-// {
-// FramedDirectionPtr forcePtr = forceDfRight->getDataField()->get<FramedDirection>();
-// initialForceRight = forcePtr->toRootEigen(localRobot);
-// }
-
-// isFirstLoop = false;
-
-// continue;
-// }
-// if (speechOutputAfterWait)
-// {
-// getTextToSpeech()->reportText(in.getTextAfterWaitForZeroTorque());
-// speechOutputAfterWait = false;
-
-// const Eigen::Vector3f desiredPositionRightVec = desiredPoseRight.topRightCorner<3, 1>();
-// const Eigen::Vector3f desiredPositionLeftVec = desiredPoseLeft.topRightCorner<3, 1>();
-// const Eigen::Vector3f positionRightError = initialPositionRight - desiredPositionRightVec;
-// const Eigen::Vector3f positionLeftError = initialPositionLeft - desiredPositionLeftVec;
-// ARMARX_INFO << "differece Pose: " << positionLeftError.norm() << ", " << positionRightError.norm();
-// }
-
-// const Eigen::Vector3f currentPosition(
-// platformPosX->getDataField()->getFloat(),
-// platformPosY->getDataField()->getFloat(),
-// platformRotAng->getDataField()->getFloat()
-// );
-
-// ARMARX_DEBUG << "currentGuardPosition: " << currentPosition[0] << " ," << currentPosition[1];
-// bool platformInPolygon = in.getIsAutoDriveEnabled() && VirtualRobot::MathTools::isInside(currentPosition.head(2), convPolygon);
-// platformInPolygon = platformInPolygon && fabs(platformTargetPose(2) - currentPosition(2)) < in.getRestrictAngle() * M_PI / 180.0;
-
-// // RemoteRobot::synchronizeLocalClone(localRobot, getRobotStateComponent());
-// Eigen::Vector3f currentPositionRight = tcpRight->getPositionInRootFrame();
-// Eigen::Vector3f currentPositionLeft = tcpLeft->getPositionInRootFrame();
-
-// bool automaticDriveCondition = in.getIsAutoDriveEnabled() && ((targetLeft - currentPositionLeft).norm() < in.getHandOutofRangeThreshold() || (targetRight - currentPositionRight).norm() < in.getHandOutofRangeThreshold());
-// bool textReported = false;
-
-// // 3D obstacle avoidance
-
-//// bool isObs3DReached = false;
-//// if (in.isObs3DPositionSet())
-//// {
-//// Eigen::Vector3f currentGuardLocalPosi = (currentPositionLeft + currentPositionRight) / 2;
-//// currentGuardLocalPosi(1) += 0.6;
-//// Eigen::Vector3f currentGuardGlobalPosi = localRobot->toGlobalCoordinateSystemVec(currentGuardLocalPosi);
-//// currentGuardGlobalPosi(2) = 0;
-//// Eigen::Vector3f currentObsPosi = in.getObs3DPosition()->toEigen();
-
-//// float distToObs = (currentGuardGlobalPosi - currentObsPosi).norm();
-//// float heightOffset = 0;
-
-//// float distToReact = in.getObs3DDistToReact();
-
-//// if (distToObs < 0.5 * distToReact)
-//// {
-//// isObs3DReached = true;
-//// }
-//// else
-//// {
-//// isObs3DReached = false;
-//// }
-
-//// if (distToObs < distToReact)
-//// {
-
-//// float changeFactor = cos(distToObs / distToReact * M_PI / 2);
-//// heightOffset = in.getObs3DHeightAmplitude() * changeFactor;
-//// std::vector<float> currentKpos = Kpos;
-//// currentKpos[0] = Kpos[0] - in.getObs3DMaxiReducedStiffness()->x * changeFactor;
-//// // currentKpos[1] = Kpos[1] + in.getObs3DMaxiReducedStiffness()->y * changeFactor;
-
-//// leftController->setPosition(
-//// {
-//// desiredPoseLeft(0, 3),
-//// desiredPoseLeft(1, 3),
-//// desiredPoseLeft(2, 3) + heightOffset
-//// });
-//// rightController->setPosition(
-//// {
-//// desiredPoseRight(0, 3),
-//// desiredPoseRight(1, 3),
-//// desiredPoseRight(2, 3) + heightOffset
-//// });
-//// leftController->setImpedanceParameters("Kpos", currentKpos);
-//// rightController->setImpedanceParameters("Kpos", currentKpos);
-
-//// }
-//// else
-//// {
-//// leftController->setImpedanceParameters("Kpos", Kpos);
-//// rightController->setImpedanceParameters("Kpos", Kpos);
-//// }
-//// }
-
-
-// if (platformInPolygon && automaticDriveCondition)
-// {
-// if (!isImpedanceParameterReset)
-// {
-// if (!textReported)
-// {
-// getTextToSpeech()->reportText(in.getHandOutofRangeSpeechOutput());
-// textReported = true;
-// }
-
-// /// TODO this is not regarded anymore, may affect the demo
-//// leftController->setImpedanceParameters("Kpos", in.getRestrictKpos());
-//// leftController->setImpedanceParameters("Dpos", in.getRestrictDpos());
-//// rightController->setImpedanceParameters("Kpos", in.getRestrictKpos());
-//// rightController->setImpedanceParameters("Dpos", in.getRestrictDpos());
-// minPosiTolerance = in.getHandOutofRangeThreshold();
-// isImpedanceParameterReset = true;
-// }
-// float posx = platformPosX->getDataField()->getFloat();
-// float posy = platformPosY->getDataField()->getFloat();
-// float ori = platformRotAng->getDataField()->getFloat();
-
-// Eigen::Vector3f pos {posx, posy, 0.0f};
-
-// if (ori > M_PI)
-// {
-// ori = - 2 * M_PI + ori;
-// }
-
-// float angleDelta = platformTargetPose(2) - ori;
-// ARMARX_INFO << deactivateSpam(1) << VAROUT(angleDelta);
-
-// // transform alpha to [-pi, pi)
-// while (angleDelta < -M_PI)
-// {
-// angleDelta += 2 * M_PI;
-// }
-
-// while (angleDelta >= M_PI)
-// {
-// angleDelta -= 2 * M_PI;
-// }
-
-// Eigen::Vector3f target {platformTargetPose(0), platformTargetPose(1), 0.0f};
-// pidTrans.update(pos, target);
-// pidRot.update(angleDelta, 0);
-
-// Eigen::Vector2f newVel(pidTrans.getControlValue()[0], pidTrans.getControlValue()[1]);
-
-
-// float newVelocityRot = -signedMinFunction(pidRot.getControlValue(), in.getRestrictMaxAngularVelocity());
-// Eigen::Vector3f globalVel {newVel[0], newVel[1], 0};
-// Eigen::Matrix3f m;
-// m = Eigen::AngleAxisf(ori, Eigen::Vector3f::UnitZ());
-// Eigen::Vector3f localVel = m.inverse() * globalVel;
-// if (localVel.norm() > in.getRestrictMaxVelocity())
-// {
-// localVel *= in.getRestrictMaxVelocity() / localVel.norm();
-// }
-
-// if (std::isnan(localVel[0]) || std::isnan(localVel[1]) || std::isnan(newVelocityRot))
-// {
-// throw LocalException("A target velocity is NaN!");
-// }
-
-// getPlatformUnit()->move(localVel[0], localVel[1], newVelocityRot);
-
-
-// if (fabs(pidTrans.previousError) < in.getPositionalAccuracy() && fabs(pidRot.previousError) < in.getOrientationalAccuracy())
-// {
-// break;
-// }
-// else
-// {
-// ARMARX_INFO << deactivateSpam(1) << "error: " << pidTrans.previousError << " x velocity: " << newVel[0] << " y velocity: " << newVel[1];
-// ARMARX_INFO << deactivateSpam(1) << "rot error: " << pidRot.previousError << " rot velocity: " << newVelocityRot;
-// }
-
-// usleep(100000);
-// }
-// else
-// {
-// if (!platformInPolygon && isImpedanceParameterReset)
-// {
-//// leftController->setImpedanceParameters("Kpos", in.getKpos());
-//// leftController->setImpedanceParameters("Dpos", in.getDpos());
-//// rightController->setImpedanceParameters("Kpos", in.getKpos());
-//// rightController->setImpedanceParameters("Dpos", in.getDpos());
-// minPosiTolerance = in.getMinPosiTolerance();
-// isImpedanceParameterReset = false;
-// }
-
-// Eigen::Vector3f positionDeltaRight = currentPositionRight - initialPositionRight;
-// Eigen::Vector3f positionDeltaLeft = currentPositionLeft - initialPositionLeft;
-// Eigen::Vector3f positionDelta = (positionDeltaLeft + positionDeltaRight) / 2;
-// Eigen::Vector3f currentVel;
-// currentVel << platformVelX->getDataField()->getFloat(), platformVelY->getDataField()->getFloat(), 0;
-// Eigen::Vector3f velocity;
-// if (positionDelta.norm() < minPosiTolerance)
-// {
-// velocity.setZero();
-// }
-// else
-// {
-// positionDelta = positionDelta - minPosiTolerance * positionDelta / positionDelta.norm();
-// velocity = positionDelta * Kplatform - currentVel * Dplatform;
-// }
-
-
-// double deltaT = (now - last).toSecondsDouble();
-// last = now;
-
-// if (deltaT == 0)
-// {
-// continue;
-// }
-// // ARMARX_INFO << "deltaT: " << deltaT;
-// float Tstar = 1 / ((filterTimeConstant / deltaT) + 1);
-// filteredLinearVel = Tstar * (velocity - filteredLinearVel) + filteredLinearVel;
-
-// if (!std::isfinite(filteredLinearVel(0)))
-// {
-// filteredLinearVel(0) = 0;
-// }
-// if (!std::isfinite(filteredLinearVel(1)))
-// {
-// filteredLinearVel(1) = 0;
-// }
-
-// velocity = math::MathUtils::LimitTo(filteredLinearVel, maxVelocity); // in mm
-
-// // modulated velocity for obstacle avoidance
-
-// //const Eigen::Vector2f agent_pos = localRobot->getGlobalPosition().head<2>();
-
-// float ori = platformRotAng->getDataField()->getFloat();
-// if (ori > M_PI)
-// {
-// ori = - 2 * M_PI + ori;
-// }
-
-// Eigen::Matrix3f m;
-// m = Eigen::AngleAxisf(ori, Eigen::Vector3f::UnitZ());
-// Eigen::Vector3f globalVel = m * velocity;
-
-// obstacleavoidance::Agent agent;
-// agent.safety_margin = 0;
-// agent.pos = localRobot->getGlobalPosition();
-// agent.desired_vel = globalVel;
-// Eigen::Vector3f modulated_vel = obstacle_avoidance->modulateVelocity(agent);
-// modulated_vel[2] = 0;
-// Eigen::Vector3f localVel = m.inverse() * modulated_vel;
-// localVel[2] = 0;
-// if (localVel.norm() > in.getMaxVelocity())
-// {
-// localVel *= in.getMaxVelocity() / localVel.norm();
-// }
-
-// //
-// float angularVelocity ;
-
-// Eigen::Vector3f currentDirection = currentPositionRight - currentPositionLeft;
-// currentDirection = currentDirection / currentDirection.norm();
-
-// float cosAlpha = currentDirection.dot(initialDirection);
-
-// float alpha = 0;
-
-// Eigen::Vector3f directionDiff = currentDirection - initialDirection;
-// if (directionDiff(1) < 0)
-// {
-// alpha = -acos(cosAlpha);
-// }
-// else
-// {
-// alpha = acos(cosAlpha);
-// }
-
-
-// float minAngleTolerance = in.getMinAngleTolerance();
-// if (fabs(alpha) < minAngleTolerance)
-// {
-// angularVelocity = 0;
-// }
-// else
-// {
-// if (alpha > 0)
-// {
-// alpha -= minAngleTolerance;
-// }
-// else
-// {
-// alpha += minAngleTolerance;
-// }
-// angularVelocity = KplatformAngle * alpha - DplatformAngle * platformRotVel;
-// }
-
-// filteredAngularVel = Tstar * (angularVelocity - filteredAngularVel) + filteredAngularVel;
-// if (!std::isfinite(filteredAngularVel))
-// {
-// filteredAngularVel = 0;
-// }
-
-// angularVelocity = math::MathUtils::LimitTo(filteredAngularVel, maxAngularVelocity);
-
-// getDebugObserver()->setDebugChannel("FollowMeTorque",
-// {
-// { "vx", new Variant(velocity(0)) },
-// { "vy", new Variant(velocity(1)) },
-// { "vangle", new Variant(angularVelocity) },
-// });
-// localVel(2) = angularVelocity;
-
-// if (std::isnan(localVel[0]) || std::isnan(localVel[1]) || std::isnan(angularVelocity))
-// {
-// ARMARX_WARNING << deactivateSpam(1) << "Some velocity target is NaN - setting to zero";
-// localVel.setZero();
-// }
-
-// float ratio = 0.7;
-// filteredPlatformTargetVelocity = filteredPlatformTargetVelocity * ratio + (1.0 - ratio) * localVel;
-// float oriVel = filteredPlatformTargetVelocity(2);
-
-//// if (isObs3DReached)
-//// {
-//// oriVel = 0;
-//// }
-
-// getPlatformUnit()->move(filteredPlatformTargetVelocity(0), filteredPlatformTargetVelocity(1), oriVel);
-
-
-// // debug drawer ....
-// // {
-// // if ((oldVisuPos - localRobot->getGlobalPosition()).norm() > 50)
-// // {
-// // draw_agent_path(dd, agent_pos, visuSphereCounter++);
-// // oldVisuPos = localRobot->getGlobalPosition();
-// // }
-// // draw_agent_safety_margin(dd, agent_pos, ori, obstacle_avoidance_cfg.agent_safety_margin);
-// // draw_agent_desired_velocity(dd, agent_pos, globalVel.head(2));
-// // draw_agent_modulated_velocity(dd, agent_pos, modulated_vel.head(2));
-// // }
-
-// }
-
-// c.waitForCycleDuration();
-// }
-
- ARMARX_INFO << "running task has been requested to stop";
- skillMemory->abortSkillAsync(skillExecutionId);
+ ARMARX_DEBUG << "running task has been requested to stop";
+ if(skillExecutionId.has_value())
+ {
+ skillMemory->abortSkill(skillExecutionId.value());
+ skillExecutionId = std::nullopt;
+ }
emitSuccess();
return;
+
+ // const std::vector<float> Kpos = in.getKpos();
+ // const std::vector<float> Kori = in.getKori();
+ // const std::vector<float> Dpos = in.getDpos();
+ // const std::vector<float> Dori = in.getDori();
+
+
+
+ // std::vector<float> desiredJointPositionLeft;
+ // if (in.isDesiredLeftJointValuesSet())
+ // {
+ // desiredJointPositionLeft = in.getDesiredLeftJointValues();
+ // }
+ // else
+ // {
+ // desiredJointPositionLeft = localRobot->getRobotNodeSet(nodeSetNameLeft)->getJointValues();
+ // }
+
+ // std::vector<float> desiredJointPositionRight;
+ // if (in.isDesiredRightJointValuesSet())
+ // {
+ // desiredJointPositionRight = in.getDesiredRightJointValues();
+ // }
+ // else
+ // {
+ // desiredJointPositionRight = localRobot->getRobotNodeSet(nodeSetNameRight)->getJointValues();
+ // }
+
+ // std::vector<float> knull = in.getKnull();
+ // std::vector<float> dnull = in.getDnull();
+
+ // NJointTaskSpaceImpedanceControlConfigPtr leftConfig = new NJointTaskSpaceImpedanceControlConfig;
+ // leftConfig->nodeSetName = nodeSetNameLeft;
+ // leftConfig->desiredPosition = simox::math::mat4f_to_pos(desiredPoseLeft);
+ // leftConfig->desiredOrientation = simox::math::mat4f_to_quat(desiredPoseLeft);
+ // leftConfig->Kpos = Eigen::Map<const Eigen::Vector3f>(Kpos.data(), Kpos.size());
+ // leftConfig->Kori = Eigen::Map<const Eigen::Vector3f>(Kori.data(), Kori.size());
+ // leftConfig->Dpos = Eigen::Map<const Eigen::Vector3f>(Dpos.data(), Dpos.size());
+ // leftConfig->Dori = Eigen::Map<const Eigen::Vector3f>(Dori.data(), Dori.size());
+ // leftConfig->desiredJointPositions = Eigen::Map<const Eigen::VectorXf>(desiredJointPositionLeft.data(), desiredJointPositionLeft.size());
+ // leftConfig->Knull = Eigen::Map<const Eigen::VectorXf>(knull.data(), knull.size());
+ // leftConfig->Dnull = Eigen::Map<const Eigen::VectorXf>(dnull.data(), dnull.size());
+ // leftConfig->torqueLimit = in.getTorqueLimit();
+
+ // NJointTaskSpaceImpedanceControlConfigPtr rightConfig = new NJointTaskSpaceImpedanceControlConfig;
+ // rightConfig->nodeSetName = nodeSetNameRight;
+ // rightConfig->desiredPosition = simox::math::mat4f_to_pos(desiredPoseRight);
+ // rightConfig->desiredOrientation = simox::math::mat4f_to_quat(desiredPoseRight);
+ // rightConfig->Kpos = Eigen::Map<const Eigen::Vector3f>(Kpos.data(), Kpos.size());
+ // rightConfig->Kori = Eigen::Map<const Eigen::Vector3f>(Kori.data(), Kori.size());
+ // rightConfig->Dpos = Eigen::Map<const Eigen::Vector3f>(Dpos.data(), Dpos.size());
+ // rightConfig->Dori = Eigen::Map<const Eigen::Vector3f>(Dori.data(), Dori.size());
+ // rightConfig->desiredJointPositions = Eigen::Map<const Eigen::VectorXf>(desiredJointPositionRight.data(), desiredJointPositionRight.size());
+ // rightConfig->Knull = Eigen::Map<const Eigen::VectorXf>(knull.data(), knull.size());
+ // rightConfig->Dnull = Eigen::Map<const Eigen::VectorXf>(dnull.data(), dnull.size());
+ // rightConfig->torqueLimit = in.getTorqueLimit();
+
+ // NJointTaskSpaceImpedanceControlInterfacePrx leftController
+ // = NJointTaskSpaceImpedanceControlInterfacePrx::checkedCast(getRobotUnit()->createNJointController("NJointTaskSpaceImpedanceController", "LeftController", leftConfig));
+ // NJointTaskSpaceImpedanceControlInterfacePrx rightController
+ // = NJointTaskSpaceImpedanceControlInterfacePrx::checkedCast(getRobotUnit()->createNJointController("NJointTaskSpaceImpedanceController", "RightController", rightConfig));
+
+
+ // leftController->activateController();
+ // rightController->activateController();
+
+
+ /* ---------------- platform ------------------- */
+
+ // float Kplatform = in.getKplatform();
+ // float Dplatform = in.getDplatform();
+
+ // float maxVelocity = in.getMaxVelocity();
+
+ // float KplatformAngle = in.getKplatformAngle();
+ // float DplatformAngle = in.getDplatformAngle();
+ // float maxAngularVelocity = in.getMaxAngularVelocity();
+
+ // Eigen::Vector3f filteredLinearVel;
+ // filteredLinearVel.setZero();
+ // float filteredAngularVel = 0;
+
+ // currentTCPLinearVelocity_filtered = (1 - filterFactor) * currentTCPLinearVelocity_filtered + filterFactor * currentTCPLinearVelocity;
+
+ // IceUtil::Time last = TimeUtil::GetTime();
+ // float filterTimeConstant = 0.01; //0.05;
+
+ // MultiDimPIDController pidTrans(in.getRestrictTranslationPID()->x, in.getRestrictTranslationPID()->y, in.getRestrictTranslationPID()->z);
+ // PIDController pidRot(in.getRestrictRotationPID()->x, in.getRestrictRotationPID()->y, in.getRestrictRotationPID()->z);
+
+ // DatafieldRefPtr platformVelX = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformVelocity", "velocityX"));
+ // DatafieldRefPtr platformVelY = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformVelocity", "velocityY"));
+ // DatafieldRefPtr platformRotVel = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformVelocity", "velocityRotation"));
+
+ // /// -------------------------------- get platform position/orientation ---------------------
+ // DatafieldRefPtr platformPosX = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformPose", "positionX"));
+ // DatafieldRefPtr platformPosY = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformPose", "positionY"));
+ // DatafieldRefPtr platformRotAng = DatafieldRefPtr::dynamicCast(getPlatformUnitObserver()->getDatafieldRefByName("platformPose", "rotation"));
+
+ // DatafieldRefPtr forceDfLeft = DatafieldRefPtr::dynamicCast(getForceTorqueObserver()->getForceDatafield("FT L_ArmL_FT"));
+ // DatafieldRefPtr forceDfRight = DatafieldRefPtr::dynamicCast(getForceTorqueObserver()->getForceDatafield("FT R_ArmR_FT"));
+ // Eigen::Vector3f initialForceLeft;
+ // initialForceLeft.setZero();
+ // Eigen::Vector3f initialForceRight;
+ // initialForceRight.setZero();
+
+ // usleep(100000);
+ // auto dd = getDebugDrawerTopic();
+
+
+ // //**** use obstacle avoidance component *****//
+ // ObstacleDetectionInterfacePrx obstacle_detection = getObstacleDetection();
+ // ObstacleAvoidanceInterfacePrx obstacle_avoidance = getObstacleAvoidance();
+ // // ObstacleAvoidanceInterfacePrx obstacle_avoidance = getObstacleAvoidance2D();
+ // // obstacleavoidance::Config obstacle_avoidance_cfg = obstacle_avoidance->getConfig();
+
+ // // clear_drawings(dd);
+
+ // CycleUtil c(5);
+ // Eigen::Vector3f oldVisuPos = Eigen::Vector3f::Zero();
+ // int visuSphereCounter = 0;
+
+
+ // /// -------------------------------- thumb feedback ----------------------------------------
+ // DatafieldRefPtr thumbDFLeft = DatafieldRefPtr::dynamicCast(getKinematicUnitObserver()->getDatafieldRefByName("jointangles", "LeftHandThumb"));
+ // DatafieldRefPtr thumbDFRight = DatafieldRefPtr::dynamicCast(getKinematicUnitObserver()->getDatafieldRefByName("jointangles", "RightHandThumb"));
+
+ // IceUtil::Time start = TimeUtil::GetTime();
+ // Eigen::Vector3f initialPositionRight;
+ // Eigen::Vector3f initialPositionLeft;
+ // Eigen::Vector3f initialDirection;
+ // Eigen::Vector3f filteredPlatformTargetVelocity = Eigen::Vector3f::Zero();
+
+ // Eigen::Vector3f dimensions;
+ // dimensions << 805, 1205, 20;
+ // bool isFirstLoop = true;
+ // bool speechOutputAfterWait = true;
+ // float minPosiTolerance = in.getMinPosiTolerance();
+ // bool isImpedanceParameterReset = false;
+
+ // obstacledetection::Obstacle obs3d;
+ // //bool isObs3d = false;
+ // if (in.isObs3DPositionSet())
+ // {
+ // obstacledetection::ObstacleList obstacleList = obstacle_detection->getObstacles();
+
+ // for (size_t i = 0; i < obstacleList.size(); ++i)
+ // {
+ // obstacledetection::Obstacle obs = obstacleList.at(i);
+ // if (obs.name == "obs3d")
+ // {
+ // ARMARX_INFO << "Removing obstacle 3d";
+ // obstacle_detection->removeObstacle("obs3d");
+ // obs3d = obs;
+ // //isObs3d = true;
+ // }
+ // }
+
+ // }
+
+ // while (!isRunningTaskStopped())
+ // {
+ // RemoteRobot::synchronizeLocalClone(localRobot, getRobotStateComponent());
+
+ // if (in.getIsConsiderFailure())
+ // {
+ // if (thumbDFLeft->getDataField()->getFloat() > in.getLeftThumbThreshold() || thumbDFRight->getDataField()->getFloat() > in.getRightThumbThreshold())
+ // {
+ // getPlatformUnit()->move(0, 0, 0);
+ // emitLoseGuard();
+ // return;
+ // }
+ // }
+ // // IceUtil::Time now = TimeUtil::GetTime();
+
+ // /* Debug Drawer */
+ // // if (in.isShowGuardSet() && in.getShowGuard())
+ // // {
+ // // std::string layerName = "guard_layer";
+ // // std::string boxName = "guard";
+ // // DrawColor color = {0.5, 0.5, 0.5, 1};
+ // // }
+
+ // /* Control Platform */
+ // // double timeDuration = (now - start).toSecondsDouble();
+ // // if (timeDuration < in.getMinExecutionTime() || isFirstLoop)
+ // // {
+ // // // std::vector<float> currPoseLeft = Helpers::pose2vec(localRobot->getRobotNodeSet("LeftArm")->getTCP()->getPoseInRootFrame());
+ // // // std::vector<float> currPoseRight = Helpers::pose2vec(localRobot->getRobotNodeSet("RightArm")->getTCP()->getPoseInRootFrame());
+
+ // // // initialPositionRight << currPoseRight.at(0), currPoseRight.at(1), currPoseRight.at(2);
+ // // // initialPositionLeft << currPoseLeft.at(0), currPoseLeft.at(1), currPoseLeft.at(2);
+ // // // initialDirection = initialPositionRight - initialPositionLeft;
+ // // // initialDirection = initialDirection / initialDirection.norm();
+
+ // // // {
+ // // // FramedDirectionPtr forcePtr = forceDfLeft->getDataField()->get<FramedDirection>();
+ // // // initialForceLeft = forcePtr->toRootEigen(localRobot);
+ // // // }
+ // // // {
+ // // // FramedDirectionPtr forcePtr = forceDfRight->getDataField()->get<FramedDirection>();
+ // // // initialForceRight = forcePtr->toRootEigen(localRobot);
+ // // // }
+
+ // // isFirstLoop = false;
+
+ // // continue;
+ // // }
+ // if (speechOutputAfterWait)
+ // {
+ // getTextToSpeech()->reportText(in.getTextAfterWaitForZeroTorque());
+ // speechOutputAfterWait = false;
+
+ // // const Eigen::Vector3f desiredPositionRightVec = desiredPoseRight.topRightCorner<3, 1>();
+ // // const Eigen::Vector3f desiredPositionLeftVec = desiredPoseLeft.topRightCorner<3, 1>();
+ // // const Eigen::Vector3f positionRightError = initialPositionRight - desiredPositionRightVec;
+ // // const Eigen::Vector3f positionLeftError = initialPositionLeft - desiredPositionLeftVec;
+ // // ARMARX_INFO << "differece Pose: " << positionLeftError.norm() << ", " << positionRightError.norm();
+ // }
+
+ // const Eigen::Vector3f currentPosition(
+ // platformPosX->getDataField()->getFloat(),
+ // platformPosY->getDataField()->getFloat(),
+ // platformRotAng->getDataField()->getFloat()
+ // );
+
+ // ARMARX_DEBUG << "currentGuardPosition: " << currentPosition[0] << " ," << currentPosition[1];
+ // bool platformInPolygon = in.getIsAutoDriveEnabled() && VirtualRobot::MathTools::isInside(currentPosition.head(2), convPolygon);
+ // platformInPolygon = platformInPolygon && fabs(platformTargetPose(2) - currentPosition(2)) < in.getRestrictAngle() * M_PI / 180.0;
+
+ // // RemoteRobot::synchronizeLocalClone(localRobot, getRobotStateComponent());
+ // Eigen::Vector3f currentPositionRight = tcpRight->getPositionInRootFrame();
+ // Eigen::Vector3f currentPositionLeft = tcpLeft->getPositionInRootFrame();
+
+ // bool automaticDriveCondition = in.getIsAutoDriveEnabled() && ((targetLeft - currentPositionLeft).norm() < in.getHandOutofRangeThreshold() || (targetRight - currentPositionRight).norm() < in.getHandOutofRangeThreshold());
+ // bool textReported = false;
+
+ // // 3D obstacle avoidance
+
+ // // bool isObs3DReached = false;
+ // // if (in.isObs3DPositionSet())
+ // // {
+ // // Eigen::Vector3f currentGuardLocalPosi = (currentPositionLeft + currentPositionRight) / 2;
+ // // currentGuardLocalPosi(1) += 0.6;
+ // // Eigen::Vector3f currentGuardGlobalPosi = localRobot->toGlobalCoordinateSystemVec(currentGuardLocalPosi);
+ // // currentGuardGlobalPosi(2) = 0;
+ // // Eigen::Vector3f currentObsPosi = in.getObs3DPosition()->toEigen();
+
+ // // float distToObs = (currentGuardGlobalPosi - currentObsPosi).norm();
+ // // float heightOffset = 0;
+
+ // // float distToReact = in.getObs3DDistToReact();
+
+ // // if (distToObs < 0.5 * distToReact)
+ // // {
+ // // isObs3DReached = true;
+ // // }
+ // // else
+ // // {
+ // // isObs3DReached = false;
+ // // }
+
+ // // if (distToObs < distToReact)
+ // // {
+
+ // // float changeFactor = cos(distToObs / distToReact * M_PI / 2);
+ // // heightOffset = in.getObs3DHeightAmplitude() * changeFactor;
+ // // std::vector<float> currentKpos = Kpos;
+ // // currentKpos[0] = Kpos[0] - in.getObs3DMaxiReducedStiffness()->x * changeFactor;
+ // // // currentKpos[1] = Kpos[1] + in.getObs3DMaxiReducedStiffness()->y * changeFactor;
+
+ // // leftController->setPosition(
+ // // {
+ // // desiredPoseLeft(0, 3),
+ // // desiredPoseLeft(1, 3),
+ // // desiredPoseLeft(2, 3) + heightOffset
+ // // });
+ // // rightController->setPosition(
+ // // {
+ // // desiredPoseRight(0, 3),
+ // // desiredPoseRight(1, 3),
+ // // desiredPoseRight(2, 3) + heightOffset
+ // // });
+ // // leftController->setImpedanceParameters("Kpos", currentKpos);
+ // // rightController->setImpedanceParameters("Kpos", currentKpos);
+
+ // // }
+ // // else
+ // // {
+ // // leftController->setImpedanceParameters("Kpos", Kpos);
+ // // rightController->setImpedanceParameters("Kpos", Kpos);
+ // // }
+ // // }
+
+
+ // if (platformInPolygon && automaticDriveCondition)
+ // {
+ // // if (!isImpedanceParameterReset)
+ // // {
+ // if (!textReported)
+ // {
+ // getTextToSpeech()->reportText(in.getHandOutofRangeSpeechOutput());
+ // textReported = true;
+ // }
+
+ // /// TODO here we execute the navigation skill synchroneously
+ // /// exit when succeeded
+ // emitSuccess();
+ // return;
+
+ // /// TODO this is not regarded anymore, may affect the demo
+ // // leftController->setImpedanceParameters("Kpos", in.getRestrictKpos());
+ // // leftController->setImpedanceParameters("Dpos", in.getRestrictDpos());
+ // // rightController->setImpedanceParameters("Kpos", in.getRestrictKpos());
+ // // rightController->setImpedanceParameters("Dpos", in.getRestrictDpos());
+ // // minPosiTolerance = in.getHandOutofRangeThreshold();
+ // // isImpedanceParameterReset = true;
+ // // }
+ // // float posx = platformPosX->getDataField()->getFloat();
+ // // float posy = platformPosY->getDataField()->getFloat();
+ // // float ori = platformRotAng->getDataField()->getFloat();
+
+ // // Eigen::Vector3f pos {posx, posy, 0.0f};
+
+ // // if (ori > M_PI)
+ // // {
+ // // ori = - 2 * M_PI + ori;
+ // // }
+
+ // // float angleDelta = platformTargetPose(2) - ori;
+ // // ARMARX_INFO << deactivateSpam(1) << VAROUT(angleDelta);
+
+ // // // transform alpha to [-pi, pi)
+ // // while (angleDelta < -M_PI)
+ // // {
+ // // angleDelta += 2 * M_PI;
+ // // }
+
+ // // while (angleDelta >= M_PI)
+ // // {
+ // // angleDelta -= 2 * M_PI;
+ // // }
+
+ // // Eigen::Vector3f target {platformTargetPose(0), platformTargetPose(1), 0.0f};
+ // // pidTrans.update(pos, target);
+ // // pidRot.update(angleDelta, 0);
+
+ // // Eigen::Vector2f newVel(pidTrans.getControlValue()[0], pidTrans.getControlValue()[1]);
+
+
+ // // float newVelocityRot = -signedMinFunction(pidRot.getControlValue(), in.getRestrictMaxAngularVelocity());
+ // // Eigen::Vector3f globalVel {newVel[0], newVel[1], 0};
+ // // Eigen::Matrix3f m;
+ // // m = Eigen::AngleAxisf(ori, Eigen::Vector3f::UnitZ());
+ // // Eigen::Vector3f localVel = m.inverse() * globalVel;
+ // // if (localVel.norm() > in.getRestrictMaxVelocity())
+ // // {
+ // // localVel *= in.getRestrictMaxVelocity() / localVel.norm();
+ // // }
+
+ // // if (std::isnan(localVel[0]) || std::isnan(localVel[1]) || std::isnan(newVelocityRot))
+ // // {
+ // // throw LocalException("A target velocity is NaN!");
+ // // }
+
+ // // getPlatformUnit()->move(localVel[0], localVel[1], newVelocityRot);
+
+
+ // // if (fabs(pidTrans.previousError) < in.getPositionalAccuracy() && fabs(pidRot.previousError) < in.getOrientationalAccuracy())
+ // // {
+ // // break;
+ // // }
+ // // else
+ // // {
+ // // ARMARX_INFO << deactivateSpam(1) << "error: " << pidTrans.previousError << " x velocity: " << newVel[0] << " y velocity: " << newVel[1];
+ // // ARMARX_INFO << deactivateSpam(1) << "rot error: " << pidRot.previousError << " rot velocity: " << newVelocityRot;
+ // // }
+
+ // // usleep(100000);
+ // }
+ // else
+ // {
+ // // if (!platformInPolygon && isImpedanceParameterReset)
+ // // {
+ // // // leftController->setImpedanceParameters("Kpos", in.getKpos());
+ // // // leftController->setImpedanceParameters("Dpos", in.getDpos());
+ // // // rightController->setImpedanceParameters("Kpos", in.getKpos());
+ // // // rightController->setImpedanceParameters("Dpos", in.getDpos());
+ // // // minPosiTolerance = in.getMinPosiTolerance();
+ // // isImpedanceParameterReset = false;
+ // // }
+
+ // // Eigen::Vector3f positionDeltaRight = currentPositionRight - initialPositionRight;
+ // // Eigen::Vector3f positionDeltaLeft = currentPositionLeft - initialPositionLeft;
+ // // Eigen::Vector3f positionDelta = (positionDeltaLeft + positionDeltaRight) / 2;
+ // // Eigen::Vector3f currentVel;
+ // // currentVel << platformVelX->getDataField()->getFloat(), platformVelY->getDataField()->getFloat(), 0;
+ // // Eigen::Vector3f velocity;
+ // // if (positionDelta.norm() < minPosiTolerance)
+ // // {
+ // // velocity.setZero();
+ // // }
+ // // else
+ // // {
+ // // positionDelta = positionDelta - minPosiTolerance * positionDelta / positionDelta.norm();
+ // // velocity = positionDelta * Kplatform - currentVel * Dplatform;
+ // // }
+
+
+ // // double deltaT = (now - last).toSecondsDouble();
+ // // last = now;
+
+ // // if (deltaT == 0)
+ // // {
+ // // continue;
+ // // }
+ // // // ARMARX_INFO << "deltaT: " << deltaT;
+ // // float Tstar = 1 / ((filterTimeConstant / deltaT) + 1);
+ // // filteredLinearVel = Tstar * (velocity - filteredLinearVel) + filteredLinearVel;
+
+ // // if (!std::isfinite(filteredLinearVel(0)))
+ // // {
+ // // filteredLinearVel(0) = 0;
+ // // }
+ // // if (!std::isfinite(filteredLinearVel(1)))
+ // // {
+ // // filteredLinearVel(1) = 0;
+ // // }
+
+ // // velocity = math::MathUtils::LimitTo(filteredLinearVel, maxVelocity); // in mm
+
+ // // modulated velocity for obstacle avoidance
+
+ // //const Eigen::Vector2f agent_pos = localRobot->getGlobalPosition().head<2>();
+
+ // // float ori = platformRotAng->getDataField()->getFloat();
+ // // if (ori > M_PI)
+ // // {
+ // // ori = - 2 * M_PI + ori;
+ // // }
+
+ // // Eigen::Matrix3f m;
+ // // m = Eigen::AngleAxisf(ori, Eigen::Vector3f::UnitZ());
+ // // Eigen::Vector3f globalVel = m * velocity;
+
+ // // obstacleavoidance::Agent agent;
+ // // agent.safety_margin = 0;
+ // // agent.pos = localRobot->getGlobalPosition();
+ // // agent.desired_vel = globalVel;
+ // // Eigen::Vector3f modulated_vel = obstacle_avoidance->modulateVelocity(agent);
+ // // modulated_vel[2] = 0;
+ // // Eigen::Vector3f localVel = m.inverse() * modulated_vel;
+ // // localVel[2] = 0;
+ // // if (localVel.norm() > in.getMaxVelocity())
+ // // {
+ // // localVel *= in.getMaxVelocity() / localVel.norm();
+ // // }
+
+ // // //
+ // // float angularVelocity ;
+
+ // // Eigen::Vector3f currentDirection = currentPositionRight - currentPositionLeft;
+ // // currentDirection = currentDirection / currentDirection.norm();
+
+ // // float cosAlpha = currentDirection.dot(initialDirection);
+
+ // // float alpha = 0;
+
+ // // Eigen::Vector3f directionDiff = currentDirection - initialDirection;
+ // // if (directionDiff(1) < 0)
+ // // {
+ // // alpha = -acos(cosAlpha);
+ // // }
+ // // else
+ // // {
+ // // alpha = acos(cosAlpha);
+ // // }
+
+
+ // // float minAngleTolerance = in.getMinAngleTolerance();
+ // // if (fabs(alpha) < minAngleTolerance)
+ // // {
+ // // angularVelocity = 0;
+ // // }
+ // // else
+ // // {
+ // // if (alpha > 0)
+ // // {
+ // // alpha -= minAngleTolerance;
+ // // }
+ // // else
+ // // {
+ // // alpha += minAngleTolerance;
+ // // }
+ // // angularVelocity = KplatformAngle * alpha - DplatformAngle * platformRotVel;
+ // // }
+
+ // // filteredAngularVel = Tstar * (angularVelocity - filteredAngularVel) + filteredAngularVel;
+ // // if (!std::isfinite(filteredAngularVel))
+ // // {
+ // // filteredAngularVel = 0;
+ // // }
+
+ // // angularVelocity = math::MathUtils::LimitTo(filteredAngularVel, maxAngularVelocity);
+
+ // // getDebugObserver()->setDebugChannel("FollowMeTorque",
+ // // {
+ // // { "vx", new Variant(velocity(0)) },
+ // // { "vy", new Variant(velocity(1)) },
+ // // { "vangle", new Variant(angularVelocity) },
+ // // });
+ // // localVel(2) = angularVelocity;
+
+ // // if (std::isnan(localVel[0]) || std::isnan(localVel[1]) || std::isnan(angularVelocity))
+ // // {
+ // // ARMARX_WARNING << deactivateSpam(1) << "Some velocity target is NaN - setting to zero";
+ // // localVel.setZero();
+ // // }
+
+ // // float ratio = 0.7;
+ // // filteredPlatformTargetVelocity = filteredPlatformTargetVelocity * ratio + (1.0 - ratio) * localVel;
+ // // float oriVel = filteredPlatformTargetVelocity(2);
+
+ // // if (isObs3DReached)
+ // // {
+ // // oriVel = 0;
+ // // }
+
+ // // getPlatformUnit()->move(filteredPlatformTargetVelocity(0), filteredPlatformTargetVelocity(1), oriVel);
+
+
+ // // debug drawer ....
+ // // {
+ // // if ((oldVisuPos - localRobot->getGlobalPosition()).norm() > 50)
+ // // {
+ // // draw_agent_path(dd, agent_pos, visuSphereCounter++);
+ // // oldVisuPos = localRobot->getGlobalPosition();
+ // // }
+ // // draw_agent_safety_margin(dd, agent_pos, ori, obstacle_avoidance_cfg.agent_safety_margin);
+ // // draw_agent_desired_velocity(dd, agent_pos, globalVel.head(2));
+ // // draw_agent_modulated_velocity(dd, agent_pos, modulated_vel.head(2));
+ // // }
+
+ // }
+
+ // c.waitForCycleDuration();
+ // }
+
+ // ARMARX_INFO << "running task has been requested to stop";
+ // skillMemory->abortSkillAsync(skillExecutionId);
+ // emitSuccess();
+ // return;
}
//void GuardCarryingSkill::onBreak()
diff --git a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.xml b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.xml
index 4232348da..d7b65f3f0 100644
--- a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.xml
+++ b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.xml
@@ -1,167 +1,62 @@
<?xml version="1.0" encoding="utf-8"?>
<State version="1.2" name="GuardCarryingSkill" uuid="30629B08-477C-4EC0-8150-549910532B42" width="800" height="391.529" type="Normal State">
- <InputParameters>
- <Parameter name="DesiredLeftJointValues" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="yes">
- <DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.037332}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.303279}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.250224}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.510265}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1.826300}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":-0.171072}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.355409}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.220867}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="0.0373319983482361\n0.303279012441635\n0.250223994255066\n0.510264992713928\n1.82630002498627\n-0.171072006225586\n0.355408996343613\n0.220866993069649"/>
- </Parameter>
- <Parameter name="DesiredPoseLeft" type="::armarx::PoseBase" docType="Pose" optional="yes"/>
- <Parameter name="DesiredPoseRight" type="::armarx::PoseBase" docType="Pose" optional="yes"/>
- <Parameter name="DesiredRightJointValues" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="yes">
- <DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":-0.014552}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":-0.222375}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.269340}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":-0.464008}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1.874799}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.169847}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":-0.419656}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.031448}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="-0.0145519999787211\n-0.222375005483627\n0.269340008497238\n-0.464008003473282\n1.87479901313782\n0.169846996665001\n-0.419656008481979\n0.0314479991793633"/>
- </Parameter>
- <Parameter name="Dnull" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="no">
- <DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="0\n0\n0\n0\n0\n0\n0\n0"/>
- </Parameter>
- <Parameter name="Dori" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="no">
- <DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":50}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":50}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":50}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="50\n50\n50"/>
- </Parameter>
- <Parameter name="Dplatform" type="::armarx::FloatVariantData" docType="float" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}}' docValue="0"/>
- </Parameter>
- <Parameter name="DplatformAngle" type="::armarx::FloatVariantData" docType="float" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}}' docValue="0"/>
- </Parameter>
- <Parameter name="Dpos" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="no">
- <DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":5}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":5}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":20}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="5\n5\n20"/>
- </Parameter>
- <Parameter name="HandOutofRangeSpeechOutput" type="::armarx::StringVariantData" docType="string" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Please follow me."}}' docValue="Please follow me."/>
- </Parameter>
- <Parameter name="HandOutofRangeThreshold" type="::armarx::FloatVariantData" docType="float" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":200}}' docValue="200"/>
- </Parameter>
- <Parameter name="IsAutoDriveEnabled" type="::armarx::BoolVariantData" docType="bool" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":true}}' docValue="True"/>
- </Parameter>
- <Parameter name="IsConsiderFailure" type="::armarx::BoolVariantData" docType="bool" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":false}}' docValue="False"/>
- </Parameter>
- <Parameter name="IsGuardPlacement" type="::armarx::BoolVariantData" docType="bool" optional="no">
- <DefaultValue profile="Armar6Real" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":false}}' docValue="False"/>
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":true}}' docValue="True"/>
- </Parameter>
- <Parameter name="IsLocalModulationEnabled" type="::armarx::BoolVariantData" docType="bool" optional="no">
- <DefaultValue profile="Armar6Real" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":true}}' docValue="True"/>
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":false}}' docValue="False"/>
- </Parameter>
- <Parameter name="Knull" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="no">
- <DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":5}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":5}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":5}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":5}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":5}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="5\n0\n5\n5\n1\n5\n5\n0"/>
- </Parameter>
- <Parameter name="Kori" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="no">
- <DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":200}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":200}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":200}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="200\n200\n200"/>
- </Parameter>
- <Parameter name="Kplatform" type="::armarx::FloatVariantData" docType="float" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":12}}' docValue="12"/>
- </Parameter>
- <Parameter name="KplatformAngle" type="::armarx::FloatVariantData" docType="float" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":2}}' docValue="2"/>
- </Parameter>
- <Parameter name="Kpos" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="no">
- <DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":50}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":50}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1500}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="50\n50\n1500"/>
- </Parameter>
- <Parameter name="LeftThumbThreshold" type="::armarx::FloatVariantData" docType="float" optional="yes"/>
- <Parameter name="MaxAngularVelocity" type="::armarx::FloatVariantData" docType="float" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1}}' docValue="1"/>
- </Parameter>
- <Parameter name="MaxVelocity" type="::armarx::FloatVariantData" docType="float" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":500}}' docValue="500"/>
- </Parameter>
- <Parameter name="MinAngleTolerance" type="::armarx::FloatVariantData" docType="float" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.05}}' docValue="0.0500000007450581"/>
- </Parameter>
- <Parameter name="MinExecutionTime" type="::armarx::FloatVariantData" docType="float" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":3}}' docValue="3"/>
- </Parameter>
- <Parameter name="MinPosiTolerance" type="::armarx::FloatVariantData" docType="float" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":30}}' docValue="30"/>
- </Parameter>
- <Parameter name="Obs3DDistToReact" type="::armarx::FloatVariantData" docType="float" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1500}}' docValue="1500"/>
- </Parameter>
- <Parameter name="Obs3DHeightAmplitude" type="::armarx::FloatVariantData" docType="float" optional="no">
- <DefaultValue profile="Armar6Real" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":250}}' docValue="250"/>
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":300}}' docValue="300"/>
- </Parameter>
- <Parameter name="Obs3DMaxiReducedStiffness" type="::armarx::Vector3Base" docType="Vector3" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::Vector3Base","value":{"x":70,"y":0,"z":0}}}' docValue="70\n0\n0"/>
- </Parameter>
- <Parameter name="Obs3DPosition" type="::armarx::Vector3Base" docType="Vector3" optional="yes"/>
- <Parameter name="OrientationalAccuracy" type="::armarx::FloatVariantData" docType="float" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.1}}' docValue="0.100000001490116"/>
- </Parameter>
- <Parameter name="PlatformTargetPose" type="::armarx::StringValueMapBase(::armarx::Vector3Base)" docType="Map(Vector3)" optional="no"/>
- <Parameter name="PositionalAccuracy" type="::armarx::FloatVariantData" docType="float" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":30}}' docValue="30"/>
- </Parameter>
- <Parameter name="ReGraspingForceThreshold" type="::armarx::FloatVariantData" docType="float" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":20}}' docValue="20"/>
- </Parameter>
- <Parameter name="ReactionTime" type="::armarx::FloatVariantData" docType="float" optional="no">
- <DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.75}}' docValue="0.75"/>
- <DefaultValue profile="Armar6Real" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.25}}' docValue="0.25"/>
- </Parameter>
- <Parameter name="RestrictAngle" type="::armarx::FloatVariantData" docType="float" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":45}}' docValue="45"/>
- </Parameter>
- <Parameter name="RestrictDpos" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="no">
- <DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":20}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="1\n1\n20"/>
- </Parameter>
- <Parameter name="RestrictKpos" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="no">
- <DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":20}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":20}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1500}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="20\n20\n1500"/>
- </Parameter>
- <Parameter name="RestrictMaxAngularVelocity" type="::armarx::FloatVariantData" docType="float" optional="no">
- <DefaultValue profile="Armar6Real" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.2}}' docValue="0.200000002980232"/>
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.3}}' docValue="0.300000011920929"/>
- </Parameter>
- <Parameter name="RestrictMaxVelocity" type="::armarx::FloatVariantData" docType="float" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":300}}' docValue="300"/>
- </Parameter>
- <Parameter name="RestrictPolygon" type="::armarx::StringValueMapBase(::armarx::MatrixFloatBase)" docType="Map(MatrixFloat)" optional="no">
- <DefaultValue value='{"map":{"GuardPlacement":{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::MatrixFloatBase","value":{"cols":2,"rowContent":["-400, -700","400, -700","400, 700","-400, 700"],"rows":4}}},"GuardLifting":{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::MatrixFloatBase","value":{"cols":2,"rowContent":["-150.0, -350","150.0, -350","150, 350","-150, 350"],"rows":4}}}},"type":"::armarx::StringValueMapBase"}' docValue="GuardLifting: -150 -350\n 150 -350\n 150 350\n-150 350\nGuardPlacement: -400 -700\n 400 -700\n 400 700\n-400 700\n"/>
- </Parameter>
- <Parameter name="RestrictRotationPID" type="::armarx::Vector3Base" docType="Vector3" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::Vector3Base","value":{"x":1.0,"y":0.00009,"z":0.0}}}' docValue="1\n9e-05\n0"/>
- </Parameter>
- <Parameter name="RestrictTranslationPID" type="::armarx::Vector3Base" docType="Vector3" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::Vector3Base","value":{"x":2.0,"y":0.0004,"z":0.0}}}' docValue="2\n0.0004\n0"/>
- </Parameter>
- <Parameter name="RightThumbThreshold" type="::armarx::FloatVariantData" docType="float" optional="yes"/>
- <Parameter name="RobotSafetyMargin" type="::armarx::DoubleVariantData" docType="double" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::DoubleVariantData","value":0.6}}' docValue="0.6"/>
- </Parameter>
- <Parameter name="SafetyMargin" type="::armarx::DoubleVariantData" docType="double" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::DoubleVariantData","value":0}}' docValue="0"/>
- </Parameter>
- <Parameter name="ShowGuard" type="::armarx::BoolVariantData" docType="bool" optional="yes"/>
- <Parameter name="TextAfterWaitForZeroTorque" type="::armarx::StringVariantData" docType="string" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Okay."}}' docValue="Okay."/>
- </Parameter>
- <Parameter name="TextBeforeThreeSeconds" type="::armarx::StringVariantData" docType="string" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Please wait a moment"}}' docValue="Please wait a moment"/>
- </Parameter>
- <Parameter name="TextRegraspingLeft" type="::armarx::StringVariantData" docType="string" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Let me get a better grasp to help you carry the board."}}' docValue="Let me get a better grasp to help you carry the board."/>
- </Parameter>
- <Parameter name="TextRegraspingRight" type="::armarx::StringVariantData" docType="string" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Let me get a better grasp to help you carry the board."}}' docValue="Let me get a better grasp to help you carry the board."/>
- </Parameter>
- <Parameter name="TorqueLimit" type="::armarx::FloatVariantData" docType="float" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":20}}' docValue="20"/>
- </Parameter>
- </InputParameters>
- <OutputParameters>
- <Parameter name="DesiredPoseLeft" type="::armarx::PoseBase" docType="Pose" optional="yes"/>
- <Parameter name="DesiredPoseRight" type="::armarx::PoseBase" docType="Pose" optional="yes"/>
- </OutputParameters>
+ <InputParameters>
+ <Parameter name="DesiredPoseLeft" type="::armarx::PoseBase" docType="Pose" optional="yes"/>
+ <Parameter name="DesiredPoseRight" type="::armarx::PoseBase" docType="Pose" optional="yes"/>
+ <Parameter name="HandOutofRangeSpeechOutput" type="::armarx::StringVariantData" docType="string" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Please follow me."}}' docValue="Please follow me."/>
+ </Parameter>
+ <Parameter name="HandOutofRangeThreshold" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":200}}' docValue="200"/>
+ </Parameter>
+ <Parameter name="IsAutoDriveEnabled" type="::armarx::BoolVariantData" docType="bool" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":true}}' docValue="True"/>
+ </Parameter>
+ <Parameter name="IsConsiderFailure" type="::armarx::BoolVariantData" docType="bool" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":true}}' docValue="True"/>
+ </Parameter>
+ <Parameter name="IsGuardPlacement" type="::armarx::BoolVariantData" docType="bool" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":false}}' docValue="False"/>
+ </Parameter>
+ <Parameter name="LeftThumbThreshold" type="::armarx::FloatVariantData" docType="float" optional="yes">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.8}}' docValue="0.800000011920929"/>
+ </Parameter>
+ <Parameter name="PlatformTargetPose" type="::armarx::StringValueMapBase(::armarx::Vector3Base)" docType="Map(Vector3)" optional="no"/>
+ <Parameter name="RestrictAngle" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":45}}' docValue="45"/>
+ </Parameter>
+ <Parameter name="RestrictMaxAngularVelocity" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.2}}' docValue="0.200000002980232"/>
+ </Parameter>
+ <Parameter name="RestrictMaxVelocity" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":300}}' docValue="300"/>
+ </Parameter>
+ <Parameter name="RestrictPolygon" type="::armarx::StringValueMapBase(::armarx::MatrixFloatBase)" docType="Map(MatrixFloat)" optional="no">
+ <DefaultValue value='{"map":{"GuardPlacement":{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::MatrixFloatBase","value":{"cols":2,"rowContent":["-400, -700","400, -700","400, 700","-400, 700"],"rows":4}}},"GuardLifting":{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::MatrixFloatBase","value":{"cols":2,"rowContent":["-150.0, -350","150.0, -350","150, 350","-150, 350"],"rows":4}}}},"type":"::armarx::StringValueMapBase"}' docValue="GuardLifting: -150 -350\n 150 -350\n 150 350\n-150 350\nGuardPlacement: -400 -700\n 400 -700\n 400 700\n-400 700\n"/>
+ </Parameter>
+ <Parameter name="RightThumbThreshold" type="::armarx::FloatVariantData" docType="float" optional="yes">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.8}}' docValue="0.800000011920929"/>
+ </Parameter>
+ <Parameter name="TextAfterWaitForZeroTorque" type="::armarx::StringVariantData" docType="string" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Okay."}}' docValue="Okay."/>
+ </Parameter>
+ <Parameter name="TextBeforeThreeSeconds" type="::armarx::StringVariantData" docType="string" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Please wait a moment"}}' docValue="Please wait a moment"/>
+ </Parameter>
+ </InputParameters>
+ <OutputParameters>
+ <Parameter name="DesiredPoseLeft" type="::armarx::PoseBase" docType="Pose" optional="yes"/>
+ <Parameter name="DesiredPoseRight" type="::armarx::PoseBase" docType="Pose" optional="yes"/>
+ </OutputParameters>
<LocalParameters/>
<Substates/>
- <Events>
- <Event name="Failure">
- <Description>Event for statechart-internal failures or optionally user-code failures</Description>
- </Event>
- <Event name="Success"/>
- <Event name="LoseGuard"/>
- </Events>
+ <Events>
+ <Event name="Failure">
+ <Description>Event for statechart-internal failures or optionally user-code failures</Description>
+ </Event>
+ <Event name="Success"/>
+ <Event name="LoseGuard"/>
+ </Events>
<Transitions/>
</State>
--
GitLab
From 1737357909eec5e4cbc5f46f01365b79d3cf292c Mon Sep 17 00:00:00 2001
From: Jianfeng Gao <jianfeng.gao@kit.edu>
Date: Tue, 30 Jul 2024 11:53:12 +0200
Subject: [PATCH 04/22] add testing statechart
---
.../Armar6BoardSupportDemo.scgxml | 1 +
.../testGuardCarryingSkill.xml | 103 ++++++++++++++++++
2 files changed, 104 insertions(+)
create mode 100644 source/armar6/skills/statecharts/Armar6BoardSupportDemo/testGuardCarryingSkill.xml
diff --git a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/Armar6BoardSupportDemo.scgxml b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/Armar6BoardSupportDemo.scgxml
index e4f468012..e148ed215 100644
--- a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/Armar6BoardSupportDemo.scgxml
+++ b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/Armar6BoardSupportDemo.scgxml
@@ -164,6 +164,7 @@ ArmarX.Armar6BoardSupportDemoRemoteStateOfferer.OpenPoseEstimationName = OpenPos
<State filename="RemoveGuardV2.xml" visibility="public"/>
<State filename="RotateBoard.xml"/>
<State filename="TestBimanualCCDMP.xml"/>
+ <State filename="testGuardCarryingSkill.xml" visibility="public"/>
<State filename="TestGuardDemo.xml"/>
<State filename="TestGuardLocalization.xml"/>
<State filename="TestRegrasping.xml"/>
diff --git a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/testGuardCarryingSkill.xml b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/testGuardCarryingSkill.xml
new file mode 100644
index 000000000..9b015dc28
--- /dev/null
+++ b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/testGuardCarryingSkill.xml
@@ -0,0 +1,103 @@
+<?xml version="1.0" encoding="utf-8"?>
+<State version="1.2" name="testGuardCarryingSkill" uuid="17DE22DF-94E0-46F7-AC80-FDFE3EF80DAC" width="781.944" height="371.889" type="Normal State">
+ <InputParameters/>
+ <OutputParameters/>
+ <LocalParameters/>
+ <Substates>
+ <EndState name="Failure" event="Failure" left="652.111" top="140" boundingSquareSize="99.6636"/>
+ <LocalState name="GuardCarryingSkill" refuuid="30629B08-477C-4EC0-8150-549910532B42" left="363.333" top="238.028" boundingSquareSize="99.6636"/>
+ <RemoteState name="ResolveLandmark" refuuid="68599B94-C644-4023-969B-486210BEFD4A" proxyName="PlatformGroupRemoteStateOfferer" left="114.167" top="157.583" boundingSquareSize="99.6636"/>
+ <EndState name="Success" event="Success" left="652.111" top="266.194" boundingSquareSize="99.6636"/>
+ </Substates>
+ <Events>
+ <Event name="Failure">
+ <Description>Event for statechart-internal failures or optionally user-code failures</Description>
+ </Event>
+ </Events>
+ <StartState substateName="ResolveLandmark">
+ <ParameterMappings>
+ <ParameterMapping sourceType="Value" from="" to="LandmarkNames">
+ <DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"GuardPlacement"}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"GuardLifting"}}],"type":"::armarx::SingleTypeVariantListBase"}'/>
+ </ParameterMapping>
+ <ParameterMapping sourceType="Value" from="" to="SceneName">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"50_19/R005"}}'/>
+ </ParameterMapping>
+ </ParameterMappings>
+ <SupportPoints>
+ <SupportPoint posX="67.1067" posY="195.583"/>
+ <SupportPoint posX="76.5839" posY="195.583"/>
+ <SupportPoint posX="88.7241" posY="195.583"/>
+ <SupportPoint posX="101.186" posY="195.583"/>
+ </SupportPoints>
+ </StartState>
+ <Transitions>
+ <Transition from="ResolveLandmark" to="Failure" eventName="Failure">
+ <ParameterMappings/>
+ <ParameterMappingsToParentsLocal/>
+ <ParameterMappingsToParentsOutput/>
+ <SupportPoints>
+ <SupportPoint posX="214.268" posY="189.757"/>
+ <SupportPoint posX="221.871" posY="189.028"/>
+ <SupportPoint posX="229.64" posY="188.389"/>
+ <SupportPoint posX="237" posY="187.917"/>
+ <SupportPoint posX="380.558" posY="178.793"/>
+ <SupportPoint posX="549.493" posY="177.426"/>
+ <SupportPoint posX="639.014" posY="177.426"/>
+ </SupportPoints>
+ </Transition>
+ <Transition from="ResolveLandmark" to="GuardCarryingSkill" eventName="Success">
+ <ParameterMappings>
+ <ParameterMapping sourceType="Value" from="" to="IsGuardPlacement">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":true}}'/>
+ </ParameterMapping>
+ <ParameterMapping sourceType="Output" from="LandmarkPoseMap" to="PlatformTargetPose"/>
+ </ParameterMappings>
+ <ParameterMappingsToParentsLocal/>
+ <ParameterMappingsToParentsOutput/>
+ <SupportPoints>
+ <SupportPoint posX="214.153" posY="219.221"/>
+ <SupportPoint posX="221.718" posY="222.29"/>
+ <SupportPoint posX="229.487" posY="225.178"/>
+ <SupportPoint posX="237" posY="227.528"/>
+ <SupportPoint posX="273.8" posY="239.042"/>
+ <SupportPoint posX="316.158" posY="247.479"/>
+ <SupportPoint posX="350.147" posY="253.128"/>
+ </SupportPoints>
+ </Transition>
+ <Transition from="GuardCarryingSkill" to="Failure" eventName="Failure">
+ <ParameterMappings/>
+ <ParameterMappingsToParentsLocal/>
+ <ParameterMappingsToParentsOutput/>
+ <SupportPoints>
+ <SupportPoint posX="463.435" posY="260.24"/>
+ <SupportPoint posX="508.285" posY="257.208"/>
+ <SupportPoint posX="575.138" posY="249.103"/>
+ <SupportPoint posX="629.278" posY="227.528"/>
+ <SupportPoint posX="633.392" posY="225.89"/>
+ <SupportPoint posX="637.494" posY="223.987"/>
+ <SupportPoint posX="641.557" posY="221.902"/>
+ </SupportPoints>
+ </Transition>
+ <Transition from="GuardCarryingSkill" to="Success" eventName="Success">
+ <ParameterMappings/>
+ <ParameterMappingsToParentsLocal/>
+ <ParameterMappingsToParentsOutput/>
+ <SupportPoints/>
+ </Transition>
+ <Transition from="GuardCarryingSkill" to="Success" eventName="LoseGuard">
+ <ParameterMappings/>
+ <ParameterMappingsToParentsLocal/>
+ <ParameterMappingsToParentsOutput/>
+ <SupportPoints>
+ <SupportPoint posX="444.524" posY="286.404"/>
+ <SupportPoint posX="456.791" posY="294.543"/>
+ <SupportPoint posX="471.459" posY="302.595"/>
+ <SupportPoint posX="486.167" posY="306.75"/>
+ <SupportPoint posX="536.192" posY="320.884"/>
+ <SupportPoint posX="595.378" posY="318.958"/>
+ <SupportPoint posX="639.129" posY="314.221"/>
+ </SupportPoints>
+ </Transition>
+ </Transitions>
+</State>
+
--
GitLab
From 2caa07c191ec01d0111d4ccb61bef594cdedfc53 Mon Sep 17 00:00:00 2001
From: Jianfeng Gao <jianfeng.gao@kit.edu>
Date: Tue, 30 Jul 2024 12:52:51 +0200
Subject: [PATCH 05/22] update default param
---
.../Armar6BoardSupportDemo/GuardCarryingSkill.cpp | 13 ++++++++++++-
.../Armar6BoardSupportDemo/GuardCarryingSkill.xml | 2 ++
2 files changed, 14 insertions(+), 1 deletion(-)
diff --git a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.cpp b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.cpp
index 60bb05799..20d2decfc 100644
--- a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.cpp
+++ b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.cpp
@@ -226,6 +226,7 @@ namespace armarx::Armar6BoardSupportDemo
{
polygon = restrictPolygonMap.at("GuardLifting")->toEigen();
platformTargetPose = platFormTargetMap.at("GuardLifting")->toEigen();
+ ARMARX_INFO << "platform target pose is:\n" << platformTargetPose;
}
std::vector<Eigen::Vector2f> points;
@@ -241,6 +242,7 @@ namespace armarx::Armar6BoardSupportDemo
for (const Eigen::Vector2f& point : points)
{
goalPolygon.addPoint(Eigen::Vector3f(point.x(), point.y(), +1));
+ ARMARX_INFO << "polygon point: " << point.x() << ", " << point.y();
}
goalPolygon.lineColor(simox::Color::green()).lineWidth(5);
goalPolygon.color(simox::Color::green(255, 64));
@@ -329,6 +331,7 @@ namespace armarx::Armar6BoardSupportDemo
ARMARX_DEBUG << "Control skill has been invoked; starting loop";
armarx::Metronome metronome(armarx::Frequency::Hertz(30));
+ ARMARX_INFO << "isRunningTaskStopped before first loop cycle: " << isRunningTaskStopped();
while (!isRunningTaskStopped()) // stop run function if returning true
{
RemoteRobot::synchronizeLocalClone(localRobot, getRobotStateComponent());
@@ -340,8 +343,14 @@ namespace armarx::Armar6BoardSupportDemo
thumbDFRight->getDataField()->getFloat() > in.getRightThumbThreshold()
)
{
+ ARMARX_INFO<< "Ups, please wait a moment";
// Thumb is closed too far, i.e., there is no guard in the hand anymore;
// transition to error recovery state
+ if(skillExecutionId.has_value())
+ {
+ skillMemory->abortSkill(skillExecutionId.value());
+ skillExecutionId = std::nullopt;
+ }
getPlatformUnit()->move(0, 0, 0);
emitLoseGuard();
return;
@@ -378,6 +387,7 @@ namespace armarx::Armar6BoardSupportDemo
if (platformInPolygon && automaticDriveCondition)
{
+ ARMARX_INFO << "platform now is in polygon";
if (!enterPolygonTextReported)
{
getTextToSpeech()->reportText(in.getHandOutofRangeSpeechOutput());
@@ -401,6 +411,7 @@ namespace armarx::Armar6BoardSupportDemo
skillMemory->abortSkill(skillExecutionId.value());
skillExecutionId = std::nullopt;
}
+ ARMARX_INFO << "leaving state after having stopped everything, with success";
emitSuccess();
return;
@@ -567,7 +578,7 @@ namespace armarx::Armar6BoardSupportDemo
// {
// RemoteRobot::synchronizeLocalClone(localRobot, getRobotStateComponent());
- // if (in.getIsConsiderFailure())
+ // if (in.\n())
// {
// if (thumbDFLeft->getDataField()->getFloat() > in.getLeftThumbThreshold() || thumbDFRight->getDataField()->getFloat() > in.getRightThumbThreshold())
// {
diff --git a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.xml b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.xml
index d7b65f3f0..6df90c189 100644
--- a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.xml
+++ b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.xml
@@ -19,6 +19,7 @@
<DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":false}}' docValue="False"/>
</Parameter>
<Parameter name="LeftThumbThreshold" type="::armarx::FloatVariantData" docType="float" optional="yes">
+ <DefaultValue profile="Armar6Real" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1.1}}' docValue="1.10000002384186"/>
<DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.8}}' docValue="0.800000011920929"/>
</Parameter>
<Parameter name="PlatformTargetPose" type="::armarx::StringValueMapBase(::armarx::Vector3Base)" docType="Map(Vector3)" optional="no"/>
@@ -35,6 +36,7 @@
<DefaultValue value='{"map":{"GuardPlacement":{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::MatrixFloatBase","value":{"cols":2,"rowContent":["-400, -700","400, -700","400, 700","-400, 700"],"rows":4}}},"GuardLifting":{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::MatrixFloatBase","value":{"cols":2,"rowContent":["-150.0, -350","150.0, -350","150, 350","-150, 350"],"rows":4}}}},"type":"::armarx::StringValueMapBase"}' docValue="GuardLifting: -150 -350\n 150 -350\n 150 350\n-150 350\nGuardPlacement: -400 -700\n 400 -700\n 400 700\n-400 700\n"/>
</Parameter>
<Parameter name="RightThumbThreshold" type="::armarx::FloatVariantData" docType="float" optional="yes">
+ <DefaultValue profile="Armar6Real" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1.1}}' docValue="1.10000002384186"/>
<DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.8}}' docValue="0.800000011920929"/>
</Parameter>
<Parameter name="TextAfterWaitForZeroTorque" type="::armarx::StringVariantData" docType="string" optional="no">
--
GitLab
From 0b76f7d22a047decb82c3782912ac0e55d646ffe Mon Sep 17 00:00:00 2001
From: Jianfeng Gao <jianfeng.gao@kit.edu>
Date: Tue, 30 Jul 2024 12:53:10 +0200
Subject: [PATCH 06/22] minor change in layout
---
.../GuardCarryingWithFailureDetection.xml | 200 ++++++++++++------
1 file changed, 133 insertions(+), 67 deletions(-)
diff --git a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingWithFailureDetection.xml b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingWithFailureDetection.xml
index b9367c5e6..38c7ae303 100644
--- a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingWithFailureDetection.xml
+++ b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingWithFailureDetection.xml
@@ -1,5 +1,5 @@
<?xml version="1.0" encoding="utf-8"?>
-<State version="1.2" name="GuardCarryingWithFailureDetection" uuid="4F66B65D-FC23-4C4A-A2F7-3F1DAA4540F5" width="825.833" height="450" type="Normal State">
+<State version="1.2" name="GuardCarryingWithFailureDetection" uuid="4F66B65D-FC23-4C4A-A2F7-3F1DAA4540F5" width="1073.28" height="663.861" type="Normal State">
<InputParameters>
<Parameter name="CloseLeftHandOri" type="::armarx::Vector3Base" docType="Vector3" optional="no">
<DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::Vector3Base","value":{"x":0.0,"y":-1.0,"z":0.0}}}' docValue="0\n-1\n0"/>
@@ -24,8 +24,7 @@
<DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"GuardPlacement"}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"GuardLifting"}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="GuardPlacement\nGuardLifting"/>
</Parameter>
<Parameter name="LandmarkSceneName" type="::armarx::StringVariantData" docType="string" optional="no">
- <DefaultValue profile="Armar6Real" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"50_19/R005"}}' docValue="50_19/R005"/>
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"GraphCeBIT"}}' docValue="GraphCeBIT"/>
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"50_19/R005"}}' docValue="50_19/R005"/>
</Parameter>
<Parameter name="LeftHandAdjVec" type="::armarx::Vector3Base" docType="Vector3" optional="no">
<DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::Vector3Base","value":{"x":0.0,"y":-150.0,"z":-50.0}}}' docValue="0\n-150\n-50"/>
@@ -45,11 +44,12 @@
<Parameter name="PlatformTargetPose" type="::armarx::StringValueMapBase(::armarx::Vector3Base)" docType="Map(Vector3)" optional="no"/>
</LocalParameters>
<Substates>
- <EndState name="Failure" event="Failure" left="695.831" top="159.5" boundingSquareSize="100.002"/>
- <LocalState name="GuardCarryingWithObstacleAvoidance" refuuid="9D06CC13-22D7-4C19-ACBE-A50B628CA63C" left="282.5" top="199.5" boundingSquareSize="100.002"/>
- <LocalState name="RegraspingFailureDetection" refuuid="90F3C6E6-08AA-4924-A787-953A1062DE6A" left="505.833" top="245.333" boundingSquareSize="100.002"/>
- <RemoteState name="ResolveLandmark" refuuid="68599B94-C644-4023-969B-486210BEFD4A" proxyName="PlatformGroupRemoteStateOfferer" left="85" top="143.667" boundingSquareSize="100.002"/>
- <EndState name="Success" event="Success" left="505.833" top="344.5" boundingSquareSize="100.002"/>
+ <EndState name="Failure" event="Failure" left="943.444" top="420.167" boundingSquareSize="99.6636"/>
+ <LocalState name="GuardCarryingSkill" refuuid="30629B08-477C-4EC0-8150-549910532B42" left="387.611" top="495.5" boundingSquareSize="99.6636"/>
+ <LocalState name="GuardCarryingWithObstacleAvoidance" refuuid="9D06CC13-22D7-4C19-ACBE-A50B628CA63C" left="363.611" top="188.889" boundingSquareSize="148.221"/>
+ <LocalState name="RegraspingFailureDetection" refuuid="90F3C6E6-08AA-4924-A787-953A1062DE6A" left="701.222" top="250.222" boundingSquareSize="104.779"/>
+ <RemoteState name="ResolveLandmark" refuuid="68599B94-C644-4023-969B-486210BEFD4A" proxyName="PlatformGroupRemoteStateOfferer" left="114.167" top="558.167" boundingSquareSize="99.6636"/>
+ <EndState name="Success" event="Success" left="703.222" top="374.167" boundingSquareSize="99.6636"/>
</Substates>
<Events>
<Event name="Failure">
@@ -62,10 +62,10 @@
<ParameterMapping sourceType="Parent" from="LandmarkSceneName" to="SceneName"/>
</ParameterMappings>
<SupportPoints>
- <SupportPoint posX="54.2308" posY="181.667"/>
- <SupportPoint posX="60.2808" posY="181.667"/>
- <SupportPoint posX="68.0858" posY="181.667"/>
- <SupportPoint posX="76.4533" posY="181.667"/>
+ <SupportPoint posX="67.1067" posY="596.167"/>
+ <SupportPoint posX="76.5839" posY="596.167"/>
+ <SupportPoint posX="88.7241" posY="596.167"/>
+ <SupportPoint posX="101.186" posY="596.167"/>
</SupportPoints>
</StartState>
<Transitions>
@@ -74,13 +74,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="605.883" posY="268.575"/>
- <SupportPoint posX="629.175" posY="260.975"/>
- <SupportPoint posX="656.95" posY="250.842"/>
- <SupportPoint posX="680.833" posY="239.167"/>
- <SupportPoint posX="683.3" posY="237.958"/>
- <SupportPoint posX="685.792" posY="236.675"/>
- <SupportPoint posX="688.283" posY="235.342"/>
+ <SupportPoint posX="805.943" posY="325.521"/>
+ <SupportPoint posX="843.306" posY="351.958"/>
+ <SupportPoint posX="893.548" posY="387.493"/>
+ <SupportPoint posX="932.609" posY="415.131"/>
</SupportPoints>
</Transition>
<Transition from="RegraspingFailureDetection" to="GuardCarryingWithObstacleAvoidance" eventName="Success">
@@ -98,13 +95,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="505.525" posY="295.342"/>
- <SupportPoint posX="473.667" posY="300.633"/>
- <SupportPoint posX="432.092" posY="303.208"/>
- <SupportPoint posX="397.5" posY="290.833"/>
- <SupportPoint posX="390.233" posY="288.233"/>
- <SupportPoint posX="383.192" posY="284.433"/>
- <SupportPoint posX="376.6" posY="280.033"/>
+ <SupportPoint posX="714.684" posY="250.515"/>
+ <SupportPoint posX="703.632" posY="241.954"/>
+ <SupportPoint posX="690.994" posY="234.044"/>
+ <SupportPoint posX="677.833" posY="229.444"/>
+ <SupportPoint posX="629.252" posY="212.45"/>
+ <SupportPoint posX="571.688" posY="211.249"/>
+ <SupportPoint posX="525.088" posY="214.558"/>
</SupportPoints>
</Transition>
<Transition from="GuardCarryingWithObstacleAvoidance" to="Failure" eventName="Failure">
@@ -112,13 +109,19 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="382.65" posY="222.217"/>
- <SupportPoint posX="387.633" posY="221.042"/>
- <SupportPoint posX="392.642" posY="219.992"/>
- <SupportPoint posX="397.5" posY="219.167"/>
- <SupportPoint posX="498.275" posY="202.025"/>
- <SupportPoint posX="617.517" posY="198.067"/>
- <SupportPoint posX="687.108" posY="197.358"/>
+ <SupportPoint posX="489.744" posY="189.194"/>
+ <SupportPoint posX="503.698" posY="181.055"/>
+ <SupportPoint posX="519.223" posY="173.631"/>
+ <SupportPoint posX="534.722" posY="169.389"/>
+ <SupportPoint posX="596.068" posY="152.586"/>
+ <SupportPoint posX="615.874" posY="155.027"/>
+ <SupportPoint posX="677.833" posY="169.389"/>
+ <SupportPoint posX="797.139" posY="197.053"/>
+ <SupportPoint posX="838.514" posY="208.847"/>
+ <SupportPoint posX="920.611" posY="299.722"/>
+ <SupportPoint posX="948.594" posY="330.696"/>
+ <SupportPoint posX="967.569" posY="374.268"/>
+ <SupportPoint posX="979.108" posY="407.886"/>
</SupportPoints>
</Transition>
<Transition from="GuardCarryingWithObstacleAvoidance" to="Success" eventName="Success">
@@ -126,13 +129,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="351.725" posY="275.108"/>
- <SupportPoint posX="362.875" posY="293.658"/>
- <SupportPoint posX="378.483" posY="315.025"/>
- <SupportPoint posX="397.5" posY="329.167"/>
- <SupportPoint posX="426.775" posY="350.935"/>
- <SupportPoint posX="465.492" posY="364.187"/>
- <SupportPoint posX="497.317" posY="372.037"/>
+ <SupportPoint posX="463.959" posY="265.069"/>
+ <SupportPoint posX="481.643" posY="288.222"/>
+ <SupportPoint posX="506.752" posY="316.806"/>
+ <SupportPoint posX="534.722" posY="335.5"/>
+ <SupportPoint posX="548.177" posY="344.496"/>
+ <SupportPoint posX="631.169" posY="372.517"/>
+ <SupportPoint posX="690.892" posY="392.067"/>
</SupportPoints>
</Transition>
<Transition from="GuardCarryingWithObstacleAvoidance" to="RegraspingFailureDetection" eventName="LoseGuard">
@@ -157,13 +160,13 @@
</ParameterMappingsToParentsLocal>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="382.683" posY="244.858"/>
- <SupportPoint posX="413.95" posY="249.783"/>
- <SupportPoint posX="454.925" posY="256.842"/>
- <SupportPoint posX="490.833" posY="265"/>
- <SupportPoint posX="493.008" posY="265.492"/>
- <SupportPoint posX="495.225" posY="266.017"/>
- <SupportPoint posX="497.45" posY="266.558"/>
+ <SupportPoint posX="507.454" posY="264.801"/>
+ <SupportPoint posX="516.424" posY="268.506"/>
+ <SupportPoint posX="525.663" posY="271.764"/>
+ <SupportPoint posX="534.722" posY="274.167"/>
+ <SupportPoint posX="584.837" posY="287.43"/>
+ <SupportPoint posX="643.576" posY="290.548"/>
+ <SupportPoint posX="687.519" posY="290.573"/>
</SupportPoints>
</Transition>
<Transition from="ResolveLandmark" to="Failure" eventName="Failure">
@@ -171,25 +174,20 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="185.317" posY="173.892"/>
- <SupportPoint posX="190.267" posY="173.325"/>
- <SupportPoint posX="195.225" posY="172.85"/>
- <SupportPoint posX="200" posY="172.5"/>
- <SupportPoint posX="379.892" posY="159.383"/>
- <SupportPoint posX="425.792" posY="159.875"/>
- <SupportPoint posX="605.833" posY="170.833"/>
- <SupportPoint posX="639.35" posY="172.875"/>
- <SupportPoint posX="647.875" posY="173.583"/>
- <SupportPoint posX="680.833" posY="180"/>
- <SupportPoint posX="683.017" posY="180.425"/>
- <SupportPoint posX="685.233" posY="180.883"/>
- <SupportPoint posX="687.467" posY="181.367"/>
+ <SupportPoint posX="214.128" posY="600.699"/>
+ <SupportPoint posX="306.383" posY="608.013"/>
+ <SupportPoint posX="509.435" posY="619.171"/>
+ <SupportPoint posX="677.833" posY="593.611"/>
+ <SupportPoint posX="789.371" posY="576.682"/>
+ <SupportPoint posX="821.507" posY="573.398"/>
+ <SupportPoint posX="920.611" posY="519.5"/>
+ <SupportPoint posX="928.699" posY="515.102"/>
+ <SupportPoint posX="936.711" posY="509.715"/>
+ <SupportPoint posX="944.301" posY="503.975"/>
</SupportPoints>
</Transition>
- <Transition from="ResolveLandmark" to="GuardCarryingWithObstacleAvoidance" eventName="Success">
+ <Transition from="ResolveLandmark" to="GuardCarryingSkill" eventName="Success">
<ParameterMappings>
- <ParameterMapping sourceType="Parent" from="NullspaceAngleLeft" to="DesiredLeftJointValues"/>
- <ParameterMapping sourceType="Parent" from="NullspaceAngleRight" to="DesiredRightJointValues"/>
<ParameterMapping sourceType="Parent" from="IsConsiderFailure" to="IsConsiderFailure"/>
<ParameterMapping sourceType="Parent" from="IsGuardPlacement" to="IsGuardPlacement"/>
<ParameterMapping sourceType="Parent" from="LeftThumbThreshold" to="LeftThumbThreshold"/>
@@ -201,10 +199,78 @@
</ParameterMappingsToParentsLocal>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="185.092" posY="195.825"/>
- <SupportPoint posX="212.242" posY="203.5"/>
- <SupportPoint posX="245.925" posY="213.025"/>
- <SupportPoint posX="274.242" posY="221.033"/>
+ <SupportPoint posX="214.319" posY="582.105"/>
+ <SupportPoint posX="259.617" posY="569.406"/>
+ <SupportPoint posX="326.432" posY="550.671"/>
+ <SupportPoint posX="375.166" posY="537.007"/>
+ </SupportPoints>
+ </Transition>
+ <Transition from="GuardCarryingSkill" to="Failure" eventName="Failure">
+ <ParameterMappings/>
+ <ParameterMappingsToParentsLocal/>
+ <ParameterMappingsToParentsOutput/>
+ <SupportPoints>
+ <SupportPoint posX="487.623" posY="533.688"/>
+ <SupportPoint posX="502.637" posY="537.311"/>
+ <SupportPoint posX="519.236" posY="540.691"/>
+ <SupportPoint posX="534.722" posY="542.5"/>
+ <SupportPoint posX="597.896" posY="549.879"/>
+ <SupportPoint posX="614.775" posY="550.78"/>
+ <SupportPoint posX="677.833" posY="542.5"/>
+ <SupportPoint posX="767.533" posY="530.723"/>
+ <SupportPoint posX="867.979" posY="500.883"/>
+ <SupportPoint posX="930.961" posY="480.055"/>
+ </SupportPoints>
+ </Transition>
+ <Transition from="GuardCarryingSkill" to="Success" eventName="Success">
+ <ParameterMappings/>
+ <ParameterMappingsToParentsLocal/>
+ <ParameterMappingsToParentsOutput/>
+ <SupportPoints>
+ <SupportPoint posX="487.598" posY="528.243"/>
+ <SupportPoint posX="538.453" posY="534.619"/>
+ <SupportPoint posX="618.046" posY="537.499"/>
+ <SupportPoint posX="677.833" posY="506.722"/>
+ <SupportPoint posX="688.72" posY="501.113"/>
+ <SupportPoint posX="705.344" posY="480.988"/>
+ <SupportPoint posX="720.204" posY="460.722"/>
+ </SupportPoints>
+ </Transition>
+ <Transition from="GuardCarryingSkill" to="RegraspingFailureDetection" eventName="LoseGuard">
+ <ParameterMappings>
+ <ParameterMapping sourceType="Parent" from="CloseLeftHandOri" to="CloseLeftHandOri"/>
+ <ParameterMapping sourceType="Parent" from="CloseRightHandOri" to="CloseRightHandOri"/>
+ <ParameterMapping sourceType="Parent" from="NullspaceAngleLeft" to="DesiredLeftJointValues"/>
+ <ParameterMapping sourceType="Parent" from="NullspaceAngleRight" to="DesiredRightJointValues"/>
+ <ParameterMapping sourceType="Parent" from="HandReturnOffsetVecLeft" to="HandReturnOffsetVecLeft"/>
+ <ParameterMapping sourceType="Parent" from="HandReturnOffsetVecRight" to="HandReturnOffsetVecRight"/>
+ <ParameterMapping sourceType="Parent" from="LeftHandAdjVec" to="LeftHandAdjVec"/>
+ <ParameterMapping sourceType="Parent" from="LeftThumbThreshold" to="LeftThumbThreshold"/>
+ <ParameterMapping sourceType="Value" from="" to="ObjHandRelativePoseName">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"handforward"}}'/>
+ </ParameterMapping>
+ <ParameterMapping sourceType="Parent" from="RightHandAdjVec" to="RightHandAdjVec"/>
+ <ParameterMapping sourceType="Parent" from="RightThumbThreshold" to="RightThumbThreshold"/>
+ </ParameterMappings>
+ <ParameterMappingsToParentsLocal>
+ <ParameterMapping sourceType="Output" from="DesiredPoseLeft" to="DesiredPoseLeft"/>
+ <ParameterMapping sourceType="Output" from="DesiredPoseRight" to="DesiredPoseRight"/>
+ </ParameterMappingsToParentsLocal>
+ <ParameterMappingsToParentsOutput/>
+ <SupportPoints>
+ <SupportPoint posX="477.133" posY="495.171"/>
+ <SupportPoint posX="494.344" posY="485.383"/>
+ <SupportPoint posX="515.044" posY="474.65"/>
+ <SupportPoint posX="534.722" posY="467.111"/>
+ <SupportPoint posX="595.826" posY="443.715"/>
+ <SupportPoint posX="630.313" posY="477.589"/>
+ <SupportPoint posX="677.833" posY="432.611"/>
+ <SupportPoint posX="703.67" posY="408.154"/>
+ <SupportPoint posX="684.567" posY="387.582"/>
+ <SupportPoint posX="700.833" posY="355.944"/>
+ <SupportPoint posX="704.232" posY="349.338"/>
+ <SupportPoint posX="708.296" posY="342.732"/>
+ <SupportPoint posX="712.64" posY="336.382"/>
</SupportPoints>
</Transition>
</Transitions>
--
GitLab
From d2c301808e2ffc1c63b8d980699c8bac3b9e75d8 Mon Sep 17 00:00:00 2001
From: Jianfeng Gao <jianfeng.gao@kit.edu>
Date: Tue, 30 Jul 2024 12:53:29 +0200
Subject: [PATCH 07/22] update foto task param
---
.../Armar6HighlevelTaskGroup/FotoTask.xml | 141 +++++++++++++-----
1 file changed, 105 insertions(+), 36 deletions(-)
diff --git a/source/armar6/skills/statecharts/Armar6HighlevelTaskGroup/FotoTask.xml b/source/armar6/skills/statecharts/Armar6HighlevelTaskGroup/FotoTask.xml
index 911f0bbf1..efc39fbe2 100644
--- a/source/armar6/skills/statecharts/Armar6HighlevelTaskGroup/FotoTask.xml
+++ b/source/armar6/skills/statecharts/Armar6HighlevelTaskGroup/FotoTask.xml
@@ -1,5 +1,5 @@
<?xml version="1.0" encoding="utf-8"?>
-<State version="1.2" name="FotoTask" uuid="C6F4F2E2-173F-47B3-8A30-1BFD7D5E0EF6" width="613.333" height="344.167" type="Normal State">
+<State version="1.2" name="FotoTask" uuid="C6F4F2E2-173F-47B3-8A30-1BFD7D5E0EF6" width="1056.67" height="528.417" type="Normal State">
<InputParameters>
<Parameter name="targetLandmark" type="::armarx::StringVariantData" docType="string" optional="no">
<DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"home"}}' docValue="home"/>
@@ -8,25 +8,27 @@
<OutputParameters/>
<LocalParameters/>
<Substates>
- <RemoteState name="Armar6Foto" refuuid="B8A79EAE-B816-45FB-AC00-2DAEE88D8EF6" proxyName="Armar6PresentationGroupRemoteStateOfferer" left="285" top="225.333" boundingSquareSize="100.002"/>
- <EndState name="Failure" event="Failure" left="483.331" top="140" boundingSquareSize="100.002"/>
- <RemoteState name="MovePlatformToLandmark" refuuid="9AE3BE73-6695-4BEF-B91F-D6B072B04F8B" proxyName="PlatformGroupRemoteStateOfferer" left="86.6667" top="174.167" boundingSquareSize="100.002"/>
- <EndState name="Success" event="Success" left="483.331" top="238.667" boundingSquareSize="100.002"/>
+ <RemoteState name="Armar6Foto" refuuid="B8A79EAE-B816-45FB-AC00-2DAEE88D8EF6" proxyName="Armar6PresentationGroupRemoteStateOfferer" left="677.667" top="394.611" boundingSquareSize="99.6636"/>
+ <EndState name="Failure" event="Failure" left="926.67" top="224.667" boundingSquareSize="99.6636"/>
+ <RemoteState name="GoToGoalPositionWithObstacleAvoidance" refuuid="EB5761E3-D8DF-4B79-82F2-DF7780B3E4CD" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="363.917" top="303.833" boundingSquareSize="164.836"/>
+ <RemoteState name="MovePlatformToLandmark" refuuid="9AE3BE73-6695-4BEF-B91F-D6B072B04F8B" proxyName="PlatformGroupRemoteStateOfferer" left="395.917" top="198.056" boundingSquareSize="100.942"/>
+ <RemoteState name="ResolveLandmark" refuuid="68599B94-C644-4023-969B-486210BEFD4A" proxyName="PlatformGroupRemoteStateOfferer" left="114.167" top="144.167" boundingSquareSize="99.6636"/>
+ <EndState name="Success" event="Success" left="926.67" top="422.722" boundingSquareSize="99.6636"/>
</Substates>
<Events>
<Event name="Failure">
<Description>Event for statechart-internal failures or optionally user-code failures</Description>
</Event>
</Events>
- <StartState substateName="MovePlatformToLandmark">
+ <StartState substateName="ResolveLandmark">
<ParameterMappings>
- <ParameterMapping sourceType="Parent" from="targetLandmark" to="targetLandmark"/>
+ <ParameterMapping sourceType="Parent" from="targetLandmark" to="LandmarkName"/>
</ParameterMappings>
<SupportPoints>
- <SupportPoint posX="54.6758" posY="199.167"/>
- <SupportPoint posX="60.8908" posY="199.167"/>
- <SupportPoint posX="69.1158" posY="199.167"/>
- <SupportPoint posX="77.9475" posY="199.167"/>
+ <SupportPoint posX="67.1067" posY="182.167"/>
+ <SupportPoint posX="76.5839" posY="182.167"/>
+ <SupportPoint posX="88.7241" posY="182.167"/>
+ <SupportPoint posX="101.186" posY="182.167"/>
</SupportPoints>
</StartState>
<Transitions>
@@ -35,13 +37,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="186.875" posY="194.467"/>
- <SupportPoint posX="191.875" posY="194.05"/>
- <SupportPoint posX="196.867" posY="193.667"/>
- <SupportPoint posX="201.667" posY="193.333"/>
- <SupportPoint posX="297.192" posY="186.708"/>
- <SupportPoint posX="408.425" posY="182"/>
- <SupportPoint posX="474.517" posY="179.525"/>
+ <SupportPoint posX="496.593" posY="225.841"/>
+ <SupportPoint posX="538.913" posY="228.243"/>
+ <SupportPoint posX="600.834" posY="231.936"/>
+ <SupportPoint posX="654.833" posY="235.833"/>
+ <SupportPoint posX="744.866" posY="242.337"/>
+ <SupportPoint posX="848.941" posY="251.269"/>
+ <SupportPoint posX="913.826" posY="257.006"/>
</SupportPoints>
</Transition>
<Transition from="MovePlatformToLandmark" to="Armar6Foto" eventName="Success">
@@ -49,10 +51,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="186.875" posY="215.275"/>
- <SupportPoint posX="214.108" posY="224.158"/>
- <SupportPoint posX="248.075" posY="235.243"/>
- <SupportPoint posX="276.525" posY="244.525"/>
+ <SupportPoint posX="496.644" posY="247.116"/>
+ <SupportPoint posX="540.945" posY="269.49"/>
+ <SupportPoint posX="605.677" posY="305.536"/>
+ <SupportPoint posX="654.833" posY="347"/>
+ <SupportPoint posX="667.892" posY="358.014"/>
+ <SupportPoint posX="680.555" posY="371.519"/>
+ <SupportPoint posX="691.544" posY="384.513"/>
</SupportPoints>
</Transition>
<Transition from="Armar6Foto" to="Failure" eventName="Failure">
@@ -60,13 +65,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="385.142" posY="250.156"/>
- <SupportPoint posX="410.6" posY="242.548"/>
- <SupportPoint posX="441.792" posY="231.95"/>
- <SupportPoint posX="468.333" posY="219.167"/>
- <SupportPoint posX="470.783" posY="217.983"/>
- <SupportPoint posX="473.258" posY="216.725"/>
- <SupportPoint posX="475.733" posY="215.408"/>
+ <SupportPoint posX="777.909" posY="423.119"/>
+ <SupportPoint posX="822.721" posY="413.884"/>
+ <SupportPoint posX="884.207" posY="399.084"/>
+ <SupportPoint posX="904" posY="384.056"/>
+ <SupportPoint posX="927.882" posY="365.911"/>
+ <SupportPoint posX="945.553" posY="337.493"/>
+ <SupportPoint posX="957.488" posY="312.704"/>
</SupportPoints>
</Transition>
<Transition from="Armar6Foto" to="Success" eventName="Success">
@@ -74,13 +79,77 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="385.133" posY="272.415"/>
- <SupportPoint posX="390.142" posY="273.11"/>
- <SupportPoint posX="395.167" posY="273.715"/>
- <SupportPoint posX="400" posY="274.167"/>
- <SupportPoint posX="424.483" posY="276.452"/>
- <SupportPoint posX="451.617" posY="277.264"/>
- <SupportPoint posX="474.925" posY="277.436"/>
+ <SupportPoint posX="777.781" posY="456.733"/>
+ <SupportPoint posX="785.243" posY="459.434"/>
+ <SupportPoint posX="792.974" posY="461.757"/>
+ <SupportPoint posX="800.5" posY="463.278"/>
+ <SupportPoint posX="837.594" posY="470.768"/>
+ <SupportPoint posX="879.965" posY="470.431"/>
+ <SupportPoint posX="913.877" posY="468.077"/>
+ </SupportPoints>
+ </Transition>
+ <Transition from="ResolveLandmark" to="Failure" eventName="Failure">
+ <ParameterMappings/>
+ <ParameterMappingsToParentsLocal/>
+ <ParameterMappingsToParentsOutput/>
+ <SupportPoints>
+ <SupportPoint posX="214.294" posY="178.18"/>
+ <SupportPoint posX="221.897" posY="177.72"/>
+ <SupportPoint posX="229.653" posY="177.311"/>
+ <SupportPoint posX="237" posY="177.056"/>
+ <SupportPoint posX="282.974" posY="175.446"/>
+ <SupportPoint posX="294.5" posY="176.749"/>
+ <SupportPoint posX="340.5" posY="177.056"/>
+ <SupportPoint posX="534.863" posY="178.372"/>
+ <SupportPoint posX="585.233" posY="165.185"/>
+ <SupportPoint posX="777.5" posY="193.667"/>
+ <SupportPoint posX="834.732" posY="202.151"/>
+ <SupportPoint posX="849.886" posY="203.889"/>
+ <SupportPoint posX="904" posY="224.333"/>
+ <SupportPoint posX="907.629" posY="225.701"/>
+ <SupportPoint posX="911.296" posY="227.208"/>
+ <SupportPoint posX="914.976" posY="228.818"/>
+ </SupportPoints>
+ </Transition>
+ <Transition from="ResolveLandmark" to="GoToGoalPositionWithObstacleAvoidance" eventName="Success">
+ <ParameterMappings>
+ <ParameterMapping sourceType="Output" from="landmarkPosition" to="GoalPose"/>
+ </ParameterMappings>
+ <ParameterMappingsToParentsLocal/>
+ <ParameterMappingsToParentsOutput/>
+ <SupportPoints>
+ <SupportPoint posX="214.217" posY="208.042"/>
+ <SupportPoint posX="262.619" posY="233.073"/>
+ <SupportPoint posX="335.989" posY="270.998"/>
+ <SupportPoint posX="387.152" posY="297.448"/>
+ </SupportPoints>
+ </Transition>
+ <Transition from="GoToGoalPositionWithObstacleAvoidance" to="Failure" eventName="Failure">
+ <ParameterMappings/>
+ <ParameterMappingsToParentsLocal/>
+ <ParameterMappingsToParentsOutput/>
+ <SupportPoints>
+ <SupportPoint posX="528.436" posY="319.285"/>
+ <SupportPoint posX="595.57" posY="312.155"/>
+ <SupportPoint posX="692.732" posY="301.486"/>
+ <SupportPoint posX="777.5" posY="290.778"/>
+ <SupportPoint posX="823.296" posY="284.989"/>
+ <SupportPoint posX="874.892" posY="277.693"/>
+ <SupportPoint posX="914.12" posY="271.982"/>
+ </SupportPoints>
+ </Transition>
+ <Transition from="GoToGoalPositionWithObstacleAvoidance" to="Armar6Foto" eventName="Success">
+ <ParameterMappings/>
+ <ParameterMappingsToParentsLocal/>
+ <ParameterMappingsToParentsOutput/>
+ <SupportPoints>
+ <SupportPoint posX="480.059" posY="352.354"/>
+ <SupportPoint posX="500.018" posY="365.681"/>
+ <SupportPoint posX="526.161" posY="381.481"/>
+ <SupportPoint posX="551.333" posY="391.722"/>
+ <SupportPoint posX="587.724" posY="406.528"/>
+ <SupportPoint posX="630.632" posY="416.625"/>
+ <SupportPoint posX="665.004" posY="423.08"/>
</SupportPoints>
</Transition>
</Transitions>
--
GitLab
From 0efa4d8cde6d272b75b55c5ad0d3f73104cf8492 Mon Sep 17 00:00:00 2001
From: Jianfeng Gao <jianfeng.gao@kit.edu>
Date: Tue, 30 Jul 2024 12:54:11 +0200
Subject: [PATCH 08/22] scenario and cfg update
---
.../Armar6HighlevelApps.scx | 54 +--
.../config/ObjectMemory.cfg | 9 +
.../config/RobotStateMemory.cfg | 11 +-
.../config/SkillsMemory.cfg | 80 +++++
.../distance_to_obstacle_costmap_provider.cfg | 33 ++
.../Armar6ManipulationPipeline.scx | 15 +-
.../config/action_executor.cfg | 128 +++----
.../config/contractor.cfg | 315 ++++++++++++++++--
.../config/manipulation_memory.cfg | 2 +-
.../config/optik_wb_target_generator.cfg | 265 +++++++++++++++
.../config/placement_around_box.cfg | 2 +-
scenarios/Armar6Skills/Armar6Skills.scx | 18 +-
.../config/control_skill_provider.cfg | 24 ++
...der.cfg => human_robot_skill_provider.cfg} | 103 +++---
.../config/view_selection_skill_provider.cfg | 32 ++
scenarios/CeBITTasks/CeBITTasks.scx | 100 +++---
scenarios/HumanAvoidance/HumanAvoidance.scx | 14 +-
17 files changed, 922 insertions(+), 283 deletions(-)
create mode 100644 scenarios/Armar6ManipulationPipeline/config/optik_wb_target_generator.cfg
rename scenarios/Armar6Skills/config/{hand_over_skill_provider.cfg => human_robot_skill_provider.cfg} (74%)
diff --git a/scenarios/Armar6HighlevelApps/Armar6HighlevelApps.scx b/scenarios/Armar6HighlevelApps/Armar6HighlevelApps.scx
index e1cc553d3..5aee3b118 100644
--- a/scenarios/Armar6HighlevelApps/Armar6HighlevelApps.scx
+++ b/scenarios/Armar6HighlevelApps/Armar6HighlevelApps.scx
@@ -1,30 +1,30 @@
<?xml version="1.0" encoding="utf-8"?>
-<scenario name="Armar6HighlevelApps" creation="2017-09-19.11:33:59 AM" globalConfigName="./config/global.cfg" package="armar6_skills" deploymentType="local" nodeName="NodeMain">
- <application name="WorkingMemory" instance="" package="MemoryX" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="ObjectMemoryObserver" instance="" package="MemoryX" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="LongtermMemory" instance="" package="MemoryX" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="CommonStorage" instance="" package="MemoryX" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="PriorKnowledge" instance="" package="MemoryX" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="RobotIK" instance="" package="RobotComponents" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="ObjectLocalizationSaliencyApp" instance="" package="RobotComponents" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="TrajectoryPlayerApp" instance="" package="RobotComponents" nodeName="" enabled="false" iceAutoRestart="false"/>
- <application name="GraphNodePoseResolverApp" instance="" package="MemoryX" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="WorkingMemoryToArVizApp" instance="" package="MemoryX" nodeName="" enabled="false" iceAutoRestart="false"/>
- <application name="ObjectMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="WorkingMemoryObjectPoseProviderApp" instance="" package="MemoryX" nodeName="" enabled="false" iceAutoRestart="false"/>
- <application name="RobotStateMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="navigation_memory" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="SimpleStatechartExecutorApp" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="control_memory" instance="" package="armarx_control" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="navigator" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="view_memory" instance="" package="armarx_view_selection" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="gaze_scheduler" instance="" package="armarx_view_selection" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="SkillsMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="distance_to_obstacle_costmap_provider" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="IndexMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="ObjectInstanceToIndex" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="person_target_provider" instance="" package="armarx_view_selection" nodeName="" enabled="false" iceAutoRestart="false"/>
- <application name="speech_memory" instance="" package="armarx_speech" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="symbolic_scene_memory" instance="" package="armarx_symbolic_planning" nodeName="" enabled="true" iceAutoRestart="false"/>
+<scenario name="Armar6HighlevelApps" creation="2017-09-19.11:33:59 AM" globalConfigName="./config/global.cfg" package="armar6_skills" nodeName="NodeMain">
+ <application name="WorkingMemory" instance="" package="MemoryX" nodeName="" enabled="true"/>
+ <application name="ObjectMemoryObserver" instance="" package="MemoryX" nodeName="" enabled="true"/>
+ <application name="LongtermMemory" instance="" package="MemoryX" nodeName="" enabled="true"/>
+ <application name="CommonStorage" instance="" package="MemoryX" nodeName="" enabled="true"/>
+ <application name="PriorKnowledge" instance="" package="MemoryX" nodeName="" enabled="true"/>
+ <application name="RobotIK" instance="" package="RobotComponents" nodeName="" enabled="true"/>
+ <application name="ObjectLocalizationSaliencyApp" instance="" package="RobotComponents" nodeName="" enabled="true"/>
+ <application name="TrajectoryPlayerApp" instance="" package="RobotComponents" nodeName="" enabled="false"/>
+ <application name="GraphNodePoseResolverApp" instance="" package="MemoryX" nodeName="" enabled="true"/>
+ <application name="WorkingMemoryToArVizApp" instance="" package="MemoryX" nodeName="" enabled="false"/>
+ <application name="ObjectMemory" instance="" package="RobotAPI" nodeName="" enabled="true"/>
+ <application name="WorkingMemoryObjectPoseProviderApp" instance="" package="MemoryX" nodeName="" enabled="false"/>
+ <application name="RobotStateMemory" instance="" package="RobotAPI" nodeName="" enabled="true"/>
+ <application name="navigation_memory" instance="" package="armarx_navigation" nodeName="" enabled="true"/>
+ <application name="SimpleStatechartExecutorApp" instance="" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="control_memory" instance="" package="armarx_control" nodeName="" enabled="true"/>
+ <application name="navigator" instance="" package="armarx_navigation" nodeName="" enabled="true"/>
+ <application name="view_memory" instance="" package="armarx_view_selection" nodeName="" enabled="true"/>
+ <application name="gaze_scheduler" instance="" package="armarx_view_selection" nodeName="" enabled="true"/>
+ <application name="SkillsMemory" instance="" package="RobotAPI" nodeName="" enabled="true"/>
+ <application name="distance_to_obstacle_costmap_provider" instance="" package="armarx_navigation" nodeName="" enabled="true"/>
+ <application name="IndexMemory" instance="" package="RobotAPI" nodeName="" enabled="true"/>
+ <application name="ObjectInstanceToIndex" instance="" package="RobotAPI" nodeName="" enabled="true"/>
+ <application name="person_target_provider" instance="" package="armarx_view_selection" nodeName="" enabled="false"/>
+ <application name="speech_memory" instance="" package="armarx_speech" nodeName="" enabled="true"/>
+ <application name="symbolic_scene_memory" instance="" package="armarx_symbolic_planning" nodeName="" enabled="true"/>
</scenario>
diff --git a/scenarios/Armar6HighlevelApps/config/ObjectMemory.cfg b/scenarios/Armar6HighlevelApps/config/ObjectMemory.cfg
index 3bf9a8af3..e4a3f44d6 100644
--- a/scenarios/Armar6HighlevelApps/config/ObjectMemory.cfg
+++ b/scenarios/Armar6HighlevelApps/config/ObjectMemory.cfg
@@ -413,6 +413,15 @@ ArmarX.ObjectMemory.mem.inst.robots.FallbackName = Armar6
ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = PriorKnowledgeData/scenes/50_19/R005/SecondHands.json
+# ArmarX.ObjectMemory.mem.inst.scene.13_autoReloadSceneSnapshotsOnFileChange:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.scene.13_autoReloadSceneSnapshotsOnFileChange = false
+
+
# ArmarX.ObjectMemory.mem.inst.seg.CoreMaxHistorySize: Maximal size of the Instance entity histories (-1 for infinite).
# Attributes:
# - Default: 64
diff --git a/scenarios/Armar6HighlevelApps/config/RobotStateMemory.cfg b/scenarios/Armar6HighlevelApps/config/RobotStateMemory.cfg
index 0130d73a8..313206002 100644
--- a/scenarios/Armar6HighlevelApps/config/RobotStateMemory.cfg
+++ b/scenarios/Armar6HighlevelApps/config/RobotStateMemory.cfg
@@ -314,6 +314,15 @@ ArmarX.RobotStateMemory.mem.ltm.exportPath = "/media/ltm"
# ArmarX.RobotStateMemory.mem.prop.seg.CoreSegmentName = Proprioception
+# ArmarX.RobotStateMemory.mem.visu.collisionModelEnabled: Enable visualization of collision model.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotStateMemory.mem.visu.collisionModelEnabled = false
+
+
# ArmarX.RobotStateMemory.mem.visu.enabled: Enable or disable visualization of objects.
# Attributes:
# - Default: true
@@ -431,6 +440,6 @@ ArmarX.RobotStateMemory.mem.visu.forceTorque.enabled = false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.Verbosity = Info
+ArmarX.Verbosity = Verbose
diff --git a/scenarios/Armar6HighlevelApps/config/SkillsMemory.cfg b/scenarios/Armar6HighlevelApps/config/SkillsMemory.cfg
index f5652af6d..092c0f206 100644
--- a/scenarios/Armar6HighlevelApps/config/SkillsMemory.cfg
+++ b/scenarios/Armar6HighlevelApps/config/SkillsMemory.cfg
@@ -176,6 +176,54 @@
# ArmarX.SkillMemory.mem.MemoryName = Skill
+# ArmarX.SkillMemory.mem.event.seg.CoreMaxHistorySize: Maximal size of the SkillEvent entity histories (-1 for infinite).
+# Attributes:
+# - Default: 10
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SkillMemory.mem.event.seg.CoreMaxHistorySize = 10
+
+
+# ArmarX.SkillMemory.mem.event.seg.CoreSegmentName: Name of the SkillEvent core segment.
+# Attributes:
+# - Default: SkillEvent
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SkillMemory.mem.event.seg.CoreSegmentName = SkillEvent
+
+
+# ArmarX.SkillMemory.mem.executableskill.seg.CoreMaxHistorySize: Maximal size of the ExecutableSkill entity histories (-1 for infinite).
+# Attributes:
+# - Default: 10
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SkillMemory.mem.executableskill.seg.CoreMaxHistorySize = 10
+
+
+# ArmarX.SkillMemory.mem.executableskill.seg.CoreSegmentName: Name of the ExecutableSkill core segment.
+# Attributes:
+# - Default: ExecutableSkill
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SkillMemory.mem.executableskill.seg.CoreSegmentName = ExecutableSkill
+
+
+# ArmarX.SkillMemory.mem.executionrequest.seg.CoreMaxHistorySize: Maximal size of the SkillExecutionRequest entity histories (-1 for infinite).
+# Attributes:
+# - Default: 10
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SkillMemory.mem.executionrequest.seg.CoreMaxHistorySize = 10
+
+
+# ArmarX.SkillMemory.mem.executionrequest.seg.CoreSegmentName: Name of the SkillExecutionRequest core segment.
+# Attributes:
+# - Default: SkillExecutionRequest
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SkillMemory.mem.executionrequest.seg.CoreSegmentName = SkillExecutionRequest
+
+
# ArmarX.SkillMemory.mem.ltm.configuration:
# Attributes:
# - Default: { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}, "PngConverter": {}, "ExrConverter": {}}
@@ -217,6 +265,38 @@
# ArmarX.SkillMemory.mem.ltm.storeFrequency = 10
+# ArmarX.SkillMemory.mem.statechartlistener.seg.CoreMaxHistorySize: Maximal size of the Transitions entity histories (-1 for infinite).
+# Attributes:
+# - Default: 10
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SkillMemory.mem.statechartlistener.seg.CoreMaxHistorySize = 10
+
+
+# ArmarX.SkillMemory.mem.statechartlistener.seg.CoreSegmentName: Name of the Transitions core segment.
+# Attributes:
+# - Default: Transitions
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SkillMemory.mem.statechartlistener.seg.CoreSegmentName = Transitions
+
+
+# ArmarX.SkillMemory.mem.statechartlistener.seg.ProviderMaxHistorySize: Maximal size of the StatechartListener entity histories (-1 for infinite).
+# Attributes:
+# - Default: 10
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SkillMemory.mem.statechartlistener.seg.ProviderMaxHistorySize = 10
+
+
+# ArmarX.SkillMemory.mem.statechartlistener.seg.ProviderSegmentName: Name of the StatechartListener provider segment.
+# Attributes:
+# - Default: StatechartListener
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SkillMemory.mem.statechartlistener.seg.ProviderSegmentName = StatechartListener
+
+
# ArmarX.SkillMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
# Set to false to use this memory as a stand-alone.
# Attributes:
diff --git a/scenarios/Armar6HighlevelApps/config/distance_to_obstacle_costmap_provider.cfg b/scenarios/Armar6HighlevelApps/config/distance_to_obstacle_costmap_provider.cfg
index cbdd3ce58..8e714efcb 100644
--- a/scenarios/Armar6HighlevelApps/config/distance_to_obstacle_costmap_provider.cfg
+++ b/scenarios/Armar6HighlevelApps/config/distance_to_obstacle_costmap_provider.cfg
@@ -261,6 +261,39 @@ ArmarX.SecondsStartupDelay = 5
# ArmarX.distance_to_obstacle_costmap_provider.mns.MemoryNameSystemName = MemoryNameSystem
+# ArmarX.distance_to_obstacle_costmap_provider.p.colModel:
+# Attributes:
+# - Default: Platform-navigation-colmodel
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.p.colModel = Platform-navigation-colmodel
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.p.costmapBuilderParams.binaryGrid:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.distance_to_obstacle_costmap_provider.p.costmapBuilderParams.binaryGrid = false
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.p.costmapBuilderParams.cellSize:
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.p.costmapBuilderParams.cellSize = 100
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.p.costmapBuilderParams.sceneBoundsMargin:
+# Attributes:
+# - Default: 1000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.p.costmapBuilderParams.sceneBoundsMargin = 1000
+
+
# ArmarX.distance_to_obstacle_costmap_provider.p.robotName: Robot name.
# Attributes:
# - Case sensitivity: yes
diff --git a/scenarios/Armar6ManipulationPipeline/Armar6ManipulationPipeline.scx b/scenarios/Armar6ManipulationPipeline/Armar6ManipulationPipeline.scx
index dc7695a79..10356df67 100644
--- a/scenarios/Armar6ManipulationPipeline/Armar6ManipulationPipeline.scx
+++ b/scenarios/Armar6ManipulationPipeline/Armar6ManipulationPipeline.scx
@@ -1,10 +1,11 @@
<?xml version="1.0" encoding="utf-8"?>
-<scenario name="Armar6ManipulationPipeline" creation="2023-10-06.16:01:17" globalConfigName="./config/global.cfg" package="armar6_skills" deploymentType="local" nodeName="NodeMain">
- <application name="contractor" instance="" package="armarx_mobile_manipulation" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="action_executor" instance="" package="armarx_mobile_manipulation" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="manipulation_memory" instance="" package="armarx_manipulation" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="placement_around_box" instance="" package="armarx_robot_placement" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="nlp_wb_target_generator" instance="" package="armarx_robot_placement" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="symbolic_scene_memory" instance="" package="armarx_symbolic_planning" nodeName="" enabled="true" iceAutoRestart="false"/>
+<scenario name="Armar6ManipulationPipeline" creation="2023-10-06.16:01:17" globalConfigName="./config/global.cfg" package="armar6_skills" nodeName="NodeMain">
+ <application name="contractor" instance="" package="armarx_mobile_manipulation" nodeName="" enabled="true"/>
+ <application name="action_executor" instance="" package="armarx_mobile_manipulation" nodeName="" enabled="true"/>
+ <application name="manipulation_memory" instance="" package="armarx_manipulation" nodeName="" enabled="true"/>
+ <application name="placement_around_box" instance="" package="armarx_robot_placement" nodeName="" enabled="true"/>
+ <application name="nlp_wb_target_generator" instance="" package="armarx_robot_placement" nodeName="" enabled="true"/>
+ <application name="symbolic_scene_memory" instance="" package="armarx_symbolic_planning" nodeName="" enabled="true"/>
+ <application name="optik_wb_target_generator" instance="" package="armarx_robot_placement" nodeName="" enabled="true"/>
</scenario>
diff --git a/scenarios/Armar6ManipulationPipeline/config/action_executor.cfg b/scenarios/Armar6ManipulationPipeline/config/action_executor.cfg
index aad9523e7..c26f212b2 100644
--- a/scenarios/Armar6ManipulationPipeline/config/action_executor.cfg
+++ b/scenarios/Armar6ManipulationPipeline/config/action_executor.cfg
@@ -166,7 +166,7 @@
# - Case sensitivity: yes
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.Verbosity = Info
+ArmarX.Verbosity = Debug
# ArmarX.action_execution.ArVizStorageName: Name of the ArViz storage
@@ -233,6 +233,14 @@
# ArmarX.action_execution.Config.DMP.Trajectories.default_to_prepose_left_ns = jointTraj_robdekon_above_box_left_6.csv
+# ArmarX.action_execution.Config.DMP.Trajectories.default_to_prepose_left_top:
+# Attributes:
+# - Default: reach_to_grasp_3_top-2024-05-29-17-51-39-ts-forward_left.csv
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.action_execution.Config.DMP.Trajectories.default_to_prepose_left_top = reach_to_grasp_3_top-2024-05-29-17-51-39-ts-forward_left.csv
+
+
# ArmarX.action_execution.Config.DMP.Trajectories.default_to_prepose_right:
# Attributes:
# - Default: taskTraj_robdekon_above_box_right_from_left_6.csv
@@ -249,6 +257,14 @@
# ArmarX.action_execution.Config.DMP.Trajectories.default_to_prepose_right_ns = jointTraj_robdekon_above_box_right_from_left_6.csv
+# ArmarX.action_execution.Config.DMP.Trajectories.default_to_prepose_right_top:
+# Attributes:
+# - Default: reach_to_grasp_3_top-2024-05-29-17-51-39-ts-forward_right.csv
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.action_execution.Config.DMP.Trajectories.default_to_prepose_right_top = reach_to_grasp_3_top-2024-05-29-17-51-39-ts-forward_right.csv
+
+
# ArmarX.action_execution.Config.DMP.Trajectories.dmp_directory.package:
# Attributes:
# - Default: armar6_motions
@@ -337,6 +353,22 @@
# ArmarX.action_execution.Config.General.left_hand_tcp_name = Hand L TCP
+# ArmarX.action_execution.Config.General.move_to_pre_pose_duration_in_s:
+# Attributes:
+# - Default: 6
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.action_execution.Config.General.move_to_pre_pose_duration_in_s = 6
+
+
+# ArmarX.action_execution.Config.General.obstacle_height_in_mm:
+# Attributes:
+# - Default: 800
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.action_execution.Config.General.obstacle_height_in_mm = 800
+
+
# ArmarX.action_execution.Config.General.right_hand_tcp_name:
# Attributes:
# - Default: Hand R TCP
@@ -401,100 +433,20 @@
# ArmarX.action_execution.Config.General.trajectory_under_box_to_above_rh_filename = under_box_to_above_rh.json
-# ArmarX.action_execution.Config.move_to_pre_pose.finalShapeClose:
-# Attributes:
-# - Default: Hook
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.action_execution.Config.move_to_pre_pose.finalShapeClose = Hook
-
-
-# ArmarX.action_execution.Config.move_to_pre_pose.finalShapeGrasp:
-# Attributes:
-# - Default: Open
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.action_execution.Config.move_to_pre_pose.finalShapeGrasp = Open
-
-
-# ArmarX.action_execution.Config.move_to_pre_pose.finalShapeOpen:
-# Attributes:
-# - Default: Hook
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.action_execution.Config.move_to_pre_pose.finalShapeOpen = Hook
-
-
-# ArmarX.action_execution.Config.move_to_pre_pose.finalShapePlace:
-# Attributes:
-# - Default: Close
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.action_execution.Config.move_to_pre_pose.finalShapePlace = Close
-
-
-# ArmarX.action_execution.Config.move_to_pre_pose.finalShapePull:
-# Attributes:
-# - Default: Hook
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.action_execution.Config.move_to_pre_pose.finalShapePull = Hook
-
-
-# ArmarX.action_execution.Config.move_to_pre_pose.finalShapePush:
-# Attributes:
-# - Default: Close
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.action_execution.Config.move_to_pre_pose.finalShapePush = Close
-
-
-# ArmarX.action_execution.Config.move_to_retract_pose.finalShapeClose:
-# Attributes:
-# - Default: Close
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.action_execution.Config.move_to_retract_pose.finalShapeClose = Close
-
-
-# ArmarX.action_execution.Config.move_to_retract_pose.finalShapeGrasp:
-# Attributes:
-# - Default: Close
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.action_execution.Config.move_to_retract_pose.finalShapeGrasp = Close
-
-
-# ArmarX.action_execution.Config.move_to_retract_pose.finalShapeOpen:
-# Attributes:
-# - Default: Open
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.action_execution.Config.move_to_retract_pose.finalShapeOpen = Open
-
-
-# ArmarX.action_execution.Config.move_to_retract_pose.finalShapePlace:
-# Attributes:
-# - Default: Open
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.action_execution.Config.move_to_retract_pose.finalShapePlace = Open
-
-
-# ArmarX.action_execution.Config.move_to_retract_pose.finalShapePull:
+# ArmarX.action_execution.Config.move_to_pre_pose.finalShapes:
# Attributes:
-# - Default: Open
+# - Default: Close:Hook,Grasp:Open,Open:Hook,Place:Close,Pour:Close,Pull:Hook,Push:Close
# - Case sensitivity: yes
# - Required: no
-# ArmarX.action_execution.Config.move_to_retract_pose.finalShapePull = Open
+# ArmarX.action_execution.Config.move_to_pre_pose.finalShapes = Close:Hook,Grasp:Open,Open:Hook,Place:Close,Pour:Close,Pull:Hook,Push:Close
-# ArmarX.action_execution.Config.move_to_retract_pose.finalShapePush:
+# ArmarX.action_execution.Config.move_to_retract_pose.finalShapes:
# Attributes:
-# - Default: Close
+# - Default: Close:Open,Grasp:Close,Open:Open,Place:Open,Pour:Close,Pull:Open,Push:Open
# - Case sensitivity: yes
# - Required: no
-# ArmarX.action_execution.Config.move_to_retract_pose.finalShapePush = Close
+# ArmarX.action_execution.Config.move_to_retract_pose.finalShapes = Close:Open,Grasp:Close,Open:Open,Place:Open,Pour:Close,Pull:Open,Push:Open
# ArmarX.action_execution.Config.robotName:
@@ -511,7 +463,7 @@
# - Case sensitivity: yes
# - Required: no
# - Possible values: {NJointCartesianNaturalPositionController, NJointCartesianWaypointController, NJointTaskSpaceDMPController, NJointTaskSpaceImpedanceController, NJointTaskSpaceImpedanceDMPController}
-ArmarX.action_execution.DRT.ControllerType = NJointCartesianWaypointController
+ArmarX.action_execution.DRT.ControllerType = NJointTaskSpaceImpedanceDMPController
# ArmarX.action_execution.DRT.EnableLeftSide: Enable/disable control of left arm.
diff --git a/scenarios/Armar6ManipulationPipeline/config/contractor.cfg b/scenarios/Armar6ManipulationPipeline/config/contractor.cfg
index f703adca2..c0683aa23 100644
--- a/scenarios/Armar6ManipulationPipeline/config/contractor.cfg
+++ b/scenarios/Armar6ManipulationPipeline/config/contractor.cfg
@@ -431,6 +431,38 @@ ArmarX.Verbosity = Debug
# ArmarX.contractor.config.discovery.action_at_pose.enable = true
+# ArmarX.contractor.config.discovery.action_at_pose.hypothesisBuilder.candidateOffsetRotation:
+# Attributes:
+# - Default: 0, 0, 0, 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.discovery.action_at_pose.hypothesisBuilder.candidateOffsetRotation = 0, 0, 0, 1
+
+
+# ArmarX.contractor.config.discovery.action_at_pose.hypothesisBuilder.candidateOffsetTranslationDistal:
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.discovery.action_at_pose.hypothesisBuilder.candidateOffsetTranslationDistal = 0
+
+
+# ArmarX.contractor.config.discovery.action_at_pose.hypothesisBuilder.candidateOffsetTranslationDorsal:
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.discovery.action_at_pose.hypothesisBuilder.candidateOffsetTranslationDorsal = 0
+
+
+# ArmarX.contractor.config.discovery.action_at_pose.hypothesisBuilder.candidateOffsetTranslationThumb:
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.discovery.action_at_pose.hypothesisBuilder.candidateOffsetTranslationThumb = 0
+
+
# ArmarX.contractor.config.discovery.external.enable:
# Attributes:
# - Default: false
@@ -457,6 +489,38 @@ ArmarX.Verbosity = Debug
# ArmarX.contractor.config.discovery.grasp_candidate_provider.enable = true
+# ArmarX.contractor.config.discovery.grasp_candidate_provider.hypothesisBuilder.candidateOffsetRotation:
+# Attributes:
+# - Default: 0, 0, 0, 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.discovery.grasp_candidate_provider.hypothesisBuilder.candidateOffsetRotation = 0, 0, 0, 1
+
+
+# ArmarX.contractor.config.discovery.grasp_candidate_provider.hypothesisBuilder.candidateOffsetTranslationDistal:
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.discovery.grasp_candidate_provider.hypothesisBuilder.candidateOffsetTranslationDistal = 0
+
+
+# ArmarX.contractor.config.discovery.grasp_candidate_provider.hypothesisBuilder.candidateOffsetTranslationDorsal:
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.discovery.grasp_candidate_provider.hypothesisBuilder.candidateOffsetTranslationDorsal = 0
+
+
+# ArmarX.contractor.config.discovery.grasp_candidate_provider.hypothesisBuilder.candidateOffsetTranslationThumb:
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.discovery.grasp_candidate_provider.hypothesisBuilder.candidateOffsetTranslationThumb = 0
+
+
# ArmarX.contractor.config.discovery.grasp_id.enable:
# Attributes:
# - Default: true
@@ -466,6 +530,38 @@ ArmarX.Verbosity = Debug
# ArmarX.contractor.config.discovery.grasp_id.enable = true
+# ArmarX.contractor.config.discovery.grasp_id.hypothesisBuilder.candidateOffsetRotation:
+# Attributes:
+# - Default: 0, 0, 0, 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.discovery.grasp_id.hypothesisBuilder.candidateOffsetRotation = 0, 0, 0, 1
+
+
+# ArmarX.contractor.config.discovery.grasp_id.hypothesisBuilder.candidateOffsetTranslationDistal:
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.discovery.grasp_id.hypothesisBuilder.candidateOffsetTranslationDistal = 0
+
+
+# ArmarX.contractor.config.discovery.grasp_id.hypothesisBuilder.candidateOffsetTranslationDorsal:
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.discovery.grasp_id.hypothesisBuilder.candidateOffsetTranslationDorsal = 0
+
+
+# ArmarX.contractor.config.discovery.grasp_id.hypothesisBuilder.candidateOffsetTranslationThumb:
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.discovery.grasp_id.hypothesisBuilder.candidateOffsetTranslationThumb = 0
+
+
# ArmarX.contractor.config.discovery.known_object_oobb.enable:
# Attributes:
# - Default: true
@@ -483,12 +579,20 @@ ArmarX.Verbosity = Debug
# ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.candidateOffsetRotation = 0, 0, 0, 1
-# ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationDistal:
+# ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationDistalLeft:
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationDistalLeft = 0
+
+
+# ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationDistalRight:
# Attributes:
# - Default: -30
# - Case sensitivity: yes
# - Required: no
-# ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationDistal = -30
+# ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationDistalRight = -30
# ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationDorsal:
@@ -499,12 +603,20 @@ ArmarX.Verbosity = Debug
# ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationDorsal = 0
-# ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationThumb:
+# ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationThumbLeft:
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationThumbLeft = 0
+
+
+# ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationThumbRight:
# Attributes:
# - Default: -30
# - Case sensitivity: yes
# - Required: no
-# ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationThumb = -30
+# ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationThumbRight = -30
# ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.candidateShiftTowardsCenterInMM:
@@ -515,13 +627,13 @@ ArmarX.Verbosity = Debug
# ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.candidateShiftTowardsCenterInMM = 30
-# ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.disableTopGraspsForHeighObjects:
+# ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.disableTopGraspsForHighObjects:
# Attributes:
# - Default: true
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.disableTopGraspsForHeighObjects = true
+# ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.disableTopGraspsForHighObjects = true
# ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.heightMaxInMM:
@@ -597,6 +709,15 @@ ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.sho
# ArmarX.contractor.config.discovery.known_object_oobb.visualizeBoundingBoxes = true
+# ArmarX.contractor.config.discovery.known_object_predefined_affordances.enable:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.contractor.config.discovery.known_object_predefined_affordances.enable = false
+
+
# ArmarX.contractor.config.discovery.known_object_predefined_grasps.enable:
# Attributes:
# - Default: true
@@ -656,6 +777,38 @@ ArmarX.contractor.config.discovery.known_object_oobb.oobb_hypothesis_builder.sho
# ArmarX.contractor.config.discovery.object_id.enable = true
+# ArmarX.contractor.config.discovery.object_id.hypothesisBuilder.candidateOffsetRotation:
+# Attributes:
+# - Default: 0, 0, 0, 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.discovery.object_id.hypothesisBuilder.candidateOffsetRotation = 0, 0, 0, 1
+
+
+# ArmarX.contractor.config.discovery.object_id.hypothesisBuilder.candidateOffsetTranslationDistal:
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.discovery.object_id.hypothesisBuilder.candidateOffsetTranslationDistal = 0
+
+
+# ArmarX.contractor.config.discovery.object_id.hypothesisBuilder.candidateOffsetTranslationDorsal:
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.discovery.object_id.hypothesisBuilder.candidateOffsetTranslationDorsal = 0
+
+
+# ArmarX.contractor.config.discovery.object_id.hypothesisBuilder.candidateOffsetTranslationThumb:
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.discovery.object_id.hypothesisBuilder.candidateOffsetTranslationThumb = 0
+
+
# ArmarX.contractor.config.discovery.pre_defined.enable:
# Attributes:
# - Default: true
@@ -781,12 +934,20 @@ ArmarX.contractor.config.discovery.unknown_object_oobb.objectCentricPointCloudFi
# ArmarX.contractor.config.discovery.unknown_object_oobb.oobb_hypothesis_builder.candidateOffsetRotation = 0, 0, 0, 1
-# ArmarX.contractor.config.discovery.unknown_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationDistal:
+# ArmarX.contractor.config.discovery.unknown_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationDistalLeft:
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.discovery.unknown_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationDistalLeft = 0
+
+
+# ArmarX.contractor.config.discovery.unknown_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationDistalRight:
# Attributes:
# - Default: -30
# - Case sensitivity: yes
# - Required: no
-# ArmarX.contractor.config.discovery.unknown_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationDistal = -30
+# ArmarX.contractor.config.discovery.unknown_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationDistalRight = -30
# ArmarX.contractor.config.discovery.unknown_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationDorsal:
@@ -797,12 +958,20 @@ ArmarX.contractor.config.discovery.unknown_object_oobb.objectCentricPointCloudFi
# ArmarX.contractor.config.discovery.unknown_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationDorsal = 0
-# ArmarX.contractor.config.discovery.unknown_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationThumb:
+# ArmarX.contractor.config.discovery.unknown_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationThumbLeft:
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.discovery.unknown_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationThumbLeft = 0
+
+
+# ArmarX.contractor.config.discovery.unknown_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationThumbRight:
# Attributes:
# - Default: -30
# - Case sensitivity: yes
# - Required: no
-# ArmarX.contractor.config.discovery.unknown_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationThumb = -30
+# ArmarX.contractor.config.discovery.unknown_object_oobb.oobb_hypothesis_builder.candidateOffsetTranslationThumbRight = -30
# ArmarX.contractor.config.discovery.unknown_object_oobb.oobb_hypothesis_builder.candidateShiftTowardsCenterInMM:
@@ -813,13 +982,13 @@ ArmarX.contractor.config.discovery.unknown_object_oobb.objectCentricPointCloudFi
# ArmarX.contractor.config.discovery.unknown_object_oobb.oobb_hypothesis_builder.candidateShiftTowardsCenterInMM = 30
-# ArmarX.contractor.config.discovery.unknown_object_oobb.oobb_hypothesis_builder.disableTopGraspsForHeighObjects:
+# ArmarX.contractor.config.discovery.unknown_object_oobb.oobb_hypothesis_builder.disableTopGraspsForHighObjects:
# Attributes:
# - Default: true
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.contractor.config.discovery.unknown_object_oobb.oobb_hypothesis_builder.disableTopGraspsForHeighObjects = true
+# ArmarX.contractor.config.discovery.unknown_object_oobb.oobb_hypothesis_builder.disableTopGraspsForHighObjects = true
# ArmarX.contractor.config.discovery.unknown_object_oobb.oobb_hypothesis_builder.heightMaxInMM:
@@ -1030,68 +1199,68 @@ ArmarX.contractor.config.execution.external.proxyName = action_execution
# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.enableThumbDownPoses = false
-# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.prePoseOffsetInMM:
+# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.offsets.prePose.otherApproach:
# Attributes:
-# - Default: 50
+# - Default: 200
# - Case sensitivity: yes
# - Required: no
-# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.prePoseOffsetInMM = 50
+# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.offsets.prePose.otherApproach = 200
-# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.trajectoryIDs.closingEntityName:
+# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.offsets.prePose.otherApproachAdditionalZ:
# Attributes:
-# - Default: Close
+# - Default: 0
# - Case sensitivity: yes
# - Required: no
-# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.trajectoryIDs.closingEntityName = Close
+# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.offsets.prePose.otherApproachAdditionalZ = 0
-# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.trajectoryIDs.graspingEntityName:
+# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.offsets.prePose.sideApproach:
# Attributes:
-# - Default: Grasp
+# - Default: 200
# - Case sensitivity: yes
# - Required: no
-# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.trajectoryIDs.graspingEntityName = Grasp
+# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.offsets.prePose.sideApproach = 200
-# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.trajectoryIDs.openingEntityName:
+# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.offsets.prePose.topApproach:
# Attributes:
-# - Default: Open
+# - Default: 200
# - Case sensitivity: yes
# - Required: no
-# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.trajectoryIDs.openingEntityName = Open
+# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.offsets.prePose.topApproach = 200
-# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.trajectoryIDs.placingEntityName:
+# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.offsets.prePose.withDirection:
# Attributes:
-# - Default: Place
+# - Default: 200
# - Case sensitivity: yes
# - Required: no
-# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.trajectoryIDs.placingEntityName = Place
+# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.offsets.prePose.withDirection = 200
-# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.trajectoryIDs.providerSegmentName:
+# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.offsets.retractPose.minZToPrePose:
# Attributes:
-# - Default: Armar6
+# - Default: 100
# - Case sensitivity: yes
# - Required: no
-# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.trajectoryIDs.providerSegmentName = Armar6
+# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.offsets.retractPose.minZToPrePose = 100
-# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.trajectoryIDs.pullingEntityName:
+# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.trajectoryIDs.entityNames:
# Attributes:
-# - Default: Pull
+# - Default: Close:Close,Grasp:Grasp,Open:Open,Place:Place,Pour:Pour,Pull:Pull,Push:Push
# - Case sensitivity: yes
# - Required: no
-# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.trajectoryIDs.pullingEntityName = Pull
+# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.trajectoryIDs.entityNames = Close:Close,Grasp:Grasp,Open:Open,Place:Place,Pour:Pour,Pull:Pull,Push:Push
-# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.trajectoryIDs.pushingEntityName:
+# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.trajectoryIDs.providerSegmentNames:
# Attributes:
-# - Default: Push
+# - Default: Close:Predefined,Grasp:Predefined,Open:Predefined,Place:Predefined,Pour:Predefined,Pull:Predefined,Push:Predefined
# - Case sensitivity: yes
# - Required: no
-# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.trajectoryIDs.pushingEntityName = Push
+# ArmarX.contractor.config.parameterization.frontal_placement.actionBuilder.trajectoryIDs.providerSegmentNames = Close:Predefined,Grasp:Predefined,Open:Predefined,Place:Predefined,Pour:Predefined,Pull:Predefined,Push:Predefined
# ArmarX.contractor.config.parameterization.frontal_placement.enable:
@@ -1471,6 +1640,80 @@ ArmarX.contractor.config.planning.visualizeActions = true
# ArmarX.contractor.config.validation.filtering.enable = true
+# ArmarX.contractor.config.validation.filtering.jacobianVelocityReachabilityFilter.alphaGradientDescent:
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.validation.filtering.jacobianVelocityReachabilityFilter.alphaGradientDescent = 1
+
+
+# ArmarX.contractor.config.validation.filtering.jacobianVelocityReachabilityFilter.gradientThreshold:
+# Attributes:
+# - Default: 0.0030000000000000001
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.validation.filtering.jacobianVelocityReachabilityFilter.gradientThreshold = 0.0030000000000000001
+
+
+# ArmarX.contractor.config.validation.filtering.jacobianVelocityReachabilityFilter.jointLimitAvoidance:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.contractor.config.validation.filtering.jacobianVelocityReachabilityFilter.jointLimitAvoidance = true
+
+
+# ArmarX.contractor.config.validation.filtering.jacobianVelocityReachabilityFilter.maxIterations:
+# Attributes:
+# - Default: 50
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.validation.filtering.jacobianVelocityReachabilityFilter.maxIterations = 50
+
+
+# ArmarX.contractor.config.validation.filtering.jacobianVelocityReachabilityFilter.maxOrientationVelRadian:
+# Attributes:
+# - Default: 0.20000000000000001
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.validation.filtering.jacobianVelocityReachabilityFilter.maxOrientationVelRadian = 0.20000000000000001
+
+
+# ArmarX.contractor.config.validation.filtering.jacobianVelocityReachabilityFilter.maxPositionVelMeter:
+# Attributes:
+# - Default: 0.29999999999999999
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.validation.filtering.jacobianVelocityReachabilityFilter.maxPositionVelMeter = 0.29999999999999999
+
+
+# ArmarX.contractor.config.validation.filtering.jacobianVelocityReachabilityFilter.thresholdOrientationReachedRadian:
+# Attributes:
+# - Default: 0.10000000000000001
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.validation.filtering.jacobianVelocityReachabilityFilter.thresholdOrientationReachedRadian = 0.10000000000000001
+
+
+# ArmarX.contractor.config.validation.filtering.jacobianVelocityReachabilityFilter.thresholdPositionReachedMeter:
+# Attributes:
+# - Default: 0.014999999999999999
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.contractor.config.validation.filtering.jacobianVelocityReachabilityFilter.thresholdPositionReachedMeter = 0.014999999999999999
+
+
+# ArmarX.contractor.config.validation.filtering.jacobianVelocityReachabilityFilter.useAnalyticalQuaternionJacobian:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.contractor.config.validation.filtering.jacobianVelocityReachabilityFilter.useAnalyticalQuaternionJacobian = true
+
+
# ArmarX.contractor.config.validation.filtering.reachabilityFilter.diffIkParams.ikStepLengthFineTune:
# Attributes:
# - Default: 0.5
diff --git a/scenarios/Armar6ManipulationPipeline/config/manipulation_memory.cfg b/scenarios/Armar6ManipulationPipeline/config/manipulation_memory.cfg
index aaa01cad0..298a52d59 100644
--- a/scenarios/Armar6ManipulationPipeline/config/manipulation_memory.cfg
+++ b/scenarios/Armar6ManipulationPipeline/config/manipulation_memory.cfg
@@ -140,7 +140,7 @@
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ManipulationMemory.handTrajectoryPackagePaths = ""
+ArmarX.ManipulationMemory.handTrajectoryPackagePaths = armar6_skills:motions/grasps
# ArmarX.ManipulationMemory.mem.MemoryName: Name of this memory server.
diff --git a/scenarios/Armar6ManipulationPipeline/config/optik_wb_target_generator.cfg b/scenarios/Armar6ManipulationPipeline/config/optik_wb_target_generator.cfg
new file mode 100644
index 000000000..0436ce074
--- /dev/null
+++ b/scenarios/Armar6ManipulationPipeline/config/optik_wb_target_generator.cfg
@@ -0,0 +1,265 @@
+# ==================================================================
+# optik_wb_target_generator properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.AutodiscoverPackages: If enabled, will discover all ArmarX packages based on the environment variables. Otherwise, the `DefaultPackages` and `AdditionalPackages` properties are used.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.AutodiscoverPackages = true
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
+# ArmarX.optik_wb_target_generator.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.optik_wb_target_generator.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.optik_wb_target_generator.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.optik_wb_target_generator.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.optik_wb_target_generator.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.optik_wb_target_generator.EnableProfiling = false
+
+
+# ArmarX.optik_wb_target_generator.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.optik_wb_target_generator.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.optik_wb_target_generator.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.optik_wb_target_generator.ObjectName = ""
+
+
+# ArmarX.optik_wb_target_generator.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.optik_wb_target_generator.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.optik_wb_target_generator.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.optik_wb_target_generator.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.optik_wb_target_generator.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.optik_wb_target_generator.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.optik_wb_target_generator.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.optik_wb_target_generator.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.optik_wb_target_generator.p.visualizeTargetManipulability:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.optik_wb_target_generator.p.visualizeTargetManipulability = false
+
+
+# ArmarX.optik_wb_target_generator.p.visualizeTargetState:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.optik_wb_target_generator.p.visualizeTargetState = false
+
+
diff --git a/scenarios/Armar6ManipulationPipeline/config/placement_around_box.cfg b/scenarios/Armar6ManipulationPipeline/config/placement_around_box.cfg
index ebf415203..7279036b9 100644
--- a/scenarios/Armar6ManipulationPipeline/config/placement_around_box.cfg
+++ b/scenarios/Armar6ManipulationPipeline/config/placement_around_box.cfg
@@ -214,7 +214,7 @@
# Attributes:
# - Case sensitivity: yes
# - Required: yes
-# ArmarX.PlacementAroundBox.p.robotName = ::_NOT_SET_::
+ArmarX.PlacementAroundBox.p.robotName = Armar6
# ArmarX.PlacementAroundBox.p.usePrimitiveApproximationColModel:
diff --git a/scenarios/Armar6Skills/Armar6Skills.scx b/scenarios/Armar6Skills/Armar6Skills.scx
index b549ad6a3..976ef7d82 100644
--- a/scenarios/Armar6Skills/Armar6Skills.scx
+++ b/scenarios/Armar6Skills/Armar6Skills.scx
@@ -1,12 +1,12 @@
<?xml version="1.0" encoding="utf-8"?>
-<scenario name="Armar6Skills" creation="2023-02-13.20:30:43" globalConfigName="./config/global.cfg" package="armar6_skills" deploymentType="local" nodeName="NodeMain">
- <application name="introduction_skill_provider" instance="" package="armar6_skills" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="navigation_skill_provider" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="view_selection_skill_provider" instance="" package="armarx_view_selection" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="articulated_objects_skill_provider" instance="" package="armarx_manipulation" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="Armar6KnownObjectGraspProvider" instance="" package="armar6_skills" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="hand_over_skill_provider" instance="legacy" package="armar6_skills" nodeName="" enabled="false" iceAutoRestart="false"/>
- <application name="hand_over_skill_provider" instance="" package="armarx_human_robot_interaction" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="control_skill_provider" instance="" package="armarx_control" nodeName="" enabled="true" iceAutoRestart="false"/>
+<scenario name="Armar6Skills" creation="2023-02-13.20:30:43" globalConfigName="./config/global.cfg" package="armar6_skills" nodeName="NodeMain">
+ <application name="introduction_skill_provider" instance="" package="armar6_skills" nodeName="" enabled="true"/>
+ <application name="navigation_skill_provider" instance="" package="armarx_navigation" nodeName="" enabled="true"/>
+ <application name="view_selection_skill_provider" instance="" package="armarx_view_selection" nodeName="" enabled="true"/>
+ <application name="articulated_objects_skill_provider" instance="" package="armarx_manipulation" nodeName="" enabled="true"/>
+ <application name="Armar6KnownObjectGraspProvider" instance="" package="armar6_skills" nodeName="" enabled="true"/>
+ <application name="hand_over_skill_provider" instance="legacy" package="armar6_skills" nodeName="" enabled="false"/>
+ <application name="control_skill_provider" instance="" package="armarx_control" nodeName="" enabled="true"/>
+ <application name="human_robot_skill_provider" instance="" package="armarx_human_robot_interaction" nodeName="" enabled="true"/>
</scenario>
diff --git a/scenarios/Armar6Skills/config/control_skill_provider.cfg b/scenarios/Armar6Skills/config/control_skill_provider.cfg
index f3e229c3f..b21ac1149 100644
--- a/scenarios/Armar6Skills/config/control_skill_provider.cfg
+++ b/scenarios/Armar6Skills/config/control_skill_provider.cfg
@@ -169,6 +169,22 @@
# ArmarX.Verbosity = Info
+# ArmarX.control_skill_provider.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.control_skill_provider.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.control_skill_provider.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.control_skill_provider.ArVizTopicName = ArVizTopic
+
+
# ArmarX.control_skill_provider.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
# - Default: false
@@ -225,6 +241,14 @@ ArmarX.control_skill_provider.RobotName = Armar6
ArmarX.control_skill_provider.RobotUnitName = Armar6Unit
+# ArmarX.control_skill_provider.TrajectoryPlayerName: Name of the TrajectoryPlayer
+# Attributes:
+# - Default: TrajectoryPlayer
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.control_skill_provider.TrajectoryPlayerName = TrajectoryPlayer
+
+
# ArmarX.control_skill_provider.cmp.KinematicUnit: Ice object name of the `KinematicUnit` component.
# Attributes:
# - Default: KinematicUnit
diff --git a/scenarios/Armar6Skills/config/hand_over_skill_provider.cfg b/scenarios/Armar6Skills/config/human_robot_skill_provider.cfg
similarity index 74%
rename from scenarios/Armar6Skills/config/hand_over_skill_provider.cfg
rename to scenarios/Armar6Skills/config/human_robot_skill_provider.cfg
index 83d8f6668..7ddc2dde3 100644
--- a/scenarios/Armar6Skills/config/hand_over_skill_provider.cfg
+++ b/scenarios/Armar6Skills/config/human_robot_skill_provider.cfg
@@ -1,5 +1,5 @@
# ==================================================================
-# hand_over_skill_provider properties
+# human_robot_skill_provider properties
# ==================================================================
# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
@@ -169,135 +169,126 @@
# ArmarX.Verbosity = Info
-# ArmarX.hand_over_skill_provider.ArVizStorageName: Name of the ArViz storage
+# ArmarX.human_robot_skill_provider.ArVizStorageName: Name of the ArViz storage
# Attributes:
# - Default: ArVizStorage
# - Case sensitivity: yes
# - Required: no
-# ArmarX.hand_over_skill_provider.ArVizStorageName = ArVizStorage
+# ArmarX.human_robot_skill_provider.ArVizStorageName = ArVizStorage
-# ArmarX.hand_over_skill_provider.ArVizTopicName: Name of the ArViz topic
+# ArmarX.human_robot_skill_provider.ArVizTopicName: Name of the ArViz topic
# Attributes:
# - Default: ArVizTopic
# - Case sensitivity: yes
# - Required: no
-# ArmarX.hand_over_skill_provider.ArVizTopicName = ArVizTopic
+# ArmarX.human_robot_skill_provider.ArVizTopicName = ArVizTopic
-# ArmarX.hand_over_skill_provider.EnableProfiling: enable profiler which is used for logging performance events
+# ArmarX.human_robot_skill_provider.DebugObserverTopicName: Name of the topic the DebugObserver listens on
# Attributes:
-# - Default: false
+# - Default: DebugObserver
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.hand_over_skill_provider.EnableProfiling = false
+# ArmarX.human_robot_skill_provider.DebugObserverTopicName = DebugObserver
-# ArmarX.hand_over_skill_provider.MinimumLoggingLevel: Local logging level only for this component
+# ArmarX.human_robot_skill_provider.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
-# - Default: Undefined
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.hand_over_skill_provider.MinimumLoggingLevel = Undefined
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.human_robot_skill_provider.EnableProfiling = false
-# ArmarX.hand_over_skill_provider.ObjectName: Name of IceGrid well-known object
+# ArmarX.human_robot_skill_provider.HandControlSkillProviderName:
# Attributes:
-# - Default: ""
+# - Default: control_skill_provider
# - Case sensitivity: yes
# - Required: no
-# ArmarX.hand_over_skill_provider.ObjectName = ""
+# ArmarX.human_robot_skill_provider.HandControlSkillProviderName = control_skill_provider
-# ArmarX.hand_over_skill_provider.RobotName:
+# ArmarX.human_robot_skill_provider.LeftHandUnit: Ice object name of the `HandUnit` component.
# Attributes:
-# - Default: Armar7
+# - Default: LeftHandUnit
# - Case sensitivity: yes
# - Required: no
-ArmarX.hand_over_skill_provider.RobotName = Armar6
+# ArmarX.human_robot_skill_provider.LeftHandUnit = LeftHandUnit
-# ArmarX.hand_over_skill_provider.cmp.RobotUnit: Ice object name of the `RobotUnit` component.
+# ArmarX.human_robot_skill_provider.MinimumLoggingLevel: Local logging level only for this component
# Attributes:
-# - Default: Armar6Unit
+# - Default: Undefined
# - Case sensitivity: yes
# - Required: no
-# ArmarX.hand_over_skill_provider.cmp.RobotUnit = Armar6Unit
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.human_robot_skill_provider.MinimumLoggingLevel = Undefined
-# ArmarX.hand_over_skill_provider.handControlSkillProviderName:
+# ArmarX.human_robot_skill_provider.ObjectName: Name of IceGrid well-known object
# Attributes:
-# - Default: control_skill_provider
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.hand_over_skill_provider.handControlSkillProviderName = control_skill_provider
+# ArmarX.human_robot_skill_provider.ObjectName = ""
-# ArmarX.hand_over_skill_provider.handoverForceThresholdLeft:
+# ArmarX.human_robot_skill_provider.RightHandUnit: Ice object name of the `HandUnit` component.
# Attributes:
-# - Default: 15
+# - Default: RightHandUnit
# - Case sensitivity: yes
# - Required: no
-# ArmarX.hand_over_skill_provider.handoverForceThresholdLeft = 15
+# ArmarX.human_robot_skill_provider.RightHandUnit = RightHandUnit
-# ArmarX.hand_over_skill_provider.handoverForceThresholdRight:
+# ArmarX.human_robot_skill_provider.RobotName:
# Attributes:
-# - Default: 15
# - Case sensitivity: yes
-# - Required: no
-# ArmarX.hand_over_skill_provider.handoverForceThresholdRight = 15
+# - Required: yes
+ArmarX.human_robot_skill_provider.RobotName = Armar6
-# ArmarX.hand_over_skill_provider.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
-# Set to false to use this memory as a stand-alone.
+# ArmarX.human_robot_skill_provider.RobotUnitName: Ice object name of the `RobotUnit` component.
# Attributes:
-# - Default: true
+# - Default: RobotUnit
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.hand_over_skill_provider.mns.MemoryNameSystemEnabled = true
+ArmarX.human_robot_skill_provider.RobotUnitName = Armar6Unit
-# ArmarX.hand_over_skill_provider.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# ArmarX.human_robot_skill_provider.ViewSelectionSkillProviderName:
# Attributes:
-# - Default: MemoryNameSystem
+# - Default: view_selection_skill_provider
# - Case sensitivity: yes
# - Required: no
-# ArmarX.hand_over_skill_provider.mns.MemoryNameSystemName = MemoryNameSystem
+# ArmarX.human_robot_skill_provider.ViewSelectionSkillProviderName = view_selection_skill_provider
-# ArmarX.hand_over_skill_provider.receiveForceThresholdLeft:
+# ArmarX.human_robot_skill_provider.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
# Attributes:
-# - Default: 15
+# - Default: true
# - Case sensitivity: yes
# - Required: no
-# ArmarX.hand_over_skill_provider.receiveForceThresholdLeft = 15
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.human_robot_skill_provider.mns.MemoryNameSystemEnabled = true
-# ArmarX.hand_over_skill_provider.receiveForceThresholdRight:
+# ArmarX.human_robot_skill_provider.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
# Attributes:
-# - Default: 15
+# - Default: MemoryNameSystem
# - Case sensitivity: yes
# - Required: no
-# ArmarX.hand_over_skill_provider.receiveForceThresholdRight = 15
+# ArmarX.human_robot_skill_provider.mns.MemoryNameSystemName = MemoryNameSystem
-# ArmarX.hand_over_skill_provider.skill.SkillManager: The name of the SkillManager (or SkillMemory) proxy this provider belongs to.
+# ArmarX.human_robot_skill_provider.skill.SkillManager: The name of the SkillManager (or SkillMemory) proxy this provider belongs to.
# Attributes:
# - Default: SkillMemory
# - Case sensitivity: yes
# - Required: no
-# ArmarX.hand_over_skill_provider.skill.SkillManager = SkillMemory
-
-
-# ArmarX.hand_over_skill_provider.viewSelectionSkillProviderName:
-# Attributes:
-# - Default: view_selection_skill_provider
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.hand_over_skill_provider.viewSelectionSkillProviderName = view_selection_skill_provider
+# ArmarX.human_robot_skill_provider.skill.SkillManager = SkillMemory
diff --git a/scenarios/Armar6Skills/config/view_selection_skill_provider.cfg b/scenarios/Armar6Skills/config/view_selection_skill_provider.cfg
index 58a732978..ee9ce404a 100644
--- a/scenarios/Armar6Skills/config/view_selection_skill_provider.cfg
+++ b/scenarios/Armar6Skills/config/view_selection_skill_provider.cfg
@@ -235,6 +235,14 @@ ArmarX.ViewSelectionSkillProvider.RobotUnitName = Armar6Unit
# ArmarX.view_selection_skill_provider.RemoteStateComponentName = RobotStateComponent
+# ArmarX.view_selection_skill_provider.cmp.GazeScheduler: Ice object name of the `GazeScheduler` component.
+# Attributes:
+# - Default: GazeScheduler
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.view_selection_skill_provider.cmp.GazeScheduler = GazeScheduler
+
+
# ArmarX.view_selection_skill_provider.cmp.KinematicUnit: Ice object name of the `KinematicUnit` component.
# Attributes:
# - Default: KinematicUnit
@@ -243,6 +251,14 @@ ArmarX.ViewSelectionSkillProvider.RobotUnitName = Armar6Unit
ArmarX.view_selection_skill_provider.cmp.KinematicUnit = Armar6KinematicUnit
+# ArmarX.view_selection_skill_provider.mem.obj.instance.MemoryName:
+# Attributes:
+# - Default: Object
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.view_selection_skill_provider.mem.obj.instance.MemoryName = Object
+
+
# ArmarX.view_selection_skill_provider.mem.robot_state.Memory:
# Attributes:
# - Default: RobotState
@@ -309,6 +325,22 @@ ArmarX.view_selection_skill_provider.p.KinematicUnit = Armar6KinematicUnit
# ArmarX.view_selection_skill_provider.p.RobotName = Armar6
+# ArmarX.view_selection_skill_provider.p.scanLocationsForObject.subSkillIDs.lookAtArticulatedObjectFrame->providerName:
+# Attributes:
+# - Default: view_selection_skill_provider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.view_selection_skill_provider.p.scanLocationsForObject.subSkillIDs.lookAtArticulatedObjectFrame->providerName = view_selection_skill_provider
+
+
+# ArmarX.view_selection_skill_provider.p.scanLocationsForObject.subSkillIDs.lookAtArticulatedObjectFrame.skillName:
+# Attributes:
+# - Default: LookAtArticulatedObjectFrame
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.view_selection_skill_provider.p.scanLocationsForObject.subSkillIDs.lookAtArticulatedObjectFrame.skillName = LookAtArticulatedObjectFrame
+
+
# ArmarX.view_selection_skill_provider.skill.SkillManager: The name of the SkillManager (or SkillMemory) proxy this provider belongs to.
# Attributes:
# - Default: SkillMemory
diff --git a/scenarios/CeBITTasks/CeBITTasks.scx b/scenarios/CeBITTasks/CeBITTasks.scx
index 5387e37da..3cd3be702 100644
--- a/scenarios/CeBITTasks/CeBITTasks.scx
+++ b/scenarios/CeBITTasks/CeBITTasks.scx
@@ -1,53 +1,53 @@
<?xml version="1.0" encoding="utf-8"?>
-<scenario name="CeBITTasks" creation="2018-05-15.15:05:44" globalConfigName="./config/global.cfg" package="armar6_skills" deploymentType="local" nodeName="NodeMain">
- <application name="MemoryXDomainGeneratorApp" instance="" package="Spoac" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="SpoacExecutiveApp" instance="" package="Spoac" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="StatechartExecutorApp" instance="" package="Spoac" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="SimplePlanningSymbolsUpdaterApp" instance="" package="Spoac" nodeName="" enabled="false" iceAutoRestart="false"/>
- <application name="GraphNodePoseResolverApp" instance="" package="MemoryX" nodeName="" enabled="false" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="PlatformGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="Armar6HighlevelTaskGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="ActionGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="MotionControlGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="WorldStateObserver" instance="" package="MemoryX" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="Armar6BoardSupportDemo" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="Armar6CleanUpTaskGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="Armar6DMPControlGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="Armar6FollowMeDemo" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="Armar6GraspingGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="Armar6GuardSupportDemoGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="Armar6LowLevelHandControlGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="Armar6NavigationGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="Armar6Poses" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="Armar6PresentationGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="HandOverGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="GazeControlGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="HandGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="MemoryXUtility" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="ObjectLocalization" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="ScanLocationGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="SpeechObserverApp" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
- <application name="DummyTextToSpeechApp" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="CoreUtility" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="Armar6Arms" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="ForceTorqueUtility" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="ForceControlGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="RTMotionControlGoup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="Armar6WipingDemoGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="Armar6FTHandControlGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="DynamicPlatformObstacleAvoidanceGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="DSControllerGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="Armar6HeadGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="Armar6VisualGraspingGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="GraspCandidateObserverApp" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
- <application name="Armar6GraspProvider" instance="" package="armar6_skills" nodeName="" enabled="false" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="Armar6SearchAction" package="ArmarXCore" nodeName="" enabled="false" iceAutoRestart="false"/>
- <application name="Armar6DemoConnector" instance="" package="armar6_skills" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="Armar6BimanualManipulationGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="LookAtHand" instance="" package="armar6_skills" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="NavigationCommands" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="NavigationGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="NavigateToLocationGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="XMLRemoteStateOfferer" instance="ObjectMemoryGroup" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
+<scenario name="CeBITTasks" creation="2018-05-15.15:05:44" globalConfigName="./config/global.cfg" package="armar6_skills" nodeName="NodeMain">
+ <application name="MemoryXDomainGeneratorApp" instance="" package="Spoac" nodeName="" enabled="true"/>
+ <application name="SpoacExecutiveApp" instance="" package="Spoac" nodeName="" enabled="true"/>
+ <application name="StatechartExecutorApp" instance="" package="Spoac" nodeName="" enabled="true"/>
+ <application name="SimplePlanningSymbolsUpdaterApp" instance="" package="Spoac" nodeName="" enabled="false"/>
+ <application name="GraphNodePoseResolverApp" instance="" package="MemoryX" nodeName="" enabled="false"/>
+ <application name="XMLRemoteStateOfferer" instance="PlatformGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="Armar6HighlevelTaskGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="ActionGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="MotionControlGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="WorldStateObserver" instance="" package="MemoryX" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="Armar6BoardSupportDemo" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="Armar6CleanUpTaskGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="Armar6DMPControlGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="Armar6FollowMeDemo" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="Armar6GraspingGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="Armar6GuardSupportDemoGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="Armar6LowLevelHandControlGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="Armar6NavigationGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="Armar6Poses" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="Armar6PresentationGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="HandOverGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="GazeControlGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="HandGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="MemoryXUtility" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="ObjectLocalization" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="ScanLocationGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="SpeechObserverApp" instance="" package="RobotAPI" nodeName="" enabled="false"/>
+ <application name="DummyTextToSpeechApp" instance="" package="RobotAPI" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="CoreUtility" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="Armar6Arms" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="ForceTorqueUtility" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="ForceControlGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="RTMotionControlGoup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="Armar6WipingDemoGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="Armar6FTHandControlGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="DynamicPlatformObstacleAvoidanceGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="DSControllerGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="Armar6HeadGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="Armar6VisualGraspingGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="GraspCandidateObserverApp" instance="" package="RobotAPI" nodeName="" enabled="false"/>
+ <application name="Armar6GraspProvider" instance="" package="armar6_skills" nodeName="" enabled="false"/>
+ <application name="XMLRemoteStateOfferer" instance="Armar6SearchAction" package="ArmarXCore" nodeName="" enabled="false"/>
+ <application name="Armar6DemoConnector" instance="" package="armar6_skills" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="Armar6BimanualManipulationGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="LookAtHand" instance="" package="armar6_skills" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="NavigationCommands" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="NavigationGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="NavigateToLocationGroup" package="ArmarXCore" nodeName="" enabled="true"/>
+ <application name="XMLRemoteStateOfferer" instance="ObjectMemoryGroup" package="ArmarXCore" nodeName="" enabled="true"/>
</scenario>
diff --git a/scenarios/HumanAvoidance/HumanAvoidance.scx b/scenarios/HumanAvoidance/HumanAvoidance.scx
index d4226def1..7da648e98 100644
--- a/scenarios/HumanAvoidance/HumanAvoidance.scx
+++ b/scenarios/HumanAvoidance/HumanAvoidance.scx
@@ -1,10 +1,10 @@
<?xml version="1.0" encoding="utf-8"?>
-<scenario name="HumanAvoidance" creation="2020-02-24.08:39:11 AM" globalConfigName="./config/global.cfg" package="armar6_skills" deploymentType="local" nodeName="NodeMain">
- <application name="DSObstacleAvoidance" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="HumanObstacleDetectionApp" instance="" package="RobotComponents" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="LaserScannerObstacleDetection" instance="" package="RobotComponents" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="DynamicObstacleManager" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="ObstacleAvoidingPlatformUnit" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
- <application name="ObstacleAwarePlatformUnit" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+<scenario name="HumanAvoidance" creation="2020-02-24.08:39:11 AM" globalConfigName="./config/global.cfg" package="armar6_skills" nodeName="NodeMain">
+ <application name="DSObstacleAvoidance" instance="" package="RobotAPI" nodeName="" enabled="true"/>
+ <application name="HumanObstacleDetectionApp" instance="" package="RobotComponents" nodeName="" enabled="true"/>
+ <application name="LaserScannerObstacleDetection" instance="" package="RobotComponents" nodeName="" enabled="true"/>
+ <application name="DynamicObstacleManager" instance="" package="RobotAPI" nodeName="" enabled="true"/>
+ <application name="ObstacleAvoidingPlatformUnit" instance="" package="RobotAPI" nodeName="" enabled="false"/>
+ <application name="ObstacleAwarePlatformUnit" instance="" package="RobotAPI" nodeName="" enabled="true"/>
</scenario>
--
GitLab
From 137202ab248dbf6b2d664d0e647a611824a0c719 Mon Sep 17 00:00:00 2001
From: Jianfeng Gao <jianfeng.gao@kit.edu>
Date: Tue, 30 Jul 2024 19:21:05 +0200
Subject: [PATCH 09/22] integrating guardcarrying skills in the SH demo
---
.../Armar6BoardSupportDemo/CarryAndRegrasp.xml | 8 ++++++--
.../Armar6BoardSupportDemo/GuardCarryingSkill.cpp | 8 ++++----
.../Armar6BoardSupportDemo/GuardCarryingSkill.xml | 12 +++++++++---
.../GuardCarryingWithFailureDetection.xml | 2 +-
.../Armar6BoardSupportDemo/InsertGuardV2.xml | 2 +-
.../testGuardCarryingSkill.xml | 10 ++++++++--
6 files changed, 29 insertions(+), 13 deletions(-)
diff --git a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/CarryAndRegrasp.xml b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/CarryAndRegrasp.xml
index 25842ee7a..9dcfbc6e7 100644
--- a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/CarryAndRegrasp.xml
+++ b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/CarryAndRegrasp.xml
@@ -28,8 +28,12 @@
<DefaultValue profile="Armar6Real" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::DoubleVariantData","value":2}}' docValue="2"/>
<DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::DoubleVariantData","value":1}}' docValue="1"/>
</Parameter>
- <Parameter name="DesiredNullspaceAngleLeft" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="yes"/>
- <Parameter name="DesiredNullspaceAngleRight" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="yes"/>
+ <Parameter name="DesiredNullspaceAngleLeft" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="yes">
+ <DefaultValue profile="Armar6Real" value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.157}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":-0.076}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.542}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.511}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1.681}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":-0.589}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.379}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.149}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="0.157000005245209\n-0.0759999975562096\n0.541999995708466\n0.510999977588654\n1.68099999427795\n-0.58899998664856\n0.379000008106232\n0.149000003933907"/>
+ </Parameter>
+ <Parameter name="DesiredNullspaceAngleRight" type="::armarx::SingleTypeVariantListBase(::armarx::FloatVariantData)" docType="List(float)" optional="yes">
+ <DefaultValue profile="Armar6Real" value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":-0.095}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.0781}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.4866}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":-0.407}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1.6170}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.5255}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":-0.349}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.0389}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="-0.0949999988079071\n0.0781000033020973\n0.486600011587143\n-0.407000005245209\n1.61699998378754\n0.525499999523163\n-0.349000006914139\n0.0388999991118908"/>
+ </Parameter>
<Parameter name="FailureRecoverAfterLoweringLeftOffset" type="::armarx::Vector3Base" docType="Vector3" optional="no">
<DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::Vector3Base","value":{"x":0.0,"y":-20.0,"z":0.0}}}' docValue="0\n-20\n0"/>
</Parameter>
diff --git a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.cpp b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.cpp
index 20d2decfc..6e3ecfd4a 100644
--- a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.cpp
+++ b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.cpp
@@ -91,10 +91,10 @@ namespace armarx::Armar6BoardSupportDemo
platformControlConfigParams.dAngle = 0;
platformControlConfigParams.dPos = 0;
platformControlConfigParams.filterTimeConstant = 0;
- platformControlConfigParams.kAngle = 8;
- platformControlConfigParams.kPos = 12;
- platformControlConfigParams.maxAngleVel = 0;
- platformControlConfigParams.maxPosVel = 0;
+ platformControlConfigParams.kAngle = in.getPlatformKAngle();
+ platformControlConfigParams.kPos = in.getPlatformKPosition();
+ platformControlConfigParams.maxAngleVel = 1.25;
+ platformControlConfigParams.maxPosVel = 750;
platformControlConfigParams.minAngleTolerance = 0;
platformControlConfigParams.minPosTolerance = 0;
platformControlConfigParams.nodeSetNameLeft = "LeftArm";
diff --git a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.xml b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.xml
index 6df90c189..6eb5a849a 100644
--- a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.xml
+++ b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingSkill.xml
@@ -19,15 +19,22 @@
<DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":false}}' docValue="False"/>
</Parameter>
<Parameter name="LeftThumbThreshold" type="::armarx::FloatVariantData" docType="float" optional="yes">
- <DefaultValue profile="Armar6Real" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1.1}}' docValue="1.10000002384186"/>
<DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.8}}' docValue="0.800000011920929"/>
</Parameter>
+ <Parameter name="PlatformKAngle" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue profile="Armar6Real" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":15}}' docValue="15"/>
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":12}}' docValue="12"/>
+ </Parameter>
+ <Parameter name="PlatformKPosition" type="::armarx::FloatVariantData" docType="float" optional="no">
+ <DefaultValue profile="Armar6Real" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":24}}' docValue="24"/>
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":18}}' docValue="18"/>
+ </Parameter>
<Parameter name="PlatformTargetPose" type="::armarx::StringValueMapBase(::armarx::Vector3Base)" docType="Map(Vector3)" optional="no"/>
<Parameter name="RestrictAngle" type="::armarx::FloatVariantData" docType="float" optional="no">
<DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":45}}' docValue="45"/>
</Parameter>
<Parameter name="RestrictMaxAngularVelocity" type="::armarx::FloatVariantData" docType="float" optional="no">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.2}}' docValue="0.200000002980232"/>
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.07}}' docValue="0.0700000002980232"/>
</Parameter>
<Parameter name="RestrictMaxVelocity" type="::armarx::FloatVariantData" docType="float" optional="no">
<DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":300}}' docValue="300"/>
@@ -36,7 +43,6 @@
<DefaultValue value='{"map":{"GuardPlacement":{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::MatrixFloatBase","value":{"cols":2,"rowContent":["-400, -700","400, -700","400, 700","-400, 700"],"rows":4}}},"GuardLifting":{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::MatrixFloatBase","value":{"cols":2,"rowContent":["-150.0, -350","150.0, -350","150, 350","-150, 350"],"rows":4}}}},"type":"::armarx::StringValueMapBase"}' docValue="GuardLifting: -150 -350\n 150 -350\n 150 350\n-150 350\nGuardPlacement: -400 -700\n 400 -700\n 400 700\n-400 700\n"/>
</Parameter>
<Parameter name="RightThumbThreshold" type="::armarx::FloatVariantData" docType="float" optional="yes">
- <DefaultValue profile="Armar6Real" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1.1}}' docValue="1.10000002384186"/>
<DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":0.8}}' docValue="0.800000011920929"/>
</Parameter>
<Parameter name="TextAfterWaitForZeroTorque" type="::armarx::StringVariantData" docType="string" optional="no">
diff --git a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingWithFailureDetection.xml b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingWithFailureDetection.xml
index 38c7ae303..f221d7da6 100644
--- a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingWithFailureDetection.xml
+++ b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/GuardCarryingWithFailureDetection.xml
@@ -44,7 +44,7 @@
<Parameter name="PlatformTargetPose" type="::armarx::StringValueMapBase(::armarx::Vector3Base)" docType="Map(Vector3)" optional="no"/>
</LocalParameters>
<Substates>
- <EndState name="Failure" event="Failure" left="943.444" top="420.167" boundingSquareSize="99.6636"/>
+ <EndState name="Failure" event="Failure" left="943.28" top="420.167" boundingSquareSize="99.6636"/>
<LocalState name="GuardCarryingSkill" refuuid="30629B08-477C-4EC0-8150-549910532B42" left="387.611" top="495.5" boundingSquareSize="99.6636"/>
<LocalState name="GuardCarryingWithObstacleAvoidance" refuuid="9D06CC13-22D7-4C19-ACBE-A50B628CA63C" left="363.611" top="188.889" boundingSquareSize="148.221"/>
<LocalState name="RegraspingFailureDetection" refuuid="90F3C6E6-08AA-4924-A787-953A1062DE6A" left="701.222" top="250.222" boundingSquareSize="104.779"/>
diff --git a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/InsertGuardV2.xml b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/InsertGuardV2.xml
index 2b5425105..feba701dd 100644
--- a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/InsertGuardV2.xml
+++ b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/InsertGuardV2.xml
@@ -31,7 +31,7 @@
<Parameter name="Value" type="::armarx::IntVariantData" docType="int" optional="no"/>
</LocalParameters>
<Substates>
- <LocalState name="CarryAndRegrasp" refuuid="181CF11B-48EF-440B-953A-A7C204A16995" left="1070" top="380.333" boundingSquareSize="100.002"/>
+ <LocalState name="CarryAndRegrasp" refuuid="181CF11B-48EF-440B-953A-A7C204A16995" left="1069.86" top="407.356" boundingSquareSize="100.002"/>
<LocalState name="DummyState" refuuid="66424431-C806-4E7D-B288-2AC1341D6F48" left="880" top="363.333" boundingSquareSize="100.002"/>
<EndState name="Failure" event="Failure" left="1863.33" top="290.333" boundingSquareSize="100.002"/>
<LocalState name="GoToPickUp" refuuid="2EFBD81F-D1D8-4283-9065-3EC47ABA7803" left="483.333" top="252.167" boundingSquareSize="100.002"/>
diff --git a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/testGuardCarryingSkill.xml b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/testGuardCarryingSkill.xml
index 9b015dc28..02dbaf397 100644
--- a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/testGuardCarryingSkill.xml
+++ b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/testGuardCarryingSkill.xml
@@ -4,10 +4,10 @@
<OutputParameters/>
<LocalParameters/>
<Substates>
- <EndState name="Failure" event="Failure" left="652.111" top="140" boundingSquareSize="99.6636"/>
+ <EndState name="Failure" event="Failure" left="651.944" top="140" boundingSquareSize="99.6636"/>
<LocalState name="GuardCarryingSkill" refuuid="30629B08-477C-4EC0-8150-549910532B42" left="363.333" top="238.028" boundingSquareSize="99.6636"/>
<RemoteState name="ResolveLandmark" refuuid="68599B94-C644-4023-969B-486210BEFD4A" proxyName="PlatformGroupRemoteStateOfferer" left="114.167" top="157.583" boundingSquareSize="99.6636"/>
- <EndState name="Success" event="Success" left="652.111" top="266.194" boundingSquareSize="99.6636"/>
+ <EndState name="Success" event="Success" left="651.944" top="266.194" boundingSquareSize="99.6636"/>
</Substates>
<Events>
<Event name="Failure">
@@ -50,7 +50,13 @@
<ParameterMapping sourceType="Value" from="" to="IsGuardPlacement">
<DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":true}}'/>
</ParameterMapping>
+ <ParameterMapping sourceType="Value" from="" to="LeftThumbThreshold">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1.1}}'/>
+ </ParameterMapping>
<ParameterMapping sourceType="Output" from="LandmarkPoseMap" to="PlatformTargetPose"/>
+ <ParameterMapping sourceType="Value" from="" to="RightThumbThreshold">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::FloatVariantData","value":1.1}}'/>
+ </ParameterMapping>
</ParameterMappings>
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
--
GitLab
From a0d9d68496e2075a49c4f69cb92abc31e07f8cbb Mon Sep 17 00:00:00 2001
From: Jianfeng Gao <jianfeng.gao@kit.edu>
Date: Tue, 30 Jul 2024 19:21:15 +0200
Subject: [PATCH 10/22] update ladder demo param
---
.../Armar6BimanualManipulationGroup/TransportLadderV2.xml | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/source/armar6/skills/statecharts/Armar6BimanualManipulationGroup/TransportLadderV2.xml b/source/armar6/skills/statecharts/Armar6BimanualManipulationGroup/TransportLadderV2.xml
index ffb0f6ae7..e55b46dd0 100644
--- a/source/armar6/skills/statecharts/Armar6BimanualManipulationGroup/TransportLadderV2.xml
+++ b/source/armar6/skills/statecharts/Armar6BimanualManipulationGroup/TransportLadderV2.xml
@@ -126,14 +126,14 @@
<DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"/home/armar-user/armar6_motion/BimanualTransportation/taskspace-trajectory-BoxApproachingForBimanualGraspingLeft-constant-orientation.csv"}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="/home/armar-user/armar6_motion/BimanualTransportation/taskspace-trajectory-BoxApproachingForBimanualGraspingLeft-constant-orientation.csv"/>
</Parameter>
<Parameter name="LeftApproachingTcpGoalPose" type="::armarx::PoseBase" docType="Pose" optional="no">
- <DefaultValue profile="Armar6Real" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::PoseBase","value":{"qw":0.71025276184082031,"qx":-0.70393157005310059,"qy":-0.0033346370328217745,"qz":-0.0032220622524619102,"x":-100,"y":715.0,"z":990}}}' docValue=" 0.999957 0.00927167 -0.000200648 -100\n 0.000117755 0.00893992 0.99996 715\n 0.00927309 -0.999917 0.00893843 990\n 0 0 0 1"/>
+ <DefaultValue profile="Armar6Real" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::PoseBase","value":{"qw":0.71025276184082031,"qx":-0.70393157005310059,"qy":-0.0033346370328217745,"qz":-0.0032220622524619102,"x":-50,"y":715.0,"z":990}}}' docValue=" 0.999957 0.00927167 -0.000200648 -50\n 0.000117755 0.00893992 0.99996 715\n 0.00927309 -0.999917 0.00893843 990\n 0 0 0 1"/>
<DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::PoseBase","value":{"qw":0.71025276184082031,"qx":-0.70393157005310059,"qy":-0.0033346370328217745,"qz":-0.0032220622524619102,"x":-300,"y":700.0,"z":1000}}}' docValue=" 0.999957 0.00927167 -0.000200648 -300\n 0.000117755 0.00893992 0.99996 700\n 0.00927309 -0.999917 0.00893843 1000\n 0 0 0 1"/>
</Parameter>
<Parameter name="LeftNavigationPose" type="::armarx::PoseBase" docType="Pose" optional="no">
<DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::PoseBase","value":{"qw":-0.34914538264274597,"qx":0.61254483461380005,"qy":0.64365655183792114,"qz":0.29764541983604431,"x":-424.730224609375,"y":311.7841796875,"z":605.43731689453125}}}' docValue="-0.00577307 0.99638 -0.0848171 -424.73\n 0.580694 0.072392 0.810897 311.784\n 0.814102 -0.0445715 -0.57901 605.437\n 0 0 0 1"/>
</Parameter>
<Parameter name="LeftViaPose" type="::armarx::StringValueMapBase(::armarx::PoseBase)" docType="Map(Pose)" optional="yes">
- <DefaultValue profile="Armar6Real" value='{"map":{"0.2":{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::PoseBase","value":{"qw":0.71025276184082031,"qx":-0.70393157005310059,"qy":-0.0033346370328217745,"qz":-0.0032220622524619102,"x":-300,"y":715.0,"z":990}}}},"type":"::armarx::StringValueMapBase"}' docValue="0.2: 0.999957 0.00927167 -0.000200648 -300\n 0.000117755 0.00893992 0.99996 715\n 0.00927309 -0.999917 0.00893843 990\n 0 0 0 1\n"/>
+ <DefaultValue profile="Armar6Real" value='{"map":{"0.3":{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::PoseBase","value":{"qw":0.71025276184082031,"qx":-0.70393157005310059,"qy":-0.0033346370328217745,"qz":-0.0032220622524619102,"x":-300,"y":715.0,"z":990}}}},"type":"::armarx::StringValueMapBase"}' docValue="0.3: 0.999957 0.00927167 -0.000200648 -300\n 0.000117755 0.00893992 0.99996 715\n 0.00927309 -0.999917 0.00893843 990\n 0 0 0 1\n"/>
<DefaultValue value='{"map":{"0.2":{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::PoseBase","value":{"qw":0.71025276184082031,"qx":-0.70393157005310059,"qy":-0.0033346370328217745,"qz":-0.0032220622524619102,"x":-300,"y":710.0,"z":1000}}}},"type":"::armarx::StringValueMapBase"}' docValue="0.2: 0.999957 0.00927167 -0.000200648 -300\n 0.000117755 0.00893992 0.99996 710\n 0.00927309 -0.999917 0.00893843 1000\n 0 0 0 1\n"/>
</Parameter>
<Parameter name="MassLeft" type="::armarx::FloatVariantData" docType="float" optional="no">
--
GitLab
From 681a2ceeba7a972d51cc0039b3130c4f692bd2cf Mon Sep 17 00:00:00 2001
From: Jianfeng Gao <jianfeng.gao@kit.edu>
Date: Tue, 30 Jul 2024 19:21:40 +0200
Subject: [PATCH 11/22] add transport skill provider, change verbosity level
---
.../config/RobotStateMemory.cfg | 2 +-
.../config/gaze_scheduler.cfg | 2 +-
scenarios/Armar6Skills/Armar6Skills.scx | 1 +
.../bimanual_transport_skill_provider.cfg | 253 ++++++++++++++++++
4 files changed, 256 insertions(+), 2 deletions(-)
create mode 100644 scenarios/Armar6Skills/config/bimanual_transport_skill_provider.cfg
diff --git a/scenarios/Armar6HighlevelApps/config/RobotStateMemory.cfg b/scenarios/Armar6HighlevelApps/config/RobotStateMemory.cfg
index 313206002..762e9faf5 100644
--- a/scenarios/Armar6HighlevelApps/config/RobotStateMemory.cfg
+++ b/scenarios/Armar6HighlevelApps/config/RobotStateMemory.cfg
@@ -440,6 +440,6 @@ ArmarX.RobotStateMemory.mem.visu.forceTorque.enabled = false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-ArmarX.Verbosity = Verbose
+# ArmarX.Verbosity = Info
diff --git a/scenarios/Armar6HighlevelApps/config/gaze_scheduler.cfg b/scenarios/Armar6HighlevelApps/config/gaze_scheduler.cfg
index 7f6f4d0e4..603664d45 100644
--- a/scenarios/Armar6HighlevelApps/config/gaze_scheduler.cfg
+++ b/scenarios/Armar6HighlevelApps/config/gaze_scheduler.cfg
@@ -301,6 +301,6 @@ ArmarX.GazeScheduler.properties.visuEnabled = false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-ArmarX.Verbosity = Verbose
+# ArmarX.Verbosity = Info
diff --git a/scenarios/Armar6Skills/Armar6Skills.scx b/scenarios/Armar6Skills/Armar6Skills.scx
index 976ef7d82..607e50138 100644
--- a/scenarios/Armar6Skills/Armar6Skills.scx
+++ b/scenarios/Armar6Skills/Armar6Skills.scx
@@ -8,5 +8,6 @@
<application name="hand_over_skill_provider" instance="legacy" package="armar6_skills" nodeName="" enabled="false"/>
<application name="control_skill_provider" instance="" package="armarx_control" nodeName="" enabled="true"/>
<application name="human_robot_skill_provider" instance="" package="armarx_human_robot_interaction" nodeName="" enabled="true"/>
+ <application name="bimanual_transport_skill_provider" instance="" package="armarx_control" nodeName="" enabled="true"/>
</scenario>
diff --git a/scenarios/Armar6Skills/config/bimanual_transport_skill_provider.cfg b/scenarios/Armar6Skills/config/bimanual_transport_skill_provider.cfg
new file mode 100644
index 000000000..32d142e53
--- /dev/null
+++ b/scenarios/Armar6Skills/config/bimanual_transport_skill_provider.cfg
@@ -0,0 +1,253 @@
+# ==================================================================
+# bimanual_transport_skill_provider properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.AutodiscoverPackages: If enabled, will discover all ArmarX packages based on the environment variables. Otherwise, the `DefaultPackages` and `AdditionalPackages` properties are used.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.AutodiscoverPackages = true
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
+# ArmarX.bimanual_transport_skill_provider.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.bimanual_transport_skill_provider.EnableProfiling = false
+
+
+# ArmarX.bimanual_transport_skill_provider.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.bimanual_transport_skill_provider.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.bimanual_transport_skill_provider.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.bimanual_transport_skill_provider.ObjectName = ""
+
+
+# ArmarX.bimanual_transport_skill_provider.RobotUnitName: Name of the RobotUnit
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+ArmarX.bimanual_transport_skill_provider.RobotUnitName = Armar6Unit
+
+
+# ArmarX.bimanual_transport_skill_provider.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.bimanual_transport_skill_provider.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.bimanual_transport_skill_provider.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.bimanual_transport_skill_provider.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.bimanual_transport_skill_provider.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.bimanual_transport_skill_provider.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.bimanual_transport_skill_provider.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.bimanual_transport_skill_provider.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.bimanual_transport_skill_provider.p.robotName:
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+ArmarX.bimanual_transport_skill_provider.p.robotName = Armar6
+
+
+# ArmarX.bimanual_transport_skill_provider.skill.SkillManager: The name of the SkillManager (or SkillMemory) proxy this provider belongs to.
+# Attributes:
+# - Default: SkillMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.bimanual_transport_skill_provider.skill.SkillManager = SkillMemory
+
+
--
GitLab
From 8d2d934d15030e61429b93337641ed77282edd84 Mon Sep 17 00:00:00 2001
From: Jianfeng Gao <jianfeng.gao@kit.edu>
Date: Thu, 19 Dec 2024 18:17:23 +0100
Subject: [PATCH 12/22] remove navigate to home or watch human location in
lower guard and set up ladder skill
---
.../TransportLadderV2.xml | 1359 ++++++++---------
.../Armar6BoardSupportDemo/RemoveGuardV2.xml | 10 +-
2 files changed, 659 insertions(+), 710 deletions(-)
diff --git a/source/armar6/skills/statecharts/Armar6BimanualManipulationGroup/TransportLadderV2.xml b/source/armar6/skills/statecharts/Armar6BimanualManipulationGroup/TransportLadderV2.xml
index e55b46dd0..6d9df6d47 100644
--- a/source/armar6/skills/statecharts/Armar6BimanualManipulationGroup/TransportLadderV2.xml
+++ b/source/armar6/skills/statecharts/Armar6BimanualManipulationGroup/TransportLadderV2.xml
@@ -1,5 +1,5 @@
<?xml version="1.0" encoding="utf-8"?>
-<State version="1.2" name="TransportLadderV2" uuid="E55F9276-3FF8-4556-874E-231868258EDD" width="8676.06" height="2831.61" type="Normal State">
+<State version="1.2" name="TransportLadderV2" uuid="E55F9276-3FF8-4556-874E-231868258EDD" width="7923.44" height="2919.01" type="Normal State">
<InputParameters>
<Parameter name="ArmMotionFileName" type="::armarx::SingleTypeVariantListBase(::armarx::StringVariantData)" docType="List(string)" optional="no">
<DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"/home/armar-user/armar6_motion/Bimanual_test/OriginalBimanualForceControl.csv"}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="/home/armar-user/armar6_motion/Bimanual_test/OriginalBimanualForceControl.csv"/>
@@ -237,43 +237,43 @@
<OutputParameters/>
<LocalParameters/>
<Substates>
- <LocalState name="AddObstacle" refuuid="6473D8D2-EB9E-41E2-BC2A-5781E129831B" left="7363.64" top="289.556" boundingSquareSize="99.6636"/>
- <RemoteState name="CloseBothHands" refuuid="4BFA225C-8FA0-4214-89F3-3D717C2585CF" proxyName="Armar6LowLevelHandControlGroupRemoteStateOfferer" left="4634.94" top="1437.11" boundingSquareSize="99.6636"/>
- <RemoteState name="CloseBothHands_2" refuuid="4BFA225C-8FA0-4214-89F3-3D717C2585CF" proxyName="Armar6LowLevelHandControlGroupRemoteStateOfferer" left="1936.28" top="1154.72" boundingSquareSize="99.6636"/>
- <RemoteState name="CloseRightHand" refuuid="802F09B5-5EB5-48AA-A909-BCEA99EE85CF" proxyName="Armar6LowLevelHandControlGroupRemoteStateOfferer" left="1379.17" top="1034.61" boundingSquareSize="99.6636"/>
- <LocalState name="CreatCtrlAndLift" refuuid="A5F0807D-B3A6-46E8-8831-494AFCE297D2" left="2185.44" top="1186.83" boundingSquareSize="99.6636"/>
- <EndState name="Failure" event="Failure" left="8546.06" top="1558.39" boundingSquareSize="99.6636"/>
- <RemoteState name="GoToGoalPositionWithObstacleAvoidance" refuuid="EB5761E3-D8DF-4B79-82F2-DF7780B3E4CD" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="7331.64" top="1529.5" boundingSquareSize="164.836"/>
- <RemoteState name="GoToGoalPositionWithObstacleAvoidance_2" refuuid="EB5761E3-D8DF-4B79-82F2-DF7780B3E4CD" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="7973.83" top="1378.72" boundingSquareSize="173.779"/>
- <RemoteState name="GoToGoalPositionWithObstacleAvoidance_3" refuuid="EB5761E3-D8DF-4B79-82F2-DF7780B3E4CD" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="7650.56" top="333.389" boundingSquareSize="173.779"/>
- <RemoteState name="GoToPreGraspLandmark" refuuid="EB5761E3-D8DF-4B79-82F2-DF7780B3E4CD" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="1055.94" top="926.278" boundingSquareSize="173.779"/>
- <RemoteState name="GoToTargetPose" refuuid="EB5761E3-D8DF-4B79-82F2-DF7780B3E4CD" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="2684.36" top="2457.78" boundingSquareSize="164.836"/>
- <LocalState name="IndependentBimanualDMPWithForceStop" refuuid="916C9BB6-A81C-495F-B471-B0E0C2F82687" left="1628.72" top="1137.72" boundingSquareSize="158.442"/>
- <RemoteState name="MoveBackToNavigationPose" refuuid="7CEA7DCE-053A-4955-9A1E-077C761FB8B6" proxyName="Armar6BoardSupportDemoRemoteStateOfferer" left="5978" top="1875.11" boundingSquareSize="109.885"/>
- <RemoteState name="MoveJoints" refuuid="1A25B687-CFBC-4CA3-A1C7-32EEAF004C45" proxyName="MotionControlGroupRemoteStateOfferer" left="114.167" top="695.889" boundingSquareSize="99.6636"/>
- <RemoteState name="MovePlatformRelative" refuuid="8B23A512-C8AD-45BF-9B0C-D7E4EC830D5E" proxyName="PlatformGroupRemoteStateOfferer" left="4884.11" top="2018.72" boundingSquareSize="99.6636"/>
- <RemoteState name="MovePlatformRelative_2" refuuid="8B23A512-C8AD-45BF-9B0C-D7E4EC830D5E" proxyName="PlatformGroupRemoteStateOfferer" left="6491.56" top="1747.83" boundingSquareSize="99.6636"/>
- <RemoteState name="MovePlatformRelative_3" refuuid="8B23A512-C8AD-45BF-9B0C-D7E4EC830D5E" proxyName="PlatformGroupRemoteStateOfferer" left="4271.42" top="1450.11" boundingSquareSize="99.6636"/>
- <RemoteState name="MovePlatformToLandmark" refuuid="9AE3BE73-6695-4BEF-B91F-D6B072B04F8B" proxyName="PlatformGroupRemoteStateOfferer" left="1092.94" top="1998.72" boundingSquareSize="100.942"/>
- <RemoteState name="MoveToAfterFinished" refuuid="9AE3BE73-6695-4BEF-B91F-D6B072B04F8B" proxyName="PlatformGroupRemoteStateOfferer" left="8004.83" top="542.611" boundingSquareSize="111.164"/>
- <RemoteState name="NavigationPose" refuuid="E7374741-A6DF-4E3F-B05D-68A1EB44D6A5" proxyName="Armar6PosesRemoteStateOfferer" left="552.444" top="754.556" boundingSquareSize="99.6636"/>
- <RemoteState name="NavigationPose_2" refuuid="E7374741-A6DF-4E3F-B05D-68A1EB44D6A5" proxyName="Armar6PosesRemoteStateOfferer" left="8297.06" top="501.556" boundingSquareSize="99.6636"/>
- <RemoteState name="NavigationPose_3" refuuid="E7374741-A6DF-4E3F-B05D-68A1EB44D6A5" proxyName="Armar6PosesRemoteStateOfferer" left="6832.72" top="1666.89" boundingSquareSize="99.6636"/>
- <RemoteState name="OpenBothHands_2" refuuid="88173D79-82C3-4FE1-ACB6-7296D8AA85CF" proxyName="Armar6LowLevelHandControlGroupRemoteStateOfferer" left="5474.44" top="1948.22" boundingSquareSize="99.6636"/>
- <LocalState name="OpenLadder" refuuid="C9CF6660-F7F8-456D-8F20-D7226E72AD59" left="3745.61" top="2184.5" boundingSquareSize="99.6636"/>
- <RemoteState name="OpenLeftHand" refuuid="06C28E15-ECCE-4330-B149-1F27809605CF" proxyName="Armar6LowLevelHandControlGroupRemoteStateOfferer" left="3496.44" top="2249.78" boundingSquareSize="99.6636"/>
- <LocalState name="OpenPlaceLadder" refuuid="C9CF6660-F7F8-456D-8F20-D7226E72AD59" left="5225.28" top="2010.72" boundingSquareSize="99.6636"/>
- <LocalState name="PrePutDownLadder" refuuid="C9CF6660-F7F8-456D-8F20-D7226E72AD59" left="2998.11" top="2410.67" boundingSquareSize="99.6636"/>
- <LocalState name="PutDownLadder" refuuid="C9CF6660-F7F8-456D-8F20-D7226E72AD59" left="3247.28" top="2335.28" boundingSquareSize="99.6636"/>
- <LocalState name="RemoveObstacle" refuuid="46B82788-5777-4DAA-917B-E677AF040AFC" left="8010.83" top="426.278" boundingSquareSize="99.6636"/>
- <RemoteState name="ResolveLadderPreLandmark" refuuid="68599B94-C644-4023-969B-486210BEFD4A" proxyName="PlatformGroupRemoteStateOfferer" left="802.167" top="842.389" boundingSquareSize="104.779"/>
- <RemoteState name="ResolveLandmark" refuuid="68599B94-C644-4023-969B-486210BEFD4A" proxyName="PlatformGroupRemoteStateOfferer" left="7081.89" top="1579.11" boundingSquareSize="99.6636"/>
- <RemoteState name="ResolveLandmark_2" refuuid="68599B94-C644-4023-969B-486210BEFD4A" proxyName="PlatformGroupRemoteStateOfferer" left="7687.56" top="1428.33" boundingSquareSize="99.6636"/>
- <RemoteState name="ResolveTargetLandmark" refuuid="68599B94-C644-4023-969B-486210BEFD4A" proxyName="PlatformGroupRemoteStateOfferer" left="2434.61" top="2461.78" boundingSquareSize="99.6636"/>
- <LocalState name="StopController" refuuid="1F2BD891-2215-459C-B636-525BAB0569DF" left="6237.28" top="1807.56" boundingSquareSize="99.6636"/>
- <EndState name="Success" event="Success" left="8546.06" top="284.444" boundingSquareSize="99.6636"/>
- <RemoteState name="Wait" refuuid="D7B2D054-3E9B-4B7D-B9DC-FE37E73E6C9D" proxyName="MotionControlGroupRemoteStateOfferer" left="3994.78" top="1691.28" boundingSquareSize="99.6636"/>
- <RemoteState name="Wait_2" refuuid="D7B2D054-3E9B-4B7D-B9DC-FE37E73E6C9D" proxyName="MotionControlGroupRemoteStateOfferer" left="5723.61" top="1881.67" boundingSquareSize="99.6636"/>
+ <LocalState name="AddObstacle" refuuid="6473D8D2-EB9E-41E2-BC2A-5781E129831B" left="6491.56" top="288.278" boundingSquareSize="99.6636"/>
+ <RemoteState name="CloseBothHands" refuuid="4BFA225C-8FA0-4214-89F3-3D717C2585CF" proxyName="Armar6LowLevelHandControlGroupRemoteStateOfferer" left="4631.11" top="1488.22" boundingSquareSize="99.6636"/>
+ <RemoteState name="CloseBothHands_2" refuuid="4BFA225C-8FA0-4214-89F3-3D717C2585CF" proxyName="Armar6LowLevelHandControlGroupRemoteStateOfferer" left="1936.28" top="2164.15" boundingSquareSize="99.6636"/>
+ <RemoteState name="CloseRightHand" refuuid="802F09B5-5EB5-48AA-A909-BCEA99EE85CF" proxyName="Armar6LowLevelHandControlGroupRemoteStateOfferer" left="1379.17" top="2505.32" boundingSquareSize="99.6636"/>
+ <LocalState name="CreatCtrlAndLift" refuuid="A5F0807D-B3A6-46E8-8831-494AFCE297D2" left="2185.44" top="2168.15" boundingSquareSize="99.6636"/>
+ <EndState name="Failure" event="Failure" left="7793.61" top="1589.04" boundingSquareSize="99.6636"/>
+ <RemoteState name="GoToGoalPositionWithObstacleAvoidance" refuuid="EB5761E3-D8DF-4B79-82F2-DF7780B3E4CD" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="7479.86" top="1586.04" boundingSquareSize="164.836"/>
+ <RemoteState name="GoToGoalPositionWithObstacleAvoidance_2" refuuid="EB5761E3-D8DF-4B79-82F2-DF7780B3E4CD" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="7156.06" top="597.611" boundingSquareSize="173.779"/>
+ <RemoteState name="GoToGoalPositionWithObstacleAvoidance_3" refuuid="EB5761E3-D8DF-4B79-82F2-DF7780B3E4CD" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="6832.78" top="342.055" boundingSquareSize="173.779"/>
+ <RemoteState name="GoToPreGraspLandmark" refuuid="EB5761E3-D8DF-4B79-82F2-DF7780B3E4CD" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="1055.94" top="2505.32" boundingSquareSize="173.779"/>
+ <RemoteState name="GoToTargetPose" refuuid="EB5761E3-D8DF-4B79-82F2-DF7780B3E4CD" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="2684.36" top="1594.99" boundingSquareSize="164.836"/>
+ <LocalState name="IndependentBimanualDMPWithForceStop" refuuid="916C9BB6-A81C-495F-B471-B0E0C2F82687" left="1628.72" top="2488.32" boundingSquareSize="158.442"/>
+ <RemoteState name="MoveBackToNavigationPose" refuuid="7CEA7DCE-053A-4955-9A1E-077C761FB8B6" proxyName="Armar6BoardSupportDemoRemoteStateOfferer" left="5974.17" top="1886.6" boundingSquareSize="109.885"/>
+ <RemoteState name="MoveJoints" refuuid="1A25B687-CFBC-4CA3-A1C7-32EEAF004C45" proxyName="MotionControlGroupRemoteStateOfferer" left="114.167" top="1113.72" boundingSquareSize="99.6636"/>
+ <RemoteState name="MovePlatformRelative" refuuid="8B23A512-C8AD-45BF-9B0C-D7E4EC830D5E" proxyName="PlatformGroupRemoteStateOfferer" left="4880.28" top="2021.26" boundingSquareSize="99.6636"/>
+ <RemoteState name="MovePlatformRelative_2" refuuid="8B23A512-C8AD-45BF-9B0C-D7E4EC830D5E" proxyName="PlatformGroupRemoteStateOfferer" left="6491.56" top="1795.1" boundingSquareSize="99.6636"/>
+ <RemoteState name="MovePlatformRelative_3" refuuid="8B23A512-C8AD-45BF-9B0C-D7E4EC830D5E" proxyName="PlatformGroupRemoteStateOfferer" left="4289.94" top="1501.22" boundingSquareSize="99.6636"/>
+ <RemoteState name="MovePlatformToLandmark" refuuid="9AE3BE73-6695-4BEF-B91F-D6B072B04F8B" proxyName="PlatformGroupRemoteStateOfferer" left="1092.94" top="2720.65" boundingSquareSize="100.942"/>
+ <RemoteState name="MoveToAfterFinished" refuuid="9AE3BE73-6695-4BEF-B91F-D6B072B04F8B" proxyName="PlatformGroupRemoteStateOfferer" left="7187.06" top="726.611" boundingSquareSize="111.164"/>
+ <RemoteState name="NavigationPose" refuuid="E7374741-A6DF-4E3F-B05D-68A1EB44D6A5" proxyName="Armar6PosesRemoteStateOfferer" left="552.444" top="1380.67" boundingSquareSize="99.6636"/>
+ <RemoteState name="NavigationPose_2" refuuid="E7374741-A6DF-4E3F-B05D-68A1EB44D6A5" proxyName="Armar6PosesRemoteStateOfferer" left="7511.86" top="510.5" boundingSquareSize="99.6636"/>
+ <RemoteState name="NavigationPose_3" refuuid="E7374741-A6DF-4E3F-B05D-68A1EB44D6A5" proxyName="Armar6PosesRemoteStateOfferer" left="6869.78" top="1753.76" boundingSquareSize="99.6636"/>
+ <RemoteState name="OpenBothHands_2" refuuid="88173D79-82C3-4FE1-ACB6-7296D8AA85CF" proxyName="Armar6LowLevelHandControlGroupRemoteStateOfferer" left="5470.61" top="1959.71" boundingSquareSize="99.6636"/>
+ <LocalState name="OpenLadder" refuuid="C9CF6660-F7F8-456D-8F20-D7226E72AD59" left="3745.61" top="2161.49" boundingSquareSize="99.6636"/>
+ <RemoteState name="OpenLeftHand" refuuid="06C28E15-ECCE-4330-B149-1F27809605CF" proxyName="Armar6LowLevelHandControlGroupRemoteStateOfferer" left="3496.44" top="2226.76" boundingSquareSize="99.6636"/>
+ <LocalState name="OpenPlaceLadder" refuuid="C9CF6660-F7F8-456D-8F20-D7226E72AD59" left="5221.44" top="2013.26" boundingSquareSize="99.6636"/>
+ <LocalState name="PrePutDownLadder" refuuid="C9CF6660-F7F8-456D-8F20-D7226E72AD59" left="2998.11" top="2290.54" boundingSquareSize="99.6636"/>
+ <LocalState name="PutDownLadder" refuuid="C9CF6660-F7F8-456D-8F20-D7226E72AD59" left="3247.28" top="2290.54" boundingSquareSize="99.6636"/>
+ <LocalState name="RemoveObstacle" refuuid="46B82788-5777-4DAA-917B-E677AF040AFC" left="7193.06" top="435.222" boundingSquareSize="99.6636"/>
+ <RemoteState name="ResolveLadderPreLandmark" refuuid="68599B94-C644-4023-969B-486210BEFD4A" proxyName="PlatformGroupRemoteStateOfferer" left="802.167" top="1372.67" boundingSquareSize="104.779"/>
+ <RemoteState name="ResolveLandmark" refuuid="68599B94-C644-4023-969B-486210BEFD4A" proxyName="PlatformGroupRemoteStateOfferer" left="7193.06" top="1668.26" boundingSquareSize="99.6636"/>
+ <RemoteState name="ResolveLandmark_2" refuuid="68599B94-C644-4023-969B-486210BEFD4A" proxyName="PlatformGroupRemoteStateOfferer" left="6869.78" top="735.5" boundingSquareSize="99.6636"/>
+ <RemoteState name="ResolveTargetLandmark" refuuid="68599B94-C644-4023-969B-486210BEFD4A" proxyName="PlatformGroupRemoteStateOfferer" left="2434.61" top="1597.99" boundingSquareSize="99.6636"/>
+ <LocalState name="StopController" refuuid="1F2BD891-2215-459C-B636-525BAB0569DF" left="6237.92" top="1787.1" boundingSquareSize="99.6636"/>
+ <EndState name="Success" event="Success" left="7793.61" top="421.167" boundingSquareSize="99.6636"/>
+ <RemoteState name="Wait" refuuid="D7B2D054-3E9B-4B7D-B9DC-FE37E73E6C9D" proxyName="MotionControlGroupRemoteStateOfferer" left="4035.67" top="1724.49" boundingSquareSize="99.6636"/>
+ <RemoteState name="Wait_2" refuuid="D7B2D054-3E9B-4B7D-B9DC-FE37E73E6C9D" proxyName="MotionControlGroupRemoteStateOfferer" left="5719.78" top="1893.15" boundingSquareSize="99.6636"/>
</Substates>
<Events>
<Event name="Failure">
@@ -286,10 +286,10 @@
<ParameterMapping sourceType="Parent" from="TorsoTolerence" to="jointTargetTolerance"/>
</ParameterMappings>
<SupportPoints>
- <SupportPoint posX="67.1067" posY="732.889"/>
- <SupportPoint posX="76.5839" posY="732.889"/>
- <SupportPoint posX="88.7241" posY="732.889"/>
- <SupportPoint posX="101.186" posY="732.889"/>
+ <SupportPoint posX="67.1067" posY="1150.72"/>
+ <SupportPoint posX="76.5839" posY="1150.72"/>
+ <SupportPoint posX="88.7241" posY="1150.72"/>
+ <SupportPoint posX="101.186" posY="1150.72"/>
</SupportPoints>
</StartState>
<Transitions>
@@ -298,25 +298,16 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="6088.46" posY="1923.59"/>
- <SupportPoint posX="6139.82" posY="1929.87"/>
- <SupportPoint posX="6218.66" posY="1937.83"/>
- <SupportPoint posX="6287.28" posY="1937.83"/>
- <SupportPoint posX="6287.28" posY="1937.83"/>
- <SupportPoint posX="6287.28" posY="1937.83"/>
- <SupportPoint posX="6712.14" posY="1937.83"/>
- <SupportPoint posX="7095.22" posY="1937.83"/>
- <SupportPoint posX="7190.28" posY="1972.33"/>
- <SupportPoint posX="7573.36" posY="1972.33"/>
- <SupportPoint posX="7573.36" posY="1972.33"/>
- <SupportPoint posX="7573.36" posY="1972.33"/>
- <SupportPoint posX="8347.06" posY="1972.33"/>
- <SupportPoint posX="8432.28" posY="1972.33"/>
- <SupportPoint posX="8468.32" posY="1962.03"/>
- <SupportPoint posX="8523.39" posY="1896.94"/>
- <SupportPoint posX="8555.21" posY="1859.31"/>
- <SupportPoint posX="8578.72" posY="1721.37"/>
- <SupportPoint posX="8589.58" posY="1645.77"/>
+ <SupportPoint posX="6084.24" posY="1931.88"/>
+ <SupportPoint posX="6291.37" posY="1946.73"/>
+ <SupportPoint posX="7042.32" posY="1988.87"/>
+ <SupportPoint posX="7644.28" posY="1872.65"/>
+ <SupportPoint posX="7702.16" posY="1861.49"/>
+ <SupportPoint posX="7728.36" posY="1873.97"/>
+ <SupportPoint posX="7770.78" posY="1833.04"/>
+ <SupportPoint posX="7813.84" posY="1791.49"/>
+ <SupportPoint posX="7831.47" posY="1723.04"/>
+ <SupportPoint posX="7838.63" posY="1676.32"/>
</SupportPoints>
</Transition>
<Transition from="MoveBackToNavigationPose" to="StopController" eventName="Success">
@@ -327,10 +318,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="6088.2" posY="1900.59"/>
- <SupportPoint posX="6128.45" posY="1889.27"/>
- <SupportPoint posX="6183.01" posY="1873.91"/>
- <SupportPoint posX="6224.79" posY="1862.13"/>
+ <SupportPoint posX="6084.62" posY="1905.39"/>
+ <SupportPoint posX="6126.02" posY="1888.84"/>
+ <SupportPoint posX="6182.5" posY="1866.25"/>
+ <SupportPoint posX="6225.43" posY="1849.08"/>
</SupportPoints>
</Transition>
<Transition from="CreatCtrlAndLift" to="Failure" eventName="Failure">
@@ -338,25 +329,25 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2285.41" posY="1218.7"/>
- <SupportPoint posX="2335.75" posY="1213.08"/>
- <SupportPoint posX="2415.36" posY="1205.67"/>
- <SupportPoint posX="2484.61" posY="1205.67"/>
- <SupportPoint posX="2484.61" posY="1205.67"/>
- <SupportPoint posX="2484.61" posY="1205.67"/>
- <SupportPoint posX="5104.69" posY="1205.67"/>
- <SupportPoint posX="6131.01" posY="1205.67"/>
- <SupportPoint posX="6387.33" posY="1171.17"/>
- <SupportPoint posX="7413.64" posY="1171.17"/>
- <SupportPoint posX="7413.64" posY="1171.17"/>
- <SupportPoint posX="7413.64" posY="1171.17"/>
- <SupportPoint posX="7899.19" posY="1171.17"/>
- <SupportPoint posX="8185.16" posY="1171.17"/>
- <SupportPoint posX="8320.35" posY="1125.68"/>
- <SupportPoint posX="8523.39" posY="1327.06"/>
- <SupportPoint posX="8554.06" posY="1357.47"/>
- <SupportPoint posX="8577.44" posY="1476.56"/>
- <SupportPoint posX="8588.68" posY="1545.38"/>
+ <SupportPoint posX="2246.31" posY="2245.01"/>
+ <SupportPoint posX="2272.24" posY="2328.58"/>
+ <SupportPoint posX="2346.36" posY="2517.93"/>
+ <SupportPoint posX="2484.61" posY="2517.93"/>
+ <SupportPoint posX="2484.61" posY="2517.93"/>
+ <SupportPoint posX="2484.61" posY="2517.93"/>
+ <SupportPoint posX="4212.81" posY="2517.93"/>
+ <SupportPoint posX="4850.16" posY="2517.93"/>
+ <SupportPoint posX="5007.84" posY="2586.93"/>
+ <SupportPoint posX="5645.19" posY="2586.93"/>
+ <SupportPoint posX="5645.19" posY="2586.93"/>
+ <SupportPoint posX="5645.19" posY="2586.93"/>
+ <SupportPoint posX="7561.86" posY="2586.93"/>
+ <SupportPoint posX="7659.61" posY="2586.93"/>
+ <SupportPoint posX="7710.34" posY="2594.78"/>
+ <SupportPoint posX="7770.78" posY="2517.93"/>
+ <SupportPoint posX="7822.91" posY="2451.59"/>
+ <SupportPoint posX="7839.01" posY="1855.05"/>
+ <SupportPoint posX="7842.72" posY="1676.39"/>
</SupportPoints>
</Transition>
<Transition from="CreatCtrlAndLift" to="ResolveTargetLandmark" eventName="Success">
@@ -367,10 +358,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2242.98" posY="1263.29"/>
- <SupportPoint posX="2279.02" posY="1447.68"/>
- <SupportPoint posX="2434.14" posY="2240.54"/>
- <SupportPoint posX="2474.77" posY="2448.61"/>
+ <SupportPoint posX="2252.18" posY="2167.78"/>
+ <SupportPoint posX="2296.14" posY="2067.13"/>
+ <SupportPoint posX="2414.59" posY="1795.57"/>
+ <SupportPoint posX="2463.27" posY="1683.93"/>
</SupportPoints>
</Transition>
<Transition from="GoToTargetPose" to="Failure" eventName="Failure">
@@ -378,25 +369,25 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2849.03" posY="2517.77"/>
- <SupportPoint posX="2905" posY="2528.96"/>
- <SupportPoint posX="2980.64" posY="2540.94"/>
- <SupportPoint posX="3048.11" posY="2540.94"/>
- <SupportPoint posX="3048.11" posY="2540.94"/>
- <SupportPoint posX="3048.11" posY="2540.94"/>
- <SupportPoint posX="5399.86" posY="2540.94"/>
- <SupportPoint posX="5622.96" posY="2540.94"/>
- <SupportPoint posX="5677.65" posY="2575.44"/>
- <SupportPoint posX="5900.75" posY="2575.44"/>
- <SupportPoint posX="5900.75" posY="2575.44"/>
- <SupportPoint posX="5900.75" posY="2575.44"/>
- <SupportPoint posX="8347.06" posY="2575.44"/>
- <SupportPoint posX="8431.26" posY="2575.44"/>
- <SupportPoint posX="8473.17" posY="2574"/>
- <SupportPoint posX="8523.39" posY="2506.44"/>
- <SupportPoint posX="8574.88" posY="2437.15"/>
- <SupportPoint posX="8591.49" posY="1827.52"/>
- <SupportPoint posX="8595.33" posY="1646.1"/>
+ <SupportPoint posX="2780.42" posY="1594.75"/>
+ <SupportPoint posX="2813" posY="1509.65"/>
+ <SupportPoint posX="2902.19" posY="1319.39"/>
+ <SupportPoint posX="3048.11" posY="1319.39"/>
+ <SupportPoint posX="3048.11" posY="1319.39"/>
+ <SupportPoint posX="3048.11" posY="1319.39"/>
+ <SupportPoint posX="5100.86" posY="1319.39"/>
+ <SupportPoint posX="5454.68" posY="1319.39"/>
+ <SupportPoint posX="5543.1" posY="1302.78"/>
+ <SupportPoint posX="5896.92" posY="1302.78"/>
+ <SupportPoint posX="5896.92" posY="1302.78"/>
+ <SupportPoint posX="5896.92" posY="1302.78"/>
+ <SupportPoint posX="7561.86" posY="1302.78"/>
+ <SupportPoint posX="7666" posY="1302.78"/>
+ <SupportPoint posX="7702.67" posY="1329.99"/>
+ <SupportPoint posX="7770.78" posY="1408.83"/>
+ <SupportPoint posX="7811.92" posY="1456.37"/>
+ <SupportPoint posX="7829.94" posY="1528.05"/>
+ <SupportPoint posX="7837.86" posY="1575.97"/>
</SupportPoints>
</Transition>
<Transition from="GoToTargetPose" to="PrePutDownLadder" eventName="Success">
@@ -410,10 +401,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2849.03" posY="2483.79"/>
- <SupportPoint posX="2892.61" posY="2475.87"/>
- <SupportPoint posX="2945.38" posY="2466.31"/>
- <SupportPoint posX="2985.63" posY="2459"/>
+ <SupportPoint posX="2782.97" posY="1675.72"/>
+ <SupportPoint posX="2831.78" posY="1795.8"/>
+ <SupportPoint posX="2975.15" posY="2148.08"/>
+ <SupportPoint posX="3028.05" posY="2278.36"/>
</SupportPoints>
</Transition>
<Transition from="PutDownLadder" to="Failure" eventName="Failure">
@@ -421,25 +412,25 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="3347.37" posY="2377.99"/>
- <SupportPoint posX="3397.58" posY="2383.23"/>
- <SupportPoint posX="3477.32" posY="2390.17"/>
- <SupportPoint posX="3546.44" posY="2390.17"/>
- <SupportPoint posX="3546.44" posY="2390.17"/>
- <SupportPoint posX="3546.44" posY="2390.17"/>
- <SupportPoint posX="5649.03" posY="2390.17"/>
- <SupportPoint posX="5877.88" posY="2390.17"/>
- <SupportPoint posX="5933.84" posY="2424.67"/>
- <SupportPoint posX="6162.69" posY="2424.67"/>
- <SupportPoint posX="6162.69" posY="2424.67"/>
- <SupportPoint posX="6162.69" posY="2424.67"/>
- <SupportPoint posX="8347.06" posY="2424.67"/>
- <SupportPoint posX="8431.26" posY="2424.67"/>
- <SupportPoint posX="8472.66" posY="2422.83"/>
- <SupportPoint posX="8523.39" posY="2355.67"/>
- <SupportPoint posX="8566.07" posY="2299.11"/>
- <SupportPoint posX="8588.3" posY="1806.87"/>
- <SupportPoint posX="8594.43" posY="1646.01"/>
+ <SupportPoint posX="3347.49" posY="2340.29"/>
+ <SupportPoint posX="3397.58" posY="2351.87"/>
+ <SupportPoint posX="3476.81" posY="2367.15"/>
+ <SupportPoint posX="3546.44" posY="2367.15"/>
+ <SupportPoint posX="3546.44" posY="2367.15"/>
+ <SupportPoint posX="3546.44" posY="2367.15"/>
+ <SupportPoint posX="4212.81" posY="2367.15"/>
+ <SupportPoint posX="4961.97" posY="2367.15"/>
+ <SupportPoint posX="5147.76" posY="2436.15"/>
+ <SupportPoint posX="5896.92" posY="2436.15"/>
+ <SupportPoint posX="5896.92" posY="2436.15"/>
+ <SupportPoint posX="5896.92" posY="2436.15"/>
+ <SupportPoint posX="7561.86" posY="2436.15"/>
+ <SupportPoint posX="7659.87" posY="2436.15"/>
+ <SupportPoint posX="7709.83" posY="2442.52"/>
+ <SupportPoint posX="7770.78" posY="2365.88"/>
+ <SupportPoint posX="7813.58" posY="2312.03"/>
+ <SupportPoint posX="7835.69" posY="1834.37"/>
+ <SupportPoint posX="7841.82" posY="1676.46"/>
</SupportPoints>
</Transition>
<Transition from="PutDownLadder" to="OpenLeftHand" eventName="Success">
@@ -447,10 +438,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="3347.75" posY="2355.99"/>
- <SupportPoint posX="3386.98" posY="2343.29"/>
- <SupportPoint posX="3441.79" posY="2325.59"/>
- <SupportPoint posX="3483.83" posY="2311.99"/>
+ <SupportPoint posX="3347.75" posY="2315.65"/>
+ <SupportPoint posX="3386.98" posY="2306.37"/>
+ <SupportPoint posX="3441.79" posY="2293.45"/>
+ <SupportPoint posX="3483.83" posY="2283.51"/>
</SupportPoints>
</Transition>
<Transition from="OpenLadder" to="Failure" eventName="Failure">
@@ -458,25 +449,19 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="3845.7" posY="2227.21"/>
- <SupportPoint posX="3895.92" posY="2232.45"/>
- <SupportPoint posX="3975.65" posY="2239.39"/>
- <SupportPoint posX="4044.78" posY="2239.39"/>
- <SupportPoint posX="4044.78" posY="2239.39"/>
- <SupportPoint posX="4044.78" posY="2239.39"/>
- <SupportPoint posX="5900.75" posY="2239.39"/>
- <SupportPoint posX="6392.82" posY="2239.39"/>
- <SupportPoint posX="6515.23" posY="2273.89"/>
- <SupportPoint posX="7007.31" posY="2273.89"/>
- <SupportPoint posX="7007.31" posY="2273.89"/>
- <SupportPoint posX="7007.31" posY="2273.89"/>
- <SupportPoint posX="8347.06" posY="2273.89"/>
- <SupportPoint posX="8431.26" posY="2273.89"/>
- <SupportPoint posX="8471.89" posY="2271.46"/>
- <SupportPoint posX="8523.39" posY="2204.89"/>
- <SupportPoint posX="8557.38" posY="2160.88"/>
- <SupportPoint posX="8584.34" posY="1783.21"/>
- <SupportPoint posX="8593.16" posY="1645.43"/>
+ <SupportPoint posX="3845.57" posY="2201.37"/>
+ <SupportPoint posX="4110.58" posY="2216.45"/>
+ <SupportPoint posX="5378.78" posY="2285.38"/>
+ <SupportPoint posX="6416.97" posY="2285.38"/>
+ <SupportPoint posX="6416.97" posY="2285.38"/>
+ <SupportPoint posX="6416.97" posY="2285.38"/>
+ <SupportPoint posX="7561.86" posY="2285.38"/>
+ <SupportPoint posX="7660.38" posY="2285.38"/>
+ <SupportPoint posX="7709.06" posY="2288.06"/>
+ <SupportPoint posX="7770.78" posY="2211.26"/>
+ <SupportPoint posX="7804" posY="2169.83"/>
+ <SupportPoint posX="7831.22" posY="1810.67"/>
+ <SupportPoint posX="7840.42" posY="1676.4"/>
</SupportPoints>
</Transition>
<Transition from="OpenLadder" to="Wait" eventName="Success">
@@ -486,10 +471,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="3814.39" posY="2184.43"/>
- <SupportPoint posX="3858.73" posY="2096.66"/>
- <SupportPoint posX="3970.03" posY="1876.17"/>
- <SupportPoint posX="4019.99" posY="1777.42"/>
+ <SupportPoint posX="3820.4" posY="2161.23"/>
+ <SupportPoint posX="3872.92" posY="2082.04"/>
+ <SupportPoint posX="3995.58" posY="1897.14"/>
+ <SupportPoint posX="4053.85" posY="1809.35"/>
</SupportPoints>
</Transition>
<Transition from="MovePlatformRelative" to="OpenPlaceLadder" eventName="TargetReached">
@@ -501,10 +486,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="4984.07" posY="2047.72"/>
- <SupportPoint posX="5044.51" posY="2047.72"/>
- <SupportPoint posX="5146.35" posY="2047.72"/>
- <SupportPoint posX="5212.16" posY="2047.72"/>
+ <SupportPoint posX="4980.24" posY="2050.26"/>
+ <SupportPoint posX="5040.68" posY="2050.26"/>
+ <SupportPoint posX="5142.52" posY="2050.26"/>
+ <SupportPoint posX="5208.32" posY="2050.26"/>
</SupportPoints>
</Transition>
<Transition from="MovePlatformRelative" to="Failure" eventName="Timeout">
@@ -512,25 +497,25 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="4984.33" posY="2076.46"/>
- <SupportPoint posX="5049.24" posY="2110.9"/>
- <SupportPoint posX="5167.05" posY="2164"/>
- <SupportPoint posX="5275.28" posY="2164"/>
- <SupportPoint posX="5275.28" posY="2164"/>
- <SupportPoint posX="5275.28" posY="2164"/>
- <SupportPoint posX="5900.75" posY="2164"/>
- <SupportPoint posX="6392.82" posY="2164"/>
- <SupportPoint posX="6515.23" posY="2198.5"/>
- <SupportPoint posX="7007.31" posY="2198.5"/>
- <SupportPoint posX="7007.31" posY="2198.5"/>
- <SupportPoint posX="7007.31" posY="2198.5"/>
- <SupportPoint posX="8347.06" posY="2198.5"/>
- <SupportPoint posX="8431.39" posY="2198.5"/>
- <SupportPoint posX="8471.38" posY="2194.68"/>
- <SupportPoint posX="8523.39" posY="2128.22"/>
- <SupportPoint posX="8552.91" posY="2090.48"/>
- <SupportPoint posX="8581.91" posY="1770.69"/>
- <SupportPoint posX="8592.26" posY="1645.43"/>
+ <SupportPoint posX="4977.94" posY="2079.93"/>
+ <SupportPoint posX="5042.08" posY="2116.96"/>
+ <SupportPoint posX="5161.04" posY="2175.49"/>
+ <SupportPoint posX="5271.44" posY="2175.49"/>
+ <SupportPoint posX="5271.44" posY="2175.49"/>
+ <SupportPoint posX="5271.44" posY="2175.49"/>
+ <SupportPoint posX="5769.78" posY="2175.49"/>
+ <SupportPoint posX="6188.89" posY="2175.49"/>
+ <SupportPoint posX="6293.03" posY="2209.99"/>
+ <SupportPoint posX="6712.14" posY="2209.99"/>
+ <SupportPoint posX="6712.14" posY="2209.99"/>
+ <SupportPoint posX="6712.14" posY="2209.99"/>
+ <SupportPoint posX="7561.86" posY="2209.99"/>
+ <SupportPoint posX="7661.4" posY="2209.99"/>
+ <SupportPoint posX="7708.68" posY="2207.27"/>
+ <SupportPoint posX="7770.78" posY="2129.49"/>
+ <SupportPoint posX="7826.11" posY="2060.15"/>
+ <SupportPoint posX="7839.39" posY="1789.75"/>
+ <SupportPoint posX="7842.59" posY="1676.44"/>
</SupportPoints>
</Transition>
<Transition from="Wait" to="MovePlatformRelative_3" eventName="Timeout">
@@ -543,10 +528,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="4086.18" posY="1690.98"/>
- <SupportPoint posX="4138.06" posY="1644.29"/>
- <SupportPoint posX="4226.35" posY="1564.76"/>
- <SupportPoint posX="4278.74" posY="1517.57"/>
+ <SupportPoint posX="4126.56" posY="1724.33"/>
+ <SupportPoint posX="4173.45" posY="1681.65"/>
+ <SupportPoint posX="4249.99" posY="1611.96"/>
+ <SupportPoint posX="4297.65" posY="1568.68"/>
</SupportPoints>
</Transition>
<Transition from="IndependentBimanualDMPWithForceStop" to="Failure" eventName="Failure">
@@ -554,25 +539,25 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1712.32" posY="1256.39"/>
- <SupportPoint posX="1734.43" posY="1536.75"/>
- <SupportPoint posX="1835.88" posY="2708.33"/>
- <SupportPoint posX="1986.28" posY="2708.33"/>
- <SupportPoint posX="1986.28" posY="2708.33"/>
- <SupportPoint posX="1986.28" posY="2708.33"/>
- <SupportPoint posX="4809.53" posY="2708.33"/>
- <SupportPoint posX="5182.77" posY="2708.33"/>
- <SupportPoint posX="5275.79" posY="2726.22"/>
- <SupportPoint posX="5649.03" posY="2726.22"/>
- <SupportPoint posX="5649.03" posY="2726.22"/>
- <SupportPoint posX="5649.03" posY="2726.22"/>
- <SupportPoint posX="8347.06" posY="2726.22"/>
- <SupportPoint posX="8431.26" posY="2726.22"/>
- <SupportPoint posX="8473.56" posY="2725.07"/>
- <SupportPoint posX="8523.39" posY="2657.22"/>
- <SupportPoint posX="8583.7" posY="2575.04"/>
- <SupportPoint posX="8594.18" posY="1845.5"/>
- <SupportPoint posX="8595.84" posY="1645.76"/>
+ <SupportPoint posX="1786.94" posY="2567.5"/>
+ <SupportPoint posX="1842.4" posY="2579.85"/>
+ <SupportPoint posX="1918.43" posY="2593.32"/>
+ <SupportPoint posX="1986.28" posY="2593.32"/>
+ <SupportPoint posX="1986.28" posY="2593.32"/>
+ <SupportPoint posX="1986.28" posY="2593.32"/>
+ <SupportPoint posX="4212.81" posY="2593.32"/>
+ <SupportPoint posX="4850.16" posY="2593.32"/>
+ <SupportPoint posX="5007.84" posY="2662.32"/>
+ <SupportPoint posX="5645.19" posY="2662.32"/>
+ <SupportPoint posX="5645.19" posY="2662.32"/>
+ <SupportPoint posX="5645.19" posY="2662.32"/>
+ <SupportPoint posX="7561.86" posY="2662.32"/>
+ <SupportPoint posX="7659.61" posY="2662.32"/>
+ <SupportPoint posX="7710.59" posY="2670.36"/>
+ <SupportPoint posX="7770.78" posY="2593.32"/>
+ <SupportPoint posX="7827.51" posY="2520.74"/>
+ <SupportPoint posX="7840.42" posY="1865.78"/>
+ <SupportPoint posX="7843.1" posY="1676.85"/>
</SupportPoints>
</Transition>
<Transition from="IndependentBimanualDMPWithForceStop" to="CloseBothHands_2" eventName="Success">
@@ -595,10 +580,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1787.2" posY="1196.72"/>
- <SupportPoint posX="1830.26" posY="1196.72"/>
- <SupportPoint posX="1882.65" posY="1196.72"/>
- <SupportPoint posX="1922.9" posY="1196.72"/>
+ <SupportPoint posX="1756.28" posY="2487.89"/>
+ <SupportPoint posX="1809.43" posY="2422.79"/>
+ <SupportPoint posX="1893.77" posY="2319.42"/>
+ <SupportPoint posX="1944.24" posY="2257.65"/>
</SupportPoints>
</Transition>
<Transition from="OpenPlaceLadder" to="Failure" eventName="Failure">
@@ -606,25 +591,25 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="5325.49" posY="2060.88"/>
- <SupportPoint posX="5375.46" posY="2072.84"/>
- <SupportPoint posX="5454.81" posY="2088.61"/>
- <SupportPoint posX="5524.44" posY="2088.61"/>
- <SupportPoint posX="5524.44" posY="2088.61"/>
- <SupportPoint posX="5524.44" posY="2088.61"/>
- <SupportPoint posX="5900.75" posY="2088.61"/>
- <SupportPoint posX="6644.29" posY="2088.61"/>
- <SupportPoint posX="6829.82" posY="2123.11"/>
- <SupportPoint posX="7573.36" posY="2123.11"/>
- <SupportPoint posX="7573.36" posY="2123.11"/>
- <SupportPoint posX="7573.36" posY="2123.11"/>
- <SupportPoint posX="8347.06" posY="2123.11"/>
- <SupportPoint posX="8431.39" posY="2123.11"/>
- <SupportPoint posX="8470.74" posY="2118.72"/>
- <SupportPoint posX="8523.39" posY="2052.83"/>
- <SupportPoint posX="8573.09" posY="1990.64"/>
- <SupportPoint posX="8589.45" posY="1750.86"/>
- <SupportPoint posX="8594.31" posY="1645.5"/>
+ <SupportPoint posX="5321.41" posY="2066.3"/>
+ <SupportPoint posX="5371.37" posY="2080.87"/>
+ <SupportPoint posX="5450.46" posY="2100.1"/>
+ <SupportPoint posX="5520.61" posY="2100.1"/>
+ <SupportPoint posX="5520.61" posY="2100.1"/>
+ <SupportPoint posX="5520.61" posY="2100.1"/>
+ <SupportPoint posX="6158.86" posY="2100.1"/>
+ <SupportPoint posX="6405.22" posY="2100.1"/>
+ <SupportPoint posX="6465.78" posY="2134.6"/>
+ <SupportPoint posX="6712.14" posY="2134.6"/>
+ <SupportPoint posX="6712.14" posY="2134.6"/>
+ <SupportPoint posX="6712.14" posY="2134.6"/>
+ <SupportPoint posX="7561.86" posY="2134.6"/>
+ <SupportPoint posX="7665.23" posY="2134.6"/>
+ <SupportPoint posX="7708.55" posY="2114.88"/>
+ <SupportPoint posX="7770.78" posY="2032.38"/>
+ <SupportPoint posX="7812.69" posY="1976.84"/>
+ <SupportPoint posX="7832.88" posY="1771.89"/>
+ <SupportPoint posX="7840.29" posY="1676.05"/>
</SupportPoints>
</Transition>
<Transition from="OpenPlaceLadder" to="OpenBothHands_2" eventName="Success">
@@ -638,10 +623,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="5325.75" posY="2036.08"/>
- <SupportPoint posX="5364.98" posY="2027.01"/>
- <SupportPoint posX="5419.79" posY="2014.37"/>
- <SupportPoint posX="5461.83" posY="2004.66"/>
+ <SupportPoint posX="5321.92" posY="2040.44"/>
+ <SupportPoint posX="5361.14" posY="2032.77"/>
+ <SupportPoint posX="5415.96" posY="2022.1"/>
+ <SupportPoint posX="5458" posY="2013.9"/>
</SupportPoints>
</Transition>
<Transition from="AddObstacle" to="Failure" eventName="Failure">
@@ -649,13 +634,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="7463.98" posY="305.728"/>
- <SupportPoint posX="7641.21" posY="236.217"/>
- <SupportPoint posX="8240.87" posY="33.0501"/>
- <SupportPoint posX="8523.39" posY="340.611"/>
- <SupportPoint posX="8564.79" posY="385.717"/>
- <SupportPoint posX="8589.58" posY="1316.96"/>
- <SupportPoint posX="8595.07" posY="1545.24"/>
+ <SupportPoint posX="6591.39" posY="306.367"/>
+ <SupportPoint posX="6789.19" posY="234.683"/>
+ <SupportPoint posX="7519.69" posY="4.17216"/>
+ <SupportPoint posX="7770.78" posY="403.222"/>
+ <SupportPoint posX="7802.47" posY="453.567"/>
+ <SupportPoint posX="7834.54" posY="1352.48"/>
+ <SupportPoint posX="7841.95" posY="1575.97"/>
</SupportPoints>
</Transition>
<Transition from="AddObstacle" to="GoToGoalPositionWithObstacleAvoidance_3" eventName="Success">
@@ -668,10 +653,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="7463.6" posY="334.222"/>
- <SupportPoint posX="7509.73" posY="341.378"/>
- <SupportPoint posX="7579.75" posY="352.111"/>
- <SupportPoint posX="7637.51" posY="361.056"/>
+ <SupportPoint posX="6591.77" posY="343.55"/>
+ <SupportPoint posX="6599.18" posY="345.85"/>
+ <SupportPoint posX="6606.98" posY="347.894"/>
+ <SupportPoint posX="6614.39" posY="349.555"/>
+ <SupportPoint posX="6682.11" posY="364.505"/>
+ <SupportPoint posX="6759.8" posY="373.194"/>
+ <SupportPoint posX="6819.86" posY="378.05"/>
</SupportPoints>
</Transition>
<Transition from="MovePlatformRelative_2" to="NavigationPose_3" eventName="TargetReached">
@@ -682,13 +670,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="6591.64" posY="1764.38"/>
- <SupportPoint posX="6599.31" posY="1762.54"/>
- <SupportPoint posX="6606.98" posY="1760.67"/>
- <SupportPoint posX="6614.39" posY="1758.94"/>
- <SupportPoint posX="6684.79" posY="1742.44"/>
- <SupportPoint posX="6765.68" posY="1724.46"/>
- <SupportPoint posX="6820.11" posY="1712.51"/>
+ <SupportPoint posX="6591.39" posY="1819.04"/>
+ <SupportPoint posX="6659.75" posY="1812.11"/>
+ <SupportPoint posX="6782.42" posY="1799.68"/>
+ <SupportPoint posX="6856.91" posY="1792.13"/>
</SupportPoints>
</Transition>
<Transition from="MovePlatformRelative_2" to="Failure" eventName="Timeout">
@@ -696,22 +681,19 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="6591.77" posY="1779.29"/>
- <SupportPoint posX="6658.34" posY="1782.3"/>
- <SupportPoint posX="6779.35" posY="1787.06"/>
- <SupportPoint posX="6882.72" posY="1787.06"/>
- <SupportPoint posX="6882.72" posY="1787.06"/>
- <SupportPoint posX="6882.72" posY="1787.06"/>
- <SupportPoint posX="7256.47" posY="1787.06"/>
- <SupportPoint posX="7542.57" posY="1787.06"/>
- <SupportPoint posX="7613.1" posY="1821.56"/>
- <SupportPoint posX="7899.19" posY="1821.56"/>
- <SupportPoint posX="7899.19" posY="1821.56"/>
- <SupportPoint posX="7899.19" posY="1821.56"/>
- <SupportPoint posX="8347.06" posY="1821.56"/>
- <SupportPoint posX="8451.71" posY="1821.56"/>
- <SupportPoint posX="8531.95" posY="1710.68"/>
- <SupportPoint posX="8570.67" posY="1644.2"/>
+ <SupportPoint posX="6591.9" posY="1832.35"/>
+ <SupportPoint posX="6716.74" posY="1851.8"/>
+ <SupportPoint posX="7051.26" posY="1897.33"/>
+ <SupportPoint posX="7329.94" posY="1876.49"/>
+ <SupportPoint posX="7470.88" posY="1865.95"/>
+ <SupportPoint posX="7507.3" posY="1864.03"/>
+ <SupportPoint posX="7644.28" posY="1829.21"/>
+ <SupportPoint posX="7702.29" posY="1814.46"/>
+ <SupportPoint posX="7726.69" posY="1823.66"/>
+ <SupportPoint posX="7770.78" posY="1783.21"/>
+ <SupportPoint posX="7801.83" posY="1754.7"/>
+ <SupportPoint posX="7820.74" posY="1710.32"/>
+ <SupportPoint posX="7831.47" posY="1675.99"/>
</SupportPoints>
</Transition>
<Transition from="RemoveObstacle" to="Failure" eventName="Failure">
@@ -719,13 +701,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="8110.79" posY="448.328"/>
- <SupportPoint posX="8207.78" posY="422.772"/>
- <SupportPoint posX="8421.29" posY="384.183"/>
- <SupportPoint posX="8523.39" posY="502.889"/>
- <SupportPoint posX="8557.76" posY="542.883"/>
- <SupportPoint posX="8587.28" posY="1336"/>
- <SupportPoint posX="8594.56" posY="1545.16"/>
+ <SupportPoint posX="7293.02" posY="464.044"/>
+ <SupportPoint posX="7411.47" posY="445.772"/>
+ <SupportPoint posX="7704.33" posY="408.461"/>
+ <SupportPoint posX="7770.78" posY="473.5"/>
+ <SupportPoint posX="7810.77" posY="512.6"/>
+ <SupportPoint posX="7836.33" posY="1358.74"/>
+ <SupportPoint posX="7842.33" posY="1575.71"/>
</SupportPoints>
</Transition>
<Transition from="RemoveObstacle" to="NavigationPose_2" eventName="Success">
@@ -733,10 +715,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="8110.67" posY="475.544"/>
- <SupportPoint posX="8158.97" posY="487.3"/>
- <SupportPoint posX="8232.18" posY="505.317"/>
- <SupportPoint posX="8284.32" posY="518.222"/>
+ <SupportPoint posX="7293.27" posY="485.639"/>
+ <SupportPoint posX="7311.93" posY="490.494"/>
+ <SupportPoint posX="7333.39" posY="495.861"/>
+ <SupportPoint posX="7352.94" posY="500.333"/>
+ <SupportPoint posX="7401.76" posY="511.45"/>
+ <SupportPoint posX="7457.34" posY="522.567"/>
+ <SupportPoint posX="7498.87" posY="530.617"/>
</SupportPoints>
</Transition>
<Transition from="GoToGoalPositionWithObstacleAvoidance_3" to="Failure" eventName="Failure">
@@ -744,16 +729,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="7824.44" posY="363.739"/>
- <SupportPoint posX="7952.09" posY="348.15"/>
- <SupportPoint posX="8196.66" posY="328.983"/>
- <SupportPoint posX="8396.89" posY="377.667"/>
- <SupportPoint posX="8458.35" posY="392.617"/>
- <SupportPoint posX="8488.76" posY="389.933"/>
- <SupportPoint posX="8523.39" posY="442.833"/>
- <SupportPoint posX="8584.34" posY="535.983"/>
- <SupportPoint posX="8594.31" posY="1335.23"/>
- <SupportPoint posX="8595.97" posY="1545.07"/>
+ <SupportPoint posX="7006.67" posY="373.45"/>
+ <SupportPoint posX="7204.34" posY="351.6"/>
+ <SupportPoint posX="7677.37" posY="312.755"/>
+ <SupportPoint posX="7770.78" posY="428.778"/>
+ <SupportPoint posX="7807.32" posY="474.139"/>
+ <SupportPoint posX="7835.56" posY="1354.4"/>
+ <SupportPoint posX="7842.21" posY="1575.84"/>
</SupportPoints>
</Transition>
<Transition from="GoToGoalPositionWithObstacleAvoidance_3" to="RemoveObstacle" eventName="Success">
@@ -763,13 +745,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="7812.94" posY="420.089"/>
- <SupportPoint posX="7824.19" posY="425.456"/>
- <SupportPoint posX="7835.94" posY="430.183"/>
- <SupportPoint posX="7847.44" posY="433.889"/>
- <SupportPoint posX="7896.51" posY="449.606"/>
- <SupportPoint posX="7954.52" posY="456.889"/>
- <SupportPoint posX="7997.71" posY="460.339"/>
+ <SupportPoint posX="6990.31" posY="426.478"/>
+ <SupportPoint posX="7003.09" posY="432.739"/>
+ <SupportPoint posX="7016.51" posY="438.489"/>
+ <SupportPoint posX="7029.67" posY="442.833"/>
+ <SupportPoint posX="7078.86" posY="458.805"/>
+ <SupportPoint posX="7137.13" posY="466.089"/>
+ <SupportPoint posX="7180.32" posY="469.411"/>
</SupportPoints>
</Transition>
<Transition from="NavigationPose" to="Failure" eventName="Failure">
@@ -777,28 +759,25 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="652.597" posY="786.556"/>
- <SupportPoint posX="703.402" posY="786.556"/>
- <SupportPoint posX="784.247" posY="786.556"/>
- <SupportPoint posX="854.167" posY="786.556"/>
- <SupportPoint posX="854.167" posY="786.556"/>
- <SupportPoint posX="854.167" posY="786.556"/>
- <SupportPoint posX="4171.92" posY="786.556"/>
- <SupportPoint posX="5225.06" posY="786.556"/>
- <SupportPoint posX="5488.41" posY="794.222"/>
- <SupportPoint posX="6541.56" posY="794.222"/>
- <SupportPoint posX="6541.56" posY="794.222"/>
- <SupportPoint posX="6541.56" posY="794.222"/>
- <SupportPoint posX="7899.19" posY="794.222"/>
- <SupportPoint posX="8121.27" posY="794.222"/>
- <SupportPoint posX="8188.74" posY="760.233"/>
- <SupportPoint posX="8396.89" posY="837.667"/>
- <SupportPoint posX="8461.16" posY="861.561"/>
- <SupportPoint posX="8488.38" posY="866.928"/>
- <SupportPoint posX="8523.39" posY="925.833"/>
- <SupportPoint posX="8554.82" posY="978.861"/>
- <SupportPoint posX="8583.96" posY="1398.99"/>
- <SupportPoint posX="8593.16" posY="1545.34"/>
+ <SupportPoint posX="604.489" posY="1444.74"/>
+ <SupportPoint posX="618.481" posY="1658.96"/>
+ <SupportPoint posX="704.309" posY="2870.59"/>
+ <SupportPoint posX="854.167" posY="2870.59"/>
+ <SupportPoint posX="854.167" posY="2870.59"/>
+ <SupportPoint posX="854.167" posY="2870.59"/>
+ <SupportPoint posX="3940.64" posY="2870.59"/>
+ <SupportPoint posX="4194.02" posY="2870.59"/>
+ <SupportPoint posX="4257.14" posY="2888.48"/>
+ <SupportPoint posX="4510.53" posY="2888.48"/>
+ <SupportPoint posX="4510.53" posY="2888.48"/>
+ <SupportPoint posX="4510.53" posY="2888.48"/>
+ <SupportPoint posX="7561.86" posY="2888.48"/>
+ <SupportPoint posX="7659.61" posY="2888.48"/>
+ <SupportPoint posX="7711.11" posY="2896.95"/>
+ <SupportPoint posX="7770.78" posY="2819.48"/>
+ <SupportPoint posX="7806.17" posY="2773.53"/>
+ <SupportPoint posX="7835.31" posY="1897.62"/>
+ <SupportPoint posX="7842.08" posY="1676.43"/>
</SupportPoints>
</Transition>
<Transition from="NavigationPose" to="ResolveLadderPreLandmark" eventName="Success">
@@ -809,13 +788,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="631.603" posY="818.755"/>
- <SupportPoint posX="643.959" posY="830.383"/>
- <SupportPoint posX="659.216" posY="842.778"/>
- <SupportPoint posX="675.278" posY="850.444"/>
- <SupportPoint posX="710.442" posY="867.311"/>
- <SupportPoint posX="753.388" posY="875.361"/>
- <SupportPoint posX="788.387" posY="879.194"/>
+ <SupportPoint posX="652.776" posY="1412.67"/>
+ <SupportPoint posX="691.748" posY="1412.67"/>
+ <SupportPoint posX="746.041" posY="1412.67"/>
+ <SupportPoint posX="788.438" posY="1412.67"/>
</SupportPoints>
</Transition>
<Transition from="GoToPreGraspLandmark" to="Failure" eventName="Failure">
@@ -823,25 +799,31 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1229.99" posY="966.339"/>
- <SupportPoint posX="1286.71" posY="964.678"/>
- <SupportPoint posX="1362.34" posY="962.889"/>
- <SupportPoint posX="1429.17" posY="962.889"/>
- <SupportPoint posX="1429.17" posY="962.889"/>
- <SupportPoint posX="1429.17" posY="962.889"/>
- <SupportPoint posX="4514.36" posY="962.889"/>
- <SupportPoint posX="4776.82" posY="962.889"/>
- <SupportPoint posX="4842.24" posY="945"/>
- <SupportPoint posX="5104.69" posY="945"/>
- <SupportPoint posX="5104.69" posY="945"/>
- <SupportPoint posX="5104.69" posY="945"/>
- <SupportPoint posX="7899.19" posY="945"/>
- <SupportPoint posX="8184.27" posY="945"/>
- <SupportPoint posX="8375.29" posY="849.55"/>
- <SupportPoint posX="8523.39" posY="1093.22"/>
- <SupportPoint posX="8569.26" posY="1168.61"/>
- <SupportPoint posX="8587.92" posY="1433.62"/>
- <SupportPoint posX="8593.92" posY="1545.21"/>
+ <SupportPoint posX="1230.29" posY="2573.44"/>
+ <SupportPoint posX="1274.56" posY="2585.68"/>
+ <SupportPoint posX="1329.37" posY="2599.3"/>
+ <SupportPoint posX="1379.33" posY="2607.38"/>
+ <SupportPoint posX="1488.97" posY="2625.09"/>
+ <SupportPoint posX="1517.72" posY="2618.71"/>
+ <SupportPoint posX="1628.5" posY="2625.26"/>
+ <SupportPoint posX="2203.63" posY="2659.24"/>
+ <SupportPoint posX="2347.38" posY="2685.32"/>
+ <SupportPoint posX="2923.53" posY="2685.32"/>
+ <SupportPoint posX="2923.53" posY="2685.32"/>
+ <SupportPoint posX="2923.53" posY="2685.32"/>
+ <SupportPoint posX="4212.81" posY="2685.32"/>
+ <SupportPoint posX="4794.58" posY="2685.32"/>
+ <SupportPoint posX="4938.84" posY="2737.71"/>
+ <SupportPoint posX="5520.61" posY="2737.71"/>
+ <SupportPoint posX="5520.61" posY="2737.71"/>
+ <SupportPoint posX="5520.61" posY="2737.71"/>
+ <SupportPoint posX="7561.86" posY="2737.71"/>
+ <SupportPoint posX="7659.61" posY="2737.71"/>
+ <SupportPoint posX="7710.72" posY="2745.91"/>
+ <SupportPoint posX="7770.78" posY="2668.71"/>
+ <SupportPoint posX="7832.11" posY="2589.83"/>
+ <SupportPoint posX="7841.82" posY="1875.11"/>
+ <SupportPoint posX="7843.36" posY="1676.83"/>
</SupportPoints>
</Transition>
<Transition from="GoToPreGraspLandmark" to="CloseRightHand" eventName="Success">
@@ -852,13 +834,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1221.26" posY="1012.72"/>
- <SupportPoint posX="1231.72" posY="1017.83"/>
- <SupportPoint posX="1242.43" posY="1022.69"/>
- <SupportPoint posX="1252.83" posY="1026.78"/>
- <SupportPoint posX="1289.66" posY="1041.47"/>
- <SupportPoint posX="1332.44" posY="1053.61"/>
- <SupportPoint posX="1366.56" posY="1062.3"/>
+ <SupportPoint posX="1229.99" posY="2547.32"/>
+ <SupportPoint posX="1273.88" posY="2547.32"/>
+ <SupportPoint posX="1326.18" posY="2547.32"/>
+ <SupportPoint posX="1366.04" posY="2547.32"/>
</SupportPoints>
</Transition>
<Transition from="StopController" to="Failure" eventName="Failure">
@@ -866,19 +845,19 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="6337.37" posY="1847.37"/>
- <SupportPoint posX="6535.55" posY="1858.25"/>
- <SupportPoint posX="7283.18" posY="1896.94"/>
- <SupportPoint posX="7899.19" posY="1896.94"/>
- <SupportPoint posX="7899.19" posY="1896.94"/>
- <SupportPoint posX="7899.19" posY="1896.94"/>
- <SupportPoint posX="8347.06" posY="1896.94"/>
- <SupportPoint posX="8434.2" posY="1896.94"/>
- <SupportPoint posX="8465.76" posY="1876.72"/>
- <SupportPoint posX="8523.39" posY="1811.33"/>
- <SupportPoint posX="8564.79" posY="1764.38"/>
- <SupportPoint posX="8582.81" posY="1693.04"/>
- <SupportPoint posX="8590.6" posY="1645.35"/>
+ <SupportPoint posX="6300.06" posY="1786.71"/>
+ <SupportPoint posX="6328.42" posY="1707.28"/>
+ <SupportPoint posX="6407.13" posY="1528.94"/>
+ <SupportPoint posX="6541.56" posY="1528.94"/>
+ <SupportPoint posX="6541.56" posY="1528.94"/>
+ <SupportPoint posX="6541.56" posY="1528.94"/>
+ <SupportPoint posX="7561.86" posY="1528.94"/>
+ <SupportPoint posX="7657.57" posY="1528.94"/>
+ <SupportPoint posX="7682.48" posY="1544.15"/>
+ <SupportPoint posX="7770.78" posY="1581.33"/>
+ <SupportPoint posX="7774.61" posY="1582.87"/>
+ <SupportPoint posX="7778.44" posY="1584.78"/>
+ <SupportPoint posX="7782.28" posY="1586.7"/>
</SupportPoints>
</Transition>
<Transition from="StopController" to="MovePlatformRelative_2" eventName="Success">
@@ -890,10 +869,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="6337.49" posY="1831.18"/>
- <SupportPoint posX="6378.13" posY="1820.37"/>
- <SupportPoint posX="6435.37" posY="1805.11"/>
- <SupportPoint posX="6478.94" posY="1793.51"/>
+ <SupportPoint posX="6338.01" posY="1824.1"/>
+ <SupportPoint posX="6378.38" posY="1824.1"/>
+ <SupportPoint posX="6435.12" posY="1824.1"/>
+ <SupportPoint posX="6478.56" posY="1824.1"/>
</SupportPoints>
</Transition>
<Transition from="NavigationPose_2" to="Failure" eventName="Failure">
@@ -901,13 +880,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="8397.02" posY="540.967"/>
- <SupportPoint posX="8438.16" posY="549.783"/>
- <SupportPoint posX="8494.26" posY="569.078"/>
- <SupportPoint posX="8523.39" posY="610.222"/>
- <SupportPoint posX="8577.82" posY="686.889"/>
- <SupportPoint posX="8592.39" posY="1354.78"/>
- <SupportPoint posX="8595.46" posY="1545.02"/>
+ <SupportPoint posX="7612.08" posY="526.783"/>
+ <SupportPoint posX="7659.48" posY="515.283"/>
+ <SupportPoint posX="7729.38" posY="508"/>
+ <SupportPoint posX="7770.78" posY="547.611"/>
+ <SupportPoint posX="7808.34" posY="583.644"/>
+ <SupportPoint posX="7835.56" posY="1367.31"/>
+ <SupportPoint posX="7842.08" posY="1575.58"/>
</SupportPoints>
</Transition>
<Transition from="NavigationPose_2" to="Success" eventName="Success">
@@ -915,10 +894,16 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="8384.88" posY="501.356"/>
- <SupportPoint posX="8426.92" posY="465.578"/>
- <SupportPoint posX="8495.28" posY="407.439"/>
- <SupportPoint posX="8542.81" posY="366.933"/>
+ <SupportPoint posX="7607.99" posY="510.428"/>
+ <SupportPoint posX="7620" posY="501.611"/>
+ <SupportPoint posX="7632.78" posY="491.9"/>
+ <SupportPoint posX="7644.28" posY="482.444"/>
+ <SupportPoint posX="7655.14" posY="473.5"/>
+ <SupportPoint posX="7654.24" posY="465.961"/>
+ <SupportPoint posX="7667.28" posY="460.722"/>
+ <SupportPoint posX="7702.93" posY="446.283"/>
+ <SupportPoint posX="7745.99" posY="445.389"/>
+ <SupportPoint posX="7780.62" posY="448.2"/>
</SupportPoints>
</Transition>
<Transition from="CloseBothHands" to="Failure" eventName="Failure">
@@ -926,22 +911,25 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="4734.91" posY="1437.58"/>
- <SupportPoint posX="4782.82" posY="1401.93"/>
- <SupportPoint posX="4858.85" posY="1356.44"/>
- <SupportPoint posX="4934.11" posY="1356.44"/>
- <SupportPoint posX="4934.11" posY="1356.44"/>
- <SupportPoint posX="4934.11" posY="1356.44"/>
- <SupportPoint posX="5524.44" posY="1356.44"/>
- <SupportPoint posX="6690.29" posY="1356.44"/>
- <SupportPoint posX="6982.01" posY="1338.56"/>
- <SupportPoint posX="8147.72" posY="1360.28"/>
- <SupportPoint posX="8318.43" posY="1363.47"/>
- <SupportPoint posX="8407.24" posY="1315.68"/>
- <SupportPoint posX="8523.39" posY="1440.78"/>
- <SupportPoint posX="8551.12" posY="1470.68"/>
- <SupportPoint posX="8570.03" posY="1512.97"/>
- <SupportPoint posX="8581.66" posY="1545.73"/>
+ <SupportPoint posX="4731.33" posY="1516.68"/>
+ <SupportPoint posX="4781.29" posY="1504.28"/>
+ <SupportPoint posX="4860.51" posY="1488.06"/>
+ <SupportPoint posX="4930.28" posY="1488.06"/>
+ <SupportPoint posX="4930.28" posY="1488.06"/>
+ <SupportPoint posX="4930.28" posY="1488.06"/>
+ <SupportPoint posX="5896.92" posY="1488.06"/>
+ <SupportPoint posX="6259.55" posY="1488.06"/>
+ <SupportPoint posX="6349.51" posY="1453.56"/>
+ <SupportPoint posX="6712.14" posY="1453.56"/>
+ <SupportPoint posX="6712.14" posY="1453.56"/>
+ <SupportPoint posX="6712.14" posY="1453.56"/>
+ <SupportPoint posX="7561.86" posY="1453.56"/>
+ <SupportPoint posX="7661.4" posY="1453.56"/>
+ <SupportPoint posX="7690.66" posY="1475.02"/>
+ <SupportPoint posX="7770.78" posY="1534.06"/>
+ <SupportPoint posX="7787.01" posY="1546.07"/>
+ <SupportPoint posX="7801.57" posY="1562.55"/>
+ <SupportPoint posX="7813.46" posY="1578.27"/>
</SupportPoints>
</Transition>
<Transition from="CloseBothHands" to="MovePlatformRelative" eventName="Success">
@@ -954,10 +942,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="4703.22" posY="1520.89"/>
- <SupportPoint posX="4749.34" posY="1626.16"/>
- <SupportPoint posX="4869.84" posY="1901.06"/>
- <SupportPoint posX="4915.84" posY="2006.09"/>
+ <SupportPoint posX="4701.04" posY="1571.88"/>
+ <SupportPoint posX="4747.81" posY="1669.42"/>
+ <SupportPoint posX="4863.83" posY="1911.64"/>
+ <SupportPoint posX="4910.47" posY="2008.88"/>
</SupportPoints>
</Transition>
<Transition from="MovePlatformRelative_3" to="CloseBothHands" eventName="TargetReached">
@@ -965,10 +953,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="4371.63" posY="1479.11"/>
- <SupportPoint posX="4436.67" posY="1479.11"/>
- <SupportPoint posX="4550.52" posY="1479.11"/>
- <SupportPoint posX="4621.57" posY="1479.11"/>
+ <SupportPoint posX="4389.91" posY="1530.22"/>
+ <SupportPoint posX="4450.34" posY="1530.22"/>
+ <SupportPoint posX="4552.18" posY="1530.22"/>
+ <SupportPoint posX="4617.99" posY="1530.22"/>
</SupportPoints>
</Transition>
<Transition from="MovePlatformRelative_3" to="Failure" eventName="Timeout">
@@ -976,22 +964,28 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="4350.93" posY="1449.47"/>
- <SupportPoint posX="4409.33" posY="1394.27"/>
- <SupportPoint posX="4545.92" posY="1281.06"/>
- <SupportPoint posX="4684.94" posY="1281.06"/>
- <SupportPoint posX="4684.94" posY="1281.06"/>
- <SupportPoint posX="4684.94" posY="1281.06"/>
- <SupportPoint posX="5104.69" posY="1281.06"/>
- <SupportPoint posX="5836.35" posY="1281.06"/>
- <SupportPoint posX="7677.88" posY="1194.17"/>
- <SupportPoint posX="8396.89" posY="1329.61"/>
- <SupportPoint posX="8455.92" posY="1340.73"/>
- <SupportPoint posX="8481.73" posY="1333.7"/>
- <SupportPoint posX="8523.39" posY="1376.89"/>
- <SupportPoint posX="8567.73" posY="1422.76"/>
- <SupportPoint posX="8585.11" posY="1496.36"/>
- <SupportPoint posX="8591.88" posY="1545.22"/>
+ <SupportPoint posX="4383.52" posY="1500.71"/>
+ <SupportPoint posX="4392.97" posY="1495.08"/>
+ <SupportPoint posX="4402.94" posY="1489.72"/>
+ <SupportPoint posX="4412.78" posY="1485.5"/>
+ <SupportPoint posX="4526.24" posY="1436.43"/>
+ <SupportPoint posX="4557.55" posY="1412.67"/>
+ <SupportPoint posX="4681.11" posY="1412.67"/>
+ <SupportPoint posX="4681.11" posY="1412.67"/>
+ <SupportPoint posX="4681.11" posY="1412.67"/>
+ <SupportPoint posX="5100.86" posY="1412.67"/>
+ <SupportPoint posX="5817.18" posY="1412.67"/>
+ <SupportPoint posX="5995.82" posY="1378.17"/>
+ <SupportPoint posX="6712.14" posY="1378.17"/>
+ <SupportPoint posX="6712.14" posY="1378.17"/>
+ <SupportPoint posX="6712.14" posY="1378.17"/>
+ <SupportPoint posX="7561.86" posY="1378.17"/>
+ <SupportPoint posX="7664.21" posY="1378.17"/>
+ <SupportPoint posX="7697.82" posY="1403.34"/>
+ <SupportPoint posX="7770.78" posY="1475.28"/>
+ <SupportPoint posX="7799.14" posY="1503.13"/>
+ <SupportPoint posX="7817.93" posY="1544.28"/>
+ <SupportPoint posX="7829.43" posY="1576.61"/>
</SupportPoints>
</Transition>
<Transition from="OpenBothHands_2" to="Failure" eventName="Failure">
@@ -999,25 +993,22 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="5574.41" posY="1997.56"/>
- <SupportPoint posX="5624.62" posY="2004.29"/>
- <SupportPoint posX="5704.36" posY="2013.22"/>
- <SupportPoint posX="5773.61" posY="2013.22"/>
- <SupportPoint posX="5773.61" posY="2013.22"/>
- <SupportPoint posX="5773.61" posY="2013.22"/>
- <SupportPoint posX="6712.14" posY="2013.22"/>
- <SupportPoint posX="7095.22" posY="2013.22"/>
- <SupportPoint posX="7190.28" posY="2047.72"/>
- <SupportPoint posX="7573.36" posY="2047.72"/>
- <SupportPoint posX="7573.36" posY="2047.72"/>
- <SupportPoint posX="7573.36" posY="2047.72"/>
- <SupportPoint posX="8347.06" posY="2047.72"/>
- <SupportPoint posX="8431.64" posY="2047.72"/>
- <SupportPoint posX="8469.72" posY="2041.55"/>
- <SupportPoint posX="8523.39" posY="1976.17"/>
- <SupportPoint posX="8564.41" posY="1926.21"/>
- <SupportPoint posX="8584.85" posY="1736.89"/>
- <SupportPoint posX="8592.52" posY="1645.44"/>
+ <SupportPoint posX="5570.57" posY="2009.04"/>
+ <SupportPoint posX="5620.79" posY="2015.78"/>
+ <SupportPoint posX="5700.52" posY="2024.71"/>
+ <SupportPoint posX="5769.78" posY="2024.71"/>
+ <SupportPoint posX="5769.78" posY="2024.71"/>
+ <SupportPoint posX="5769.78" posY="2024.71"/>
+ <SupportPoint posX="6158.86" posY="2024.71"/>
+ <SupportPoint posX="6569.16" posY="2024.71"/>
+ <SupportPoint posX="6671.12" posY="2059.21"/>
+ <SupportPoint posX="7081.42" posY="2059.21"/>
+ <SupportPoint posX="7081.42" posY="2059.21"/>
+ <SupportPoint posX="7081.42" posY="2059.21"/>
+ <SupportPoint posX="7561.86" posY="2059.21"/>
+ <SupportPoint posX="7742.92" posY="2059.21"/>
+ <SupportPoint posX="7813.58" posY="1790.15"/>
+ <SupportPoint posX="7835.43" posY="1676.13"/>
</SupportPoints>
</Transition>
<Transition from="OpenBothHands_2" to="Wait_2" eventName="Success">
@@ -1029,10 +1020,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="5574.92" posY="1975.74"/>
- <SupportPoint posX="5614.14" posY="1964.45"/>
- <SupportPoint posX="5668.96" posY="1948.72"/>
- <SupportPoint posX="5711" posY="1936.63"/>
+ <SupportPoint posX="5571.08" posY="1987.22"/>
+ <SupportPoint posX="5610.31" posY="1975.94"/>
+ <SupportPoint posX="5665.13" posY="1960.21"/>
+ <SupportPoint posX="5707.17" posY="1948.11"/>
</SupportPoints>
</Transition>
<Transition from="Wait_2" to="MoveBackToNavigationPose" eventName="Timeout">
@@ -1046,10 +1037,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="5823.57" posY="1918.17"/>
- <SupportPoint posX="5863.69" posY="1917.79"/>
- <SupportPoint posX="5920.17" posY="1917.22"/>
- <SupportPoint posX="5964.51" posY="1916.79"/>
+ <SupportPoint posX="5819.74" posY="1929.66"/>
+ <SupportPoint posX="5859.86" posY="1929.26"/>
+ <SupportPoint posX="5916.34" posY="1928.71"/>
+ <SupportPoint posX="5960.68" posY="1928.26"/>
</SupportPoints>
</Transition>
<Transition from="CloseRightHand" to="Failure" eventName="Failure">
@@ -1057,25 +1048,19 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1479.26" posY="1065.49"/>
- <SupportPoint posX="1534.71" posY="1054.25"/>
- <SupportPoint posX="1627.22" posY="1038.28"/>
- <SupportPoint posX="1707.72" posY="1038.28"/>
- <SupportPoint posX="1707.72" posY="1038.28"/>
- <SupportPoint posX="1707.72" posY="1038.28"/>
- <SupportPoint posX="4514.36" posY="1038.28"/>
- <SupportPoint posX="4908.04" posY="1038.28"/>
- <SupportPoint posX="5006.18" posY="1020.39"/>
- <SupportPoint posX="5399.86" posY="1020.39"/>
- <SupportPoint posX="5399.86" posY="1020.39"/>
- <SupportPoint posX="5399.86" posY="1020.39"/>
- <SupportPoint posX="7899.19" posY="1020.39"/>
- <SupportPoint posX="8185.29" posY="1020.39"/>
- <SupportPoint posX="8371.33" posY="935.289"/>
- <SupportPoint posX="8523.39" posY="1177.56"/>
- <SupportPoint posX="8561.47" posY="1238.25"/>
- <SupportPoint posX="8583.7" posY="1448.06"/>
- <SupportPoint posX="8592.26" posY="1545.27"/>
+ <SupportPoint posX="1432.49" posY="2505.66"/>
+ <SupportPoint posX="1452.29" posY="2266.32"/>
+ <SupportPoint posX="1558.09" posY="1076.61"/>
+ <SupportPoint posX="1707.72" posY="1076.61"/>
+ <SupportPoint posX="1707.72" posY="1076.61"/>
+ <SupportPoint posX="1707.72" posY="1076.61"/>
+ <SupportPoint posX="7561.86" posY="1076.61"/>
+ <SupportPoint posX="7667.53" posY="1076.61"/>
+ <SupportPoint posX="7709.44" posY="1104.21"/>
+ <SupportPoint posX="7770.78" posY="1190.33"/>
+ <SupportPoint posX="7814.48" posY="1251.54"/>
+ <SupportPoint posX="7833.9" posY="1474.89"/>
+ <SupportPoint posX="7840.67" posY="1575.84"/>
</SupportPoints>
</Transition>
<Transition from="CloseRightHand" to="IndependentBimanualDMPWithForceStop" eventName="Success">
@@ -1096,10 +1081,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1479.26" posY="1098.21"/>
- <SupportPoint posX="1517.21" posY="1114.56"/>
- <SupportPoint posX="1570.49" posY="1137.56"/>
- <SupportPoint posX="1615.98" posY="1157.11"/>
+ <SupportPoint posX="1479.26" posY="2547.32"/>
+ <SupportPoint posX="1516.95" posY="2547.32"/>
+ <SupportPoint posX="1569.59" posY="2547.32"/>
+ <SupportPoint posX="1614.83" posY="2547.32"/>
</SupportPoints>
</Transition>
<Transition from="OpenLeftHand" to="Failure" eventName="Failure">
@@ -1107,25 +1092,25 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="3596.41" posY="2299.11"/>
- <SupportPoint posX="3646.62" posY="2305.85"/>
- <SupportPoint posX="3726.36" posY="2314.78"/>
- <SupportPoint posX="3795.61" posY="2314.78"/>
- <SupportPoint posX="3795.61" posY="2314.78"/>
- <SupportPoint posX="3795.61" posY="2314.78"/>
- <SupportPoint posX="5649.03" posY="2314.78"/>
- <SupportPoint posX="6252.91" posY="2314.78"/>
- <SupportPoint posX="6403.43" posY="2349.28"/>
- <SupportPoint posX="7007.31" posY="2349.28"/>
- <SupportPoint posX="7007.31" posY="2349.28"/>
- <SupportPoint posX="7007.31" posY="2349.28"/>
- <SupportPoint posX="8347.06" posY="2349.28"/>
- <SupportPoint posX="8431.26" posY="2349.28"/>
- <SupportPoint posX="8472.28" posY="2347.18"/>
- <SupportPoint posX="8523.39" posY="2280.28"/>
- <SupportPoint posX="8561.72" posY="2230.04"/>
- <SupportPoint posX="8586.38" posY="1795.73"/>
- <SupportPoint posX="8593.79" posY="1645.94"/>
+ <SupportPoint posX="3596.41" posY="2276.1"/>
+ <SupportPoint posX="3646.62" posY="2282.83"/>
+ <SupportPoint posX="3726.36" posY="2291.76"/>
+ <SupportPoint posX="3795.61" posY="2291.76"/>
+ <SupportPoint posX="3795.61" posY="2291.76"/>
+ <SupportPoint posX="3795.61" posY="2291.76"/>
+ <SupportPoint posX="4339.94" posY="2291.76"/>
+ <SupportPoint posX="5032.63" posY="2291.76"/>
+ <SupportPoint posX="5204.23" posY="2360.76"/>
+ <SupportPoint posX="5896.92" posY="2360.76"/>
+ <SupportPoint posX="5896.92" posY="2360.76"/>
+ <SupportPoint posX="5896.92" posY="2360.76"/>
+ <SupportPoint posX="7561.86" posY="2360.76"/>
+ <SupportPoint posX="7659.99" posY="2360.76"/>
+ <SupportPoint posX="7709.44" posY="2365.82"/>
+ <SupportPoint posX="7770.78" posY="2289.21"/>
+ <SupportPoint posX="7808.98" posY="2241.52"/>
+ <SupportPoint posX="7833.64" posY="1822.26"/>
+ <SupportPoint posX="7841.18" posY="1676.07"/>
</SupportPoints>
</Transition>
<Transition from="OpenLeftHand" to="OpenLadder" eventName="Success">
@@ -1139,10 +1124,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="3596.92" posY="2277.56"/>
- <SupportPoint posX="3636.14" posY="2266.47"/>
- <SupportPoint posX="3690.96" posY="2251.02"/>
- <SupportPoint posX="3733" posY="2239.15"/>
+ <SupportPoint posX="3596.92" posY="2254.54"/>
+ <SupportPoint posX="3636.14" posY="2243.45"/>
+ <SupportPoint posX="3690.96" posY="2228"/>
+ <SupportPoint posX="3733" posY="2216.12"/>
</SupportPoints>
</Transition>
<Transition from="PrePutDownLadder" to="Failure" eventName="Failure">
@@ -1150,25 +1135,25 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="3098.2" posY="2453.38"/>
- <SupportPoint posX="3148.42" posY="2458.62"/>
- <SupportPoint posX="3228.15" posY="2465.56"/>
- <SupportPoint posX="3297.28" posY="2465.56"/>
- <SupportPoint posX="3297.28" posY="2465.56"/>
- <SupportPoint posX="3297.28" posY="2465.56"/>
- <SupportPoint posX="5399.86" posY="2465.56"/>
- <SupportPoint posX="5739.24" posY="2465.56"/>
- <SupportPoint posX="5823.32" posY="2500.06"/>
- <SupportPoint posX="6162.69" posY="2500.06"/>
- <SupportPoint posX="6162.69" posY="2500.06"/>
- <SupportPoint posX="6162.69" posY="2500.06"/>
- <SupportPoint posX="8347.06" posY="2500.06"/>
- <SupportPoint posX="8431.26" posY="2500.06"/>
- <SupportPoint posX="8472.92" posY="2498.43"/>
- <SupportPoint posX="8523.39" posY="2431.06"/>
- <SupportPoint posX="8570.54" posY="2368.11"/>
- <SupportPoint posX="8589.96" posY="1816.96"/>
- <SupportPoint posX="8594.94" posY="1645.84"/>
+ <SupportPoint posX="3096.16" posY="2364.88"/>
+ <SupportPoint posX="3144.2" posY="2398.46"/>
+ <SupportPoint posX="3221.76" posY="2442.54"/>
+ <SupportPoint posX="3297.28" posY="2442.54"/>
+ <SupportPoint posX="3297.28" posY="2442.54"/>
+ <SupportPoint posX="3297.28" posY="2442.54"/>
+ <SupportPoint posX="4212.81" posY="2442.54"/>
+ <SupportPoint posX="4850.16" posY="2442.54"/>
+ <SupportPoint posX="5007.84" posY="2511.54"/>
+ <SupportPoint posX="5645.19" posY="2511.54"/>
+ <SupportPoint posX="5645.19" posY="2511.54"/>
+ <SupportPoint posX="5645.19" posY="2511.54"/>
+ <SupportPoint posX="7561.86" posY="2511.54"/>
+ <SupportPoint posX="7659.87" posY="2511.54"/>
+ <SupportPoint posX="7710.08" posY="2518.19"/>
+ <SupportPoint posX="7770.78" posY="2441.26"/>
+ <SupportPoint posX="7818.18" posY="2381.11"/>
+ <SupportPoint posX="7837.48" posY="1843.76"/>
+ <SupportPoint posX="7842.33" posY="1675.9"/>
</SupportPoints>
</Transition>
<Transition from="PrePutDownLadder" to="PutDownLadder" eventName="Success">
@@ -1180,10 +1165,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="3098.58" posY="2432.41"/>
- <SupportPoint posX="3137.81" posY="2420.51"/>
- <SupportPoint posX="3192.63" posY="2403.94"/>
- <SupportPoint posX="3234.67" posY="2391.2"/>
+ <SupportPoint posX="3098.58" posY="2327.54"/>
+ <SupportPoint posX="3137.68" posY="2327.54"/>
+ <SupportPoint posX="3192.12" posY="2327.54"/>
+ <SupportPoint posX="3234.16" posY="2327.54"/>
</SupportPoints>
</Transition>
<Transition from="CloseBothHands_2" to="Failure" eventName="Failure">
@@ -1191,28 +1176,19 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2036.49" posY="1169.76"/>
- <SupportPoint posX="2043.91" posY="1166.44"/>
- <SupportPoint posX="2051.57" posY="1163.37"/>
- <SupportPoint posX="2059.11" posY="1160.94"/>
- <SupportPoint posX="2134.76" posY="1136.28"/>
- <SupportPoint posX="2155.84" posY="1130.28"/>
- <SupportPoint posX="2235.44" posY="1130.28"/>
- <SupportPoint posX="2235.44" posY="1130.28"/>
- <SupportPoint posX="2235.44" posY="1130.28"/>
- <SupportPoint posX="4809.53" posY="1130.28"/>
- <SupportPoint posX="5411.11" posY="1130.28"/>
- <SupportPoint posX="5561.12" posY="1095.78"/>
- <SupportPoint posX="6162.69" posY="1095.78"/>
- <SupportPoint posX="6162.69" posY="1095.78"/>
- <SupportPoint posX="6162.69" posY="1095.78"/>
- <SupportPoint posX="7899.19" posY="1095.78"/>
- <SupportPoint posX="8185.8" posY="1095.78"/>
- <SupportPoint posX="8359.58" posY="1022.82"/>
- <SupportPoint posX="8523.39" posY="1258.06"/>
- <SupportPoint posX="8555.46" posY="1304.06"/>
- <SupportPoint posX="8579.48" posY="1463.01"/>
- <SupportPoint posX="8590.22" posY="1545.21"/>
+ <SupportPoint posX="1987.04" posY="2164.5"/>
+ <SupportPoint posX="1991.64" posY="1969.01"/>
+ <SupportPoint posX="2023.59" posY="1152"/>
+ <SupportPoint posX="2235.44" posY="1152"/>
+ <SupportPoint posX="2235.44" posY="1152"/>
+ <SupportPoint posX="2235.44" posY="1152"/>
+ <SupportPoint posX="7561.86" posY="1152"/>
+ <SupportPoint posX="7667.28" posY="1152"/>
+ <SupportPoint posX="7708.04" posY="1179.73"/>
+ <SupportPoint posX="7770.78" posY="1264.44"/>
+ <SupportPoint posX="7806.94" posY="1313.26"/>
+ <SupportPoint posX="7829.56" posY="1488.31"/>
+ <SupportPoint posX="7838.76" posY="1575.71"/>
</SupportPoints>
</Transition>
<Transition from="CloseBothHands_2" to="CreatCtrlAndLift" eventName="Success">
@@ -1237,10 +1213,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2036.75" posY="1202.47"/>
- <SupportPoint posX="2075.85" posY="1206.82"/>
- <SupportPoint posX="2130.28" posY="1212.95"/>
- <SupportPoint posX="2172.32" posY="1217.68"/>
+ <SupportPoint posX="2036.75" posY="2206.15"/>
+ <SupportPoint posX="2075.85" posY="2206.15"/>
+ <SupportPoint posX="2130.28" posY="2206.15"/>
+ <SupportPoint posX="2172.32" posY="2206.15"/>
</SupportPoints>
</Transition>
<Transition from="ResolveLadderPreLandmark" to="Failure" eventName="Failure">
@@ -1248,19 +1224,19 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="906.837" posY="878.683"/>
- <SupportPoint posX="964.694" posY="874.85"/>
- <SupportPoint posX="1060.39" posY="869.611"/>
- <SupportPoint posX="1142.94" posY="869.611"/>
- <SupportPoint posX="1142.94" posY="869.611"/>
- <SupportPoint posX="1142.94" posY="869.611"/>
- <SupportPoint posX="7899.19" posY="869.611"/>
- <SupportPoint posX="8183.5" posY="869.611"/>
- <SupportPoint posX="8377.34" posY="766.239"/>
- <SupportPoint posX="8523.39" posY="1010.17"/>
- <SupportPoint posX="8577.31" posY="1100.12"/>
- <SupportPoint posX="8591.49" posY="1420.59"/>
- <SupportPoint posX="8595.07" posY="1545.3"/>
+ <SupportPoint posX="862.089" posY="1372.8"/>
+ <SupportPoint posX="885.166" posY="1269.56"/>
+ <SupportPoint posX="962.509" posY="1001.22"/>
+ <SupportPoint posX="1142.94" posY="1001.22"/>
+ <SupportPoint posX="1142.94" posY="1001.22"/>
+ <SupportPoint posX="1142.94" posY="1001.22"/>
+ <SupportPoint posX="7561.86" posY="1001.22"/>
+ <SupportPoint posX="7667.53" posY="1001.22"/>
+ <SupportPoint posX="7710.47" posY="1028.06"/>
+ <SupportPoint posX="7770.78" posY="1114.94"/>
+ <SupportPoint posX="7822.14" posY="1188.93"/>
+ <SupportPoint posX="7837.73" posY="1461.99"/>
+ <SupportPoint posX="7842.08" posY="1575.71"/>
</SupportPoints>
</Transition>
<Transition from="ResolveLadderPreLandmark" to="GoToPreGraspLandmark" eventName="Success">
@@ -1271,13 +1247,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="906.952" posY="922.256"/>
- <SupportPoint posX="914.273" posY="926.472"/>
- <SupportPoint posX="921.927" posY="930.433"/>
- <SupportPoint posX="929.556" posY="933.5"/>
- <SupportPoint posX="965.269" posY="947.811"/>
- <SupportPoint posX="1006.57" posY="956.372"/>
- <SupportPoint posX="1043.25" posY="961.483"/>
+ <SupportPoint posX="864.274" posY="1452.41"/>
+ <SupportPoint posX="907.706" posY="1622.99"/>
+ <SupportPoint posX="1078.49" posY="2294.08"/>
+ <SupportPoint posX="1128.88" posY="2492.06"/>
</SupportPoints>
</Transition>
<Transition from="MovePlatformToLandmark" to="Failure" eventName="Failure">
@@ -1285,25 +1258,25 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1146.78" posY="2050.41"/>
- <SupportPoint posX="1167.24" posY="2182.99"/>
- <SupportPoint posX="1268.79" posY="2783.72"/>
- <SupportPoint posX="1429.17" posY="2783.72"/>
- <SupportPoint posX="1429.17" posY="2783.72"/>
- <SupportPoint posX="1429.17" posY="2783.72"/>
- <SupportPoint posX="4684.94" posY="2783.72"/>
- <SupportPoint posX="5113.51" posY="2783.72"/>
- <SupportPoint posX="5220.46" posY="2801.61"/>
- <SupportPoint posX="5649.03" posY="2801.61"/>
- <SupportPoint posX="5649.03" posY="2801.61"/>
- <SupportPoint posX="5649.03" posY="2801.61"/>
- <SupportPoint posX="8347.06" posY="2801.61"/>
- <SupportPoint posX="8431.26" posY="2801.61"/>
- <SupportPoint posX="8473.81" posY="2800.57"/>
- <SupportPoint posX="8523.39" posY="2732.61"/>
- <SupportPoint posX="8555.84" posY="2688.13"/>
- <SupportPoint posX="8587.02" posY="1860.26"/>
- <SupportPoint posX="8594.43" posY="1645.78"/>
+ <SupportPoint posX="1193.43" posY="2755.71"/>
+ <SupportPoint posX="1250.46" posY="2765.16"/>
+ <SupportPoint posX="1346.11" posY="2778.59"/>
+ <SupportPoint posX="1429.17" posY="2778.59"/>
+ <SupportPoint posX="1429.17" posY="2778.59"/>
+ <SupportPoint posX="1429.17" posY="2778.59"/>
+ <SupportPoint posX="3940.64" posY="2778.59"/>
+ <SupportPoint posX="4456.48" posY="2778.59"/>
+ <SupportPoint posX="4585.02" posY="2813.09"/>
+ <SupportPoint posX="5100.86" posY="2813.09"/>
+ <SupportPoint posX="5100.86" posY="2813.09"/>
+ <SupportPoint posX="5100.86" posY="2813.09"/>
+ <SupportPoint posX="7561.86" posY="2813.09"/>
+ <SupportPoint posX="7659.61" posY="2813.09"/>
+ <SupportPoint posX="7710.98" posY="2821.44"/>
+ <SupportPoint posX="7770.78" posY="2744.1"/>
+ <SupportPoint posX="7803.87" posY="2701.33"/>
+ <SupportPoint posX="7834.41" posY="1889.28"/>
+ <SupportPoint posX="7841.82" posY="1676.64"/>
</SupportPoints>
</Transition>
<Transition from="MovePlatformToLandmark" to="CloseRightHand" eventName="Success">
@@ -1314,10 +1287,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1150.75" posY="1998.87"/>
- <SupportPoint posX="1189.21" posY="1871.48"/>
- <SupportPoint posX="1358.89" posY="1309.42"/>
- <SupportPoint posX="1412.68" posY="1131.04"/>
+ <SupportPoint posX="1179.78" posY="2721"/>
+ <SupportPoint posX="1227.43" posY="2687.81"/>
+ <SupportPoint posX="1311.1" posY="2629.52"/>
+ <SupportPoint posX="1368.34" posY="2589.7"/>
</SupportPoints>
</Transition>
<Transition from="ResolveTargetLandmark" to="Failure" eventName="Failure">
@@ -1325,25 +1298,19 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2530.23" posY="2536.15"/>
- <SupportPoint posX="2583.51" posY="2575.85"/>
- <SupportPoint posX="2675.89" posY="2632.94"/>
- <SupportPoint posX="2766.36" posY="2632.94"/>
- <SupportPoint posX="2766.36" posY="2632.94"/>
- <SupportPoint posX="2766.36" posY="2632.94"/>
- <SupportPoint posX="5399.86" posY="2632.94"/>
- <SupportPoint posX="5622.58" posY="2632.94"/>
- <SupportPoint posX="5678.03" posY="2650.83"/>
- <SupportPoint posX="5900.75" posY="2650.83"/>
- <SupportPoint posX="5900.75" posY="2650.83"/>
- <SupportPoint posX="5900.75" posY="2650.83"/>
- <SupportPoint posX="8347.06" posY="2650.83"/>
- <SupportPoint posX="8431.26" posY="2650.83"/>
- <SupportPoint posX="8473.43" posY="2649.54"/>
- <SupportPoint posX="8523.39" posY="2581.83"/>
- <SupportPoint posX="8579.23" posY="2506.1"/>
- <SupportPoint posX="8592.9" posY="1836.61"/>
- <SupportPoint posX="8595.58" posY="1645.89"/>
+ <SupportPoint posX="2491.64" posY="1597.69"/>
+ <SupportPoint posX="2513.11" posY="1497.13"/>
+ <SupportPoint posX="2587.22" posY="1227.39"/>
+ <SupportPoint posX="2766.36" posY="1227.39"/>
+ <SupportPoint posX="2766.36" posY="1227.39"/>
+ <SupportPoint posX="2766.36" posY="1227.39"/>
+ <SupportPoint posX="7561.86" posY="1227.39"/>
+ <SupportPoint posX="7666.77" posY="1227.39"/>
+ <SupportPoint posX="7705.87" posY="1254.86"/>
+ <SupportPoint posX="7770.78" posY="1337.28"/>
+ <SupportPoint posX="7799.78" posY="1374.08"/>
+ <SupportPoint posX="7824.19" posY="1503.52"/>
+ <SupportPoint posX="7836.07" posY="1575.97"/>
</SupportPoints>
</Transition>
<Transition from="ResolveTargetLandmark" to="GoToTargetPose" eventName="Success">
@@ -1357,10 +1324,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2534.7" posY="2498.78"/>
- <SupportPoint posX="2572.27" posY="2498.78"/>
- <SupportPoint posX="2624.78" posY="2498.78"/>
- <SupportPoint posX="2670.4" posY="2498.78"/>
+ <SupportPoint posX="2534.7" posY="1634.99"/>
+ <SupportPoint posX="2572.27" posY="1634.99"/>
+ <SupportPoint posX="2624.78" posY="1634.99"/>
+ <SupportPoint posX="2670.4" posY="1634.99"/>
</SupportPoints>
</Transition>
<Transition from="NavigationPose_3" to="Failure" eventName="Failure">
@@ -1368,13 +1335,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="6932.56" posY="1701.14"/>
- <SupportPoint posX="7133.81" posY="1709.23"/>
- <SupportPoint posX="7904.56" posY="1730.64"/>
- <SupportPoint posX="8523.39" posY="1617.11"/>
- <SupportPoint posX="8526.71" posY="1616.5"/>
- <SupportPoint posX="8530.16" posY="1615.78"/>
- <SupportPoint posX="8533.61" posY="1615"/>
+ <SupportPoint posX="6969.61" posY="1796.83"/>
+ <SupportPoint posX="7108.63" posY="1824.8"/>
+ <SupportPoint posX="7507.81" posY="1885.16"/>
+ <SupportPoint posX="7770.78" posY="1723.15"/>
+ <SupportPoint posX="7789.94" posY="1711.33"/>
+ <SupportPoint posX="7805.79" posY="1692.47"/>
+ <SupportPoint posX="7817.8" posY="1674.5"/>
</SupportPoints>
</Transition>
<Transition from="NavigationPose_3" to="ResolveLandmark" eventName="Success">
@@ -1385,10 +1352,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="6933.19" posY="1682.34"/>
- <SupportPoint posX="6972.42" posY="1669.44"/>
- <SupportPoint posX="7027.24" posY="1651.46"/>
- <SupportPoint posX="7069.28" posY="1637.65"/>
+ <SupportPoint posX="6969.99" posY="1773.27"/>
+ <SupportPoint posX="7026.6" posY="1759.17"/>
+ <SupportPoint posX="7118.73" posY="1736.21"/>
+ <SupportPoint posX="7180.19" posY="1720.92"/>
</SupportPoints>
</Transition>
<Transition from="MoveToAfterFinished" to="Failure" eventName="Failure">
@@ -1396,13 +1363,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="8116.8" posY="583.005"/>
- <SupportPoint posX="8234.1" posY="609.328"/>
- <SupportPoint posX="8497.19" posY="671.428"/>
- <SupportPoint posX="8523.39" posY="703.5"/>
- <SupportPoint posX="8576.93" posY="768.794"/>
- <SupportPoint posX="8592.01" posY="1366.03"/>
- <SupportPoint posX="8595.46" posY="1544.98"/>
+ <SupportPoint posX="7299.02" posY="753.333"/>
+ <SupportPoint posX="7421.31" posY="751.928"/>
+ <SupportPoint posX="7706.76" posY="755.889"/>
+ <SupportPoint posX="7770.78" posY="822.333"/>
+ <SupportPoint posX="7823.04" posY="876.639"/>
+ <SupportPoint posX="7838.88" posY="1407.56"/>
+ <SupportPoint posX="7842.59" posY="1575.46"/>
</SupportPoints>
</Transition>
<Transition from="MoveToAfterFinished" to="NavigationPose_2" eventName="Success">
@@ -1410,13 +1377,16 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="8116.8" posY="574.444"/>
- <SupportPoint posX="8160.24" posY="576.233"/>
- <SupportPoint posX="8221.71" posY="575.594"/>
- <SupportPoint posX="8274.22" posY="564.222"/>
- <SupportPoint posX="8277.67" posY="563.456"/>
- <SupportPoint posX="8281.12" posY="562.561"/>
- <SupportPoint posX="8284.57" posY="561.539"/>
+ <SupportPoint posX="7298.77" posY="739.022"/>
+ <SupportPoint posX="7345.92" posY="723.561"/>
+ <SupportPoint posX="7412.49" posY="695.45"/>
+ <SupportPoint posX="7456.44" posY="651.111"/>
+ <SupportPoint posX="7473.31" posY="634.117"/>
+ <SupportPoint posX="7464.24" posY="620.955"/>
+ <SupportPoint posX="7479.44" posY="602.555"/>
+ <SupportPoint posX="7485.83" posY="594.761"/>
+ <SupportPoint posX="7493.5" posY="587.478"/>
+ <SupportPoint posX="7501.55" posY="580.833"/>
</SupportPoints>
</Transition>
<Transition from="MoveJoints" to="NavigationPose" eventName="EvJointTargetReached">
@@ -1424,13 +1394,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="214.281" posY="751.289"/>
- <SupportPoint posX="221.82" posY="753.589"/>
- <SupportPoint posX="229.551" posY="755.633"/>
- <SupportPoint posX="237" posY="757.167"/>
- <SupportPoint posX="341.509" posY="778.505"/>
- <SupportPoint posX="465.786" posY="784.511"/>
- <SupportPoint posX="539.169" posY="786.044"/>
+ <SupportPoint posX="214.013" posY="1180.49"/>
+ <SupportPoint posX="294.641" posY="1228.67"/>
+ <SupportPoint posX="453.443" posY="1323.61"/>
+ <SupportPoint posX="540.958" posY="1375.87"/>
</SupportPoints>
</Transition>
<Transition from="MoveJoints" to="Failure" eventName="EvTimeout">
@@ -1438,28 +1405,19 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="214.153" posY="721.389"/>
- <SupportPoint posX="221.782" posY="719.983"/>
- <SupportPoint posX="229.576" posY="718.578"/>
- <SupportPoint posX="237" posY="717.556"/>
- <SupportPoint posX="398.256" posY="695.322"/>
- <SupportPoint posX="439.668" posY="693.278"/>
- <SupportPoint posX="602.444" posY="693.278"/>
- <SupportPoint posX="602.444" posY="693.278"/>
- <SupportPoint posX="602.444" posY="693.278"/>
- <SupportPoint posX="4171.92" posY="693.278"/>
- <SupportPoint posX="5225.19" posY="693.278"/>
- <SupportPoint posX="5488.28" posY="718.833"/>
- <SupportPoint posX="6541.56" posY="718.833"/>
- <SupportPoint posX="6541.56" posY="718.833"/>
- <SupportPoint posX="6541.56" posY="718.833"/>
- <SupportPoint posX="7899.19" posY="718.833"/>
- <SupportPoint posX="8041.28" posY="718.833"/>
- <SupportPoint posX="8443.02" posY="740.939"/>
- <SupportPoint posX="8523.39" posY="858.111"/>
- <SupportPoint posX="8562.11" posY="914.717"/>
- <SupportPoint posX="8586.89" posY="1388.77"/>
- <SupportPoint posX="8594.05" posY="1545.38"/>
+ <SupportPoint posX="204.136" posY="1113.54"/>
+ <SupportPoint posX="277.237" posY="1049.65"/>
+ <SupportPoint posX="439.949" posY="925.833"/>
+ <SupportPoint posX="602.444" posY="925.833"/>
+ <SupportPoint posX="602.444" posY="925.833"/>
+ <SupportPoint posX="602.444" posY="925.833"/>
+ <SupportPoint posX="7561.86" posY="925.833"/>
+ <SupportPoint posX="7667.53" posY="925.833"/>
+ <SupportPoint posX="7711.23" posY="952.155"/>
+ <SupportPoint posX="7770.78" posY="1039.56"/>
+ <SupportPoint posX="7800.81" posY="1083.51"/>
+ <SupportPoint posX="7830.07" posY="1441.93"/>
+ <SupportPoint posX="7840.03" posY="1575.84"/>
</SupportPoints>
</Transition>
<Transition from="ResolveLandmark" to="Failure" eventName="Failure">
@@ -1467,13 +1425,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="7181.85" posY="1618.26"/>
- <SupportPoint posX="7362.66" posY="1622.07"/>
- <SupportPoint posX="7999.37" posY="1632.33"/>
- <SupportPoint posX="8523.39" posY="1601.78"/>
- <SupportPoint posX="8526.58" posY="1601.59"/>
- <SupportPoint posX="8529.91" posY="1601.38"/>
- <SupportPoint posX="8533.23" posY="1601.15"/>
+ <SupportPoint posX="7293.27" posY="1715.35"/>
+ <SupportPoint posX="7369.94" posY="1728.95"/>
+ <SupportPoint posX="7520.46" posY="1748.47"/>
+ <SupportPoint posX="7644.28" posY="1720.6"/>
+ <SupportPoint posX="7693.09" posY="1709.61"/>
+ <SupportPoint posX="7743.94" posY="1685.25"/>
+ <SupportPoint posX="7782.02" posY="1664.02"/>
</SupportPoints>
</Transition>
<Transition from="ResolveLandmark" to="GoToGoalPositionWithObstacleAvoidance" eventName="Success">
@@ -1484,10 +1442,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="7181.98" posY="1605.99"/>
- <SupportPoint posX="7219.67" posY="1597.61"/>
- <SupportPoint posX="7272.57" posY="1585.86"/>
- <SupportPoint posX="7318.19" posY="1575.7"/>
+ <SupportPoint posX="7293.27" posY="1692.79"/>
+ <SupportPoint posX="7339.53" posY="1681.29"/>
+ <SupportPoint posX="7409.55" posY="1663.9"/>
+ <SupportPoint posX="7466.41" posY="1649.75"/>
</SupportPoints>
</Transition>
<Transition from="GoToGoalPositionWithObstacleAvoidance" to="Failure" eventName="Failure">
@@ -1495,13 +1453,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="7496.31" posY="1557.36"/>
- <SupportPoint posX="7720.05" posY="1565.09"/>
- <SupportPoint posX="8333.51" posY="1586.3"/>
- <SupportPoint posX="8532.72" posY="1593.19"/>
+ <SupportPoint posX="7644.53" posY="1626.04"/>
+ <SupportPoint posX="7687.98" posY="1626.04"/>
+ <SupportPoint posX="7740.37" posY="1626.04"/>
+ <SupportPoint posX="7780.62" posY="1626.04"/>
</SupportPoints>
</Transition>
- <Transition from="GoToGoalPositionWithObstacleAvoidance" to="ResolveLandmark_2" eventName="Success">
+ <Transition from="GoToGoalPositionWithObstacleAvoidance" to="Success" eventName="Success">
<ParameterMappings>
<ParameterMapping sourceType="Parent" from="LandmarkAfterFinished" to="LandmarkName"/>
<ParameterMapping sourceType="Parent" from="GraphName" to="SceneName"/>
@@ -1509,10 +1467,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="7496.18" posY="1532.02"/>
- <SupportPoint posX="7551.89" posY="1516.93"/>
- <SupportPoint posX="7624.09" posY="1497.26"/>
- <SupportPoint posX="7674.94" posY="1483.33"/>
+ <SupportPoint posX="7571.7" posY="1585.55"/>
+ <SupportPoint posX="7614.38" posY="1408.45"/>
+ <SupportPoint posX="7784.45" posY="703.5"/>
+ <SupportPoint posX="7831.47" posY="508.383"/>
</SupportPoints>
</Transition>
<Transition from="GoToGoalPositionWithObstacleAvoidance_2" to="Failure" eventName="Failure">
@@ -1520,13 +1478,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="8148.11" posY="1401.42"/>
- <SupportPoint posX="8246.75" posY="1402.96"/>
- <SupportPoint posX="8408.64" posY="1417.91"/>
- <SupportPoint posX="8523.39" posY="1491.89"/>
- <SupportPoint posX="8544.09" posY="1505.18"/>
- <SupportPoint posX="8560.57" posY="1526.71"/>
- <SupportPoint posX="8572.58" posY="1546.65"/>
+ <SupportPoint posX="7329.94" posY="630.922"/>
+ <SupportPoint posX="7453.25" posY="624.15"/>
+ <SupportPoint posX="7674.56" posY="633.733"/>
+ <SupportPoint posX="7770.78" posY="773.778"/>
+ <SupportPoint posX="7816.27" posY="839.839"/>
+ <SupportPoint posX="7836.84" posY="1401.93"/>
+ <SupportPoint posX="7842.21" posY="1575.33"/>
</SupportPoints>
</Transition>
<Transition from="GoToGoalPositionWithObstacleAvoidance_2" to="NavigationPose_2" eventName="Success">
@@ -1534,13 +1492,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="8064.41" posY="1378.42"/>
- <SupportPoint posX="8079.49" posY="1275.56"/>
- <SupportPoint posX="8145.29" posY="879.578"/>
- <SupportPoint posX="8297.22" posY="593.611"/>
- <SupportPoint posX="8300.29" posY="587.733"/>
- <SupportPoint posX="8303.99" posY="581.983"/>
- <SupportPoint posX="8308.08" posY="576.489"/>
+ <SupportPoint posX="7330.2" posY="613.033"/>
+ <SupportPoint posX="7383.61" posY="596.805"/>
+ <SupportPoint posX="7450.95" posY="576.233"/>
+ <SupportPoint posX="7499.12" posY="561.539"/>
</SupportPoints>
</Transition>
<Transition from="ResolveLandmark_2" to="Failure" eventName="Failure">
@@ -1548,13 +1503,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="7787.52" posY="1468.63"/>
- <SupportPoint posX="7912.99" posY="1475.15"/>
- <SupportPoint posX="8251.09" posY="1498.28"/>
- <SupportPoint posX="8523.39" posY="1567.28"/>
- <SupportPoint posX="8526.84" posY="1568.17"/>
- <SupportPoint posX="8530.42" posY="1569.18"/>
- <SupportPoint posX="8533.99" posY="1570.27"/>
+ <SupportPoint posX="6970.12" posY="779.016"/>
+ <SupportPoint posX="7150.93" posY="802.655"/>
+ <SupportPoint posX="7756.72" posY="883.028"/>
+ <SupportPoint posX="7770.78" posY="897.722"/>
+ <SupportPoint posX="7817.54" posY="946.661"/>
+ <SupportPoint posX="7836.97" posY="1419.57"/>
+ <SupportPoint posX="7842.08" posY="1576.09"/>
</SupportPoints>
</Transition>
<Transition from="ResolveLandmark_2" to="GoToGoalPositionWithObstacleAvoidance_2" eventName="Success">
@@ -1565,10 +1520,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="7787.77" posY="1456.62"/>
- <SupportPoint posX="7833.77" posY="1447.68"/>
- <SupportPoint posX="7903.41" posY="1434.26"/>
- <SupportPoint posX="7960.91" posY="1423.02"/>
+ <SupportPoint posX="6969.99" posY="751.928"/>
+ <SupportPoint posX="7016.25" posY="732.889"/>
+ <SupportPoint posX="7086.27" posY="704.011"/>
+ <SupportPoint posX="7143.77" posY="680.372"/>
</SupportPoints>
</Transition>
</Transitions>
diff --git a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/RemoveGuardV2.xml b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/RemoveGuardV2.xml
index c1dcc6b67..b2722056a 100644
--- a/source/armar6/skills/statecharts/Armar6BoardSupportDemo/RemoveGuardV2.xml
+++ b/source/armar6/skills/statecharts/Armar6BoardSupportDemo/RemoveGuardV2.xml
@@ -288,7 +288,7 @@
<SupportPoint posX="3455.34" posY="621.646"/>
</SupportPoints>
</Transition>
- <Transition from="RaiseAndGoToDefaultPose" to="GoToLandmarkWithObstacleAvoidance" eventName="Success">
+ <Transition from="RaiseAndGoToDefaultPose" to="Success" eventName="Success">
<ParameterMappings>
<ParameterMapping sourceType="Parent" from="LandmarkAfterPutdown" to="LandmarkName"/>
<ParameterMapping sourceType="Value" from="" to="RotateIntoMovementDirection">
@@ -298,13 +298,7 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2999.68" posY="761.728"/>
- <SupportPoint posX="3007.35" posY="764.194"/>
- <SupportPoint posX="3015.14" posY="766.584"/>
- <SupportPoint posX="3022.56" posY="768.667"/>
- <SupportPoint posX="3059.36" posY="778.927"/>
- <SupportPoint posX="3100.37" posY="788.357"/>
- <SupportPoint posX="3135.77" posY="795.883"/>
+ <SupportPoint posX="3208.1" posY="879.563"/>
</SupportPoints>
</Transition>
<Transition from="NavigationPose" to="Failure" eventName="Failure">
--
GitLab
From 419d7c20d2b9070daee9b9ccd2edb64760bedd99 Mon Sep 17 00:00:00 2001
From: Jianfeng Gao <jianfeng.gao@kit.edu>
Date: Mon, 20 Jan 2025 18:46:43 +0100
Subject: [PATCH 13/22] remove go home after placing of spray bottle
---
.../BringObjectAndPutAwayTask.xml | 502 ++++++++----------
1 file changed, 230 insertions(+), 272 deletions(-)
diff --git a/source/armar6/skills/statecharts/Armar6HighlevelTaskGroup/BringObjectAndPutAwayTask.xml b/source/armar6/skills/statecharts/Armar6HighlevelTaskGroup/BringObjectAndPutAwayTask.xml
index 93fa82de9..b3cdb5fab 100644
--- a/source/armar6/skills/statecharts/Armar6HighlevelTaskGroup/BringObjectAndPutAwayTask.xml
+++ b/source/armar6/skills/statecharts/Armar6HighlevelTaskGroup/BringObjectAndPutAwayTask.xml
@@ -1,5 +1,5 @@
<?xml version="1.0" encoding="utf-8"?>
-<State version="1.2" name="BringObjectAndPutAwayTask" uuid="3680C1FE-3C6D-49BA-99B9-464106A0EDA4" width="3752.78" height="1369.81" type="Normal State">
+<State version="1.2" name="BringObjectAndPutAwayTask" uuid="3680C1FE-3C6D-49BA-99B9-464106A0EDA4" width="3499.78" height="1161.56" type="Normal State">
<InputParameters>
<Parameter name="DefaultObjectName" type="::armarx::StringVariantData" docType="string" optional="no">
<DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Maintenance/spraybottle"}}' docValue="Maintenance/spraybottle"/>
@@ -30,21 +30,21 @@
<Parameter name="ObjectName" type="::armarx::StringVariantData" docType="string" optional="no"/>
</LocalParameters>
<Substates>
- <RemoteState name="BringObjectToLandmark" refuuid="D38220A7-F179-4C96-BE6D-937C53EC2376" proxyName="Armar6GraspingGroupRemoteStateOfferer" left="363.333" top="294.944" boundingSquareSize="99.6636"/>
- <RemoteState name="CartesianGazeControl" refuuid="0967749A-6240-4FA3-99E9-4EF063B4A5F9" proxyName="MotionControlGroupRemoteStateOfferer" left="1894.11" top="594.611" boundingSquareSize="99.6636"/>
- <EndState name="Failure" event="Failure" left="3622.78" top="578.333" boundingSquareSize="99.6636"/>
- <RemoteState name="GoHome" refuuid="19478C7D-BC94-4BA1-8C85-33EC9EECCC74" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="3373.78" top="932.222" boundingSquareSize="99.6636"/>
- <RemoteState name="HandOverToRobot" refuuid="3FAF4F4E-8A9A-4F23-8D15-3752F7B20612" proxyName="HandOverGroupRemoteStateOfferer" left="861.667" top="334.278" boundingSquareSize="99.6636"/>
- <RemoteState name="LookAhead" refuuid="1A25B687-CFBC-4CA3-A1C7-32EEAF004C45" proxyName="MotionControlGroupRemoteStateOfferer" left="2392.44" top="706.111" boundingSquareSize="99.6636"/>
- <RemoteState name="LookStraight" refuuid="2D4453C8-0CEB-4773-9493-ADD75AD31346" proxyName="Armar6HeadGroupRemoteStateOfferer" left="1110.83" top="404.667" boundingSquareSize="99.6636"/>
- <RemoteState name="MoveToLocationTask_2" refuuid="6A0FD8EC-064D-4A4A-B036-3C2995564588" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="1644.94" top="587.611" boundingSquareSize="99.6636"/>
- <RemoteState name="NavigationPose_2" refuuid="E7374741-A6DF-4E3F-B05D-68A1EB44D6A5" proxyName="Armar6PosesRemoteStateOfferer" left="3102.25" top="856.778" boundingSquareSize="99.6636"/>
- <RemoteState name="PlaceObjectOnTable" refuuid="37E2E508-1C2C-4DE1-B783-9C92862A952C" proxyName="Armar6GraspingGroupRemoteStateOfferer" left="2143.28" top="639.833" boundingSquareSize="99.6636"/>
- <LocalState name="PrepareNavigation" refuuid="04B94514-D96B-45AE-9035-4E396ACD9E5B" left="114.167" top="868.5" boundingSquareSize="99.6636"/>
- <RemoteState name="ReceiveFromRobot" refuuid="6546C898-D397-4835-A12B-0CE39B852D7D" proxyName="HandOverGroupRemoteStateOfferer" left="612.5" top="296.944" boundingSquareSize="99.6636"/>
- <RemoteState name="ResolveWorkbenchEntryWaypoints" refuuid="68599B94-C644-4023-969B-486210BEFD4A" proxyName="PlatformGroupRemoteStateOfferer" left="1359.89" top="481" boundingSquareSize="135.442"/>
- <RemoteState name="RetreatPlatform" refuuid="19478C7D-BC94-4BA1-8C85-33EC9EECCC74" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="2830.72" top="781.444" boundingSquareSize="99.6636"/>
- <EndState name="Success" event="Success" left="3622.78" top="934.833" boundingSquareSize="99.6636"/>
+ <RemoteState name="BringObjectToLandmark" refuuid="D38220A7-F179-4C96-BE6D-937C53EC2376" proxyName="Armar6GraspingGroupRemoteStateOfferer" left="363.333" top="969.611" boundingSquareSize="99.6636"/>
+ <RemoteState name="CartesianGazeControl" refuuid="0967749A-6240-4FA3-99E9-4EF063B4A5F9" proxyName="MotionControlGroupRemoteStateOfferer" left="1894.11" top="442.556" boundingSquareSize="99.6636"/>
+ <EndState name="Failure" event="Failure" left="3369.94" top="642.222" boundingSquareSize="99.6636"/>
+ <RemoteState name="GoHome" refuuid="19478C7D-BC94-4BA1-8C85-33EC9EECCC74" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="3120.78" top="140" boundingSquareSize="99.6636"/>
+ <RemoteState name="HandOverToRobot" refuuid="3FAF4F4E-8A9A-4F23-8D15-3752F7B20612" proxyName="HandOverGroupRemoteStateOfferer" left="861.667" top="932.278" boundingSquareSize="99.6636"/>
+ <RemoteState name="LookAhead" refuuid="1A25B687-CFBC-4CA3-A1C7-32EEAF004C45" proxyName="MotionControlGroupRemoteStateOfferer" left="2392.44" top="570.667" boundingSquareSize="99.6636"/>
+ <RemoteState name="LookStraight" refuuid="2D4453C8-0CEB-4773-9493-ADD75AD31346" proxyName="Armar6HeadGroupRemoteStateOfferer" left="1110.83" top="805.889" boundingSquareSize="99.6636"/>
+ <RemoteState name="MoveToLocationTask_2" refuuid="6A0FD8EC-064D-4A4A-B036-3C2995564588" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="1644.94" top="435.556" boundingSquareSize="99.6636"/>
+ <RemoteState name="NavigationPose_2" refuuid="E7374741-A6DF-4E3F-B05D-68A1EB44D6A5" proxyName="Armar6PosesRemoteStateOfferer" left="3120.78" top="449.167" boundingSquareSize="99.6636"/>
+ <RemoteState name="PlaceObjectOnTable" refuuid="37E2E508-1C2C-4DE1-B783-9C92862A952C" proxyName="Armar6GraspingGroupRemoteStateOfferer" left="2143.28" top="594.667" boundingSquareSize="99.6636"/>
+ <LocalState name="PrepareNavigation" refuuid="04B94514-D96B-45AE-9035-4E396ACD9E5B" left="114.167" top="1056.33" boundingSquareSize="99.6636"/>
+ <RemoteState name="ReceiveFromRobot" refuuid="6546C898-D397-4835-A12B-0CE39B852D7D" proxyName="HandOverGroupRemoteStateOfferer" left="612.5" top="971.611" boundingSquareSize="99.6636"/>
+ <RemoteState name="ResolveWorkbenchEntryWaypoints" refuuid="68599B94-C644-4023-969B-486210BEFD4A" proxyName="PlatformGroupRemoteStateOfferer" left="1359.89" top="713.556" boundingSquareSize="135.442"/>
+ <RemoteState name="RetreatPlatform" refuuid="19478C7D-BC94-4BA1-8C85-33EC9EECCC74" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="2830.72" top="542.5" boundingSquareSize="99.6636"/>
+ <EndState name="Success" event="Success" left="3369.94" top="285.722" boundingSquareSize="99.6636"/>
</Substates>
<Events>
<Event name="Failure">
@@ -56,10 +56,10 @@
<ParameterMapping sourceType="Parent" from="RetreatLandmark" to="targetLandmark"/>
</ParameterMappings>
<SupportPoints>
- <SupportPoint posX="67.1067" posY="887.5"/>
- <SupportPoint posX="76.5839" posY="887.5"/>
- <SupportPoint posX="88.7241" posY="887.5"/>
- <SupportPoint posX="101.186" posY="887.5"/>
+ <SupportPoint posX="67.1067" posY="1075.33"/>
+ <SupportPoint posX="76.5839" posY="1075.33"/>
+ <SupportPoint posX="88.7241" posY="1075.33"/>
+ <SupportPoint posX="101.186" posY="1075.33"/>
</SupportPoints>
</StartState>
<Transitions>
@@ -68,25 +68,25 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="712.755" posY="298.01"/>
- <SupportPoint posX="762.857" posY="287.187"/>
- <SupportPoint posX="842.13" posY="272.889"/>
- <SupportPoint posX="911.667" posY="272.889"/>
- <SupportPoint posX="911.667" posY="272.889"/>
- <SupportPoint posX="911.667" posY="272.889"/>
- <SupportPoint posX="1819.53" posY="272.889"/>
- <SupportPoint posX="2041.48" posY="272.889"/>
- <SupportPoint posX="2095.91" posY="238.389"/>
- <SupportPoint posX="2317.86" posY="238.389"/>
- <SupportPoint posX="2317.86" posY="238.389"/>
- <SupportPoint posX="2317.86" posY="238.389"/>
- <SupportPoint posX="3423.78" posY="238.389"/>
- <SupportPoint posX="3514.63" posY="238.389"/>
- <SupportPoint posX="3545.17" posY="269.554"/>
- <SupportPoint posX="3600.11" posY="341.889"/>
- <SupportPoint posX="3626.69" posY="376.874"/>
- <SupportPoint posX="3651.73" posY="496.589"/>
- <SupportPoint posX="3664.38" posY="565.462"/>
+ <SupportPoint posX="712.755" posY="996.546"/>
+ <SupportPoint posX="762.857" posY="1007.37"/>
+ <SupportPoint posX="842.13" posY="1021.67"/>
+ <SupportPoint posX="911.667" posY="1021.67"/>
+ <SupportPoint posX="911.667" posY="1021.67"/>
+ <SupportPoint posX="911.667" posY="1021.67"/>
+ <SupportPoint posX="1570.36" posY="1021.67"/>
+ <SupportPoint posX="1902.97" posY="1021.67"/>
+ <SupportPoint posX="1985.26" posY="1056.17"/>
+ <SupportPoint posX="2317.86" posY="1056.17"/>
+ <SupportPoint posX="2317.86" posY="1056.17"/>
+ <SupportPoint posX="2317.86" posY="1056.17"/>
+ <SupportPoint posX="3170.78" posY="1056.17"/>
+ <SupportPoint posX="3258.18" posY="1056.17"/>
+ <SupportPoint posX="3292.17" posY="1037.24"/>
+ <SupportPoint posX="3347.11" posY="969.278"/>
+ <SupportPoint posX="3376.63" posY="932.682"/>
+ <SupportPoint posX="3400.91" posY="802.553"/>
+ <SupportPoint posX="3412.53" posY="729.643"/>
</SupportPoints>
</Transition>
<Transition from="ReceiveFromRobot" to="HandOverToRobot" eventName="Success">
@@ -97,13 +97,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="691.339" posY="322.748"/>
- <SupportPoint posX="704.501" posY="328.012"/>
- <SupportPoint posX="720.434" posY="333.622"/>
- <SupportPoint posX="735.333" posY="336.778"/>
- <SupportPoint posX="772.427" posY="344.649"/>
- <SupportPoint posX="814.798" posY="347.601"/>
- <SupportPoint posX="848.711" posY="348.546"/>
+ <SupportPoint posX="712.921" posY="976.855"/>
+ <SupportPoint posX="752.059" posY="970.837"/>
+ <SupportPoint posX="806.544" posY="962.454"/>
+ <SupportPoint posX="848.57" posY="955.989"/>
</SupportPoints>
</Transition>
<Transition from="HandOverToRobot" to="Failure" eventName="Failure">
@@ -111,25 +108,22 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="961.781" posY="348.278"/>
- <SupportPoint posX="1012.09" posY="348.278"/>
- <SupportPoint posX="1091.83" posY="348.278"/>
- <SupportPoint posX="1160.83" posY="348.278"/>
- <SupportPoint posX="1160.83" posY="348.278"/>
- <SupportPoint posX="1160.83" posY="348.278"/>
- <SupportPoint posX="1819.53" posY="348.278"/>
- <SupportPoint posX="2194.04" posY="348.278"/>
- <SupportPoint posX="2287.07" posY="313.778"/>
- <SupportPoint posX="2661.58" posY="313.778"/>
- <SupportPoint posX="2661.58" posY="313.778"/>
- <SupportPoint posX="2661.58" posY="313.778"/>
- <SupportPoint posX="3423.78" posY="313.778"/>
- <SupportPoint posX="3514.12" posY="313.778"/>
- <SupportPoint posX="3541.97" posY="345.633"/>
- <SupportPoint posX="3600.11" posY="414.722"/>
- <SupportPoint posX="3636.66" posY="458.141"/>
- <SupportPoint posX="3655.57" posY="521.404"/>
- <SupportPoint posX="3664.77" posY="565.232"/>
+ <SupportPoint posX="961.781" posY="946.278"/>
+ <SupportPoint posX="1012.09" posY="946.278"/>
+ <SupportPoint posX="1091.83" posY="946.278"/>
+ <SupportPoint posX="1160.83" posY="946.278"/>
+ <SupportPoint posX="1160.83" posY="946.278"/>
+ <SupportPoint posX="1160.83" posY="946.278"/>
+ <SupportPoint posX="1819.53" posY="946.278"/>
+ <SupportPoint posX="2041.48" posY="946.278"/>
+ <SupportPoint posX="2095.91" posY="980.778"/>
+ <SupportPoint posX="2317.86" posY="980.778"/>
+ <SupportPoint posX="2317.86" posY="980.778"/>
+ <SupportPoint posX="2317.86" posY="980.778"/>
+ <SupportPoint posX="3170.78" posY="980.778"/>
+ <SupportPoint posX="3299.71" posY="980.778"/>
+ <SupportPoint posX="3374.33" posY="815.088"/>
+ <SupportPoint posX="3404.36" posY="729.349"/>
</SupportPoints>
</Transition>
<Transition from="HandOverToRobot" transitionCodeEnabled="1" toClass="LookStraight" toGroup="Armar6HeadGroup" toPackage="armar6_skills" fromClass="HandOverToRobot" fromGroup="HandOverGroup" fromPackage="RobotSkillTemplates" to="LookStraight" eventName="Success">
@@ -139,13 +133,10 @@
</ParameterMappingsToParentsLocal>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="923.537" posY="362.384"/>
- <SupportPoint posX="936.813" posY="377.207"/>
- <SupportPoint posX="959.737" posY="400.002"/>
- <SupportPoint posX="984.5" posY="412.167"/>
- <SupportPoint posX="1019.75" posY="429.481"/>
- <SupportPoint posX="1062.98" posY="438.118"/>
- <SupportPoint posX="1097.83" posY="442.412"/>
+ <SupportPoint posX="947.368" posY="932.184"/>
+ <SupportPoint posX="986.647" posY="916.672"/>
+ <SupportPoint posX="1050.52" posY="891.448"/>
+ <SupportPoint posX="1098.18" posY="872.627"/>
</SupportPoints>
</Transition>
<Transition from="BringObjectToLandmark" to="Failure" eventName="Failure">
@@ -153,25 +144,19 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="428.756" posY="294.994"/>
- <SupportPoint posX="465.761" posY="260.763"/>
- <SupportPoint posX="563.881" posY="179.611"/>
- <SupportPoint posX="662.5" posY="179.611"/>
- <SupportPoint posX="662.5" posY="179.611"/>
- <SupportPoint posX="662.5" posY="179.611"/>
- <SupportPoint posX="1819.53" posY="179.611"/>
- <SupportPoint posX="2041.09" posY="179.611"/>
- <SupportPoint posX="2096.29" posY="163"/>
- <SupportPoint posX="2317.86" posY="163"/>
- <SupportPoint posX="2317.86" posY="163"/>
- <SupportPoint posX="2317.86" posY="163"/>
- <SupportPoint posX="3423.78" posY="163"/>
- <SupportPoint posX="3514.88" posY="163"/>
- <SupportPoint posX="3547.08" posY="193.616"/>
- <SupportPoint posX="3600.11" posY="267.778"/>
- <SupportPoint posX="3633.84" posY="315.03"/>
- <SupportPoint posX="3657.36" posY="481.001"/>
- <SupportPoint posX="3667.45" posY="565.334"/>
+ <SupportPoint posX="414.828" posY="969.789"/>
+ <SupportPoint posX="425.958" posY="863.836"/>
+ <SupportPoint posX="499.711" posY="225.611"/>
+ <SupportPoint posX="662.5" posY="225.611"/>
+ <SupportPoint posX="662.5" posY="225.611"/>
+ <SupportPoint posX="662.5" posY="225.611"/>
+ <SupportPoint posX="2317.86" posY="225.611"/>
+ <SupportPoint posX="2432.99" posY="225.611"/>
+ <SupportPoint posX="3268.78" posY="257.619"/>
+ <SupportPoint posX="3347.11" posY="341.889"/>
+ <SupportPoint posX="3385.32" posY="383.033"/>
+ <SupportPoint posX="3406.66" posY="544.953"/>
+ <SupportPoint posX="3415.34" posY="628.597"/>
</SupportPoints>
</Transition>
<Transition from="BringObjectToLandmark" transitionCodeEnabled="1" toClass="ReceiveFromRobot" toGroup="HandOverGroup" toPackage="RobotSkillTemplates" fromClass="BringObjectToLandmark" fromGroup="Armar6GraspingGroup" fromPackage="armar6_skills" to="ReceiveFromRobot" eventName="Success">
@@ -186,10 +171,10 @@
</ParameterMappingsToParentsLocal>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="463.754" posY="309.944"/>
- <SupportPoint posX="502.893" posY="309.944"/>
- <SupportPoint posX="557.377" posY="309.944"/>
- <SupportPoint posX="599.403" posY="309.944"/>
+ <SupportPoint posX="463.754" posY="984.611"/>
+ <SupportPoint posX="502.893" posY="984.611"/>
+ <SupportPoint posX="557.377" posY="984.611"/>
+ <SupportPoint posX="599.403" posY="984.611"/>
</SupportPoints>
</Transition>
<Transition from="NavigationPose_2" to="Failure" eventName="Failure">
@@ -197,16 +182,16 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="3202.21" posY="884.459"/>
- <SupportPoint posX="3306.22" posY="874.479"/>
- <SupportPoint posX="3541.59" posY="846.611"/>
- <SupportPoint posX="3600.11" posY="794.222"/>
- <SupportPoint posX="3637.29" posY="760.974"/>
- <SupportPoint posX="3655.82" posY="706.004"/>
- <SupportPoint posX="3664.89" posY="665.754"/>
+ <SupportPoint posX="3220.87" posY="506.16"/>
+ <SupportPoint posX="3257.92" posY="525.966"/>
+ <SupportPoint posX="3308.39" posY="555.891"/>
+ <SupportPoint posX="3347.11" posY="589.778"/>
+ <SupportPoint posX="3361.17" posY="602.096"/>
+ <SupportPoint posX="3374.71" posY="617.327"/>
+ <SupportPoint posX="3386.08" posY="631.702"/>
</SupportPoints>
</Transition>
- <Transition from="NavigationPose_2" to="GoHome" eventName="Success">
+ <Transition from="NavigationPose_2" to="Success" eventName="Success">
<ParameterMappings>
<ParameterMapping sourceType="Parent" from="HomeLandmark" to="LandmarkName"/>
<ParameterMapping sourceType="Value" from="" to="PredefinedRotationDirection">
@@ -219,13 +204,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="3202.21" posY="907.778"/>
- <SupportPoint posX="3216.65" posY="912.851"/>
- <SupportPoint posX="3232.62" posY="918.026"/>
- <SupportPoint posX="3247.44" posY="922"/>
- <SupportPoint posX="3284.63" posY="931.928"/>
- <SupportPoint posX="3327.05" posY="940.017"/>
- <SupportPoint posX="3360.91" posY="945.728"/>
+ <SupportPoint posX="3221.25" posY="449.107"/>
+ <SupportPoint posX="3260.99" posY="423.782"/>
+ <SupportPoint posX="3316.7" posY="388.362"/>
+ <SupportPoint posX="3358.99" posY="361.464"/>
</SupportPoints>
</Transition>
<Transition from="CartesianGazeControl" to="Failure" eventName="Failure">
@@ -233,19 +215,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1993.94" posY="602.773"/>
- <SupportPoint posX="2001.61" posY="601.124"/>
- <SupportPoint posX="2009.41" posY="599.681"/>
- <SupportPoint posX="2016.94" posY="598.722"/>
- <SupportPoint posX="2475.54" posY="540.059"/>
- <SupportPoint posX="2594.76" posY="583.772"/>
- <SupportPoint posX="3057.06" posY="585.944"/>
- <SupportPoint posX="3298.43" posY="587.082"/>
- <SupportPoint posX="3362.06" posY="551.266"/>
- <SupportPoint posX="3600.11" posY="591.056"/>
- <SupportPoint posX="3603.56" posY="591.618"/>
- <SupportPoint posX="3607.01" posY="592.321"/>
- <SupportPoint posX="3610.46" posY="593.126"/>
+ <SupportPoint posX="1994.07" posY="459.764"/>
+ <SupportPoint posX="2231.23" posY="437.467"/>
+ <SupportPoint posX="3237.48" posY="348.431"/>
+ <SupportPoint posX="3347.11" posY="431.333"/>
+ <SupportPoint posX="3378.67" posY="455.189"/>
+ <SupportPoint posX="3401.29" posY="563.813"/>
+ <SupportPoint posX="3412.28" posY="629.095"/>
</SupportPoints>
</Transition>
<Transition from="CartesianGazeControl" to="PlaceObjectOnTable" eventName="Success">
@@ -256,13 +232,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1994.33" posY="633.171"/>
- <SupportPoint posX="2001.87" posY="635.19"/>
- <SupportPoint posX="2009.53" posY="637.004"/>
- <SupportPoint posX="2016.94" posY="638.333"/>
- <SupportPoint posX="2054.13" posY="645.003"/>
- <SupportPoint posX="2096.29" posY="647.917"/>
- <SupportPoint posX="2130.16" posY="649.143"/>
+ <SupportPoint posX="1983.59" posY="487.236"/>
+ <SupportPoint posX="2032.15" posY="515.092"/>
+ <SupportPoint posX="2113.54" posY="561.871"/>
+ <SupportPoint posX="2159.29" posY="588.181"/>
</SupportPoints>
</Transition>
<Transition from="LookAhead" to="RetreatPlatform" eventName="EvJointTargetReached">
@@ -275,13 +248,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2492.53" posY="779.336"/>
- <SupportPoint posX="2499.82" posY="783.259"/>
- <SupportPoint posX="2507.61" posY="786.709"/>
- <SupportPoint posX="2515.28" posY="789.111"/>
- <SupportPoint posX="2617.63" posY="821.068"/>
- <SupportPoint posX="2744" posY="817.542"/>
- <SupportPoint posX="2818.11" posY="811.434"/>
+ <SupportPoint posX="2492.28" posY="602.875"/>
+ <SupportPoint posX="2572.39" posY="595.157"/>
+ <SupportPoint posX="2729.82" posY="580.028"/>
+ <SupportPoint posX="2817.6" posY="571.569"/>
</SupportPoints>
</Transition>
<Transition from="LookAhead" to="Failure" eventName="EvTimeout">
@@ -289,16 +259,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2492.28" posY="739.265"/>
- <SupportPoint posX="2616.22" posY="729.669"/>
- <SupportPoint posX="2947.93" posY="703.679"/>
- <SupportPoint posX="3224.44" posY="679.222"/>
- <SupportPoint posX="3391.45" posY="664.438"/>
- <SupportPoint posX="3436.81" posY="681.944"/>
- <SupportPoint posX="3600.11" posY="643.444"/>
- <SupportPoint posX="3603.56" posY="642.639"/>
- <SupportPoint posX="3607.01" posY="641.707"/>
- <SupportPoint posX="3610.46" posY="640.697"/>
+ <SupportPoint posX="2492.66" posY="611.334"/>
+ <SupportPoint posX="2656.34" posY="623.332"/>
+ <SupportPoint posX="3175.51" posY="661.321"/>
+ <SupportPoint posX="3356.69" posY="674.597"/>
</SupportPoints>
</Transition>
<Transition from="PlaceObjectOnTable" to="Failure" eventName="Failure">
@@ -306,10 +270,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2243.37" posY="648.658"/>
- <SupportPoint posX="2465.7" posY="643.483"/>
- <SupportPoint posX="3363.21" posY="622.553"/>
- <SupportPoint posX="3609.95" posY="616.803"/>
+ <SupportPoint posX="2230.59" posY="620.496"/>
+ <SupportPoint posX="2270.2" posY="633.529"/>
+ <SupportPoint posX="2335.11" posY="653.104"/>
+ <SupportPoint posX="2392.61" posY="662.611"/>
+ <SupportPoint posX="2753.33" posY="722.168"/>
+ <SupportPoint posX="3195.06" posY="696.996"/>
+ <SupportPoint posX="3356.82" posY="684.602"/>
</SupportPoints>
</Transition>
<Transition from="PlaceObjectOnTable" to="LookAhead" eventName="Success">
@@ -321,13 +288,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2201.97" posY="660.439"/>
- <SupportPoint posX="2214.36" posY="675.018"/>
- <SupportPoint posX="2239.02" posY="700.893"/>
- <SupportPoint posX="2266.11" posY="713.722"/>
- <SupportPoint posX="2301.38" posY="730.461"/>
- <SupportPoint posX="2344.69" posY="737.847"/>
- <SupportPoint posX="2379.45" posY="741.016"/>
+ <SupportPoint posX="2243.75" posY="607.667"/>
+ <SupportPoint posX="2282.85" posY="607.667"/>
+ <SupportPoint posX="2337.28" posY="607.667"/>
+ <SupportPoint posX="2379.32" posY="607.667"/>
</SupportPoints>
</Transition>
<Transition from="RetreatPlatform" to="Failure" eventName="Failure">
@@ -335,13 +299,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2930.81" posY="804.023"/>
- <SupportPoint posX="3075.07" posY="801.978"/>
- <SupportPoint posX="3488.56" posY="789.916"/>
- <SupportPoint posX="3600.11" posY="718.833"/>
- <SupportPoint posX="3620.81" posY="705.596"/>
- <SupportPoint posX="3637.42" posY="684.129"/>
- <SupportPoint posX="3649.43" posY="664.17"/>
+ <SupportPoint posX="2930.81" posY="571.557"/>
+ <SupportPoint posX="3016.93" posY="582.686"/>
+ <SupportPoint posX="3198.38" posY="609.149"/>
+ <SupportPoint posX="3347.11" posY="651.111"/>
+ <SupportPoint posX="3350.56" posY="652.095"/>
+ <SupportPoint posX="3354.14" posY="653.181"/>
+ <SupportPoint posX="3357.72" posY="654.331"/>
</SupportPoints>
</Transition>
<Transition from="RetreatPlatform" to="NavigationPose_2" eventName="Success">
@@ -349,13 +313,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="2902.06" posY="827.662"/>
- <SupportPoint posX="2915.48" posY="840.733"/>
- <SupportPoint posX="2934.01" posY="856.092"/>
- <SupportPoint posX="2953.56" posY="864.5"/>
- <SupportPoint posX="2996.36" posY="882.849"/>
- <SupportPoint posX="3048.62" posY="888.65"/>
- <SupportPoint posX="3088.87" posY="890.004"/>
+ <SupportPoint posX="2930.56" posY="550.997"/>
+ <SupportPoint posX="2979.75" posY="536.699"/>
+ <SupportPoint posX="3055.14" posY="514.798"/>
+ <SupportPoint posX="3108.29" posY="499.349"/>
</SupportPoints>
</Transition>
<Transition from="GoHome" to="Failure" eventName="Failure">
@@ -363,13 +324,19 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="3473.99" posY="940.758"/>
- <SupportPoint posX="3522.93" posY="926.268"/>
- <SupportPoint posX="3591.17" posY="904.763"/>
- <SupportPoint posX="3600.11" posY="895.167"/>
- <SupportPoint posX="3630.91" posY="861.996"/>
- <SupportPoint posX="3654.42" posY="736.646"/>
- <SupportPoint posX="3665.66" posY="665.486"/>
+ <SupportPoint posX="3196.84" posY="186.498"/>
+ <SupportPoint posX="3205.15" posY="194.714"/>
+ <SupportPoint posX="3213.84" posY="204.349"/>
+ <SupportPoint posX="3220.61" posY="214.111"/>
+ <SupportPoint posX="3234.28" posY="233.712"/>
+ <SupportPoint posX="3225.72" posY="246.835"/>
+ <SupportPoint posX="3243.61" posY="262.667"/>
+ <SupportPoint posX="3279.9" posY="294.828"/>
+ <SupportPoint posX="3315.17" posY="260.673"/>
+ <SupportPoint posX="3347.11" posY="297.167"/>
+ <SupportPoint posX="3358.1" posY="309.663"/>
+ <SupportPoint posX="3395.16" posY="528.444"/>
+ <SupportPoint posX="3411.77" posY="629.184"/>
</SupportPoints>
</Transition>
<Transition from="GoHome" to="Success" eventName="Success">
@@ -377,13 +344,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="3473.99" posY="970.223"/>
- <SupportPoint posX="3481.53" posY="971.948"/>
- <SupportPoint posX="3489.2" posY="973.431"/>
- <SupportPoint posX="3496.61" posY="974.389"/>
- <SupportPoint posX="3534.05" posY="979.219"/>
- <SupportPoint posX="3576.22" posY="978.823"/>
- <SupportPoint posX="3609.95" posY="977.085"/>
+ <SupportPoint posX="3220.74" posY="169.593"/>
+ <SupportPoint posX="3258.43" posY="176.57"/>
+ <SupportPoint posX="3309.8" posY="190.549"/>
+ <SupportPoint posX="3347.11" posY="217.944"/>
+ <SupportPoint posX="3366.92" posY="232.511"/>
+ <SupportPoint posX="3383.27" posY="254.182"/>
+ <SupportPoint posX="3395.41" posY="274.052"/>
</SupportPoints>
</Transition>
<Transition from="PrepareNavigation" to="Failure" eventName="Failure">
@@ -391,22 +358,25 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="174.887" posY="906.36"/>
- <SupportPoint posX="205.196" posY="956.973"/>
- <SupportPoint posX="296.941" posY="1091.94"/>
- <SupportPoint posX="413.333" posY="1091.94"/>
- <SupportPoint posX="413.333" posY="1091.94"/>
- <SupportPoint posX="413.333" posY="1091.94"/>
- <SupportPoint posX="1694.94" posY="1091.94"/>
- <SupportPoint posX="2541.73" posY="1091.94"/>
- <SupportPoint posX="3000.96" posY="1664.87"/>
- <SupportPoint posX="3600.11" posY="1066.39"/>
- <SupportPoint posX="3624.13" posY="1042.43"/>
- <SupportPoint posX="3617.74" posY="949.076"/>
- <SupportPoint posX="3623.11" posY="915.611"/>
- <SupportPoint posX="3637.17" posY="827.547"/>
- <SupportPoint posX="3654.29" posY="725.12"/>
- <SupportPoint posX="3664.38" posY="665.524"/>
+ <SupportPoint posX="214.409" posY="1087.67"/>
+ <SupportPoint posX="264.485" posY="1098.88"/>
+ <SupportPoint posX="343.746" posY="1113.67"/>
+ <SupportPoint posX="413.333" posY="1113.67"/>
+ <SupportPoint posX="413.333" posY="1113.67"/>
+ <SupportPoint posX="413.333" posY="1113.67"/>
+ <SupportPoint posX="1570.36" posY="1113.67"/>
+ <SupportPoint posX="1791.93" posY="1113.67"/>
+ <SupportPoint posX="1847.13" posY="1131.56"/>
+ <SupportPoint posX="2068.69" posY="1131.56"/>
+ <SupportPoint posX="2068.69" posY="1131.56"/>
+ <SupportPoint posX="2068.69" posY="1131.56"/>
+ <SupportPoint posX="3170.78" posY="1131.56"/>
+ <SupportPoint posX="3255.75" posY="1131.56"/>
+ <SupportPoint posX="3293.57" posY="1123.46"/>
+ <SupportPoint posX="3347.11" posY="1057.44"/>
+ <SupportPoint posX="3387.36" posY="1007.8"/>
+ <SupportPoint posX="3408.19" posY="821.362"/>
+ <SupportPoint posX="3415.98" posY="730.129"/>
</SupportPoints>
</Transition>
<Transition from="PrepareNavigation" to="BringObjectToLandmark" eventName="Success">
@@ -416,10 +386,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="172.178" posY="868.934"/>
- <SupportPoint posX="208.608" posY="784.486"/>
- <SupportPoint posX="358.772" posY="436.419"/>
- <SupportPoint posX="401.782" posY="336.727"/>
+ <SupportPoint posX="214.588" posY="1056.97"/>
+ <SupportPoint posX="257.036" posY="1041.52"/>
+ <SupportPoint posX="317.551" posY="1019.48"/>
+ <SupportPoint posX="360.638" posY="1003.8"/>
</SupportPoints>
</Transition>
<Transition from="ResolveWorkbenchEntryWaypoints" to="Failure" eventName="Failure">
@@ -427,22 +397,19 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1495.74" posY="518.376"/>
- <SupportPoint posX="1549.53" posY="509.444"/>
- <SupportPoint posX="1626.71" posY="499.056"/>
- <SupportPoint posX="1694.94" posY="499.056"/>
- <SupportPoint posX="1694.94" posY="499.056"/>
- <SupportPoint posX="1694.94" posY="499.056"/>
- <SupportPoint posX="2068.69" posY="499.056"/>
- <SupportPoint posX="2485.25" posY="499.056"/>
- <SupportPoint posX="2588.75" posY="464.556"/>
- <SupportPoint posX="3005.31" posY="464.556"/>
- <SupportPoint posX="3005.31" posY="464.556"/>
- <SupportPoint posX="3005.31" posY="464.556"/>
- <SupportPoint posX="3423.78" posY="464.556"/>
- <SupportPoint posX="3505.43" posY="464.556"/>
- <SupportPoint posX="3583.24" posY="524.573"/>
- <SupportPoint posX="3629.5" posY="568.873"/>
+ <SupportPoint posX="1495.74" posY="776.18"/>
+ <SupportPoint posX="1549.53" posY="785.112"/>
+ <SupportPoint posX="1626.71" posY="795.5"/>
+ <SupportPoint posX="1694.94" posY="795.5"/>
+ <SupportPoint posX="1694.94" posY="795.5"/>
+ <SupportPoint posX="1694.94" posY="795.5"/>
+ <SupportPoint posX="2068.69" posY="795.5"/>
+ <SupportPoint posX="2638.46" posY="795.5"/>
+ <SupportPoint posX="2786.81" posY="803.972"/>
+ <SupportPoint posX="3347.11" posY="700.944"/>
+ <SupportPoint posX="3350.43" posY="700.331"/>
+ <SupportPoint posX="3353.88" posY="699.616"/>
+ <SupportPoint posX="3357.33" posY="698.836"/>
</SupportPoints>
</Transition>
<Transition from="ResolveWorkbenchEntryWaypoints" to="MoveToLocationTask_2" eventName="Success">
@@ -458,13 +425,10 @@
</ParameterMappingsToParentsLocal>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1495.61" posY="555.214"/>
- <SupportPoint posX="1503.41" posY="557.872"/>
- <SupportPoint posX="1511.2" posY="560.491"/>
- <SupportPoint posX="1518.61" posY="562.944"/>
- <SupportPoint posX="1556.18" posY="575.288"/>
- <SupportPoint posX="1598.6" posY="588.232"/>
- <SupportPoint posX="1632.46" posY="598.313"/>
+ <SupportPoint posX="1473.25" posY="712.841"/>
+ <SupportPoint posX="1525.89" posY="653.82"/>
+ <SupportPoint posX="1611.76" posY="557.654"/>
+ <SupportPoint posX="1659.81" posY="503.962"/>
</SupportPoints>
</Transition>
<Transition from="MoveToLocationTask_2" to="Failure" eventName="Failure">
@@ -472,19 +436,19 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1698.27" posY="646.051"/>
- <SupportPoint posX="1710.92" posY="740.632"/>
- <SupportPoint posX="1764.33" posY="1030.61"/>
- <SupportPoint posX="1944.11" posY="1030.61"/>
- <SupportPoint posX="1944.11" posY="1030.61"/>
- <SupportPoint posX="1944.11" posY="1030.61"/>
- <SupportPoint posX="3005.31" posY="1030.61"/>
- <SupportPoint posX="3270.32" posY="1030.61"/>
- <SupportPoint posX="3401.67" posY="1164.06"/>
- <SupportPoint posX="3600.11" posY="988.444"/>
- <SupportPoint posX="3612.25" posY="977.711"/>
- <SupportPoint posX="3648.54" posY="764.297"/>
- <SupportPoint posX="3664.89" posY="665.154"/>
+ <SupportPoint posX="1722.42" posY="434.822"/>
+ <SupportPoint posX="1765.35" posY="391.876"/>
+ <SupportPoint posX="1852.75" posY="317.611"/>
+ <SupportPoint posX="1944.11" posY="317.611"/>
+ <SupportPoint posX="1944.11" posY="317.611"/>
+ <SupportPoint posX="1944.11" posY="317.611"/>
+ <SupportPoint posX="3025.75" posY="317.611"/>
+ <SupportPoint posX="3098.97" posY="317.611"/>
+ <SupportPoint posX="3294.72" posY="339.295"/>
+ <SupportPoint posX="3347.11" posY="390.444"/>
+ <SupportPoint posX="3380.72" posY="423.258"/>
+ <SupportPoint posX="3403.33" posY="554.958"/>
+ <SupportPoint posX="3413.56" posY="628.648"/>
</SupportPoints>
</Transition>
<Transition from="MoveToLocationTask_2" to="CartesianGazeControl" eventName="Success">
@@ -496,10 +460,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1745.42" posY="616.611"/>
- <SupportPoint posX="1784.52" posY="616.611"/>
- <SupportPoint posX="1838.95" posY="616.611"/>
- <SupportPoint posX="1880.99" posY="616.611"/>
+ <SupportPoint posX="1745.42" posY="464.556"/>
+ <SupportPoint posX="1784.52" posY="464.556"/>
+ <SupportPoint posX="1838.95" posY="464.556"/>
+ <SupportPoint posX="1880.99" posY="464.556"/>
</SupportPoints>
</Transition>
<Transition from="LookStraight" to="Failure" eventName="Failure">
@@ -507,25 +471,22 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1210.92" posY="439.767"/>
- <SupportPoint posX="1264.51" posY="432.994"/>
- <SupportPoint posX="1351.99" posY="423.667"/>
- <SupportPoint posX="1427.89" posY="423.667"/>
- <SupportPoint posX="1427.89" posY="423.667"/>
- <SupportPoint posX="1427.89" posY="423.667"/>
- <SupportPoint posX="2068.69" posY="423.667"/>
- <SupportPoint posX="2332.68" posY="423.667"/>
- <SupportPoint posX="2397.59" posY="389.167"/>
- <SupportPoint posX="2661.58" posY="389.167"/>
- <SupportPoint posX="2661.58" posY="389.167"/>
- <SupportPoint posX="2661.58" posY="389.167"/>
- <SupportPoint posX="3423.78" posY="389.167"/>
- <SupportPoint posX="3512.46" posY="389.167"/>
- <SupportPoint posX="3536.99" posY="420.217"/>
- <SupportPoint posX="3600.11" posY="482.444"/>
- <SupportPoint posX="3623.88" posY="505.815"/>
- <SupportPoint posX="3641.89" posY="538.577"/>
- <SupportPoint posX="3654.16" posY="565.717"/>
+ <SupportPoint posX="1210.92" posY="854.789"/>
+ <SupportPoint posX="1264.51" posY="861.561"/>
+ <SupportPoint posX="1351.99" posY="870.889"/>
+ <SupportPoint posX="1427.89" posY="870.889"/>
+ <SupportPoint posX="1427.89" posY="870.889"/>
+ <SupportPoint posX="1427.89" posY="870.889"/>
+ <SupportPoint posX="1819.53" posY="870.889"/>
+ <SupportPoint posX="2389.29" posY="870.889"/>
+ <SupportPoint posX="2533.68" posY="852.885"/>
+ <SupportPoint posX="3097.94" posY="773.778"/>
+ <SupportPoint posX="3209.37" posY="758.163"/>
+ <SupportPoint posX="3242.97" posY="771.708"/>
+ <SupportPoint posX="3347.11" posY="729.056"/>
+ <SupportPoint posX="3351.46" posY="727.267"/>
+ <SupportPoint posX="3355.8" posY="725.184"/>
+ <SupportPoint posX="3360.14" posY="722.909"/>
</SupportPoints>
</Transition>
<Transition from="LookStraight" to="ResolveWorkbenchEntryWaypoints" eventName="Success">
@@ -535,13 +496,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1210.85" posY="477.218"/>
- <SupportPoint posX="1218.33" posY="481.064"/>
- <SupportPoint posX="1226.09" posY="484.655"/>
- <SupportPoint posX="1233.67" posY="487.556"/>
- <SupportPoint posX="1269.88" posY="501.419"/>
- <SupportPoint posX="1311.61" posY="511.476"/>
- <SupportPoint posX="1347.13" posY="518.401"/>
+ <SupportPoint posX="1211.06" posY="832.032"/>
+ <SupportPoint posX="1249.46" posY="819.906"/>
+ <SupportPoint posX="1303.15" posY="802.949"/>
+ <SupportPoint posX="1347.52" posY="788.945"/>
</SupportPoints>
</Transition>
</Transitions>
--
GitLab
From 78228669d456810cf000cd9d481e35553ef86799 Mon Sep 17 00:00:00 2001
From: Tilman Daab <tilman.daab@kit.edu>
Date: Tue, 25 Feb 2025 13:00:36 +0100
Subject: [PATCH 14/22] uncommitted changes, adapting demo statechart
parameters
---
.../TransportLadderV2.xml | 4 +-
.../BringObjectAndPutAwayTask.xml | 4 +-
.../PlaceObjectTask.xml | 236 ++++++++++--------
3 files changed, 130 insertions(+), 114 deletions(-)
diff --git a/source/armar6/skills/statecharts/Armar6BimanualManipulationGroup/TransportLadderV2.xml b/source/armar6/skills/statecharts/Armar6BimanualManipulationGroup/TransportLadderV2.xml
index 6d9df6d47..b048aabff 100644
--- a/source/armar6/skills/statecharts/Armar6BimanualManipulationGroup/TransportLadderV2.xml
+++ b/source/armar6/skills/statecharts/Armar6BimanualManipulationGroup/TransportLadderV2.xml
@@ -242,7 +242,7 @@
<RemoteState name="CloseBothHands_2" refuuid="4BFA225C-8FA0-4214-89F3-3D717C2585CF" proxyName="Armar6LowLevelHandControlGroupRemoteStateOfferer" left="1936.28" top="2164.15" boundingSquareSize="99.6636"/>
<RemoteState name="CloseRightHand" refuuid="802F09B5-5EB5-48AA-A909-BCEA99EE85CF" proxyName="Armar6LowLevelHandControlGroupRemoteStateOfferer" left="1379.17" top="2505.32" boundingSquareSize="99.6636"/>
<LocalState name="CreatCtrlAndLift" refuuid="A5F0807D-B3A6-46E8-8831-494AFCE297D2" left="2185.44" top="2168.15" boundingSquareSize="99.6636"/>
- <EndState name="Failure" event="Failure" left="7793.61" top="1589.04" boundingSquareSize="99.6636"/>
+ <EndState name="Failure" event="Failure" left="7793.44" top="1589.04" boundingSquareSize="99.6636"/>
<RemoteState name="GoToGoalPositionWithObstacleAvoidance" refuuid="EB5761E3-D8DF-4B79-82F2-DF7780B3E4CD" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="7479.86" top="1586.04" boundingSquareSize="164.836"/>
<RemoteState name="GoToGoalPositionWithObstacleAvoidance_2" refuuid="EB5761E3-D8DF-4B79-82F2-DF7780B3E4CD" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="7156.06" top="597.611" boundingSquareSize="173.779"/>
<RemoteState name="GoToGoalPositionWithObstacleAvoidance_3" refuuid="EB5761E3-D8DF-4B79-82F2-DF7780B3E4CD" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="6832.78" top="342.055" boundingSquareSize="173.779"/>
@@ -271,7 +271,7 @@
<RemoteState name="ResolveLandmark_2" refuuid="68599B94-C644-4023-969B-486210BEFD4A" proxyName="PlatformGroupRemoteStateOfferer" left="6869.78" top="735.5" boundingSquareSize="99.6636"/>
<RemoteState name="ResolveTargetLandmark" refuuid="68599B94-C644-4023-969B-486210BEFD4A" proxyName="PlatformGroupRemoteStateOfferer" left="2434.61" top="1597.99" boundingSquareSize="99.6636"/>
<LocalState name="StopController" refuuid="1F2BD891-2215-459C-B636-525BAB0569DF" left="6237.92" top="1787.1" boundingSquareSize="99.6636"/>
- <EndState name="Success" event="Success" left="7793.61" top="421.167" boundingSquareSize="99.6636"/>
+ <EndState name="Success" event="Success" left="7793.44" top="421.167" boundingSquareSize="99.6636"/>
<RemoteState name="Wait" refuuid="D7B2D054-3E9B-4B7D-B9DC-FE37E73E6C9D" proxyName="MotionControlGroupRemoteStateOfferer" left="4035.67" top="1724.49" boundingSquareSize="99.6636"/>
<RemoteState name="Wait_2" refuuid="D7B2D054-3E9B-4B7D-B9DC-FE37E73E6C9D" proxyName="MotionControlGroupRemoteStateOfferer" left="5719.78" top="1893.15" boundingSquareSize="99.6636"/>
</Substates>
diff --git a/source/armar6/skills/statecharts/Armar6HighlevelTaskGroup/BringObjectAndPutAwayTask.xml b/source/armar6/skills/statecharts/Armar6HighlevelTaskGroup/BringObjectAndPutAwayTask.xml
index b3cdb5fab..32e9b8f83 100644
--- a/source/armar6/skills/statecharts/Armar6HighlevelTaskGroup/BringObjectAndPutAwayTask.xml
+++ b/source/armar6/skills/statecharts/Armar6HighlevelTaskGroup/BringObjectAndPutAwayTask.xml
@@ -32,7 +32,7 @@
<Substates>
<RemoteState name="BringObjectToLandmark" refuuid="D38220A7-F179-4C96-BE6D-937C53EC2376" proxyName="Armar6GraspingGroupRemoteStateOfferer" left="363.333" top="969.611" boundingSquareSize="99.6636"/>
<RemoteState name="CartesianGazeControl" refuuid="0967749A-6240-4FA3-99E9-4EF063B4A5F9" proxyName="MotionControlGroupRemoteStateOfferer" left="1894.11" top="442.556" boundingSquareSize="99.6636"/>
- <EndState name="Failure" event="Failure" left="3369.94" top="642.222" boundingSquareSize="99.6636"/>
+ <EndState name="Failure" event="Failure" left="3369.78" top="642.222" boundingSquareSize="99.6636"/>
<RemoteState name="GoHome" refuuid="19478C7D-BC94-4BA1-8C85-33EC9EECCC74" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="3120.78" top="140" boundingSquareSize="99.6636"/>
<RemoteState name="HandOverToRobot" refuuid="3FAF4F4E-8A9A-4F23-8D15-3752F7B20612" proxyName="HandOverGroupRemoteStateOfferer" left="861.667" top="932.278" boundingSquareSize="99.6636"/>
<RemoteState name="LookAhead" refuuid="1A25B687-CFBC-4CA3-A1C7-32EEAF004C45" proxyName="MotionControlGroupRemoteStateOfferer" left="2392.44" top="570.667" boundingSquareSize="99.6636"/>
@@ -44,7 +44,7 @@
<RemoteState name="ReceiveFromRobot" refuuid="6546C898-D397-4835-A12B-0CE39B852D7D" proxyName="HandOverGroupRemoteStateOfferer" left="612.5" top="971.611" boundingSquareSize="99.6636"/>
<RemoteState name="ResolveWorkbenchEntryWaypoints" refuuid="68599B94-C644-4023-969B-486210BEFD4A" proxyName="PlatformGroupRemoteStateOfferer" left="1359.89" top="713.556" boundingSquareSize="135.442"/>
<RemoteState name="RetreatPlatform" refuuid="19478C7D-BC94-4BA1-8C85-33EC9EECCC74" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="2830.72" top="542.5" boundingSquareSize="99.6636"/>
- <EndState name="Success" event="Success" left="3369.94" top="285.722" boundingSquareSize="99.6636"/>
+ <EndState name="Success" event="Success" left="3369.78" top="285.722" boundingSquareSize="99.6636"/>
</Substates>
<Events>
<Event name="Failure">
diff --git a/source/armar6/skills/statecharts/Armar6HighlevelTaskGroup/PlaceObjectTask.xml b/source/armar6/skills/statecharts/Armar6HighlevelTaskGroup/PlaceObjectTask.xml
index cc65ff82b..304473b32 100644
--- a/source/armar6/skills/statecharts/Armar6HighlevelTaskGroup/PlaceObjectTask.xml
+++ b/source/armar6/skills/statecharts/Armar6HighlevelTaskGroup/PlaceObjectTask.xml
@@ -1,5 +1,5 @@
<?xml version="1.0" encoding="utf-8"?>
-<State version="1.2" name="PlaceObjectTask" uuid="AC3D3B4A-C95E-412E-9BF9-65A0A8EBA4D9" width="1944.72" height="605.083" type="Normal State">
+<State version="1.2" name="PlaceObjectTask" uuid="AC3D3B4A-C95E-412E-9BF9-65A0A8EBA4D9" width="1687.89" height="608.917" type="Normal State">
<InputParameters>
<Parameter name="FixedObjectName" type="::armarx::StringVariantData" docType="string" optional="yes">
<DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"spraybottle"}}' docValue="spraybottle"/>
@@ -9,6 +9,7 @@
</Parameter>
<Parameter name="RetreatLandmark" type="::armarx::StringVariantData" docType="string" optional="no">
<DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"wb_retreat"}}' docValue="wb_retreat"/>
+ <DefaultValue profile="Armar6Real" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"wb_exit"}}' docValue="wb_exit"/>
</Parameter>
<Parameter name="args" type="::armarx::SingleTypeVariantListBase(::memoryx::EntityRefBase)" docType="List(EntityRef)" optional="yes"/>
</InputParameters>
@@ -18,15 +19,15 @@
<Parameter name="ObjectName" type="::armarx::StringVariantData" docType="string" optional="no"/>
</LocalParameters>
<Substates>
- <RemoteState name="DetachObjectInMemory" refuuid="0A56EFF2-73D5-4887-A2AF-1DC488C70E4F" proxyName="MemoryXUtilityRemoteStateOfferer" left="282.872" top="315.111" boundingSquareSize="99.6636"/>
- <EndState name="Failure" event="Failure" left="1814.72" top="387.944" boundingSquareSize="99.6636"/>
- <RemoteState name="GoHome" refuuid="6A0FD8EC-064D-4A4A-B036-3C2995564588" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="1565.72" top="238.778" boundingSquareSize="99.6636"/>
- <RemoteState name="LookAhead" refuuid="1A25B687-CFBC-4CA3-A1C7-32EEAF004C45" proxyName="MotionControlGroupRemoteStateOfferer" left="532.038" top="394.333" boundingSquareSize="99.6636"/>
- <RemoteState name="NavigationPose" refuuid="E7374741-A6DF-4E3F-B05D-68A1EB44D6A5" proxyName="Armar6PosesRemoteStateOfferer" left="1294.19" top="308.611" boundingSquareSize="99.6636"/>
- <RemoteState name="PlaceObject" refuuid="B9EE5C67-0F89-466A-832F-F4025B493344" proxyName="Armar6GraspingGroupRemoteStateOfferer" left="31.7666" top="303.611" boundingSquareSize="99.6636"/>
- <RemoteState name="PlaceObjectOnTable" refuuid="37E2E508-1C2C-4DE1-B783-9C92862A952C" proxyName="Armar6GraspingGroupRemoteStateOfferer" left="282.872" top="500.083" boundingSquareSize="99.6636"/>
- <RemoteState name="RetreatPlatform" refuuid="19478C7D-BC94-4BA1-8C85-33EC9EECCC74" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="970.511" top="380.389" boundingSquareSize="152.058"/>
- <EndState name="Success" event="Success" left="1814.72" top="163.056" boundingSquareSize="99.6636"/>
+ <RemoteState name="DetachObjectInMemory" refuuid="0A56EFF2-73D5-4887-A2AF-1DC488C70E4F" proxyName="MemoryXUtilityRemoteStateOfferer" left="279" top="318.944" boundingSquareSize="99.6636"/>
+ <EndState name="Failure" event="Failure" left="1558.06" top="453.111" boundingSquareSize="99.6636"/>
+ <RemoteState name="GoHome" refuuid="6A0FD8EC-064D-4A4A-B036-3C2995564588" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="1308.89" top="140.389" boundingSquareSize="99.6636"/>
+ <RemoteState name="LookAhead" refuuid="1A25B687-CFBC-4CA3-A1C7-32EEAF004C45" proxyName="MotionControlGroupRemoteStateOfferer" left="528.167" top="398.167" boundingSquareSize="99.6636"/>
+ <RemoteState name="NavigationPose" refuuid="E7374741-A6DF-4E3F-B05D-68A1EB44D6A5" proxyName="Armar6PosesRemoteStateOfferer" left="1308.89" top="311.167" boundingSquareSize="99.6636"/>
+ <RemoteState name="PlaceObject" refuuid="B9EE5C67-0F89-466A-832F-F4025B493344" proxyName="Armar6GraspingGroupRemoteStateOfferer" left="30" top="258.889" boundingSquareSize="99.6636"/>
+ <RemoteState name="PlaceObjectOnTable" refuuid="37E2E508-1C2C-4DE1-B783-9C92862A952C" proxyName="Armar6GraspingGroupRemoteStateOfferer" left="279" top="547.389" boundingSquareSize="99.6636"/>
+ <RemoteState name="RetreatPlatform" refuuid="19478C7D-BC94-4BA1-8C85-33EC9EECCC74" proxyName="NavigateToLocationGroupRemoteStateOfferer" left="966.639" top="381" boundingSquareSize="152.058"/>
+ <EndState name="Success" event="Success" left="1558.06" top="241" boundingSquareSize="99.6636"/>
</Substates>
<Events>
<Event name="Failure">
@@ -39,10 +40,10 @@
<ParameterMapping sourceType="Parent" from="ObjectName" to="ObjectName"/>
</ParameterMappings>
<SupportPoints>
- <SupportPoint posX="100.639" posY="556.556"/>
- <SupportPoint posX="136.118" posY="556.556"/>
- <SupportPoint posX="214.204" posY="556.556"/>
- <SupportPoint posX="270.031" posY="556.556"/>
+ <SupportPoint posX="98.5617" posY="560.389"/>
+ <SupportPoint posX="133.661" posY="560.389"/>
+ <SupportPoint posX="210.793" posY="560.389"/>
+ <SupportPoint posX="266.159" posY="560.389"/>
</SupportPoints>
</StartState>
<Transitions>
@@ -51,25 +52,22 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="118.923" posY="303.351"/>
- <SupportPoint posX="164.946" posY="261.325"/>
- <SupportPoint posX="247.746" posY="198.778"/>
- <SupportPoint posX="332.872" posY="198.778"/>
- <SupportPoint posX="332.872" posY="198.778"/>
- <SupportPoint posX="332.872" posY="198.778"/>
- <SupportPoint posX="801.177" posY="198.778"/>
- <SupportPoint posX="1185.47" posY="198.778"/>
- <SupportPoint posX="1303.6" posY="90.2178"/>
- <SupportPoint posX="1665.56" posY="219.222"/>
- <SupportPoint posX="1677.82" posY="223.567"/>
- <SupportPoint posX="1677.57" posY="230.339"/>
- <SupportPoint posX="1688.56" posY="237.111"/>
- <SupportPoint posX="1731.87" posY="263.663"/>
- <SupportPoint posX="1755.38" posY="250.528"/>
- <SupportPoint posX="1792.06" posY="285.667"/>
- <SupportPoint posX="1817.99" posY="310.443"/>
- <SupportPoint posX="1836.39" posY="346.425"/>
- <SupportPoint posX="1848.28" posY="375.609"/>
+ <SupportPoint posX="130.022" posY="282.728"/>
+ <SupportPoint posX="180.062" posY="270.768"/>
+ <SupportPoint posX="259.297" posY="255"/>
+ <SupportPoint posX="329" posY="255"/>
+ <SupportPoint posX="329" posY="255"/>
+ <SupportPoint posX="329" posY="255"/>
+ <SupportPoint posX="1213.86" posY="255"/>
+ <SupportPoint posX="1302.02" posY="255"/>
+ <SupportPoint posX="1324.01" posY="267.561"/>
+ <SupportPoint posX="1408.72" posY="292.056"/>
+ <SupportPoint posX="1466.73" posY="308.846"/>
+ <SupportPoint posX="1490.76" posY="300.936"/>
+ <SupportPoint posX="1535.22" posY="341.889"/>
+ <SupportPoint posX="1563.97" posY="368.39"/>
+ <SupportPoint posX="1582.76" posY="408.729"/>
+ <SupportPoint posX="1594" posY="440.636"/>
</SupportPoints>
</Transition>
<Transition from="PlaceObject" to="DetachObjectInMemory" eventName="Success">
@@ -79,10 +77,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="131.977" posY="342.911"/>
- <SupportPoint posX="171.667" posY="344.726"/>
- <SupportPoint posX="227.289" posY="347.281"/>
- <SupportPoint posX="269.954" posY="349.236"/>
+ <SupportPoint posX="129.794" posY="312.385"/>
+ <SupportPoint posX="137.42" posY="314.685"/>
+ <SupportPoint posX="145.227" posY="316.934"/>
+ <SupportPoint posX="152.667" posY="318.889"/>
+ <SupportPoint posX="189.927" posY="328.664"/>
+ <SupportPoint posX="232.144" posY="337.608"/>
+ <SupportPoint posX="265.929" posY="344.266"/>
</SupportPoints>
</Transition>
<Transition from="DetachObjectInMemory" to="LookAhead" eventName="Failure">
@@ -94,13 +95,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="382.82" posY="360.787"/>
- <SupportPoint posX="418.636" posY="367.789"/>
- <SupportPoint posX="467.703" posY="378.996"/>
- <SupportPoint posX="509.205" posY="394.278"/>
- <SupportPoint posX="512.872" posY="395.632"/>
- <SupportPoint posX="516.603" posY="397.127"/>
- <SupportPoint posX="520.334" posY="398.712"/>
+ <SupportPoint posX="378.948" posY="364.621"/>
+ <SupportPoint posX="414.764" posY="371.623"/>
+ <SupportPoint posX="463.831" posY="382.829"/>
+ <SupportPoint posX="505.333" posY="398.111"/>
+ <SupportPoint posX="509.013" posY="399.466"/>
+ <SupportPoint posX="512.744" posY="400.961"/>
+ <SupportPoint posX="516.476" posY="402.545"/>
</SupportPoints>
</Transition>
<Transition from="DetachObjectInMemory" to="LookAhead" eventName="Success">
@@ -112,13 +113,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="353.342" posY="389.358"/>
- <SupportPoint posX="365.8" posY="407.976"/>
- <SupportPoint posX="383.587" posY="428.676"/>
- <SupportPoint posX="405.705" posY="439"/>
- <SupportPoint posX="440.793" posY="455.372"/>
- <SupportPoint posX="484.199" posY="453.715"/>
- <SupportPoint posX="519.159" posY="447.837"/>
+ <SupportPoint posX="349.483" posY="393.192"/>
+ <SupportPoint posX="361.941" posY="411.809"/>
+ <SupportPoint posX="379.715" posY="432.509"/>
+ <SupportPoint posX="401.833" posY="442.833"/>
+ <SupportPoint posX="436.921" posY="459.206"/>
+ <SupportPoint posX="480.327" posY="457.548"/>
+ <SupportPoint posX="515.3" posY="451.67"/>
</SupportPoints>
</Transition>
<Transition from="LookAhead" to="RetreatPlatform" eventName="EvJointTargetReached">
@@ -128,10 +129,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="632.178" posY="430.362"/>
- <SupportPoint posX="709.816" posY="428.867"/>
- <SupportPoint posX="860.185" posY="425.979"/>
- <SupportPoint posX="956.836" posY="424.114"/>
+ <SupportPoint posX="628.319" posY="433.097"/>
+ <SupportPoint posX="705.944" posY="429.889"/>
+ <SupportPoint posX="856.313" posY="423.692"/>
+ <SupportPoint posX="952.964" posY="419.706"/>
</SupportPoints>
</Transition>
<Transition from="LookAhead" to="Failure" eventName="EvTimeout">
@@ -139,13 +140,16 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="632.114" posY="440.061"/>
- <SupportPoint posX="705.893" posY="452.424"/>
- <SupportPoint posX="848.302" posY="474.442"/>
- <SupportPoint posX="970.483" posY="482.444"/>
- <SupportPoint posX="1282.54" posY="502.886"/>
- <SupportPoint posX="1655.84" posY="455.794"/>
- <SupportPoint posX="1802.02" posY="434.604"/>
+ <SupportPoint posX="628.166" posY="441.99"/>
+ <SupportPoint posX="726.351" posY="455.022"/>
+ <SupportPoint posX="951.38" posY="482.984"/>
+ <SupportPoint posX="1141.67" posY="492.667"/>
+ <SupportPoint posX="1205.76" posY="495.928"/>
+ <SupportPoint posX="1221.89" posY="492.921"/>
+ <SupportPoint posX="1286.06" posY="492.667"/>
+ <SupportPoint posX="1376.01" posY="492.31"/>
+ <SupportPoint posX="1480.15" posY="491.391"/>
+ <SupportPoint posX="1545.06" posY="490.759"/>
</SupportPoints>
</Transition>
<Transition from="NavigationPose" to="Failure" eventName="Failure">
@@ -153,23 +157,29 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1394.16" posY="348.699"/>
- <SupportPoint posX="1489.48" posY="364.135"/>
- <SupportPoint posX="1697.24" posY="397.779"/>
- <SupportPoint posX="1801.89" posY="414.735"/>
+ <SupportPoint posX="1400.16" posY="375.188"/>
+ <SupportPoint posX="1410.26" posY="382.599"/>
+ <SupportPoint posX="1421.24" posY="390.24"/>
+ <SupportPoint posX="1431.72" posY="396.833"/>
+ <SupportPoint posX="1468.52" posY="419.987"/>
+ <SupportPoint posX="1511.71" posY="442.884"/>
+ <SupportPoint posX="1546.21" posY="460.202"/>
</SupportPoints>
</Transition>
- <Transition from="NavigationPose" to="GoHome" eventName="Success">
+ <Transition from="NavigationPose" to="Success" eventName="Success">
<ParameterMappings>
<ParameterMapping sourceType="Parent" from="HomeLandmark" to="LandmarkName"/>
</ParameterMappings>
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1394.67" posY="327.079"/>
- <SupportPoint posX="1439.39" posY="315.068"/>
- <SupportPoint posX="1505.07" posY="297.486"/>
- <SupportPoint posX="1553.11" posY="284.581"/>
+ <SupportPoint posX="1408.85" posY="349.581"/>
+ <SupportPoint posX="1445.01" posY="352.303"/>
+ <SupportPoint posX="1494.59" posY="351.945"/>
+ <SupportPoint posX="1535.22" posY="336.778"/>
+ <SupportPoint posX="1544.17" posY="333.456"/>
+ <SupportPoint posX="1552.73" posY="328.587"/>
+ <SupportPoint posX="1560.78" posY="323.054"/>
</SupportPoints>
</Transition>
<Transition from="PlaceObjectOnTable" to="Failure" eventName="Failure">
@@ -177,16 +187,16 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="383.114" posY="557.105"/>
- <SupportPoint posX="521.472" posY="558.213"/>
- <SupportPoint posX="919.538" posY="558.664"/>
- <SupportPoint posX="1249.04" posY="532.278"/>
- <SupportPoint posX="1491.78" posY="512.839"/>
- <SupportPoint posX="1558.61" posY="528.577"/>
- <SupportPoint posX="1792.06" posY="459.444"/>
- <SupportPoint posX="1795.63" posY="458.402"/>
- <SupportPoint posX="1799.21" posY="457.21"/>
- <SupportPoint posX="1802.79" posY="455.911"/>
+ <SupportPoint posX="378.936" posY="559.915"/>
+ <SupportPoint posX="503.736" posY="558.514"/>
+ <SupportPoint posX="839.332" posY="553.44"/>
+ <SupportPoint posX="1118.67" posY="537.389"/>
+ <SupportPoint posX="1304.17" posY="526.73"/>
+ <SupportPoint posX="1350.71" posY="524.066"/>
+ <SupportPoint posX="1535.22" posY="501.611"/>
+ <SupportPoint posX="1538.42" posY="501.219"/>
+ <SupportPoint posX="1541.74" posY="500.791"/>
+ <SupportPoint posX="1545.06" posY="500.338"/>
</SupportPoints>
</Transition>
<Transition from="PlaceObjectOnTable" to="LookAhead" eventName="Success">
@@ -198,13 +208,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="381.197" posY="546.248"/>
- <SupportPoint posX="417.984" posY="537.062"/>
- <SupportPoint posX="468.981" posY="521.377"/>
- <SupportPoint posX="509.205" posY="497.778"/>
- <SupportPoint posX="518.967" posY="492.05"/>
- <SupportPoint posX="528.538" posY="484.848"/>
- <SupportPoint posX="537.38" posY="477.315"/>
+ <SupportPoint posX="378.859" posY="549.696"/>
+ <SupportPoint posX="415.531" posY="540.459"/>
+ <SupportPoint posX="465.671" posY="524.883"/>
+ <SupportPoint posX="505.333" posY="501.611"/>
+ <SupportPoint posX="515.096" posY="495.883"/>
+ <SupportPoint posX="524.679" posY="488.681"/>
+ <SupportPoint posX="533.508" posY="481.149"/>
</SupportPoints>
</Transition>
<Transition from="GoHome" to="Failure" eventName="Failure">
@@ -212,16 +222,19 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1648.69" posY="297.243"/>
- <SupportPoint posX="1660.7" posY="307.006"/>
- <SupportPoint posX="1674.76" posY="317.394"/>
- <SupportPoint posX="1688.56" posY="325.278"/>
- <SupportPoint posX="1731.36" posY="349.619"/>
- <SupportPoint posX="1749.51" posY="340.1"/>
- <SupportPoint posX="1792.06" posY="364.889"/>
- <SupportPoint posX="1799.59" posY="369.272"/>
- <SupportPoint posX="1807.13" posY="374.408"/>
- <SupportPoint posX="1814.29" posY="379.813"/>
+ <SupportPoint posX="1386.11" posY="199.148"/>
+ <SupportPoint posX="1393.64" posY="207.876"/>
+ <SupportPoint posX="1401.69" posY="217.587"/>
+ <SupportPoint posX="1408.72" posY="226.889"/>
+ <SupportPoint posX="1420.09" posY="242.018"/>
+ <SupportPoint posX="1416.77" posY="251.064"/>
+ <SupportPoint posX="1431.72" posY="262.667"/>
+ <SupportPoint posX="1470.06" posY="292.413"/>
+ <SupportPoint posX="1500.21" posY="263.561"/>
+ <SupportPoint posX="1535.22" posY="297.167"/>
+ <SupportPoint posX="1556.31" posY="317.432"/>
+ <SupportPoint posX="1580.07" posY="390.342"/>
+ <SupportPoint posX="1594.64" posY="440.495"/>
</SupportPoints>
</Transition>
<Transition from="GoHome" to="Success" eventName="Success">
@@ -229,10 +242,13 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1666.19" posY="254.067"/>
- <SupportPoint posX="1705.42" posY="243.385"/>
- <SupportPoint posX="1760.24" posY="228.499"/>
- <SupportPoint posX="1802.41" posY="217.05"/>
+ <SupportPoint posX="1408.85" posY="177.337"/>
+ <SupportPoint posX="1445.39" posY="184.492"/>
+ <SupportPoint posX="1495.23" posY="197.155"/>
+ <SupportPoint posX="1535.22" posY="217.944"/>
+ <SupportPoint posX="1543.14" posY="222.059"/>
+ <SupportPoint posX="1550.94" posY="227.132"/>
+ <SupportPoint posX="1558.48" posY="232.575"/>
</SupportPoints>
</Transition>
<Transition from="RetreatPlatform" to="Failure" eventName="Failure">
@@ -240,10 +256,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1122.77" posY="422.632"/>
- <SupportPoint posX="1283.38" posY="423.13"/>
- <SupportPoint posX="1653.93" posY="424.28"/>
- <SupportPoint posX="1802.02" posY="424.753"/>
+ <SupportPoint posX="1118.9" posY="425.992"/>
+ <SupportPoint posX="1231.63" posY="440.776"/>
+ <SupportPoint posX="1440.67" posY="468.173"/>
+ <SupportPoint posX="1545.19" posY="481.869"/>
</SupportPoints>
</Transition>
<Transition from="RetreatPlatform" to="NavigationPose" eventName="Success">
@@ -251,10 +267,10 @@
<ParameterMappingsToParentsLocal/>
<ParameterMappingsToParentsOutput/>
<SupportPoints>
- <SupportPoint posX="1122.79" posY="401.433"/>
- <SupportPoint posX="1172.08" posY="387.902"/>
- <SupportPoint posX="1235.43" posY="370.498"/>
- <SupportPoint posX="1281.74" posY="357.772"/>
+ <SupportPoint posX="1118.79" posY="398.456"/>
+ <SupportPoint posX="1173.22" posY="385.934"/>
+ <SupportPoint posX="1245.54" posY="369.272"/>
+ <SupportPoint posX="1296.55" posY="357.529"/>
</SupportPoints>
</Transition>
</Transitions>
--
GitLab
From 98be52aedb2b932c2ab0fc5be59f6b7f0b486346 Mon Sep 17 00:00:00 2001
From: Tilman Daab <tilman.daab@kit.edu>
Date: Tue, 25 Feb 2025 13:02:24 +0100
Subject: [PATCH 15/22] uncomitted changes, adapting core max history sizes of
memories
---
scenarios/Armar6HighlevelApps/config/RobotStateMemory.cfg | 8 ++++----
scenarios/Armar6HighlevelApps/config/SkillsMemory.cfg | 8 ++++----
2 files changed, 8 insertions(+), 8 deletions(-)
diff --git a/scenarios/Armar6HighlevelApps/config/RobotStateMemory.cfg b/scenarios/Armar6HighlevelApps/config/RobotStateMemory.cfg
index 762e9faf5..38706f6b5 100644
--- a/scenarios/Armar6HighlevelApps/config/RobotStateMemory.cfg
+++ b/scenarios/Armar6HighlevelApps/config/RobotStateMemory.cfg
@@ -211,10 +211,10 @@ ArmarX.RobotStateMemory.WaitForRobotUnit = true
# ArmarX.RobotStateMemory.mem.desc.seg.CoreMaxHistorySize: Maximal size of the Description entity histories (-1 for infinite).
# Attributes:
-# - Default: -1
+# - Default: 10
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RobotStateMemory.mem.desc.seg.CoreMaxHistorySize = -1
+# ArmarX.RobotStateMemory.mem.desc.seg.CoreMaxHistorySize = 10
# ArmarX.RobotStateMemory.mem.desc.seg.CoreSegmentName: Name of the Description core segment.
@@ -227,10 +227,10 @@ ArmarX.RobotStateMemory.WaitForRobotUnit = true
# ArmarX.RobotStateMemory.mem.ext.seg.CoreMaxHistorySize: Maximal size of the Exteroception entity histories (-1 for infinite).
# Attributes:
-# - Default: -1
+# - Default: 10
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RobotStateMemory.mem.ext.seg.CoreMaxHistorySize = -1
+# ArmarX.RobotStateMemory.mem.ext.seg.CoreMaxHistorySize = 10
# ArmarX.RobotStateMemory.mem.ext.seg.CoreSegmentName: Name of the Exteroception core segment.
diff --git a/scenarios/Armar6HighlevelApps/config/SkillsMemory.cfg b/scenarios/Armar6HighlevelApps/config/SkillsMemory.cfg
index 092c0f206..654f91f66 100644
--- a/scenarios/Armar6HighlevelApps/config/SkillsMemory.cfg
+++ b/scenarios/Armar6HighlevelApps/config/SkillsMemory.cfg
@@ -178,10 +178,10 @@
# ArmarX.SkillMemory.mem.event.seg.CoreMaxHistorySize: Maximal size of the SkillEvent entity histories (-1 for infinite).
# Attributes:
-# - Default: 10
+# - Default: 300
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SkillMemory.mem.event.seg.CoreMaxHistorySize = 10
+# ArmarX.SkillMemory.mem.event.seg.CoreMaxHistorySize = 300
# ArmarX.SkillMemory.mem.event.seg.CoreSegmentName: Name of the SkillEvent core segment.
@@ -210,10 +210,10 @@
# ArmarX.SkillMemory.mem.executionrequest.seg.CoreMaxHistorySize: Maximal size of the SkillExecutionRequest entity histories (-1 for infinite).
# Attributes:
-# - Default: 10
+# - Default: 50
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SkillMemory.mem.executionrequest.seg.CoreMaxHistorySize = 10
+# ArmarX.SkillMemory.mem.executionrequest.seg.CoreMaxHistorySize = 50
# ArmarX.SkillMemory.mem.executionrequest.seg.CoreSegmentName: Name of the SkillExecutionRequest core segment.
--
GitLab
From 6598e8b70de7e35d8df454c25e5faf2c7b2e9244 Mon Sep 17 00:00:00 2001
From: Tilman Daab <tilman.daab@kit.edu>
Date: Tue, 25 Feb 2025 13:06:24 +0100
Subject: [PATCH 16/22] uncommitted changes, only commented-out modifications
in scenarios
---
.../config/ObjectMemory.cfg | 215 +++++++++++++++++-
.../config/RobotStateMemory.cfg | 34 +++
.../config/SkillsMemory.cfg | 40 +++-
.../config/control_memory.cfg | 39 +++-
.../config/navigation_memory.cfg | 40 +++-
.../config/speech_memory.cfg | 36 +++
.../config/symbolic_scene_memory.cfg | 31 ++-
.../config/view_memory.cfg | 40 +++-
.../config/action_executor.cfg | 97 ++------
.../config/manipulation_memory.cfg | 40 +++-
10 files changed, 517 insertions(+), 95 deletions(-)
diff --git a/scenarios/Armar6HighlevelApps/config/ObjectMemory.cfg b/scenarios/Armar6HighlevelApps/config/ObjectMemory.cfg
index e4a3f44d6..786f04a2f 100644
--- a/scenarios/Armar6HighlevelApps/config/ObjectMemory.cfg
+++ b/scenarios/Armar6HighlevelApps/config/ObjectMemory.cfg
@@ -266,6 +266,183 @@ ArmarX.ObjectMemory.mem.cls.Floor.Height = -2
# ArmarX.ObjectMemory.mem.cls.seg.CoreSegmentName = Class
+# ArmarX.ObjectMemory.mem.fam_obj_inst.decay.delaySeconds: Duration after latest localization before decay starts.
+# Attributes:
+# - Default: 5
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.fam_obj_inst.decay.delaySeconds = 5
+
+
+# ArmarX.ObjectMemory.mem.fam_obj_inst.decay.durationSeconds: How long to reach minimal confidence.
+# Attributes:
+# - Default: 20
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.fam_obj_inst.decay.durationSeconds = 20
+
+
+# ArmarX.ObjectMemory.mem.fam_obj_inst.decay.enabled: If true, object poses decay over time when not localized anymore.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.fam_obj_inst.decay.enabled = false
+
+
+# ArmarX.ObjectMemory.mem.fam_obj_inst.decay.maxConfidence: Confidence when decay starts.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.fam_obj_inst.decay.maxConfidence = 1
+
+
+# ArmarX.ObjectMemory.mem.fam_obj_inst.decay.minConfidence: Confidence after decay duration.
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.fam_obj_inst.decay.minConfidence = 0
+
+
+# ArmarX.ObjectMemory.mem.fam_obj_inst.decay.removeObjectsBelowConfidence: Remove objects whose confidence is lower than this value.
+# Attributes:
+# - Default: 0.100000001
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.fam_obj_inst.decay.removeObjectsBelowConfidence = 0.100000001
+
+
+# ArmarX.ObjectMemory.mem.fam_obj_inst.robots.FallbackName: Robot name to use as fallback if the robot name is not specified in a provided object pose.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.fam_obj_inst.robots.FallbackName = ""
+
+
+# ArmarX.ObjectMemory.mem.fam_obj_inst.seg.CoreMaxHistorySize: Maximal size of the FamiliarObjectInstance entity histories (-1 for infinite).
+# Attributes:
+# - Default: 64
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.fam_obj_inst.seg.CoreMaxHistorySize = 64
+
+
+# ArmarX.ObjectMemory.mem.fam_obj_inst.seg.CoreSegmentName: Name of the FamiliarObjectInstance core segment.
+# Attributes:
+# - Default: FamiliarObjectInstance
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.fam_obj_inst.seg.CoreSegmentName = FamiliarObjectInstance
+
+
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.alpha: Alpha of objects (1 = solid, 0 = transparent).
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.alpha = 1
+
+
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.alphaByConfidence: If true, use the pose confidence as alpha (if < 1.0).
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.alphaByConfidence = false
+
+
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.enabled: Enable or disable visualization of objects.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.enabled = true
+
+
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.frequenzyHz: Frequency of visualization.
+# Attributes:
+# - Default: 25
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.frequenzyHz = 25
+
+
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.inGlobalFrame: If true, show global poses. If false, show poses in robot frame.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.inGlobalFrame = true
+
+
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.objectFrames: Enable showing object frames.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.objectFrames = false
+
+
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.objectFramesScale: Scaling of object frames.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.objectFramesScale = 1
+
+
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.oobbs: Enable showing oriented bounding boxes.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.oobbs = false
+
+
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.sizePixel: Pixel size of point cloud.
+# Attributes:
+# - Default: 5
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.sizePixel = 5
+
+
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.visualizeBoundingBox:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.visualizeBoundingBox = true
+
+
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.visualizePointCloud:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.visualizePointCloud = true
+
+
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.visualizePose:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.fam_obj_inst.visu.visualizePose = true
+
+
# ArmarX.ObjectMemory.mem.inst.DiscardSnapshotsWhileAttached: If true, no new snapshots are stored while an object is attached to a robot node.
# If false, new snapshots are stored, but the attachment is kept in the new snapshots.
# Attributes:
@@ -595,10 +772,10 @@ ArmarX.ObjectMemory.mem.inst.visu.alphaByConfidence = true
# ArmarX.ObjectMemory.mem.ltm.configuration:
# Attributes:
-# - Default: { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}, "PngConverter": {}, "ExrConverter": {}}
+# - Default: { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.mem.ltm.configuration = { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}, "PngConverter": {}, "ExrConverter": {}}
+# ArmarX.ObjectMemory.mem.ltm.configuration = { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}}
# ArmarX.ObjectMemory.mem.ltm.enabled:
@@ -625,6 +802,32 @@ ArmarX.ObjectMemory.mem.ltm.exportName = RecordingsJoana
# - Required: no
ArmarX.ObjectMemory.mem.ltm.exportPath = "/media/ltm"
+# ArmarX.ObjectMemory.mem.ltm.importOnStartUp:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.ltm.importOnStartUp = false
+
+
+# ArmarX.ObjectMemory.mem.ltm.loadedCoreSegments:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.ltm.loadedCoreSegments = ""
+
+
+# ArmarX.ObjectMemory.mem.ltm.maxAmountSnapshotsLoaded:
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.ltm.maxAmountSnapshotsLoaded = 1
+
+
+
# ArmarX.ObjectMemory.mem.ltm.storeFrequency:
# Attributes:
@@ -634,6 +837,14 @@ ArmarX.ObjectMemory.mem.ltm.exportPath = "/media/ltm"
# ArmarX.ObjectMemory.mem.ltm.storeFrequency = 10
+# ArmarX.ObjectMemory.mem.ltm.storeOnStop:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.ltm.storeOnStop = false
+
# ArmarX.ObjectMemory.mem.robot_state.Memory:
# Attributes:
# - Default: RobotState
diff --git a/scenarios/Armar6HighlevelApps/config/RobotStateMemory.cfg b/scenarios/Armar6HighlevelApps/config/RobotStateMemory.cfg
index 38706f6b5..44e0d9a91 100644
--- a/scenarios/Armar6HighlevelApps/config/RobotStateMemory.cfg
+++ b/scenarios/Armar6HighlevelApps/config/RobotStateMemory.cfg
@@ -289,6 +289,31 @@ ArmarX.RobotStateMemory.mem.ltm.exportName = "RecordingsJoana"
# - Required: no
ArmarX.RobotStateMemory.mem.ltm.exportPath = "/media/ltm"
+# ArmarX.RobotStateMemory.mem.ltm.importOnStartUp:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotStateMemory.mem.ltm.importOnStartUp = false
+
+
+# ArmarX.RobotStateMemory.mem.ltm.loadedCoreSegments:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.ltm.loadedCoreSegments = ""
+
+
+# ArmarX.RobotStateMemory.mem.ltm.maxAmountSnapshotsLoaded:
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.ltm.maxAmountSnapshotsLoaded = 1
+
+
# ArmarX.RobotStateMemory.mem.ltm.storeFrequency:
# Attributes:
@@ -298,6 +323,15 @@ ArmarX.RobotStateMemory.mem.ltm.exportPath = "/media/ltm"
# ArmarX.RobotStateMemory.mem.ltm.storeFrequency = 10
+# ArmarX.RobotStateMemory.mem.ltm.storeOnStop:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotStateMemory.mem.ltm.storeOnStop = false
+
+
# ArmarX.RobotStateMemory.mem.prop.seg.CoreMaxHistorySize: Maximal size of the Proprioception entity histories (-1 for infinite).
# Attributes:
# - Default: 1024
diff --git a/scenarios/Armar6HighlevelApps/config/SkillsMemory.cfg b/scenarios/Armar6HighlevelApps/config/SkillsMemory.cfg
index 654f91f66..3b74e01e6 100644
--- a/scenarios/Armar6HighlevelApps/config/SkillsMemory.cfg
+++ b/scenarios/Armar6HighlevelApps/config/SkillsMemory.cfg
@@ -226,10 +226,10 @@
# ArmarX.SkillMemory.mem.ltm.configuration:
# Attributes:
-# - Default: { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}, "PngConverter": {}, "ExrConverter": {}}
+# - Default: { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SkillMemory.mem.ltm.configuration = { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}, "PngConverter": {}, "ExrConverter": {}}
+# ArmarX.SkillMemory.mem.ltm.configuration = { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}}
# ArmarX.SkillMemory.mem.ltm.enabled:
@@ -257,6 +257,33 @@
# ArmarX.SkillMemory.mem.ltm.exportPath = ""
+# ArmarX.SkillMemory.mem.ltm.importOnStartUp:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.SkillMemory.mem.ltm.importOnStartUp = false
+
+
+# ArmarX.SkillMemory.mem.ltm.loadedCoreSegments:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SkillMemory.mem.ltm.loadedCoreSegments = ""
+
+
+# ArmarX.SkillMemory.mem.ltm.maxAmountSnapshotsLoaded:
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SkillMemory.mem.ltm.maxAmountSnapshotsLoaded = 1
+
+
+
+
# ArmarX.SkillMemory.mem.ltm.storeFrequency:
# Attributes:
# - Default: 10
@@ -265,6 +292,15 @@
# ArmarX.SkillMemory.mem.ltm.storeFrequency = 10
+# ArmarX.SkillMemory.mem.ltm.storeOnStop:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.SkillMemory.mem.ltm.storeOnStop = false
+
+
# ArmarX.SkillMemory.mem.statechartlistener.seg.CoreMaxHistorySize: Maximal size of the Transitions entity histories (-1 for infinite).
# Attributes:
# - Default: 10
diff --git a/scenarios/Armar6HighlevelApps/config/control_memory.cfg b/scenarios/Armar6HighlevelApps/config/control_memory.cfg
index 84606c5a4..58c1a5ef5 100644
--- a/scenarios/Armar6HighlevelApps/config/control_memory.cfg
+++ b/scenarios/Armar6HighlevelApps/config/control_memory.cfg
@@ -103,10 +103,10 @@
# ArmarX.ControlMemory.mem.ltm.configuration:
# Attributes:
-# - Default: { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}, "PngConverter": {}, "ExrConverter": {}}
+# - Default: { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ControlMemory.mem.ltm.configuration = { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}, "PngConverter": {}, "ExrConverter": {}}
+# ArmarX.ControlMemory.mem.ltm.configuration = { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}}
# ArmarX.ControlMemory.mem.ltm.enabled:
@@ -134,6 +134,32 @@
# ArmarX.ControlMemory.mem.ltm.exportPath = ""
+# ArmarX.ControlMemory.mem.ltm.importOnStartUp:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ControlMemory.mem.ltm.importOnStartUp = false
+
+
+# ArmarX.ControlMemory.mem.ltm.loadedCoreSegments:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.mem.ltm.loadedCoreSegments = ""
+
+
+# ArmarX.ControlMemory.mem.ltm.maxAmountSnapshotsLoaded:
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.mem.ltm.maxAmountSnapshotsLoaded = 1
+
+
+
# ArmarX.ControlMemory.mem.ltm.storeFrequency:
# Attributes:
# - Default: 10
@@ -142,6 +168,15 @@
# ArmarX.ControlMemory.mem.ltm.storeFrequency = 10
+# ArmarX.ControlMemory.mem.ltm.storeOnStop:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ControlMemory.mem.ltm.storeOnStop = false
+
+
# ArmarX.ControlMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
# Set to false to use this memory as a stand-alone.
# Attributes:
diff --git a/scenarios/Armar6HighlevelApps/config/navigation_memory.cfg b/scenarios/Armar6HighlevelApps/config/navigation_memory.cfg
index b59cbc1ed..9ff5ef6df 100644
--- a/scenarios/Armar6HighlevelApps/config/navigation_memory.cfg
+++ b/scenarios/Armar6HighlevelApps/config/navigation_memory.cfg
@@ -237,10 +237,10 @@
# ArmarX.navigation_memory.mem.ltm.configuration:
# Attributes:
-# - Default: { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}, "PngConverter": {}, "ExrConverter": {}}
+# - Default: { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.navigation_memory.mem.ltm.configuration = { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}, "PngConverter": {}, "ExrConverter": {}}
+# ArmarX.navigation_memory.mem.ltm.configuration = { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}}
# ArmarX.navigation_memory.mem.ltm.enabled:
@@ -268,6 +268,33 @@
# ArmarX.navigation_memory.mem.ltm.exportPath = ""
+# ArmarX.navigation_memory.mem.ltm.importOnStartUp:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.navigation_memory.mem.ltm.importOnStartUp = false
+
+
+# ArmarX.navigation_memory.mem.ltm.loadedCoreSegments:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigation_memory.mem.ltm.loadedCoreSegments = ""
+
+
+# ArmarX.navigation_memory.mem.ltm.maxAmountSnapshotsLoaded:
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigation_memory.mem.ltm.maxAmountSnapshotsLoaded = 1
+
+
+
+
# ArmarX.navigation_memory.mem.ltm.storeFrequency:
# Attributes:
# - Default: 10
@@ -276,6 +303,15 @@
# ArmarX.navigation_memory.mem.ltm.storeFrequency = 10
+# ArmarX.navigation_memory.mem.ltm.storeOnStop:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.navigation_memory.mem.ltm.storeOnStop = false
+
+
# ArmarX.navigation_memory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
# Set to false to use this memory as a stand-alone.
# Attributes:
diff --git a/scenarios/Armar6HighlevelApps/config/speech_memory.cfg b/scenarios/Armar6HighlevelApps/config/speech_memory.cfg
index 91c23b44a..8282041b4 100644
--- a/scenarios/Armar6HighlevelApps/config/speech_memory.cfg
+++ b/scenarios/Armar6HighlevelApps/config/speech_memory.cfg
@@ -244,6 +244,33 @@
# ArmarX.speech_memory.mem.ltm.exportPath = ""
+# ArmarX.speech_memory.mem.ltm.importOnStartUp:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.speech_memory.mem.ltm.importOnStartUp = false
+
+
+# ArmarX.speech_memory.mem.ltm.loadedCoreSegments:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.speech_memory.mem.ltm.loadedCoreSegments = ""
+
+
+# ArmarX.speech_memory.mem.ltm.maxAmountSnapshotsLoaded:
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.speech_memory.mem.ltm.maxAmountSnapshotsLoaded = 1
+
+
+
+
# ArmarX.speech_memory.mem.ltm.storeFrequency:
# Attributes:
# - Default: 10
@@ -252,6 +279,15 @@
# ArmarX.speech_memory.mem.ltm.storeFrequency = 10
+# ArmarX.speech_memory.mem.ltm.storeOnStop:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.speech_memory.mem.ltm.storeOnStop = false
+
+
# ArmarX.speech_memory.memory.cmd.EntityName: Name of the command entities.
# Attributes:
# - Default: command
diff --git a/scenarios/Armar6HighlevelApps/config/symbolic_scene_memory.cfg b/scenarios/Armar6HighlevelApps/config/symbolic_scene_memory.cfg
index ddf38e96f..404c14275 100644
--- a/scenarios/Armar6HighlevelApps/config/symbolic_scene_memory.cfg
+++ b/scenarios/Armar6HighlevelApps/config/symbolic_scene_memory.cfg
@@ -223,10 +223,10 @@
# ArmarX.SymbolicSceneMemory.mem.ltm.configuration:
# Attributes:
-# - Default: { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}, "PngConverter": {}, "ExrConverter": {}}
+# - Default: { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SymbolicSceneMemory.mem.ltm.configuration = { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}, "PngConverter": {}, "ExrConverter": {}}
+# ArmarX.SymbolicSceneMemory.mem.ltm.configuration = { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}}
# ArmarX.SymbolicSceneMemory.mem.ltm.enabled:
@@ -254,6 +254,33 @@
# ArmarX.SymbolicSceneMemory.mem.ltm.exportPath = ""
+# ArmarX.SymbolicSceneMemory.mem.ltm.importOnStartUp:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.SymbolicSceneMemory.mem.ltm.importOnStartUp = false
+
+
+# ArmarX.SymbolicSceneMemory.mem.ltm.loadedCoreSegments:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SymbolicSceneMemory.mem.ltm.loadedCoreSegments = ""
+
+
+# ArmarX.SymbolicSceneMemory.mem.ltm.maxAmountSnapshotsLoaded:
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SymbolicSceneMemory.mem.ltm.maxAmountSnapshotsLoaded = 1
+
+
+
+
# ArmarX.SymbolicSceneMemory.mem.ltm.storeFrequency:
# Attributes:
# - Default: 10
diff --git a/scenarios/Armar6HighlevelApps/config/view_memory.cfg b/scenarios/Armar6HighlevelApps/config/view_memory.cfg
index 2e0949b67..41f0807fe 100644
--- a/scenarios/Armar6HighlevelApps/config/view_memory.cfg
+++ b/scenarios/Armar6HighlevelApps/config/view_memory.cfg
@@ -205,10 +205,10 @@
# ArmarX.ViewMemory.mem.ltm.configuration:
# Attributes:
-# - Default: { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}, "PngConverter": {}, "ExrConverter": {}}
+# - Default: { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ViewMemory.mem.ltm.configuration = { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}, "PngConverter": {}, "ExrConverter": {}}
+# ArmarX.ViewMemory.mem.ltm.configuration = { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}}
# ArmarX.ViewMemory.mem.ltm.enabled:
@@ -236,6 +236,33 @@
# ArmarX.ViewMemory.mem.ltm.exportPath = ""
+# ArmarX.ViewMemory.mem.ltm.importOnStartUp:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ViewMemory.mem.ltm.importOnStartUp = false
+
+
+# ArmarX.ViewMemory.mem.ltm.loadedCoreSegments:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ViewMemory.mem.ltm.loadedCoreSegments = ""
+
+
+# ArmarX.ViewMemory.mem.ltm.maxAmountSnapshotsLoaded:
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ViewMemory.mem.ltm.maxAmountSnapshotsLoaded = 1
+
+
+
+
# ArmarX.ViewMemory.mem.ltm.storeFrequency:
# Attributes:
# - Default: 10
@@ -244,6 +271,15 @@
# ArmarX.ViewMemory.mem.ltm.storeFrequency = 10
+# ArmarX.ViewMemory.mem.ltm.storeOnStop:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ViewMemory.mem.ltm.storeOnStop = false
+
+
# ArmarX.ViewMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
# Set to false to use this memory as a stand-alone.
# Attributes:
diff --git a/scenarios/Armar6ManipulationPipeline/config/action_executor.cfg b/scenarios/Armar6ManipulationPipeline/config/action_executor.cfg
index c26f212b2..1fa78ffc5 100644
--- a/scenarios/Armar6ManipulationPipeline/config/action_executor.cfg
+++ b/scenarios/Armar6ManipulationPipeline/config/action_executor.cfg
@@ -722,87 +722,6 @@ ArmarX.action_execution.RobotUnitName = Armar6Unit
# - Required: no
# ArmarX.action_execution.Visualization.head_path = armar6_models/robotmodel/armar6/simox/Armar6-Head.xml
-
-# ArmarX.action_execution.Visualization.head_root_node_name:
-# Attributes:
-# - Default: Head Base
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.action_execution.Visualization.head_root_node_name = Head Base
-
-
-# ArmarX.action_execution.Visualization.left_hand_closed_joints:
-# Attributes:
-# - Default: Index L 1 Joint:90,Index L 2 Joint:90,Index L 3 Joint:90,Middle L 1 Joint:90,Middle L 2 Joint:90,Middle L 3 Joint:90,Pinky L 1 Joint:90,Pinky L 2 Joint:90,Pinky L 3 Joint:90,Ring L 1 Joint:90,Ring L 2 Joint:90,Ring L 3 Joint:90,Thumb L 1 Joint:90,Thumb L 2 Joint:90
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.action_execution.Visualization.left_hand_closed_joints = Index L 1 Joint:90,Index L 2 Joint:90,Index L 3 Joint:90,Middle L 1 Joint:90,Middle L 2 Joint:90,Middle L 3 Joint:90,Pinky L 1 Joint:90,Pinky L 2 Joint:90,Pinky L 3 Joint:90,Ring L 1 Joint:90,Ring L 2 Joint:90,Ring L 3 Joint:90,Thumb L 1 Joint:90,Thumb L 2 Joint:90
-
-
-# ArmarX.action_execution.Visualization.left_hand_opened_joints:
-# Attributes:
-# - Default: Index L 1 Joint:0,Index L 2 Joint:0,Index L 3 Joint:0,Middle L 1 Joint:0,Middle L 2 Joint:0,Middle L 3 Joint:0,Pinky L 1 Joint:0,Pinky L 2 Joint:0,Pinky L 3 Joint:0,Ring L 1 Joint:0,Ring L 2 Joint:0,Ring L 3 Joint:0,Thumb L 1 Joint:0,Thumb L 2 Joint:0
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.action_execution.Visualization.left_hand_opened_joints = Index L 1 Joint:0,Index L 2 Joint:0,Index L 3 Joint:0,Middle L 1 Joint:0,Middle L 2 Joint:0,Middle L 3 Joint:0,Pinky L 1 Joint:0,Pinky L 2 Joint:0,Pinky L 3 Joint:0,Ring L 1 Joint:0,Ring L 2 Joint:0,Ring L 3 Joint:0,Thumb L 1 Joint:0,Thumb L 2 Joint:0
-
-
-# ArmarX.action_execution.Visualization.left_hand_path:
-# Attributes:
-# - Default: armar6_models/robotmodel/armar6/simox/Armar6-LeftHand-v3.xml
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.action_execution.Visualization.left_hand_path = armar6_models/robotmodel/armar6/simox/Armar6-LeftHand-v3.xml
-
-
-# ArmarX.action_execution.Visualization.left_hand_root_node_name:
-# Attributes:
-# - Default: Hand L Root
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.action_execution.Visualization.left_hand_root_node_name = Hand L Root
-
-
-# ArmarX.action_execution.Visualization.right_hand_closed_joints:
-# Attributes:
-# - Default: Index R 1 Joint:90,Index R 2 Joint:90,Index R 3 Joint:90,Middle R 1 Joint:90,Middle R 2 Joint:90,Middle R 3 Joint:90,Pinky R 1 Joint:90,Pinky R 2 Joint:90,Pinky R 3 Joint:90,Ring R 1 Joint:90,Ring R 2 Joint:90,Ring R 3 Joint:90,Thumb R 1 Joint:90,Thumb R 2 Joint:90
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.action_execution.Visualization.right_hand_closed_joints = Index R 1 Joint:90,Index R 2 Joint:90,Index R 3 Joint:90,Middle R 1 Joint:90,Middle R 2 Joint:90,Middle R 3 Joint:90,Pinky R 1 Joint:90,Pinky R 2 Joint:90,Pinky R 3 Joint:90,Ring R 1 Joint:90,Ring R 2 Joint:90,Ring R 3 Joint:90,Thumb R 1 Joint:90,Thumb R 2 Joint:90
-
-
-# ArmarX.action_execution.Visualization.right_hand_opened_joints:
-# Attributes:
-# - Default: Index R 1 Joint:0,Index R 2 Joint:0,Index R 3 Joint:0,Middle R 1 Joint:0,Middle R 2 Joint:0,Middle R 3 Joint:0,Pinky R 1 Joint:0,Pinky R 2 Joint:0,Pinky R 3 Joint:0,Ring R 1 Joint:0,Ring R 2 Joint:0,Ring R 3 Joint:0,Thumb R 1 Joint:0,Thumb R 2 Joint:0
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.action_execution.Visualization.right_hand_opened_joints = Index R 1 Joint:0,Index R 2 Joint:0,Index R 3 Joint:0,Middle R 1 Joint:0,Middle R 2 Joint:0,Middle R 3 Joint:0,Pinky R 1 Joint:0,Pinky R 2 Joint:0,Pinky R 3 Joint:0,Ring R 1 Joint:0,Ring R 2 Joint:0,Ring R 3 Joint:0,Thumb R 1 Joint:0,Thumb R 2 Joint:0
-
-
-# ArmarX.action_execution.Visualization.right_hand_path:
-# Attributes:
-# - Default: armar6_models/robotmodel/armar6/simox/Armar6-RightHand-v3.xml
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.action_execution.Visualization.right_hand_path = armar6_models/robotmodel/armar6/simox/Armar6-RightHand-v3.xml
-
-
-# ArmarX.action_execution.Visualization.right_hand_root_node_name:
-# Attributes:
-# - Default: Hand R Root
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.action_execution.Visualization.right_hand_root_node_name = Hand R Root
-
-
-# ArmarX.action_execution.Visualization.robot_path:
-# Attributes:
-# - Default: armar6_models/robotmodel/armar6/simox/Armar6-SH.xml
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.action_execution.Visualization.robot_path = armar6_models/robotmodel/armar6/simox/Armar6-SH.xml
-
-
# ArmarX.action_execution.Visualization.robot_project_dir:
# Attributes:
# - Default: armar6_models
@@ -853,6 +772,22 @@ ArmarX.action_execution.RobotUnitName = Armar6Unit
# ArmarX.action_execution.mem.manip.executed_action.Provider = ""
+# ArmarX.action_execution.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.action_execution.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.action_execution.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.action_execution.mem.robot_state.localizationSegment = Localization
+
+
# ArmarX.action_execution.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
# Set to false to use this memory as a stand-alone.
# Attributes:
diff --git a/scenarios/Armar6ManipulationPipeline/config/manipulation_memory.cfg b/scenarios/Armar6ManipulationPipeline/config/manipulation_memory.cfg
index 298a52d59..2689ed088 100644
--- a/scenarios/Armar6ManipulationPipeline/config/manipulation_memory.cfg
+++ b/scenarios/Armar6ManipulationPipeline/config/manipulation_memory.cfg
@@ -153,10 +153,10 @@ ArmarX.ManipulationMemory.handTrajectoryPackagePaths = armar6_skills:motions/gra
# ArmarX.ManipulationMemory.mem.ltm.configuration:
# Attributes:
-# - Default: { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}, "PngConverter": {}, "ExrConverter": {}}
+# - Default: { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ManipulationMemory.mem.ltm.configuration = { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}, "PngConverter": {}, "ExrConverter": {}}
+# ArmarX.ManipulationMemory.mem.ltm.configuration = { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}}
# ArmarX.ManipulationMemory.mem.ltm.enabled:
@@ -184,6 +184,33 @@ ArmarX.ManipulationMemory.handTrajectoryPackagePaths = armar6_skills:motions/gra
# ArmarX.ManipulationMemory.mem.ltm.exportPath = ""
+# ArmarX.ManipulationMemory.mem.ltm.importOnStartUp:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ManipulationMemory.mem.ltm.importOnStartUp = false
+
+
+# ArmarX.ManipulationMemory.mem.ltm.loadedCoreSegments:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ManipulationMemory.mem.ltm.loadedCoreSegments = ""
+
+
+# ArmarX.ManipulationMemory.mem.ltm.maxAmountSnapshotsLoaded:
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ManipulationMemory.mem.ltm.maxAmountSnapshotsLoaded = 1
+
+
+
+
# ArmarX.ManipulationMemory.mem.ltm.storeFrequency:
# Attributes:
# - Default: 10
@@ -192,6 +219,15 @@ ArmarX.ManipulationMemory.handTrajectoryPackagePaths = armar6_skills:motions/gra
# ArmarX.ManipulationMemory.mem.ltm.storeFrequency = 10
+# ArmarX.ManipulationMemory.mem.ltm.storeOnStop:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ManipulationMemory.mem.ltm.storeOnStop = false
+
+
# ArmarX.ManipulationMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
# Set to false to use this memory as a stand-alone.
# Attributes:
--
GitLab
From c55b0689b8171197420f8755ef183bc1ba09f2dc Mon Sep 17 00:00:00 2001
From: Tilman Daab <tilman.daab@kit.edu>
Date: Tue, 25 Feb 2025 13:07:30 +0100
Subject: [PATCH 17/22] adapt LTM configuration for HARIA recordings
---
.../config/ObjectMemory.cfg | 12 ++++++++++--
.../config/RobotStateMemory.cfg | 14 +++++++++++---
.../config/SkillsMemory.cfg | 10 ++++++++--
.../config/control_memory.cfg | 11 +++++++++--
.../config/navigation_memory.cfg | 10 ++++++++--
.../config/speech_memory.cfg | 14 ++++++++++----
.../config/symbolic_scene_memory.cfg | 19 +++++++++++++++++--
.../config/manipulation_memory.cfg | 10 ++++++++--
8 files changed, 81 insertions(+), 19 deletions(-)
diff --git a/scenarios/Armar6HighlevelApps/config/ObjectMemory.cfg b/scenarios/Armar6HighlevelApps/config/ObjectMemory.cfg
index 786f04a2f..0eb1e7010 100644
--- a/scenarios/Armar6HighlevelApps/config/ObjectMemory.cfg
+++ b/scenarios/Armar6HighlevelApps/config/ObjectMemory.cfg
@@ -792,7 +792,7 @@ ArmarX.ObjectMemory.mem.ltm.enabled = false
# - Default: MemoryExport
# - Case sensitivity: yes
# - Required: no
-ArmarX.ObjectMemory.mem.ltm.exportName = RecordingsJoana
+ArmarX.ObjectMemory.mem.ltm.exportName = MemoryExport_HARIA
# ArmarX.ObjectMemory.mem.ltm.exportPath:
@@ -800,7 +800,8 @@ ArmarX.ObjectMemory.mem.ltm.exportName = RecordingsJoana
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-ArmarX.ObjectMemory.mem.ltm.exportPath = "/media/ltm"
+ArmarX.ObjectMemory.mem.ltm.exportPath = $HOME
+
# ArmarX.ObjectMemory.mem.ltm.importOnStartUp:
# Attributes:
@@ -827,6 +828,12 @@ ArmarX.ObjectMemory.mem.ltm.exportPath = "/media/ltm"
# ArmarX.ObjectMemory.mem.ltm.maxAmountSnapshotsLoaded = 1
+# ArmarX.ObjectMemory.mem.ltm.recordingMode:
+# Attributes:
+# - Default: CONSOLIDATE_REMOVED
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.ObjectMemory.mem.ltm.recordingMode = CONSOLIDATE_ALL
# ArmarX.ObjectMemory.mem.ltm.storeFrequency:
@@ -845,6 +852,7 @@ ArmarX.ObjectMemory.mem.ltm.exportPath = "/media/ltm"
# - Possible values: {0, 1, false, no, true, yes}
# ArmarX.ObjectMemory.mem.ltm.storeOnStop = false
+
# ArmarX.ObjectMemory.mem.robot_state.Memory:
# Attributes:
# - Default: RobotState
diff --git a/scenarios/Armar6HighlevelApps/config/RobotStateMemory.cfg b/scenarios/Armar6HighlevelApps/config/RobotStateMemory.cfg
index 44e0d9a91..59c528d8f 100644
--- a/scenarios/Armar6HighlevelApps/config/RobotStateMemory.cfg
+++ b/scenarios/Armar6HighlevelApps/config/RobotStateMemory.cfg
@@ -259,7 +259,7 @@ ArmarX.RobotStateMemory.WaitForRobotUnit = true
# ArmarX.RobotStateMemory.mem.ltm.configuration:
# Attributes:
-# - Default: { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}, "PngConverter": {}, "ExrConverter": {}}
+# - Default: { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}}
# - Case sensitivity: yes
# - Required: no
ArmarX.RobotStateMemory.mem.ltm.configuration = { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 100}, "PngConverter": {}}
@@ -279,7 +279,7 @@ ArmarX.RobotStateMemory.mem.ltm.enabled = false
# - Default: MemoryExport
# - Case sensitivity: yes
# - Required: no
-ArmarX.RobotStateMemory.mem.ltm.exportName = "RecordingsJoana"
+ArmarX.RobotStateMemory.mem.ltm.exportName = MemoryExport_HARIA
# ArmarX.RobotStateMemory.mem.ltm.exportPath:
@@ -287,7 +287,8 @@ ArmarX.RobotStateMemory.mem.ltm.exportName = "RecordingsJoana"
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-ArmarX.RobotStateMemory.mem.ltm.exportPath = "/media/ltm"
+ArmarX.RobotStateMemory.mem.ltm.exportPath = $HOME
+
# ArmarX.RobotStateMemory.mem.ltm.importOnStartUp:
# Attributes:
@@ -314,6 +315,13 @@ ArmarX.RobotStateMemory.mem.ltm.exportPath = "/media/ltm"
# ArmarX.RobotStateMemory.mem.ltm.maxAmountSnapshotsLoaded = 1
+# ArmarX.RobotStateMemory.mem.ltm.recordingMode:
+# Attributes:
+# - Default: CONSOLIDATE_REMOVED
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.RobotStateMemory.mem.ltm.recordingMode = CONSOLIDATE_ALL
+
# ArmarX.RobotStateMemory.mem.ltm.storeFrequency:
# Attributes:
diff --git a/scenarios/Armar6HighlevelApps/config/SkillsMemory.cfg b/scenarios/Armar6HighlevelApps/config/SkillsMemory.cfg
index 3b74e01e6..d9a3bfbb3 100644
--- a/scenarios/Armar6HighlevelApps/config/SkillsMemory.cfg
+++ b/scenarios/Armar6HighlevelApps/config/SkillsMemory.cfg
@@ -246,7 +246,7 @@
# - Default: MemoryExport
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SkillMemory.mem.ltm.exportName = MemoryExport
+ArmarX.SkillMemory.mem.ltm.exportName = MemoryExport_HARIA
# ArmarX.SkillMemory.mem.ltm.exportPath:
@@ -254,7 +254,7 @@
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SkillMemory.mem.ltm.exportPath = ""
+ArmarX.SkillMemory.mem.ltm.exportPath = $HOME
# ArmarX.SkillMemory.mem.ltm.importOnStartUp:
@@ -282,6 +282,12 @@
# ArmarX.SkillMemory.mem.ltm.maxAmountSnapshotsLoaded = 1
+# ArmarX.SkillMemory.mem.ltm.recordingMode:
+# Attributes:
+# - Default: CONSOLIDATE_REMOVED
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.SkillMemory.mem.ltm.recordingMode = CONSOLIDATE_ALL
# ArmarX.SkillMemory.mem.ltm.storeFrequency:
diff --git a/scenarios/Armar6HighlevelApps/config/control_memory.cfg b/scenarios/Armar6HighlevelApps/config/control_memory.cfg
index 58c1a5ef5..9ad4b5435 100644
--- a/scenarios/Armar6HighlevelApps/config/control_memory.cfg
+++ b/scenarios/Armar6HighlevelApps/config/control_memory.cfg
@@ -123,7 +123,7 @@
# - Default: MemoryExport
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ControlMemory.mem.ltm.exportName = MemoryExport
+ArmarX.ControlMemory.mem.ltm.exportName = MemoryExport_HARIA
# ArmarX.ControlMemory.mem.ltm.exportPath:
@@ -131,7 +131,7 @@
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ControlMemory.mem.ltm.exportPath = ""
+ArmarX.ControlMemory.mem.ltm.exportPath = $HOME
# ArmarX.ControlMemory.mem.ltm.importOnStartUp:
@@ -159,6 +159,13 @@
# ArmarX.ControlMemory.mem.ltm.maxAmountSnapshotsLoaded = 1
+# ArmarX.ControlMemory.mem.ltm.recordingMode:
+# Attributes:
+# - Default: CONSOLIDATE_REMOVED
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.ControlMemory.mem.ltm.recordingMode = CONSOLIDATE_ALL
+
# ArmarX.ControlMemory.mem.ltm.storeFrequency:
# Attributes:
diff --git a/scenarios/Armar6HighlevelApps/config/navigation_memory.cfg b/scenarios/Armar6HighlevelApps/config/navigation_memory.cfg
index 9ff5ef6df..d60a1b190 100644
--- a/scenarios/Armar6HighlevelApps/config/navigation_memory.cfg
+++ b/scenarios/Armar6HighlevelApps/config/navigation_memory.cfg
@@ -257,7 +257,7 @@
# - Default: MemoryExport
# - Case sensitivity: yes
# - Required: no
-# ArmarX.navigation_memory.mem.ltm.exportName = MemoryExport
+ArmarX.navigation_memory.mem.ltm.exportName = MemoryExport_HARIA
# ArmarX.navigation_memory.mem.ltm.exportPath:
@@ -265,7 +265,7 @@
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.navigation_memory.mem.ltm.exportPath = ""
+ArmarX.navigation_memory.mem.ltm.exportPath = $HOME
# ArmarX.navigation_memory.mem.ltm.importOnStartUp:
@@ -293,6 +293,12 @@
# ArmarX.navigation_memory.mem.ltm.maxAmountSnapshotsLoaded = 1
+# ArmarX.navigation_memory.mem.ltm.recordingMode:
+# Attributes:
+# - Default: CONSOLIDATE_REMOVED
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.navigation_memory.mem.ltm.recordingMode = CONSOLIDATE_ALL
# ArmarX.navigation_memory.mem.ltm.storeFrequency:
diff --git a/scenarios/Armar6HighlevelApps/config/speech_memory.cfg b/scenarios/Armar6HighlevelApps/config/speech_memory.cfg
index 8282041b4..723724a8e 100644
--- a/scenarios/Armar6HighlevelApps/config/speech_memory.cfg
+++ b/scenarios/Armar6HighlevelApps/config/speech_memory.cfg
@@ -213,10 +213,10 @@
# ArmarX.speech_memory.mem.ltm.configuration:
# Attributes:
-# - Default: { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}, "PngConverter": {}, "ExrConverter": {}}
+# - Default: { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.speech_memory.mem.ltm.configuration = { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}, "PngConverter": {}, "ExrConverter": {}}
+# ArmarX.speech_memory.mem.ltm.configuration = { "SnapshotFrequencyFilter": {"WaitingTimeInMsForFilter" : 50}}
# ArmarX.speech_memory.mem.ltm.enabled:
@@ -233,7 +233,7 @@
# - Default: MemoryExport
# - Case sensitivity: yes
# - Required: no
-# ArmarX.speech_memory.mem.ltm.exportName = MemoryExport
+ArmarX.speech_memory.mem.ltm.exportName = MemoryExport_HARIA
# ArmarX.speech_memory.mem.ltm.exportPath:
@@ -241,7 +241,7 @@
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.speech_memory.mem.ltm.exportPath = ""
+ArmarX.speech_memory.mem.ltm.exportPath = $HOME
# ArmarX.speech_memory.mem.ltm.importOnStartUp:
@@ -269,6 +269,12 @@
# ArmarX.speech_memory.mem.ltm.maxAmountSnapshotsLoaded = 1
+# ArmarX.speech_memory.mem.ltm.recordingMode:
+# Attributes:
+# - Default: CONSOLIDATE_REMOVED
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.speech_memory.mem.ltm.recordingMode = CONSOLIDATE_ALL
# ArmarX.speech_memory.mem.ltm.storeFrequency:
diff --git a/scenarios/Armar6HighlevelApps/config/symbolic_scene_memory.cfg b/scenarios/Armar6HighlevelApps/config/symbolic_scene_memory.cfg
index 404c14275..e68b37f1c 100644
--- a/scenarios/Armar6HighlevelApps/config/symbolic_scene_memory.cfg
+++ b/scenarios/Armar6HighlevelApps/config/symbolic_scene_memory.cfg
@@ -243,7 +243,7 @@
# - Default: MemoryExport
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SymbolicSceneMemory.mem.ltm.exportName = MemoryExport
+ArmarX.SymbolicSceneMemory.mem.ltm.exportName = MemoryExport_HARIA
# ArmarX.SymbolicSceneMemory.mem.ltm.exportPath:
@@ -251,7 +251,7 @@
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SymbolicSceneMemory.mem.ltm.exportPath = ""
+ArmarX.SymbolicSceneMemory.mem.ltm.exportPath = $HOME
# ArmarX.SymbolicSceneMemory.mem.ltm.importOnStartUp:
@@ -279,6 +279,12 @@
# ArmarX.SymbolicSceneMemory.mem.ltm.maxAmountSnapshotsLoaded = 1
+# ArmarX.SymbolicSceneMemory.mem.ltm.recordingMode:
+# Attributes:
+# - Default: CONSOLIDATE_REMOVED
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.SymbolicSceneMemory.mem.ltm.recordingMode = CONSOLIDATE_ALL
# ArmarX.SymbolicSceneMemory.mem.ltm.storeFrequency:
@@ -289,6 +295,15 @@
# ArmarX.SymbolicSceneMemory.mem.ltm.storeFrequency = 10
+# ArmarX.SymbolicSceneMemory.mem.ltm.storeOnStop:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.SymbolicSceneMemory.mem.ltm.storeOnStop = false
+
+
# ArmarX.SymbolicSceneMemory.mem.nav.graph.CoreSegment:
# Attributes:
# - Default: Location
diff --git a/scenarios/Armar6ManipulationPipeline/config/manipulation_memory.cfg b/scenarios/Armar6ManipulationPipeline/config/manipulation_memory.cfg
index 2689ed088..c1ebf4a0a 100644
--- a/scenarios/Armar6ManipulationPipeline/config/manipulation_memory.cfg
+++ b/scenarios/Armar6ManipulationPipeline/config/manipulation_memory.cfg
@@ -173,7 +173,7 @@ ArmarX.ManipulationMemory.handTrajectoryPackagePaths = armar6_skills:motions/gra
# - Default: MemoryExport
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ManipulationMemory.mem.ltm.exportName = MemoryExport
+ArmarX.ManipulationMemory.mem.ltm.exportName = MemoryExport_HARIA
# ArmarX.ManipulationMemory.mem.ltm.exportPath:
@@ -181,7 +181,7 @@ ArmarX.ManipulationMemory.handTrajectoryPackagePaths = armar6_skills:motions/gra
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ManipulationMemory.mem.ltm.exportPath = ""
+ArmarX.ManipulationMemory.mem.ltm.exportPath = $HOME
# ArmarX.ManipulationMemory.mem.ltm.importOnStartUp:
@@ -209,6 +209,12 @@ ArmarX.ManipulationMemory.handTrajectoryPackagePaths = armar6_skills:motions/gra
# ArmarX.ManipulationMemory.mem.ltm.maxAmountSnapshotsLoaded = 1
+# ArmarX.ManipulationMemory.mem.ltm.recordingMode:
+# Attributes:
+# - Default: CONSOLIDATE_REMOVED
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.ManipulationMemory.mem.ltm.recordingMode = CONSOLIDATE_ALL
# ArmarX.ManipulationMemory.mem.ltm.storeFrequency:
--
GitLab
From ee22739d65f9c838630931042e07e4d8c52ed706 Mon Sep 17 00:00:00 2001
From: Tilman Daab <tilman.daab@kit.edu>
Date: Tue, 25 Feb 2025 13:07:49 +0100
Subject: [PATCH 18/22] uncommitted change, startup-delay for navigator
---
scenarios/Armar6HighlevelApps/config/navigator.cfg | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/scenarios/Armar6HighlevelApps/config/navigator.cfg b/scenarios/Armar6HighlevelApps/config/navigator.cfg
index 651edda2b..d12f0aa65 100644
--- a/scenarios/Armar6HighlevelApps/config/navigator.cfg
+++ b/scenarios/Armar6HighlevelApps/config/navigator.cfg
@@ -147,7 +147,7 @@ ArmarX.Navigator.p.occupancy_grid.occopied_threshold = 0.8
# - Default: 0
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SecondsStartupDelay = 0
+ArmarX.SecondsStartupDelay = 10
# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
--
GitLab
From b4dcbb38f2910ec5b537036eeab6213ac0f661fb Mon Sep 17 00:00:00 2001
From: Tilman Daab <tilman.daab@kit.edu>
Date: Tue, 25 Feb 2025 13:08:27 +0100
Subject: [PATCH 19/22] uncommitted changes, adapting manipulation pipeline
properties
---
.../config/action_executor.cfg | 17 +++++------------
1 file changed, 5 insertions(+), 12 deletions(-)
diff --git a/scenarios/Armar6ManipulationPipeline/config/action_executor.cfg b/scenarios/Armar6ManipulationPipeline/config/action_executor.cfg
index 1fa78ffc5..95af70e47 100644
--- a/scenarios/Armar6ManipulationPipeline/config/action_executor.cfg
+++ b/scenarios/Armar6ManipulationPipeline/config/action_executor.cfg
@@ -454,7 +454,7 @@ ArmarX.Verbosity = Debug
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.action_execution.Config.robotName = ""
+ArmarX.action_execution.Config.robotName = "Armar6"
# ArmarX.action_execution.DRT.ControllerType: Type of the controller
@@ -566,7 +566,7 @@ ArmarX.action_execution.DRT.ControllerType = NJointTaskSpaceImpedanceDMPControll
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-ArmarX.action_execution.Global.attach_detach_object_to_eef = true
+# ArmarX.action_execution.Global.attach_detach_object_to_eef = false
# ArmarX.action_execution.Global.cartesian_wp_config.threshold_orientation_near:
@@ -715,19 +715,12 @@ ArmarX.action_execution.Global.attach_detach_object_to_eef = true
ArmarX.action_execution.RobotUnitName = Armar6Unit
-# ArmarX.action_execution.Visualization.head_path:
+# ArmarX.action_execution.Visualization.robotName:
# Attributes:
-# - Default: armar6_models/robotmodel/armar6/simox/Armar6-Head.xml
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.action_execution.Visualization.head_path = armar6_models/robotmodel/armar6/simox/Armar6-Head.xml
-
-# ArmarX.action_execution.Visualization.robot_project_dir:
-# Attributes:
-# - Default: armar6_models
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.action_execution.Visualization.robot_project_dir = armar6_models
+ArmarX.action_execution.Visualization.robotName = "Armar6"
# ArmarX.action_execution.Visualization.use_collision_model_for_hands:
--
GitLab
From 749c67267b59c5dd514b04e32a66a2d89e2ff238 Mon Sep 17 00:00:00 2001
From: Tilman Daab <tilman.daab@kit.edu>
Date: Tue, 25 Feb 2025 13:08:43 +0100
Subject: [PATCH 20/22] further LTM configuration change for HARIA recordings
---
scenarios/Armar6HighlevelApps/config/view_memory.cfg | 10 ++++++++--
1 file changed, 8 insertions(+), 2 deletions(-)
diff --git a/scenarios/Armar6HighlevelApps/config/view_memory.cfg b/scenarios/Armar6HighlevelApps/config/view_memory.cfg
index 41f0807fe..436a64ce0 100644
--- a/scenarios/Armar6HighlevelApps/config/view_memory.cfg
+++ b/scenarios/Armar6HighlevelApps/config/view_memory.cfg
@@ -225,7 +225,7 @@
# - Default: MemoryExport
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ViewMemory.mem.ltm.exportName = MemoryExport
+ArmarX.ViewMemory.mem.ltm.exportName = MemoryExport_HARIA
# ArmarX.ViewMemory.mem.ltm.exportPath:
@@ -233,7 +233,7 @@
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ViewMemory.mem.ltm.exportPath = ""
+ArmarX.ViewMemory.mem.ltm.exportPath = $HOME
# ArmarX.ViewMemory.mem.ltm.importOnStartUp:
@@ -261,6 +261,12 @@
# ArmarX.ViewMemory.mem.ltm.maxAmountSnapshotsLoaded = 1
+# ArmarX.ViewMemory.mem.ltm.recordingMode:
+# Attributes:
+# - Default: CONSOLIDATE_REMOVED
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.ViewMemory.mem.ltm.recordingMode = CONSOLIDATE_ALL
# ArmarX.ViewMemory.mem.ltm.storeFrequency:
--
GitLab
From 830474aa174bf2d8904a5f44a8da37a971560f40 Mon Sep 17 00:00:00 2001
From: Tilman Daab <tilman.daab@kit.edu>
Date: Tue, 25 Feb 2025 13:09:06 +0100
Subject: [PATCH 21/22] uncommitted change, disabling
articulated_objects_skill_provider
---
scenarios/Armar6Skills/Armar6Skills.scx | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/scenarios/Armar6Skills/Armar6Skills.scx b/scenarios/Armar6Skills/Armar6Skills.scx
index 607e50138..5cda5b967 100644
--- a/scenarios/Armar6Skills/Armar6Skills.scx
+++ b/scenarios/Armar6Skills/Armar6Skills.scx
@@ -3,7 +3,7 @@
<application name="introduction_skill_provider" instance="" package="armar6_skills" nodeName="" enabled="true"/>
<application name="navigation_skill_provider" instance="" package="armarx_navigation" nodeName="" enabled="true"/>
<application name="view_selection_skill_provider" instance="" package="armarx_view_selection" nodeName="" enabled="true"/>
- <application name="articulated_objects_skill_provider" instance="" package="armarx_manipulation" nodeName="" enabled="true"/>
+ <application name="articulated_objects_skill_provider" instance="" package="armarx_manipulation" nodeName="" enabled="false"/>
<application name="Armar6KnownObjectGraspProvider" instance="" package="armar6_skills" nodeName="" enabled="true"/>
<application name="hand_over_skill_provider" instance="legacy" package="armar6_skills" nodeName="" enabled="false"/>
<application name="control_skill_provider" instance="" package="armarx_control" nodeName="" enabled="true"/>
--
GitLab
From aa92786101c77bc4b3e4bad65572d8fd29bf85c0 Mon Sep 17 00:00:00 2001
From: Tilman Daab <tilman.daab@kit.edu>
Date: Tue, 25 Feb 2025 16:56:45 +0100
Subject: [PATCH 22/22] add missing property
---
.../config/placement_around_box.cfg | 7 +++++++
1 file changed, 7 insertions(+)
diff --git a/scenarios/Armar6MobileManipulation/config/placement_around_box.cfg b/scenarios/Armar6MobileManipulation/config/placement_around_box.cfg
index 87338e0c5..b8ae25065 100644
--- a/scenarios/Armar6MobileManipulation/config/placement_around_box.cfg
+++ b/scenarios/Armar6MobileManipulation/config/placement_around_box.cfg
@@ -160,6 +160,13 @@
# ArmarX.PlacementAroundBox.p.debugging.visualizeForAllObjects = false
+# ArmarX.PlacementAroundBox.p.robotName: Name of the robot to use for collision checking
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+ArmarX.PlacementAroundBox.p.robotName = Armar6
+
+
# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
# Attributes:
# - Default: true
--
GitLab