diff --git a/scenarios/IkDemo/IkDemo.scx b/scenarios/IkDemo/IkDemo.scx
new file mode 100644
index 0000000000000000000000000000000000000000..a6ae92de68fb42a32bae8a0e2ecd4f8c1785a067
--- /dev/null
+++ b/scenarios/IkDemo/IkDemo.scx
@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="IkDemo" creation="2022-03-01.11:51:52" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="ik_demo" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/scenarios/IkDemo/config/global.cfg b/scenarios/IkDemo/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..87eb82db50df2df5b80b0dab8ee03aa81318c849
--- /dev/null
+++ b/scenarios/IkDemo/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario IkDemo
+# ==================================================================
+
diff --git a/scenarios/IkDemo/config/ik_demo.cfg b/scenarios/IkDemo/config/ik_demo.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..47f70e0264c4550308bb307c41cebc709d03b6d7
--- /dev/null
+++ b/scenarios/IkDemo/config/ik_demo.cfg
@@ -0,0 +1,222 @@
+# ==================================================================
+# ik_demo 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.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.IkDemo.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.IkDemo.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.IkDemo.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.IkDemo.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.IkDemo.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.IkDemo.EnableProfiling = false
+
+
+# ArmarX.IkDemo.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.IkDemo.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.IkDemo.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.IkDemo.ObjectName = ""
+
+
+# ArmarX.IkDemo.p.robotFile: The ArmarX data path to the robot XML file.
+# For ARMAR-III: 'RobotAPI/robots/Armar3/ArmarIII.xml'
+# For ARMAR-6: 'armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml'
+# Attributes:
+# - Default: armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.IkDemo.p.robotFile = armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml
+
+
+# ArmarX.IkDemo.p.robotNodeSetNames: Names of robot node sets for TCPs.
+# Attributes:
+# - Default: LeftArm; RightArm
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.IkDemo.p.robotNodeSetNames = LeftArm; RightArm
+
+
+# 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
+
+
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index 4a6fbed267afc454f8d65c305270ea7ba0105f05..7debb7e6f4aa280c4e28cc1a40bbc59af0bf25c3 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -3,6 +3,7 @@ add_subdirectory(units)
add_subdirectory(armem)
add_subdirectory(skills)
+add_subdirectory(ArticulatedObjectLocalizerExample)
add_subdirectory(ArViz)
add_subdirectory(CyberGloveObserver)
@@ -17,6 +18,7 @@ add_subdirectory(EarlyVisionGraph)
# add_subdirectory(FrameTracking)
add_subdirectory(GamepadControlUnit)
+add_subdirectory(ik_demo)
add_subdirectory(KITHandUnit)
add_subdirectory(KITProstheticHandUnit)
add_subdirectory(MultiHandUnit)
@@ -30,5 +32,3 @@ add_subdirectory(RobotToArViz)
add_subdirectory(StatechartExecutorExample)
add_subdirectory(TopicTimingTest)
add_subdirectory(ViewSelection)
-
-add_subdirectory(ArticulatedObjectLocalizerExample)
diff --git a/source/RobotAPI/components/ik_demo/CMakeLists.txt b/source/RobotAPI/components/ik_demo/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..68bdb926aa9da39ecd30029a936067a717e781df
--- /dev/null
+++ b/source/RobotAPI/components/ik_demo/CMakeLists.txt
@@ -0,0 +1,72 @@
+armarx_component_set_name(ik_demo)
+
+set(COMPONENT_LIBS
+ # DecoupledSingleComponent
+ ArViz
+ RobotAPIComponentPlugins
+ diffik
+)
+
+set(SOURCES
+ ik_demo.cpp
+ IkDemo.cpp
+ PoseGizmo.cpp
+)
+set(HEADERS
+ ik_demo.h
+ IkDemo.h
+ PoseGizmo.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+
+# add unit tests
+# add_subdirectory(test)
+
+# generate the application
+armarx_generate_and_add_component_executable(
+ COMPONENT_NAMESPACE ::armar6::skills::components::armar6_ik_demo
+)
+
+
+
+if (false) # Modern
+ armarx_add_component(ik_demo
+ ICE_FILES
+ ComponentInterface.ice
+
+ ICE_DEPENDENCIES
+ ArmarXCoreInterfaces
+ RobotAPIInterfaces
+
+ SOURCES
+ ik_demo.cpp
+ IkDemo.cpp
+ PoseGizmo.cpp
+
+ HEADERS
+ ik_demo.h
+ IkDemo.h
+ PoseGizmo.h
+
+ DEPENDENCIES
+ # ArmarXCore
+ ArmarXCore
+ ## ArmarXCoreComponentPlugins # For DebugObserver plugin.
+ # ArmarXGui
+ ## ArmarXGuiComponentPlugins # For RemoteGui plugin.
+ # RobotAPI
+ ## RobotAPICore
+ # RobotAPIInterfaces
+ RobotAPIComponentPlugins # For ArViz and other plugins.
+
+ diffik
+
+ # DEPENDENCIES_LEGACY
+ ## Add libraries that do not provide any targets but ${FOO_*} variables.
+ # FOO
+ # If you need a separate shared component library you can enable it with the following flag.
+ # SHARED_COMPONENT_LIBRARY
+ )
+endif()
diff --git a/source/RobotAPI/components/ik_demo/ComponentInterface.ice b/source/RobotAPI/components/ik_demo/ComponentInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..edde6d507af83320f080fb20c61856a4036747b3
--- /dev/null
+++ b/source/RobotAPI/components/ik_demo/ComponentInterface.ice
@@ -0,0 +1,35 @@
+/*
+ * 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::Armar6IkDemo
+ * author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * date 2022
+ * copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+
+module armar6 { module skills { module components { module armar6_ik_demo
+{
+
+ interface ComponentInterface
+ {
+ // Define your interface here.
+ };
+
+};};};};
diff --git a/source/RobotAPI/components/ik_demo/IkDemo.cpp b/source/RobotAPI/components/ik_demo/IkDemo.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2d60ef2feeae53056890925f44a799a7ea8f5b10
--- /dev/null
+++ b/source/RobotAPI/components/ik_demo/IkDemo.cpp
@@ -0,0 +1,436 @@
+#include "IkDemo.h"
+
+#include <SimoxUtility/algorithm/string.h>
+#include <SimoxUtility/meta/EnumNames.hpp>
+#include <SimoxUtility/math/pose/invert.h>
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.h>
+#include <VirtualRobot/IK/CompositeDiffIK/ManipulabilityNullspaceGradient.h>
+#include <VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.h>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
+
+#include "PoseGizmo.h"
+
+
+namespace viz = armarx::viz;
+
+
+namespace armar6::skills::components::armar6_ik_demo
+{
+
+ enum class IkMethod
+ {
+ SimpleDiffIk,
+ CompositeDiffIk,
+ };
+ const simox::meta::EnumNames<IkMethod> IkMethodNames =
+ {
+ { IkMethod::SimpleDiffIk, "Simple Diff IK" },
+ { IkMethod::CompositeDiffIk, "Composite Diff IK" },
+ };
+
+ struct Manipulator
+ {
+ Manipulator()
+ {
+ gizmo.box.color(simox::Color::cyan(255, 64));
+ }
+ virtual ~Manipulator()
+ {
+ }
+
+ virtual bool handle(viz::InteractionFeedback const& interaction, viz::StagedCommit* stage)
+ {
+ bool updated = false;
+ if (interaction.layer() == gizmo.layer.data_.name)
+ {
+ updated |= gizmo.handleInteraction(interaction, stage);
+ }
+ return updated;
+ }
+
+ virtual void visualize(viz::Client& arviz) = 0;
+ virtual void runIk(IkDemo::Robot& robot) = 0;
+
+ viz::PoseGizmo gizmo;
+ };
+
+
+ struct TcpManipulator : public Manipulator
+ {
+ TcpManipulator()
+ {
+ std::vector<std::string> options;
+ for (IkMethod method : IkMethodNames.values())
+ {
+ options.push_back("Use " + IkMethodNames.to_name(method));
+ }
+ gizmo.box
+ .size({75, 100, 200})
+ .enable(viz::interaction().selection().transform().hideDuringTransform()
+ .contextMenu(options));
+ };
+
+ void visualize(viz::Client& arviz) override
+ {
+ gizmo.setLayer(arviz.layer(tcp->getName()));
+ gizmo.update();
+ }
+
+ bool handle(viz::InteractionFeedback const& interaction, viz::StagedCommit* stage) override
+ {
+ bool updated = Manipulator::handle(interaction, stage);
+
+ if (interaction.layer() == gizmo.layer.data_.name)
+ {
+ switch (interaction.type())
+ {
+ case viz::InteractionFeedbackType::ContextMenuChosen:
+ {
+ int i = 0;
+ for (IkMethod method : IkMethodNames.values())
+ {
+ if (i == interaction.chosenContextMenuEntry())
+ {
+ this->method = method;
+ updated |= true;
+ ARMARX_IMPORTANT << "[" << tcp->getName() << "] Using " << IkMethodNames.to_name(method);
+ break;
+ }
+ ++i;
+ }
+ } break;
+ default:
+ break;
+ }
+ }
+
+ return updated;
+ }
+
+ void runIk(IkDemo::Robot& robot) override
+ {
+ const Eigen::Matrix4f tcpPoseInRobotFrame =
+ simox::math::inverted_pose(robot.robot->getGlobalPose()) * gizmo.getCurrent();
+
+ switch (method)
+ {
+ case IkMethod::SimpleDiffIk:
+ {
+ armarx::SimpleDiffIK ik;
+ armarx::SimpleDiffIK::Result result = ik.CalculateDiffIK(tcpPoseInRobotFrame, nodeSet, tcp);
+
+ if (result.reached)
+ {
+ gizmo.box.color(simox::Color::kit_green(64));
+
+ std::map<std::string, float> jointValues;
+ size_t i = 0;
+ for (const auto& rn : nodeSet->getAllRobotNodes())
+ {
+ jointValues[rn->getName()] = result.jointValues(i);
+ ++i;
+ }
+
+ robot.robot->setJointValues(jointValues);
+ }
+ else
+ {
+ gizmo.box.color(simox::Color::red(255, 64));
+ }
+ } break;
+
+ case IkMethod::CompositeDiffIk:
+ {
+ // Code taken from: simox/VirtualRobot/examples/RobotViewer/DiffIKWidget.cpp
+
+ const float jointLimitAvoidance = 0;
+ const float kGainManipulabilityAsNullspace = 0.01;
+ const float kSoechtingAsNullspace = 0.0;
+ const int steps = 100;
+
+ VirtualRobot::CompositeDiffIK ik(nodeSet);
+ Eigen::Matrix4f pose = tcpPoseInRobotFrame;
+
+ const bool ori = true;
+ VirtualRobot::CompositeDiffIK::TargetPtr target1 = ik.addTarget(
+ tcp, pose, ori ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position);
+
+
+ if (jointLimitAvoidance > 0)
+ {
+ VirtualRobot::CompositeDiffIK::NullspaceJointLimitAvoidancePtr nsjla(
+ new VirtualRobot::CompositeDiffIK::NullspaceJointLimitAvoidance(nodeSet));
+ nsjla->kP = jointLimitAvoidance;
+ for (auto node : nodeSet->getAllRobotNodes())
+ {
+ if (node->isLimitless())
+ {
+ nsjla->setWeight(node->getName(), 0);
+ }
+ }
+ ik.addNullspaceGradient(nsjla);
+ }
+
+ VirtualRobot::NullspaceManipulabilityPtr nsman = nullptr;
+ if (kGainManipulabilityAsNullspace > 0)
+ {
+#if 0
+ std::cout << "Adding manipulability as nullspace target" << std::endl;
+ auto manipTracking = getManipulabilityTracking(nodeSet, nullptr);
+ if (!manipTracking)
+ {
+ std::cout << "Manip tracking zero!" << std::endl;
+ return;
+ }
+ Eigen::MatrixXd followManip = readFollowManipulability();
+ if (followManip.rows() != manipTracking->getTaskVars())
+ {
+ std::cout << "Wrong manipulability matrix!" << std::endl;
+ return;
+ }
+ nsman = VirtualRobot::NullspaceManipulabilityPtr(new VirtualRobot::NullspaceManipulability(manipTracking, followManip, Eigen::MatrixXd(), true));
+ nsman->kP = kGainManipulabilityAsNullspace;
+ ik.addNullspaceGradient(nsman);
+#endif
+ }
+
+ if (kSoechtingAsNullspace > 0)
+ {
+ if (robot.robot->getName() == "Armar6" and nodeSet->getName() == "RightArm")
+ {
+ std::cout << "Adding soechting nullspace" << std::endl;
+ VirtualRobot::SoechtingNullspaceGradient::ArmJoints armjoints;
+ armjoints.clavicula = robot.robot->getRobotNode("ArmR1_Cla1");
+ armjoints.shoulder1 = robot.robot->getRobotNode("ArmR2_Sho1");
+ armjoints.shoulder2 = robot.robot->getRobotNode("ArmR3_Sho2");
+ armjoints.shoulder3 = robot.robot->getRobotNode("ArmR4_Sho3");
+ armjoints.elbow = robot.robot->getRobotNode("ArmR5_Elb1");
+
+ auto gradient = std::make_shared<VirtualRobot::SoechtingNullspaceGradient>(
+ target1, "ArmR2_Sho1", VirtualRobot::Soechting::ArmType::Right, armjoints);
+
+ gradient->kP = kSoechtingAsNullspace;
+ ik.addNullspaceGradient(gradient);
+ }
+ else if (robot.robot->getName() == "Armar6" and nodeSet->getName() == "LeftArm")
+ {
+ std::cout << "Adding soechting nullspace" << std::endl;
+ VirtualRobot::SoechtingNullspaceGradient::ArmJoints armjoints;
+ armjoints.clavicula = robot.robot->getRobotNode("ArmL1_Cla1");
+ armjoints.shoulder1 = robot.robot->getRobotNode("ArmL2_Sho1");
+ armjoints.shoulder2 = robot.robot->getRobotNode("ArmL3_Sho2");
+ armjoints.shoulder3 = robot.robot->getRobotNode("ArmL4_Sho3");
+ armjoints.elbow = robot.robot->getRobotNode("ArmL5_Elb1");
+
+ auto gradient = std::make_shared<VirtualRobot::SoechtingNullspaceGradient>(
+ target1, "ArmL2_Sho1", VirtualRobot::Soechting::ArmType::Left, armjoints);
+ gradient->kP = kSoechtingAsNullspace;
+ ik.addNullspaceGradient(gradient);
+ }
+ else
+ {
+ ARMARX_INFO << "Soechting currently supports only Armar6 and RightArm/LeftArm robot node set "
+ "for first robot node set for demonstration purposes.";
+ }
+ }
+
+ {
+ VirtualRobot::CompositeDiffIK::Parameters cp;
+ cp.resetRnsValues = false;
+ cp.returnIKSteps = true;
+ cp.steps = 1;
+ VirtualRobot::CompositeDiffIK::SolveState state;
+ ik.solve(cp, state);
+
+ int i = 0;
+ while (i < steps or (steps < 0 and not ik.getLastResult().reached and i < 1000))
+ {
+ ik.step(cp, state, i);
+ i++;
+ }
+
+ if (ik.getLastResult().reached)
+ {
+ gizmo.box.color(simox::Color::kit_green(64));
+ robot.robot->setJointValues(ik.getRobotNodeSet()->getJointValueMap());
+ }
+ else
+ {
+ gizmo.box.color(simox::Color::red(255, 64));
+ }
+ }
+ } break;
+ }
+ }
+
+
+ VirtualRobot::RobotNodeSetPtr nodeSet;
+ VirtualRobot::RobotNodePtr tcp;
+
+ IkMethod method = IkMethod::SimpleDiffIk;
+ };
+
+
+ struct PlatformManipulator : public Manipulator
+ {
+ PlatformManipulator()
+ {
+ gizmo.box.size({1000, 1000, 100});
+ }
+
+ void visualize(viz::Client& arviz) override
+ {
+ gizmo.setLayer(arviz.layer(root->getName()));
+ gizmo.update();
+ }
+ void runIk(IkDemo::Robot& robot) override
+ {
+ robot.robot->setGlobalPose(gizmo.getCurrent());
+ }
+
+ VirtualRobot::RobotNodePtr root;
+ };
+
+
+
+ IkDemo::IkDemo()
+ {
+ }
+
+ IkDemo::~IkDemo()
+ {
+ }
+
+
+ IkDemo::Params::Params() :
+ robotFile("armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml"),
+ robotNodeSetNamesStr(simox::alg::join({"LeftArm", "RightArm"}, "; "))
+ {
+ }
+
+
+ std::vector<std::string> IkDemo::Params::robotNodeSetNames() const
+ {
+ bool trim = true;
+ return simox::alg::split(robotNodeSetNamesStr, ";", trim);
+ }
+
+
+ void IkDemo::defineProperties(armarx::PropertyDefinitionContainer& defs)
+ {
+ defs.optional(params.robotFile, "p.robotFile",
+ "The ArmarX data path to the robot XML file."
+ "\n For ARMAR-III: 'RobotAPI/robots/Armar3/ArmarIII.xml'"
+ "\n For ARMAR-6: 'armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml'"
+ );
+ defs.optional(params.robotNodeSetNamesStr, "p.robotNodeSetNames",
+ "Names of robot node sets for TCPs.");
+ }
+
+
+ void IkDemo::start()
+ {
+ this->stage.reset();
+
+ {
+ params.robotFile = armarx::ArmarXDataPath::resolvePath(params.robotFile);
+ robot.robot = VirtualRobot::RobotIO::loadRobot(params.robotFile, VirtualRobot::RobotIO::eStructure);
+
+ robot.layer = remote.arviz->layer("Robot");
+
+ robot.visu.file("", robot.robot->getFilename()).joints(robot.robot->getJointValues());
+ robot.layer.add(robot.visu);
+ stage.add(robot.layer);
+ }
+
+ {
+ auto root = robot.robot->getRootNode();
+
+ std::unique_ptr<PlatformManipulator> manip = std::make_unique<PlatformManipulator>();
+
+ manip->gizmo.initial = root->getGlobalPose();
+ manip->root = root;
+
+ manipulators.emplace_back(std::move(manip));
+ }
+
+ for (const std::string& rnsName : params.robotNodeSetNames())
+ {
+ ARMARX_CHECK(robot.robot->hasRobotNodeSet(rnsName)) << VAROUT(rnsName) << " must exist.";
+
+ const auto& rns = robot.robot->getRobotNodeSet(rnsName);
+ const auto tcp = rns->getTCP();
+ ARMARX_CHECK_NOT_NULL(tcp) << VAROUT(rnsName) << " must have a TCP.";
+
+ std::unique_ptr<TcpManipulator> manip = std::make_unique<TcpManipulator>();
+
+ manip->gizmo.initial = tcp->getGlobalPose();
+ manip->tcp = tcp;
+ manip->nodeSet = rns;
+
+ manipulators.emplace_back(std::move(manip));
+ }
+
+ for (auto& manip : manipulators)
+ {
+ manip->visualize(remote.arviz.value());
+ stage.add(manip->gizmo.layer);
+ }
+
+ runIk();
+
+ viz::CommitResult result = remote.arviz->commit(stage);
+ ARMARX_VERBOSE << "Initial commit at revision: " << result.revision();
+ }
+
+
+ void IkDemo::update()
+ {
+ viz::CommitResult result = remote.arviz->commit(stage);
+
+ // Reset the stage, so that it can be rebuild during the interaction handling
+ stage.reset();
+
+ for (auto& manip : manipulators)
+ {
+ stage.requestInteraction(manip->gizmo.layer);
+ }
+
+ viz::InteractionFeedbackRange interactions = result.interactions();
+
+ bool updated = false;
+ for (viz::InteractionFeedback const& inter : interactions)
+ {
+ for (auto& manip : manipulators)
+ {
+ updated |= manip->handle(inter, &stage);
+ }
+ }
+ if (updated)
+ {
+ runIk();
+ }
+ }
+
+
+ void IkDemo::runIk()
+ {
+ for (auto& manip : manipulators)
+ {
+ manip->runIk(robot);
+ }
+
+ robot.visu.pose(robot.robot->getGlobalPose());
+ robot.visu.joints(robot.robot->getJointValues());
+ stage.add(robot.layer);
+ }
+
+}
diff --git a/source/RobotAPI/components/ik_demo/IkDemo.h b/source/RobotAPI/components/ik_demo/IkDemo.h
new file mode 100644
index 0000000000000000000000000000000000000000..b48b040efe2ee6f10272edea446701c7695c14d0
--- /dev/null
+++ b/source/RobotAPI/components/ik_demo/IkDemo.h
@@ -0,0 +1,68 @@
+#pragma once
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <ArmarXCore/core/application/properties/forward_declarations.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+
+namespace armar6::skills::components::armar6_ik_demo
+{
+
+ struct Manipulator;
+
+
+ class IkDemo
+ {
+ public:
+
+ struct Params
+ {
+ Params();
+
+ std::string robotFile;
+ std::string robotNodeSetNamesStr;
+ std::vector<std::string> robotNodeSetNames() const;
+ };
+ struct Remote
+ {
+ std::optional<armarx::viz::Client> arviz;
+ };
+ struct Robot
+ {
+ VirtualRobot::RobotPtr robot;
+
+ armarx::viz::Layer layer;
+ armarx::viz::Robot visu { "robot" };
+ };
+
+
+ public:
+
+ IkDemo();
+ ~IkDemo();
+
+ void defineProperties(armarx::PropertyDefinitionContainer& defs);
+
+ void connect(Remote&& remote);
+
+ void start();
+ void update();
+
+ void runIk();
+
+
+ public:
+
+ Remote remote;
+ Params params;
+ Robot robot;
+
+ armarx::viz::StagedCommit stage;
+
+ std::vector<std::unique_ptr<Manipulator>> manipulators;
+
+ };
+
+}
diff --git a/source/RobotAPI/components/ik_demo/PoseGizmo.cpp b/source/RobotAPI/components/ik_demo/PoseGizmo.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..04153d3f8e8c2b412b948f3037c002b3d78e52ab
--- /dev/null
+++ b/source/RobotAPI/components/ik_demo/PoseGizmo.cpp
@@ -0,0 +1,102 @@
+#include "PoseGizmo.h"
+
+
+namespace viz = armarx::viz;
+
+
+namespace armarx::viz
+{
+
+ PoseGizmo::PoseGizmo()
+ {
+ box.enable(viz::interaction().selection().transform().hideDuringTransform());
+ }
+
+ void PoseGizmo::setLayer(const Layer& layer_)
+ {
+ this->layer = layer_;
+ this->layer.add(box);
+ this->layer.add(pose);
+ }
+
+ void PoseGizmo::update()
+ {
+ const Eigen::Matrix4f current = getCurrent();
+ box.pose(current);
+ pose.pose(current);
+ }
+
+ void PoseGizmo::updateDuringTransform()
+ {
+ const Eigen::Matrix4f current = getCurrent();
+ box.pose(initial);
+ pose.pose(current);
+ }
+
+ bool
+ PoseGizmo::handleInteraction(
+ const InteractionFeedback& interaction,
+ StagedCommit* stage)
+ {
+ if (interaction.element() != box.data_->id)
+ {
+ return false;
+ }
+
+ switch (interaction.type())
+ {
+ case viz::InteractionFeedbackType::Select:
+ {
+ // Nothing to do.
+ } break;
+
+ case viz::InteractionFeedbackType::Transform:
+ {
+ // Update state of tcp object
+ transform = interaction.transformation();
+
+ if (interaction.isTransformDuring())
+ {
+ updateDuringTransform();
+ }
+
+ stage->add(layer);
+ return true;
+ } break;
+
+ case viz::InteractionFeedbackType::Deselect:
+ {
+ // If an object is deselected, we apply the transformation
+
+ initial = getCurrent();
+ transform.setIdentity();
+
+ update();
+ stage->add(layer);
+
+ return true;
+ } break;
+
+ case viz::InteractionFeedbackType::ContextMenuChosen:
+ {
+ }
+
+ default:
+ {
+ // Ignore other interaction types
+ } break;
+
+ }
+
+ return false;
+ }
+
+
+ Eigen::Matrix4f PoseGizmo::getCurrent() const
+ {
+ return transform * initial;
+ }
+
+}
+
+
diff --git a/source/RobotAPI/components/ik_demo/PoseGizmo.h b/source/RobotAPI/components/ik_demo/PoseGizmo.h
new file mode 100644
index 0000000000000000000000000000000000000000..482eea5aa420ee1d302f7c2d5f6f94bfd944b1dc
--- /dev/null
+++ b/source/RobotAPI/components/ik_demo/PoseGizmo.h
@@ -0,0 +1,40 @@
+#pragma once
+
+#include <RobotAPI/components/ArViz/Client/Elements.h>
+#include <RobotAPI/components/ArViz/Client/Layer.h>
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+
+namespace armarx::viz
+{
+
+ class PoseGizmo
+ {
+ public:
+
+ PoseGizmo();
+
+ void setLayer(const viz::Layer& layer);
+
+ void update();
+ void updateDuringTransform();
+
+ bool handleInteraction(const viz::InteractionFeedback& interaction,
+ viz::StagedCommit* stage);
+
+ Eigen::Matrix4f getCurrent() const;
+
+
+ public:
+
+ Eigen::Matrix4f initial = Eigen::Matrix4f::Identity();
+ Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
+
+ viz::Layer layer;
+
+ viz::Box box { "box" };
+ viz::Pose pose { "pose" };
+
+ };
+
+}
diff --git a/source/RobotAPI/components/ik_demo/ik_demo.cpp b/source/RobotAPI/components/ik_demo/ik_demo.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..73b838422a9f579a290dcce2814e1b5c236173b6
--- /dev/null
+++ b/source/RobotAPI/components/ik_demo/ik_demo.cpp
@@ -0,0 +1,227 @@
+/**
+ * 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::ArmarXObjects::armar6_ik_demo
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#include "ik_demo.h"
+
+#include <Eigen/Core>
+
+#include <SimoxUtility/color/Color.h>
+
+// #include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+#include "IkDemo.h"
+
+
+namespace viz = armarx::viz;
+
+
+namespace armar6::skills::components::armar6_ik_demo
+{
+
+ const std::string
+ ik_demo::defaultName = "IkDemo";
+
+
+ ik_demo::ik_demo() : impl(new IkDemo)
+ {
+ }
+
+
+ armarx::PropertyDefinitionsPtr
+ ik_demo::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr def = new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
+
+ // Publish to a topic (passing the TopicListenerPrx).
+ // def->topic(myTopicListener);
+
+ // Subscribe to a topic (passing the topic name).
+ // def->topic<PlatformUnitListener>("MyTopic");
+
+ // Use (and depend on) another component (passing the ComponentInterfacePrx).
+ // def->component(myComponentProxy)
+
+ impl->defineProperties(*def);
+
+
+ // Add a required property. (The component won't start without a value being set.)
+ // def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
+
+ // Add an optionalproperty.
+ // def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
+
+ return def;
+ }
+
+
+ void
+ ik_demo::onInitComponent()
+ {
+ // Topics and properties defined above are automagically registered.
+
+ // Keep debug observer data until calling `sendDebugObserverBatch()`.
+ // (Requies the armarx::DebugObserverComponentPluginUser.)
+ // setDebugObserverBatchModeEnabled(true);
+ }
+
+
+ void
+ ik_demo::onConnectComponent()
+ {
+ impl->remote.arviz = arviz;
+
+ task = new armarx::SimpleRunningTask<>([this]() { this->run(); });
+ task->start();
+
+ /* (Requies the armarx::DebugObserverComponentPluginUser.)
+ // Use the debug observer to log data over time.
+ // The data can be viewed in the ObserverView and the LivePlotter.
+ // (Before starting any threads, we don't need to lock mutexes.)
+ {
+ setDebugObserverDatafield("numBoxes", properties.numBoxes);
+ setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
+ sendDebugObserverBatch();
+ }
+ */
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ // Setup the remote GUI.
+ {
+ createRemoteGuiTab();
+ RemoteGui_startRunningTask();
+ }
+ */
+ }
+
+
+ void
+ ik_demo::onDisconnectComponent()
+ {
+ const bool join = true;
+ task->stop(join);
+ task = nullptr;
+ }
+
+
+ void
+ ik_demo::onExitComponent()
+ {
+ }
+
+
+ void ik_demo::run()
+ {
+ impl->start();
+
+ armarx::CycleUtil cycle(10.0f);
+ while (!task->isStopped())
+ {
+ impl->update();
+
+ cycle.waitForCycleDuration();
+ }
+ }
+
+
+ std::string
+ ik_demo::getDefaultName() const
+ {
+ return ik_demo::defaultName;
+ }
+
+
+ std::string
+ ik_demo::GetDefaultName()
+ {
+ return ik_demo::defaultName;
+ }
+
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ void
+ Component::createRemoteGuiTab()
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ // Setup the widgets.
+
+ tab.boxLayerName.setValue(properties.boxLayerName);
+
+ tab.numBoxes.setValue(properties.numBoxes);
+ tab.numBoxes.setRange(0, 100);
+
+ tab.drawBoxes.setLabel("Draw Boxes");
+
+ // Setup the layout.
+
+ GridLayout grid;
+ int row = 0;
+ {
+ grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
+ ++row;
+
+ grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
+ ++row;
+
+ grid.add(tab.drawBoxes, {row, 0}, {2, 1});
+ ++row;
+ }
+
+ VBoxLayout root = {grid, VSpacer()};
+ RemoteGui_createTab(getName(), root, &tab);
+ }
+
+
+ void
+ Component::RemoteGui_update()
+ {
+ if (tab.boxLayerName.hasValueChanged() || tab.numBoxes.hasValueChanged())
+ {
+ std::scoped_lock lock(propertiesMutex);
+ properties.boxLayerName = tab.boxLayerName.getValue();
+ properties.numBoxes = tab.numBoxes.getValue();
+
+ {
+ setDebugObserverDatafield("numBoxes", properties.numBoxes);
+ setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
+ sendDebugObserverBatch();
+ }
+ }
+ if (tab.drawBoxes.wasClicked())
+ {
+ // Lock shared variables in methods running in seperate threads
+ // and pass them to functions. This way, the called functions do
+ // not need to think about locking.
+ std::scoped_lock lock(propertiesMutex, arvizMutex);
+ drawBoxes(properties, arviz);
+ }
+ }
+ */
+
+
+ // ARMARX_REGISTER_COMPONENT_EXECUTABLE(ik_demo, ik_demo::GetDefaultName());
+
+} // namespace armar6::skills::components::armar6_ik_demo
diff --git a/source/RobotAPI/components/ik_demo/ik_demo.h b/source/RobotAPI/components/ik_demo/ik_demo.h
new file mode 100644
index 0000000000000000000000000000000000000000..45eeb06a386e8dcf67fd2c11e337b24e858d68a1
--- /dev/null
+++ b/source/RobotAPI/components/ik_demo/ik_demo.h
@@ -0,0 +1,137 @@
+/**
+ * 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::ArmarXObjects::armar6_ik_demo
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+#include <memory>
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+
+// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+
+// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+// #include <armar6/skills/components/armar6_ik_demo/ComponentInterface.h>
+
+
+namespace armar6::skills::components::armar6_ik_demo
+{
+
+ class IkDemo;
+
+
+ class ik_demo :
+ virtual public armarx::Component
+ // , virtual public armar6::skills::components::armar6_ik_demo::ComponentInterface
+ // , virtual public armarx::DebugObserverComponentPluginUser
+ // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ , virtual public armarx::ArVizComponentPluginUser
+ {
+ public:
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+ /// Get the component's default name.
+ static std::string GetDefaultName();
+
+ ik_demo();
+
+
+ protected:
+
+ /// @see PropertyUser::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ /// @see armarx::ManagedIceObject::onInitComponent()
+ void onInitComponent() override;
+
+ /// @see armarx::ManagedIceObject::onConnectComponent()
+ void onConnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onDisconnectComponent()
+ void onDisconnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onExitComponent()
+ void onExitComponent() override;
+
+
+ /* (Requires armarx::LightweightRemoteGuiComponentPluginUser.)
+ /// This function should be called once in onConnect() or when you
+ /// need to re-create the Remote GUI tab.
+ void createRemoteGuiTab();
+
+ /// After calling `RemoteGui_startRunningTask`, this function is
+ /// called periodically in a separate thread. If you update variables,
+ /// make sure to synchronize access to them.
+ void RemoteGui_update() override;
+ */
+
+
+ private:
+
+ void run();
+
+ // Private methods go here.
+
+ // Forward declare `Properties` if you used it before its defined.
+ // struct Properties;
+
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ /// Draw some boxes in ArViz.
+ void drawBoxes(const Properties& p, viz::Client& arviz);
+ */
+
+
+ private:
+
+ static const std::string defaultName;
+
+ std::unique_ptr<IkDemo> impl;
+ armarx::SimpleRunningTask<>::pointer_type task;
+
+
+ /// Properties shown in the Scenario GUI.
+ struct Properties
+ {
+ };
+ Properties properties;
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ /// Tab shown in the Remote GUI.
+ struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
+ {
+ armarx::RemoteGui::Client::LineEdit boxLayerName;
+ armarx::RemoteGui::Client::IntSpinBox numBoxes;
+
+ armarx::RemoteGui::Client::Button drawBoxes;
+ };
+ RemoteGuiTab tab;
+ */
+
+ };
+
+} // namespace armar6::skills::components::armar6_ik_demo
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/Writer.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/Writer.cpp
index 4ed774fb7f798efb5c5f3bf16cadaae5be22eb52..117a6cbf164c65a0e141ff7fd49bf5a3a11a0c96 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/Writer.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/Writer.cpp
@@ -52,6 +52,17 @@ namespace armarx::aron::codegenerator::cpp
dataWriters.push_back(toAron);
}
+ {
+ codegenerator::WriterInfo toAronDTO;
+ toAronDTO.methodName = "toAronDTO";
+ toAronDTO.returnType = "armarx::aron::data::dto::DictPtr";
+ toAronDTO.writerClassType = "armarx::aron::data::writer::VariantWriter";
+ toAronDTO.include = "<RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h>";
+ toAronDTO.enforceConversion = "armarx::aron::data::Dict::DynamicCastAndCheck";
+ toAronDTO.enforceMemberAccess = "->toAronDictDTO()";
+ dataWriters.push_back(toAronDTO);
+ }
+
{
// The toAron Serializer is visible by default
codegenerator::WriterInfo ToAronType;
@@ -63,6 +74,18 @@ namespace armarx::aron::codegenerator::cpp
initialTypeWriters.push_back(ToAronType);
}
+ {
+ // The toAron Serializer is visible by default
+ codegenerator::WriterInfo ToAronTypeDTO;
+ ToAronTypeDTO.methodName = "ToAronTypeDTO";
+ ToAronTypeDTO.returnType = "armarx::aron::type::dto::AronObjectPtr";
+ ToAronTypeDTO.writerClassType = "armarx::aron::type::writer::VariantWriter";
+ ToAronTypeDTO.include = "<RobotAPI/libraries/aron/core/type/rw/writer/variant/VariantWriter.h>";
+ ToAronTypeDTO.enforceConversion = "armarx::aron::type::Object::DynamicCastAndCheck";
+ ToAronTypeDTO.enforceMemberAccess = "->toAronObjectDTO()";
+ initialTypeWriters.push_back(ToAronTypeDTO);
+ }
+
// toJSON Method
/*
{
@@ -87,6 +110,15 @@ namespace armarx::aron::codegenerator::cpp
staticDataReaders.push_back(fromAron);
}
+ {
+ codegenerator::StaticReaderInfo fromAronDTO;
+ fromAronDTO.methodName = "FromAron";
+ fromAronDTO.argumentType = "armarx::aron::data::dto::DictPtr";
+ fromAronDTO.returnType = OWN_TYPE_NAME;
+ staticDataReaders.push_back(fromAronDTO);
+ }
+
+
// The fromAron Deserializer is visible by default
{
codegenerator::ReaderInfo fromAron;
@@ -97,6 +129,16 @@ namespace armarx::aron::codegenerator::cpp
dataReaders.push_back(fromAron);
}
+ {
+ codegenerator::ReaderInfo fromAronDTO;
+ fromAronDTO.methodName = "fromAron";
+ fromAronDTO.argumentType = "armarx::aron::data::dto::DictPtr";
+ fromAronDTO.readerClassType = "armarx::aron::data::reader::VariantReader";
+ fromAronDTO.include = "<RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.h>";
+ fromAronDTO.enforceConversion = "std::make_shared<armarx::aron::data::Dict>";
+ dataReaders.push_back(fromAronDTO);
+ }
+
// fromJSON Method
/*
{
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.cpp
index ceb4481881b3f5b985529dddc84b7559ca8a28b9..8e437dd62935c3de5fadf1c93daf10d89270c813 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.cpp
@@ -264,7 +264,7 @@ namespace armarx::aron::codegenerator::cpp
CppMethodPtr m = CppMethodPtr(new CppMethod(info.returnType + " " + info.methodName + "() const", doc.str()));
m->addLine(info.writerClassType + " writer;");
- m->addLine("return " + info.enforceConversion + "(this->write(writer));");
+ m->addLine("return " + info.enforceConversion + "(this->write(writer))" + info.enforceMemberAccess + ";");
return m;
}
@@ -276,7 +276,7 @@ namespace armarx::aron::codegenerator::cpp
CppMethodPtr m = CppMethodPtr(new CppMethod("void " + info.methodName + "(const " + info.argumentType + "& input)", doc.str()));
m->addLine(info.readerClassType + " reader;");
- m->addLine("this->read(reader, " + info.enforceConversion + "(input));");
+ m->addLine("this->read(reader, " + info.enforceConversion + "(input)" + info.enforceMemberAccess + ");");
return m;
}
@@ -301,7 +301,7 @@ namespace armarx::aron::codegenerator::cpp
CppMethodPtr m = CppMethodPtr(new CppMethod("static " + info.returnType + " " + info.methodName + "()", doc.str()));
m->addLine(info.writerClassType + " writer;");
- m->addLine("return " + info.enforceConversion + "(writeType(writer));");
+ m->addLine("return " + info.enforceConversion + "(writeType(writer))" + info.enforceMemberAccess + ";");
return m;
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/helper/ReaderInfo.h b/source/RobotAPI/libraries/aron/core/codegenerator/helper/ReaderInfo.h
index a025da99a26a9538f53306ce9ba96d1fb8396ad5..3bda10de7b14849fe6e18da11f7201704c2f3670 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/helper/ReaderInfo.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/helper/ReaderInfo.h
@@ -36,6 +36,7 @@ namespace armarx::aron::codegenerator
std::string readerClassType;
std::string include;
std::string enforceConversion = "";
+ std::string enforceMemberAccess = "";
};
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/helper/WriterInfo.h b/source/RobotAPI/libraries/aron/core/codegenerator/helper/WriterInfo.h
index 33090bcbe0a07d6e9c8f5f124905e13424945ec3..1989ac5de9e93ee5ddda8ac66cfa1e65edeab98b 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/helper/WriterInfo.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/helper/WriterInfo.h
@@ -36,5 +36,6 @@ namespace armarx::aron::codegenerator
std::string writerClassType;
std::string include;
std::string enforceConversion = "";
+ std::string enforceMemberAccess = "";
};
}