diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index 011fbdf9812a63be65deb69e6f63a58df3807ff3..29237b27d212682c022ca4f0e2f8aa93c8b672c9 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -93,7 +93,6 @@
propertyIsOptional="true"
propertyDefaultValue="TCPControlUnitObserver" />
<Proxy include="RobotAPI/interface/units/HapticUnit.h"
- humanName="Haptic Unit Observer"
typeName="HapticUnitObserverInterfacePrx"
memberName="hapticObserver"
getterName="getHapticObserver"
@@ -101,7 +100,7 @@
propertyIsOptional="true"
propertyDefaultValue="HapticUnitObserver" />
<Proxy include="RobotAPI/interface/units/HandUnitInterface.h"
- humanName="Hand Unit "
+ humanName="Hand Unit"
typeName="HandUnitInterfacePrx"
memberName="handUnit"
getterName="getHandUnit"
@@ -109,7 +108,7 @@
propertyIsOptional="true"
propertyDefaultValue="HandUnit" />
<Proxy include="RobotAPI/interface/units/WeissHapticUnit.h"
- humanName="Weiss Haptic Unit"
+ humanName="Weiss Haptic Unit"
typeName="WeissHapticUnitInterfacePrx"
memberName="weissHapticUnit"
getterName="getWeissHapticUnit"
@@ -117,7 +116,7 @@
propertyIsOptional="true"
propertyDefaultValue="WeissHapticUnit" />
<Proxy include="RobotAPI/interface/units/HeadIKUnit.h"
- humanName="Head IK Unit"
+ humanName="Head IK Unit"
typeName="HeadIKUnitInterfacePrx"
memberName="headIKUnit"
getterName="getHeadIKUnit"
@@ -125,7 +124,7 @@
propertyIsOptional="true"
propertyDefaultValue="HeadIKUnit" />
<Proxy include="RobotAPI/interface/components/FrameTrackingInterface.h"
- humanName="Frame Tracking Component"
+ humanName="Frame Tracking Component"
typeName="FrameTrackingInterfacePrx"
memberName="frameTrackingComponent"
getterName="getFrameTrackingComponent"
@@ -288,5 +287,13 @@
propertyName="MultiHandUnitName"
propertyIsOptional="true"
propertyDefaultValue="MultiHandUnit" />
+ <Topic include="RobotAPI/interface/core/BlackWhitelist.h"
+ humanName="DebugDrawerToArViz Layer-BlackWhitelist"
+ typeName="BlackWhitelistTopicPrx"
+ memberName="debugDrawerToArVizLayerBlackWhitelist"
+ getterName="getDebugDrawerToArVizLayerBlackWhitelist"
+ propertyName="DebugDrawerToArVizLayerBlackWhitelistTopicName"
+ propertyIsOptional="true"
+ propertyDefaultValue="DebugDrawerToArVizLayerBlackWhitelistUpdates" />
</Lib>
</VariantInfo>
diff --git a/source/RobotAPI/applications/CMakeLists.txt b/source/RobotAPI/applications/CMakeLists.txt
index b8d05894adc9cf5f85a21b9369ba844277cbbc34..8af364a4a4213d9210698826a3540b553343260d 100644
--- a/source/RobotAPI/applications/CMakeLists.txt
+++ b/source/RobotAPI/applications/CMakeLists.txt
@@ -52,4 +52,4 @@ add_subdirectory(MultiHandUnit)
add_subdirectory(StatechartExecutorExample)
-add_subdirectory(NaturalIKTest)
\ No newline at end of file
+add_subdirectory(NaturalIKTest)
diff --git a/source/RobotAPI/components/ArViz/Client/Layer.h b/source/RobotAPI/components/ArViz/Client/Layer.h
index b7fda3dedc4d5bf4c32ffb78765cb3bd552c20b5..56056eabf3fbd031d0aeb0b25bee28956c85ffba 100644
--- a/source/RobotAPI/components/ArViz/Client/Layer.h
+++ b/source/RobotAPI/components/ArViz/Client/Layer.h
@@ -31,6 +31,15 @@ namespace armarx::viz
data_.elements.push_back(element.data_);
}
+ template <typename ElementT>
+ void add(std::vector<ElementT> const& elements)
+ {
+ for (const auto& e : elements)
+ {
+ add(e);
+ }
+ }
+
template<class...Ts>
std::enable_if_t < sizeof...(Ts) != 1 > add(Ts&& ...elems)
{
diff --git a/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp b/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp
index d377c8faafd1230fb84d59a929a9337a95fb54fb..219a5d5821f4a4748fccbd65b481d549affbddd9 100644
--- a/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp
+++ b/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp
@@ -1,5 +1,7 @@
#include "json_elements.h"
+#include <boost/algorithm/string.hpp>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include "json_base.h"
@@ -229,13 +231,44 @@ namespace armarx::viz
}
+ namespace data::ModelDrawStyle
+ {
+ std::string to_flag_names(const int drawStyle)
+ {
+ std::vector<std::string> flag_names;
+ for (int flag : names.values())
+ {
+ if (drawStyle == flag // Necessary if flag is 0
+ || drawStyle & flag)
+ {
+ flag_names.push_back(names.to_name(flag));
+ }
+ }
+ return boost::join(flag_names, " | ");
+ }
+
+ int from_flag_names(const std::string& flagString)
+ {
+ std::vector<std::string> split;
+ boost::split(split, flagString, boost::is_any_of("|"));
+ int flag = 0;
+ for (auto& s : split)
+ {
+ boost::algorithm::trim(s);
+ flag |= names.from_name(s);
+ }
+ return flag;
+ }
+ }
+
+
void data::to_json(nlohmann::json& j, const ElementObject& object)
{
json::to_json_base(j, object);
j["project"] = object.project;
j["filename"] = object.filename;
- j["drawStyle"] = ModelDrawStyle::names.to_name(object.drawStyle);
+ j["drawStyle"] = ModelDrawStyle::to_flag_names(object.drawStyle) + " (" + std::to_string(object.drawStyle) + ")";
j[json::meta::KEY]["drawStyle"] = json::meta::READ_ONLY;
}
void data::from_json(const nlohmann::json& j, ElementObject& object)
@@ -243,7 +276,7 @@ namespace armarx::viz
json::from_json_base(j, object);
object.project = j.at("project");
object.filename = j.at("filename");
- object.drawStyle = ModelDrawStyle::names.from_name(j.at("drawStyle"));
+ object.drawStyle = ModelDrawStyle::from_flag_names(j.at("drawStyle"));
}
@@ -254,7 +287,7 @@ namespace armarx::viz
j["filename"] = robot.filename;
j["jointValues"] = robot.jointValues;
- j["drawStyle"] = ModelDrawStyle::names.to_name(robot.drawStyle);
+ j["drawStyle"] = ModelDrawStyle::to_flag_names(robot.drawStyle);
j[json::meta::KEY]["drawStyle"] = json::meta::READ_ONLY;
}
void data::from_json(const nlohmann::json& j, ElementRobot& robot)
@@ -263,7 +296,7 @@ namespace armarx::viz
robot.project = j.at("project");
robot.filename = j.at("filename");
robot.jointValues = j.at("jointValues").get<armarx::StringFloatDictionary>();
- robot.drawStyle = ModelDrawStyle::names.from_name(j.at("drawStyle"));
+ robot.drawStyle = ModelDrawStyle::from_flag_names(j.at("drawStyle"));
}
}
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index 3cb8522a6f43aea2bd32e9d7a6f13a23cb93c01f..a001f08c8413d4cfd75914331cae73e584c79e25 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -3,6 +3,7 @@ add_subdirectory(units)
add_subdirectory(ArViz)
add_subdirectory(CyberGloveObserver)
add_subdirectory(DebugDrawer)
+add_subdirectory(DebugDrawerToArViz)
add_subdirectory(DSObstacleAvoidance)
add_subdirectory(DummyTextToSpeech)
add_subdirectory(EarlyVisionGraph)
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelist.cpp b/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelist.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..68c8929d5154bf46f117f2e01ea0c779d87d499b
--- /dev/null
+++ b/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelist.cpp
@@ -0,0 +1,7 @@
+#include "BlackWhitelist.h"
+
+
+namespace armarx
+{
+
+}
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelist.h b/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelist.h
new file mode 100644
index 0000000000000000000000000000000000000000..9b581a520d3c54ff6fee8d9427f6dfd321fd1725
--- /dev/null
+++ b/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelist.h
@@ -0,0 +1,80 @@
+#pragma once
+
+#include <set>
+#include <string>
+#include <ostream>
+
+
+namespace armarx
+{
+
+ /**
+ * @brief A combination of blacklist and whitelist.
+ *
+ * An element is included if
+ * (1) it is not in the blacklist, and
+ * (2) the whitelist is empty or it contains the element.
+ */
+ template <typename Key>
+ class BlackWhitelist
+ {
+ public:
+
+ /// Construct an empty blacklist and whitelist.
+ BlackWhitelist() = default;
+
+
+ /**
+ * An element is included if
+ * (1) it is not in the blacklist, and
+ * (2) the whitelist is empty or it contains the element.
+ */
+ bool isIncluded(const Key& element) const
+ {
+ return !isExcluded(element);
+ }
+
+ /**
+ * An element is excluded if
+ * (1) it is in the blacklist, or
+ * (2) it is not in the non-empty whitelist
+ */
+ bool isExcluded(const Key& element) const
+ {
+ return black.count(element) > 0 || (!white.empty() && white.count(element) == 0);
+ }
+
+
+ /// Elements in this list are always excluded.
+ std::set<Key> black;
+ /// If not empty, only these elements are included.
+ std::set<Key> white;
+
+ template <class K>
+ friend std::ostream& operator<< (std::ostream& os, const BlackWhitelist<K>& bw);
+ };
+
+
+ template <class K>
+ std::ostream& operator<< (std::ostream& os, const BlackWhitelist<K>& bw)
+ {
+ os << "Blacklist (" << bw.black.size() << "): ";
+ for (const auto& e : bw.black)
+ {
+ os << "\n\t" << e;
+ }
+ os << "\n";
+
+ os << "Whitelist (" << bw.white.size() << "):";
+ for (const auto& e : bw.white)
+ {
+ os << "\n\t" << e;
+ }
+ return os;
+ }
+
+
+ using StringBlackWhitelist = BlackWhitelist<std::string>;
+
+}
+
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelistUpdate.cpp b/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelistUpdate.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8d2039c24127ac25aa4b1a20d2d0c92552f145ca
--- /dev/null
+++ b/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelistUpdate.cpp
@@ -0,0 +1,18 @@
+#include "BlackWhitelistUpdate.h"
+
+
+void armarx::updateStringList(std::set<std::string>& list, const StringListUpdate& update)
+{
+ if (update.clear)
+ {
+ list.clear();
+ }
+ else if (!update.add.empty())
+ {
+ list.insert(update.add.begin(), update.add.end());
+ }
+ else if (!update.set.empty())
+ {
+ list = { update.set.begin(), update.set.end() };
+ }
+}
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelistUpdate.h b/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelistUpdate.h
new file mode 100644
index 0000000000000000000000000000000000000000..5f11bf0bbe82fbac3424fbf051e35790991b964b
--- /dev/null
+++ b/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelistUpdate.h
@@ -0,0 +1,22 @@
+#pragma once
+
+#include "BlackWhitelist.h"
+
+#include <RobotAPI/interface/core/BlackWhitelist.h>
+
+
+namespace armarx
+{
+
+ void updateStringList(std::set<std::string>& list, const StringListUpdate& update);
+
+
+ inline
+ void updateBlackWhitelist(StringBlackWhitelist& bw, const armarx::BlackWhitelistUpdate& update)
+ {
+ updateStringList(bw.black, update.blacklist);
+ updateStringList(bw.white, update.whitelist);
+ }
+
+}
+
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/CMakeLists.txt b/source/RobotAPI/components/DebugDrawerToArViz/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..678b67dd3df935ebf6d7cb2bb530117f6430de9f
--- /dev/null
+++ b/source/RobotAPI/components/DebugDrawerToArViz/CMakeLists.txt
@@ -0,0 +1,44 @@
+armarx_component_set_name("DebugDrawerToArViz")
+
+
+set(COMPONENT_LIBS
+ ArmarXCore
+
+ ${PROJECT_NAME}Interfaces
+ RobotAPIComponentPlugins
+)
+
+set(SOURCES
+ DebugDrawerToArViz.cpp
+ DebugDrawerToArVizComponent.cpp
+
+ BlackWhitelist.cpp # To be moved to SimoxUtility
+ BlackWhitelistUpdate.cpp # To be moved to RobotAPI core (or ArmarXCore)
+)
+set(HEADERS
+ DebugDrawerToArViz.h
+ DebugDrawerToArVizComponent.h
+
+ BlackWhitelist.h # To be moved to SimoxUtility
+ BlackWhitelistUpdate.h # To be moved to RobotAPI core (or ArmarXCore)
+)
+
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+#find_package(MyLib QUIET)
+#armarx_build_if(MyLib_FOUND "MyLib not available")
+# all target_include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+#if(MyLib_FOUND)
+# target_include_directories(DebugDrawerToArViz PUBLIC ${MyLib_INCLUDE_DIRS})
+#endif()
+
+# add unit tests
+add_subdirectory(test)
+
+
+armarx_component_set_name("DebugDrawerToArVizApp")
+set(COMPONENT_LIBS DebugDrawerToArViz)
+armarx_add_component_executable(main.cpp)
+
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.cpp b/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4ac857086a55e5ec19b709ca00dcd24f2b23a574
--- /dev/null
+++ b/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.cpp
@@ -0,0 +1,808 @@
+/*
+ * 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 RobotAPI::ArmarXObjects::DebugDrawerToArViz
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "DebugDrawerToArViz.h"
+
+#include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/color/interpolation.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "BlackWhitelistUpdate.h"
+
+
+#define FUNCTION_NOT_IMPLEMENTED_MESSAGE \
+ "Function DebugDrawerToArViz::" << __FUNCTION__ << "(): Not implemented."
+
+#define LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE() \
+ ARMARX_VERBOSE << FUNCTION_NOT_IMPLEMENTED_MESSAGE
+
+
+namespace armarx
+{
+
+ namespace
+ {
+ Eigen::Vector3f toEigen(Vector3BasePtr v)
+ {
+ ARMARX_CHECK_NOT_NULL(v);
+ return { v->x, v->y, v->z };
+ }
+
+ Eigen::Vector3f toEigen(DebugDrawerPointCloudElement e)
+ {
+ return { e.x, e.y, e.z };
+ }
+
+ Eigen::Quaternionf toEigen(QuaternionBasePtr q)
+ {
+ ARMARX_CHECK_NOT_NULL(q);
+ return Eigen::Quaternionf(q->qw, q->qx, q->qy, q->qz);
+ }
+
+ Eigen::Matrix4f toEigen(PoseBasePtr pose)
+ {
+ ARMARX_CHECK_NOT_NULL(pose);
+ return simox::math::pose(toEigen(pose->position), toEigen(pose->orientation));
+ }
+
+
+ simox::Color toSimox(DrawColor c)
+ {
+ return simox::Color(c.r, c.g, c.b, c.a);
+ }
+
+ viz::Color toViz(DrawColor c)
+ {
+ return viz::Color(toSimox(c));
+ }
+ }
+
+
+ void DebugDrawerToArViz::setArViz(viz::Client arviz)
+ {
+ this->arviz = arviz;
+ }
+
+
+ void DebugDrawerToArViz::updateBlackWhitelist(const BlackWhitelistUpdate& update, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+
+ armarx::updateBlackWhitelist(layerBlackWhitelist, update);
+ ARMARX_VERBOSE << "Updated layer black-whitelist: \n" << layerBlackWhitelist;
+
+ // Remove all excluded layers.
+ std::vector<viz::Layer> cleared;
+ for (const auto& [name, layer] : layers)
+ {
+ if (layerBlackWhitelist.isExcluded(name))
+ {
+ cleared.push_back(arviz.layer(name));
+ }
+ }
+ if (!cleared.empty())
+ {
+ arviz.commit(cleared);
+ }
+ }
+
+
+ void DebugDrawerToArViz::exportScene(const std::string&, const Ice::Current&)
+ {
+ LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
+ }
+ void DebugDrawerToArViz::exportLayer(const std::string&, const std::string&, const Ice::Current&)
+ {
+ LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
+ }
+
+
+ void DebugDrawerToArViz::setPoseVisu(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isExcluded(layer))
+ {
+ return;
+ }
+ setAndCommit(layer, viz::Pose(name).pose(toEigen(globalPose)));
+ }
+
+ void DebugDrawerToArViz::setScaledPoseVisu(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, Ice::Float scale, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isExcluded(layer))
+ {
+ return;
+ }
+ setAndCommit(layer, viz::Pose(name).pose(toEigen(globalPose)).scale(scale));
+ }
+
+ void DebugDrawerToArViz::setLineVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& globalPosition1, const Vector3BasePtr& globalPosition2, Ice::Float lineWidth, const DrawColor& color, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isExcluded(layer))
+ {
+ return;
+ }
+ setAndCommit(layer, viz::Polygon(name).addPoint(toEigen(globalPosition1)).addPoint(toEigen(globalPosition2))
+ .lineColor(toViz(color)).lineWidth(lineWidth).color(simox::Color::black(0)));
+ }
+
+ void DebugDrawerToArViz::setLineSetVisu(const std::string& layerName, const std::string& name, const DebugDrawerLineSet& lineSet, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isExcluded(layerName))
+ {
+ return;
+ }
+
+ viz::Layer& layer = getLayer(layerName);
+ ARMARX_CHECK_EQUAL(lineSet.points.size() % 2, 0) << VAROUT(lineSet.points.size());
+ ARMARX_CHECK_EQUAL(lineSet.intensities.size(), lineSet.points.size() / 2);
+
+ if (lineSet.useHeatMap)
+ {
+ ARMARX_VERBOSE << "DebugDrawerToArViz::" << __FUNCTION__ << "(): " << "'useHeatMap' not supported.";
+ }
+
+ simox::Color color0 = toSimox(lineSet.colorNoIntensity);
+ simox::Color color1 = toSimox(lineSet.colorFullIntensity);
+
+ for (size_t i = 0; i + 1 < lineSet.points.size(); i += 2)
+ {
+ const auto& p1 = lineSet.points[i];
+ const auto& p2 = lineSet.points[i + 1];
+ float intensity = lineSet.intensities[i / 2];
+ simox::Color color = simox::color::interpol::linear(intensity, color0, color1);
+
+ std::stringstream ss;
+ ss << name << "/" << i << "_" << i + 1;
+ setLayerElement(layer, viz::Polygon(ss.str()).addPoint(toEigen(p1)).addPoint(toEigen(p2))
+ .lineColor(color).lineWidth(lineSet.lineWidth).color(simox::Color::black(0)));
+ }
+ arviz.commit(layer);
+ }
+
+ void DebugDrawerToArViz::setBoxVisu(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, const Vector3BasePtr& dimensions, const DrawColor& color, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isExcluded(layer))
+ {
+ return;
+ }
+ setAndCommit(layer, viz::Box(name).pose(toEigen(globalPose)).size(toEigen(dimensions)).color(toViz(color)));
+ }
+
+ void DebugDrawerToArViz::setTextVisu(const std::string& layer, const std::string& name, const std::string& text, const Vector3BasePtr& globalPosition, const DrawColor& color, Ice::Int size, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isExcluded(layer))
+ {
+ return;
+ }
+ setAndCommit(layer, viz::Text(name).text(text).position(toEigen(globalPosition))
+ .scale(size).color(toViz(color)));
+ }
+
+ void DebugDrawerToArViz::setSphereVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& globalPosition, const DrawColor& color, Ice::Float radius, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isExcluded(layer))
+ {
+ return;
+ }
+ setAndCommit(layer, viz::Sphere(name).position(toEigen(globalPosition)).radius(radius).color(toViz(color)));
+ }
+
+ void DebugDrawerToArViz::setPointCloudVisu(const std::string& layer, const std::string& name, const DebugDrawerPointCloud& pointCloud, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isExcluded(layer))
+ {
+ return;
+ }
+
+ viz::PointCloud cloud(name);
+ for (const auto& p : pointCloud.points)
+ {
+ cloud.addPoint(p.x, p.y, p.z);
+ }
+ setAndCommit(layer, cloud.pointSizeInPixels(pointCloud.pointSize));
+ }
+
+ void DebugDrawerToArViz::setColoredPointCloudVisu(const std::string& layer, const std::string& name, const DebugDrawerColoredPointCloud& pointCloud, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isExcluded(layer))
+ {
+ return;
+ }
+
+ viz::PointCloud cloud(name);
+ for (const auto& p : pointCloud.points)
+ {
+ viz::ColoredPoint cp;
+ cp.x = p.x;
+ cp.y = p.y;
+ cp.z = p.z;
+ cp.color = toViz(p.color);
+ cloud.addPoint(cp);
+ }
+ setAndCommit(layer, cloud.pointSizeInPixels(pointCloud.pointSize));
+ }
+
+ void DebugDrawerToArViz::set24BitColoredPointCloudVisu(const std::string& layer, const std::string& name, const DebugDrawer24BitColoredPointCloud& pointCloud, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isExcluded(layer))
+ {
+ return;
+ }
+
+ viz::PointCloud cloud(name);
+ for (const auto& p : pointCloud.points)
+ {
+ viz::ColoredPoint cp;
+ cp.x = p.x;
+ cp.y = p.y;
+ cp.z = p.z;
+ cp.color = viz::Color(simox::Color(p.color.r, p.color.g, p.color.b));
+ cloud.addPoint(cp);
+ }
+ setAndCommit(layer, cloud.pointSizeInPixels(pointCloud.pointSize));
+ }
+
+ void DebugDrawerToArViz::setPolygonVisu(const std::string& layer, const std::string& name, const PolygonPointList& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, Ice::Float lineWidth, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isExcluded(layer))
+ {
+ return;
+ }
+
+ viz::Polygon poly(name);
+ for (const auto& p : polygonPoints)
+ {
+ poly.addPoint(toEigen(p));
+ }
+ setAndCommit(layer, poly.color(toViz(colorInner)).lineColor(toViz(colorBorder)).lineWidth(lineWidth));
+ }
+
+ void DebugDrawerToArViz::setTriMeshVisu(const std::string& layer, const std::string& name, const DebugDrawerTriMesh& triMesh, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isExcluded(layer))
+ {
+ return;
+ }
+
+ std::vector<Eigen::Vector3f> vertices;
+ std::vector<viz::data::Color> colors;
+ std::vector<viz::data::Face> faces;
+ for (const auto& v : triMesh.vertices)
+ {
+ vertices.emplace_back(v.x, v.y, v.z);
+ }
+ for (const auto& c : triMesh.colors)
+ {
+ colors.emplace_back(toViz(c));
+ }
+ for (const auto& f : triMesh.faces)
+ {
+ viz::data::Face& face = faces.emplace_back();
+ face.v0 = f.vertex1.vertexID;
+ face.v1 = f.vertex2.vertexID;
+ face.v2 = f.vertex3.vertexID;
+ face.c0 = f.vertex1.colorID;
+ face.c1 = f.vertex2.colorID;
+ face.c2 = f.vertex3.colorID;
+ }
+ setAndCommit(layer, viz::Mesh(name).vertices(vertices).colors(colors).faces(faces));
+ }
+
+ void DebugDrawerToArViz::setArrowVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& position, const Vector3BasePtr& direction, const DrawColor& color, Ice::Float length, Ice::Float width, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isExcluded(layer))
+ {
+ return;
+ }
+ setAndCommit(layer, viz::Arrow(name).position(toEigen(position)).direction(toEigen(direction))
+ .color(toViz(color)).width(width).length(length));
+ }
+
+ void DebugDrawerToArViz::setCylinderVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& globalPosition, const Vector3BasePtr& direction, Ice::Float length, Ice::Float radius, const DrawColor& color, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isExcluded(layer))
+ {
+ return;
+ }
+ setAndCommit(layer, viz::Cylinder(name)
+ .fromTo(toEigen(globalPosition), toEigen(globalPosition) + length * toEigen(direction))
+ .color(toViz(color)).radius(radius));
+ }
+
+ void DebugDrawerToArViz::setCircleArrowVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& globalPosition, const Vector3BasePtr& directionVec, Ice::Float radius, Ice::Float circleCompletion, Ice::Float width, const DrawColor& color, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isExcluded(layer))
+ {
+ return;
+ }
+ setAndCommit(layer, viz::ArrowCircle(name).position(toEigen(globalPosition)).normal(toEigen(directionVec))
+ .radius(radius).completion(circleCompletion).color(toViz(color)).width(width));
+ }
+
+
+ void DebugDrawerToArViz::setRobotVisu(const std::string& layer, const std::string& name, const std::string& robotFile, const std::string& armarxProject, DrawStyle drawStyleType, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isExcluded(layer))
+ {
+ return;
+ }
+
+ viz::Robot robot = viz::Robot(name).file(armarxProject, robotFile);
+ switch (drawStyleType)
+ {
+ case DrawStyle::CollisionModel:
+ robot.useCollisionModel();
+ break;
+ case DrawStyle::FullModel:
+ robot.useFullModel();
+ break;
+ }
+
+ robots.emplace(std::make_pair(layer, name), robot);
+ setAndCommit(layer, robot);
+ }
+
+ void DebugDrawerToArViz::updateRobotPose(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isExcluded(layer))
+ {
+ return;
+ }
+ if (auto it = robots.find(std::make_pair(layer, name)); it != robots.end())
+ {
+ viz::Robot& robot = it->second;
+ robot.pose(toEigen(globalPose));
+ }
+ arviz.commit(getLayer(layer));
+ }
+
+ void DebugDrawerToArViz::updateRobotConfig(const std::string& layer, const std::string& name, const NameValueMap& configuration, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isExcluded(layer))
+ {
+ return;
+ }
+ if (auto it = robots.find(std::make_pair(layer, name)); it != robots.end())
+ {
+ viz::Robot& robot = it->second;
+ robot.joints(configuration);
+ }
+ arviz.commit(getLayer(layer));
+ }
+
+ void DebugDrawerToArViz::updateRobotColor(const std::string& layer, const std::string& name, const DrawColor& color, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isExcluded(layer))
+ {
+ return;
+ }
+ if (auto it = robots.find(std::make_pair(layer, name)); it != robots.end())
+ {
+ viz::Robot& robot = it->second;
+ robot.overrideColor(toViz(color));
+ }
+ arviz.commit(getLayer(layer));
+ }
+
+ void DebugDrawerToArViz::updateRobotNodeColor(const std::string& layer, const std::string& name, const std::string& robotNodeName, const DrawColor& color, const Ice::Current&)
+ {
+ (void) layer, (void) name, (void) robotNodeName, (void) color;
+ LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
+ }
+
+ void DebugDrawerToArViz::removeRobotVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isExcluded(layer))
+ {
+ return;
+ }
+ robots.erase(std::make_pair(layer, name));
+ removeAndCommit(layer, name);
+ }
+
+
+ void DebugDrawerToArViz::setPoseDebugLayerVisu(const std::string& name, const PoseBasePtr& globalPose, const Ice::Current& c)
+ {
+ setPoseVisu(DEBUG_LAYER_NAME, name, globalPose, c);
+ }
+ void DebugDrawerToArViz::setScaledPoseDebugLayerVisu(const std::string& name, const PoseBasePtr& globalPose, Ice::Float scale, const Ice::Current& c)
+ {
+ setScaledPoseVisu(DEBUG_LAYER_NAME, name, globalPose, scale, c);
+ }
+ void DebugDrawerToArViz::setLineDebugLayerVisu(const std::string& name, const Vector3BasePtr& globalPosition1, const Vector3BasePtr& globalPosition2, Ice::Float lineWidth, const DrawColor& color, const Ice::Current& c)
+ {
+ setLineVisu(DEBUG_LAYER_NAME, name, globalPosition1, globalPosition2, lineWidth, color, c);
+ }
+ void DebugDrawerToArViz::setLineSetDebugLayerVisu(const std::string& name, const DebugDrawerLineSet& lineSet, const Ice::Current& c)
+ {
+ setLineSetVisu(DEBUG_LAYER_NAME, name, lineSet, c);
+ }
+ void DebugDrawerToArViz::setBoxDebugLayerVisu(const std::string& name, const PoseBasePtr& globalPose, const Vector3BasePtr& dimensions, const DrawColor& color, const Ice::Current& c)
+ {
+ setBoxVisu(DEBUG_LAYER_NAME, name, globalPose, dimensions, color, c);
+ }
+ void DebugDrawerToArViz::setTextDebugLayerVisu(const std::string& name, const std::string& text, const Vector3BasePtr& globalPosition, const DrawColor& color, Ice::Int size, const Ice::Current& c)
+ {
+ setTextVisu(DEBUG_LAYER_NAME, name, text, globalPosition, color, size, c);
+ }
+ void DebugDrawerToArViz::setSphereDebugLayerVisu(const std::string& name, const Vector3BasePtr& globalPosition, const DrawColor& color, Ice::Float radius, const Ice::Current& c)
+ {
+ setSphereVisu(DEBUG_LAYER_NAME, name, globalPosition, color, radius, c);
+ }
+ void DebugDrawerToArViz::setPointCloudDebugLayerVisu(const std::string& name, const DebugDrawerPointCloud& pointCloud, const Ice::Current& c)
+ {
+ setPointCloudVisu(DEBUG_LAYER_NAME, name, pointCloud, c);
+ }
+ void DebugDrawerToArViz::set24BitColoredPointCloudDebugLayerVisu(const std::string& name, const DebugDrawer24BitColoredPointCloud& pointCloud, const Ice::Current& c)
+ {
+ set24BitColoredPointCloudVisu(DEBUG_LAYER_NAME, name, pointCloud, c);
+ }
+ void DebugDrawerToArViz::setPolygonDebugLayerVisu(const std::string& name, const PolygonPointList& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, Ice::Float lineWidth, const Ice::Current& c)
+ {
+ setPolygonVisu(DEBUG_LAYER_NAME, name, polygonPoints, colorInner, colorBorder, lineWidth, c);
+ }
+ void DebugDrawerToArViz::setTriMeshDebugLayerVisu(const std::string& name, const DebugDrawerTriMesh& triMesh, const Ice::Current& c)
+ {
+ setTriMeshVisu(DEBUG_LAYER_NAME, name, triMesh, c);
+ }
+ void DebugDrawerToArViz::setArrowDebugLayerVisu(const std::string& name, const Vector3BasePtr& position, const Vector3BasePtr& direction, const DrawColor& color, Ice::Float length, Ice::Float width, const Ice::Current& c)
+ {
+ setArrowVisu(DEBUG_LAYER_NAME, name, position, direction, color, length, width, c);
+ }
+ void DebugDrawerToArViz::setCylinderDebugLayerVisu(const std::string& name, const Vector3BasePtr& globalPosition, const Vector3BasePtr& direction, Ice::Float length, Ice::Float radius, const DrawColor& color, const Ice::Current& c)
+ {
+ setCylinderVisu(DEBUG_LAYER_NAME, name, globalPosition, direction, length, radius, color, c);
+ }
+ void DebugDrawerToArViz::setCircleDebugLayerVisu(const std::string& name, const Vector3BasePtr& globalPosition, const Vector3BasePtr& directionVec, Ice::Float radius, Ice::Float circleCompletion, Ice::Float width, const DrawColor& color, const Ice::Current&)
+ {
+ (void) name, (void) globalPosition, (void) directionVec, (void) radius, (void) circleCompletion, (void) width, (void) color;
+ LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
+ }
+
+
+ void DebugDrawerToArViz::removePoseVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isIncluded(layer))
+ {
+ removeAndCommit(layer, name);
+ }
+ }
+ void DebugDrawerToArViz::removeLineVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isIncluded(layer))
+ {
+ removeAndCommit(layer, name);
+ }
+ }
+ void DebugDrawerToArViz::removeLineSetVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isIncluded(layer))
+ {
+ removeAndCommit(layer, name);
+ }
+ }
+ void DebugDrawerToArViz::removeBoxVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isIncluded(layer))
+ {
+ removeAndCommit(layer, name);
+ }
+ }
+ void DebugDrawerToArViz::removeTextVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isIncluded(layer))
+ {
+ removeAndCommit(layer, name);
+ }
+ }
+ void DebugDrawerToArViz::removeSphereVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isIncluded(layer))
+ {
+ removeAndCommit(layer, name);
+ }
+ }
+ void DebugDrawerToArViz::removePointCloudVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isIncluded(layer))
+ {
+ removeAndCommit(layer, name);
+ }
+ }
+ void DebugDrawerToArViz::removeColoredPointCloudVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isIncluded(layer))
+ {
+ removeAndCommit(layer, name);
+ }
+ }
+ void DebugDrawerToArViz::remove24BitColoredPointCloudVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isIncluded(layer))
+ {
+ removeAndCommit(layer, name);
+ }
+ }
+ void DebugDrawerToArViz::removePolygonVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isIncluded(layer))
+ {
+ removeAndCommit(layer, name);
+ }
+ }
+ void DebugDrawerToArViz::removeTriMeshVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isIncluded(layer))
+ {
+ removeAndCommit(layer, name);
+ }
+ }
+ void DebugDrawerToArViz::removeArrowVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isIncluded(layer))
+ {
+ removeAndCommit(layer, name);
+ }
+ }
+ void DebugDrawerToArViz::removeCylinderVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isIncluded(layer))
+ {
+ removeAndCommit(layer, name);
+ }
+ }
+ void DebugDrawerToArViz::removeCircleVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+ if (layerBlackWhitelist.isIncluded(layer))
+ {
+ removeAndCommit(layer, name);
+ }
+ }
+
+
+ void DebugDrawerToArViz::removePoseDebugLayerVisu(const std::string& name, const Ice::Current&)
+ {
+ removePointCloudVisu(DEBUG_LAYER_NAME, name);
+ }
+ void DebugDrawerToArViz::removeLineDebugLayerVisu(const std::string& name, const Ice::Current&)
+ {
+ removeLineVisu(DEBUG_LAYER_NAME, name);
+ }
+ void DebugDrawerToArViz::removeLineSetDebugLayerVisu(const std::string& name, const Ice::Current&)
+ {
+ removeLineSetVisu(DEBUG_LAYER_NAME, name);
+ }
+ void DebugDrawerToArViz::removeBoxDebugLayerVisu(const std::string& name, const Ice::Current&)
+ {
+ removeBoxVisu(DEBUG_LAYER_NAME, name);
+ }
+ void DebugDrawerToArViz::removeTextDebugLayerVisu(const std::string& name, const Ice::Current&)
+ {
+ removeTextVisu(DEBUG_LAYER_NAME, name);
+ }
+ void DebugDrawerToArViz::removeSphereDebugLayerVisu(const std::string& name, const Ice::Current&)
+ {
+ removeSphereVisu(DEBUG_LAYER_NAME, name);
+ }
+ void DebugDrawerToArViz::removePointCloudDebugLayerVisu(const std::string& name, const Ice::Current&)
+ {
+ removePointCloudVisu(DEBUG_LAYER_NAME, name);
+ }
+ void DebugDrawerToArViz::removeColoredPointCloudDebugLayerVisu(const std::string& name, const Ice::Current&)
+ {
+ removeColoredPointCloudVisu(DEBUG_LAYER_NAME, name);
+ }
+ void DebugDrawerToArViz::remove24BitColoredPointCloudDebugLayerVisu(const std::string& name, const Ice::Current&)
+ {
+ remove24BitColoredPointCloudVisu(DEBUG_LAYER_NAME, name);
+ }
+ void DebugDrawerToArViz::removePolygonDebugLayerVisu(const std::string& name, const Ice::Current&)
+ {
+ removePolygonVisu(DEBUG_LAYER_NAME, name);
+ }
+ void DebugDrawerToArViz::removeTriMeshDebugLayerVisu(const std::string& name, const Ice::Current&)
+ {
+ removeTriMeshVisu(DEBUG_LAYER_NAME, name);
+ }
+ void DebugDrawerToArViz::removeArrowDebugLayerVisu(const std::string& name, const Ice::Current&)
+ {
+ removeArrowVisu(DEBUG_LAYER_NAME, name);
+ }
+ void DebugDrawerToArViz::removeCylinderDebugLayerVisu(const std::string& name, const Ice::Current&)
+ {
+ removeCylinderVisu(DEBUG_LAYER_NAME, name);
+ }
+ void DebugDrawerToArViz::removeCircleDebugLayerVisu(const std::string& name, const Ice::Current&)
+ {
+ removeCircleVisu(DEBUG_LAYER_NAME, name);
+ }
+
+ void DebugDrawerToArViz::clearAll(const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+
+ std::vector<const viz::Layer*> commit;
+ commit.reserve(layers.size());
+ for (auto& [name, layer] : layers)
+ {
+ layer.clear();
+ commit.push_back(&layer);
+ }
+ arviz.commit(commit);
+ }
+ void DebugDrawerToArViz::clearLayer(const std::string& layerName, const Ice::Current&)
+ {
+ std::scoped_lock lock(mutex);
+
+ viz::Layer layer = getLayer(layerName);
+ layer.clear();
+ arviz.commit(layer);
+ }
+ void DebugDrawerToArViz::clearDebugLayer(const Ice::Current&)
+ {
+ clearLayer(DEBUG_LAYER_NAME);
+ }
+
+ void DebugDrawerToArViz::enableLayerVisu(const std::string& layer, bool visible, const Ice::Current&)
+ {
+ (void) layer, (void) visible;
+ LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
+ }
+ void DebugDrawerToArViz::enableDebugLayerVisu(bool visible, const Ice::Current&)
+ {
+ enableLayerVisu(DEBUG_LAYER_NAME, visible);
+ }
+
+ Ice::StringSeq DebugDrawerToArViz::layerNames(const Ice::Current&)
+ {
+ LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
+ return {};
+ }
+ LayerInformationSequence DebugDrawerToArViz::layerInformation(const Ice::Current&)
+ {
+ LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
+ return {};
+ }
+
+ bool DebugDrawerToArViz::hasLayer(const std::string&, const Ice::Current&)
+ {
+ LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
+ return false;
+ }
+ void DebugDrawerToArViz::removeLayer(const std::string&, const Ice::Current&)
+ {
+ LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
+ }
+
+ void DebugDrawerToArViz::disableAllLayers(const Ice::Current&)
+ {
+ LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
+ }
+ void DebugDrawerToArViz::enableAllLayers(const Ice::Current&)
+ {
+ LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
+ }
+
+ void DebugDrawerToArViz::enableSelections(const std::string&, const Ice::Current&)
+ {
+ LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
+ }
+ void DebugDrawerToArViz::disableSelections(const std::string&, const Ice::Current&)
+ {
+ LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
+ }
+ void DebugDrawerToArViz::clearSelections(const std::string&, const Ice::Current&)
+ {
+ LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
+ }
+
+ void DebugDrawerToArViz::select(const std::string& layer, const std::string& elementName, const Ice::Current&)
+ {
+ (void) layer, (void) elementName;
+ LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
+ }
+ void DebugDrawerToArViz::deselect(const std::string& layer, const std::string& elementName, const Ice::Current&)
+ {
+ (void) layer, (void) elementName;
+ LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
+ }
+
+ DebugDrawerSelectionList DebugDrawerToArViz::getSelections(const Ice::Current&)
+ {
+ LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
+ return {};
+ }
+
+
+ viz::Layer& DebugDrawerToArViz::getLayer(const std::string& layerName)
+ {
+ if (auto it = layers.find(layerName); it != layers.end())
+ {
+ return it->second;
+ }
+ else
+ {
+ return layers.emplace(layerName, arviz.layer(layerName)).first->second;
+ }
+ }
+
+ viz::data::ElementSeq::iterator DebugDrawerToArViz::findLayerElement(viz::Layer& layer, const std::string& elementName)
+ {
+ return std::find_if(layer.data_.elements.begin(), layer.data_.elements.end(),
+ [&elementName](const viz::data::ElementPtr & e)
+ {
+ return e->id == elementName;
+ });
+ }
+
+ void DebugDrawerToArViz::removeLayerElement(viz::Layer& layer, const std::string& name)
+ {
+ auto it = findLayerElement(layer, name);
+ if (it != layer.data_.elements.end())
+ {
+ layer.data_.elements.erase(it);
+ }
+ }
+
+ void DebugDrawerToArViz::removeAndCommit(const std::string& layerName, const std::string& name)
+ {
+ viz::Layer& layer = getLayer(layerName);
+ removeLayerElement(layer, name);
+ arviz.commit(layer);
+ }
+
+}
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.h b/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.h
new file mode 100644
index 0000000000000000000000000000000000000000..f4ce170c84fb0ce7507a8e87bf2901b53d3b7d89
--- /dev/null
+++ b/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.h
@@ -0,0 +1,215 @@
+/*
+ * 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 RobotAPI::ArmarXObjects::DebugDrawerToArViz
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <map>
+#include <mutex>
+
+#include <RobotAPI/interface/visualization/DebugDrawerToArViz.h>
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+#include "BlackWhitelist.h"
+
+
+namespace armarx
+{
+
+ /**
+ * @brief Passes updates from DebugDrawerInterface to ArViz.
+ */
+ class DebugDrawerToArViz : virtual public armarx::DebugDrawerToArvizInterface
+ {
+ public:
+
+ void setArViz(viz::Client arviz);
+
+
+ // BlackWhitelistTopic interface
+ public:
+ void updateBlackWhitelist(const BlackWhitelistUpdate& update, const Ice::Current& = Ice::emptyCurrent) override;
+
+
+ // DebugDrawerInterface interface
+ public:
+
+ void exportScene(const std::string& filename, const Ice::Current& = Ice::emptyCurrent) override;
+ void exportLayer(const std::string& filename, const std::string& layerName, const Ice::Current& = Ice::emptyCurrent) override;
+
+ void setPoseVisu(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, const Ice::Current& = Ice::emptyCurrent) override;
+ void setScaledPoseVisu(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, Ice::Float scale, const Ice::Current& = Ice::emptyCurrent) override;
+ void setLineVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& globalPosition1, const Vector3BasePtr& globalPosition2, Ice::Float lineWidth, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
+ void setLineSetVisu(const std::string& layer, const std::string& name, const DebugDrawerLineSet& lineSet, const Ice::Current& = Ice::emptyCurrent) override;
+ void setBoxVisu(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, const Vector3BasePtr& dimensions, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
+ void setTextVisu(const std::string& layer, const std::string& name, const std::string& text, const Vector3BasePtr& globalPosition, const DrawColor& color, Ice::Int size, const Ice::Current& = Ice::emptyCurrent) override;
+ void setSphereVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& globalPosition, const DrawColor& color, Ice::Float radius, const Ice::Current& = Ice::emptyCurrent) override;
+ void setPointCloudVisu(const std::string& layer, const std::string& name, const DebugDrawerPointCloud& pointCloud, const Ice::Current& = Ice::emptyCurrent) override;
+ void setColoredPointCloudVisu(const std::string& layer, const std::string& name, const DebugDrawerColoredPointCloud& pointCloud, const Ice::Current& = Ice::emptyCurrent) override;
+ void set24BitColoredPointCloudVisu(const std::string& layer, const std::string& name, const DebugDrawer24BitColoredPointCloud& pointCloud, const Ice::Current& = Ice::emptyCurrent) override;
+ void setPolygonVisu(const std::string& layer, const std::string& name, const PolygonPointList& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, Ice::Float lineWidth, const Ice::Current& = Ice::emptyCurrent) override;
+ void setTriMeshVisu(const std::string& layer, const std::string& name, const DebugDrawerTriMesh& triMesh, const Ice::Current& = Ice::emptyCurrent) override;
+ void setArrowVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& position, const Vector3BasePtr& direction, const DrawColor& color, Ice::Float length, Ice::Float width, const Ice::Current& = Ice::emptyCurrent) override;
+ void setCylinderVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& globalPosition, const Vector3BasePtr& direction, Ice::Float length, Ice::Float radius, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
+ void setCircleArrowVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& globalPosition, const Vector3BasePtr& directionVec, Ice::Float radius, Ice::Float circleCompletion, Ice::Float width, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
+
+ void setRobotVisu(const std::string& layer, const std::string& name, const std::string& robotFile, const std::string& armarxProject, DrawStyle drawStyleType, const Ice::Current& = Ice::emptyCurrent) override;
+ void updateRobotPose(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, const Ice::Current& = Ice::emptyCurrent) override;
+ void updateRobotConfig(const std::string& layer, const std::string& name, const NameValueMap& configuration, const Ice::Current& = Ice::emptyCurrent) override;
+ void updateRobotColor(const std::string& layer, const std::string& name, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
+ void updateRobotNodeColor(const std::string& layer, const std::string& name, const std::string& robotNodeName, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeRobotVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+
+ void setPoseDebugLayerVisu(const std::string& name, const PoseBasePtr& globalPose, const Ice::Current& = Ice::emptyCurrent) override;
+ void setScaledPoseDebugLayerVisu(const std::string& name, const PoseBasePtr& globalPose, Ice::Float scale, const Ice::Current& = Ice::emptyCurrent) override;
+ void setLineDebugLayerVisu(const std::string& name, const Vector3BasePtr& globalPosition1, const Vector3BasePtr& globalPosition2, Ice::Float lineWidth, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
+ void setLineSetDebugLayerVisu(const std::string& name, const DebugDrawerLineSet& lineSet, const Ice::Current& = Ice::emptyCurrent) override;
+ void setBoxDebugLayerVisu(const std::string& name, const PoseBasePtr& globalPose, const Vector3BasePtr& dimensions, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
+ void setTextDebugLayerVisu(const std::string& name, const std::string& text, const Vector3BasePtr& globalPosition, const DrawColor& color, Ice::Int size, const Ice::Current& = Ice::emptyCurrent) override;
+ void setSphereDebugLayerVisu(const std::string& name, const Vector3BasePtr& globalPosition, const DrawColor& color, Ice::Float radius, const Ice::Current& = Ice::emptyCurrent) override;
+ void setPointCloudDebugLayerVisu(const std::string& name, const DebugDrawerPointCloud& pointCloud, const Ice::Current& = Ice::emptyCurrent) override;
+ void set24BitColoredPointCloudDebugLayerVisu(const std::string& name, const DebugDrawer24BitColoredPointCloud& pointCloud, const Ice::Current& = Ice::emptyCurrent) override;
+ void setPolygonDebugLayerVisu(const std::string& name, const PolygonPointList& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, Ice::Float lineWidth, const Ice::Current& = Ice::emptyCurrent) override;
+ void setTriMeshDebugLayerVisu(const std::string& name, const DebugDrawerTriMesh& triMesh, const Ice::Current& = Ice::emptyCurrent) override;
+ void setArrowDebugLayerVisu(const std::string& name, const Vector3BasePtr& position, const Vector3BasePtr& direction, const DrawColor& color, Ice::Float length, Ice::Float width, const Ice::Current& = Ice::emptyCurrent) override;
+ void setCylinderDebugLayerVisu(const std::string& name, const Vector3BasePtr& globalPosition, const Vector3BasePtr& direction, Ice::Float length, Ice::Float radius, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
+ void setCircleDebugLayerVisu(const std::string& name, const Vector3BasePtr& globalPosition, const Vector3BasePtr& directionVec, Ice::Float radius, Ice::Float circleCompletion, Ice::Float width, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
+
+ void removePoseVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeLineVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeLineSetVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeBoxVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeTextVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeSphereVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removePointCloudVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeColoredPointCloudVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void remove24BitColoredPointCloudVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removePolygonVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeTriMeshVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeArrowVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeCylinderVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeCircleVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+
+ void removePoseDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeLineDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeLineSetDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeBoxDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeTextDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeSphereDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removePointCloudDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeColoredPointCloudDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void remove24BitColoredPointCloudDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removePolygonDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeTriMeshDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeArrowDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeCylinderDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeCircleDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+
+ void clearAll(const Ice::Current& = Ice::emptyCurrent) override;
+ void clearLayer(const std::string& layer, const Ice::Current& = Ice::emptyCurrent) override;
+ void clearDebugLayer(const Ice::Current& = Ice::emptyCurrent) override;
+
+ void enableLayerVisu(const std::string& layer, bool visible, const Ice::Current& = Ice::emptyCurrent) override;
+ void enableDebugLayerVisu(bool visible, const Ice::Current& = Ice::emptyCurrent) override;
+
+ Ice::StringSeq layerNames(const Ice::Current& = Ice::emptyCurrent) override;
+ LayerInformationSequence layerInformation(const Ice::Current& = Ice::emptyCurrent) override;
+
+ bool hasLayer(const std::string&, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeLayer(const std::string&, const Ice::Current& = Ice::emptyCurrent) override;
+
+ void disableAllLayers(const Ice::Current& = Ice::emptyCurrent) override;
+ void enableAllLayers(const Ice::Current& = Ice::emptyCurrent) override;
+
+ void enableSelections(const std::string&, const Ice::Current& = Ice::emptyCurrent) override;
+ void disableSelections(const std::string&, const Ice::Current& = Ice::emptyCurrent) override;
+ void clearSelections(const std::string&, const Ice::Current& = Ice::emptyCurrent) override;
+
+ void select(const std::string& layer, const std::string& elementName, const Ice::Current& = Ice::emptyCurrent) override;
+ void deselect(const std::string& layer, const std::string& elementName, const Ice::Current& = Ice::emptyCurrent) override;
+
+ DebugDrawerSelectionList getSelections(const Ice::Current& = Ice::emptyCurrent) override;
+
+
+ private:
+
+ viz::Layer& getLayer(const std::string& layerName);
+ viz::data::ElementSeq::iterator
+ findLayerElement(viz::Layer& layer, const std::string& elementName);
+
+
+ template <class Element>
+ void setLayerElement(viz::Layer& layer, Element element)
+ {
+ bool found = false;
+ for (size_t i = 0; i < layer.data_.elements.size(); ++i)
+ {
+ if (layer.data_.elements[i]->id == element.data_->id)
+ {
+ // Replace existing element with given element.
+ layer.data_.elements[i] = element.data_;
+ found = true;
+ break;
+ }
+ }
+ if (!found)
+ {
+ // Add given element.
+ layer.add(element);
+ }
+ }
+ template <class Element>
+ void setLayerElement(const std::string& layerName, Element element)
+ {
+ setLayerElement(getLayer(layerName), element);
+ }
+
+ template <class Element>
+ void setAndCommit(const std::string& layerName, Element element)
+ {
+ viz::Layer& layer = getLayer(layerName);
+ setLayerElement(layer, element);
+ arviz.commit(layer);
+ }
+
+ void removeLayerElement(viz::Layer& layer, const std::string& name);
+ void removeAndCommit(const std::string& layerName, const std::string& name);
+
+
+ public:
+
+ armarx::StringBlackWhitelist layerBlackWhitelist;
+
+
+ private:
+
+ const std::string DEBUG_LAYER_NAME = "debug";
+
+ std::mutex mutex;
+
+ viz::Client arviz;
+
+ std::map<std::string, viz::Layer> layers;
+
+ std::map<std::pair<std::string, std::string>, viz::Robot> robots;
+
+ };
+}
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArVizComponent.cpp b/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArVizComponent.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9db1d84ddf17abd2aaf65b17b61c317b4a71d749
--- /dev/null
+++ b/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArVizComponent.cpp
@@ -0,0 +1,90 @@
+/*
+ * 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 RobotAPI::ArmarXObjects::DebugDrawerToArViz
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "DebugDrawerToArVizComponent.h"
+
+#include "BlackWhitelistUpdate.h"
+
+
+namespace armarx
+{
+ DebugDrawerToArVizPropertyDefinitions::DebugDrawerToArVizPropertyDefinitions(std::string prefix) :
+ armarx::ComponentPropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<std::string>("DebugDrawerTopicName", "DebugDrawerUpdates",
+ "Name of the topic the DebugDrawer listens to.");
+
+
+ defineOptionalProperty<std::string>(
+ "LayerBlackWhitelistTopic", "DebugDrawerToArVizLayerBlackWhitelistUpdates",
+ "The layer where updates to the layer black-whitelist are published.");
+ defineOptionalProperty<std::vector<std::string>>("LayerWhitelist", {},
+ "If not empty, layers are shown (comma separated list).")
+ .map("[empty whitelist]", {});
+ defineOptionalProperty<std::vector<std::string>>("LayerBlacklist", {},
+ "These layers will never be shown (comma separated list).")
+ .map("[empty blacklist]", {});
+ }
+
+
+ std::string DebugDrawerToArVizComponent::getDefaultName() const
+ {
+ return "DebugDrawerToArViz";
+ }
+
+
+ void DebugDrawerToArVizComponent::onInitComponent()
+ {
+ {
+ BlackWhitelistUpdate update;
+ getProperty(update.whitelist.set, "LayerWhitelist");
+ getProperty(update.blacklist.set, "LayerBlacklist");
+ armarx::updateBlackWhitelist(DebugDrawerToArViz::layerBlackWhitelist, update);
+ ARMARX_VERBOSE << "Layer black-white-list: \n" << DebugDrawerToArViz::layerBlackWhitelist;
+ }
+
+ usingTopicFromProperty("DebugDrawerTopicName");
+ usingTopicFromProperty("LayerBlackWhitelistTopic");
+ }
+
+
+ void DebugDrawerToArVizComponent::onConnectComponent()
+ {
+ DebugDrawerToArViz::setArViz(ArVizComponentPluginUser::arviz);
+ }
+
+
+ void DebugDrawerToArVizComponent::onDisconnectComponent()
+ {
+ }
+
+
+ void DebugDrawerToArVizComponent::onExitComponent()
+ {
+ }
+
+
+ armarx::PropertyDefinitionsPtr DebugDrawerToArVizComponent::createPropertyDefinitions()
+ {
+ return armarx::PropertyDefinitionsPtr(new DebugDrawerToArVizPropertyDefinitions(getConfigIdentifier()));
+ }
+}
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArVizComponent.h b/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArVizComponent.h
new file mode 100644
index 0000000000000000000000000000000000000000..957237826c330671688be6c1c88427c04d0b9a1e
--- /dev/null
+++ b/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArVizComponent.h
@@ -0,0 +1,89 @@
+/*
+ * 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 RobotAPI::ArmarXObjects::DebugDrawerToArViz
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+#include <ArmarXCore/core/Component.h>
+
+#include <RobotAPI/interface/visualization/DebugDrawerToArViz.h>
+
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+#include "DebugDrawerToArViz.h"
+
+
+namespace armarx
+{
+ /**
+ * @class DebugDrawerToArVizPropertyDefinitions
+ * @brief Property definitions of `DebugDrawerToArViz`.
+ */
+ class DebugDrawerToArVizPropertyDefinitions :
+ public armarx::ComponentPropertyDefinitions
+ {
+ public:
+ DebugDrawerToArVizPropertyDefinitions(std::string prefix);
+ };
+
+
+ /**
+ * @defgroup Component-DebugDrawerToArViz DebugDrawerToArViz
+ * @ingroup RobotAPI-Components
+ * A description of the component DebugDrawerToArViz.
+ *
+ * @class DebugDrawerToArViz
+ * @ingroup Component-DebugDrawerToArViz
+ * @brief Brief description of class DebugDrawerToArViz.
+ *
+ * Detailed description of class DebugDrawerToArViz.
+ */
+ class DebugDrawerToArVizComponent :
+ virtual public armarx::Component,
+ virtual public armarx::DebugDrawerToArViz, // Implements armarx::DebugDrawerToArvizInterface
+ virtual public armarx::ArVizComponentPluginUser
+ {
+ public:
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+
+ protected:
+
+ /// @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;
+
+ /// @see PropertyUser::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ };
+}
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/main.cpp b/source/RobotAPI/components/DebugDrawerToArViz/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a498c8dc5741c89e7812cdefc0c77e8b5b03446d
--- /dev/null
+++ b/source/RobotAPI/components/DebugDrawerToArViz/main.cpp
@@ -0,0 +1,32 @@
+/*
+ * 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 RobotAPI::application::DebugDrawerToArViz
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArVizComponent.h>
+
+#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+ return armarx::runSimpleComponentApp < armarx::DebugDrawerToArVizComponent > (argc, argv, "DebugDrawerToArViz");
+}
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/test/CMakeLists.txt b/source/RobotAPI/components/DebugDrawerToArViz/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9fc18bead2185a2115b4eeda6663da1a0aa2c55c
--- /dev/null
+++ b/source/RobotAPI/components/DebugDrawerToArViz/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore DebugDrawerToArViz)
+
+armarx_add_test(DebugDrawerToArVizTest DebugDrawerToArVizTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/test/DebugDrawerToArVizTest.cpp b/source/RobotAPI/components/DebugDrawerToArViz/test/DebugDrawerToArVizTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..42047e6b01ac9b12ff0e96ecf5eef9f1be0cdef8
--- /dev/null
+++ b/source/RobotAPI/components/DebugDrawerToArViz/test/DebugDrawerToArVizTest.cpp
@@ -0,0 +1,37 @@
+/*
+ * 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 RobotAPI::ArmarXObjects::DebugDrawerToArViz
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::DebugDrawerToArViz
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include <RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArVizComponent.h>
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+ armarx::DebugDrawerToArVizComponent instance;
+
+ BOOST_CHECK_EQUAL(true, true);
+}
diff --git a/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp b/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp
index f0e7ae58e5a83e9e2ea939b867f7cd0626c809b7..b788ccb9171ff5a4ed253198232aabd4b292c8af 100644
--- a/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp
+++ b/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp
@@ -36,7 +36,8 @@ namespace armarx
{
armarx::PropertyDefinitionsPtr defs(new RobotToArVizPropertyDefinitions(getConfigIdentifier()));
- defs->optional(updateFrequency, "updateFrequency", "Target number of updates per second.");
+ // defs->optional(updateFrequency, "updateFrequency", "Target number of updates per second.");
+ defs->defineOptionalProperty("updateFrequency", updateFrequency, "Target number of updates per second.");
return defs;
}
@@ -50,6 +51,7 @@ namespace armarx
void RobotToArViz::onInitComponent()
{
+ getProperty(updateFrequency, "updateFrequency");
}
@@ -86,7 +88,7 @@ namespace armarx
RobotState::synchronizeLocalClone(robotName);
robotViz.joints(robot->getConfig()->getRobotNodeJointValueMap())
- .pose(robot->getGlobalPose());
+ .pose(robot->getGlobalPose());
arviz.commitLayerContaining(robot->getName(), robotViz);
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
index d2a81100abbfa7a3a0b94964e3a260152dc04adb..67405541ac6bd334191fee0f5d740c609e0a3e46 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
@@ -5,10 +5,25 @@
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
+#include <VirtualRobot/math/Helpers.h>
+
#include "NJointCartesianNaturalPositionController.h"
namespace armarx
{
+ std::string vec2str(const std::vector<float>& vec)
+ {
+ std::stringstream ss;
+ for (size_t i = 0; i < vec.size(); i++)
+ {
+ if (i > 0)
+ {
+ ss << ", ";
+ }
+ ss << vec.at(i);
+ }
+ return ss.str();
+ }
std::ostream& operator<<(std::ostream& out, const CartesianNaturalPositionControllerConfig& cfg)
{
out << "maxPositionAcceleration " << cfg.maxPositionAcceleration << '\n'
@@ -17,12 +32,18 @@ namespace armarx
<< "KpPos " << cfg.KpPos << '\n'
<< "KpOri " << cfg.KpOri << '\n'
- << "maxPosVel " << cfg.maxPosVel << '\n'
- << "maxOriVel " << cfg.maxOriVel << '\n'
+ << "maxTcpPosVel " << cfg.maxTcpPosVel << '\n'
+ << "maxTcpOriVel " << cfg.maxTcpOriVel << '\n'
+ << "maxElpPosVel " << cfg.maxElbPosVel << '\n'
+
+ << "maxJointVelocity " << vec2str(cfg.maxJointVelocity) << '\n'
+ << "maxNullspaceVelocity " << vec2str(cfg.maxNullspaceVelocity) << '\n'
+ << "jointCosts " << vec2str(cfg.jointCosts) << '\n'
<< "jointLimitAvoidanceKp " << cfg.jointLimitAvoidanceKp << '\n'
- << "jointLimitAvoidanceScale " << cfg.jointLimitAvoidanceScale << '\n'
<< "elbowKp " << cfg.elbowKp << '\n'
+ << "nullspaceTargetKp " << cfg.nullspaceTargetKp << '\n'
+
;
return out;
}
@@ -100,7 +121,8 @@ namespace armarx
config->runCfg.maxNullspaceAcceleration
);
- _rtPosController->setConfig({});
+ _rtPosController->setConfig(config->runCfg);
+ ARMARX_IMPORTANT << "controller config: " << config->runCfg;
for (size_t i = 0; i < _rtRns->getSize(); ++i)
{
@@ -114,6 +136,11 @@ namespace armarx
_rtVelSensors.emplace_back(&(sv->velocity));
}
}
+
+ _rtFFvelMaxAgeMS = config->feedforwardVelocityTTLms;
+
+ _rtFTHistory.resize(config->ftHistorySize, FTval());
+
//ARMARX_IMPORTANT << "got control";
//visu
{
@@ -125,13 +152,17 @@ namespace armarx
void NJointCartesianNaturalPositionController::rtPreActivateController()
{
- _publishIsAtTarget = false;
+ //_publishIsAtTarget = false;
//_rtForceOffset = Eigen::Vector3f::Zero();
- _publishErrorPos = 0;
- _publishErrorOri = 0;
- _publishErrorPosMax = 0;
- _publishErrorOriMax = 0;
+ _publish.errorPos = 0;
+ _publish.errorOri = 0;
+ _publish.errorPosMax = 0;
+ _publish.errorOriMax = 0;
+ _publish.tcpPosVel = 0;
+ _publish.tcpOriVel = 0;
+ _publish.elbPosVel = 0;
+ //_publish.targetReached = false;
//_publishForceThreshold = _rtForceThreshold;
_rtPosController->setTarget(_rtTcp->getPoseInRootFrame(), _rtElbow->getPositionInRootFrame());
@@ -141,39 +172,122 @@ namespace armarx
{
auto& rt2nonrtBuf = _tripRt2NonRt.getWriteBuffer();
- if (_tripBufPosCtrl.updateReadBuffer())
+ if (_tripBufCfg.updateReadBuffer())
{
- ARMARX_RT_LOGF_IMPORTANT("updates in tripple buffer");
- auto& r = _tripBufPosCtrl._getNonConstReadBuffer();
- if (r.cfgUpdated)
+ ARMARX_RT_LOGF_IMPORTANT("updates in tripple buffer cfg");
+ auto& r = _tripBufCfg._getNonConstReadBuffer();
+ if (r.updated)
{
- r.cfgUpdated = false;
+ r.updated = false;
_rtPosController->setConfig(r.cfg);
ARMARX_RT_LOGF_IMPORTANT("updated the controller config");
}
- if (r.targetUpdated)
+ }
+ if (_tripBufTarget.updateReadBuffer())
+ {
+ ARMARX_RT_LOGF_IMPORTANT("updates in tripple buffer target");
+ auto& r = _tripBufTarget._getNonConstReadBuffer();
+ if (r.updated)
{
- r.targetUpdated = false;
+ r.updated = false;
+ _rtSetOrientation = r.setOrientation;
_rtPosController->setTarget(r.tcpTarget, r.elbowTarget);
+ _rtStopConditionReached = false;
+ _publishIsAtForceLimit = false;
}
}
+ if (_tripBufNullspaceTarget.updateReadBuffer())
+ {
+ ARMARX_RT_LOGF_IMPORTANT("updates in tripple buffer NullspaceTarget");
+ auto& r = _tripBufNullspaceTarget._getNonConstReadBuffer();
+ if (r.updated)
+ {
+ r.updated = false;
+ if (r.clearRequested)
+ {
+ _rtPosController->clearNullspaceTarget();
+ }
+ else
+ {
+ ARMARX_RT_LOGF_IMPORTANT("_rtPosController->setNullspaceTarget");
+ _rtPosController->setNullspaceTarget(r.nullspaceTarget);
+ }
+ r.clearRequested = false;
+
+ }
+ }
+ if (_tripBufFFvel.updateReadBuffer())
+ {
+ _rtFFvel = _tripBufFFvel.getReadBuffer();
+ _rtFFvelLastUpdateMS = sensorValuesTimestamp.toMilliSeconds();
+ }
+ if (_tripBufFToffset.updateReadBuffer())
+ {
+ _rtFTOffset = _tripBufFToffset.getReadBuffer();
+ }
+ if (_tripBufFTlimit.updateReadBuffer())
+ {
+ _rtFTlimit = _tripBufFTlimit.getReadBuffer();
+ if (_rtFTlimit.force < 0 && _rtFTlimit.torque < 0)
+ {
+ _publishIsAtForceLimit = false;
+ }
+ }
+ if (_tripBufFTfake.updateReadBuffer())
+ {
+ _rtFTfake = _tripBufFTfake.getReadBuffer();
+ }
+
//run
//bool reachedTarget = false;
//bool reachedFtLimit = false;
if (_rtForceSensor)
{
- const Eigen::Vector3f ftInRoot =
+ /*const Eigen::Vector3f ftInRoot =
_rtFT->getTransformationTo(_rtRobotRoot)
.topLeftCorner<3, 3>()
.transpose() *
- (*_rtForceSensor);
+ (*_rtForceSensor);*/
//rt2nonrtBuf.ft.force = ftInRoot;
//rt2nonrtBuf.ft.torque = *_rtTorqueSensor;
//rt2nonrtBuf.ft.timestampInUs = sensorValuesTimestamp.toMicroSeconds();
- rt2nonrtBuf.ftInRoot = _rtFT->getPoseInRootFrame();
+ //rt2nonrtBuf.ftInRoot = _rtFT->getPoseInRootFrame();
+
+ FTval ft;
+ ft.force = *_rtForceSensor;
+ ft.torque = *_rtTorqueSensor;
+
+ if (_rtFTfake.use)
+ {
+ ft.force = _rtFTfake.force;
+ ft.torque = _rtFTfake.torque;
+ ARMARX_RT_LOGF("Using fake ft values");
+ }
+
+ _rtFTHistory.at(_rtFTHistoryIndex) = ft;
+ _rtFTHistoryIndex = (_rtFTHistoryIndex + 1) % _rtFTHistory.size();
- Eigen::Vector3f ftVal = ftInRoot;
+ FTval ftAvg;
+ for (size_t i = 0; i < _rtFTHistory.size(); i++)
+ {
+ ftAvg.force += _rtFTHistory.at(i).force;
+ ftAvg.torque += _rtFTHistory.at(i).torque;
+ }
+ ftAvg.force = ftAvg.force / _rtFTHistory.size();
+ ftAvg.torque = ftAvg.torque / _rtFTHistory.size();
+
+
+ rt2nonrtBuf.currentFT = ft;
+ rt2nonrtBuf.averageFT = ftAvg;
+
+ if ((_rtFTlimit.force > 0 && ft.force.norm() > _rtFTlimit.force) || (_rtFTlimit.torque > 0 && ft.torque.norm() > _rtFTlimit.torque))
+ {
+ _rtStopConditionReached = true;
+ _publishIsAtForceLimit = true;
+ }
+
+ //Eigen::Vector3f ftVal = ftInRoot;
//if (_rtForceThresholdInRobotRootZ)
//{
// ftVal(0) = 0;
@@ -185,7 +299,7 @@ namespace armarx
// _setFTOffset = false;
// _publishIsAtForceLimit = false;
//}
- rt2nonrtBuf.ftUsed = ftVal;
+ //rt2nonrtBuf.ftUsed = ftVal;
//const float currentForce = std::abs(ftVal.norm() - _rtForceOffset.norm());
@@ -198,8 +312,42 @@ namespace armarx
rt2nonrtBuf.rootPose = _rtRobot->getGlobalPose();
rt2nonrtBuf.tcp = _rtTcp->getPoseInRootFrame();
+ rt2nonrtBuf.elb = _rtElbow->getPoseInRootFrame();
+ rt2nonrtBuf.tcpTarg = _rtPosController->getCurrentTarget();
+ rt2nonrtBuf.elbTarg = math::Helpers::CreatePose(_rtPosController->getCurrentElbowTarget(), Eigen::Matrix3f::Identity());
+
+
+ if (_rtStopConditionReached)
+ {
+ setNullVelocity();
+ }
+ else
{
- const Eigen::VectorXf& goal = _rtPosController->calculate(timeSinceLastIteration.toSecondsDouble());
+ VirtualRobot::IKSolver::CartesianSelection mode = _rtSetOrientation ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position;
+ _rtPosController->setNullSpaceControl(_nullspaceControlEnabled);
+
+ if (_rtFFvel.use)
+ {
+ if (_rtFFvelLastUpdateMS + _rtFFvelMaxAgeMS > sensorValuesTimestamp.toMilliSeconds())
+ {
+ _rtPosController->setFeedForwardVelocity(_rtFFvel.feedForwardVelocity);
+ }
+ else
+ {
+ ARMARX_RT_LOGF_WARN("Feed forward velocity cleared due to timeout");
+ _rtFFvel.use = false;
+ }
+ }
+
+ const Eigen::VectorXf& goal = _rtPosController->calculate(timeSinceLastIteration.toSecondsDouble(), mode);
+
+ const Eigen::VectorXf actualTcpVel = _rtPosController->actualTcpVel(goal);
+ const Eigen::VectorXf actualElbVel = _rtPosController->actualElbVel(goal);
+ //ARMARX_IMPORTANT << VAROUT(actualTcpVel) << VAROUT(actualElbVel);
+ _publish.tcpPosVel = actualTcpVel.block<3, 1>(0, 0).norm();
+ _publish.tcpOriVel = actualTcpVel.block<3, 1>(3, 0).norm();
+ _publish.elbPosVel = actualElbVel.block<3, 1>(0, 0).norm();
+
//write targets
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(goal.size()), _rtVelTargets.size());
for (const auto& [idx, ptr] : MakeIndexedContainer(_rtVelTargets))
@@ -215,38 +363,67 @@ namespace armarx
}
- bool NJointCartesianNaturalPositionController::hasReachedTarget(const Ice::Current&)
- {
- return _publishIsAtTarget;
- }
void NJointCartesianNaturalPositionController::setConfig(const CartesianNaturalPositionControllerConfig& cfg, const Ice::Current&)
{
- std::lock_guard g{_tripBufPosCtrlMut};
- auto& w = _tripBufPosCtrl.getWriteBuffer();
- w.targetUpdated = false;
- w.cfgUpdated = true;
- w.feedForwardVelocityUpdated = false;
+ std::lock_guard g{_mutexSetTripBuf};
+ auto& w = _tripBufCfg.getWriteBuffer();
w.cfg = cfg;
- _tripBufPosCtrl.commitWrite();
+ w.updated = true;
+ _tripBufCfg.commitWrite();
ARMARX_IMPORTANT << "set new config\n" << cfg;
}
- void NJointCartesianNaturalPositionController::setTarget(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& elbowTarget, const Ice::Current&)
+ void NJointCartesianNaturalPositionController::setTarget(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& elbowTarget, bool setOrientation, const Ice::Current&)
{
- std::lock_guard g{_tripBufPosCtrlMut};
- auto& w = _tripBufPosCtrl.getWriteBuffer();
- w.targetUpdated = true;
- w.cfgUpdated = false;
- w.feedForwardVelocityUpdated = false;
+ std::lock_guard g{_mutexSetTripBuf};
+ auto& w = _tripBufTarget.getWriteBuffer();
w.tcpTarget = tcpTarget;
w.elbowTarget = elbowTarget;
- _tripBufPosCtrl.commitWrite();
+ w.setOrientation = setOrientation;
+ w.updated = true;
+ _tripBufTarget.commitWrite();
ARMARX_IMPORTANT << "set new target\n" << tcpTarget << "\nelbow: " << elbowTarget.transpose();
}
+ void NJointCartesianNaturalPositionController::setTargetFeedForward(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& elbowTarget, bool setOrientation, const Eigen::Vector6f& ffVel, const Ice::Current&)
+ {
+ std::lock_guard g{_mutexSetTripBuf};
+ {
+ auto& w = _tripBufTarget.getWriteBuffer();
+ w.tcpTarget = tcpTarget;
+ w.elbowTarget = elbowTarget;
+ w.setOrientation = setOrientation;
+ w.updated = true;
+ }
+ {
+ auto& w = _tripBufFFvel.getWriteBuffer();
+ w.feedForwardVelocity = ffVel;
+ w.updated = true;
+ }
+ _tripBufFFvel.commitWrite();
+ _tripBufTarget.commitWrite();
+ }
+
void NJointCartesianNaturalPositionController::setFeedForwardVelocity(const Eigen::Vector6f& vel, const Ice::Current&)
{
+ std::lock_guard g{_mutexSetTripBuf};
+ auto& w = _tripBufFFvel.getWriteBuffer();
+ w.feedForwardVelocity = vel;
+ w.use = true;
+ w.updated = true;
+ _tripBufFFvel.commitWrite();
+ ARMARX_IMPORTANT << "set new FeedForwardVelocity\n" << vel.transpose();
+ }
+ void NJointCartesianNaturalPositionController::clearFeedForwardVelocity(const Ice::Current&)
+ {
+ std::lock_guard g{_mutexSetTripBuf};
+ auto& w = _tripBufFFvel.getWriteBuffer();
+ w.feedForwardVelocity = Eigen::Vector6f::Zero();
+ w.use = false;
+ w.updated = true;
+ _tripBufFFvel.commitWrite();
+ ARMARX_IMPORTANT << "cleared FeedForwardVelocity";
}
void NJointCartesianNaturalPositionController::setNullVelocity()
{
@@ -262,22 +439,161 @@ namespace armarx
// return _publishIsAtForceLimit;
//}
+ void NJointCartesianNaturalPositionController::setVisualizationRobotGlobalPose(const Eigen::Matrix4f& p, const Ice::Current&)
+ {
+ std::lock_guard g{_tripFakeRobotGPWriteMutex};
+ _tripFakeRobotGP.getWriteBuffer() = p;
+ _tripFakeRobotGP.commitWrite();
+ }
+
+ void NJointCartesianNaturalPositionController::resetVisualizationRobotGlobalPose(const Ice::Current&)
+ {
+ std::lock_guard g{_tripFakeRobotGPWriteMutex};
+ _tripFakeRobotGP.getWriteBuffer()(0, 0) = std::nanf("");
+ _tripFakeRobotGP.commitWrite();
+ }
+
void NJointCartesianNaturalPositionController::stopMovement(const Ice::Current&)
{
_stopNowRequested = true;
}
+ void NJointCartesianNaturalPositionController::setNullspaceTarget(const Ice::FloatSeq& nullspaceTarget, const Ice::Current&)
+ {
+ std::lock_guard g{_mutexSetTripBuf};
+ auto& w = _tripBufNullspaceTarget.getWriteBuffer();
+ w.nullspaceTarget = nullspaceTarget;
+ w.clearRequested = false;
+ w.updated = true;
+ _tripBufNullspaceTarget.commitWrite();
+ ARMARX_IMPORTANT << "set new nullspaceTarget\n" << nullspaceTarget;
+ }
+
+ void NJointCartesianNaturalPositionController::clearNullspaceTarget(const Ice::Current&)
+ {
+ std::lock_guard g{_mutexSetTripBuf};
+ auto& w = _tripBufNullspaceTarget.getWriteBuffer();
+ w.clearRequested = true;
+ w.updated = true;
+ _tripBufNullspaceTarget.commitWrite();
+ ARMARX_IMPORTANT << "reset nullspaceTarget";
+ }
+
+ void NJointCartesianNaturalPositionController::setNullspaceControlEnabled(bool enabled, const Ice::Current&)
+ {
+ _nullspaceControlEnabled = enabled;
+ }
+
+ FTSensorValue NJointCartesianNaturalPositionController::frost(const FTval& ft)
+ {
+ FTSensorValue r;
+ r.force = ft.force;
+ r.torque = ft.torque;
+ return r;
+ }
+
+ FTSensorValue NJointCartesianNaturalPositionController::getCurrentFTValue(const Ice::Current&)
+ {
+ std::lock_guard g{_tripRt2NonRtMutex};
+ return frost(_tripRt2NonRt.getUpToDateReadBuffer().currentFT);
+ }
+
+ FTSensorValue NJointCartesianNaturalPositionController::getAverageFTValue(const Ice::Current&)
+ {
+ std::lock_guard g{_tripRt2NonRtMutex};
+ return frost(_tripRt2NonRt.getUpToDateReadBuffer().averageFT);
+ }
+
+ void NJointCartesianNaturalPositionController::setFTOffset(const FTSensorValue& offset, const Ice::Current&)
+ {
+ std::lock_guard g{_mutexSetTripBuf};
+ auto& w = _tripBufFToffset.getWriteBuffer();
+ w.force = offset.force;
+ w.torque = offset.torque;
+ w.updated = true;
+ _tripBufFToffset.commitWrite();
+ ARMARX_IMPORTANT << "set FT offset:\n" << offset.force.transpose() << "\n" << offset.torque.transpose();
+ }
+ void NJointCartesianNaturalPositionController::resetFTOffset(const Ice::Current&)
+ {
+ std::lock_guard g{_mutexSetTripBuf};
+ auto& w = _tripBufFToffset.getWriteBuffer();
+ w.force = Eigen::Vector3f::Zero();
+ w.torque = Eigen::Vector3f::Zero();
+ w.updated = true;
+ _tripBufFToffset.commitWrite();
+ ARMARX_IMPORTANT << "reset FT offset";
+ }
+
+
+
+ void NJointCartesianNaturalPositionController::setFTLimit(float force, float torque, const Ice::Current&)
+ {
+ std::lock_guard g{_mutexSetTripBuf};
+ auto& w = _tripBufFTlimit.getWriteBuffer();
+ w.force = force;
+ w.torque = torque;
+ w.updated = true;
+ _tripBufFTlimit.commitWrite();
+ ARMARX_IMPORTANT << "set FT limit:" << force << " " << torque;
+ }
+
+ void NJointCartesianNaturalPositionController::clearFTLimit(const Ice::Current&)
+ {
+ std::lock_guard g{_mutexSetTripBuf};
+ auto& w = _tripBufFTlimit.getWriteBuffer();
+ w.force = -1;
+ w.torque = -1;
+ w.updated = true;
+ _tripBufFTlimit.commitWrite();
+ ARMARX_IMPORTANT << "reset FT limit";
+ }
+
+ void NJointCartesianNaturalPositionController::setFakeFTValue(const FTSensorValue& ftValue, const Ice::Current&)
+ {
+ std::lock_guard g{_mutexSetTripBuf};
+ auto& w = _tripBufFTfake.getWriteBuffer();
+ w.force = ftValue.force;
+ w.torque = ftValue.torque;
+ w.use = true;
+ w.updated = true;
+ _tripBufFTfake.commitWrite();
+ ARMARX_IMPORTANT << "set fake FT value:\n" << ftValue.force.transpose() << "\n" << ftValue.torque.transpose();
+
+ }
+
+ void NJointCartesianNaturalPositionController::clearFakeFTValue(const Ice::Current&)
+ {
+ std::lock_guard g{_mutexSetTripBuf};
+ auto& w = _tripBufFTfake.getWriteBuffer();
+ w.force = Eigen::Vector3f::Zero();
+ w.torque = Eigen::Vector3f::Zero();
+ w.use = false;
+ w.updated = true;
+ _tripBufFTfake.commitWrite();
+ ARMARX_IMPORTANT << "cleared fake FT value";
+ }
+
+ bool NJointCartesianNaturalPositionController::isAtForceLimit(const Ice::Current&)
+ {
+ ARMARX_CHECK_NOT_NULL(_rtForceSensor);
+ return _publishIsAtForceLimit;
+ }
+
void NJointCartesianNaturalPositionController::onPublish(
const SensorAndControl&,
const DebugDrawerInterfacePrx& drawer,
const DebugObserverInterfacePrx& obs)
{
- const float errorPos = _publishErrorPos;
- const float errorOri = _publishErrorOri;
- const float errorPosMax = _publishErrorPosMax;
- const float errorOriMax = _publishErrorOriMax;
+ const float errorPos = _publish.errorPos;
+ const float errorOri = _publish.errorOri;
+ const float errorPosMax = _publish.errorPosMax;
+ const float errorOriMax = _publish.errorOriMax;
//const float forceThreshold = _publishForceThreshold;
//const float forceCurrent = _publishForceCurrent;
+ const float tcpPosVel = _publish.tcpPosVel;
+ const float tcpOriVel = _publish.tcpOriVel;
+ const float elbPosVel = _publish.elbPosVel;
{
StringVariantBaseMap datafields;
@@ -287,6 +603,10 @@ namespace armarx
datafields["errorPosMax"] = new Variant(errorPosMax);
datafields["errorOriMax"] = new Variant(errorOriMax);
+ datafields["tcpPosVel"] = new Variant(tcpPosVel);
+ datafields["tcpOriVel"] = new Variant(tcpOriVel);
+ datafields["elbPosVel"] = new Variant(elbPosVel);
+
//datafields["forceThreshold"] = new Variant(forceThreshold);
//datafields["forceCurrent"] = new Variant(forceCurrent);
@@ -309,6 +629,8 @@ namespace armarx
{
const Eigen::Matrix4f gtcp = gp * buf.tcp;
const Eigen::Matrix4f gtcptarg = gp * buf.tcpTarg;
+ const Eigen::Matrix4f gelb = gp * buf.elb;
+ const Eigen::Matrix4f gelbtarg = gp * buf.elbTarg;
drawer->setPoseVisu(getName(), "tcp", new Pose(gtcp));
drawer->setPoseVisu(getName(), "tcptarg", new Pose(gtcptarg));
drawer->setLineVisu(
@@ -316,6 +638,11 @@ namespace armarx
new Vector3(Eigen::Vector3f{gtcp.topRightCorner<3, 1>()}),
new Vector3(Eigen::Vector3f{gtcptarg.topRightCorner<3, 1>()}),
3, {0, 0, 1, 1});
+ drawer->setLineVisu(
+ getName(), "elb2targ",
+ new Vector3(Eigen::Vector3f{gelb.topRightCorner<3, 1>()}),
+ new Vector3(Eigen::Vector3f{gelbtarg.topRightCorner<3, 1>()}),
+ 3, {0, 0, 1, 1});
}
else
{
@@ -341,3 +668,5 @@ namespace armarx
}
+
+
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
index e102409abd5b317120616353a52f993454fd0e07..2a59d6d7dd6481df81fe6299e7ab2d9d598f9e7c 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
@@ -37,29 +37,87 @@ namespace armarx
void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
public:
- bool hasReachedTarget(const Ice::Current& = Ice::emptyCurrent) override;
+ //bool hasReachedTarget(const Ice::Current& = Ice::emptyCurrent) override;
//bool hasReachedForceLimit(const Ice::Current& = Ice::emptyCurrent) override;
void setConfig(const CartesianNaturalPositionControllerConfig& cfg, const Ice::Current& = Ice::emptyCurrent) override;
- void setTarget(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& elbowTarget, const Ice::Current& = Ice::emptyCurrent) override;
+ void setTarget(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& elbowTarget, bool setOrientation, const Ice::Current& = Ice::emptyCurrent) override;
+ void setTargetFeedForward(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& elbowTarget, bool setOrientation, const Eigen::Vector6f& ffVel, const Ice::Current&) override;
void setFeedForwardVelocity(const Eigen::Vector6f& vel, const Ice::Current&) override;
+ void clearFeedForwardVelocity(const Ice::Current&) override;
+ void setNullspaceTarget(const Ice::FloatSeq& nullspaceTarget, const Ice::Current&) override;
+ void clearNullspaceTarget(const Ice::Current&) override;
+ void setNullspaceControlEnabled(bool enabled, const Ice::Current&) override;
+
+ FTSensorValue getCurrentFTValue(const Ice::Current&) override;
+ FTSensorValue getAverageFTValue(const Ice::Current&) override;
+ void setFTOffset(const FTSensorValue& offset, const Ice::Current&) override;
+ void resetFTOffset(const Ice::Current&) override;
+ void setFTLimit(float force, float torque, const Ice::Current&) override;
+ void clearFTLimit(const Ice::Current&) override;
+ void setFakeFTValue(const FTSensorValue& ftValue, const Ice::Current&) override;
+ void clearFakeFTValue(const Ice::Current&) override;
+ bool isAtForceLimit(const Ice::Current&) override;
+
void stopMovement(const Ice::Current& = Ice::emptyCurrent) override;
+ void setVisualizationRobotGlobalPose(const Eigen::Matrix4f& p, const Ice::Current& = Ice::emptyCurrent) override;
+ void resetVisualizationRobotGlobalPose(const Ice::Current& = Ice::emptyCurrent) override;
+
protected:
void rtPreActivateController() override;
void rtPostDeactivateController() override;
//structs
private:
- struct CtrlData
+
+ struct TB_Target
{
Eigen::Matrix4f tcpTarget;
Eigen::Vector3f elbowTarget;
- Eigen::Vector6f feedForwardVelocity;
+ bool setOrientation;
+ bool updated = false;
+ };
+ struct TB_FFvel
+ {
+ Eigen::Vector6f feedForwardVelocity = Eigen::Vector6f::Zero();
+ bool use = false;
+ bool updated = false;
+ };
+ struct TB_Cfg
+ {
CartesianNaturalPositionControllerConfig cfg;
- bool targetUpdated = false;
- bool cfgUpdated = false;
- bool feedForwardVelocityUpdated = false;
+ bool updated = false;
+ };
+ struct TB_NT
+ {
+ std::vector<float> nullspaceTarget;
+ bool clearRequested = false;
+ bool updated = false;
+ };
+ struct TB_FT
+ {
+ Eigen::Vector3f force = Eigen::Vector3f::Zero();
+ Eigen::Vector3f torque = Eigen::Vector3f::Zero();
+ bool updated = false;
+ };
+ struct TB_FTlimit
+ {
+ float force = -1;
+ float torque = -1;
+ bool updated = false;
+ };
+ struct TB_FTfake
+ {
+ Eigen::Vector3f force = Eigen::Vector3f::Zero();
+ Eigen::Vector3f torque = Eigen::Vector3f::Zero();
+ bool use = false;
+ bool updated = false;
+ };
+ struct FTval
+ {
+ Eigen::Vector3f force = Eigen::Vector3f::Zero();
+ Eigen::Vector3f torque = Eigen::Vector3f::Zero();
};
struct RtToNonRtData
@@ -67,13 +125,32 @@ namespace armarx
Eigen::Matrix4f rootPose = Eigen::Matrix4f::Identity();
Eigen::Matrix4f tcp = Eigen::Matrix4f::Identity();
Eigen::Matrix4f tcpTarg = Eigen::Matrix4f::Identity();
- Eigen::Matrix4f ftInRoot = Eigen::Matrix4f::Identity();
- Eigen::Vector3f ftUsed = Eigen::Vector3f::Zero();
+ Eigen::Matrix4f elb = Eigen::Matrix4f::Identity();
+ Eigen::Matrix4f elbTarg = Eigen::Matrix4f::Identity();
+
+ FTval currentFT;
+ FTval averageFT;
+
+ //Eigen::Matrix4f ftInRoot = Eigen::Matrix4f::Identity();
+ //Eigen::Vector3f ftUsed = Eigen::Vector3f::Zero();
+ };
+
+ struct PublishData
+ {
+ std::atomic<float> errorPos{0};
+ std::atomic<float> errorOri{0};
+ std::atomic<float> errorPosMax{0};
+ std::atomic<float> errorOriMax{0};
+ std::atomic<float> tcpPosVel{0};
+ std::atomic<float> tcpOriVel{0};
+ std::atomic<float> elbPosVel{0};
+ //std::atomic<bool> targetReached;
};
//data
private:
void setNullVelocity();
+ FTSensorValue frost(const FTval& ft);
using Ctrl = CartesianNaturalPositionController;
@@ -86,7 +163,11 @@ namespace armarx
VirtualRobot::RobotNodePtr _rtRobotRoot;
std::unique_ptr<Ctrl> _rtPosController;
+ bool _rtSetOrientation = true;
Eigen::VectorXf _rtJointVelocityFeedbackBuffer;
+ TB_FFvel _rtFFvel;
+ int _rtFFvelMaxAgeMS;
+ long _rtFFvelLastUpdateMS = 0;
std::vector<const float*> _rtVelSensors;
std::vector<float*> _rtVelTargets;
@@ -94,21 +175,37 @@ namespace armarx
const Eigen::Vector3f* _rtForceSensor = nullptr;
const Eigen::Vector3f* _rtTorqueSensor = nullptr;
+ std::vector<FTval> _rtFTHistory;
+ size_t _rtFTHistoryIndex = 0;
+
+ TB_FT _rtFTOffset;
+ TB_FTlimit _rtFTlimit;
+ TB_FTfake _rtFTfake;
+
//Eigen::Vector3f _rtForceOffset;
//float _rtForceThreshold = -1;
- //bool _rtStopConditionReached = false;
+ bool _rtStopConditionReached = false;
+
+ std::atomic_bool _nullspaceControlEnabled{true};
//flags
- std::atomic_bool _publishIsAtTarget{false};
- //std::atomic_bool _publishIsAtForceLimit{false};
+ //std::atomic_bool _publishIsAtTarget{false};
+ std::atomic_bool _publishIsAtForceLimit{false};
//std::atomic_bool _setFTOffset{false};
std::atomic_bool _stopNowRequested{false};
//buffers
- mutable std::recursive_mutex _tripBufPosCtrlMut;
- TripleBuffer<CtrlData> _tripBufPosCtrl;
- TripleBuffer<Eigen::Matrix4f> _tripBufTarget;
+ mutable std::recursive_mutex _mutexSetTripBuf;
+ //TripleBuffer<CtrlData> _tripBufPosCtrl;
+ TripleBuffer<TB_Target> _tripBufTarget;
+ TripleBuffer<TB_NT> _tripBufNullspaceTarget;
+ TripleBuffer<TB_FFvel> _tripBufFFvel;
+ TripleBuffer<TB_Cfg> _tripBufCfg;
+ TripleBuffer<TB_FT> _tripBufFToffset;
+ TripleBuffer<TB_FTlimit> _tripBufFTlimit;
+ TripleBuffer<TB_FTfake> _tripBufFTfake;
+
mutable std::recursive_mutex _tripRt2NonRtMutex;
TripleBuffer<RtToNonRtData> _tripRt2NonRt;
@@ -117,13 +214,13 @@ namespace armarx
TripleBuffer<Eigen::Matrix4f> _tripFakeRobotGP;
//publish data
- std::atomic<float> _publishErrorPos{0};
- std::atomic<float> _publishErrorOri{0};
- std::atomic<float> _publishErrorPosMax{0};
- std::atomic<float> _publishErrorOriMax{0};
+ PublishData _publish;
+
//std::atomic<float> _publishForceThreshold{0};
//std::atomic<float> _publishForceCurrent{0};
//std::atomic_bool _publishForceThresholdInRobotRootZ{0};
+
+
};
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
index 6d91aee3547bb27a2818de55270774c786be3bce..cb80274fe3e8e0fb67715fcc887576afb9fba8a2 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
@@ -78,8 +78,6 @@ NJointTaskSpaceImpedanceController::NJointTaskSpaceImpedanceController(const Rob
desiredJointPosition(i) = cfg->desiredJointPositions.at(i);
}
- knull = cfg->Knull;
- dnull = cfg->Dnull;
ARMARX_CHECK_EQUAL(cfg->desiredPoseVec.size(), 7);
@@ -104,8 +102,18 @@ NJointTaskSpaceImpedanceController::NJointTaskSpaceImpedanceController(const Rob
Kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
Eigen::Vector3f Dori;
Dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
- Eigen::Vector3f Knull;
+ ARMARX_CHECK_EQUAL(cfg->Knull.size(), 8);
+ ARMARX_CHECK_EQUAL(cfg->Dnull.size(), 8);
+ Eigen::VectorXf Knull;
+ Eigen::VectorXf Dnull;
+ Knull.setZero(8);
+ Dnull.setZero(8);
+ for (size_t i = 0; i < 8; ++i)
+ {
+ Knull[i] = cfg->Knull.at(i);
+ Dnull[i] = cfg->Dnull.at(i);
+ }
NJointTaskSpaceImpedanceControllerControlData initData;
initData.desiredOrientation = initDesiredOrientation;
@@ -114,6 +122,8 @@ NJointTaskSpaceImpedanceController::NJointTaskSpaceImpedanceController(const Rob
initData.Dpos = Dpos;
initData.Kori = Kori;
initData.Dori = Dori;
+ initData.Knull = Knull;
+ initData.Dnull = Dnull;
reinitTripleBuffer(initData);
}
@@ -151,6 +161,8 @@ void NJointTaskSpaceImpedanceController::rtRun(const IceUtil::Time& sensorValues
Eigen::Vector3f Dpos = rtGetControlStruct().Dpos;
Eigen::Vector3f Kori = rtGetControlStruct().Kori;
Eigen::Vector3f Dori = rtGetControlStruct().Dori;
+ Eigen::VectorXf Knull = rtGetControlStruct().Knull;
+ Eigen::VectorXf Dnull = rtGetControlStruct().Dnull;
Eigen::Vector3f currentTCPPosition = tcp->getPositionInRootFrame();
@@ -184,7 +196,7 @@ void NJointTaskSpaceImpedanceController::rtRun(const IceUtil::Time& sensorValues
nullqerror(i) = 0;
}
}
- Eigen::VectorXf nullspaceTorque = knull * nullqerror - dnull * qvel;
+ Eigen::VectorXf nullspaceTorque = Knull.cwiseProduct(nullqerror) - Dnull.cwiseProduct(qvel);
Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * tcpDesiredWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(jointDesiredTorques.rows()), targets.size());
@@ -273,31 +285,78 @@ void NJointTaskSpaceImpedanceController::setOrientation(const Ice::FloatSeq& tar
void NJointTaskSpaceImpedanceController::setImpedanceParameters(const std::string& name, const Ice::FloatSeq& vals, const Ice::Current&)
{
- ARMARX_CHECK_EQUAL(vals.size(), 3);
- Eigen::Vector3f vec;
- for (size_t i = 0; i < 3; ++i)
- {
- vec(i) = vals.at(i);
- }
LockGuardType guard {controlDataMutex};
if (name == "Kpos")
{
+ ARMARX_CHECK_EQUAL(vals.size(), 3);
+
+ Eigen::Vector3f vec;
+ for (size_t i = 0; i < 3; ++i)
+ {
+ vec(i) = vals.at(i);
+ }
getWriterControlStruct().Kpos = vec;
}
else if (name == "Kori")
{
+ ARMARX_CHECK_EQUAL(vals.size(), 3);
+
+ Eigen::Vector3f vec;
+ for (size_t i = 0; i < 3; ++i)
+ {
+ vec(i) = vals.at(i);
+ }
getWriterControlStruct().Kori = vec;
}
else if (name == "Dpos")
{
+ ARMARX_CHECK_EQUAL(vals.size(), 3);
+
+ Eigen::Vector3f vec;
+ for (size_t i = 0; i < 3; ++i)
+ {
+ vec(i) = vals.at(i);
+ }
getWriterControlStruct().Dpos = vec;
}
else if (name == "Dori")
{
+ ARMARX_CHECK_EQUAL(vals.size(), 3);
+
+ Eigen::Vector3f vec;
+ for (size_t i = 0; i < 3; ++i)
+ {
+ vec(i) = vals.at(i);
+ }
getWriterControlStruct().Dori = vec;
}
+ else if (name == "Knull")
+ {
+ ARMARX_CHECK_EQUAL(vals.size(), 8);
+
+ Eigen::VectorXf vec;
+ vec.setZero(8);
+ for (size_t i = 0; i < 8; ++i)
+ {
+ vec(i) = vals.at(i);
+ }
+ getWriterControlStruct().Knull = vec;
+ }
+ else if (name == "Dnull")
+ {
+ ARMARX_CHECK_EQUAL(vals.size(), 8);
+
+ Eigen::VectorXf vec;
+ vec.setZero(8);
+ for (size_t i = 0; i < 8; ++i)
+ {
+ vec(i) = vals.at(i);
+ }
+ getWriterControlStruct().Dnull = vec;
+ }
+
writeControlStruct();
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
index d6ad7b0625f26928c54dcb1e4c64933fad493513..fd5cea5dff98a6207ef9d28fd41faf4a98d426f4 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
@@ -43,6 +43,8 @@ namespace armarx
Eigen::Vector3f Dpos;
Eigen::Vector3f Kori;
Eigen::Vector3f Dori;
+ Eigen::VectorXf Knull;
+ Eigen::VectorXf Dnull;
};
/**
* @defgroup Library-NJointTaskSpaceImpedanceController NJointTaskSpaceImpedanceController
@@ -83,7 +85,6 @@ namespace armarx
Eigen::VectorXf desiredJointPosition;
- float knull, dnull;
VirtualRobot::RobotNodePtr tcp;
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidget.ui b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidget.ui
index 377459cf2064309997ab6defca16213164a164a6..998d97de62e1a976636ce4ca531cb03f396126f7 100644
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidget.ui
+++ b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidget.ui
@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>400</width>
- <height>300</height>
+ <width>767</width>
+ <height>493</height>
</rect>
</property>
<property name="windowTitle">
@@ -18,7 +18,7 @@
<rect>
<x>20</x>
<y>20</y>
- <width>129</width>
+ <width>191</width>
<height>101</height>
</rect>
</property>
@@ -42,102 +42,388 @@
</item>
</layout>
</widget>
- <widget class="QWidget" name="widget_2" native="true">
+ <widget class="QGroupBox" name="groupBox_2">
<property name="geometry">
<rect>
<x>20</x>
<y>140</y>
- <width>301</width>
- <height>70</height>
+ <width>321</width>
+ <height>181</height>
</rect>
</property>
+ <property name="title">
+ <string>Control target</string>
+ </property>
<layout class="QGridLayout" name="gridLayout_2">
- <item row="1" column="4">
- <widget class="QPushButton" name="pushButtonRyn">
+ <item row="2" column="3">
+ <widget class="QPushButton" name="pushButtonRxp">
<property name="text">
- <string>ry-</string>
+ <string>rx+</string>
</property>
</widget>
</item>
- <item row="0" column="2">
- <widget class="QPushButton" name="pushButtonPzp">
+ <item row="2" column="5">
+ <widget class="QPushButton" name="pushButtonRzp">
<property name="text">
- <string>pz+</string>
+ <string>rz+</string>
</property>
</widget>
</item>
- <item row="0" column="3">
- <widget class="QPushButton" name="pushButtonRxp">
+ <item row="2" column="4">
+ <widget class="QPushButton" name="pushButtonRyp">
<property name="text">
- <string>rx+</string>
+ <string>ry+</string>
</property>
</widget>
</item>
- <item row="0" column="1">
- <widget class="QPushButton" name="pushButtonPyp">
- <property name="text">
- <string>py+</string>
- </property>
+ <item row="1" column="3" colspan="3">
+ <widget class="QWidget" name="widget_3" native="true">
+ <layout class="QGridLayout" name="gridLayout_4">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <property name="verticalSpacing">
+ <number>0</number>
+ </property>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_3">
+ <property name="text">
+ <string>dOri</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QSpinBox" name="spinBoxDori">
+ <property name="value">
+ <number>10</number>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <spacer name="horizontalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
</widget>
</item>
- <item row="1" column="3">
+ <item row="3" column="3">
<widget class="QPushButton" name="pushButtonRxn">
<property name="text">
<string>rx-</string>
</property>
</widget>
</item>
- <item row="1" column="1">
+ <item row="1" column="0" colspan="3">
+ <widget class="QWidget" name="widget_2" native="true">
+ <layout class="QGridLayout" name="gridLayout_3">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <property name="verticalSpacing">
+ <number>0</number>
+ </property>
+ <item row="0" column="1">
+ <widget class="QSpinBox" name="spinBoxDpos">
+ <property name="maximum">
+ <number>500</number>
+ </property>
+ <property name="value">
+ <number>100</number>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string>dPos</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <spacer name="horizontalSpacer">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item row="0" column="3" colspan="3">
+ <widget class="QCheckBox" name="checkBoxSetOri">
+ <property name="text">
+ <string>Set Orientation</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QPushButton" name="pushButtonPxn">
+ <property name="text">
+ <string>px-</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
<widget class="QPushButton" name="pushButtonPyn">
<property name="text">
<string>py-</string>
</property>
</widget>
</item>
- <item row="1" column="5">
+ <item row="3" column="5">
<widget class="QPushButton" name="pushButtonRzn">
<property name="text">
<string>rz-</string>
</property>
</widget>
</item>
- <item row="0" column="5">
- <widget class="QPushButton" name="pushButtonRzp">
+ <item row="3" column="2">
+ <widget class="QPushButton" name="pushButtonPzn">
<property name="text">
- <string>rz+</string>
+ <string>pz-</string>
</property>
</widget>
</item>
- <item row="0" column="0">
+ <item row="2" column="1">
+ <widget class="QPushButton" name="pushButtonPyp">
+ <property name="text">
+ <string>py+</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="4">
+ <widget class="QPushButton" name="pushButtonRyn">
+ <property name="text">
+ <string>ry-</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
<widget class="QPushButton" name="pushButtonPxp">
<property name="text">
<string>px+</string>
</property>
</widget>
</item>
- <item row="1" column="2">
- <widget class="QPushButton" name="pushButtonPzn">
+ <item row="2" column="2">
+ <widget class="QPushButton" name="pushButtonPzp">
<property name="text">
- <string>pz-</string>
+ <string>pz+</string>
</property>
</widget>
</item>
- <item row="1" column="0">
- <widget class="QPushButton" name="pushButtonPxn">
+ <item row="4" column="0">
+ <spacer name="verticalSpacer">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QGroupBox" name="groupBox">
+ <property name="geometry">
+ <rect>
+ <x>20</x>
+ <y>330</y>
+ <width>321</width>
+ <height>121</height>
+ </rect>
+ </property>
+ <property name="title">
+ <string>Parameters</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_5">
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_4">
<property name="text">
- <string>px-</string>
+ <string>KpElbow</string>
</property>
</widget>
</item>
- <item row="0" column="4">
- <widget class="QPushButton" name="pushButtonRyp">
+ <item row="3" column="1">
+ <widget class="QSlider" name="sliderKpJla">
+ <property name="maximum">
+ <number>200</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QLabel" name="label_5">
<property name="text">
- <string>ry+</string>
+ <string>KpJLA</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="2">
+ <widget class="QLabel" name="labelKpJla">
+ <property name="text">
+ <string>0.0</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QSlider" name="sliderKpElbow">
+ <property name="maximum">
+ <number>200</number>
+ </property>
+ <property name="singleStep">
+ <number>1</number>
+ </property>
+ <property name="value">
+ <number>100</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="2">
+ <widget class="QLabel" name="labelKpElbow">
+ <property name="text">
+ <string>0.0</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0" colspan="2">
+ <widget class="QCheckBox" name="checkBoxAutoKp">
+ <property name="text">
+ <string>Automatic Kp</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_6">
+ <property name="text">
+ <string>PosVel</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QSlider" name="sliderPosVel">
+ <property name="maximum">
+ <number>400</number>
+ </property>
+ <property name="value">
+ <number>80</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QLabel" name="labelPosVel">
+ <property name="text">
+ <string>80</string>
</property>
</widget>
</item>
</layout>
</widget>
+ <widget class="QGroupBox" name="groupBoxNullspaceTargets">
+ <property name="geometry">
+ <rect>
+ <x>390</x>
+ <y>160</y>
+ <width>371</width>
+ <height>291</height>
+ </rect>
+ </property>
+ <property name="title">
+ <string>Nullspace Targets</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_6">
+ <item row="1" column="1">
+ <widget class="QWidget" name="widgetNullspaceTargets" native="true">
+ <layout class="QGridLayout" name="gridLayoutNullspaceTargets">
+ <item row="0" column="1">
+ <widget class="QSlider" name="horizontalSliderExampleJoint1">
+ <property name="maximum">
+ <number>360</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QCheckBox" name="checkBoxExampleJoint1">
+ <property name="text">
+ <string>Joint1</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QLabel" name="labelExampleJoint1">
+ <property name="text">
+ <string>0</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <spacer name="verticalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </widget>
</widget>
<resources/>
<connections/>
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp
index 4401588e307573d1476b8dca57f88c4e80e8124c..f14d3a669f9433a4400b3a7fdcf5193708404c07 100644
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp
@@ -35,6 +35,8 @@ namespace armarx
std::lock_guard g{_allMutex};
_ui.setupUi(getWidget());
+ connect(this, SIGNAL(invokeConnectComponentQt()), this, SLOT(onConnectComponentQt()), Qt::QueuedConnection);
+ connect(this, SIGNAL(invokeDisconnectComponentQt()), this, SLOT(onDisconnectComponentQt()), Qt::QueuedConnection);
//connect(_ui.pushButtonStop, SIGNAL(clicked()), this, SLOT(on_pushButtonStop_clicked()));
@@ -42,6 +44,11 @@ namespace armarx
//connect(_ui.pushButtonZeroFT, SIGNAL(clicked()), this, SLOT(on_pushButtonZeroFT_clicked()));
//connect(_ui.pushButtonSendSettings, SIGNAL(clicked()), this, SLOT(on_pushButtonSendSettings_clicked()));
connect(_ui.pushButtonCreateController, SIGNAL(clicked()), this, SLOT(on_pushButtonCreateController_clicked()));
+ connect(_ui.sliderKpElbow, SIGNAL(valueChanged(int)), this, SLOT(on_sliders_valueChanged(int)));
+ connect(_ui.sliderKpJla, SIGNAL(valueChanged(int)), this, SLOT(on_sliders_valueChanged(int)));
+ connect(_ui.checkBoxAutoKp, SIGNAL(stateChanged(int)), this, SLOT(on_checkBoxAutoKp_stateChanged(int)));
+ connect(_ui.checkBoxSetOri, SIGNAL(stateChanged(int)), this, SLOT(on_checkBoxSetOri_stateChanged(int)));
+ connect(_ui.sliderPosVel, SIGNAL(valueChanged(int)), this, SLOT(on_horizontalSliderPosVel_valueChanged(int)));
auto addBtn = [&](QPushButton * btn, float px, float py, float pz, float rx, float ry, float rz)
{
@@ -66,13 +73,13 @@ namespace armarx
addBtn(_ui.pushButtonRzn, 0, 0, 0, 0, 0, -1);
//_ui.widgetSpacer->setVisible(false);
- //_timer = startTimer(100);
+ _timer = startTimer(100);
}
CartesianNaturalPositionControllerWidgetController::~CartesianNaturalPositionControllerWidgetController()
{
-
+ killTimer(_timer);
}
@@ -112,54 +119,197 @@ namespace armarx
_robot = RemoteRobot::createLocalCloneFromFile(_robotStateComponent, VirtualRobot::RobotIO::eStructure);
}
+ _controller.reset();
+ invokeConnectComponentQt();
+ }
+ void CartesianNaturalPositionControllerWidgetController::onConnectComponentQt()
+ {
_ui.comboBoxSide->addItem("Right");
_ui.comboBoxSide->addItem("Left");
_ui.comboBoxSide->setCurrentIndex(0);
- _controller.reset();
}
void CartesianNaturalPositionControllerWidgetController::onDisconnectComponent()
{
std::lock_guard g{_allMutex};
deleteOldController();
+ _robotStateComponent = nullptr;
}
void CartesianNaturalPositionControllerWidgetController::on_pushButtonCreateController_clicked()
{
std::lock_guard g{_allMutex};
deleteOldController();
- static const std::string njointControllerClassName = "NJointCartesianWaypointController";
std::string side = _ui.comboBoxSide->currentText().toStdString();
VirtualRobot::RobotNodeSetPtr rns = _robot->getRobotNodeSet(side + "Arm");
+ _ui.gridLayoutNullspaceTargets->removeWidget(_ui.checkBoxExampleJoint1);
+ _ui.gridLayoutNullspaceTargets->removeWidget(_ui.labelExampleJoint1);
+ _ui.gridLayoutNullspaceTargets->removeWidget(_ui.horizontalSliderExampleJoint1);
+
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ QCheckBox* checkBox = new QCheckBox(QString::fromStdString(rns->getNode(i)->getName()));
+ QSlider* slider = new QSlider(Qt::Orientation::Horizontal);
+ slider->setMinimum(rns->getNode(i)->getJointLimitLo() / M_PI * 180);
+ slider->setMaximum(rns->getNode(i)->getJointLimitHi() / M_PI * 180);
+ slider->setValue(rns->getNode(i)->getJointValue() / M_PI * 180);
+ QLabel* label = new QLabel(QString::number(slider->value()));
+ _ui.gridLayoutNullspaceTargets->addWidget(checkBox, i, 0);
+ _ui.gridLayoutNullspaceTargets->addWidget(slider, i, 1);
+ _ui.gridLayoutNullspaceTargets->addWidget(label, i, 2);
+ connect(checkBox, SIGNAL(stateChanged(int)), this, SLOT(on_anyNullspaceCheckbox_stateChanged(int)));
+ connect(slider, SIGNAL(valueChanged(int)), this, SLOT(on_anyNullspaceSlider_valueChanged(int)));
+ NullspaceTarget nt;
+ nt.checkBox = checkBox;
+ nt.slider = slider;
+ nt.label = label;
+ nt.i = i;
+ _nullspaceTargets.emplace_back(nt);
+ }
+
+ VirtualRobot::RobotNodePtr cla = rns->getNode(0);
VirtualRobot::RobotNodePtr sho1 = rns->getNode(1);
+ ARMARX_IMPORTANT << VAROUT(cla->getJointValue());
+ cla->setJointValue(0);
Eigen::Vector3f shoulder = sho1->getPositionInRootFrame();
VirtualRobot::RobotNodePtr elb = rns->getNode(4);
+ VirtualRobot::RobotNodePtr wri1 = rns->getNode(6);
+ VirtualRobot::RobotNodePtr tcp = rns->getTCP();
+ NaturalIK::ArmJoints arm;
+
+ arm.rns = rns;
+ arm.shoulder = sho1;
+ arm.elbow = elb;
+ arm.tcp = tcp;
+
+ std::vector<float> jointCosts = std::vector<float>(rns->getSize(), 1);
+ jointCosts.at(0) = 4;
+
armarx::RemoteRobot::synchronizeLocalClone(_robot, _robotStateComponent);
_tcpTarget = rns->getTCP()->getPoseInRootFrame();
NaturalIK ik(side, shoulder, 1.3f);
- _controller.reset(new CartesianNaturalPositionControllerProxy(ik, _robotUnit, side + "NaturalPosition", CartesianNaturalPositionControllerProxy::MakeConfig(rns->getName(), elb->getName())));
+ float upper_arm_length = (sho1->getPositionInRootFrame() - elb->getPositionInRootFrame()).norm();
+ float lower_arm_length = (elb->getPositionInRootFrame() - wri1->getPositionInRootFrame()).norm()
+ + (wri1->getPositionInRootFrame() - tcp->getPositionInRootFrame()).norm();
+ ik.setUpperArmLength(upper_arm_length);
+ ik.setLowerArmLength(lower_arm_length);
+ NJointCartesianNaturalPositionControllerConfigPtr config = CartesianNaturalPositionControllerProxy::MakeConfig(rns->getName(), elb->getName());
+ config->runCfg.jointCosts = jointCosts;
+ CartesianNaturalPositionControllerConfig runCfg = config->runCfg;
+ updateKpSliders(runCfg);
+ //config->runCfg = runCfg;
+ _controller.reset(new CartesianNaturalPositionControllerProxy(ik, arm, _robotUnit, side + "NaturalPosition", config));
_controller->init();
}
void CartesianNaturalPositionControllerWidgetController::on_anyDeltaPushButton_clicked()
{
+ Eigen::Matrix4f newTarget = _tcpTarget;
ARMARX_IMPORTANT << "on_anyDeltaPushButton_clicked";
std::lock_guard g{_allMutex};
Eigen::Vector3f deltaPos = _deltaMapPos.at(QObject::sender());
Eigen::Vector3f deltaOri = _deltaMapOri.at(QObject::sender());
- _tcpTarget = math::Helpers::TranslatePose(_tcpTarget, deltaPos * 100);
+ newTarget = math::Helpers::TranslatePose(newTarget, deltaPos * _ui.spinBoxDpos->value());
if (deltaOri.norm() > 0)
{
- deltaOri = deltaOri * 10 / 180 * M_PI;
+ deltaOri = deltaOri * _ui.spinBoxDori->value() / 180 * M_PI;
Eigen::AngleAxisf aa(deltaOri.norm(), deltaOri.normalized());
- math::Helpers::Orientation(_tcpTarget) = aa.toRotationMatrix() * math::Helpers::Orientation(_tcpTarget);
+ math::Helpers::Orientation(newTarget) = aa.toRotationMatrix() * math::Helpers::Orientation(newTarget);
+ }
+ updateTarget(newTarget);
+ }
+ void CartesianNaturalPositionControllerWidgetController::updateTarget(const Eigen::Matrix4f& newTarget)
+ {
+ if (!_controller->setTarget(newTarget, _setOri ? NaturalDiffIK::Mode::All : NaturalDiffIK::Mode::Position))
+ {
+ return;
+ }
+ _tcpTarget = newTarget;
+ if (_controller->getDynamicKp().enabled)
+ {
+ updateKpSliders(_controller->getRuntimeConfig());
}
- _controller->setTarget(_tcpTarget);
+ }
+
+ void CartesianNaturalPositionControllerWidgetController::readRunCfgFromUi(CartesianNaturalPositionControllerConfig& runCfg)
+ {
+ runCfg.elbowKp = _ui.sliderKpElbow->value() * 0.01f;
+ runCfg.jointLimitAvoidanceKp = _ui.sliderKpJla->value() * 0.01f;
+ }
+ void CartesianNaturalPositionControllerWidgetController::updateKpSliderLabels(const CartesianNaturalPositionControllerConfig& runCfg)
+ {
+ _ui.labelKpElbow->setText(QString::number(runCfg.elbowKp, 'f', 2));
+ _ui.labelKpJla->setText(QString::number(runCfg.jointLimitAvoidanceKp, 'f', 2));
+ }
+ void CartesianNaturalPositionControllerWidgetController::updateKpSliders(const CartesianNaturalPositionControllerConfig& runCfg)
+ {
+ const QSignalBlocker blockKpElb(_ui.sliderKpElbow);
+ const QSignalBlocker blockKpJla(_ui.sliderKpJla);
+ _ui.sliderKpElbow->setValue(runCfg.elbowKp * 100);
+ _ui.sliderKpJla->setValue(runCfg.jointLimitAvoidanceKp * 100);
+ updateKpSliderLabels(runCfg);
+ ARMARX_IMPORTANT << VAROUT(runCfg.elbowKp) << VAROUT(runCfg.jointLimitAvoidanceKp);
+ }
+
+ void CartesianNaturalPositionControllerWidgetController::on_sliders_valueChanged(int)
+ {
+ CartesianNaturalPositionControllerConfig runCfg = _controller->getRuntimeConfig();
+ //ARMARX_IMPORTANT << VAROUT(_runCfg.elbowKp) << VAROUT(_runCfg.jointLimitAvoidanceKp);
+ readRunCfgFromUi(runCfg);
+ updateKpSliderLabels(runCfg);
+ _controller->setRuntimeConfig(runCfg);
+ _controller->updateBaseKpValues(runCfg);
+ }
+
+ void CartesianNaturalPositionControllerWidgetController::on_checkBoxAutoKp_stateChanged(int)
+ {
+ CartesianNaturalPositionControllerProxy::DynamicKp dynamicKp;
+ dynamicKp.enabled = _ui.checkBoxAutoKp->isChecked();
+ _controller->setDynamicKp(dynamicKp);
+ if (_controller->getDynamicKp().enabled)
+ {
+ updateKpSliders(_controller->getRuntimeConfig());
+ }
+ }
+
+ void CartesianNaturalPositionControllerWidgetController::on_checkBoxSetOri_stateChanged(int)
+ {
+ _setOri = _ui.checkBoxSetOri->isChecked();
+ updateTarget(_tcpTarget);
+ }
+
+ void CartesianNaturalPositionControllerWidgetController::updateNullspaceTargets()
+ {
+ std::vector<float> nsTargets;
+ for (const NullspaceTarget& nt : _nullspaceTargets)
+ {
+ nsTargets.push_back(nt.checkBox->isChecked() ? nt.slider->value() * M_PI / 180 : std::nanf(""));
+ nt.label->setText(QString::number(nt.slider->value()));
+ }
+ _controller->setNullspaceTarget(nsTargets);
+ }
+
+ void CartesianNaturalPositionControllerWidgetController::on_anyNullspaceCheckbox_stateChanged(int)
+ {
+ updateNullspaceTargets();
+ }
+
+ void CartesianNaturalPositionControllerWidgetController::on_anyNullspaceSlider_valueChanged(int)
+ {
+ updateNullspaceTargets();
+ }
+
+ void CartesianNaturalPositionControllerWidgetController::on_horizontalSliderPosVel_valueChanged(int)
+ {
+ _ui.labelPosVel->setText(QString::number(_ui.sliderPosVel->value()));
+ float posVel = _ui.sliderPosVel->value();
+ _controller->setMaxVelocities(posVel);
+ //_runCfg = _controller->getRuntimeConfig();
}
//void CartesianNaturalPositionControllerWidgetController::on_pushButtonSendSettings_clicked()
@@ -230,5 +380,20 @@ namespace armarx
}
}
+ void CartesianNaturalPositionControllerWidgetController::timerEvent(QTimerEvent*)
+ {
+ std::lock_guard g{_allMutex};
+ if (!_robot || !_robotStateComponent)
+ {
+ return;
+ }
+ RemoteRobot::synchronizeLocalClone(_robot, _robotStateComponent);
+ if (_controller)
+ {
+ ARMARX_INFO << deactivateSpam() << "setting visu gp to:\n" << _robot->getGlobalPose();
+ _controller->getInternalController()->setVisualizationRobotGlobalPose(_robot->getGlobalPose());
+ }
+ }
+
}
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h
index 19d8932a1c4efcf565e246283abcabb08dd51064..f133beec6a99782c94321d108f3972030c3e6d7f 100644
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h
+++ b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h
@@ -64,6 +64,14 @@ namespace armarx
Q_OBJECT
public:
+ struct NullspaceTarget
+ {
+ QCheckBox* checkBox;
+ QSlider* slider;
+ QLabel* label;
+ size_t i;
+ };
+
/**
* Controller Constructor
*/
@@ -102,14 +110,30 @@ namespace armarx
public slots:
/* QT slot declarations */
+ void onConnectComponentQt();
void on_pushButtonCreateController_clicked();
void on_anyDeltaPushButton_clicked();
+ void on_sliders_valueChanged(int);
+ void on_checkBoxAutoKp_stateChanged(int);
+ void on_checkBoxSetOri_stateChanged(int);
+ void on_anyNullspaceCheckbox_stateChanged(int);
+ void on_anyNullspaceSlider_valueChanged(int);
+ void on_horizontalSliderPosVel_valueChanged(int);
signals:
/* QT signal declarations */
+ void invokeConnectComponentQt();
+ void invokeDisconnectComponentQt();
private:
void deleteOldController();
+ void readRunCfgFromUi(CartesianNaturalPositionControllerConfig& runCfg);
+ void timerEvent(QTimerEvent*) override;
+ void updateTarget(const Eigen::Matrix4f& newTarget);
+ void updateKpSliders(const CartesianNaturalPositionControllerConfig& runCfg);
+ void updateKpSliderLabels(const CartesianNaturalPositionControllerConfig& runCfg);
+ void updateNullspaceTargets();
+
std::string _robotStateComponentName;
@@ -123,12 +147,16 @@ namespace armarx
VirtualRobot::RobotPtr _robot;
std::vector<Eigen::Matrix4f> _lastParsedWPs;
bool _supportsFT{false};
+ bool _setOri = true;
mutable std::recursive_mutex _allMutex;
int _timer;
Eigen::Matrix4f _tcpTarget;
std::map<QObject*, Eigen::Vector3f> _deltaMapPos;
std::map<QObject*, Eigen::Vector3f> _deltaMapOri;
+ //CartesianNaturalPositionControllerConfig _runCfg;
+
+ std::vector<NullspaceTarget> _nullspaceTargets;
};
}
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/CMakeLists.txt b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/CMakeLists.txt
index a4bb4d600d2d3b8917d7cb3fd970d81d69ea5a33..9ddde59ae018fa3a7391c0ed72ad98f9f2a4689e 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/CMakeLists.txt
@@ -5,7 +5,7 @@ set(HEADERS KinematicUnitGuiPlugin.h KinematicUnitConfigDialog.h)
set(GUI_MOC_HDRS KinematicUnitGuiPlugin.h KinematicUnitConfigDialog.h)
set(GUI_UIS kinematicunitguiplugin.ui KinematicUnitConfigDialog.ui)
-set(COMPONENT_LIBS RobotAPIUnits DebugDrawer)
+set(COMPONENT_LIBS RobotAPIUnits DebugDrawer ArmarXCoreJsonObject)
if (ArmarXGui_FOUND)
armarx_gui_library(KinematicUnitGuiPlugin "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${COMPONENT_LIBS}" )
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 5daefe9bf2cb5d04f029e5ff36652ca40d23a960..81016bf6b2087b33f1c4ab2bc6e67136bc161829 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -10,6 +10,7 @@ set(SLICE_FILES
observers/ObserverFilters.ice
observers/GraspCandidateObserverInterface.ice
+ core/BlackWhitelist.ice
core/PoseBase.ice
core/OrientedPoint.ice
core/LinkedPoseBase.ice
@@ -63,7 +64,7 @@ set(SLICE_FILES
units/RobotUnit/NJointCartesianNaturalPositionController.ice
units/RobotUnit/RobotUnitInterface.ice
-
+
units/RobotUnit/NJointBimanualForceController.ice
units/RobotUnit/NJointBimanualObjLevelController.ice
@@ -72,7 +73,6 @@ set(SLICE_FILES
units/RobotUnit/NJointBimanualForceMPController.ice
units/RobotUnit/DSControllerBase.ice
- units/RobotUnit/NJointBimanualForceController.ice
units/RobotUnit/TaskSpaceActiveImpedanceControl.ice
@@ -85,6 +85,7 @@ set(SLICE_FILES
components/ViewSelectionInterface.ice
visualization/DebugDrawerInterface.ice
+ visualization/DebugDrawerToArViz.ice
ArViz.ice
ArViz/Elements.ice
diff --git a/source/RobotAPI/interface/core/BlackWhitelist.ice b/source/RobotAPI/interface/core/BlackWhitelist.ice
new file mode 100644
index 0000000000000000000000000000000000000000..dc93760a9530067a1254b35770783e87559b5ff6
--- /dev/null
+++ b/source/RobotAPI/interface/core/BlackWhitelist.ice
@@ -0,0 +1,55 @@
+/*
+* 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 MemoryX::WorkingMemory
+* @author Kai Welke ( welke at kit dot edu), Alexey Kozlov ( kozlov at kit dot edu)
+* @date 2012
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include <Ice/BuiltinSequences.ice>
+
+
+module armarx
+{
+
+ struct StringListUpdate
+ {
+ bool clear = false;
+ Ice::StringSeq add;
+ Ice::StringSeq set;
+ };
+
+ struct BlackWhitelistUpdate
+ {
+ StringListUpdate blacklist;
+ StringListUpdate whitelist;
+ };
+
+
+ interface BlackWhitelistInterface
+ {
+ void updateBlackWhitelist(BlackWhitelistUpdate update);
+ }
+
+ interface BlackWhitelistTopic
+ {
+ void updateBlackWhitelist(BlackWhitelistUpdate update);
+ }
+};
+
diff --git a/source/RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.ice b/source/RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.ice
index 86e63f5024c6e6091ec7a9834a9dac72fad3064a..a876489d1f7f4042817f61c8b2fe751b97f06690 100644
--- a/source/RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.ice
+++ b/source/RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.ice
@@ -22,6 +22,9 @@
#pragma once
+//#include <ArmarXCore/interface/serialization/Eigen.ice>
+#include <ArmarXCore/interface/core/BasicVectorTypes.ice>
+
module armarx
{
struct CartesianNaturalPositionControllerConfig
@@ -29,11 +32,14 @@ module armarx
float maxPositionAcceleration = 500;
float maxOrientationAcceleration = 1;
float maxNullspaceAcceleration = 2;
- float maxJointVelocity = 999;
- float jointLimitAvoidanceKp = 0.01;
- float jointLimitAvoidanceScale = 0;
+ Ice::FloatSeq maxJointVelocity;
+ Ice::FloatSeq maxNullspaceVelocity;
+ Ice::FloatSeq jointCosts;
+
+ float jointLimitAvoidanceKp = 0.1;
float elbowKp = 1;
+ float nullspaceTargetKp = 1;
float thresholdOrientationNear = 0.1;
float thresholdOrientationReached = 0.05;
@@ -41,8 +47,9 @@ module armarx
float thresholdPositionReached = 5;
- float maxOriVel = 1;
- float maxPosVel = 80;
+ float maxTcpPosVel = 80;
+ float maxTcpOriVel = 1;
+ float maxElbPosVel = 80;
float KpOri = 1;
float KpPos = 1;
};
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.ice
index a1e8048c190519891741a44680c20c4186e6d029..0ad9238233bda6c9af2de8dc5843c381f48013bc 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.ice
@@ -26,14 +26,10 @@
#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
#include <RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.ice>
+#include <ArmarXCore/interface/core/BasicVectorTypes.ice>
module armarx
{
- struct NJointCartesianNaturalPositionControllerRuntimeConfig
- {
- CartesianNaturalPositionControllerConfig posCtrlConfig;
- };
-
class NJointCartesianNaturalPositionControllerConfig extends NJointControllerConfig
{
@@ -42,20 +38,42 @@ module armarx
int feedforwardVelocityTTLms = 100;
CartesianNaturalPositionControllerConfig runCfg;
string ftName;
+ int ftHistorySize = 100;
+ };
+
+ struct FTSensorValue
+ {
+ Eigen::Vector3f force;
+ Eigen::Vector3f torque;
};
interface NJointCartesianNaturalPositionControllerInterface extends NJointControllerInterface
{
- idempotent bool hasReachedTarget();
+ //idempotent bool hasReachedTarget();
//idempotent bool hasReachedForceLimit();
void setConfig(CartesianNaturalPositionControllerConfig cfg);
- void setTarget(Eigen::Matrix4f tcpTarget, Eigen::Vector3f elbowTarget);
+ void setTarget(Eigen::Matrix4f tcpTarget, Eigen::Vector3f elbowTarget, bool setOrientation);
+ void setTargetFeedForward(Eigen::Matrix4f tcpTarget, Eigen::Vector3f elbowTarget, bool setOrientation, Eigen::Vector6f ffVel);
void setFeedForwardVelocity(Eigen::Vector6f vel);
+ void clearFeedForwardVelocity();
+ void setNullspaceTarget(Ice::FloatSeq nullspaceTarget);
+ void clearNullspaceTarget();
+ void setNullspaceControlEnabled(bool enabled);
+
+ FTSensorValue getCurrentFTValue();
+ FTSensorValue getAverageFTValue();
+ void resetFTOffset();
+ void setFTOffset(FTSensorValue offset);
+ void setFTLimit(float force, float torque);
+ void clearFTLimit();
+ void setFakeFTValue(FTSensorValue ftValue);
+ void clearFakeFTValue();
+ bool isAtForceLimit();
- //void setConfigAndWaypoints(NJointCartesianNaturalPositionControllerRuntimeConfig cfg, Eigen::Matrix4fSeq wps);
- //void setConfigAndWaypoint(NJointCartesianNaturalPositionControllerRuntimeConfig cfg, Eigen::Matrix4f wp);
void stopMovement();
+ void setVisualizationRobotGlobalPose(Eigen::Matrix4f p);
+ void resetVisualizationRobotGlobalPose();
};
};
diff --git a/source/RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.ice b/source/RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.ice
index b905ef0afcf8a7b561e03234a170972597f5e3b1..8d1800bd02d45d0e06e556efafee1bc415e45958 100644
--- a/source/RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.ice
@@ -36,8 +36,8 @@ module armarx
Ice::FloatSeq Kori;
Ice::FloatSeq Dpos;
Ice::FloatSeq Dori;
- float Knull;
- float Dnull;
+ Ice::FloatSeq Knull;
+ Ice::FloatSeq Dnull;
float torqueLimit;
};
diff --git a/source/RobotAPI/interface/visualization/DebugDrawerToArViz.ice b/source/RobotAPI/interface/visualization/DebugDrawerToArViz.ice
new file mode 100644
index 0000000000000000000000000000000000000000..79f15cfd506da5ea1dc05222ff54c00290514745
--- /dev/null
+++ b/source/RobotAPI/interface/visualization/DebugDrawerToArViz.ice
@@ -0,0 +1,39 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 ArmarX::RobotAPI
+ * @author Nikolaus Vahrenkamp
+ * @copyright 2014
+ * @license http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/core/BlackWhitelist.ice>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.ice>
+
+
+module armarx
+{
+
+ interface DebugDrawerToArvizInterface extends DebugDrawerInterface, BlackWhitelistTopic
+ {
+ };
+
+};
+
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
index 6ba6135d061e5ca60e6c32a20f1eca80b759a055..02e92853f94cd09f89d443c72438f2b81cf55d25 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
@@ -288,7 +288,7 @@ namespace armarx
double deltaT = rt2ControlBuffer.getReadBuffer().deltaT;
Eigen::Matrix4f currentPose = rt2ControlBuffer.getReadBuffer().currentPose;
Eigen::VectorXf currentTwist = rt2ControlBuffer.getReadBuffer().currentTwist;
- ARMARX_IMPORTANT << "canVal: " << objectDMP->canVal;
+ //ARMARX_IMPORTANT << "canVal: " << objectDMP->canVal;
if (objectDMP->canVal < 1e-8)
{
@@ -400,7 +400,8 @@ namespace armarx
// Eigen::MatrixXf pinvG = leftIK->computePseudoInverseJacobianMatrix(graspMatrix, 0);
// Eigen::MatrixXf G_range = pinvG * graspMatrix;
// Eigen::MatrixXf PG = Eigen::MatrixXf::Identity(12, 12) - G_range;
- Eigen::MatrixXf pinvGraspMatrixT = leftIK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), 0);
+ float lambda = 1;
+ Eigen::MatrixXf pinvGraspMatrixT = leftIK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), lambda);
// ---------------------------------------------- object pose ----------------------------------------------
Eigen::Matrix4f boxCurrentPose = currentRightPose;
@@ -559,7 +560,7 @@ namespace armarx
Eigen::VectorXf rightNullspaceTorque = cfg->knull * (rightDesiredJointValues - rightqpos) - cfg->dnull * rightqvel;
// --------------------------------------------- Set Torque Control Command ---------------------------------------------
- float lambda = 1;
+ // float lambda = 1;
Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * forceImpedance.head(6) + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
@@ -726,13 +727,15 @@ namespace armarx
{
usleep(1000);
}
-
+ ARMARX_IMPORTANT << "obj level control: setup dmp ...";
objectDMP->prepareExecution(starts, goals);
objectDMP->canVal = timeDuration;
objectDMP->config.motionTimeDuration = timeDuration;
finished = false;
dmpStarted = true;
+
+ ARMARX_IMPORTANT << "obj level control: run dmp with virtual start.";
}
void NJointBimanualObjLevelController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
diff --git a/source/RobotAPI/libraries/aron/AronNavigator.cpp b/source/RobotAPI/libraries/aron/AronNavigator.cpp
index ea94f9b5d6b78fa886f86eb14d2c380007a2e57f..b24b7f8dcf085c74f3767df01ab6ee42abebe99f 100644
--- a/source/RobotAPI/libraries/aron/AronNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/AronNavigator.cpp
@@ -174,6 +174,14 @@ namespace armarx
bool AronNavigator::is##upperType() \
{ \
return Aron##upperType##Ptr::dynamicCast(data); \
+ } \
+ void AronNavigator::append##upperType(const cppType& value) \
+ { \
+ append(new Aron##upperType(value)); \
+ } \
+ bool AronNavigator::append##upperType(const std::string& key, const cppType& value) \
+ { \
+ return append(key, new Aron##upperType(value)); \
}
HANDLE_TYPE(std::string, String)
@@ -201,6 +209,82 @@ namespace armarx
return AronNullPtr::dynamicCast(data);
}
+ bool AronNavigator::appendVec(const std::string& key, const std::vector<float>& vec)
+ {
+ AronListPtr list = new AronList();
+ for (float f : vec)
+ {
+ list->elements.push_back(new AronFloat(f));
+ }
+ return append(key, list);
+ }
+
+ std::vector<float> armarx::aron::AronNavigator::AronNavigator::asVecFloat()
+ {
+ AronListPtr list = AronListPtr::dynamicCast(data);
+ if (!list)
+ {
+ throw LocalException("value is not a list. path: ") << path;
+ }
+ std::vector<float> result;
+ for (size_t index = 0; index < list->elements.size(); index++)
+ {
+ float f = AronNavigator(list->elements.at(index), path + "/" + std::to_string(index), "", index).asFloat();
+ result.push_back(f);
+ }
+ return result;
+ }
+
+ void AronNavigator::writeXml(RapidXmlWriterNode parent)
+ {
+ /*if (key != "")
+ {
+ parent.append_attribute("key", key);
+ }
+ AronListPtr list = AronListPtr::dynamicCast(data);
+ AronObjectPtr obj = AronObjectPtr::dynamicCast(data);
+ AronStringPtr String = AronStringPtr::dynamicCast(data);
+ AronIntPtr Int = AronIntPtr::dynamicCast(data);
+ AronLongPtr Long = AronLongPtr::dynamicCast(data);
+ AronFloatPtr Float = AronFloatPtr::dynamicCast(data);
+ AronDoublePtr Double = AronDoublePtr::dynamicCast(data);
+ AronBoolPtr Bool = AronBoolPtr::dynamicCast(data);
+ AronBlobPtr Blob = AronBlobPtr::dynamicCast(data);
+
+ AronVector2fPtr Vector2f = AronVector2fPtr::dynamicCast(data);
+ AronVector3fPtr Vector3f = AronVector3fPtr::dynamicCast(data);
+ AronVector6fPtr Vector6f = AronVector6fPtr::dynamicCast(data);
+ AronVectorXfPtr VectorXf = AronVectorXfPtr::dynamicCast(data);
+ AronMatrix3fPtr Matrix3f = AronMatrix3fPtr::dynamicCast(data);
+ AronMatrix4fPtr Matrix4f = AronMatrix4fPtr::dynamicCast(data);
+ AronMatrixXfPtr MatrixXf = AronMatrixXfPtr::dynamicCast(data);
+
+ if (list)
+ {
+ RapidXmlWriterNode node = node.append_node("List");
+ for (AronNavigator nav : children())
+ {
+ nav.writeXml(node);
+ }
+ }
+ else if (obj)
+ {
+ RapidXmlWriterNode node = node.append_node("Object");
+ for (AronNavigator nav : children())
+ {
+ nav.writeXml(node);
+ }
+ }
+ else if (String)
+ {
+
+ }
+ else if (Int)
+ {
+ parent.append_node("Int")
+ }*/
+ }
+
bool AronNavigator::isList()
{
return AronListPtr::dynamicCast(data);
diff --git a/source/RobotAPI/libraries/aron/AronNavigator.h b/source/RobotAPI/libraries/aron/AronNavigator.h
index 03c14b07ed70d376df7efa1825fd0ec782776de6..ae39728626f92f277c908c7b5bf8d176c7e7eb83 100644
--- a/source/RobotAPI/libraries/aron/AronNavigator.h
+++ b/source/RobotAPI/libraries/aron/AronNavigator.h
@@ -25,6 +25,8 @@
#include <RobotAPI/interface/aron.h>
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlWriter.h>
+
namespace armarx
{
namespace aron
@@ -54,12 +56,16 @@ namespace armarx
int keyIndex(const std::string& key);
bool deleteKey(const std::string& key);
+
+
bool isList();
bool isObject();
bool isNull();
#define HANDLE_TYPE(cppType, upperType) \
cppType as##upperType(); \
- bool is##upperType();
+ bool is##upperType(); \
+ void append##upperType(const cppType& value); \
+ bool append##upperType(const std::string& key, const cppType& value);
HANDLE_TYPE(std::string, String)
HANDLE_TYPE(int, Int)
@@ -85,9 +91,15 @@ namespace armarx
template <typename T>
std::vector<T> asStdVector();
+ //TODO
+ bool appendVec(const std::string& key, const std::vector<float>& vec);
+ std::vector<float> asVecFloat();
+
template <typename T>
std::map<std::string, T> asStdMap();
+ void writeXml(RapidXmlWriterNode parent);
+
private:
AronDataPtr data;
diff --git a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
index 6cd6e5140ea2d282f6ffa6368c23dfaba6f4053e..a6a30cd5bfd80eeb40876fb236a6b1a3ea9580f1 100644
--- a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
@@ -46,52 +46,105 @@ namespace armarx
maxNullspaceAcceleration,
tcp ? tcp : rns->getTCP()),
pcTcp(tcp ? tcp : rns->getTCP()),
- vcElb(rns, tcp),
- pcElb(tcp ? tcp : rns->getTCP()),
+ vcElb(rns, elbow),
+ pcElb(elbow),
+ lastJointVelocity(Eigen::VectorXf::Constant(rns->getSize(), 0.f)),
rns(rns)
{
ARMARX_CHECK_NOT_NULL(rns);
+ clearNullspaceTarget();
}
- Eigen::VectorXf CartesianNaturalPositionController::LimitInfNormTo(Eigen::VectorXf vec, float maxValue)
+ Eigen::VectorXf CartesianNaturalPositionController::LimitInfNormTo(const Eigen::VectorXf& vec, const std::vector<float>& maxValue)
{
- float infNorm = vec.lpNorm<Eigen::Infinity>();
- if (infNorm > maxValue)
+ if (maxValue.size() == 0)
{
- vec = vec / infNorm * maxValue;
+ return vec;
}
- return vec;
+ if (maxValue.size() != 1 && (int)maxValue.size() != vec.rows())
+ {
+ throw std::invalid_argument("maxValue.size != 1 and != maxValue.size");
+ }
+ float scale = 1;
+ for (int i = 0; i < vec.rows(); i++)
+ {
+ int j = maxValue.size() == 1 ? 0 : i;
+ if (std::abs(vec(i)) > maxValue.at(j) && maxValue.at(j) >= 0)
+ {
+ scale = std::min(scale, maxValue.at(j) / std::abs(vec(i)));
+ }
+ }
+ return vec * scale;
}
- const Eigen::VectorXf CartesianNaturalPositionController::calculate(float dt, VirtualRobot::IKSolver::CartesianSelection mode)
+ const Eigen::VectorXf CartesianNaturalPositionController::calculateNullspaceTargetDifference()
{
-
- int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
- int oriLen = mode & VirtualRobot::IKSolver::Orientation ? 3 : 0;
- Eigen::Vector3f pdTcp = posLen ? pcTcp.getPositionDiff(tcpTarget) : Eigen::Vector3f::Zero();
- Eigen::Vector3f odTcp = oriLen ? pcTcp.getOrientationDiff(tcpTarget) : Eigen::Vector3f::Zero();
- Eigen::VectorXf cartesianVel(posLen + oriLen);
- if (posLen)
+ Eigen::VectorXf jvNT = Eigen::VectorXf(rns->getSize());
+ for (size_t i = 0; i < rns->getSize(); i++)
{
- cartesianVel.block<3, 1>(0, 0) = pdTcp;
- }
- if (oriLen)
- {
- cartesianVel.block<3, 1>(posLen, 0) = odTcp;
+ if (std::isnan(nullspaceTarget(i)))
+ {
+ jvNT(i) = 0;
+ }
+ else
+ {
+ float diff = nullspaceTarget(i) - rns->getNode(i)->getJointValue();
+ if (rns->getNode(i)->isLimitless())
+ {
+ diff = math::Helpers::AngleModPI(diff);
+ }
+ jvNT(i) = diff;
+ }
}
+ return jvNT;
+ }
+
+ const Eigen::VectorXf CartesianNaturalPositionController::calculate(float dt, VirtualRobot::IKSolver::CartesianSelection mode)
+ {
- Eigen::Vector3f pdElb = pcElb.getPositionDiffVec3(elbowTarget);
- Eigen::VectorXf cartesianVelElb(3);
- cartesianVelElb.block<3, 1>(0, 0) = pdElb;
- Eigen::VectorXf jnv = Eigen::VectorXf::Zero(rns->getSize());
+ //int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
+ //int oriLen = mode & VirtualRobot::IKSolver::Orientation ? 3 : 0;
+ //Eigen::Vector3f pdTcp = posLen ? pcTcp.getPositionDiff(tcpTarget) : Eigen::Vector3f::Zero();
+ //Eigen::Vector3f odTcp = oriLen ? pcTcp.getOrientationDiff(tcpTarget) : Eigen::Vector3f::Zero();
+ //Eigen::VectorXf cartesianVel(posLen + oriLen);
+ //if (posLen)
+ //{
+ // cartesianVel.block<3, 1>(0, 0) = pdTcp;
+ //}
+ //if (oriLen)
+ //{
+ // cartesianVel.block<3, 1>(posLen, 0) = odTcp;
+ //}
+ Eigen::VectorXf cartesianVelTcp = pcTcp.calculate(tcpTarget, mode);
+
+ //Eigen::Vector3f pdElb = pcElb.getPositionDiffVec3(elbowTarget);
+ ////ARMARX_IMPORTANT << deactivateSpam(0.1) << VAROUT(pdElb) << VAROUT(elbowTarget) << VAROUT(pcElb.getTcp()->getPositionInRootFrame());
+ //Eigen::VectorXf cartesianVelElb(3);
+ //cartesianVelElb.block<3, 1>(0, 0) = pdElb;
+
+ Eigen::VectorXf cartesianVelElb = pcElb.calculatePos(elbowTarget);
+ Eigen::VectorXf jnvClamped = Eigen::VectorXf::Zero(rns->getSize());
if (nullSpaceControlEnabled)
{
- Eigen::VectorXf jvElb = elbowKp * vcElb.calculate(cartesianVelElb, VirtualRobot::IKSolver::Position);
+ Eigen::VectorXf jnv = Eigen::VectorXf::Zero(rns->getSize());
+ Eigen::VectorXf jvElb = vcElb.calculate(cartesianVelElb, VirtualRobot::IKSolver::Position);
+
Eigen::VectorXf jvLA = jointLimitAvoidanceKp * vcTcp.controller.calculateJointLimitAvoidance();
- jnv = jvElb + jvLA;
+ Eigen::VectorXf jvNT = nullspaceTargetKp * calculateNullspaceTargetDifference();
+ //ARMARX_IMPORTANT << deactivateSpam(1) << VAROUT(jvNT);
+
+ //ARMARX_IMPORTANT << VAROUT(jvElb);
+ jnv = jvElb + jvLA + jvNT;
+ jnvClamped = LimitInfNormTo(jnv, maxNullspaceVelocity);
}
- Eigen::VectorXf jv = vcTcp.calculate(cartesianVel, jnv, mode);
+ //ARMARX_IMPORTANT << VAROUT(jnv);
+ if (vcTcp.getMode() != mode)
+ {
+ vcTcp.switchMode(lastJointVelocity, mode);
+ }
+ Eigen::VectorXf jv = vcTcp.calculate(cartesianVelTcp, jnvClamped, dt);
+ //ARMARX_IMPORTANT << VAROUT(jv);
Eigen::VectorXf jvClamped = LimitInfNormTo(jv, maxJointVelocity);
@@ -100,6 +153,7 @@ namespace armarx
clearFeedForwardVelocity();
}
+ lastJointVelocity = jvClamped;
return jvClamped;
}
@@ -122,6 +176,25 @@ namespace armarx
feedForwardVelocity.block<3, 1>(3, 0) = feedForwardVelocityOri;
}
+ void CartesianNaturalPositionController::setNullspaceTarget(const Eigen::VectorXf& nullspaceTarget)
+ {
+ this->nullspaceTarget = nullspaceTarget;
+ }
+
+ void CartesianNaturalPositionController::setNullspaceTarget(const std::vector<float>& nullspaceTarget)
+ {
+ ARMARX_CHECK_EXPRESSION(rns->getSize() == nullspaceTarget.size());
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ this->nullspaceTarget(i) = nullspaceTarget.at(i);
+ }
+ }
+
+ void CartesianNaturalPositionController::clearNullspaceTarget()
+ {
+ this->nullspaceTarget = Eigen::VectorXf::Constant(rns->getSize(), std::nanf(""));
+ }
+
void CartesianNaturalPositionController::clearFeedForwardVelocity()
{
feedForwardVelocity = Eigen::Vector6f::Zero();
@@ -157,6 +230,28 @@ namespace armarx
return ::math::Helpers::GetPosition(tcpTarget);
}
+ const Eigen::Vector3f& CartesianNaturalPositionController::getCurrentElbowTarget() const
+ {
+ return elbowTarget;
+ }
+
+ const Eigen::VectorXf& CartesianNaturalPositionController::getCurrentNullspaceTarget() const
+ {
+ return nullspaceTarget;
+ }
+
+ bool CartesianNaturalPositionController::hasNullspaceTarget() const
+ {
+ for (int i = 0; i < nullspaceTarget.rows(); i++)
+ {
+ if (!std::isnan(nullspaceTarget(i)))
+ {
+ return true;
+ }
+ }
+ return false;
+ }
+
void CartesianNaturalPositionController::setNullSpaceControl(bool enabled)
{
nullSpaceControlEnabled = enabled;
@@ -173,8 +268,6 @@ namespace armarx
void CartesianNaturalPositionController::setConfig(const CartesianNaturalPositionControllerConfig& cfg)
{
jointLimitAvoidanceKp = cfg.jointLimitAvoidanceKp;
- jointLimitAvoidanceScale = cfg.jointLimitAvoidanceScale;
- elbowKp = cfg.elbowKp;
thresholdOrientationNear = cfg.thresholdOrientationNear;
thresholdOrientationReached = cfg.thresholdOrientationReached;
@@ -182,16 +275,35 @@ namespace armarx
thresholdPositionReached = cfg.thresholdPositionReached;
maxJointVelocity = cfg.maxJointVelocity;
+ maxNullspaceVelocity = cfg.maxNullspaceVelocity;
+
+ nullspaceTargetKp = cfg.nullspaceTargetKp;
- pcTcp.maxOriVel = cfg.maxOriVel;
- pcTcp.maxPosVel = cfg.maxPosVel;
+ pcTcp.maxPosVel = cfg.maxTcpPosVel;
+ pcTcp.maxOriVel = cfg.maxTcpOriVel;
pcTcp.KpOri = cfg.KpOri;
pcTcp.KpPos = cfg.KpPos;
+ pcElb.maxPosVel = cfg.maxElbPosVel;
+ pcElb.KpPos = cfg.elbowKp;
vcTcp.setMaxAccelerations(
cfg.maxPositionAcceleration,
cfg.maxOrientationAcceleration,
cfg.maxNullspaceAcceleration
);
+ if (cfg.jointCosts.size() > 0)
+ {
+ vcTcp.controller.setJointCosts(cfg.jointCosts);
+ }
+ }
+
+ Eigen::VectorXf CartesianNaturalPositionController::actualTcpVel(const Eigen::VectorXf& jointVel)
+ {
+ return (vcTcp.controller.ik->getJacobianMatrix(VirtualRobot::IKSolver::All) * jointVel);
+ }
+
+ Eigen::VectorXf CartesianNaturalPositionController::actualElbVel(const Eigen::VectorXf& jointVel)
+ {
+ return (vcElb.jacobi * jointVel);
}
}
diff --git a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.h b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.h
index b42998fab1b3390151a9a5287f5cdff526a364be..81c51f351c69f315cb48d3efc8fd26f46a037625 100644
--- a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.h
+++ b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.h
@@ -58,12 +58,16 @@ namespace armarx
);
- static Eigen::VectorXf LimitInfNormTo(Eigen::VectorXf vec, float maxValue);
+ static Eigen::VectorXf LimitInfNormTo(const Eigen::VectorXf& vec, const std::vector<float>& maxValue);
+ const Eigen::VectorXf calculateNullspaceTargetDifference();
const Eigen::VectorXf calculate(float dt, VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All);
void setTarget(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& elbowTarget);
void setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity);
void setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri);
+ void setNullspaceTarget(const Eigen::VectorXf& nullspaceTarget);
+ void setNullspaceTarget(const std::vector<float>& nullspaceTarget);
+ void clearNullspaceTarget();
void clearFeedForwardVelocity();
float getPositionError() const;
@@ -76,6 +80,9 @@ namespace armarx
const Eigen::Matrix4f& getCurrentTarget() const;
const Eigen::Vector3f getCurrentTargetPosition() const;
+ const Eigen::Vector3f& getCurrentElbowTarget() const;
+ const Eigen::VectorXf& getCurrentNullspaceTarget() const;
+ bool hasNullspaceTarget() const;
void setNullSpaceControl(bool enabled);
@@ -83,20 +90,27 @@ namespace armarx
void setConfig(const CartesianNaturalPositionControllerConfig& cfg);
+ Eigen::VectorXf actualTcpVel(const Eigen::VectorXf& jointVel);
+ Eigen::VectorXf actualElbVel(const Eigen::VectorXf& jointVel);
+
CartesianVelocityControllerWithRamp vcTcp;
CartesianPositionController pcTcp;
CartesianVelocityController vcElb;
CartesianPositionController pcElb;
+ Eigen::VectorXf lastJointVelocity;
Eigen::Matrix4f tcpTarget;
Eigen::Vector3f elbowTarget;
+ Eigen::VectorXf nullspaceTarget = Eigen::VectorXf(0);
float thresholdPositionReached = 0.f;
float thresholdOrientationReached = 0.f;
float thresholdPositionNear = 0.f;
float thresholdOrientationNear = 0.f;
- float maxJointVelocity = -1;
+ std::vector<float> maxJointVelocity;
+ std::vector<float> maxNullspaceVelocity;
+
Eigen::Vector6f feedForwardVelocity = Eigen::Vector6f::Zero();
Eigen::Vector6f cartesianVelocity = Eigen::Vector6f::Zero();
@@ -106,7 +120,7 @@ namespace armarx
bool nullSpaceControlEnabled = true;
float jointLimitAvoidanceScale = 0.f;
float jointLimitAvoidanceKp = 0.f;
- float elbowKp = 0;
+ float nullspaceTargetKp = 0;
private:
VirtualRobot::RobotNodeSetPtr rns;
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.cpp b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
index efe266741f146f560da821f0c8f17ecc1cdb45fd..2765f2985a9ac0df68913aa73fa58c25bc5da51d 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
@@ -69,14 +69,38 @@ namespace armarx
return cartesianVel;
}
+ Eigen::VectorXf CartesianPositionController::calculatePos(const Eigen::Vector3f& targetPos) const
+ {
+ Eigen::VectorXf cartesianVel(3);
+ Eigen::Vector3f currentPos = tcp->getPositionInRootFrame();
+ Eigen::Vector3f errPos = targetPos - currentPos;
+ Eigen::Vector3f posVel = errPos * KpPos;
+ if (maxPosVel > 0)
+ {
+ posVel = math::MathUtils::LimitTo(posVel, maxPosVel);
+ }
+ cartesianVel.block<3, 1>(0, 0) = posVel;
+ return cartesianVel;
+ }
+
float CartesianPositionController::getPositionError(const Eigen::Matrix4f& targetPose) const
+ {
+ return GetPositionError(targetPose, tcp);
+ }
+
+ float CartesianPositionController::getOrientationError(const Eigen::Matrix4f& targetPose) const
+ {
+ return GetOrientationError(targetPose, tcp);
+ }
+
+ float CartesianPositionController::GetPositionError(const Eigen::Matrix4f& targetPose, const VirtualRobot::RobotNodePtr& tcp)
{
Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
Eigen::Vector3f errPos = targetPos - tcp->getPositionInRootFrame();
return errPos.norm();
}
- float CartesianPositionController::getOrientationError(const Eigen::Matrix4f& targetPose) const
+ float CartesianPositionController::GetOrientationError(const Eigen::Matrix4f& targetPose, const VirtualRobot::RobotNodePtr& tcp)
{
Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
@@ -85,6 +109,12 @@ namespace armarx
return aa.angle();
}
+ bool CartesianPositionController::Reached(const Eigen::Matrix4f& targetPose, const VirtualRobot::RobotNodePtr& tcp, bool checkOri, float thresholdPosReached, float thresholdOriReached)
+ {
+ return GetPositionError(targetPose, tcp) < thresholdPosReached
+ && (!checkOri || GetOrientationError(targetPose, tcp) < thresholdOriReached);
+ }
+
Eigen::Vector3f CartesianPositionController::getPositionDiff(const Eigen::Matrix4f& targetPose) const
{
Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.h b/source/RobotAPI/libraries/core/CartesianPositionController.h
index cde4cd4734cf8daf6f21551e741e56c04629819a..5ecf2c66e3f25dcb7354930e2c4a7a888617f006 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.h
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.h
@@ -47,9 +47,14 @@ namespace armarx
CartesianPositionController& operator=(CartesianPositionController&&) = default;
Eigen::VectorXf calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode) const;
+ Eigen::VectorXf calculatePos(const Eigen::Vector3f& targetPos) const;
float getPositionError(const Eigen::Matrix4f& targetPose) const;
float getOrientationError(const Eigen::Matrix4f& targetPose) const;
+ static float GetPositionError(const Eigen::Matrix4f& targetPose, const VirtualRobot::RobotNodePtr& tcp);
+ static float GetOrientationError(const Eigen::Matrix4f& targetPose, const VirtualRobot::RobotNodePtr& tcp);
+ static bool Reached(const Eigen::Matrix4f& targetPose, const VirtualRobot::RobotNodePtr& tcp, bool checkOri, float thresholdPosReached, float thresholdOriReached);
+
Eigen::Vector3f getPositionDiff(const Eigen::Matrix4f& targetPose) const;
Eigen::Vector3f getPositionDiffVec3(const Eigen::Vector3f& targetPosition) const;
Eigen::Vector3f getOrientationDiff(const Eigen::Matrix4f& targetPose) const;
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index 228df7d268204bb6d3578d49d158afd9164f5ddb..f140f9512bcb990a82e50ed3eb271536817d4d88 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -29,40 +29,60 @@
#include <RobotAPI/libraries/core/math/MathUtils.h>
#include <VirtualRobot/math/Helpers.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
using namespace armarx;
CartesianVelocityController::CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp, const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod, bool considerJointLimits)
: rns(rns),
- considerJointLimits(considerJointLimits)
+ _considerJointLimits(considerJointLimits)
{
//ARMARX_IMPORTANT << VAROUT(rns->getRobot()->getRootNode());
ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), invJacMethod));
- this->tcp = tcp ? tcp : rns->getTCP();
+ _tcp = tcp ? tcp : rns->getTCP();
- this->cartesianMMRegularization = 100;
- this->cartesianRadianRegularization = 1;
+ _cartesianMMRegularization = 100;
+ _cartesianRadianRegularization = 1;
+ _jointCosts = Eigen::VectorXf::Constant(rns->getSize(), 1);
}
-Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, VirtualRobot::IKSolver::CartesianSelection mode)
+void CartesianVelocityController::calculateJacobis(VirtualRobot::IKSolver::CartesianSelection mode)
{
- jacobi = ik->getJacobianMatrix(tcp, mode);
+ jacobi = ik->getJacobianMatrix(_tcp, mode);
+ _jacobiWithCosts = Eigen::MatrixXf(jacobi.rows(), jacobi.cols());
+ for (int r = 0; r < jacobi.rows(); r++)
+ {
+ for (int c = 0; c < jacobi.cols(); c++)
+ {
+ _jacobiWithCosts(r, c) = jacobi(r, c) / _jointCosts(c);
+ }
+ }
+ _inv = ik->computePseudoInverseJacobianMatrix(_jacobiWithCosts, ik->getJacobiRegularization(mode));
+}
- inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
+Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, VirtualRobot::IKSolver::CartesianSelection mode)
+{
+ return calculate(cartesianVel, Eigen::VectorXf::Zero(0), mode);
+ /*calculateJacobis(mode);
- if (considerJointLimits)
+ if (_considerJointLimits)
{
- adjustJacobiForJointsAtJointLimits(mode, cartesianVel);
+ clampJacobiAtJointLimits(mode, cartesianVel, _jacobiWithCosts, _inv);
}
- Eigen::VectorXf jointVel = inv * cartesianVel;
+ Eigen::VectorXf jointVel = _inv * cartesianVel;
+ jointVel += nsv;
+ for (int r = 0; r < jointVel.rows(); r++)
+ {
+ jointVel(r) = jointVel(r) / _jointCosts(r);
+ }
if (maximumJointVelocities.rows() > 0)
{
jointVel = ::math::Helpers::LimitVectorLength(jointVel, maximumJointVelocities);
}
- return jointVel;
+ return jointVel;*/
}
Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, float KpJointLimitAvoidanceScale, VirtualRobot::IKSolver::CartesianSelection mode)
@@ -73,50 +93,44 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, VirtualRobot::IKSolver::CartesianSelection mode)
{
- //ARMARX_IMPORTANT << VAROUT(rns.get());
- //ARMARX_IMPORTANT << VAROUT(tcp.get());
- //ARMARX_IMPORTANT << VAROUT(ik.get());
- jacobi = ik->getJacobianMatrix(tcp, mode);
- // ARMARX_IMPORTANT << "nullspaceVel " << nullspaceVel.transpose();
- Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
- //ARMARX_IMPORTANT << "The rank of the jacobi is " << lu_decomp.rank();
- //ARMARX_IMPORTANT << "Null space: " << lu_decomp.kernel();
- Eigen::MatrixXf nullspace = lu_decomp.kernel();
+ calculateJacobis(mode);
- // Eigen::MatrixXf nullspace = Eigen::MatrixXf::Identity(jacobi.cols(), jacobi.cols()) - inv * jacobi;
+ Eigen::VectorXf nsv = Eigen::VectorXf::Zero(rns->getSize());
- Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
- for (int i = 0; i < nullspace.cols(); i++)
+ if (nullspaceVel.rows() > 0)
{
- float squaredNorm = nullspace.col(i).squaredNorm();
- // Prevent division by zero
- if (squaredNorm > 1.0e-32f)
+ // ARMARX_IMPORTANT << "nullspaceVel " << nullspaceVel.transpose();
+ Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(_jacobiWithCosts);
+ //ARMARX_IMPORTANT << "The rank of the _jacobiWithCosts is " << lu_decomp.rank();
+ //ARMARX_IMPORTANT << "Null space: " << lu_decomp.kernel();
+ Eigen::MatrixXf nullspace = lu_decomp.kernel();
+
+ for (int i = 0; i < nullspace.cols(); i++)
{
- nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
+ float squaredNorm = nullspace.col(i).squaredNorm();
+ // Prevent division by zero
+ if (squaredNorm > 1.0e-32f)
+ {
+ nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
+ }
}
}
- // Eigen::VectorXf nsv = nullspace * nullspaceVel;
-
- //nsv /= kernel.cols();
-
-
-
- inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
-
-
- if (considerJointLimits)
+ if (_considerJointLimits)
{
- adjustJacobiForJointsAtJointLimits(mode, cartesianVel);
+ clampJacobiAtJointLimits(mode, cartesianVel, _jacobiWithCosts, _inv);
}
- Eigen::VectorXf jointVel = inv * cartesianVel;
+ Eigen::VectorXf jointVel = _inv * cartesianVel;
jointVel += nsv;
-
+ for (int r = 0; r < jointVel.rows(); r++)
+ {
+ jointVel(r) = jointVel(r) / _jointCosts(r);
+ }
if (maximumJointVelocities.rows() > 0)
{
@@ -189,8 +203,8 @@ Eigen::VectorXf CartesianVelocityController::calculateNullspaceVelocity(const Ei
void CartesianVelocityController::setCartesianRegularization(float cartesianMMRegularization, float cartesianRadianRegularization)
{
- this->cartesianMMRegularization = cartesianMMRegularization;
- this->cartesianRadianRegularization = cartesianRadianRegularization;
+ _cartesianMMRegularization = cartesianMMRegularization;
+ _cartesianRadianRegularization = cartesianRadianRegularization;
}
Eigen::VectorXf CartesianVelocityController::calculateRegularization(VirtualRobot::IKSolver::CartesianSelection mode)
@@ -201,31 +215,30 @@ Eigen::VectorXf CartesianVelocityController::calculateRegularization(VirtualRobo
if (mode & VirtualRobot::IKSolver::X)
{
- regularization(i++) = 1 / cartesianMMRegularization;
+ regularization(i++) = 1 / _cartesianMMRegularization;
}
if (mode & VirtualRobot::IKSolver::Y)
{
- regularization(i++) = 1 / cartesianMMRegularization;
+ regularization(i++) = 1 / _cartesianMMRegularization;
}
if (mode & VirtualRobot::IKSolver::Z)
{
- regularization(i++) = 1 / cartesianMMRegularization;
+ regularization(i++) = 1 / _cartesianMMRegularization;
}
if (mode & VirtualRobot::IKSolver::Orientation)
{
- regularization(i++) = 1 / cartesianRadianRegularization;
- regularization(i++) = 1 / cartesianRadianRegularization;
- regularization(i++) = 1 / cartesianRadianRegularization;
+ regularization(i++) = 1 / _cartesianRadianRegularization;
+ regularization(i++) = 1 / _cartesianRadianRegularization;
+ regularization(i++) = 1 / _cartesianRadianRegularization;
}
return regularization.topRows(i);
}
-bool CartesianVelocityController::adjustJacobiForJointsAtJointLimits(VirtualRobot::IKSolver::CartesianSelection mode, const Eigen::VectorXf& cartesianVel, float jointLimitCheckAccuracy)
+bool CartesianVelocityController::clampJacobiAtJointLimits(VirtualRobot::IKSolver::CartesianSelection mode, const Eigen::VectorXf& cartesianVel, Eigen::MatrixXf& jacobi, Eigen::MatrixXf& inv, float jointLimitCheckAccuracy)
{
-
bool modifiedJacobi = false;
Eigen::VectorXf jointVel = inv * cartesianVel;
@@ -252,22 +265,32 @@ bool CartesianVelocityController::adjustJacobiForJointsAtJointLimits(VirtualRobo
modifiedJacobi = true;
}
}
-
if (modifiedJacobi)
{
- // calculate inverse jacobi again since atleast one joint would be driven into the limit
inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
}
+
return modifiedJacobi;
}
bool CartesianVelocityController::getConsiderJointLimits() const
{
- return considerJointLimits;
+ return _considerJointLimits;
}
void CartesianVelocityController::setConsiderJointLimits(bool value)
{
- considerJointLimits = value;
+ _considerJointLimits = value;
}
+
+void CartesianVelocityController::setJointCosts(const std::vector<float>& jointCosts)
+{
+ ARMARX_CHECK((int)jointCosts.size() == _jointCosts.rows());
+ for (size_t i = 0; i < jointCosts.size(); i++)
+ {
+ _jointCosts(i) = jointCosts.at(i);
+ }
+}
+
+
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.h b/source/RobotAPI/libraries/core/CartesianVelocityController.h
index 1639e82f6c98b52b56bb6965afe58ec28f150417..a3a577198b9a6a80534013ef3aea1218aec7da12 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.h
@@ -39,7 +39,7 @@ namespace armarx
public:
CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp = nullptr,
const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod = VirtualRobot::JacobiProvider::eSVDDamped,
- bool considerJointLimits = true);
+ bool _considerJointLimits = true);
CartesianVelocityController(CartesianVelocityController&&) = default;
CartesianVelocityController& operator=(CartesianVelocityController&&) = default;
@@ -57,18 +57,23 @@ namespace armarx
void setConsiderJointLimits(bool value);
Eigen::MatrixXf jacobi;
- Eigen::MatrixXf inv;
VirtualRobot::RobotNodeSetPtr rns;
VirtualRobot::DifferentialIKPtr ik;
- VirtualRobot::RobotNodePtr tcp;
+ VirtualRobot::RobotNodePtr _tcp;
Eigen::VectorXf maximumJointVelocities;
+ void setJointCosts(const std::vector<float>& _jointCosts);
+
private:
+ void calculateJacobis(VirtualRobot::IKSolver::CartesianSelection mode);
+ Eigen::MatrixXf _jacobiWithCosts;
+ Eigen::MatrixXf _inv;
Eigen::VectorXf calculateRegularization(VirtualRobot::IKSolver::CartesianSelection mode);
- bool adjustJacobiForJointsAtJointLimits(VirtualRobot::IKSolver::CartesianSelection mode, const Eigen::VectorXf& cartesianVel, float jointLimitCheckAccuracy = 0.001f);
- bool considerJointLimits = true;
- float cartesianMMRegularization;
- float cartesianRadianRegularization;
+ bool clampJacobiAtJointLimits(VirtualRobot::IKSolver::CartesianSelection mode, const Eigen::VectorXf& cartesianVel, Eigen::MatrixXf& jacobi, Eigen::MatrixXf& _inv, float jointLimitCheckAccuracy = 0.001f);
+ bool _considerJointLimits = true;
+ float _cartesianMMRegularization;
+ float _cartesianRadianRegularization;
+ Eigen::VectorXf _jointCosts;
};
}
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
index 3c9f18f9b17a83be41a6807b3447a11024a1ac55..a72fe97e50b3249595840b6fe18e1aca8be36542 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
@@ -40,8 +40,8 @@ namespace armarx
void CartesianVelocityControllerWithRamp::setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity)
{
- Eigen::MatrixXf jacobi = controller.ik->getJacobianMatrix(controller.tcp, mode);
- Eigen::MatrixXf inv = controller.ik->computePseudoInverseJacobianMatrix(jacobi, controller.ik->getJacobiRegularization(mode));
+ Eigen::MatrixXf jacobi = controller.ik->getJacobianMatrix(controller._tcp, mode);
+ //Eigen::MatrixXf inv = controller.ik->computePseudoInverseJacobianMatrix(jacobi, controller.ik->getJacobiRegularization(mode));
Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
@@ -57,6 +57,18 @@ namespace armarx
}
+ void CartesianVelocityControllerWithRamp::switchMode(const Eigen::VectorXf& currentJointVelocity, VirtualRobot::IKSolver::CartesianSelection mode)
+ {
+ Eigen::MatrixXf jacobi = controller.ik->getJacobianMatrix(controller._tcp, mode);
+ cartesianVelocityRamp.setState(jacobi * currentJointVelocity, mode);
+ this->mode = mode;
+ }
+
+ VirtualRobot::IKSolver::CartesianSelection CartesianVelocityControllerWithRamp::getMode()
+ {
+ return mode;
+ }
+
Eigen::VectorXf CartesianVelocityControllerWithRamp::calculate(const Eigen::VectorXf& cartesianVel, float jointLimitAvoidanceScale, float dt)
{
Eigen::VectorXf nullspaceVel = controller.calculateNullspaceVelocity(cartesianVel, jointLimitAvoidanceScale, mode);
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
index 0c47ae498f31e26c99baebde360335416150f015..ff1ff84535fad72d2fd3ef06e3adda8b7c9d08ba 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
@@ -52,6 +52,9 @@ namespace armarx
[[deprecated("compued null space velocity does not match pseudo inverse svd method in simox. never use this function.")]]
void setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity);
+ void switchMode(const Eigen::VectorXf& currentJointVelocity, VirtualRobot::IKSolver::CartesianSelection mode);
+ VirtualRobot::IKSolver::CartesianSelection getMode();
+
Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, float jointLimitAvoidanceScale, float dt);
Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, float dt);
diff --git a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
index 216c48d906af3f8ea6e14f3a2a89424a1ce13c22..ce26e1c141573e7033a5dd3272f63d5dad44578a 100644
--- a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
+++ b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
@@ -116,19 +116,19 @@ BOOST_AUTO_TEST_CASE(testJointLimitAwareness)
// ARMARX_INFO << "diff\n" << (inv-inv2);
rns->setJointValues(oldJV);
- Eigen::Vector3f posBefore = h->tcp->getPositionInRootFrame();
+ Eigen::Vector3f posBefore = h->_tcp->getPositionInRootFrame();
float accuracy = 0.001f;
Eigen::VectorXf jointVel = inv * cartesianVel;
rns->setJointValues(oldJV + jointVel * accuracy);
- Eigen::VectorXf resultCartesianVel = ((h->tcp->getPositionInRootFrame() - posBefore) / accuracy);
+ Eigen::VectorXf resultCartesianVel = ((h->_tcp->getPositionInRootFrame() - posBefore) / accuracy);
Eigen::VectorXf jointVel2 = inv2 * cartesianVel;
rns->setJointValues(oldJV + jointVel2 * accuracy);
- Eigen::VectorXf resultCartesianVel2 = ((h->tcp->getPositionInRootFrame() - posBefore) / accuracy);
+ Eigen::VectorXf resultCartesianVel2 = ((h->_tcp->getPositionInRootFrame() - posBefore) / accuracy);
Eigen::Vector3f diff = (resultCartesianVel - cartesianVel);
Eigen::Vector3f diff2 = (resultCartesianVel2 - cartesianVel);
diff --git a/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp b/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp
index d77793a8454d01d93aa75480505c174d7a82b402..f9a6e2a1022e46cae08520a4313a503fc52e022c 100644
--- a/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp
+++ b/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp
@@ -39,8 +39,10 @@ namespace armarx
return vec;
}
- NaturalDiffIK::Result NaturalDiffIK::CalculateDiffIK(const Eigen::Matrix4f& targetPose, const Eigen::Vector3f& elbowTarget, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, VirtualRobot::RobotNodePtr elbow, VirtualRobot::IKSolver::CartesianSelection mode, Parameters params)
+ NaturalDiffIK::Result NaturalDiffIK::CalculateDiffIK(const Eigen::Matrix4f& targetPose, const Eigen::Vector3f& elbowTarget, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, VirtualRobot::RobotNodePtr elbow, Mode setOri, Parameters params)
{
+ VirtualRobot::IKSolver::CartesianSelection mode = ModeToCartesianSelection(setOri);
+
CartesianVelocityController vcTcp(rns);
CartesianPositionController pcTcp(tcp);
CartesianVelocityController vcElb(rns, elbow);
@@ -130,7 +132,7 @@ namespace armarx
result.oriDiff = pcTcp.getOrientationDiff(targetPose);
result.posError = pcTcp.getPositionError(targetPose);
result.oriError = pcTcp.getOrientationError(targetPose);
- result.reached = result.posError < params.maxPosError && result.oriError < params.maxOriError;
+ result.reached = result.posError < params.maxPosError && (setOri == Mode::Position || result.oriError < params.maxOriError);
result.posDiffElbow = pcElb.getPositionDiffVec3(elbowTarget);
result.posErrorElbow = result.posDiffElbow.norm();
@@ -153,6 +155,11 @@ namespace armarx
return result;
}
+ VirtualRobot::IKSolver::CartesianSelection NaturalDiffIK::ModeToCartesianSelection(NaturalDiffIK::Mode mode)
+ {
+ return mode == Mode::All ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position;
+ }
+
}
diff --git a/source/RobotAPI/libraries/diffik/NaturalDiffIK.h b/source/RobotAPI/libraries/diffik/NaturalDiffIK.h
index 852100d69d23c3a56adae83c3a6ea9fc44cfe99f..f4c9a5af180f2ee62426584c3cbd4e0e06befc05 100644
--- a/source/RobotAPI/libraries/diffik/NaturalDiffIK.h
+++ b/source/RobotAPI/libraries/diffik/NaturalDiffIK.h
@@ -35,6 +35,7 @@ namespace armarx
class NaturalDiffIK
{
public:
+ enum class Mode {Position, All};
struct Parameters
{
Parameters() {}
@@ -84,7 +85,7 @@ namespace armarx
};
static Eigen::VectorXf LimitInfNormTo(Eigen::VectorXf vec, float maxValue);
- static Result CalculateDiffIK(const Eigen::Matrix4f& targetPose, const Eigen::Vector3f& elbowTarget, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, VirtualRobot::RobotNodePtr elbow, VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All, Parameters params = Parameters());
-
+ static Result CalculateDiffIK(const Eigen::Matrix4f& targetPose, const Eigen::Vector3f& elbowTarget, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, VirtualRobot::RobotNodePtr elbow, Mode setOri, Parameters params = Parameters());
+ static VirtualRobot::IKSolver::CartesianSelection ModeToCartesianSelection(Mode mode);
};
}
diff --git a/source/RobotAPI/libraries/natik/CMakeLists.txt b/source/RobotAPI/libraries/natik/CMakeLists.txt
index 4666fee389b0750db77e15320a9aa1a6443fac29..6a0001ea4a3a087f76a134786156612cf8a70d4b 100644
--- a/source/RobotAPI/libraries/natik/CMakeLists.txt
+++ b/source/RobotAPI/libraries/natik/CMakeLists.txt
@@ -7,6 +7,7 @@ set(LIBS
ArmarXCore ArmarXCoreInterfaces
RobotAPICore RobotAPIInterfaces
diffik
+ aron
)
set(LIB_FILES
diff --git a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
index baa385003c479c2ded340abbc6334283d9abf72c..ea98170c468c82a599e5dd1f22a73f2a76ffaaa5 100644
--- a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
+++ b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
@@ -23,13 +23,43 @@
#include "CartesianNaturalPositionControllerProxy.h"
#include <VirtualRobot/math/Helpers.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/core/CartesianPositionController.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/libraries/aron/AronNavigator.h>
using namespace armarx;
-CartesianNaturalPositionControllerProxy::CartesianNaturalPositionControllerProxy(const NaturalIK& ik, const RobotUnitInterfacePrx& robotUnit, const std::string& controllerName, NJointCartesianNaturalPositionControllerConfigPtr config)
- : ik(ik), robotUnit(robotUnit), controllerName(controllerName), config(config)
+CartesianNaturalPositionControllerProxy::CartesianNaturalPositionControllerProxy(const NaturalIK& ik, const NaturalIK::ArmJoints& arm, const RobotUnitInterfacePrx& robotUnit, const std::string& controllerName, NJointCartesianNaturalPositionControllerConfigPtr config)
+ : _ik(ik), _robotUnit(robotUnit), _controllerName(controllerName), _config(config), _arm(arm)
{
+ _runCfg = _config->runCfg;
+ _velocityBaseSettings.basePosVel = _runCfg.maxTcpPosVel;
+ _velocityBaseSettings.baseOriVel = _runCfg.maxTcpOriVel;
+ _velocityBaseSettings.baseJointVelocity = CalculateMaxJointVelocity(arm.rns, _runCfg.maxTcpPosVel);
+ _userNullspaceTargets = std::vector<float>(arm.rns->getSize(), std::nanf(""));
+ _naturalNullspaceTargets = std::vector<float>(arm.rns->getSize(), std::nanf(""));
+ updateBaseKpValues(_runCfg);
+ VirtualRobot::RobotNodeSetPtr rns = arm.rns;
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ VirtualRobot::RobotNodePtr rn = rns->getNode(i);
+ if (rn == arm.elbow)
+ {
+ break;
+ }
+ _rnsToElb.emplace_back(rn);
+ }
+
+ {
+ WaypointConfig wpc;
+ wpc.referencePosVel = 80;
+ wpc.thresholdTcpPosReached = 5;
+ _defaultWaypointConfigs["default"] = wpc;
+ }
+ _ftOffset.force = Eigen::Vector3f::Zero();
+ _ftOffset.torque = Eigen::Vector3f::Zero();
}
NJointCartesianNaturalPositionControllerConfigPtr CartesianNaturalPositionControllerProxy::MakeConfig(const std::string& rns, const std::string& elbowNode)
@@ -42,39 +72,452 @@ NJointCartesianNaturalPositionControllerConfigPtr CartesianNaturalPositionContro
void CartesianNaturalPositionControllerProxy::init()
{
- NJointControllerInterfacePrx ctrl = robotUnit->getNJointController(controllerName);
+ NJointControllerInterfacePrx ctrl = _robotUnit->getNJointController(_controllerName);
if (ctrl)
{
- controllerCreated = false;
- controller = NJointCartesianNaturalPositionControllerInterfacePrx::checkedCast(ctrl);
- controller->setConfig(config->runCfg);
+ _controllerCreated = false;
+ _controller = NJointCartesianNaturalPositionControllerInterfacePrx::checkedCast(ctrl);
+ _controller->setConfig(_runCfg);
}
else
{
- ctrl = robotUnit->createNJointController("NJointCartesianNaturalPositionController", controllerName, config);
- controller = NJointCartesianNaturalPositionControllerInterfacePrx::checkedCast(ctrl);
- controllerCreated = true;
+ ctrl = _robotUnit->createNJointController("NJointCartesianNaturalPositionController", _controllerName, _config);
+ _controller = NJointCartesianNaturalPositionControllerInterfacePrx::checkedCast(ctrl);
+ _controllerCreated = true;
+ }
+ _controller->activateController();
+}
+
+bool CartesianNaturalPositionControllerProxy::setTarget(const Eigen::Matrix4f& tcpTarget, NaturalDiffIK::Mode setOri, std::optional<float> minElbowHeight)
+{
+ ScopedJointValueRestore jvr(_arm.rns);
+ _tcpTarget = tcpTarget;
+ _setOri = setOri;
+ _fwd = _ik.solveSoechtingIK(math::Helpers::Position(tcpTarget), minElbowHeight);
+ //VirtualRobot::IKSolver::CartesianSelection mode = setOri ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position;
+ NaturalDiffIK::Result ikResult = NaturalDiffIK::CalculateDiffIK(tcpTarget, _fwd.elbow, _arm.rns, _arm.tcp, _arm.elbow, setOri, _natikParams.diffIKparams);
+ if (!ikResult.reached)
+ {
+ ARMARX_ERROR << "could not solve natural IK for target: " << tcpTarget;
+ return false;
+ }
+ _elbTarget = _arm.elbow->getPoseInRootFrame();
+ _controller->setTarget(_tcpTarget, _arm.elbow->getPositionInRootFrame(), setOri == NaturalDiffIK::Mode::All);
+ if (_setJointNullspaceFromNaturalIK)
+ {
+ for (size_t i = 0; i < _rnsToElb.size(); i++)
+ {
+ _naturalNullspaceTargets.at(i) = ikResult.jointValues(i);
+ }
+ }
+ updateNullspaceTargets();
+ updateDynamicKp();
+ return true;
+}
+
+void CartesianNaturalPositionControllerProxy::setNullspaceTarget(const std::vector<float>& nullspaceTargets)
+{
+ ARMARX_CHECK(_arm.rns->getSize() == nullspaceTargets.size());
+ _userNullspaceTargets = nullspaceTargets;
+ updateNullspaceTargets();
+}
+
+Eigen::Vector3f CartesianNaturalPositionControllerProxy::getCurrentTargetPosition()
+{
+ return math::Helpers::Position(_tcpTarget);
+}
+
+Eigen::Vector3f CartesianNaturalPositionControllerProxy::getCurrentElbowTargetPosition()
+{
+ return math::Helpers::Position(_elbTarget);
+}
+
+float CartesianNaturalPositionControllerProxy::getTcpPositionError()
+{
+ return CartesianPositionController::GetPositionError(_tcpTarget, _arm.tcp);
+}
+
+float CartesianNaturalPositionControllerProxy::getTcpOrientationError()
+{
+ if (_setOri == NaturalDiffIK::Mode::Position)
+ {
+ return 0;
+ }
+ return CartesianPositionController::GetOrientationError(_tcpTarget, _arm.tcp);
+}
+
+float CartesianNaturalPositionControllerProxy::getElbPositionError()
+{
+ return CartesianPositionController::GetPositionError(_elbTarget, _arm.elbow);
+}
+
+bool CartesianNaturalPositionControllerProxy::isFinalWaypointReached()
+{
+ return isLastWaypoint() && currentWaypoint().reached(_arm.tcp);
+}
+
+void CartesianNaturalPositionControllerProxy::useCurrentFTasOffset()
+{
+ _ftOffset = _controller->getAverageFTValue();
+ _controller->setFTOffset(_ftOffset);
+}
+
+void CartesianNaturalPositionControllerProxy::enableFTLimit(float force, float torque, bool useCurrentFTasOffset)
+{
+ if (useCurrentFTasOffset)
+ {
+ this->useCurrentFTasOffset();
+ }
+ _controller->setFTLimit(force, torque);
+}
+
+void CartesianNaturalPositionControllerProxy::disableFTLimit()
+{
+ _controller->clearFTLimit();
+ //_controller->resetFTOffset();
+}
+
+FTSensorValue CartesianNaturalPositionControllerProxy::getCurrentFTValue(bool substactOffset)
+{
+ FTSensorValue ft = _controller->getCurrentFTValue();
+ if (substactOffset)
+ {
+ ft.force = ft.force - _ftOffset.force;
+ ft.torque = ft.torque - _ftOffset.torque;
+ }
+ return ft;
+}
+armarx::FTSensorValue armarx::CartesianNaturalPositionControllerProxy::getAverageFTValue(bool substactOffset)
+{
+ FTSensorValue ft = _controller->getAverageFTValue();
+ if (substactOffset)
+ {
+ ft.force = ft.force - _ftOffset.force;
+ ft.torque = ft.torque - _ftOffset.torque;
+ }
+ return ft;
+}
+
+void CartesianNaturalPositionControllerProxy::stopClear()
+{
+ _controller->stopMovement();
+ _controller->clearFTLimit();
+ _controller->setNullspaceControlEnabled(true);
+ clearWaypoints();
+}
+
+
+
+void CartesianNaturalPositionControllerProxy::updateDynamicKp()
+{
+ if (_dynamicKp.enabled)
+ {
+ float error = (math::Helpers::Position(_tcpTarget) - _fwd.wrist).norm();
+ float KpElb, KpJla;
+ ARMARX_IMPORTANT << VAROUT(error);
+ _dynamicKp.calculate(error, KpElb, KpJla);
+ //ARMARX_IMPORTANT << VAROUT()
+ _runCfg.elbowKp = KpElb;
+ _runCfg.jointLimitAvoidanceKp = KpJla;
+ _controller->setConfig(_runCfg);
+ ARMARX_IMPORTANT << VAROUT(_runCfg.elbowKp) << VAROUT(_runCfg.jointLimitAvoidanceKp);
+
+ }
+}
+
+void CartesianNaturalPositionControllerProxy::updateNullspaceTargets()
+{
+ std::vector<float> nsTargets = _userNullspaceTargets;
+ for (size_t i = 0; i < _naturalNullspaceTargets.size(); i++)
+ {
+ if (std::isnan(nsTargets.at(i)))
+ {
+ nsTargets.at(i) = _naturalNullspaceTargets.at(i);
+ }
+ }
+ _controller->setNullspaceTarget(nsTargets);
+}
+
+void CartesianNaturalPositionControllerProxy::DynamicKp::calculate(float error, float& KpElb, float& KpJla)
+{
+ float f = std::exp(-0.5f * (error * error) / (sigmaMM * sigmaMM));
+ KpElb = f * maxKp;
+ KpJla = (1 - f) * maxKp;
+}
+
+void CartesianNaturalPositionControllerProxy::setRuntimeConfig(CartesianNaturalPositionControllerConfig runCfg)
+{
+ _controller->setConfig(runCfg);
+ this->_runCfg = runCfg;
+}
+
+CartesianNaturalPositionControllerConfig CartesianNaturalPositionControllerProxy::getRuntimeConfig()
+{
+ return _runCfg;
+}
+
+void CartesianNaturalPositionControllerProxy::addWaypoint(const CartesianNaturalPositionControllerProxy::Waypoint& waypoint)
+{
+ if (_waypoints.size() == 0)
+ {
+ _waypointChanged = true;
+ }
+ _waypoints.emplace_back(waypoint);
+}
+
+CartesianNaturalPositionControllerProxy::Waypoint CartesianNaturalPositionControllerProxy::createWaypoint(const Eigen::Vector3f& tcpTarget, const std::vector<float>& userNullspaceTargets, std::optional<float> minElbowHeight)
+{
+ return createWaypoint(tcpTarget, minElbowHeight).setNullspaceTargets(userNullspaceTargets);
+}
+
+CartesianNaturalPositionControllerProxy::Waypoint CartesianNaturalPositionControllerProxy::createWaypoint(const Eigen::Vector3f& tcpTarget, std::optional<float> minElbowHeight)
+{
+ Waypoint w;
+ w.config = _defaultWaypointConfigs["default"];
+ w.targets.tcpTarget = math::Helpers::CreatePose(tcpTarget, Eigen::Matrix3f::Identity());
+ w.targets.setOri = NaturalDiffIK::Mode::Position;
+ w.targets.minElbowHeight = minElbowHeight;
+ w.targets.userNullspaceTargets = std::vector<float>(_arm.rns->getSize(), std::nanf(""));
+ return w;
+}
+
+CartesianNaturalPositionControllerProxy::Waypoint CartesianNaturalPositionControllerProxy::createWaypoint(const Eigen::Matrix4f& tcpTarget, std::optional<float> minElbowHeight)
+{
+ Waypoint w;
+ w.config = _defaultWaypointConfigs["default"];
+ w.targets.tcpTarget = tcpTarget;
+ w.targets.setOri = NaturalDiffIK::Mode::All;
+ w.targets.minElbowHeight = minElbowHeight;
+ w.targets.userNullspaceTargets = std::vector<float>(_arm.rns->getSize(), std::nanf(""));
+ return w;
+}
+
+void CartesianNaturalPositionControllerProxy::clearWaypoints()
+{
+ _waypoints.clear();
+ _currentWaypointIndex = 0;
+}
+
+void CartesianNaturalPositionControllerProxy::setDefaultWaypointConfig(const CartesianNaturalPositionControllerProxy::WaypointConfig& config)
+{
+ _defaultWaypointConfigs["default"] = config;
+}
+
+std::string CartesianNaturalPositionControllerProxy::getStatusText()
+{
+ std::stringstream ss;
+ ss.precision(2);
+ ss << std::fixed << "Waypoint: " << (_currentWaypointIndex + 1) << "/" << _waypoints.size() << " distance: " << getTcpPositionError() << " mm " << VirtualRobot::MathTools::rad2deg(getTcpOrientationError()) << " deg";
+ return ss.str();
+}
+
+std::vector<float> CartesianNaturalPositionControllerProxy::CalculateMaxJointVelocity(const VirtualRobot::RobotNodeSetPtr& rns, float maxPosVel)
+{
+ size_t len = rns->getSize();
+ std::vector<Eigen::Vector3f> positions;
+ for (size_t i = 0; i < len; i++)
+ {
+ positions.push_back(rns->getNode(i)->getPositionInRootFrame());
+ }
+ positions.push_back(rns->getTCP()->getPositionInRootFrame());
+
+ std::vector<float> dists;
+ for (size_t i = 0; i < len; i++)
+ {
+ dists.push_back((positions.at(i) - positions.at(i + 1)).norm());
+ }
+
+ std::vector<float> result(len, 0);
+ float dist = 0;
+ for (int i = len - 1; i >= 0; i--)
+ {
+ dist += dists.at(i);
+ result.at(i) = maxPosVel / dist;
}
- controller->activateController();
+ return result;
+}
+
+std::vector<float> CartesianNaturalPositionControllerProxy::ScaleVec(const std::vector<float>& vec, float scale)
+{
+ std::vector<float> result(vec.size(), 0);
+ for (size_t i = 0; i < vec.size(); i++)
+ {
+ result.at(i) = vec.at(i) * scale;
+ }
+ return result;
+}
+
+void CartesianNaturalPositionControllerProxy::setMaxVelocities(float referencePosVel)
+{
+ VelocityBaseSettings& v = _velocityBaseSettings;
+ KpBaseSettings& k = _kpBaseSettings;
+ float scale = referencePosVel / v.basePosVel;
+ _runCfg.maxTcpPosVel = v.basePosVel * v.scaleTcpPosVel * scale;
+ _runCfg.maxTcpOriVel = v.baseOriVel * v.scaleTcpOriVel * scale;
+ _runCfg.maxElbPosVel = v.basePosVel * v.scaleElbPosVel * scale;
+ _runCfg.maxJointVelocity = ScaleVec(v.baseJointVelocity, v.scaleJointVelocities * scale);
+ _runCfg.maxNullspaceVelocity = ScaleVec(v.baseJointVelocity, v.scaleNullspaceVelocities * scale);
+ _runCfg.KpPos = k.baseKpTcpPos; // * scale;
+ _runCfg.KpOri = k.baseKpTcpOri; // * scale;
+ _runCfg.elbowKp = k.baseKpElbPos; // * scale;
+ _runCfg.jointLimitAvoidanceKp = k.baseKpJla; // * scale;
+ _runCfg.nullspaceTargetKp = k.baseKpNs; // * scale;
+ _runCfg.maxNullspaceAcceleration = k.maxNullspaceAcceleration; // * scale;
+ _runCfg.maxPositionAcceleration = k.maxPositionAcceleration; // * scale;
+ _runCfg.maxOrientationAcceleration = k.maxOrientationAcceleration; // * scale;
+
+ _controller->setConfig(_runCfg);
}
-void CartesianNaturalPositionControllerProxy::setTarget(const Eigen::Matrix4f& tcpTarget, std::optional<float> minElbowHeigh)
+void CartesianNaturalPositionControllerProxy::updateBaseKpValues(const CartesianNaturalPositionControllerConfig& runCfg)
{
- NaturalIK::SoechtingForwardPositions fwd = ik.solveSoechtingIK(math::Helpers::Position(tcpTarget), minElbowHeigh);
- controller->setTarget(tcpTarget, fwd.elbow);
+ _kpBaseSettings.baseKpTcpPos = _runCfg.KpPos;
+ _kpBaseSettings.baseKpTcpOri = _runCfg.KpOri;
+ _kpBaseSettings.baseKpElbPos = _runCfg.elbowKp;
+ _kpBaseSettings.baseKpJla = _runCfg.jointLimitAvoidanceKp;
+ _kpBaseSettings.baseKpNs = _runCfg.nullspaceTargetKp;
+ _kpBaseSettings.maxNullspaceAcceleration = _runCfg.maxNullspaceAcceleration;
+ _kpBaseSettings.maxPositionAcceleration = _runCfg.maxPositionAcceleration;
+ _kpBaseSettings.maxOrientationAcceleration = _runCfg.maxOrientationAcceleration;
}
void CartesianNaturalPositionControllerProxy::cleanup()
{
- if (controllerCreated)
+ if (_controllerCreated)
{
// delete controller only if it was created
- controller->deactivateAndDeleteController();
+ _controller->deactivateAndDeleteController();
}
else
{
// if the controller existed, only deactivate it
- controller->deactivateController();
+ _controller->deactivateController();
+ }
+ _controllerCreated = false;
+}
+
+bool CartesianNaturalPositionControllerProxy::update()
+{
+ if (_waypoints.size() == 0)
+ {
+ return true;
+ }
+
+ Waypoint& w = currentWaypoint();
+ if (!isLastWaypoint() && w.reached(_arm.tcp))
+ {
+ _currentWaypointIndex++;
+ _waypointChanged = true;
+ }
+ if (_waypointChanged)
+ {
+ _waypointChanged = false;
+ ARMARX_IMPORTANT << "Waypoint changed. Setting new target: ";
+ return onWaypointChanged();
}
- controllerCreated = false;
+ return true;
+
+}
+bool CartesianNaturalPositionControllerProxy::onWaypointChanged()
+{
+ Waypoint& w = currentWaypoint();
+ setMaxVelocities(w.config.referencePosVel);
+ _userNullspaceTargets = w.targets.userNullspaceTargets;
+ ARMARX_IMPORTANT << "Waypoint target position: " << math::Helpers::GetPosition(w.targets.tcpTarget).transpose();
+ return setTarget(w.targets.tcpTarget, w.targets.setOri, w.targets.minElbowHeight);
+}
+
+CartesianNaturalPositionControllerProxy::Waypoint& CartesianNaturalPositionControllerProxy::currentWaypoint()
+{
+ ARMARX_CHECK(_waypoints.size() > 0);
+ return _waypoints.at(_currentWaypointIndex);
+}
+
+bool CartesianNaturalPositionControllerProxy::isLastWaypoint()
+{
+ return _waypoints.size() == 0 || _currentWaypointIndex == _waypoints.size() - 1;
+}
+
+NJointCartesianNaturalPositionControllerInterfacePrx CartesianNaturalPositionControllerProxy::getInternalController()
+{
+ return _controller;
+}
+
+void CartesianNaturalPositionControllerProxy::setDynamicKp(DynamicKp dynamicKp)
+{
+ _dynamicKp = dynamicKp;
+ updateDynamicKp();
+}
+
+CartesianNaturalPositionControllerProxy::DynamicKp CartesianNaturalPositionControllerProxy::getDynamicKp()
+{
+ return _dynamicKp;
+}
+
+
+bool CartesianNaturalPositionControllerProxy::Waypoint::reached(const VirtualRobot::RobotNodePtr& tcp)
+{
+ return CartesianPositionController::Reached(targets.tcpTarget, tcp, targets.setOri == NaturalDiffIK::Mode::All, config.thresholdTcpPosReached, config.thresholdTcpPosReached / config.rad2mmFactor);
+}
+
+CartesianNaturalPositionControllerProxy::Waypoint& CartesianNaturalPositionControllerProxy::Waypoint::setConfig(const CartesianNaturalPositionControllerProxy::WaypointConfig& config)
+{
+ this->config = config;
+ return *this;
+}
+
+CartesianNaturalPositionControllerProxy::Waypoint& CartesianNaturalPositionControllerProxy::Waypoint::setNullspaceTargets(const std::vector<float>& userNullspaceTargets)
+{
+ ARMARX_CHECK(this->targets.userNullspaceTargets.size() == userNullspaceTargets.size());
+ this->targets.userNullspaceTargets = userNullspaceTargets;
+ return *this;
+}
+
+/*
+aron::AronObjectPtr CartesianNaturalPositionControllerProxy::Waypoint::toAron()
+{
+ aron::AronObjectPtr obj = new aron::AronObject();
+ aron::AronNavigator nav(obj);
+ nav.appendMatrix4f("tcpTarget", tcpTarget);
+ nav.appendVector3f("elbTarget", elbTarget);
+ nav.appendVec("userNullspaceTargets", userNullspaceTargets);
+ nav.appendVec("naturalNullspaceTargets", naturalNullspaceTargets);
+ nav.appendBool("setOri", setOri == NaturalDiffIK::Mode::All);
+ nav.appendFloat("referencePosVel", referencePosVel);
+ nav.appendFloat("thresholdTcpPosReached", thresholdTcpPosReached);
+ nav.appendFloat("thresholdTcpOriReached", thresholdTcpOriReached);
+ nav.appendBool("resetFTonEnter", resetFTonEnter);
+ nav.appendBool("resetFTonExit", resetFTonExit);
+ nav.appendFloat("forceTheshold", forceTheshold);
+ return obj;
}
+
+void CartesianNaturalPositionControllerProxy::Waypoint::fromAron(aron::AronObjectPtr aron)
+{
+ aron::AronNavigator nav(aron);
+ tcpTarget = nav.atKey("tcpTarget").asMatrix4f();
+ elbTarget = nav.atKey("elbTarget").asVector3f();
+ userNullspaceTargets = nav.atKey("userNullspaceTargets").asVecFloat();
+ naturalNullspaceTargets = nav.atKey("naturalNullspaceTargets").asVecFloat();
+ setOri = nav.atKey("setOri").asBool() ? NaturalDiffIK::Mode::All : NaturalDiffIK::Mode::Position;
+ referencePosVel = nav.atKey("referencePosVel").asFloat();
+ thresholdTcpPosReached = nav.atKey("thresholdTcpPosReached").asFloat();
+ thresholdTcpOriReached = nav.atKey("thresholdTcpOriReached").asFloat();
+ resetFTonEnter = nav.atKey("resetFTonEnter").asBool();
+ resetFTonExit = nav.atKey("resetFTonExit").asBool();
+ forceTheshold = nav.atKey("forceTheshold").asFloat();
+
+}*/
+
+CartesianNaturalPositionControllerProxy::ScopedJointValueRestore::ScopedJointValueRestore(const VirtualRobot::RobotNodeSetPtr& rns)
+ : jointValues(rns->getJointValues()), rns(rns)
+{
+}
+
+CartesianNaturalPositionControllerProxy::ScopedJointValueRestore::~ScopedJointValueRestore()
+{
+ //ARMARX_IMPORTANT << "restoring joint values";
+ rns->setJointValues(jointValues);
+}
+
+
diff --git a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
index 46c5b4cae23d763d5bdb8a20acf140054af2ed8a..d87c65c4bf5136cf2ea3812059242ed6710c87b5 100644
--- a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
+++ b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
@@ -28,35 +28,184 @@
#include <boost/shared_ptr.hpp>
#include <RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include <RobotAPI/interface/aron.h>
namespace armarx
{
- typedef boost::shared_ptr<class CartesianNaturalPositionControllerProxy> CartesianNaturalPositionControllerProxyPtr;
+ typedef std::shared_ptr<class CartesianNaturalPositionControllerProxy> CartesianNaturalPositionControllerProxyPtr;
class CartesianNaturalPositionControllerProxy
{
public:
- CartesianNaturalPositionControllerProxy(const NaturalIK& ik, const RobotUnitInterfacePrx& robotUnit, const std::string& controllerName, NJointCartesianNaturalPositionControllerConfigPtr config);
+ struct DynamicKp
+ {
+ bool enabled = false;
+ float maxKp = 1;
+ float sigmaMM = 50;
+ void calculate(float error, float& KpElb, float& KpJla);
+ };
+
+ struct KpBaseSettings
+ {
+ float baseKpTcpPos;
+ float baseKpTcpOri;
+ float baseKpElbPos;
+ float baseKpJla;
+ float baseKpNs;
+ float maxPositionAcceleration;
+ float maxOrientationAcceleration;
+ float maxNullspaceAcceleration;
+ };
+
+ struct VelocityBaseSettings
+ {
+ float scaleTcpPosVel = 1;
+ float scaleTcpOriVel = 1;
+ float scaleElbPosVel = 1;
+ float scaleJointVelocities = 1;
+ float scaleNullspaceVelocities = 1;
+
+ Ice::FloatSeq baseJointVelocity;
+ float basePosVel;
+ float baseOriVel;
+ };
+
+ struct WaypointConfig
+ {
+ float referencePosVel;
+ float thresholdTcpPosReached;
+ float rad2mmFactor = 100 / M_PI;
+ //float thresholdTcpOriReached;
+ };
+ struct WaypointTargets
+ {
+ Eigen::Matrix4f tcpTarget;
+ NaturalDiffIK::Mode setOri;
+ std::optional<float> minElbowHeight = std::nullopt;
+ std::vector<float> userNullspaceTargets;
+ };
+
+ //typedef std::shared_ptr<class Waypoint> WaypointPtr;
+ class Waypoint
+ {
+ public:
+ WaypointTargets targets;
+ WaypointConfig config;
+ //bool offsetFTonEnter = false;
+ //bool resetFTonExit = false;
+ //float forceTheshold = -1;
+
+ bool reached(const VirtualRobot::RobotNodePtr& tcp);
+ Waypoint& setConfig(const WaypointConfig& config);
+ Waypoint& setNullspaceTargets(const std::vector<float>& userNullspaceTargets);
+
+ //aron::AronObjectPtr toAron();
+ //void fromAron(aron::AronObjectPtr aron);
+ };
+
+ class ScopedJointValueRestore
+ {
+ public:
+ ScopedJointValueRestore(const VirtualRobot::RobotNodeSetPtr& rns);
+ ~ScopedJointValueRestore();
+ private:
+ std::vector<float> jointValues;
+ VirtualRobot::RobotNodeSetPtr rns;
+ };
+
+
+ CartesianNaturalPositionControllerProxy(const NaturalIK& _ik, const NaturalIK::ArmJoints& arm, const RobotUnitInterfacePrx& _robotUnit, const std::string& _controllerName, NJointCartesianNaturalPositionControllerConfigPtr _config);
static NJointCartesianNaturalPositionControllerConfigPtr MakeConfig(const std::string& rns, const std::string& elbowNode);
void init();
- void setTarget(const Eigen::Matrix4f& tcpTarget, std::optional<float> minElbowHeight = std::nullopt);
+ bool setTarget(const Eigen::Matrix4f& tcpTarget, NaturalDiffIK::Mode setOri, std::optional<float> minElbowHeight = std::nullopt);
+ void setNullspaceTarget(const std::vector<float>& nullspaceTargets);
+
+ Eigen::Vector3f getCurrentTargetPosition();
+ Eigen::Vector3f getCurrentElbowTargetPosition();
+
+ float getTcpPositionError();
+ float getTcpOrientationError();
+ float getElbPositionError();
+
+ bool isFinalWaypointReached();
+
+ void useCurrentFTasOffset();
+ void enableFTLimit(float force, float torque, bool useCurrentFTasOffset);
+ void disableFTLimit();
+ FTSensorValue getCurrentFTValue(bool substactOffset);
+ FTSensorValue getAverageFTValue(bool substactOffset);
+ void stopClear();
+
+
+ void setRuntimeConfig(CartesianNaturalPositionControllerConfig runCfg);
+ CartesianNaturalPositionControllerConfig getRuntimeConfig();
+
+ void addWaypoint(const Waypoint& waypoint);
+ Waypoint createWaypoint(const Eigen::Vector3f& tcpTarget, const std::vector<float>& userNullspaceTargets, std::optional<float> minElbowHeight = std::nullopt);
+ Waypoint createWaypoint(const Eigen::Vector3f& tcpTarget, std::optional<float> minElbowHeight = std::nullopt);
+ Waypoint createWaypoint(const Eigen::Matrix4f& tcpTarget, std::optional<float> minElbowHeight = std::nullopt);
+ void clearWaypoints();
+ void setDefaultWaypointConfig(const WaypointConfig& config);
+
+
+ std::string getStatusText();
//void setTargetVelocity(const Eigen::VectorXf& cv);
//void setNullSpaceControl(bool enabled);
+ static std::vector<float> CalculateMaxJointVelocity(const VirtualRobot::RobotNodeSetPtr& rns, float maxPosVel);
+
+ void setMaxVelocities(float referencePosVel);
+ void updateBaseKpValues(const CartesianNaturalPositionControllerConfig& runCfg);
+
+ bool update();
+ Waypoint& currentWaypoint();
+ bool isLastWaypoint();
+
void cleanup();
+ NJointCartesianNaturalPositionControllerInterfacePrx getInternalController();
+
+ void setDynamicKp(DynamicKp dynamicKp);
+ DynamicKp getDynamicKp();
+ static std::vector<float> ScaleVec(const std::vector<float>& vec, float scale);
+
private:
- NaturalIK ik;
- RobotUnitInterfacePrx robotUnit;
- std::string side;
- std::string controllerName;
- NJointCartesianNaturalPositionControllerConfigPtr config;
- bool controllerCreated = false;
- NJointCartesianNaturalPositionControllerInterfacePrx controller;
+ void updateDynamicKp();
+ void updateNullspaceTargets();
+
+ bool onWaypointChanged();
+
+ NaturalIK _ik;
+ RobotUnitInterfacePrx _robotUnit;
+ std::string _side;
+ std::string _controllerName;
+ NJointCartesianNaturalPositionControllerConfigPtr _config;
+ CartesianNaturalPositionControllerConfig _runCfg;
+ bool _controllerCreated = false;
+ NJointCartesianNaturalPositionControllerInterfacePrx _controller;
+ DynamicKp _dynamicKp;
+ Eigen::Matrix4f _tcpTarget;
+ Eigen::Matrix4f _elbTarget;
+ NaturalDiffIK::Mode _setOri;
+ NaturalIK::SoechtingForwardPositions _fwd;
+ VelocityBaseSettings _velocityBaseSettings;
+ KpBaseSettings _kpBaseSettings;
+ NaturalIK::ArmJoints _arm;
+ std::vector<Waypoint> _waypoints;
+ size_t _currentWaypointIndex = 0;
+ NaturalIK::Parameters _natikParams;
+ std::vector<float> _userNullspaceTargets;
+ std::vector<float> _naturalNullspaceTargets;
+ std::vector<VirtualRobot::RobotNodePtr> _rnsToElb;
+ bool _setJointNullspaceFromNaturalIK = true;
+
+ std::map<std::string, WaypointConfig> _defaultWaypointConfigs;
+ bool _waypointChanged = false;
+ FTSensorValue _ftOffset;
};
}
diff --git a/source/RobotAPI/libraries/natik/NaturalIK.cpp b/source/RobotAPI/libraries/natik/NaturalIK.cpp
index 37c2406dd00d78d3289b804e464433c1bb65fd1d..88c57b35768281fee04794b07cf789318203f140 100644
--- a/source/RobotAPI/libraries/natik/NaturalIK.cpp
+++ b/source/RobotAPI/libraries/natik/NaturalIK.cpp
@@ -85,14 +85,22 @@ NaturalDiffIK::Result NaturalIK::calculateIK(const Eigen::Matrix4f& targetPose,
{
Eigen::Vector3f targetPos = math::Helpers::Position(targetPose);
NaturalIK::SoechtingForwardPositions fwd = solveSoechtingIK(targetPos, params.minimumElbowHeight);
- return NaturalDiffIK::CalculateDiffIK(targetPose, fwd.elbow, arm.rns, arm.tcp, arm.elbow, VirtualRobot::IKSolver::All, params.diffIKparams);
+ return NaturalDiffIK::CalculateDiffIK(targetPose, fwd.elbow, arm.rns, arm.tcp, arm.elbow, NaturalDiffIK::Mode::All, params.diffIKparams);
}
NaturalDiffIK::Result NaturalIK::calculateIKpos(const Eigen::Vector3f& targetPos, NaturalIK::ArmJoints arm, NaturalIK::Parameters params)
{
Eigen::Matrix4f targetPose = math::Helpers::Pose(targetPos, Eigen::Matrix3f::Identity());
NaturalIK::SoechtingForwardPositions fwd = solveSoechtingIK(targetPos, params.minimumElbowHeight);
- return NaturalDiffIK::CalculateDiffIK(targetPose, fwd.elbow, arm.rns, arm.tcp, arm.elbow, VirtualRobot::IKSolver::Position, params.diffIKparams);
+ return NaturalDiffIK::CalculateDiffIK(targetPose, fwd.elbow, arm.rns, arm.tcp, arm.elbow, NaturalDiffIK::Mode::Position, params.diffIKparams);
+}
+
+NaturalDiffIK::Result NaturalIK::calculateIK(const Eigen::Matrix4f& targetPose, NaturalIK::ArmJoints arm, NaturalDiffIK::Mode setOri, NaturalIK::Parameters params)
+{
+ Eigen::Vector3f targetPos = math::Helpers::Position(targetPose);
+ NaturalIK::SoechtingForwardPositions fwd = solveSoechtingIK(targetPos, params.minimumElbowHeight);
+ //VirtualRobot::IKSolver::CartesianSelection mode = setOri ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position;
+ return NaturalDiffIK::CalculateDiffIK(targetPose, fwd.elbow, arm.rns, arm.tcp, arm.elbow, setOri, params.diffIKparams);
}
Eigen::Vector3f NaturalIK::getShoulderPos()
diff --git a/source/RobotAPI/libraries/natik/NaturalIK.h b/source/RobotAPI/libraries/natik/NaturalIK.h
index 3a7a0506456dfb88f2dded2868644188a61130d3..6db3aa965d5860f4bb5bc1e2bca48901b08e8b04 100644
--- a/source/RobotAPI/libraries/natik/NaturalIK.h
+++ b/source/RobotAPI/libraries/natik/NaturalIK.h
@@ -97,6 +97,7 @@ namespace armarx
NaturalDiffIK::Result calculateIK(const Eigen::Matrix4f& targetPose, ArmJoints arm, NaturalIK::Parameters params = NaturalIK::Parameters());
NaturalDiffIK::Result calculateIKpos(const Eigen::Vector3f& targetPos, ArmJoints arm, NaturalIK::Parameters params = NaturalIK::Parameters());
+ NaturalDiffIK::Result calculateIK(const Eigen::Matrix4f& targetPose, ArmJoints arm, NaturalDiffIK::Mode setOri, NaturalIK::Parameters params = NaturalIK::Parameters());
//static SimpleDiffIK::Result CalculateNaturalIK(const Eigen::Matrix4f targetPose, ArmJoints armjoints, Parameters params = Parameters());
diff --git a/source/RobotAPI/statecharts/CMakeLists.txt b/source/RobotAPI/statecharts/CMakeLists.txt
index e0c5fed5a7aacbf6021d906083b0e13592bad5d8..006016c5daa24d555c8a5d9537b8bfbbd0c96229 100644
--- a/source/RobotAPI/statecharts/CMakeLists.txt
+++ b/source/RobotAPI/statecharts/CMakeLists.txt
@@ -11,4 +11,5 @@ add_subdirectory(ForceTorqueUtility)
add_subdirectory(RobotNameHelperTestGroup)
add_subdirectory(StatechartExecutionGroup)
-add_subdirectory(ProsthesisKinestheticTeachIn)
\ No newline at end of file
+add_subdirectory(ProsthesisKinestheticTeachIn)
+add_subdirectory(DebugDrawerToArVizGroup)
diff --git a/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/CMakeLists.txt b/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..ce60f3cb72843143bb065b659ef5d26e9ff30b6c
--- /dev/null
+++ b/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/CMakeLists.txt
@@ -0,0 +1,33 @@
+armarx_component_set_name("DebugDrawerToArVizGroup")
+
+
+#find_package(Simox QUIET)
+#armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+set(COMPONENT_LIBS
+# VirtualRobot
+ )
+
+# Sources
+
+set(SOURCES
+DebugDrawerToArVizGroupRemoteStateOfferer.cpp
+)
+
+set(HEADERS
+DebugDrawerToArVizGroupRemoteStateOfferer.h
+DebugDrawerToArVizGroup.scgxml
+)
+
+# adds all existing state headers and sources to CMake
+armarx_generate_statechart_cmake_lists()
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+#find_package(MyLib QUIET)
+#armarx_build_if(MyLib_FOUND "MyLib not available")
+# all target_include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+#if(MyLib_FOUND)
+# target_include_directories(DebugDrawerToArVizGroup PUBLIC ${MyLib_INCLUDE_DIRS})
+#endif()
diff --git a/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/DebugDrawerToArVizGroup.scgxml b/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/DebugDrawerToArVizGroup.scgxml
new file mode 100644
index 0000000000000000000000000000000000000000..5509897ee85397385900f8294d7bba2725f5298a
--- /dev/null
+++ b/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/DebugDrawerToArVizGroup.scgxml
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="utf-8"?>
+<StatechartGroup name="DebugDrawerToArVizGroup" package="RobotAPI" generateContext="true">
+ <Proxies>
+ <Proxy value="RobotAPIInterfaces.debugDrawerToArVizLayerBlackWhitelist"/>
+ </Proxies>
+ <State filename="UpateLayerBlackWhitelist.xml" visibility="public"/>
+</StatechartGroup>
+
diff --git a/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/DebugDrawerToArVizGroupRemoteStateOfferer.cpp b/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/DebugDrawerToArVizGroupRemoteStateOfferer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..74f4eb5bf9130cde377e4fa99cce17b62f944bac
--- /dev/null
+++ b/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/DebugDrawerToArVizGroupRemoteStateOfferer.cpp
@@ -0,0 +1,51 @@
+/*
+ * 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 RobotAPI::DebugDrawerToArVizGroup::DebugDrawerToArVizGroupRemoteStateOfferer
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "DebugDrawerToArVizGroupRemoteStateOfferer.h"
+
+namespace armarx::DebugDrawerToArVizGroup
+{
+ // DO NOT EDIT NEXT LINE
+ DebugDrawerToArVizGroupRemoteStateOfferer::SubClassRegistry DebugDrawerToArVizGroupRemoteStateOfferer::Registry(DebugDrawerToArVizGroupRemoteStateOfferer::GetName(), &DebugDrawerToArVizGroupRemoteStateOfferer::CreateInstance);
+
+ DebugDrawerToArVizGroupRemoteStateOfferer::DebugDrawerToArVizGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader) :
+ XMLRemoteStateOfferer < DebugDrawerToArVizGroupStatechartContext > (reader)
+ {}
+
+ void DebugDrawerToArVizGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer() {}
+
+ void DebugDrawerToArVizGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer() {}
+
+ void DebugDrawerToArVizGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer() {}
+
+ // DO NOT EDIT NEXT FUNCTION
+ std::string DebugDrawerToArVizGroupRemoteStateOfferer::GetName()
+ {
+ return "DebugDrawerToArVizGroupRemoteStateOfferer";
+ }
+
+ // DO NOT EDIT NEXT FUNCTION
+ XMLStateOffererFactoryBasePtr DebugDrawerToArVizGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
+ {
+ return XMLStateOffererFactoryBasePtr(new DebugDrawerToArVizGroupRemoteStateOfferer(reader));
+ }
+}
diff --git a/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/DebugDrawerToArVizGroupRemoteStateOfferer.h b/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/DebugDrawerToArVizGroupRemoteStateOfferer.h
new file mode 100644
index 0000000000000000000000000000000000000000..245714e2eda9b91edc7b56e474739517670a7e81
--- /dev/null
+++ b/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/DebugDrawerToArVizGroupRemoteStateOfferer.h
@@ -0,0 +1,50 @@
+/*
+ * 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 RobotAPI::DebugDrawerToArVizGroup
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
+#include "DebugDrawerToArVizGroupStatechartContext.generated.h"
+
+namespace armarx::DebugDrawerToArVizGroup
+{
+ class DebugDrawerToArVizGroupRemoteStateOfferer :
+ virtual public XMLRemoteStateOfferer < DebugDrawerToArVizGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
+ {
+ public:
+ DebugDrawerToArVizGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
+
+ // inherited from RemoteStateOfferer
+ void onInitXMLRemoteStateOfferer() override;
+ void onConnectXMLRemoteStateOfferer() override;
+ void onExitXMLRemoteStateOfferer() override;
+
+ // static functions for AbstractFactory Method
+ static std::string GetName();
+ static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
+ static SubClassRegistry Registry;
+
+
+ };
+}
+
+
diff --git a/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/UpateLayerBlackWhitelist.cpp b/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/UpateLayerBlackWhitelist.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f2be2a6d19333b692ea006f7174ed61cb61450ea
--- /dev/null
+++ b/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/UpateLayerBlackWhitelist.cpp
@@ -0,0 +1,87 @@
+/*
+ * 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 RobotAPI::DebugDrawerToArVizGroup
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "UpateLayerBlackWhitelist.h"
+
+//#include <ArmarXCore/core/time/TimeUtil.h>
+//#include <ArmarXCore/observers/variant/DatafieldRef.h>
+
+namespace armarx::DebugDrawerToArVizGroup
+{
+ // DO NOT EDIT NEXT LINE
+ UpateLayerBlackWhitelist::SubClassRegistry UpateLayerBlackWhitelist::Registry(UpateLayerBlackWhitelist::GetName(), &UpateLayerBlackWhitelist::CreateInstance);
+
+ void UpateLayerBlackWhitelist::onEnter()
+ {
+ // put your user code for the enter-point here
+ // execution time should be short (<100ms)
+
+ armarx::BlackWhitelistUpdate update;
+ update.blacklist.clear = in.getClearBlacklist();
+ update.blacklist.add = in.getAddToBlacklist();
+ update.blacklist.set = in.getSetBlacklist();
+ update.whitelist.clear = in.getClearWhitelist();
+ update.whitelist.add = in.getAddToWhitelist();
+ update.whitelist.set = in.getSetWhitelist();
+
+ getDebugDrawerToArVizLayerBlackWhitelist()->updateBlackWhitelist(update);
+
+ emitSuccess();
+ }
+
+ //void UpateLayerBlackWhitelist::run()
+ //{
+ // // put your user code for the execution-phase here
+ // // runs in seperate thread, thus can do complex operations
+ // // should check constantly whether isRunningTaskStopped() returns true
+ //
+ // // get a private kinematic instance for this state of the robot (tick "Robot State Component" proxy checkbox in statechart group)
+ // VirtualRobot::RobotPtr robot = getLocalRobot();
+ //
+ //// uncomment this if you need a continous run function. Make sure to use sleep or use blocking wait to reduce cpu load.
+ // while (!isRunningTaskStopped()) // stop run function if returning true
+ // {
+ // // do your calculations
+ // // synchronize robot clone to most recent state
+ // RemoteRobot::synchronizeLocalClone(robot, getRobotStateComponent());
+ // }
+ //}
+
+ //void UpateLayerBlackWhitelist::onBreak()
+ //{
+ // // put your user code for the breaking point here
+ // // execution time should be short (<100ms)
+ //}
+
+ void UpateLayerBlackWhitelist::onExit()
+ {
+ // put your user code for the exit point here
+ // execution time should be short (<100ms)
+ }
+
+
+ // DO NOT EDIT NEXT FUNCTION
+ XMLStateFactoryBasePtr UpateLayerBlackWhitelist::CreateInstance(XMLStateConstructorParams stateData)
+ {
+ return XMLStateFactoryBasePtr(new UpateLayerBlackWhitelist(stateData));
+ }
+}
diff --git a/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/UpateLayerBlackWhitelist.h b/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/UpateLayerBlackWhitelist.h
new file mode 100644
index 0000000000000000000000000000000000000000..35cef461547e726ea7ecd3fb77ad2ef35e824cac
--- /dev/null
+++ b/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/UpateLayerBlackWhitelist.h
@@ -0,0 +1,54 @@
+/*
+ * 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 RobotAPI::DebugDrawerToArVizGroup
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <RobotAPI/statecharts/DebugDrawerToArVizGroup/UpateLayerBlackWhitelist.generated.h>
+
+namespace armarx::DebugDrawerToArVizGroup
+{
+ class UpateLayerBlackWhitelist :
+ public UpateLayerBlackWhitelistGeneratedBase < UpateLayerBlackWhitelist >
+ {
+ public:
+ UpateLayerBlackWhitelist(const XMLStateConstructorParams& stateData):
+ XMLStateTemplate < UpateLayerBlackWhitelist > (stateData), UpateLayerBlackWhitelistGeneratedBase < UpateLayerBlackWhitelist > (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/RobotAPI/statecharts/DebugDrawerToArVizGroup/UpateLayerBlackWhitelist.xml b/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/UpateLayerBlackWhitelist.xml
new file mode 100644
index 0000000000000000000000000000000000000000..2a983200ca4bda988d93faae8c9d6f4a2b323ab5
--- /dev/null
+++ b/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/UpateLayerBlackWhitelist.xml
@@ -0,0 +1,34 @@
+<?xml version="1.0" encoding="utf-8"?>
+<State version="1.2" name="UpateLayerBlackWhitelist" uuid="E175EAE8-47AD-40B8-8BFB-7BB10F4D38FE" width="800" height="600" type="Normal State">
+ <InputParameters>
+ <Parameter name="AddToBlacklist" type="::armarx::SingleTypeVariantListBase(::armarx::StringVariantData)" docType="List(string)" optional="no">
+ <DefaultValue value='{"array":[],"type":"::armarx::SingleTypeVariantListBase"}' docValue=""/>
+ </Parameter>
+ <Parameter name="AddToWhitelist" type="::armarx::SingleTypeVariantListBase(::armarx::StringVariantData)" docType="List(string)" optional="no">
+ <DefaultValue value='{"array":[],"type":"::armarx::SingleTypeVariantListBase"}' docValue=""/>
+ </Parameter>
+ <Parameter name="ClearBlacklist" type="::armarx::BoolVariantData" docType="bool" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":false}}' docValue="False"/>
+ </Parameter>
+ <Parameter name="ClearWhitelist" type="::armarx::BoolVariantData" docType="bool" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":false}}' docValue="False"/>
+ </Parameter>
+ <Parameter name="SetBlacklist" type="::armarx::SingleTypeVariantListBase(::armarx::StringVariantData)" docType="List(string)" optional="no">
+ <DefaultValue value='{"array":[],"type":"::armarx::SingleTypeVariantListBase"}' docValue=""/>
+ </Parameter>
+ <Parameter name="SetWhitelist" type="::armarx::SingleTypeVariantListBase(::armarx::StringVariantData)" docType="List(string)" optional="no">
+ <DefaultValue value='{"array":[],"type":"::armarx::SingleTypeVariantListBase"}' docValue=""/>
+ </Parameter>
+ </InputParameters>
+ <OutputParameters/>
+ <LocalParameters/>
+ <Substates/>
+ <Events>
+ <Event name="Failure">
+ <Description>Event for statechart-internal failures or optionally user-code failures</Description>
+ </Event>
+ <Event name="Success"/>
+ </Events>
+ <Transitions/>
+</State>
+