diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
index 7ab4f089abe260f55ff832fb7777d4c16f8b2dc3..1b4935111f9e5e7dd40e765cb3187722ae3fda73 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
@@ -34,9 +34,11 @@
#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/exceptions/Exception.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlWriter.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXGui/libraries/StructuralJson/JPathNavigator.h>
#include <ArmarXGui/libraries/StructuralJson/StructuralJsonParser.h>
@@ -44,6 +46,8 @@
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
+#include <eigen3/Eigen/src/Core/util/Constants.h>
+
namespace armarx
{
@@ -138,6 +142,8 @@ namespace armarx
GraspTrajectoryPtr
GraspTrajectory::ReadFromFile(const std::string& filename)
{
+ ARMARX_IMPORTANT << filename;
+
ARMARX_TRACE;
const auto ext = std::filesystem::path(filename).extension();
if (ext == ".xml")
@@ -159,15 +165,37 @@ namespace armarx
inline void
from_json(const nlohmann::json& j, GraspTrajectoryKeypoint& kp)
{
- const std::optional<std::string> shape =
- j.contains("shape") ? std::optional<std::string>(j.at("shape").get<std::string>())
- : std::nullopt;
+ ARMARX_TRACE;
+ std::optional<std::string> shape;
- const std::map<std::string, float> targetHandValues =
- j.contains("HandValues") ? j.at("HandValues").get<std::map<std::string, float>>()
- : std::map<std::string, float>{};
+ if (j.contains("shape"))
+ {
+ if (not j.at("shape").is_null())
+ {
+ shape = j.at("shape");
+ }
+ }
+
+ ARMARX_TRACE;
+ std::optional<std::map<std::string, float>> targetHandValues;
- kp = GraspTrajectoryKeypoint(j.at("Pose"), targetHandValues, shape);
+ if (j.contains("fingerValues"))
+ {
+ if (not j.at("fingerValues").is_null())
+ {
+ targetHandValues = j.at("fingerValues");
+ }
+ }
+
+ // VERY hacky!
+ const Eigen::Matrix<float, 4, 4, Eigen::RowMajor> aronTf = j.at("pose");
+ const Eigen::Matrix4f tf = aronTf.transpose();
+
+ ARMARX_IMPORTANT << tf;
+
+ ARMARX_TRACE;
+ kp = GraspTrajectoryKeypoint(
+ tf, targetHandValues.value_or(std::map<std::string, float>{}), shape);
}
GraspTrajectoryPtr
@@ -179,9 +207,27 @@ namespace armarx
const nlohmann::json j = nlohmann::json::parse(ifs);
std::vector<GraspTrajectoryKeypoint> traj;
- traj = j;
+ traj = j.at("keypoints");
- return std::make_shared<GraspTrajectory>(traj);
+ const armarx::Duration duration =
+ armarx::Duration::MicroSeconds(j.at("duration").at("microSeconds").get<std::int64_t>());
+
+ // at the moment, we assume that all keypoints are sampled equidistant
+
+ ARMARX_CHECK_NOT_EMPTY(traj);
+ const float dtSeconds = duration.toSecondsDouble() / (traj.size() - 1);
+
+ for (auto& kp : traj)
+ {
+ kp.dt = dtSeconds;
+ }
+
+ const std::string frame = j.at("frame").at("frame");
+
+ auto graspTrajectory = std::make_shared<GraspTrajectory>(traj);
+ graspTrajectory->setFrame(frame);
+
+ return graspTrajectory;
}
GraspTrajectoryPtr
@@ -557,7 +603,8 @@ namespace armarx
traj->addKeypoint(
::math::Helpers::TranslateAndRotatePose(kp->getTargetPose(), translation, rotation),
kp->handJointsTarget,
- kp->dt);
+ kp->dt,
+ kp->shape);
}
return traj;
}
@@ -566,12 +613,13 @@ namespace armarx
GraspTrajectory::getTransformed(const Eigen::Matrix4f& transform) const
{
ARMARX_TRACE;
- GraspTrajectoryPtr traj(
- new GraspTrajectory(transform * getStartPose(), getKeypoint(0)->handJointsTarget));
+ GraspTrajectoryPtr traj(new GraspTrajectory(
+ transform * getStartPose(), getKeypoint(0)->handJointsTarget, getKeypoint(0)->shape));
for (size_t i = 1; i < keypoints.size(); i++)
{
const KeypointPtr& kp = keypoints.at(i);
- traj->addKeypoint(transform * kp->getTargetPose(), kp->handJointsTarget, kp->dt);
+ traj->addKeypoint(
+ transform * kp->getTargetPose(), kp->handJointsTarget, kp->dt, kp->shape);
}
return traj;
}
@@ -620,7 +668,7 @@ namespace armarx
const GraspTrajectory::KeypointPtr& kp = getKeypoint(i);
Eigen::Matrix4f target_pose = kp->getTargetPose();
target_pose.block<3, 3>(0, 0) = flip_yz * target_pose.block<3, 3>(0, 0) * flip_yz;
- output_trajectory->addKeypoint(target_pose, kp->handJointsTarget, kp->dt);
+ output_trajectory->addKeypoint(target_pose, kp->handJointsTarget, kp->dt, kp->shape);
}
return output_trajectory;
}
@@ -721,4 +769,16 @@ namespace armarx
feedForwardOriVelocity(0, 0, 0)
{
}
+
+ void
+ GraspTrajectory::setFrame(const std::string& frame)
+ {
+ frame_ = frame;
+ }
+
+ const std::optional<std::string>&
+ GraspTrajectory::getFrame() const
+ {
+ return frame_;
+ }
} // namespace armarx
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h
index 718642c2ae5028e4cbf7cbee4cabf6fcc4373497..15786c619c4833f5e77e23148cc04dfa8294e9c6 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h
@@ -175,11 +175,16 @@ namespace armarx
static GraspTrajectoryPtr ReadFromJSON(const std::string& filename);
static GraspTrajectoryPtr ReadFromString(const std::string& xml);
+ void setFrame(const std::string& frame);
+ const std::optional<std::string>& getFrame() const;
+
private:
void updateKeypointMap();
private:
std::vector<KeypointPtr> keypoints;
std::map<float, size_t> keypointMap;
+
+ std::optional<std::string> frame_;
};
} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp
index c36d567350b3050ed49e7d362dfcaf2f63f3a643..58e15d056ae74b3d79b48ef917822d20c9de206c 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp
@@ -51,18 +51,29 @@ namespace armarx::armem::gui::instance
}
else
{
- return;
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
}
try
{
key = path.getLastElement();
+ if (!elementData)
+ {
+ // push NULL
+ jsonStack.push({key, nullptr});
+ }
+ else
+ {
+ // push new object as there is data
+ jsonStack.push({key, nlohmann::json(nlohmann::json::value_t::object)});
+ }
}
catch (const aron::error::AronException& e)
{
// This happens when we start at the top-level object.
+ jsonStack.push({key, nlohmann::json(nlohmann::json::value_t::object)});
}
- jsonStack.push({key, nlohmann::json(nlohmann::json::value_t::object)});
}
void
@@ -111,10 +122,34 @@ namespace armarx::armem::gui::instance
}
void
- TreeTypedJSONConverter::visitListOnEnter(DataInput& elementData, TypeInput& /*elementType*/)
+ TreeTypedJSONConverter::visitListOnEnter(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- jsonStack.push({key, nlohmann::json(nlohmann::json::value_t::array)});
+ std::string key = "";
+ aron::Path path;
+ if (elementData)
+ {
+ path = elementData->getPath();
+ }
+ else if(elementType)
+ {
+ path = elementType->getPath();
+ }
+ else
+ {
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
+ }
+
+ key = path.getLastElement();
+
+ if (!elementData)
+ {
+ jsonStack.push({key, nlohmann::json(nlohmann::json::value_t::array)});
+ }
+ else
+ {
+ jsonStack.push({key, nullptr});
+ }
}
void
@@ -128,115 +163,263 @@ namespace armarx::armem::gui::instance
void
TreeTypedJSONConverter::visitMatrix(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- auto nd = *aron::data::NDArray::DynamicCastAndCheck(elementData);
- auto t = *aron::type::Matrix::DynamicCastAndCheck(elementType);
- nlohmann::json obj;
- if (nd.getType() == "float")
+ if (elementData)
{
- Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic>> m(
- reinterpret_cast<float*>(nd.getData()), t.getRows(), t.getCols());
- Eigen::to_json(obj, m);
+ auto e = aron::data::NDArray::DynamicCast(elementData);
+ auto t = *aron::type::Matrix::DynamicCastAndCheck(elementType);
+
+ auto& nd = *e;
+ const std::string key = nd.getPath().getLastElement();
+
+ nlohmann::json obj;
+ if (nd.getType() == "float") // TODO: are these types correct?
+ {
+ Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic>> m(
+ reinterpret_cast<float*>(nd.getData()), t.getRows(), t.getCols());
+ Eigen::to_json(obj, m);
+ }
+ else if (nd.getType() == "double")
+ {
+ Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>> m(
+ reinterpret_cast<double*>(nd.getData()), t.getRows(), t.getCols());
+ Eigen::to_json(obj, m);
+ }
+ else
+ {
+ obj = handleGenericNDArray(nd);
+ }
+
+ insertIntoJSON(key, obj);
}
- else if (nd.getType() == "double")
+ else if(elementType)
{
- Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>> m(
- reinterpret_cast<double*>(nd.getData()), t.getRows(), t.getCols());
- Eigen::to_json(obj, m);
+ std::string key = elementType->getPath().getLastElement();
+ insertNULLIntoJSON(key);
}
else
{
- obj = handleGenericNDArray(nd);
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
}
- insertIntoJSON(key, obj);
}
void
- TreeTypedJSONConverter::visitNDArray(DataInput& elementData, TypeInput& /*elementType*/)
+ TreeTypedJSONConverter::visitNDArray(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- auto nd = *aron::data::NDArray::DynamicCastAndCheck(elementData);
- insertIntoJSON(key, handleGenericNDArray(nd));
+ if (elementData)
+ {
+ const std::string key = elementData->getPath().getLastElement();
+ auto nd = *aron::data::NDArray::DynamicCastAndCheck(elementData);
+ insertIntoJSON(key, handleGenericNDArray(nd));
+ }
+ else if(elementType)
+ {
+ std::string key = elementType->getPath().getLastElement();
+ insertNULLIntoJSON(key);
+ }
+ else
+ {
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
+ }
}
void
- TreeTypedJSONConverter::visitQuaternion(DataInput& elementData, TypeInput& /*elementType*/)
+ TreeTypedJSONConverter::visitQuaternion(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- auto nd = *aron::data::NDArray::DynamicCastAndCheck(elementData);
- nlohmann::json obj;
- Eigen::to_json(obj, aron::data::converter::AronEigenConverter::ConvertToQuaternionf(nd));
- insertIntoJSON(key, obj);
+ if (elementData)
+ {
+ const std::string key = elementData->getPath().getLastElement();
+ auto nd = *aron::data::NDArray::DynamicCastAndCheck(elementData);
+ nlohmann::json obj;
+ Eigen::to_json(obj, aron::data::converter::AronEigenConverter::ConvertToQuaternionf(nd));
+ insertIntoJSON(key, obj);
+ }
+ else if (elementType)
+ {
+ std::string key = elementType->getPath().getLastElement();
+ insertNULLIntoJSON(key);
+ }
+ else
+ {
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
+ }
+
}
void
- TreeTypedJSONConverter::visitImage(DataInput& elementData, TypeInput& /*elementType*/)
+ TreeTypedJSONConverter::visitImage(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- auto nd = *aron::data::NDArray::DynamicCastAndCheck(elementData);
- insertIntoJSON(key, handleGenericNDArray(nd));
+ if (elementData)
+ {
+ const std::string key = elementData->getPath().getLastElement();
+ auto nd = *aron::data::NDArray::DynamicCastAndCheck(elementData);
+ insertIntoJSON(key, handleGenericNDArray(nd));
+ }
+ else if (elementType)
+ {
+ std::string key = elementType->getPath().getLastElement();
+ insertNULLIntoJSON(key);
+ }
+ else
+ {
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
+ }
}
void
- TreeTypedJSONConverter::visitPointCloud(DataInput& elementData, TypeInput& /*elementType*/)
+ TreeTypedJSONConverter::visitPointCloud(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- auto nd = *aron::data::NDArray::DynamicCastAndCheck(elementData);
- insertIntoJSON(key, handleGenericNDArray(nd));
+ if (elementData)
+ {
+ const std::string key = elementData->getPath().getLastElement();
+ auto nd = *aron::data::NDArray::DynamicCastAndCheck(elementData);
+ insertIntoJSON(key, handleGenericNDArray(nd));
+ }
+ else if (elementType)
+ {
+ std::string key = elementType->getPath().getLastElement();
+ insertNULLIntoJSON(key);
+ }
+ else
+ {
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
+ }
}
void
TreeTypedJSONConverter::visitIntEnum(DataInput& elementData, TypeInput& elementType)
{
- /* Not handled by the TreeTypedDataVisitor either */
+ this->visitInt(elementData, elementType);
}
void
- TreeTypedJSONConverter::visitInt(DataInput& elementData, TypeInput& /*elementType*/)
+ TreeTypedJSONConverter::visitInt(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- int i = *aron::data::Int::DynamicCastAndCheck(elementData);
- insertIntoJSON(key, i);
+ if (elementData)
+ {
+ const std::string key = elementData->getPath().getLastElement();
+ int i = *aron::data::Int::DynamicCastAndCheck(elementData);
+ insertIntoJSON(key, i);
+ }
+ else if (elementType)
+ {
+ std::string key = elementType->getPath().getLastElement();
+ insertNULLIntoJSON(key);
+ }
+ else
+ {
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
+ }
}
void
- TreeTypedJSONConverter::visitLong(DataInput& elementData, TypeInput& /*elementType*/)
+ TreeTypedJSONConverter::visitLong(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- int64_t l = *aron::data::Long::DynamicCastAndCheck(elementData);
- insertIntoJSON(key, l);
+ if (elementData)
+ {
+ const std::string key = elementData->getPath().getLastElement();
+ int64_t l = *aron::data::Long::DynamicCastAndCheck(elementData);
+ insertIntoJSON(key, l);
+ }
+ else if (elementType)
+ {
+ std::string key = elementType->getPath().getLastElement();
+ insertNULLIntoJSON(key);
+ }
+ else
+ {
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
+ }
}
void
- TreeTypedJSONConverter::visitFloat(DataInput& elementData, TypeInput& /*elementType*/)
+ TreeTypedJSONConverter::visitFloat(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- float f = *aron::data::Float::DynamicCastAndCheck(elementData);
- insertIntoJSON(key, f);
+ if (elementData)
+ {
+ const std::string key = elementData->getPath().getLastElement();
+ float f = *aron::data::Float::DynamicCastAndCheck(elementData);
+ insertIntoJSON(key, f);
+ }
+ else if (elementType)
+ {
+ std::string key = elementType->getPath().getLastElement();
+ insertNULLIntoJSON(key);
+ }
+ else
+ {
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
+ }
}
void
- TreeTypedJSONConverter::visitDouble(DataInput& elementData, TypeInput& /*elementType*/)
+ TreeTypedJSONConverter::visitDouble(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- double d = *aron::data::Double::DynamicCastAndCheck(elementData);
- insertIntoJSON(key, d);
+ if (elementData)
+ {
+ const std::string key = elementData->getPath().getLastElement();
+ double d = *aron::data::Double::DynamicCastAndCheck(elementData);
+ insertIntoJSON(key, d);
+ }
+ else if (elementType)
+ {
+ std::string key = elementType->getPath().getLastElement();
+ insertNULLIntoJSON(key);
+ }
+ else
+ {
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
+ }
}
void
- TreeTypedJSONConverter::visitBool(DataInput& elementData, TypeInput& /*elementType*/)
+ TreeTypedJSONConverter::visitBool(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- bool b = *aron::data::Bool::DynamicCastAndCheck(elementData);
- insertIntoJSON(key, b);
+ if (elementData)
+ {
+ const std::string key = elementData->getPath().getLastElement();
+ bool b = *aron::data::Bool::DynamicCastAndCheck(elementData);
+ insertIntoJSON(key, b);
+ }
+ else if (elementType)
+ {
+ std::string key = elementType->getPath().getLastElement();
+ insertNULLIntoJSON(key);
+ }
+ else
+ {
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
+ }
}
void
- TreeTypedJSONConverter::visitString(DataInput& elementData, TypeInput& /*elementType*/)
+ TreeTypedJSONConverter::visitString(DataInput& elementData, TypeInput& elementType)
{
- const std::string key = elementData->getPath().getLastElement();
- std::string s = *aron::data::String::DynamicCastAndCheck(elementData);
- insertIntoJSON(key, s);
+ if (elementData)
+ {
+ const std::string key = elementData->getPath().getLastElement();
+ std::string s = *aron::data::String::DynamicCastAndCheck(elementData);
+ insertIntoJSON(key, s);
+ }
+ else if (elementType)
+ {
+ std::string key = elementType->getPath().getLastElement();
+ insertNULLIntoJSON(key);
+ }
+ else
+ {
+ // should not happen
+ throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
+ }
}
//void
@@ -266,6 +449,19 @@ namespace armarx::armem::gui::instance
}
}
+ void
+ TreeTypedJSONConverter::insertNULLIntoJSON(const std::string& key)
+ {
+ if (jsonStack.top().second.is_object())
+ {
+ jsonStack.top().second[key] = nullptr;
+ }
+ else
+ {
+ jsonStack.top().second.emplace_back(nullptr);
+ }
+ }
+
nlohmann::json
TreeTypedJSONConverter::handleGenericNDArray(const aron::data::NDArray& nd)
{
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.h b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.h
index e299f8f64974bc32a5af1883fc0a10e52ebdedec..a83a6a2352f88c0c2aed408e38677e1be3c35ecf 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.h
@@ -48,6 +48,7 @@ namespace armarx::armem::gui::instance
template <typename ElementType>
void insertIntoJSON(const std::string& key, const ElementType& data);
+ void insertNULLIntoJSON(const std::string& key);
static nlohmann::json handleGenericNDArray(const aron::data::NDArray& nd);
};
diff --git a/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp b/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp
index b0799cdbd06bc44004359ebf87a1e1469296f18d..d060c70606a439a9b7abe8ec03f89c565aefaa17 100644
--- a/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp
@@ -508,7 +508,7 @@ namespace armarx::aron::data
RecursiveConstTypedVariantVisitor::GetObjectElementsWithNullType(DataInput& o, TypeInput& t)
{
std::map<std::string, std::pair<aron::data::VariantPtr, aron::type::VariantPtr>> ret;
- auto data = aron::data::Dict::DynamicCastAndCheck(o);
+ auto data = aron::data::Dict::DynamicCast(o);
auto type = aron::type::Object::DynamicCastAndCheck(t);
if (data)