diff --git a/source/RobotAPI/components/ArViz/Client/Elements.cpp b/source/RobotAPI/components/ArViz/Client/Elements.cpp
index 3ce3ea6f48c69658788abc3ba2ed00c6a81e1002..daec8bc655e0f6431dc531f0401478838e7f053b 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.cpp
+++ b/source/RobotAPI/components/ArViz/Client/Elements.cpp
@@ -9,15 +9,18 @@ namespace armarx::viz
{
const std::string Object::DefaultObjectsPackage = armarx::ObjectFinder::DefaultObjectsPackageName;
+ const std::string Object::DefaultRelativeObjectsDirectory = armarx::ObjectFinder::DefaultObjectsDirectory;
- Object& Object::fileByObjectFinder(const std::string& objectID, const std::string& objectsPackage)
+ Object& Object::fileByObjectFinder(const std::string& objectID, const std::string& objectsPackage,
+ const std::string& relativeObjectsDirectory)
{
- return this->fileByObjectFinder(armarx::ObjectID(objectID), objectsPackage);
+ return this->fileByObjectFinder(armarx::ObjectID(objectID), objectsPackage, relativeObjectsDirectory);
}
- Object& Object::fileByObjectFinder(const armarx::ObjectID& objectID, const std::string& objectsPackage)
+ Object& Object::fileByObjectFinder(const armarx::ObjectID& objectID, const std::string& objectsPackage,
+ const std::string& relativeObjectsDirectory)
{
- ObjectInfo info(objectsPackage, "", objectID);
+ ObjectInfo info(objectsPackage, "", relativeObjectsDirectory, objectID);
armarx::PackageFileLocation file = info.simoxXML();
return this->file(file.package, file.relativePath);
}
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.h b/source/RobotAPI/components/ArViz/Client/Elements.h
index 66f07992e0ba2dc0c382b3a9742a5413c9f192b6..e9157a24d9f60b9ba71d70cccd2141b0258665c3 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.h
+++ b/source/RobotAPI/components/ArViz/Client/Elements.h
@@ -461,6 +461,7 @@ namespace armarx::viz
{
private:
static const std::string DefaultObjectsPackage;
+ static const std::string DefaultRelativeObjectsDirectory;
public:
using ElementOps::ElementOps;
@@ -483,8 +484,12 @@ namespace armarx::viz
* @param objectsPackage The objects package ("ArmarXObjects" by default)
* @see <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
*/
- Object& fileByObjectFinder(const armarx::ObjectID& objectID, const std::string& objectsPackage = DefaultObjectsPackage);
- Object& fileByObjectFinder(const std::string& objectID, const std::string& objectsPackage = DefaultObjectsPackage);
+ Object& fileByObjectFinder(const armarx::ObjectID& objectID,
+ const std::string& objectsPackage = DefaultObjectsPackage,
+ const std::string& relativeObjectsDirectory = DefaultRelativeObjectsDirectory);
+ Object& fileByObjectFinder(const std::string& objectID,
+ const std::string& objectsPackage = DefaultObjectsPackage,
+ const std::string& relativeObjectsDirectory = DefaultRelativeObjectsDirectory);
Object& alpha(float alpha);
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index d39e94c87ce0ea5d5ed5b21e252d31f1e2ca41f9..b00a86caf56aec6a6e3f6c59e734ded8aea893b8 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -345,10 +345,12 @@ module armarx
void setMPWeights(DoubleSeqSeq weights);
DoubleSeqSeq getMPWeights();
- void setLinearVelocityKd(Ice::FloatSeq kd);
- void setLinearVelocityKp(Ice::FloatSeq kp);
- void setAngularVelocityKd(Ice::FloatSeq kd);
- void setAngularVelocityKp(Ice::FloatSeq kp);
+ void setLinearVelocityKd(Eigen::Vector3f kd);
+ void setLinearVelocityKp(Eigen::Vector3f kp);
+ void setAngularVelocityKd(Eigen::Vector3f kd);
+ void setAngularVelocityKp(Eigen::Vector3f kp);
+ void setNullspaceVelocityKd(Eigen::VectorXf jointValues);
+ void setNullspaceVelocityKp(Eigen::VectorXf jointValues);
};
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
index f964364b9e1e43083d9c6755370549c33cfa66ad..92a5760ae47f099c3408606903efb25db007f764 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -1,23 +1,24 @@
#include <VirtualRobot/XML/ObjectIO.h>
-#include <boost/algorithm/string.hpp>
+#include <set>
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/filesystem/list_directory.h>
-#include <VirtualRobot/XML/RobotIO.h>
-
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include "ObjectFinder.h"
+
namespace armarx
{
namespace fs = std::filesystem;
- ObjectFinder::ObjectFinder(const std::string& objectsPackageName) : packageName(objectsPackageName)
+
+ ObjectFinder::ObjectFinder(const std::string& objectsPackageName, const ObjectFinder::path& relObjectsDir) :
+ packageName(objectsPackageName), relObjectsDir(relObjectsDir)
{
Logging::setTag("ObjectFinder");
}
@@ -25,7 +26,7 @@ namespace armarx
void ObjectFinder::setPath(const std::string& path)
{
packageName = path;
- packageDataDir.clear();
+ absPackageDataDir.clear();
}
std::string ObjectFinder::getPackageName() const
@@ -35,11 +36,11 @@ namespace armarx
void ObjectFinder::init() const
{
- if (packageDataDir.empty())
+ if (absPackageDataDir.empty())
{
CMakePackageFinder packageFinder(packageName);
- packageDataDir = packageFinder.getDataDir();
- if (packageDataDir.empty())
+ absPackageDataDir = packageFinder.getDataDir();
+ if (absPackageDataDir.empty())
{
ARMARX_WARNING << "Could not find package '" << packageName << "'.";
// throw LocalException() << "Could not find package '" << packageName << "'.";
@@ -49,7 +50,7 @@ namespace armarx
ARMARX_VERBOSE << "Objects root directory: " << _rootDirAbs();
// make sure this data path is available => e.g. for findArticulatedObjects
- armarx::ArmarXDataPath::addDataPaths(std::vector<std::string> {packageDataDir});
+ armarx::ArmarXDataPath::addDataPaths(std::vector<std::string> {absPackageDataDir});
}
}
}
@@ -70,7 +71,7 @@ namespace armarx
}
if (!dataset.empty())
{
- return ObjectInfo(packageName, packageDataDir, dataset, name);
+ return ObjectInfo(packageName, absPackageDataDir, relObjectsDir, dataset, name);
}
// Search for object in datasets.
const std::vector<std::string>& datasets = getDatasets();
@@ -78,7 +79,7 @@ namespace armarx
{
if (fs::is_directory(_rootDirAbs() / dataset / name))
{
- return ObjectInfo(packageName, packageDataDir, dataset, name);
+ return ObjectInfo(packageName, absPackageDataDir, relObjectsDir, dataset, name);
}
}
@@ -217,7 +218,7 @@ namespace armarx
{
if (fs::is_directory(datasetDir / dir))
{
- ObjectInfo object(packageName, packageDataDir, dataset, dir.filename());
+ ObjectInfo object(packageName, absPackageDataDir, relObjectsDir, dataset, dir.filename());
if (!checkPaths || object.checkPaths())
{
objects.push_back(object);
@@ -268,51 +269,22 @@ namespace armarx
return {};
}
- const std::vector<std::string> validExtensions{".urdf", ".xml"};
-
- const auto hasValidExtension = [&](const std::string & file) -> bool
- {
- return std::find(validExtensions.begin(), validExtensions.end(), boost::algorithm::to_lower_copy(std::filesystem::path(file).extension().string())) != validExtensions.end();
- };
-
std::vector<armem::articulated_object::ArticulatedObjectDescription> objects;
const bool local = true;
for (const path& dir : simox::fs::list_directory(datasetDir, local))
{
- if (not fs::is_directory(datasetDir / dir))
- {
- continue;
- }
-
- for (const auto& file : std::filesystem::directory_iterator(datasetDir / dir))
+ if (fs::is_directory(datasetDir / dir))
{
- const std::string xml = std::filesystem::path(file).string();
-
- if (hasValidExtension(xml))
+ ObjectInfo object(packageName, absPackageDataDir, relObjectsDir, dataset, dir.filename());
+ std::optional<PackageFileLocation> modelFile = object.getArticulatedModel();
+ if (modelFile.has_value())
{
- try
+ objects.emplace_back(armem::articulated_object::ArticulatedObjectDescription
{
- const auto robot = VirtualRobot::RobotIO::loadRobot(xml, VirtualRobot::RobotIO::RobotDescription::eStructure);
- if (robot != nullptr && robot->isPassive())
- {
- const std::string relativeXMLPath = armarx::ArmarXDataPath::getRelativeArmarXPath(xml);
-
- objects.emplace_back(armem::articulated_object::ArticulatedObjectDescription
- {
- .name = robot->getName(),
- .xml = {packageName, relativeXMLPath}
- // .dataset = dataset
- });
- }
- }
- catch (const armarx::LocalException& ex)
- {
- ARMARX_WARNING << ex.what();
- }
- catch (...)
- {
-
- }
+ .name = object.idStr(),
+ .xml = {modelFile->package, modelFile->relativePath}
+ // .dataset = dataset
+ });
}
}
}
@@ -401,7 +373,7 @@ namespace armarx
ObjectFinder::path ObjectFinder::_rootDirAbs() const
{
- return packageDataDir / packageName / "objects";
+ return absPackageDataDir / packageName / relObjectsDir;
}
ObjectFinder::path ObjectFinder::_rootDirRel() const
@@ -411,7 +383,7 @@ namespace armarx
bool ObjectFinder::_ready() const
{
- return !packageDataDir.empty();
+ return !absPackageDataDir.empty();
}
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
index 18c23dbdb3f727201ed6f66d6558b1f007f8cb41..ea3a7e9cb08f423716931086f309e6dabeedd1b9 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
@@ -7,10 +7,11 @@
#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
+
#include "ObjectInfo.h"
#include "ObjectPose.h"
-#include <RobotAPI/libraries/armem_objects/types.h>
namespace armarx
{
@@ -23,18 +24,23 @@ namespace armarx
class ObjectFinder : Logging
{
public:
+
using path = std::filesystem::path;
inline static const std::string DefaultObjectsPackageName = "PriorKnowledgeData";
+ inline static const std::string DefaultObjectsDirectory = "objects";
+
public:
- ObjectFinder(const std::string& objectsPackageName = DefaultObjectsPackageName);
+ ObjectFinder(const std::string& objectsPackageName = DefaultObjectsPackageName,
+ const path& relObjectsDir = DefaultObjectsDirectory);
ObjectFinder(ObjectFinder&&) = default;
ObjectFinder(const ObjectFinder&) = default;
ObjectFinder& operator=(ObjectFinder&&) = default;
ObjectFinder& operator=(const ObjectFinder&) = default;
+
void setPath(const std::string& path);
std::string getPackageName() const;
@@ -110,7 +116,10 @@ namespace armarx
* @brief Absolute path to data directory (e.g. "/.../repos/ArmarXObjects/data").
* Empty if package could not be found.
*/
- mutable path packageDataDir;
+ mutable path absPackageDataDir;
+
+ /// Path to the directory containing objects in the package's data directory.
+ path relObjectsDir;
};
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
index bbc89162cd8556c88ccde6f872a5da023b6adbd9..47728cbd7249f4983717f9a1dbb079e79215fadc 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
@@ -14,14 +14,17 @@ namespace armarx
ObjectInfo::ObjectInfo(const std::string& packageName, const ObjectInfo::path& packageDataDir,
- const ObjectID& id) :
- _packageName(packageName), _packageDataDir(packageDataDir), _id(id)
+ const path& relObjectsPath, const ObjectID& id) :
+ _packageName(packageName), _absPackageDataDir(packageDataDir),
+ _relObjectsPath(relObjectsPath), _id(id)
{
}
ObjectInfo::ObjectInfo(const std::string& packageName, const ObjectInfo::path& packageDataDir,
- const std::string& dataset, const std::string& name) :
- _packageName(packageName), _packageDataDir(packageDataDir), _id(dataset, name)
+ const path& relObjectsPath,
+ const std::string& dataset, const std::string& className) :
+ _packageName(packageName), _absPackageDataDir(packageDataDir),
+ _relObjectsPath(relObjectsPath), _id(dataset, className)
{
}
@@ -40,7 +43,7 @@ namespace armarx
return _id.dataset();
}
- std::string ObjectInfo::name() const
+ std::string ObjectInfo::className() const
{
return _id.className();
}
@@ -57,7 +60,7 @@ namespace armarx
ObjectInfo::path ObjectInfo::objectDirectory() const
{
- return path(_packageName) / "objects" / _id.dataset() / _id.className();
+ return path(_packageName) / _relObjectsPath / _id.dataset() / _id.className();
}
PackageFileLocation ObjectInfo::file(const std::string& _extension, const std::string& suffix) const
@@ -72,10 +75,11 @@ namespace armarx
PackageFileLocation loc;
loc.package = _packageName;
loc.relativePath = objectDirectory() / filename;
- loc.absolutePath = _packageDataDir / loc.relativePath;
+ loc.absolutePath = _absPackageDataDir / loc.relativePath;
return loc;
}
+
PackageFileLocation ObjectInfo::simoxXML() const
{
return file(".xml");
@@ -86,6 +90,29 @@ namespace armarx
return file(".xml", "_articulated");
}
+ PackageFileLocation ObjectInfo::articulatedUrdf() const
+ {
+ return file(".urdf", "_articulated");
+ }
+
+ std::optional<PackageFileLocation> ObjectInfo::getArticulatedModel() const
+ {
+ if (fs::is_regular_file(articulatedSimoxXML().absolutePath))
+ {
+ return articulatedSimoxXML();
+ }
+ else if (fs::is_regular_file(articulatedUrdf().absolutePath))
+ {
+ return articulatedUrdf();
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+
+
PackageFileLocation ObjectInfo::meshWrl() const
{
return file(".wrl");
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
index 9e91fa2bbfef02c7be4fe6e78f45d33355fd7b22..ebd0fc51b4ddb660613dd0c5f18aa322c2942795 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
@@ -41,10 +41,19 @@ namespace armarx
public:
+ /**
+ * @brief ObjectInfo
+ *
+ * @param packageName The ArmarX package.
+ * @param absPackageDataDir Absolute path to the package's data directory.
+ * @param localObjectsPath The path where objects are stored in the data directory.
+ * @param id The object class ID (with dataset and class name).
+ */
+ ObjectInfo(const std::string& packageName, const path& absPackageDataDir,
+ const path& relObjectsPath, const ObjectID& id);
ObjectInfo(const std::string& packageName, const path& packageDataDir,
- const ObjectID& id);
- ObjectInfo(const std::string& packageName, const path& packageDataDir,
- const std::string& dataset, const std::string& name);
+ const path& relObjectsPath,
+ const std::string& dataset, const std::string& className);
virtual ~ObjectInfo() = default;
@@ -56,15 +65,28 @@ namespace armarx
std::string package() const;
std::string dataset() const;
- std::string name() const;
+ std::string className() const;
+ [[deprecated("This function is deprecated. Use className() instead.")]]
+ std::string name() const
+ {
+ return className();
+ }
+
/// Return "dataset/name".
ObjectID id() const;
std::string idStr() const;
+
PackageFileLocation file(const std::string& extension, const std::string& suffix = "") const;
+
PackageFileLocation simoxXML() const;
+
PackageFileLocation articulatedSimoxXML() const;
+ PackageFileLocation articulatedUrdf() const;
+ /// Return the articulated Simox XML or URDF, if one exists.
+ std::optional<PackageFileLocation> getArticulatedModel() const;
+
PackageFileLocation meshWrl() const;
PackageFileLocation wavefrontObj() const;
@@ -116,7 +138,8 @@ namespace armarx
private:
std::string _packageName;
- path _packageDataDir;
+ path _absPackageDataDir;
+ path _relObjectsPath;
ObjectID _id;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp b/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
index 7a875741d06d205b2994d4a3459b07233287e5d1..85ca31bec07bbe8af4108a64a24805da44dd19d5 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
@@ -106,10 +106,3 @@ BOOST_AUTO_TEST_CASE(test_find)
BOOST_AUTO_TEST_SUITE_END()
-
-
-
-
-
-
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index c375e084dea6b48720d50e59cc9e828e070826ba..762d3f240eb99b6f37b4ecf7d47b63a55e7932f0 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -84,22 +84,21 @@ namespace armarx
Eigen::Vector3f dpos(cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2]);
Eigen::Vector3f kori(cfg->Kori[0], cfg->Kori[1], cfg->Kori[2]);
Eigen::Vector3f dori(cfg->Dori[0], cfg->Dori[1], cfg->Dori[2]);
-
- CtrlParams initParams = {kpos, dpos, kori, dori};
- ctrlParams.reinitAllBuffers(initParams);
+ Eigen::VectorXf knull(targets.size());
+ Eigen::VectorXf dnull(targets.size());
ARMARX_CHECK_EQUAL(cfg->Knull.size(), targets.size());
ARMARX_CHECK_EQUAL(cfg->Dnull.size(), targets.size());
- knull.setZero(targets.size());
- dnull.setZero(targets.size());
-
for (size_t i = 0; i < targets.size(); ++i)
{
knull(i) = cfg->Knull.at(i);
dnull(i) = cfg->Dnull.at(i);
}
+ CtrlParams initParams = {kpos, dpos, kori, dori, knull, dnull};
+ ctrlParams.reinitAllBuffers(initParams);
+
torqueLimit = cfg->torqueLimit;
timeDuration = cfg->timeDuration;
@@ -284,6 +283,8 @@ namespace armarx
Eigen::Vector3f dpos = ctrlParams.getUpToDateReadBuffer().dpos;
Eigen::Vector3f kori = ctrlParams.getUpToDateReadBuffer().kori;
Eigen::Vector3f dori = ctrlParams.getUpToDateReadBuffer().dori;
+ Eigen::Vector3f knull = ctrlParams.getUpToDateReadBuffer().knull;
+ Eigen::Vector3f dnull = ctrlParams.getUpToDateReadBuffer().dnull;
Eigen::Vector6f jointControlWrench;
{
@@ -363,6 +364,7 @@ namespace armarx
debugOutputData.getWriteBuffer().targetPose_qx = targetQuat.x;
debugOutputData.getWriteBuffer().targetPose_qy = targetQuat.y;
debugOutputData.getWriteBuffer().targetPose_qz = targetQuat.z;
+ debugOutputData.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
debugOutputData.getWriteBuffer().currentPose_x = currentPose(0, 3);
debugOutputData.getWriteBuffer().currentPose_y = currentPose(1, 3);
@@ -374,6 +376,26 @@ namespace armarx
debugOutputData.getWriteBuffer().currentPose_qz = currentQuat.z;
debugOutputData.getWriteBuffer().deltaT = deltaT;
+ debugOutputData.getWriteBuffer().currentKpos_x = kpos.x();
+ debugOutputData.getWriteBuffer().currentKpos_y = kpos.y();
+ debugOutputData.getWriteBuffer().currentKpos_z = kpos.z();
+ debugOutputData.getWriteBuffer().currentKori_x = kori.x();
+ debugOutputData.getWriteBuffer().currentKori_y = kori.y();
+ debugOutputData.getWriteBuffer().currentKori_z = kori.z();
+ debugOutputData.getWriteBuffer().currentKnull_x = knull.x();
+ debugOutputData.getWriteBuffer().currentKnull_y = knull.y();
+ debugOutputData.getWriteBuffer().currentKnull_z = knull.z();
+
+ debugOutputData.getWriteBuffer().currentDpos_x = dpos.x();
+ debugOutputData.getWriteBuffer().currentDpos_y = dpos.y();
+ debugOutputData.getWriteBuffer().currentDpos_z = dpos.z();
+ debugOutputData.getWriteBuffer().currentDori_x = dori.x();
+ debugOutputData.getWriteBuffer().currentDori_y = dori.y();
+ debugOutputData.getWriteBuffer().currentDori_z = dori.z();
+ debugOutputData.getWriteBuffer().currentDnull_x = dnull.x();
+ debugOutputData.getWriteBuffer().currentDnull_y = dnull.y();
+ debugOutputData.getWriteBuffer().currentDnull_z = dnull.z();
+
debugOutputData.commitWrite();
}
@@ -547,6 +569,26 @@ namespace armarx
datafields["currentPose_qy"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qy);
datafields["currentPose_qz"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qz);
+ datafields["currentKpos_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_x);
+ datafields["currentKpos_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_y);
+ datafields["currentKpos_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_z);
+ datafields["currentKori_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_x);
+ datafields["currentKori_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_y);
+ datafields["currentKori_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_z);
+ datafields["currentKnull_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_x);
+ datafields["currentKnull_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_y);
+ datafields["currentKnull_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_z);
+
+ datafields["currentDpos_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_x);
+ datafields["currentDpos_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_y);
+ datafields["currentDpos_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_z);
+ datafields["currentDori_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_x);
+ datafields["currentDori_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_y);
+ datafields["currentDori_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_z);
+ datafields["currentDnull_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_x);
+ datafields["currentDnull_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_y);
+ datafields["currentDnull_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_z);
+
datafields["forceDesired_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_x);
datafields["forceDesired_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_y);
datafields["forceDesired_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_z);
@@ -613,42 +655,64 @@ namespace armarx
dmpCtrl->removeAllViaPoints();
}
- void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&)
{
ARMARX_CHECK_EQUAL(kd.size(), 3);
+ ARMARX_INFO << "set linear kd " << VAROUT(kd);
LockGuardType guard(controllerMutex);
- ctrlParams.getWriteBuffer().dpos << kd[0], kd[1], kd[2];
+ ctrlParams.getWriteBuffer().dpos = kd;
ctrlParams.commitWrite();
}
- void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&)
{
ARMARX_CHECK_EQUAL(kp.size(), 3);
+ ARMARX_INFO << "set linear kp " << VAROUT(kp);
LockGuardType guard(controllerMutex);
- ctrlParams.getWriteBuffer().kpos << kp[0], kp[1], kp[2];
+ ctrlParams.getWriteBuffer().kpos = kp;
ctrlParams.commitWrite();
}
- void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&)
{
ARMARX_CHECK_EQUAL(kd.size(), 3);
+ ARMARX_INFO << "set angular kd " << VAROUT(kd);
LockGuardType guard(controllerMutex);
- ctrlParams.getWriteBuffer().dori << kd[0], kd[1], kd[2];
+ ctrlParams.getWriteBuffer().dori = kd;
ctrlParams.commitWrite();
-
}
- void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&)
{
ARMARX_CHECK_EQUAL(kp.size(), 3);
+ ARMARX_INFO << "set angular kp " << VAROUT(kp);
LockGuardType guard(controllerMutex);
- ctrlParams.getWriteBuffer().kori << kp[0], kp[1], kp[2];
+ ctrlParams.getWriteBuffer().kori = kp;
ctrlParams.commitWrite();
}
+ void NJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKd(const Eigen::VectorXf& kd, const Ice::Current&)
+ {
+ ARMARX_CHECK_EQUAL(kd.size(), targets.size());
+ ARMARX_INFO << "set nullspace kd " << VAROUT(kd);
+ LockGuardType guard(controllerMutex);
+ ctrlParams.getWriteBuffer().dnull = kd;
+ ctrlParams.commitWrite();
+ }
+
+ void NJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKp(const Eigen::VectorXf& kp, const Ice::Current&)
+ {
+ ARMARX_CHECK_EQUAL(kp.size(), targets.size());
+ ARMARX_INFO << "set linear kp " << VAROUT(kp);
+ LockGuardType guard(controllerMutex);
+ ctrlParams.getWriteBuffer().knull = kp;
+ ctrlParams.commitWrite();
+ }
+
+
void NJointTaskSpaceImpedanceDMPController::setDefaultNullSpaceJointValues(const Eigen::VectorXf& jointValues,
const Ice::Current&)
{
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
index ea8cef187de6296e07eeb270e4ca5ac2ad7b78f6..9ded363e9e110bf803ada28b5523057b69b3bd4f 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -76,10 +76,12 @@ namespace armarx
void removeAllViaPoints(const Ice::Current&) override;
- void setLinearVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&) override;
- void setLinearVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&) override;
- void setAngularVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&) override;
- void setAngularVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&) override;
+ void setLinearVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&) override;
+ void setLinearVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&) override;
+ void setAngularVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&) override;
+ void setAngularVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&) override;
+ void setNullspaceVelocityKd(const Eigen::VectorXf& kd, const Ice::Current&) override;
+ void setNullspaceVelocityKp(const Eigen::VectorXf& kp, const Ice::Current&) override;
void setUseNullSpaceJointDMP(bool enable, const Ice::Current&) override;
void setDefaultNullSpaceJointValues(const Eigen::VectorXf& jointValues, const Ice::Current&) override;
@@ -112,6 +114,26 @@ namespace armarx
float currentPose_qy;
float currentPose_qz;
+ float currentKpos_x;
+ float currentKpos_y;
+ float currentKpos_z;
+ float currentKori_x;
+ float currentKori_y;
+ float currentKori_z;
+ float currentKnull_x;
+ float currentKnull_y;
+ float currentKnull_z;
+
+ float currentDpos_x;
+ float currentDpos_y;
+ float currentDpos_z;
+ float currentDori_x;
+ float currentDori_y;
+ float currentDori_z;
+ float currentDnull_x;
+ float currentDnull_y;
+ float currentDnull_z;
+
StringFloatDictionary desired_torques;
StringFloatDictionary desired_nullspaceJoint;
float forceDesired_x;
@@ -123,9 +145,11 @@ namespace armarx
float deltaT;
+
+
};
- TripleBuffer<DebugBufferData> debugOutputData;
+ WriteBufferedTripleBuffer<DebugBufferData> debugOutputData;
struct NJointTaskSpaceImpedanceDMPControllerSensorData
{
@@ -134,14 +158,14 @@ namespace armarx
Eigen::Matrix4f currentPose;
Eigen::VectorXf currentTwist;
};
- TripleBuffer<NJointTaskSpaceImpedanceDMPControllerSensorData> controllerSensorData;
+ WriteBufferedTripleBuffer<NJointTaskSpaceImpedanceDMPControllerSensorData> controllerSensorData;
struct NJointTaskSpaceImpedanceDMPControllerInterfaceData
{
Eigen::Matrix4f currentTcpPose;
};
- TripleBuffer<NJointTaskSpaceImpedanceDMPControllerInterfaceData> interfaceData;
+ WriteBufferedTripleBuffer<NJointTaskSpaceImpedanceDMPControllerInterfaceData> interfaceData;
struct CtrlParams
{
@@ -149,9 +173,11 @@ namespace armarx
Eigen::Vector3f dpos;
Eigen::Vector3f kori;
Eigen::Vector3f dori;
+ Eigen::VectorXf knull;
+ Eigen::VectorXf dnull;
};
- TripleBuffer<CtrlParams> ctrlParams;
+ WriteBufferedTripleBuffer<CtrlParams> ctrlParams;
DMP::Vec<DMP::DMPState> currentJointState;
@@ -188,15 +214,15 @@ namespace armarx
// Eigen::Vector3f kori;
// Eigen::Vector3f dpos;
// Eigen::Vector3f dori;
- Eigen::VectorXf knull;
- Eigen::VectorXf dnull;
+ // Eigen::VectorXf knull;
+ // Eigen::VectorXf dnull;
int numOfJoints;
std::atomic_bool useNullSpaceJointDMP;
bool isNullSpaceJointDMPLearned;
- armarx::TripleBuffer<Eigen::VectorXf> defaultNullSpaceJointValues;
+ WriteBufferedTripleBuffer<Eigen::VectorXf> defaultNullSpaceJointValues;
std::vector<std::string> jointNames;
mutable MutexType controllerMutex;
PeriodicTask<NJointTaskSpaceImpedanceDMPController>::pointer_type controllerTask;
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h
index 4c3be719fce4148970fa0ef16d8465d2df7a10d3..dcce63977438bdd0d646b50aa8e5dd9679354df8 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h
+++ b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h
@@ -41,9 +41,9 @@ namespace armarx::armem::server::obj::clazz
{
bool show = true;
- std::string entityName = "Environment/floor-20x20";
+ std::string entityName = "Building/floor-20x20";
std::string layerName = "Floor";
- bool height = 1;
+ float height = -1;
void define(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
};
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index f85d7146065dc8f9ac648b889ab1db27ab5c30a9..646c004013fa28dc961f6a6c404157d492da9e14 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -25,6 +25,7 @@
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/json.h>
#include <Eigen/Geometry>