From 833fed3065017a099c98c9c3fb8397f339bb8de3 Mon Sep 17 00:00:00 2001
From: Your Name <you@example.com>
Date: Fri, 10 Feb 2023 13:34:08 +0100
Subject: [PATCH] SimpleVirtualRobot component: can now specify global pose
---
.../armem/client/SimpleVirtualRobot/SimpleVirtualRobot.cpp | 7 ++++++-
.../armem/client/SimpleVirtualRobot/SimpleVirtualRobot.h | 4 ++++
2 files changed, 10 insertions(+), 1 deletion(-)
diff --git a/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.cpp b/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.cpp
index 1cd517681..df79259c8 100644
--- a/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.cpp
+++ b/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.cpp
@@ -56,6 +56,10 @@ namespace armarx::simple_virtual_robot
defs->optional(properties.robot.jointValues, "p.robot.jointValues", "Specify a certain joint configuration.");
+ defs->optional(properties.robot.globalPositionX, "p.robot.globalPositionX", "");
+ defs->optional(properties.robot.globalPositionY, "p.robot.globalPositionY", "");
+ defs->optional(properties.robot.globalPositionYaw, "p.robot.globalPositionYaw", "");
+
return defs;
}
@@ -125,7 +129,8 @@ namespace armarx::simple_virtual_robot
robot->setJointValues(jointValues);
}
-
+ const Eigen::Isometry3f global_T_robot = Eigen::Translation3f{properties.robot.globalPositionX, properties.robot.globalPositionY, 0} * Eigen::AngleAxisf{properties.robot.globalPositionYaw, Eigen::Vector3f::UnitZ()};
+ robot->setGlobalPose(global_T_robot.matrix());
if (not p.name.empty())
{
diff --git a/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.h b/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.h
index 677f4e085..d960e0ff3 100644
--- a/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.h
+++ b/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.h
@@ -62,6 +62,10 @@ namespace armarx::simple_virtual_robot
std::string path;
std::string jointValues; // json-style map<std::string, float>
+
+ float globalPositionX = 0;
+ float globalPositionY = 0;
+ float globalPositionYaw = 0;
};
Robot robot;
};
--
GitLab