diff --git a/scenarios/ArVizExample/ArVizExample.scx b/scenarios/ArVizExample/ArVizExample.scx
index 00880778f94bdf201adca7c5810da52568c0ae91..bcc46e466e16bf2cb69264abcfbbef745296e3e1 100644
--- a/scenarios/ArVizExample/ArVizExample.scx
+++ b/scenarios/ArVizExample/ArVizExample.scx
@@ -2,5 +2,6 @@
<scenario name="ArVizExample" creation="2019-05-21.14:52:29" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
<application name="ArVizExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RobotToArVizApp" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/ArVizExample/config/RobotToArVizApp.cfg b/scenarios/ArVizExample/config/RobotToArVizApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4f4ea65faf5690963ba6e1ef3a2970d18dcd6563
--- /dev/null
+++ b/scenarios/ArVizExample/config/RobotToArVizApp.cfg
@@ -0,0 +1,212 @@
+# ==================================================================
+# RobotToArVizApp properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.RobotToArViz.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotToArViz.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.RobotToArViz.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotToArViz.EnableProfiling = false
+
+
+# ArmarX.RobotToArViz.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.RobotToArViz.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.RobotToArViz.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotToArViz.ObjectName = ""
+
+
+# ArmarX.RobotToArViz.RemoteStateComponentName: Name of the robot state component
+# Attributes:
+# - Default: RobotStateComponent
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotToArViz.RemoteStateComponentName = RobotStateComponent
+
+
+# ArmarX.RobotToArViz.updateFrequency: Target number of updates per second.
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotToArViz.updateFrequency = 100
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/source/RobotAPI/components/ArViz/ArVizStorage.cpp b/source/RobotAPI/components/ArViz/ArVizStorage.cpp
index 900dbd23d29d5ba0895e84c9328c77b6ebb761c1..2b27537f1a3c0cd903b360b271ad7b02b94459ef 100644
--- a/source/RobotAPI/components/ArViz/ArVizStorage.cpp
+++ b/source/RobotAPI/components/ArViz/ArVizStorage.cpp
@@ -28,6 +28,7 @@
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/util/IceBlobToObject.h>
#include <ArmarXCore/core/util/ObjectToIceBlob.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <SimoxUtility/json/json.hpp>
diff --git a/source/RobotAPI/components/ArViz/ArVizStorage.h b/source/RobotAPI/components/ArViz/ArVizStorage.h
index c191b47dcbc7ff72d3278b24e5150ba1b5888fe9..0afaed33a07d95b74c2325e6fd7dfa64e018938b 100644
--- a/source/RobotAPI/components/ArViz/ArVizStorage.h
+++ b/source/RobotAPI/components/ArViz/ArVizStorage.h
@@ -33,6 +33,7 @@
#include <mutex>
#include <atomic>
#include <condition_variable>
+#include <filesystem>
namespace armarx
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.h b/source/RobotAPI/components/ArViz/Client/Elements.h
index 611561fb09e27e09e6c5d282ca5dadcc13693fd0..02d2c5b10fea35b8e3d7961da0a9c3e8a9dd517d 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.h
+++ b/source/RobotAPI/components/ArViz/Client/Elements.h
@@ -7,6 +7,7 @@
#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
#include <SimoxUtility/math/pose/transform.h>
#include <SimoxUtility/shapes/OrientedBoxBase.h>
+
#include <Eigen/Core>
#include <Eigen/Geometry>
@@ -19,6 +20,14 @@
#include "elements/PointCloud.h"
#include "elements/Mesh.h"
+// The has_member macro causes compile errors if *any* other header uses
+// the identifier has_member. Boost.Thread does, so this causes compile
+// errors down the line.
+// Offending file: simox/SimoxUtility/meta/has_member_macros/has_member.hpp
+#ifdef has_member
+#undef has_member
+#endif
+
namespace Eigen
{
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Mesh.cpp b/source/RobotAPI/components/ArViz/Client/elements/Mesh.cpp
index c0ce2f90900a6a7c3d493e72b3be2ed10b4d92ff..c66cc79b05827c82ad1d4a8f055c52bc446b972e 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Mesh.cpp
+++ b/source/RobotAPI/components/ArViz/Client/elements/Mesh.cpp
@@ -1,43 +1,67 @@
#include "Mesh.h"
-#include <numeric> // for std::accumulate
-
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
namespace armarx::viz
{
+ Mesh& Mesh::grid2D(Eigen::Vector2f extents, Eigen::Vector2i numPoints,
+ std::function<viz::Color(size_t, size_t, const Eigen::Vector3f&)> colorFunc)
+ {
+ // Create vertices.
+ std::vector<std::vector<Eigen::Vector3f>> vertices = grid::makeGrid2DVertices(extents, numPoints);
+
+ // Create colors.
+ std::vector<std::vector<viz::data::Color>> colors = grid::makeGrid2DColors(vertices, colorFunc);
+
+ return this->grid2D(vertices, colors);
+ }
+
- template <class T>
- std::vector<T> flatten(const std::vector<std::vector<T>>& vector)
+ Mesh& Mesh::grid2D(const std::vector<std::vector<Eigen::Vector3f> >& vertices,
+ const std::vector<std::vector<viz::data::Color>>& colors)
{
- size_t size = std::accumulate(vector.begin(), vector.end(), size_t(0), [](size_t s, const auto & v)
+ ARMARX_CHECK_EQUAL(vertices.size(), colors.size()) << "Numbers of vertices and colors must match.";
+
+ if (vertices.empty())
{
- return s + v.size();
- });
+ return *this;
+ }
+
+ const size_t num_x = vertices.size();
+ const size_t num_y = vertices.front().size();
- std::vector<T> flat;
- flat.reserve(size);
- for (const auto& v : vector)
+ bool check = false; // This could unnecessarily slow down building large meshes.
+ if (check)
{
- for (const auto& val : v)
+ // Check consistent sizes.
+ for (const auto& vv : vertices)
+ {
+ ARMARX_CHECK_EQUAL(vv.size(), num_y) << "All nested vectors must have equal length.";
+ }
+ for (const auto& cv : colors)
{
- flat.push_back(val);
+ ARMARX_CHECK_EQUAL(cv.size(), num_y) << "All nested vectors must have equal length.";
}
}
- return flat;
+
+ // Create faces.
+ const std::vector<viz::data::Face> faces = grid::makeGrid2DFaces(num_x, num_y);
+
+ // Flatten
+ return this->vertices(grid::flatten(vertices)).colors(grid::flatten(colors)).faces(faces);
}
- Mesh& Mesh::grid2D(Eigen::Vector2f extents, Eigen::Vector2i numPoints,
- std::function<viz::Color(size_t, size_t, const Eigen::Vector3f&)> colorFunc)
+ std::vector<std::vector<Eigen::Vector3f>> grid::makeGrid2DVertices(
+ Eigen::Vector2f extents, Eigen::Vector2i numPoints, float height)
{
const Eigen::Vector2f minimum = - extents / 2;
// extents = (num - 1) * step => step = extents / (num - 1)
const Eigen::Vector2f step = (extents.array() / (numPoints.array() - 1).cast<float>()).matrix();
- ARMARX_CHECK_POSITIVE(numPoints.minCoeff()) << VAROUT(numPoints);
+ ARMARX_CHECK_POSITIVE(numPoints.minCoeff()) << "Number of points must be positive. " << VAROUT(numPoints);
const size_t num_x = size_t(numPoints.x());
const size_t num_y = size_t(numPoints.y());
@@ -51,49 +75,67 @@ namespace armarx::viz
{
gridVertices[i][j].x() = minimum.x() + i * step.x();
gridVertices[i][j].y() = minimum.y() + j * step.y();
- // gridVertices[i][j].z() == 0;
+ gridVertices[i][j].z() = height;
}
}
- // Create colors.
- std::vector<std::vector<viz::data::Color>> gridColors(
+ return gridVertices;
+ }
+
+
+ std::vector<std::vector<viz::data::Color> > grid::makeGrid2DColors(
+ const std::vector<std::vector<Eigen::Vector3f> >& vertices,
+ std::function<viz::Color(size_t x, size_t y, const Eigen::Vector3f& p)> colorFunc)
+ {
+ size_t num_x = vertices.size();
+ size_t num_y = vertices.front().size();
+
+ std::vector<std::vector<viz::data::Color>> colors(
num_x, std::vector<viz::data::Color>(num_y, viz::Color::black()));
+
for (size_t i = 0; i < num_x; i++)
{
for (size_t j = 0; j < num_y; j++)
{
- gridColors[i][j] = colorFunc(i, j, gridVertices[i][j]);
+ colors[i][j] = colorFunc(i, j, vertices[i][j]);
}
}
- // Create faces.
+ return colors;
+ }
+
+
+ std::vector<viz::data::Face> grid::makeGrid2DFaces(size_t num_x, size_t num_y)
+ {
std::vector<viz::data::Face> faces(2 * (num_x - 1) * (num_y - 1));
+
size_t index = 0;
for (size_t x = 0; x < num_x - 1; x++)
{
for (size_t y = 0; y < num_y - 1; y++)
{
- faces[index].v0 = int(x * num_y + y);
- faces[index].v1 = int((x + 1) * num_y + y);
- faces[index].v2 = int((x + 1) * num_y + (y + 1));
- faces[index].c0 = int(x * num_y + y);
- faces[index].c1 = int((x + 1) * num_y + y);
- faces[index].c2 = int((x + 1) * num_y + (y + 1));
+ /* In counter-clockwise order.
+ * (x) (x+1)
+ * (y) *----*
+ * | \f1|
+ * |f2\ |
+ * (y+1) *----*
+ */
+ faces[index].v0 = faces[index].c0 = int(x * num_y + y);
+ faces[index].v1 = faces[index].c1 = int((x + 1) * num_y + (y + 1));
+ faces[index].v2 = faces[index].c2 = int((x + 1) * num_y + y);
index++;
- faces[index].v0 = int(x * num_y + y);
- faces[index].v1 = int(x * num_y + (y + 1));
- faces[index].v2 = int((x + 1) * num_y + (y + 1));
- faces[index].c0 = int(x * num_y + y);
- faces[index].c1 = int(x * num_y + (y + 1));
- faces[index].c2 = int((x + 1) * num_y + (y + 1));
+ faces[index].v0 = faces[index].c0 = int(x * num_y + y);
+ faces[index].v1 = faces[index].c1 = int(x * num_y + (y + 1));
+ faces[index].v2 = faces[index].c2 = int((x + 1) * num_y + (y + 1));
index++;
}
}
-
- // Flatten
- return this->vertices(flatten(gridVertices)).colors(flatten(gridColors)).faces(faces);
+ return faces;
}
+
+
}
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Mesh.h b/source/RobotAPI/components/ArViz/Client/elements/Mesh.h
index 3e8315a175939d20122fee99da2e73f4b01cc188..31cca73a22fdc087a8acda05afdb0c13629f30f2 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Mesh.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/Mesh.h
@@ -1,6 +1,9 @@
#pragma once
#include <functional>
+#include <numeric> // for std::accumulate
+
+#include <SimoxUtility/EigenStdVector.h>
#include <RobotAPI/interface/ArViz/Elements.h>
@@ -69,12 +72,92 @@ namespace armarx::viz
/**
* @brief Builds a regular 2D grid in the xy-plane.
- * @param extents The extents in x and y direction.
+ * @param extents The full extents in x and y direction.
* @param numPoints The number of points in x and y direction.
* @param colorFunc A function determining the color of each vertex.
*/
Mesh& grid2D(Eigen::Vector2f extents, Eigen::Vector2i numPoints,
std::function<viz::Color(size_t i, size_t j, const Eigen::Vector3f& p)> colorFunc);
+
+ /**
+ * @brief Builds a regular 2D grid.
+ *
+ * The shape of `vertices` and `colors` must match, i.e.:
+ * - `vertices` and `colors` must have equal size.
+ * - Each element (nested vector) of `vertices` and `colors` must have equal size.
+ *
+ * @param vertices The vertices.
+ * @param colors The colors.
+ */
+ Mesh& grid2D(const std::vector<std::vector<Eigen::Vector3f>>& vertices,
+ const std::vector<std::vector<viz::data::Color>>& colors);
};
+
+
+ namespace grid
+ {
+ /**
+ * @brief Builds vertices of a regular 2D grid in the xy-plane.
+ *
+ * If the result is indexed as result[i][j], the i index represents the x-axis,
+ * the j index represents the y-axis.
+ *
+ * @param extents The full extents per axis.
+ * @param numPoints The number of points per axis.
+ * @param height The height (z-value).
+ * @return The vertices.
+ */
+ std::vector<std::vector<Eigen::Vector3f>> makeGrid2DVertices(
+ Eigen::Vector2f extents, Eigen::Vector2i numPoints, float height = 0);
+
+
+ /**
+ * @brief Build colors of a 2D grid.
+ * @param vertices The vertices.
+ * @param colorFunc A function determining the color of each vertex.
+ * @return The colors.
+ *
+ * @see `makeGrid2DVertices()`
+ */
+ std::vector<std::vector<viz::data::Color>> makeGrid2DColors(
+ const std::vector<std::vector<Eigen::Vector3f>>& vertices,
+ std::function<viz::Color(size_t x, size_t y, const Eigen::Vector3f& p)> colorFunc);
+
+
+ /**
+ * @brief Builds faces of a 2D grid.
+ *
+ * The built indexes refer to flattened arrays of vertices and colors,
+ * such as produced by `flatten()` applied to the result of `makeGrid2DVertices()`.
+ *
+ * @param num_x The number of vertices in x-axis.
+ * @param num_y The number of vertices in y-axis.
+ * @return The faces.
+ */
+ std::vector<viz::data::Face> makeGrid2DFaces(size_t num_x, size_t num_y);
+
+
+ template <class T>
+ /// @brief Flattens a 2D vector of nested vectors to a 1D vector.
+ std::vector<T> flatten(const std::vector<std::vector<T>>& vector)
+ {
+ size_t size = std::accumulate(vector.begin(), vector.end(), size_t(0), [](size_t s, const auto & v)
+ {
+ return s + v.size();
+ });
+
+ std::vector<T> flat;
+ flat.reserve(size);
+ for (const auto& v : vector)
+ {
+ for (const auto& val : v)
+ {
+ flat.push_back(val);
+ }
+ }
+ return flat;
+ }
+ }
+
}
diff --git a/source/RobotAPI/components/ArViz/Client/elements/PointCloud.h b/source/RobotAPI/components/ArViz/Client/elements/PointCloud.h
index b47681375895fb97462421df70b08d146259aacf..c0051c7f4c1824538eb531976e2125defef6c0c4 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/PointCloud.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/PointCloud.h
@@ -49,34 +49,33 @@ namespace armarx::viz
return *this;
}
- PointCloud& addPoint(float x, float y, float z)
+ PointCloud& addPoint(float x, float y, float z, const data::Color& color)
{
ColoredPoint p;
p.x = x;
p.y = y;
p.z = z;
- p.color = viz::Color::fromRGBA(0, 0, 0, 255);
+ p.color = color;
return addPoint(p);
}
-
- PointCloud& addPoint(float x, float y, float z, std::size_t id, int alpha = 255)
+ PointCloud& addPoint(float x, float y, float z, const simox::color::Color& color)
{
- ColoredPoint p;
- p.x = x;
- p.y = y;
- p.z = z;
- p.color = Color::fromRGBA(simox::color::GlasbeyLUT::at(id, alpha));
- return addPoint(p);
+ return addPoint(x, y, z, Color{color});
}
PointCloud& addPoint(float x, float y, float z, std::uint8_t r, std::uint8_t g, std::uint8_t b, std::uint8_t a = 255)
{
- ColoredPoint p;
- p.x = x;
- p.y = y;
- p.z = z;
- p.color = Color::fromRGBA(r, g, b, a);
- return addPoint(p);
+ return addPoint(x, y, z, Color{r, g, b, a});
+ }
+
+ PointCloud& addPoint(float x, float y, float z)
+ {
+ return addPoint(x, y, z, 0, 0, 0, 255);
+ }
+
+ PointCloud& addPoint(float x, float y, float z, std::size_t id, int alpha = 255)
+ {
+ return addPoint(x, y, z, simox::color::GlasbeyLUT::at(id, alpha));
}
// Templated setters for PCL point clouds.
@@ -85,35 +84,20 @@ namespace armarx::viz
std::enable_if_t < !detail::has_members_rgba<PointT>::value, int > = 0 >
PointCloud & addPoint(const PointT& p, Color color = Color::gray())
{
- ColoredPoint& pv = data_->points.emplace_back(ColoredPoint());
- pv.x = p.x;
- pv.y = p.y;
- pv.z = p.z;
- pv.color = color;
- return *this;
+ return addPoint(p.x, p.y, p.z, color);
}
template < class PointT,
std::enable_if_t < detail::has_members_rgba<PointT>::value, int> = 0>
PointCloud & addPoint(const PointT& p)
{
- ColoredPoint& pv = data_->points.emplace_back(ColoredPoint());
- pv.x = p.x;
- pv.y = p.y;
- pv.z = p.z;
- pv.color = Color::fromRGBA(p.r, p.g, p.b, p.a);
- return *this;
+ return addPoint(p.x, p.y, p.z, p.r, p.g, p.b, p.a);
}
template < class PointT >
PointCloud& addPoint(const PointT& p, Color color)
{
- ColoredPoint& pv = data_->points.emplace_back(ColoredPoint());
- pv.x = p.x;
- pv.y = p.y;
- pv.z = p.z;
- pv.color = color;
- return *this;
+ return addPoint(p.x, p.y, p.z, color);
}
template <class PointCloudT>
diff --git a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp
index 2103961b774d0ac85f304072d78ba398a302ac95..e2436920ed1c5fe526b7abb556d8a8e4e1605e1f 100644
--- a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp
@@ -1,3 +1,5 @@
+#include <SimoxUtility/EigenStdVector.h>
+
#include "ElementVisualizer.h"
#include <RobotAPI/components/ArViz/IceConversions.h>
diff --git a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
index 384403949af29ae5885c7b926b1f4006397179f8..4df1b1f39bc32b133a0273d5408eed51c7a99672 100644
--- a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
+++ b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
@@ -22,17 +22,21 @@
#include "ArVizExample.h"
+
#include <RobotAPI/components/ArViz/Client/Client.h>
-#include <Eigen/Eigen>
-#include <SimoxUtility/color/cmaps.h>
#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <SimoxUtility/color/cmaps.h>
+#include <Eigen/Eigen>
namespace armarx
{
+ ARMARX_DECOUPLED_REGISTER_COMPONENT(ArVizExample);
+
void ArVizExample::onInitComponent()
{
offeringTopicFromProperty("ArVizTopicName");
diff --git a/source/RobotAPI/components/ArViz/Example/CMakeLists.txt b/source/RobotAPI/components/ArViz/Example/CMakeLists.txt
index c72b86cc4e025e07737c2bc7fde2c65cda1982ea..2e21d21e1a0bc042bfb70e0dcd08a2891de3bd56 100644
--- a/source/RobotAPI/components/ArViz/Example/CMakeLists.txt
+++ b/source/RobotAPI/components/ArViz/Example/CMakeLists.txt
@@ -2,6 +2,7 @@ armarx_component_set_name("ArVizExample")
set(COMPONENT_LIBS
ArmarXCore
+ DecoupledSingleComponent
ArViz
RobotAPIComponentPlugins # For ArVizComponentPluginUser
)
diff --git a/source/RobotAPI/components/ArViz/Example/main.cpp b/source/RobotAPI/components/ArViz/Example/main.cpp
index 7fb1cbfed93189305721444106968613f4ec06ea..39a144f6892a546c18168eea8d5878d0c9a521c6 100644
--- a/source/RobotAPI/components/ArViz/Example/main.cpp
+++ b/source/RobotAPI/components/ArViz/Example/main.cpp
@@ -1,10 +1,6 @@
-#include <RobotAPI/components/ArViz/Example/ArVizExample.h>
-
-#include <ArmarXCore/core/application/Application.h>
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/libraries/DecoupledSingleComponent/DecoupledMain.h>
int main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp < armarx::ArVizExample > (argc, argv, "ArVizExample");
+ return armarx::DecoupledMain(argc, argv);
}
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index bce9fcf04fd88c2dbbac0321f2346a520ff001a3..34bc50559c07e57e39290ce44ad23d668d6f8ff4 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -1,22 +1,20 @@
add_subdirectory(units)
+
+add_subdirectory(ArViz)
+add_subdirectory(CyberGloveObserver)
add_subdirectory(DebugDrawer)
-add_subdirectory(RobotState)
-add_subdirectory(EarlyVisionGraph)
-add_subdirectory(ViewSelection)
-add_subdirectory(GamepadControlUnit)
-add_subdirectory(RobotNameService)
add_subdirectory(DummyTextToSpeech)
-add_subdirectory(KITProstheticHandUnit)
-add_subdirectory(CyberGloveObserver)
-add_subdirectory(RobotHealth)
+add_subdirectory(EarlyVisionGraph)
add_subdirectory(FrameTracking)
-
+add_subdirectory(GamepadControlUnit)
add_subdirectory(KITHandUnit)
+add_subdirectory(KITProstheticHandUnit)
add_subdirectory(MultiHandUnit)
-
-add_subdirectory(TopicTimingTest)
-
-add_subdirectory(ArViz)
+add_subdirectory(NaturalIKTest)
+add_subdirectory(RobotHealth)
+add_subdirectory(RobotNameService)
+add_subdirectory(RobotState)
+add_subdirectory(RobotToArViz)
add_subdirectory(StatechartExecutorExample)
-
-add_subdirectory(NaturalIKTest)
\ No newline at end of file
+add_subdirectory(TopicTimingTest)
+add_subdirectory(ViewSelection)
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
index 2e27aed9ea0292e0161d8429301ccebb81eb6908..c91d4aca1747454bd6533595ef0bda8359bae6cc 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -37,6 +37,7 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/core/system/Synchronization.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
diff --git a/source/RobotAPI/components/FrameTracking/FrameTracking.cpp b/source/RobotAPI/components/FrameTracking/FrameTracking.cpp
index 0653de63c14522702e32af57b8bc851547bc2adb..89f5f9341d30e703cfd6d456c073a3ea40b45603 100644
--- a/source/RobotAPI/components/FrameTracking/FrameTracking.cpp
+++ b/source/RobotAPI/components/FrameTracking/FrameTracking.cpp
@@ -22,6 +22,7 @@
#include "FrameTracking.h"
+#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/interface/core/BasicVectorTypesHelpers.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
diff --git a/source/RobotAPI/components/NaturalIKTest/CMakeLists.txt b/source/RobotAPI/components/NaturalIKTest/CMakeLists.txt
index 6763470e25059e03126992507025854fcf50c0b0..8f4db4a898236c85436645ae747cc281d701011f 100644
--- a/source/RobotAPI/components/NaturalIKTest/CMakeLists.txt
+++ b/source/RobotAPI/components/NaturalIKTest/CMakeLists.txt
@@ -8,6 +8,7 @@ set(COMPONENT_LIBS
RobotAPIInterfaces
ArmarXGuiComponentPlugins
natik
+ diffik
)
set(SOURCES
diff --git a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
index 7044defce2986a619dacb3bf0ec084797989cd65..2259c55abcd28e037715d2cc01c68a8a21ca235b 100644
--- a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
+++ b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
@@ -26,6 +26,7 @@
#include <RobotAPI/libraries/natik/NaturalIK.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/math/Helpers.h>
@@ -161,6 +162,7 @@ namespace armarx
p.ikparams.diffIKparams.elbowKp = prx.getValue<float>("elbowKp").get();
p.ikparams.diffIKparams.jointLimitAvoidanceKp = prx.getValue<float>("jlaKp").get();
p.ikparams.diffIKparams.jointLimitAvoidanceMargins = prx.getValue<float>("jlaMargin").get();
+ p.ikparams.diffIKparams.returnIKSteps = true;
p.setOri = prx.getValue<bool>("setOri").get();
p.targetValid = true;
@@ -181,17 +183,25 @@ namespace armarx
void NaturalIKTest::vizTaskRun()
{
+ //ARMARX_IMPORTANT << "vizTask starts";
viz::Client arviz(*this);
+ ARMARX_IMPORTANT << 1;
+ viz::Layer layer_iksteps = arviz.layer("ikSteps");
viz::Layer layer_robot = arviz.layer("Robot");
+ ARMARX_IMPORTANT << 2;
viz::Robot vizrobot = viz::Robot("robot")
.position(Eigen::Vector3f::Zero())
.file("Armar6RT", "Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml");
+ ARMARX_IMPORTANT << 3;
vizrobot.useFullModel();
+ ARMARX_IMPORTANT << 4;
layer_robot.add(vizrobot);
+ ARMARX_IMPORTANT << 5;
CMakePackageFinder finder("Armar6RT");
+ ARMARX_IMPORTANT << 6;
std::string robotFile = finder.getDataDir() + "/Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml";
ARMARX_IMPORTANT << "loading robot from " << robotFile;
@@ -333,19 +343,32 @@ namespace armarx
//ARMARX_IMPORTANT << arm_R.rns->getJointValueMap();
+ NaturalDiffIK::Result ikResult;
if (p.setOri)
{
- ik_R.calculateIK(target_R, arm_R, p.ikparams);
+ ikResult = ik_R.calculateIK(target_R, arm_R, p.ikparams);
}
else
{
- ik_R.calculateIKpos(math::Helpers::Position(target_R), arm_R, p.ikparams);
+ ikResult = ik_R.calculateIKpos(math::Helpers::Position(target_R), arm_R, p.ikparams);
}
+ layer_iksteps.clear();
+ std::stringstream ss;
+ int i = 0;
+ for (const NaturalDiffIK::IKStep& s : ikResult.ikSteps)
+ {
+ ss << s.pdTcp.norm() << "; " << s.odTcp.norm() << "; " << s.pdElb.norm() << "\n";
+ layer_iksteps.add(viz::Box("tcp_" + std::to_string(i)).size(20).pose(s.tcpPose).color(viz::Color::blue()));
+ layer_iksteps.add(viz::Box("elb_" + std::to_string(i)).size(20).pose(s.elbPose).color(viz::Color::blue()));
+ i++;
+ }
+ //ARMARX_IMPORTANT << ss.str();
+
vizrobot.joints(arm_R.rns->getJointValueMap());
- arviz.commit({layer_R, layer_L, layer_robot});
+ arviz.commit({layer_R, layer_L, layer_robot, layer_iksteps});
//ARMARX_IMPORTANT << "arviz.commit";
/*
diff --git a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.h b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.h
index 596ba14062b365867469bfdc32cffe5ea0fa10bc..e7c918f1151ff0e879ad6d578629492d6fdc646b 100644
--- a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.h
+++ b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.h
@@ -31,7 +31,7 @@
//#include <RobotAPI/libraries/core/visualization/DebugDrawerTopic.h>
#include <Eigen/Dense>
-#include <RobotAPI/libraries/core/diffik/NaturalDiffIK.h>
+#include <RobotAPI/libraries/diffik/NaturalDiffIK.h>
#include <RobotAPI/libraries/natik/NaturalIK.h>
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
index e6c314d471c3c20e1081323fcfaf3e6db417ab31..1365ca76a761f5956aaebc5d96ef1adcca8034f5 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
@@ -24,7 +24,7 @@
#include "RobotHealth.h"
-#include <boost/regex.h>
+#include <boost/regex.hpp>
namespace armarx
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.h b/source/RobotAPI/components/RobotHealth/RobotHealth.h
index 616053dbc94121c0654e8190ee624bdb0c9cb755..97852dab631cc1e6e1df0c4f738585f43395b0f4 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.h
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.h
@@ -22,15 +22,19 @@
#pragma once
+#include <RobotAPI/interface/components/RobotHealthInterface.h>
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include <RobotAPI/interface/speech/SpeechInterface.h>
+
+#include <ArmarXGui/interface/RemoteGuiInterface.h>
+
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-#include <RobotAPI/interface/components/RobotHealthInterface.h>
+#include <ArmarXCore/core/system/Synchronization.h>
#include <ArmarXCore/interface/components/EmergencyStopInterface.h>
-#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
#include <atomic>
-#include <ArmarXGui/interface/RemoteGuiInterface.h>
-#include <RobotAPI/interface/speech/SpeechInterface.h>
namespace armarx
{
diff --git a/source/RobotAPI/components/RobotToArViz/CMakeLists.txt b/source/RobotAPI/components/RobotToArViz/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1f07bddf7f12741de4f2a0486fdecef5c6247d57
--- /dev/null
+++ b/source/RobotAPI/components/RobotToArViz/CMakeLists.txt
@@ -0,0 +1,35 @@
+armarx_component_set_name("RobotToArViz")
+
+
+set(COMPONENT_LIBS
+ ArmarXCore
+ RobotAPIComponentPlugins
+)
+
+set(SOURCES
+ RobotToArViz.cpp
+)
+set(HEADERS
+ RobotToArViz.h
+)
+
+
+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(RobotToArViz PUBLIC ${MyLib_INCLUDE_DIRS})
+#endif()
+
+# add unit tests
+add_subdirectory(test)
+
+
+armarx_component_set_name("RobotToArVizApp")
+set(COMPONENT_LIBS RobotToArViz)
+armarx_add_component_executable(main.cpp)
+
+
diff --git a/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp b/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f0e7ae58e5a83e9e2ea939b867f7cd0626c809b7
--- /dev/null
+++ b/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp
@@ -0,0 +1,93 @@
+/*
+ * 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::RobotToArViz
+ * @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 "RobotToArViz.h"
+
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+
+namespace armarx
+{
+ RobotToArVizPropertyDefinitions::RobotToArVizPropertyDefinitions(std::string prefix) :
+ armarx::ComponentPropertyDefinitions(prefix)
+ {
+ }
+
+ armarx::PropertyDefinitionsPtr RobotToArViz::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs(new RobotToArVizPropertyDefinitions(getConfigIdentifier()));
+
+ defs->optional(updateFrequency, "updateFrequency", "Target number of updates per second.");
+
+ return defs;
+ }
+
+
+ std::string RobotToArViz::getDefaultName() const
+ {
+ return "RobotToArViz";
+ }
+
+
+ void RobotToArViz::onInitComponent()
+ {
+ }
+
+
+ void RobotToArViz::onConnectComponent()
+ {
+ // Load robot.
+ this->robot = RobotState::addRobot(robotName, VirtualRobot::RobotIO::RobotDescription::eStructure);
+
+ // Initialize robot visu element.
+ this->robotViz = viz::Robot(robot->getName()).file("", robot->getFilename());
+
+ task = new SimplePeriodicTask<>([this]()
+ {
+ this->updateRobot();
+ }, int(1000 / updateFrequency));
+ task->start();
+ }
+
+
+ void RobotToArViz::onDisconnectComponent()
+ {
+ task->stop();
+ task = nullptr;
+ }
+
+
+ void RobotToArViz::onExitComponent()
+ {
+ }
+
+
+ void RobotToArViz::updateRobot()
+ {
+ RobotState::synchronizeLocalClone(robotName);
+
+ robotViz.joints(robot->getConfig()->getRobotNodeJointValueMap())
+ .pose(robot->getGlobalPose());
+
+ arviz.commitLayerContaining(robot->getName(), robotViz);
+ }
+}
diff --git a/source/RobotAPI/components/RobotToArViz/RobotToArViz.h b/source/RobotAPI/components/RobotToArViz/RobotToArViz.h
new file mode 100644
index 0000000000000000000000000000000000000000..4e6a64ca03a9daea07df8f6e3cc8deb91ad67374
--- /dev/null
+++ b/source/RobotAPI/components/RobotToArViz/RobotToArViz.h
@@ -0,0 +1,108 @@
+/*
+ * 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::RobotToArViz
+ * @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 <ArmarXCore/core/services/tasks/TaskUtil.h>
+
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
+
+
+namespace armarx
+{
+ /**
+ * @class RobotToArVizPropertyDefinitions
+ * @brief Property definitions of `RobotToArViz`.
+ */
+ class RobotToArVizPropertyDefinitions :
+ public armarx::ComponentPropertyDefinitions
+ {
+ public:
+ RobotToArVizPropertyDefinitions(std::string prefix);
+ };
+
+
+
+ /**
+ * @defgroup Component-RobotToArViz RobotToArViz
+ * @ingroup RobotAPI-Components
+ *
+ * Visualizes a robot via ArViz.
+ *
+ * @class RobotToArViz
+ * @ingroup Component-RobotToArViz
+ * @brief Brief description of class RobotToArViz.
+ *
+ * Detailed description of class RobotToArViz.
+ */
+ class RobotToArViz :
+ virtual public armarx::Component,
+ virtual public armarx::ArVizComponentPluginUser,
+ virtual public armarx::RobotStateComponentPluginUser
+ {
+ using RobotState = RobotStateComponentPluginUser;
+
+ 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;
+
+
+ private:
+
+ void updateRobot();
+
+
+ private:
+
+ float updateFrequency = 100;
+ SimplePeriodicTask<>::pointer_type task;
+
+ std::string robotName = "robot";
+
+ VirtualRobot::RobotPtr robot;
+
+ viz::Robot robotViz { "" };
+
+ };
+}
diff --git a/source/RobotAPI/components/RobotToArViz/main.cpp b/source/RobotAPI/components/RobotToArViz/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f0d3c514ddc51065741b36cd42347b8ee3db848e
--- /dev/null
+++ b/source/RobotAPI/components/RobotToArViz/main.cpp
@@ -0,0 +1,33 @@
+/*
+ * 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 OML::application::SceneVisualizer
+ * @author Fabian Paus ( fabian dot paus at kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <RobotAPI/components/RobotToArViz/RobotToArViz.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::RobotToArViz > (argc, argv, "RobotToArViz");
+}
diff --git a/source/RobotAPI/components/RobotToArViz/test/CMakeLists.txt b/source/RobotAPI/components/RobotToArViz/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6ddf1aed81a6864438a0e1250ddc7b258cbc6942
--- /dev/null
+++ b/source/RobotAPI/components/RobotToArViz/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore RobotToArViz)
+
+armarx_add_test(RobotToArVizTest RobotToArVizTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/RobotToArViz/test/RobotToArVizTest.cpp b/source/RobotAPI/components/RobotToArViz/test/RobotToArVizTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8a7082d680abfa3573e6d5f60417161c36ca7ea8
--- /dev/null
+++ b/source/RobotAPI/components/RobotToArViz/test/RobotToArVizTest.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::RobotToArViz
+ * @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::RobotToArViz
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include <RobotAPI/components/RobotToArViz/RobotToArViz.h>
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+ armarx::RobotToArViz instance;
+
+ BOOST_CHECK_EQUAL(true, true);
+}
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.cpp b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
index 31b2119d689804ba960fda6054587556936ffa45..a365530d8022e8ac1f7e51f7246c44f28903c5ef 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
@@ -28,6 +28,7 @@
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
#include <ArmarXCore/observers/checks/ConditionCheckEqualsWithTolerance.h>
+#include <ArmarXCore/core/util/StringHelpers.h>
#include <RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h>
#include <RobotAPI/libraries/core/observerfilters/OffsetFilter.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
diff --git a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
index f7784390bfd569406cb5f2ffb0e60c9693531881..6cfb8340d95092343a8fa14eb1ba0818ad67ae29 100644
--- a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
@@ -25,6 +25,8 @@
#include "ForceTorqueUnitSimulation.h"
+#include <ArmarXCore/core/util/StringHelpers.h>
+
#include <boost/algorithm/string.hpp>
namespace armarx
diff --git a/source/RobotAPI/components/units/HeadIKUnit.cpp b/source/RobotAPI/components/units/HeadIKUnit.cpp
index 9de7dca530084c57728842fa1f213afc5a710ef1..d240a3c60d2fa0a382ae4fe69cb33df69a643c9c 100644
--- a/source/RobotAPI/components/units/HeadIKUnit.cpp
+++ b/source/RobotAPI/components/units/HeadIKUnit.cpp
@@ -25,6 +25,7 @@
#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/util/StringHelpers.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/IK/GazeIK.h>
diff --git a/source/RobotAPI/components/units/HeadIKUnit.h b/source/RobotAPI/components/units/HeadIKUnit.h
index 6783a2b2e1a7609b19bfae27a898e893879bc8f9..251c88d4d5a1cfd0822f79a744b3757a23cd451d 100644
--- a/source/RobotAPI/components/units/HeadIKUnit.h
+++ b/source/RobotAPI/components/units/HeadIKUnit.h
@@ -25,6 +25,7 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/core/system/Synchronization.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
diff --git a/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp b/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
index 0cb76bb4defe8da5dffdee5255a7cd9c3b2a9d1a..0944336bfaf52400a6685ce58d4b1c409a3e86c3 100644
--- a/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
+++ b/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
@@ -27,12 +27,11 @@
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
-
-#include <RobotAPI/libraries/core/Pose.h>
-
#include <ArmarXCore/observers/variant/TimestampVariant.h>
+#include <RobotAPI/libraries/core/Pose.h>
+#include <cfloat>
namespace armarx
{
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
index 7bb3d462007d4b2af909fad70df17139466b8386..1f6e157696ee50f33a216cf6665d271b11be7939 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
@@ -35,6 +35,7 @@
#include <ArmarXCore/core/util/TripleBuffer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/services/tasks/ThreadPool.h>
+#include <ArmarXCore/core/system/Synchronization.h>
#include <ArmarXGui/interface/WidgetDescription.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
index ac9a8cc669471ed043e0fb253ae6c97c88336881..c29199ada8042ed04c30a83e48fe9da77444010e 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
@@ -22,11 +22,15 @@
#pragma once
#include "../EigenForwardDeclarations.h"
-#include <string>
-#include <chrono>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/observers/variant/TimedVariant.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
+
+#include <string>
+#include <chrono>
+
namespace armarx::introspection
{
template<class T, class = void> struct DataFieldsInfo;
diff --git a/source/RobotAPI/components/units/SensorActorUnit.h b/source/RobotAPI/components/units/SensorActorUnit.h
index bd6485fc64dc62047bbb84d6f4ac2a776b8bc70d..bef26a5e7c92e97d3ab2889e4081b59caee0daec 100644
--- a/source/RobotAPI/components/units/SensorActorUnit.h
+++ b/source/RobotAPI/components/units/SensorActorUnit.h
@@ -27,6 +27,7 @@
#include <ArmarXCore/core/exceptions/Exception.h>
#include <RobotAPI/interface/units/UnitInterface.h>
+#include <boost/thread/mutex.hpp>
namespace armarx
{
diff --git a/source/RobotAPI/components/units/TCPControlUnit.h b/source/RobotAPI/components/units/TCPControlUnit.h
index 6d254bba5031b1d1b3ddc3dfe2b0316a409f42e8..cc984d4358f9e0b1dbec9455f6cb26283aa876bf 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.h
+++ b/source/RobotAPI/components/units/TCPControlUnit.h
@@ -25,6 +25,7 @@
#include <RobotAPI/interface/units/TCPControlUnit.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/core/system/Synchronization.h>
#include <ArmarXCore/core/Component.h>
#include <VirtualRobot/IK/DifferentialIK.h>
diff --git a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
index ee0e66a2e7e10ed62e1a634cc4787f48239db3de..85ac99dc262ec38a2b1e4e3b1464e97e4866f22e 100644
--- a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
+++ b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
@@ -31,6 +31,7 @@
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/core/system/Synchronization.h>
#include <RobotAPI/interface/units/GamepadUnit.h>
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
index 0216b7b017e230414f452f1a812e32022795a053..9f1508cce0928aa91169c61cd36f5389ef6d75aa 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
@@ -23,9 +23,11 @@
#include "HokuyoLaserUnit.h"
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-#include <sys/resource.h>
#include <boost/algorithm/string/split.hpp>
+#include <boost/algorithm/string/replace.hpp>
+#include <boost/algorithm/string/classification.hpp>
+
#include <HokuyoLaserScannerDriver/urg_utils.h>
using namespace armarx;
@@ -161,12 +163,12 @@ armarx::PropertyDefinitionsPtr HokuyoLaserUnit::createPropertyDefinitions()
getConfigIdentifier()));
}
-std::string HokuyoLaserUnit::getReportTopicName(const Ice::Current& c) const
+std::string HokuyoLaserUnit::getReportTopicName(const Ice::Current&) const
{
return topicName;
}
-LaserScannerInfoSeq HokuyoLaserUnit::getConnectedDevices(const Ice::Current& c) const
+LaserScannerInfoSeq HokuyoLaserUnit::getConnectedDevices(const Ice::Current&) const
{
return connectedDevices;
}
diff --git a/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp
index 4faa7fa0135d28010b0ce8a788ff865ec485e1c3..dc56112a064925baa1c0892697883cb1fa4f94fa 100644
--- a/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp
+++ b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp
@@ -23,7 +23,7 @@
#include "OptoForceUnit.h"
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-
+#include <ArmarXCore/core/util/StringHelpers.h>
using namespace armarx;
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp
index 6ba5e9afab4c7a82670a2429897b5ac0b2892799..5f391cb974af80cf9f724f7ecf530e2603c2ba51 100644
--- a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp
@@ -91,7 +91,7 @@ void LaserScannerPluginWidgetController::configured()
void LaserScannerPluginWidgetController::reportSensorValues(const std::string& device, const std::string& name, const LaserScan& newScan, const TimestampBasePtr& timestamp, const Ice::Current& c)
{
{
- boost::mutex::scoped_lock lock(scanMutex);
+ std::unique_lock lock(scanMutex);
LaserScan& scan = scans[device];
// TODO: Do some filtering? aggregation?
@@ -105,7 +105,7 @@ void LaserScannerPluginWidgetController::onNewSensorValuesReported()
{
QComboBox* deviceBox = widget.deviceComboBox;
- boost::mutex::scoped_lock lock(scanMutex);
+ std::unique_lock lock(scanMutex);
for (auto& pair : scans)
{
QString deviceName(QString::fromStdString(pair.first.c_str()));
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.h b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.h
index 194e9903c3742828147d465198436f684781d90a..9aca4525b7c91bb801e13f6ec7bca97b8e233561 100644
--- a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.h
+++ b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.h
@@ -23,12 +23,15 @@
#pragma once
#include <RobotAPI/gui-plugins/LaserScannerPlugin/ui_LaserScannerPluginWidget.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+#include <mutex>
namespace armarx
{
@@ -119,7 +122,7 @@ namespace armarx
LaserScannerUnitInterfacePrx laserScannerUnit;
LaserScannerInfoSeq laserScanners;
- Mutex scanMutex;
+ std::mutex scanMutex;
std::unordered_map<std::string, LaserScan> scans;
std::unordered_map<std::string, std::deque<int>> numberOfRingsHistory;
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
index 7effcca3153c7baa310c43b195e7441ec62694e2..1c1c83237601ca2dda19101551d3c93295336c10 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
@@ -24,6 +24,7 @@
#include "NJointControllerClassesWidget.h"
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/util/StringHelpers.h>
#include <QGridLayout>
#include <QDir>
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h
index b08368250a6e58bdeeb58eb809f50aa123f81941..e9bc549d71890e6345cd2ef8706b0d338baafcc6 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h
@@ -1,6 +1,12 @@
#pragma once
#include "ArmarXEtherCATFwd.h"
+#include "AbstractFunctionalDevice.h"
+
+#include <ArmarXCore/core/system/Synchronization.h>
+
+#include <IceUtil/Time.h>
+
#include <iostream>
#include <fstream>
#include <vector>
@@ -9,10 +15,6 @@
#include <atomic>
-#include "AbstractFunctionalDevice.h"
-//#include "Armar6Unit.h"
-#include <IceUtil/Time.h>
-
/**
* The actual size of the mapped prozess image will be smaller but with 4096 we are quite
* confident that we will have enough space
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index b2585cd5f44a8d0f5715218975cb7300b251fa35..ba76ea6b41f3cb5a0f09815680360970884cb6a2 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -15,3 +15,5 @@ add_subdirectory(SimpleTrajectory)
add_subdirectory(widgets)
add_subdirectory(natik)
add_subdirectory(aron)
+
+add_subdirectory(diffik)
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp
index a8fcab6cf91276feb3b6437485854b0c25bb2ee8..5e80be3b747c17305416f842b2984ac370ca4d96 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp
@@ -5,7 +5,7 @@ namespace armarx::plugins
void ArVizComponentPlugin::preOnInitComponent()
{
- parent<Component>().offeringTopicFromProperty(PROPERTY_NAME);
+ parent<Component>().offeringTopicFromProperty(makePropertyName(PROPERTY_NAME));
}
void ArVizComponentPlugin::preOnConnectComponent()
@@ -15,10 +15,10 @@ namespace armarx::plugins
void ArVizComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
{
- if (!properties->hasDefinition(PROPERTY_NAME))
+ if (!properties->hasDefinition(makePropertyName(PROPERTY_NAME)))
{
properties->defineOptionalProperty<std::string>(
- PROPERTY_NAME,
+ makePropertyName(PROPERTY_NAME),
"ArVizTopic",
"Name of the ArViz topic");
}
@@ -26,7 +26,7 @@ namespace armarx::plugins
viz::Client ArVizComponentPlugin::createClient()
{
- viz::Client client(parent<Component>(), PROPERTY_NAME);
+ viz::Client client(parent<Component>(), makePropertyName(PROPERTY_NAME));
return client;
}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
index 223bbe561f31fe016d8721708149fd64f1c964c9..1292e78fa38178ff7f555aae561465bbb4e27ac6 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
@@ -6,6 +6,7 @@ armarx_set_target("Library: ${LIB_NAME}")
set(LIBS
RobotAPICore
DebugDrawer
+ diffik
)
set(LIB_FILES
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
index 500ac2a183f124dd6f89dd7dde6e09f88d8881b2..a8506de8061c42eec724280fb61144fd4cb2ce81 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
@@ -196,7 +196,7 @@ namespace armarx::plugins
{
if (!_robotStateComponent)
{
- parent<Component>().usingProxyFromProperty(_propertyName);
+ parent<Component>().usingProxyFromProperty(makePropertyName(_propertyName));
}
}
@@ -204,7 +204,7 @@ namespace armarx::plugins
{
if (!_robotStateComponent)
{
- parent<Component>().getProxyFromProperty(_robotStateComponent, _propertyName);
+ parent<Component>().getProxyFromProperty(_robotStateComponent, makePropertyName(_propertyName));
}
}
@@ -215,10 +215,10 @@ namespace armarx::plugins
void RobotStateComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
{
- if (!properties->hasDefinition(_propertyName))
+ if (!properties->hasDefinition(makePropertyName(_propertyName)))
{
properties->defineOptionalProperty<std::string>(
- _propertyName,
+ makePropertyName(_propertyName),
"RobotStateComponent",
"Name of the robot state component");
}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
index ca6b3390edfce1b6ed70aac2efa53e55a8ca0ff9..846de6628fd2b1015f23b017a13e6fbcf9616aae 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
@@ -29,7 +29,7 @@
#include <RobotAPI/interface/core/RobotState.h>
-#include <RobotAPI/libraries/core/diffik/SimpleDiffIK.h>
+#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
namespace armarx::plugins
{
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.cpp
index f16885b6faf9fc3bcea14ba91c4bbc5404e6a4a1..cfd77287685f0719fe865819dc5a94eadcb760df 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.cpp
@@ -1,6 +1,7 @@
#include "NJointBimanualCCDMPVelocityController.h"
#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
namespace armarx
{
diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index b010fec83eed8cbd1e80c7ce92050071fd3bc0b2..7a13be5ae14d0338abde28ab8f2dd5ab65b2e808 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/CMakeLists.txt
@@ -54,8 +54,8 @@ set(LIB_FILES
visualization/DebugDrawerTopic.cpp
visualization/GlasbeyLUT.cpp
- diffik/NaturalDiffIK.cpp
- diffik/SimpleDiffIK.cpp
+ #diffik/NaturalDiffIK.cpp
+ #diffik/SimpleDiffIK.cpp
)
set(LIB_HEADERS
@@ -107,8 +107,8 @@ set(LIB_HEADERS
visualization/DebugDrawerTopic.h
visualization/GlasbeyLUT.h
- diffik/NaturalDiffIK.h
- diffik/SimpleDiffIK.h
+ #diffik/NaturalDiffIK.h
+ #diffik/SimpleDiffIK.h
)
diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h
index 0880da76b522cd03f87422c3c5e6acce52ac7de4..e7a796a6eb65e02cb59ac06751278d73912ca8cc 100644
--- a/source/RobotAPI/libraries/core/FramedPose.h
+++ b/source/RobotAPI/libraries/core/FramedPose.h
@@ -36,6 +36,8 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
+#include <boost/shared_ptr.hpp>
+
#include <sstream>
namespace VirtualRobot
diff --git a/source/RobotAPI/libraries/core/MultiDimPIDController.h b/source/RobotAPI/libraries/core/MultiDimPIDController.h
index 0edd6a7cf25dea0f550821116ae2c16839760ef8..56ff6d28cd17522453014f9b880d8e88294ea3dc 100644
--- a/source/RobotAPI/libraries/core/MultiDimPIDController.h
+++ b/source/RobotAPI/libraries/core/MultiDimPIDController.h
@@ -23,11 +23,14 @@
*/
#pragma once
+#include <RobotAPI/libraries/core/math/MathUtils.h>
+
#include <ArmarXCore/core/logging/Logging.h>
-#include <Eigen/Core>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
+#include <ArmarXCore/core/system/Synchronization.h>
+
+#include <Eigen/Core>
namespace armarx
{
diff --git a/source/RobotAPI/libraries/core/PIDController.h b/source/RobotAPI/libraries/core/PIDController.h
index a1c6b006029187f32a71b56d7a2fb41cd58edbb1..b8631b35b9154e28962149e3c1ca5688447d59ac 100644
--- a/source/RobotAPI/libraries/core/PIDController.h
+++ b/source/RobotAPI/libraries/core/PIDController.h
@@ -24,10 +24,13 @@
#pragma once
+#include "MultiDimPIDController.h"
+
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/observers/filters/rtfilters/RTFilterBase.h>
+#include <ArmarXCore/core/system/Synchronization.h>
+
#include <Eigen/Core>
-#include "MultiDimPIDController.h"
namespace armarx
{
diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h
index 318c66bef78228a14e7b77c4defe2e768728890d..2feaea611590e10f71b227bcb845c46dd37f59ce 100644
--- a/source/RobotAPI/libraries/core/Trajectory.h
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -23,6 +23,12 @@
*/
#pragma once
+#include <RobotAPI/interface/core/Trajectory.h>
+
+#include <ArmarXCore/core/exceptions/Exception.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/util/StringHelpers.h>
+#include <ArmarXCore/observers/variant/Variant.h>
#include <boost/tokenizer.hpp>
#include <boost/unordered_map.hpp>
@@ -32,16 +38,10 @@
#include <boost/multi_index/ordered_index.hpp>
#include <boost/multi_index/member.hpp>
#include <boost/typeof/typeof.hpp>
-#include <ArmarXCore/core/exceptions/Exception.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/observers/variant/Variant.h>
#include <boost/type_traits/is_arithmetic.hpp>
-
#include <Eigen/Core>
-
-#include <RobotAPI/interface/core/Trajectory.h>
namespace armarx::VariantType
{
// Variant types
diff --git a/source/RobotAPI/libraries/core/math/LinearizeAngularTrajectory.cpp b/source/RobotAPI/libraries/core/math/LinearizeAngularTrajectory.cpp
index 672c8731c20ca9dd0fdd389977287a540bed9392..eecf1a0b4c6a8f7d607a02c1e932f70d7223953e 100644
--- a/source/RobotAPI/libraries/core/math/LinearizeAngularTrajectory.cpp
+++ b/source/RobotAPI/libraries/core/math/LinearizeAngularTrajectory.cpp
@@ -24,70 +24,70 @@
#include "LinearizeAngularTrajectory.h"
#include "MathUtils.h"
-using namespace armarx;
-using namespace math;
-
-LinearizeAngularTrajectory::LinearizeAngularTrajectory(float initialLinearValue)
- : linearValue(initialLinearValue)
-{
-}
-
-float LinearizeAngularTrajectory::update(float angle)
-{
- linearValue = linearValue + MathUtils::angleModPI(angle - linearValue);
- return linearValue;
-}
-
-float LinearizeAngularTrajectory::getLinearValue()
-{
- return linearValue;
-}
-
-std::vector<float> LinearizeAngularTrajectory::Linearize(const std::vector<float>& data)
+namespace armarx::math
{
- std::vector<float> result;
- result.reserve(data.size());
- if (data.size() == 0)
+ LinearizeAngularTrajectory::LinearizeAngularTrajectory(float initialLinearValue)
+ : linearValue(initialLinearValue)
{
- return result;
}
- LinearizeAngularTrajectory lat(data.at(0));
- for (float v : data)
+
+ float LinearizeAngularTrajectory::update(float angle)
{
- result.push_back(lat.update(v));
+ linearValue = linearValue + MathUtils::angleModPI(angle - linearValue);
+ return linearValue;
}
- return result;
-}
-void LinearizeAngularTrajectory::LinearizeRef(std::vector<float>& data)
-{
- if (data.size() == 0)
+ float LinearizeAngularTrajectory::getLinearValue()
{
- return;
+ return linearValue;
}
- LinearizeAngularTrajectory lat(data.at(0));
- for (size_t i = 0; i < data.size(); i++)
+
+ std::vector<float> LinearizeAngularTrajectory::Linearize(const std::vector<float>& data)
{
- data.at(i) = lat.update(data.at(i));
+ std::vector<float> result;
+ result.reserve(data.size());
+ if (data.size() == 0)
+ {
+ return result;
+ }
+ LinearizeAngularTrajectory lat(data.at(0));
+ for (float v : data)
+ {
+ result.push_back(lat.update(v));
+ }
+ return result;
}
-}
-std::vector<float> LinearizeAngularTrajectory::Angularize(const std::vector<float>& data, float center)
-{
- std::vector<float> result;
- result.reserve(data.size());
- for (float v : data)
+ void LinearizeAngularTrajectory::LinearizeRef(std::vector<float>& data)
{
- result.push_back(MathUtils::angleModX(v, center));
+ if (data.size() == 0)
+ {
+ return;
+ }
+ LinearizeAngularTrajectory lat(data.at(0));
+ for (size_t i = 0; i < data.size(); i++)
+ {
+ data.at(i) = lat.update(data.at(i));
+ }
}
- return result;
-}
-void LinearizeAngularTrajectory::AngularizeRef(std::vector<float>& data, float center)
-{
- for (size_t i = 0; i < data.size(); i++)
+ std::vector<float> LinearizeAngularTrajectory::Angularize(const std::vector<float>& data, float center)
{
- data.at(i) = MathUtils::angleModX(data.at(i), center);
+ std::vector<float> result;
+ result.reserve(data.size());
+ for (float v : data)
+ {
+ result.push_back(MathUtils::angleModX(v, center));
+ }
+ return result;
}
+ void LinearizeAngularTrajectory::AngularizeRef(std::vector<float>& data, float center)
+ {
+ for (size_t i = 0; i < data.size(); i++)
+ {
+ data.at(i) = MathUtils::angleModX(data.at(i), center);
+ }
+
+ }
}
diff --git a/source/RobotAPI/libraries/core/math/TimeSeriesUtils.cpp b/source/RobotAPI/libraries/core/math/TimeSeriesUtils.cpp
index 4045ee1d8545f44bcd408f2a6d6ad346c7930d82..6b1607730ab21a9ab904df146742609ae50ede76 100644
--- a/source/RobotAPI/libraries/core/math/TimeSeriesUtils.cpp
+++ b/source/RobotAPI/libraries/core/math/TimeSeriesUtils.cpp
@@ -25,99 +25,99 @@
#include "TimeSeriesUtils.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-using namespace armarx;
-using namespace math;
-
-TimeSeriesUtils::TimeSeriesUtils()
+namespace armarx::math
{
-}
-
-std::vector<float> TimeSeriesUtils::Resample(const std::vector<float>& timestamps, const std::vector<float>& data, const std::vector<float>& newTimestamps)
-{
- ARMARX_CHECK_EQUAL(data.size(), timestamps.size());
- ARMARX_CHECK_EQUAL(data.size(), newTimestamps.size());
- std::vector<float> result;
- result.reserve(data.size());
-
- if (data.size() == 0)
+ TimeSeriesUtils::TimeSeriesUtils()
{
- return result;
}
- if (data.size() == 1)
- {
- result.push_back(data.at(0));
- return result;
- }
-
- size_t i = 0;
- size_t j = 0;
- while (j < data.size())
+ std::vector<float> TimeSeriesUtils::Resample(const std::vector<float>& timestamps, const std::vector<float>& data, const std::vector<float>& newTimestamps)
{
- while (newTimestamps.at(j) > timestamps.at(i + 1) && i < data.size() - 2)
+ ARMARX_CHECK_EQUAL(data.size(), timestamps.size());
+ ARMARX_CHECK_EQUAL(data.size(), newTimestamps.size());
+ std::vector<float> result;
+ result.reserve(data.size());
+
+ if (data.size() == 0)
{
- i++;
+ return result;
+ }
+ if (data.size() == 1)
+ {
+ result.push_back(data.at(0));
+ return result;
}
- float f = math::MathUtils::ILerp(timestamps.at(i), timestamps.at(i + 1), newTimestamps.at(j));
- result.push_back(math::MathUtils::LerpClamp(data.at(i), data.at(i + 1), f));
- j++;
- }
- return result;
-}
+ size_t i = 0;
+ size_t j = 0;
-std::vector<float> TimeSeriesUtils::ApplyFilter(const std::vector<float>& data, const std::vector<float>& filter, BorderMode mode)
-{
- std::vector<float> result;
- size_t start = filter.size() / 2;
- for (size_t i = start; i < data.size() + start; i++)
- {
- float y = 0;
- float w = 0;
- for (size_t j = 0; j < filter.size(); j++)
+ while (j < data.size())
{
- int k = (int)i - (int)j;
- if (k < 0)
+ while (newTimestamps.at(j) > timestamps.at(i + 1) && i < data.size() - 2)
{
- k = 0;
+ i++;
}
- if (k >= (int)data.size())
+ float f = math::MathUtils::ILerp(timestamps.at(i), timestamps.at(i + 1), newTimestamps.at(j));
+ result.push_back(math::MathUtils::LerpClamp(data.at(i), data.at(i + 1), f));
+ j++;
+ }
+
+ return result;
+ }
+
+ std::vector<float> TimeSeriesUtils::ApplyFilter(const std::vector<float>& data, const std::vector<float>& filter, BorderMode mode)
+ {
+ std::vector<float> result;
+ size_t start = filter.size() / 2;
+ for (size_t i = start; i < data.size() + start; i++)
+ {
+ float y = 0;
+ float w = 0;
+ for (size_t j = 0; j < filter.size(); j++)
{
- k = data.size() - 1;
+ int k = (int)i - (int)j;
+ if (k < 0)
+ {
+ k = 0;
+ }
+ if (k >= (int)data.size())
+ {
+ k = data.size() - 1;
+ }
+ y += data.at(k) * filter.at(j);
+ w += filter.at(j);
}
- y += data.at(k) * filter.at(j);
- w += filter.at(j);
+ result.push_back(w == 0 ? 0 : y / w);
}
- result.push_back(w == 0 ? 0 : y / w);
+ return result;
}
- return result;
-}
-std::vector<float> TimeSeriesUtils::ApplyGaussianFilter(const std::vector<float>& data, float sigma, float sampleTime, BorderMode mode)
-{
- std::vector<float> filter = CreateGaussianFilter(sigma, sampleTime);
- return ApplyFilter(data, filter, mode);
-}
+ std::vector<float> TimeSeriesUtils::ApplyGaussianFilter(const std::vector<float>& data, float sigma, float sampleTime, BorderMode mode)
+ {
+ std::vector<float> filter = CreateGaussianFilter(sigma, sampleTime);
+ return ApplyFilter(data, filter, mode);
+ }
-std::vector<float> TimeSeriesUtils::CreateGaussianFilter(const float sigma, float sampleTime, float truncate)
-{
- std::vector<float> filter;
- int range = (int)(truncate * sigma / sampleTime);
- for (int i = -range; i <= range; i++)
+ std::vector<float> TimeSeriesUtils::CreateGaussianFilter(const float sigma, float sampleTime, float truncate)
{
- float x = i * sampleTime;
- filter.push_back(exp(-x * x / (2 * sigma * sigma) / (sigma * sqrt(2 * M_PI))));
+ std::vector<float> filter;
+ int range = (int)(truncate * sigma / sampleTime);
+ for (int i = -range; i <= range; i++)
+ {
+ float x = i * sampleTime;
+ filter.push_back(exp(-x * x / (2 * sigma * sigma) / (sigma * sqrt(2 * M_PI))));
+ }
+ return filter;
}
- return filter;
-}
-std::vector<float> TimeSeriesUtils::MakeTimestamps(float start, float end, size_t count)
-{
- std::vector<float> result;
- for (size_t i = 0; i < count; i++)
+ std::vector<float> TimeSeriesUtils::MakeTimestamps(float start, float end, size_t count)
{
- result.push_back(MathUtils::Lerp(start, end, (float)i / (float)(count - 1)));
+ std::vector<float> result;
+ for (size_t i = 0; i < count; i++)
+ {
+ result.push_back(MathUtils::Lerp(start, end, (float)i / (float)(count - 1)));
+ }
+ return result;
}
- return result;
}
diff --git a/source/RobotAPI/libraries/diffik/CMakeLists.txt b/source/RobotAPI/libraries/diffik/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9699d191373f2210ec6677e05a8af1b6315ca71d
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/CMakeLists.txt
@@ -0,0 +1,32 @@
+set(LIB_NAME diffik)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+set(LIBS
+ ArmarXCore
+ RobotAPICore
+)
+
+set(LIB_FILES
+ NaturalDiffIK.cpp
+ SimpleDiffIK.cpp
+)
+set(LIB_HEADERS
+ NaturalDiffIK.h
+ SimpleDiffIK.h
+)
+
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+#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(diffik PUBLIC ${MyLib_INCLUDE_DIRS})
+#endif()
+
+# add unit tests
+add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/core/diffik/NaturalDiffIK.cpp b/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp
similarity index 80%
rename from source/RobotAPI/libraries/core/diffik/NaturalDiffIK.cpp
rename to source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp
index 177418b427d9dc01d3bc35f14b89fac9ed00900c..eba6a1bfc7c69a2731de9878ce526cee389206d5 100644
--- a/source/RobotAPI/libraries/core/diffik/NaturalDiffIK.cpp
+++ b/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp
@@ -21,10 +21,12 @@
* GNU General Public License
*/
-#include "../CartesianPositionController.h"
-#include "../CartesianVelocityController.h"
+
#include "NaturalDiffIK.h"
+#include <RobotAPI/libraries/core/CartesianPositionController.h>
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+
namespace armarx
{
Eigen::VectorXf NaturalDiffIK::LimitInfNormTo(Eigen::VectorXf vec, float maxValue)
@@ -48,24 +50,23 @@ namespace armarx
//ARMARX_IMPORTANT << "start";
+ std::vector<IKStep> ikSteps;
Eigen::VectorXf currentJointValues = rns->getJointValuesEigen();
for (size_t i = 0; i <= params.stepsInitial + params.stepsFineTune; i++)
{
-
-
//ss << pdTcp.norm() << " ## " << odTcp.norm() << " ## " << pdElb.norm() << std::endl;
int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
int oriLen = mode & VirtualRobot::IKSolver::Orientation ? 3 : 0;
+ Eigen::Vector3f pdTcp = posLen ? pcTcp.getPositionDiff(targetPose) : Eigen::Vector3f::Zero();
+ Eigen::Vector3f odTcp = oriLen ? pcTcp.getOrientationDiff(targetPose) : Eigen::Vector3f::Zero();
Eigen::VectorXf cartesianVel(posLen + oriLen);
if (posLen)
{
- Eigen::Vector3f pdTcp = pcTcp.getPositionDiff(targetPose);
cartesianVel.block<3, 1>(0, 0) = pdTcp;
}
if (oriLen)
{
- Eigen::Vector3f odTcp = pcTcp.getOrientationDiff(targetPose);
cartesianVel.block<3, 1>(posLen, 0) = odTcp;
}
@@ -73,20 +74,33 @@ namespace armarx
Eigen::VectorXf cartesianVelElb(3);
cartesianVelElb.block<3, 1>(0, 0) = pdElb;
Eigen::VectorXf jvElb = params.elbowKp * vcElb.calculate(cartesianVelElb, VirtualRobot::IKSolver::Position);
- //jvElb = LimitInfNormTo(jvElb, params.maxJointAngleStep);
-
Eigen::VectorXf jvLA = params.jointLimitAvoidanceKp * vcTcp.calculateJointLimitAvoidance();
- //jvLA = LimitInfNormTo(jvLA, params.maxJointAngleStep);
-
Eigen::VectorXf jv = vcTcp.calculate(cartesianVel, jvElb + jvLA, mode);
float stepLength = i < params.stepsInitial ? params.ikStepLengthInitial : params.ikStepLengthFineTune;
- jv = jv * stepLength;
+ Eigen::VectorXf jvClamped = jv * stepLength;
+ jvClamped = LimitInfNormTo(jvClamped, params.maxJointAngleStep);
+
+ if (params.returnIKSteps)
+ {
+ IKStep s;
+ s.pdTcp = pdTcp;
+ s.odTcp = odTcp;
+ s.pdElb = pdElb;
+ s.tcpPose = tcp->getPoseInRootFrame();
+ s.elbPose = elbow->getPoseInRootFrame();
+ s.cartesianVel = cartesianVel;
+ s.cartesianVelElb = cartesianVelElb;
+ s.jvElb = jvElb;
+ s.jvLA = jvLA;
+ s.jv = jv;
+ s.jvClamped = jvClamped;
+ ikSteps.emplace_back(s);
+ }
- jv = LimitInfNormTo(jv, params.maxJointAngleStep);
- Eigen::VectorXf newJointValues = currentJointValues + jv;
+ Eigen::VectorXf newJointValues = currentJointValues + jvClamped;
rns->setJointValues(newJointValues);
currentJointValues = newJointValues;
}
@@ -94,6 +108,7 @@ namespace armarx
//ARMARX_IMPORTANT << ss.str();
Result result;
+ result.ikSteps = ikSteps;
result.jointValues = rns->getJointValuesEigen();
result.posDiff = pcTcp.getPositionDiff(targetPose);
result.oriDiff = pcTcp.getOrientationDiff(targetPose);
diff --git a/source/RobotAPI/libraries/core/diffik/NaturalDiffIK.h b/source/RobotAPI/libraries/diffik/NaturalDiffIK.h
similarity index 81%
rename from source/RobotAPI/libraries/core/diffik/NaturalDiffIK.h
rename to source/RobotAPI/libraries/diffik/NaturalDiffIK.h
index 08cd4cc32f7891ad414bab0ded7add490ed421ab..9b1711766253d44007e182c76525af62b87d60b2 100644
--- a/source/RobotAPI/libraries/core/diffik/NaturalDiffIK.h
+++ b/source/RobotAPI/libraries/diffik/NaturalDiffIK.h
@@ -49,7 +49,24 @@ namespace armarx
float jointLimitAvoidanceMargins = 5.0f / 180 * M_PI;
float elbowKp = 1.0f;
float maxJointAngleStep = 0.1f;
+ bool returnIKSteps = false;
};
+ struct IKStep
+ {
+ Eigen::VectorXf jointValues;
+ Eigen::Vector3f pdTcp;
+ Eigen::Vector3f odTcp;
+ Eigen::Vector3f pdElb;
+ Eigen::Matrix4f tcpPose;
+ Eigen::Matrix4f elbPose;
+ Eigen::VectorXf cartesianVel;
+ Eigen::VectorXf cartesianVelElb;
+ Eigen::VectorXf jvElb;
+ Eigen::VectorXf jvLA;
+ Eigen::VectorXf jv;
+ Eigen::VectorXf jvClamped;
+ };
+
struct Result
{
Eigen::VectorXf jointValues;
@@ -63,6 +80,7 @@ namespace armarx
Eigen::VectorXf jointLimitMargins;
float minimumJointLimitMargin;
Eigen::Vector3f elbowPosDiff;
+ std::vector<IKStep> ikSteps;
};
static Eigen::VectorXf LimitInfNormTo(Eigen::VectorXf vec, float maxValue);
diff --git a/source/RobotAPI/libraries/core/diffik/SimpleDiffIK.cpp b/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
similarity index 71%
rename from source/RobotAPI/libraries/core/diffik/SimpleDiffIK.cpp
rename to source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
index fb3dd0ef492db58d7535421f20b6278b61513485..6d0021aec8bab8f99e5fe64ddf86f969b4bdcf80 100644
--- a/source/RobotAPI/libraries/core/diffik/SimpleDiffIK.cpp
+++ b/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
@@ -21,18 +21,36 @@
* GNU General Public License
*/
-#include "../CartesianPositionController.h"
-#include "../CartesianVelocityController.h"
#include "SimpleDiffIK.h"
+#include <RobotAPI/libraries/core/CartesianPositionController.h>
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+
namespace armarx
{
SimpleDiffIK::Result SimpleDiffIK::CalculateDiffIK(const Eigen::Matrix4f targetPose, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, Parameters params)
{
+
tcp = tcp ? tcp : rns->getTCP();
CartesianVelocityController velocityController(rns);
CartesianPositionController positionController(tcp);
+ if (params.resetRnsValues)
+ {
+ for (VirtualRobot::RobotNodePtr rn : rns->getAllRobotNodes())
+ {
+ if (rn->isLimitless())
+ {
+ rn->setJointValue(0);
+ }
+ else
+ {
+ rn->setJointValue((rn->getJointLimitHi() + rn->getJointLimitLo()) / 2);
+ }
+ }
+ }
+
+ std::vector<IKStep> ikSteps;
Eigen::VectorXf currentJointValues = rns->getJointValuesEigen();
for (size_t i = 0; i <= params.stepsInitial + params.stepsFineTune; i++)
{
@@ -41,27 +59,43 @@ namespace armarx
//ARMARX_IMPORTANT << VAROUT(posDiff) << VAROUT(oriDiff);
- Eigen::VectorXf cartesialVel(6);
- cartesialVel << posDiff(0), posDiff(1), posDiff(2), oriDiff(0), oriDiff(1), oriDiff(2);
- Eigen::VectorXf jnv = params.jointLimitAvoidanceKp * velocityController.calculateJointLimitAvoidance();
- Eigen::VectorXf jv = velocityController.calculate(cartesialVel, jnv, VirtualRobot::IKSolver::All);
+ Eigen::VectorXf cartesianVel(6);
+ cartesianVel << posDiff(0), posDiff(1), posDiff(2), oriDiff(0), oriDiff(1), oriDiff(2);
+ const Eigen::VectorXf jnv = params.jointLimitAvoidanceKp * velocityController.calculateJointLimitAvoidance();
+ const Eigen::VectorXf jv = velocityController.calculate(cartesianVel, jnv, VirtualRobot::IKSolver::All);
+
float stepLength = i < params.stepsInitial ? params.ikStepLengthInitial : params.ikStepLengthFineTune;
- jv = jv * stepLength;
+ Eigen::VectorXf jvClamped = jv * stepLength;
- float infNorm = jv.lpNorm<Eigen::Infinity>();
+ float infNorm = jvClamped.lpNorm<Eigen::Infinity>();
if (infNorm > params.maxJointAngleStep)
{
- jv = jv / infNorm * params.maxJointAngleStep;
+ jvClamped = jvClamped / infNorm * params.maxJointAngleStep;
+ }
+
+ if (params.returnIKSteps)
+ {
+ IKStep s;
+ s.posDiff = posDiff;
+ s.oriDiff = oriDiff;
+ s.cartesianVel = cartesianVel;
+ s.jnv = jnv;
+ s.jv = jv;
+ s.infNorm = infNorm;
+ s.jvClamped = jvClamped;
+ ikSteps.emplace_back(s);
}
- Eigen::VectorXf newJointValues = currentJointValues + jv;
+
+ Eigen::VectorXf newJointValues = currentJointValues + jvClamped;
rns->setJointValues(newJointValues);
currentJointValues = newJointValues;
}
Result result;
+ result.ikSteps = ikSteps;
result.jointValues = rns->getJointValuesEigen();
result.posDiff = positionController.getPositionDiff(targetPose);
result.oriDiff = positionController.getOrientationDiff(targetPose);
diff --git a/source/RobotAPI/libraries/core/diffik/SimpleDiffIK.h b/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
similarity index 86%
rename from source/RobotAPI/libraries/core/diffik/SimpleDiffIK.h
rename to source/RobotAPI/libraries/diffik/SimpleDiffIK.h
index e6659dcba3d275d44fa8e0b23e05cb044eb75047..4ae4e22fa9ddf740455e9eec2cb561f365a7ca53 100644
--- a/source/RobotAPI/libraries/core/diffik/SimpleDiffIK.h
+++ b/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
@@ -47,7 +47,21 @@ namespace armarx
float maxOriError = 0.05f;
float jointLimitAvoidanceKp = 2.0f;
float maxJointAngleStep = 0.1f;
+ bool returnIKSteps = false;
+ bool resetRnsValues = true;
};
+ struct IKStep
+ {
+ Eigen::VectorXf jointValues;
+ Eigen::Vector3f posDiff;
+ Eigen::Vector3f oriDiff;
+ Eigen::VectorXf cartesianVel;
+ Eigen::VectorXf jnv;
+ Eigen::VectorXf jv;
+ float infNorm;
+ Eigen::VectorXf jvClamped;
+ };
+
struct Result
{
Eigen::VectorXf jointValues;
@@ -58,6 +72,7 @@ namespace armarx
bool reached;
Eigen::VectorXf jointLimitMargins;
float minimumJointLimitMargin;
+ std::vector<IKStep> ikSteps;
};
@@ -69,9 +84,11 @@ namespace armarx
Eigen::VectorXf jointLimitMargins;
float maxPosError = 0;
float maxOriError = 0;
+ std::vector<Result> ikResults;
void aggregate(const Result& result)
{
+ ikResults.emplace_back(result);
reachable = reachable && result.reached;
minimumJointLimitMargin = std::min(minimumJointLimitMargin, result.minimumJointLimitMargin);
if (jointLimitMargins.rows() == 0)
diff --git a/source/RobotAPI/libraries/diffik/diffik.cpp b/source/RobotAPI/libraries/diffik/diffik.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e6151ce4817a173f13184c0ea1f2b5b32c1774a4
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/diffik.cpp
@@ -0,0 +1,28 @@
+/*
+ * 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::diffik
+ * @author Your Name ( you at example dot com )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "diffik.h"
+
+namespace armarx
+{
+
+}
diff --git a/source/RobotAPI/libraries/diffik/diffik.h b/source/RobotAPI/libraries/diffik/diffik.h
new file mode 100644
index 0000000000000000000000000000000000000000..0cca7135d50c63101d0229593e0b2245786da79d
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/diffik.h
@@ -0,0 +1,45 @@
+/*
+ * 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::diffik
+ * @author Your Name ( you at example dot com )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+namespace armarx
+{
+ /**
+ * @defgroup Library-diffik diffik
+ * @ingroup RobotAPI
+ * A description of the library diffik.
+ *
+ * @class diffik
+ * @ingroup Library-diffik
+ * @brief Brief description of class diffik.
+ *
+ * Detailed description of class diffik.
+ */
+ class diffik
+ {
+ public:
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/diffik/test/CMakeLists.txt b/source/RobotAPI/libraries/diffik/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..56cc688f148b052563da7ef78322c81a95b3be7b
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore diffik)
+
+armarx_add_test(diffikTest diffikTest.cpp "${LIBS}")
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/diffik/test/diffikTest.cpp b/source/RobotAPI/libraries/diffik/test/diffikTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8ebfea4268229e26053befcac6ca0ad005a6f135
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/test/diffikTest.cpp
@@ -0,0 +1,36 @@
+/*
+ * 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::diffik
+ * @author Your Name ( you at example dot com )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::diffik
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include "../diffik.h"
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+
+ BOOST_CHECK_EQUAL(true, true);
+}
diff --git a/source/RobotAPI/libraries/natik/CMakeLists.txt b/source/RobotAPI/libraries/natik/CMakeLists.txt
index ad6f3baee96b735c823f9318914670a104d5f864..0fabf7d5c502742598287c4cc8e837d6081eefce 100644
--- a/source/RobotAPI/libraries/natik/CMakeLists.txt
+++ b/source/RobotAPI/libraries/natik/CMakeLists.txt
@@ -7,6 +7,7 @@ set(LIBS
ArmarXCoreInterfaces
ArmarXCore
RobotAPICore
+ diffik
)
set(LIB_FILES
diff --git a/source/RobotAPI/libraries/natik/NaturalIK.h b/source/RobotAPI/libraries/natik/NaturalIK.h
index 36f6abd8ed7b20be7f8888faea38e7010fe3735f..3a7a0506456dfb88f2dded2868644188a61130d3 100644
--- a/source/RobotAPI/libraries/natik/NaturalIK.h
+++ b/source/RobotAPI/libraries/natik/NaturalIK.h
@@ -27,7 +27,7 @@
//#include <RobotAPI/libraries/core/SimpleDiffIK.h>
#include <VirtualRobot/Nodes/RobotNode.h>
-#include <RobotAPI/libraries/core/diffik/NaturalDiffIK.h>
+#include <RobotAPI/libraries/diffik/NaturalDiffIK.h>
#include <optional>
namespace armarx