diff --git a/source/RobotAPI/components/ArViz/CMakeLists.txt b/source/RobotAPI/components/ArViz/CMakeLists.txt index 696d953ae61d0d27f4894265b3a963a89298f1ac..7e1ec4053f83a4d33bb323bd129ff87f0d7b703a 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 ef4c6f66a502b9c844551636431fc0d1a6ec52e0..a976534bfd392f4579d137b9ecdcd776ec738622 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 f6f45c63bcc815897b95652d40c254c4c3184563..b77725be1ca0ee3dafdc24a5e1513275aebe796b 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 b19d92de2110acadd10d5b57b577e40749a63687..1c90e0209498ae0d6c37d80a19905df32f7565d9 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 0000000000000000000000000000000000000000..b2699e8ea2a0602eafd79f78e91d160a15debb45 --- /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 0000000000000000000000000000000000000000..8a25be6e2695ddbce0f3571660a7530dca1d674c --- /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; + + }; + +}