From 6bf6fd7e3c4bd8a4cea9e3e5a7e3fc178d3072f0 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Mon, 21 Sep 2020 13:03:51 +0200
Subject: [PATCH] Add RobotHand
---
.../RobotAPI/components/ArViz/CMakeLists.txt | 3 ++
.../components/ArViz/Client/Elements.h | 1 +
.../ArViz/Client/elements/Robot.cpp | 4 +-
.../components/ArViz/Client/elements/Robot.h | 11 +----
.../ArViz/Client/elements/RobotHand.cpp | 46 ++++++++++++++++++
.../ArViz/Client/elements/RobotHand.h | 48 +++++++++++++++++++
6 files changed, 102 insertions(+), 11 deletions(-)
create mode 100644 source/RobotAPI/components/ArViz/Client/elements/RobotHand.cpp
create mode 100644 source/RobotAPI/components/ArViz/Client/elements/RobotHand.h
diff --git a/source/RobotAPI/components/ArViz/CMakeLists.txt b/source/RobotAPI/components/ArViz/CMakeLists.txt
index 696d953ae..7e1ec4053 100644
--- a/source/RobotAPI/components/ArViz/CMakeLists.txt
+++ b/source/RobotAPI/components/ArViz/CMakeLists.txt
@@ -5,6 +5,7 @@ set(COMPONENT_LIBS
RobotAPICore
RobotAPIInterfaces
RobotAPIArmarXObjects
+ RobotStatechartHelpers # For RobotNameHelper, used by RobotHand
boost_iostreams #compression
)
@@ -13,6 +14,7 @@ set(SOURCES
Client/Elements.cpp
Client/elements/Mesh.cpp
Client/elements/Robot.cpp
+ Client/elements/RobotHand.cpp
Coin/ElementVisualizer.cpp
@@ -71,6 +73,7 @@ set(HEADERS
Client/elements/MeshCGALExtensions.h
Client/elements/PointCloud.h
Client/elements/Robot.h
+ Client/elements/RobotHand.h
Client/elements/point_cloud_type_traits.hpp
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.h b/source/RobotAPI/components/ArViz/Client/Elements.h
index ef4c6f66a..a976534bf 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.h
+++ b/source/RobotAPI/components/ArViz/Client/Elements.h
@@ -20,6 +20,7 @@
#include "elements/Mesh.h"
#include "elements/PointCloud.h"
#include "elements/Robot.h"
+//#include "elements/RobotHand.h" // Not included by default (exposes additional headers).
// The has_member macro causes compile errors if *any* other header uses
// the identifier has_member. Boost.Thread does, so this causes compile
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Robot.cpp b/source/RobotAPI/components/ArViz/Client/elements/Robot.cpp
index f6f45c63b..b77725be1 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Robot.cpp
+++ b/source/RobotAPI/components/ArViz/Client/elements/Robot.cpp
@@ -1,6 +1,8 @@
#include "Robot.h"
-Robot::Robot()
+
+namespace armarx::viz
{
+
}
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Robot.h b/source/RobotAPI/components/ArViz/Client/elements/Robot.h
index b19d92de2..1c90e0209 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Robot.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/Robot.h
@@ -3,16 +3,6 @@
#include "ElementOps.h"
-namespace armarx
-{
- ///#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
- class RobotNameHelper;
- ///#include <RobotAPI/interface/core/RobotState.h>
- class RobotInfoNode;
- class RobotStateComponentInterfacePrx;
-}
-
-
namespace armarx::viz
{
@@ -70,6 +60,7 @@ namespace armarx::viz
return *this;
}
+
};
}
diff --git a/source/RobotAPI/components/ArViz/Client/elements/RobotHand.cpp b/source/RobotAPI/components/ArViz/Client/elements/RobotHand.cpp
new file mode 100644
index 000000000..b2699e8ea
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/elements/RobotHand.cpp
@@ -0,0 +1,46 @@
+#include "RobotHand.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+namespace armarx::viz
+{
+
+ RobotHand& RobotHand::fileBySide(const std::string& side, RobotStateComponentInterfacePrx robotStateComponent)
+ {
+ ARMARX_CHECK_NOT_NULL(robotStateComponent);
+ return this->fileBySide(side, robotStateComponent->getRobotInfo());
+ }
+
+ RobotHand& RobotHand::fileBySide(const std::string& side, RobotInfoNodePtr robotInfo)
+ {
+ ARMARX_CHECK_NOT_NULL(robotInfo);
+ RobotNameHelper nh(robotInfo, nullptr);
+ return this->fileBySide(side, nh);
+ }
+
+ RobotHand& RobotHand::fileBySide(const std::string& side, const RobotNameHelper& nameHelper)
+ {
+ RobotNameHelper::Arm arm = nameHelper.getArm(side);
+ return this->fileBySide(arm);
+ }
+
+ RobotHand& RobotHand::fileBySide(const RobotNameHelper::Arm& arm)
+ {
+ this->arm = arm;
+ this->file(arm.getHandModelPackage(), arm.getHandModelPath());
+ return *this;
+ }
+
+ RobotHand& RobotHand::tcpPose(const Eigen::Matrix4f& tcpPose, VirtualRobot::RobotPtr robot)
+ {
+ ARMARX_CHECK(arm) << "Set RobotHand::side() before setting the TCP pose.";
+ RobotNameHelper::RobotArm robotArm = arm->addRobot(robot);
+ this->pose(tcpPose * robotArm.getTcp2HandRootTransform());
+ return *this;
+ }
+}
+
+
+
+
diff --git a/source/RobotAPI/components/ArViz/Client/elements/RobotHand.h b/source/RobotAPI/components/ArViz/Client/elements/RobotHand.h
new file mode 100644
index 000000000..8a25be6e2
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/elements/RobotHand.h
@@ -0,0 +1,48 @@
+#pragma once
+
+#include <optional>
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
+
+#include "Robot.h"
+
+
+namespace armarx::viz
+{
+
+ /**
+ * @brief Left or right hand of a robot.
+ */
+ class RobotHand : virtual Robot
+ {
+ public:
+
+ using Robot::Robot;
+
+ /**
+ * @brief Set the robot file according the desired side.
+ * @param robotStateComponent The robot state component from which to get the robot info.
+ */
+ RobotHand& fileBySide(const std::string& side, RobotStateComponentInterfacePrx robotStateComponent);
+ RobotHand& fileBySide(const std::string& side, RobotInfoNodePtr robotInfo);
+ RobotHand& fileBySide(const std::string& side, const RobotNameHelper& nameHelper);
+ RobotHand& fileBySide(const RobotNameHelper::Arm& arm);
+
+
+ /**
+ * @brief Set the pose of `robotViz` according to the given TCP pose.
+ *
+ * You must specify the side beforehand using `fileBySide()`.
+ */
+ RobotHand& tcpPose(const Eigen::Matrix4f& tcpPose, VirtualRobot::RobotPtr robot);
+
+
+ /// The arm name helper. Set by `fileBySide()`.
+ std::optional<RobotNameHelper::Arm> arm;
+
+ };
+
+}
--
GitLab