diff --git a/source/RobotAPI/components/ArViz/CMakeLists.txt b/source/RobotAPI/components/ArViz/CMakeLists.txt
index 23bc568751839f9758cd09597b2b4f58b8b5e419..9a5a3adcbfedd3a756b8231b61db7591ad66047b 100644
--- a/source/RobotAPI/components/ArViz/CMakeLists.txt
+++ b/source/RobotAPI/components/ArViz/CMakeLists.txt
@@ -15,12 +15,16 @@ set(SOURCES
Client/elements/Mesh.cpp
Client/elements/Robot.cpp
Client/elements/RobotHand.cpp
+ Client/elements/Line.cpp
+ Client/elements/Path.cpp
+
Client/drawer/ArVizDrawerBase.cpp
Client/ScopedClient.cpp
Coin/ElementVisualizer.cpp
Coin/VisualizationRobot.cpp
+ Coin/VisualizationPath.cpp
Coin/VisualizationObject.cpp
Coin/Visualizer.cpp
@@ -49,6 +53,7 @@ set(HEADERS
Coin/VisualizationEllipsoid.h
Coin/VisualizationSphere.h
Coin/VisualizationPose.h
+ Coin/VisualizationPath.h
Coin/VisualizationLine.h
Coin/VisualizationText.h
Coin/VisualizationArrow.h
@@ -77,6 +82,8 @@ set(HEADERS
Client/elements/PointCloud.h
Client/elements/Robot.h
Client/elements/RobotHand.h
+ Client/elements/Line.h
+ Client/elements/Path.h
Client/drawer/ArVizDrawerBase.h
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Line.cpp b/source/RobotAPI/components/ArViz/Client/elements/Line.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..985162f1726a66c88c4a310e6b287641294fa9f9
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/elements/Line.cpp
@@ -0,0 +1,20 @@
+#include "Line.h"
+
+#include "ArmarXCore/interface/core/BasicVectorTypesHelpers.h"
+
+namespace armarx::viz
+{
+ Line& Line::lineWidth(float w)
+ {
+ data_->lineWidth = w;
+
+ return *this;
+ }
+ Line& Line::fromTo(Eigen::Vector3f from, Eigen::Vector3f to)
+ {
+ data_->from = ToBasicVectorType(from);
+ data_->to = ToBasicVectorType(to);
+
+ return *this;
+ }
+} // namespace armarx::viz
\ No newline at end of file
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Line.h b/source/RobotAPI/components/ArViz/Client/elements/Line.h
new file mode 100644
index 0000000000000000000000000000000000000000..6f6d9427e05b9d91606e8ecdfa8a2164f4847515
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/elements/Line.h
@@ -0,0 +1,38 @@
+/*
+ * 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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "RobotAPI/components/ArViz/Client/elements/ElementOps.h"
+#include <RobotAPI/interface/ArViz/Elements.h>
+
+namespace armarx::viz
+{
+ class Line : public ElementOps<Line, data::ElementLine>
+ {
+ public:
+ using ElementOps::ElementOps;
+
+ Line& lineWidth(float w);
+
+ Line& fromTo(Eigen::Vector3f from, Eigen::Vector3f to);
+ };
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Path.cpp b/source/RobotAPI/components/ArViz/Client/elements/Path.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0a88d089382eebb915104c24ee41339a18dd2f29
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/elements/Path.cpp
@@ -0,0 +1,51 @@
+#include "Path.h"
+
+namespace armarx::viz
+{
+
+ Path& Path::clear()
+ {
+ data_->points.clear();
+ return *this;
+ }
+
+ Path& Path::lineColor(Color color)
+ {
+ data_->lineColor = color;
+
+ return *this;
+ }
+
+ Path& Path::lineColorGlasbeyLUT(std::size_t id, int alpha)
+ {
+ return lineColor(Color::fromRGBA(simox::color::GlasbeyLUT::at(id, alpha)));
+ }
+
+ Path& Path::lineWidth(float w)
+ {
+ data_->lineWidth = w;
+
+ return *this;
+ }
+
+ Path& Path::points(std::vector<Eigen::Vector3f> const& ps)
+ {
+ auto& points = data_->points;
+ points.clear();
+ points.reserve(ps.size());
+ for (auto& p : ps)
+ {
+ points.push_back(armarx::Vector3f{p.x(), p.y(), p.z()});
+ }
+
+ return *this;
+ }
+
+ Path& Path::addPoint(Eigen::Vector3f p)
+ {
+ data_->points.push_back(armarx::Vector3f{p.x(), p.y(), p.z()});
+
+ return *this;
+ }
+
+} // namespace armarx::viz
\ No newline at end of file
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Path.h b/source/RobotAPI/components/ArViz/Client/elements/Path.h
new file mode 100644
index 0000000000000000000000000000000000000000..50eec6abe454bbf56bc45053c4d35f5e41ca5bc6
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/elements/Path.h
@@ -0,0 +1,59 @@
+
+
+/*
+ * 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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "RobotAPI/components/ArViz/Client/elements/ElementOps.h"
+#include <RobotAPI/interface/ArViz/Elements.h>
+
+namespace armarx::viz
+{
+ class Path : public ElementOps<Path, data::ElementPath>
+ {
+ public:
+ using ElementOps::ElementOps;
+
+ Path& clear();
+
+ Path& lineColor(Color color);
+
+ template<class...Ts>
+ Path& lineColor(Ts&& ...ts)
+ {
+ return lineColor({std::forward<Ts>(ts)...});
+ }
+
+ Path& lineColorGlasbeyLUT(std::size_t id, int alpha = 255);
+
+ Path& lineWidth(float w);
+
+ Path& points(std::vector<Eigen::Vector3f> const& ps);
+
+ Path& addPoint(Eigen::Vector3f p);
+ };
+} // namespace armarx::viz
+
+
+
+
+
diff --git a/source/RobotAPI/components/ArViz/Coin/RegisterVisualizationTypes.cpp b/source/RobotAPI/components/ArViz/Coin/RegisterVisualizationTypes.cpp
index 33febe24be91fd428f06762cbcfc6d7e48f101a0..5ca14001aff6c7be187ad4c7736e86fd3ab6b3c6 100644
--- a/source/RobotAPI/components/ArViz/Coin/RegisterVisualizationTypes.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/RegisterVisualizationTypes.cpp
@@ -15,13 +15,14 @@
#include "VisualizationMesh.h"
#include "VisualizationRobot.h"
#include "VisualizationObject.h"
+#include "VisualizationPath.h"
void armarx::viz::CoinVisualizer::registerVisualizationTypes()
{
using namespace armarx::viz::coin;
- elementVisualizers.reserve(15);
+ elementVisualizers.reserve(16);
registerVisualizerFor<VisualizationBox>();
registerVisualizerFor<VisualizationCylinder>();
@@ -38,4 +39,5 @@ void armarx::viz::CoinVisualizer::registerVisualizationTypes()
registerVisualizerFor<VisualizationMesh>();
registerVisualizerFor<VisualizationRobot>();
registerVisualizerFor<VisualizationObject>();
+ registerVisualizerFor<VisualizationPath>();
}
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3018a3af51e25222b97c0c60ed06fea65a8c5822
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp
@@ -0,0 +1,63 @@
+#include "VisualizationPath.h"
+
+#include <Inventor/SbVec3f.h>
+#include <Inventor/nodes/SoCoordinate3.h>
+#include <Inventor/nodes/SoDrawStyle.h>
+#include <Inventor/nodes/SoLineSet.h>
+
+namespace armarx::viz::coin
+{
+
+ VisualizationPath::VisualizationPath()
+ {
+ coordinate3 = new SoCoordinate3;
+
+ // create line around polygon
+ SoSeparator* lineSep = new SoSeparator;
+
+ lineMaterial = new SoMaterial;
+ lineSep->addChild(lineMaterial);
+ lineSep->addChild(coordinate3);
+
+ lineStyle = new SoDrawStyle();
+ lineSep->addChild(lineStyle);
+
+ lineSet = new SoLineSet;
+ lineSep->addChild(lineSet);
+
+ node->addChild(coordinate3);
+ node->addChild(lineSep);
+ }
+
+ bool VisualizationPath::update(ElementType const& element)
+ {
+ // set position
+ coordinate3->point.setValuesPointer(element.points.size(), reinterpret_cast<const float*>(element.points.data()));
+
+ // set color
+ const auto lineColor = element.lineColor;
+
+ constexpr float toUnit = 1.0F / 255.0F;
+
+ const auto color = SbVec3f(lineColor.r, lineColor.g, lineColor.b) * toUnit;
+ const float transparency = 1.0F - static_cast<float>(lineColor.a) * toUnit;
+
+ lineMaterial->diffuseColor.setValue(color);
+ lineMaterial->ambientColor.setValue(color);
+ lineMaterial->transparency.setValue(transparency);
+
+ if (element.lineWidth > 0.0F)
+ {
+ lineStyle->lineWidth.setValue(element.lineWidth);
+ }
+ else
+ {
+ lineStyle->style = SoDrawStyleElement::INVISIBLE;
+ }
+
+ const int pointSize = static_cast<int>(element.points.size());
+ lineSet->numVertices.set1Value(0, pointSize);
+
+ return true;
+ }
+} // namespace armarx::viz::coin
\ No newline at end of file
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationPath.h b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.h
new file mode 100644
index 0000000000000000000000000000000000000000..e1e446bbe66bc49a6d0c4bdf1999c7f0615f06ec
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.h
@@ -0,0 +1,47 @@
+/*
+ * 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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "ElementVisualizer.h"
+
+#include <RobotAPI/interface/ArViz/Elements.h>
+
+class SoCoordinate3;
+class SoDrawStyle;
+class SoLineSet;
+
+namespace armarx::viz::coin
+{
+ struct VisualizationPath : TypedElementVisualization<SoSeparator>
+ {
+ using ElementType = data::ElementPath;
+
+ VisualizationPath();
+
+ bool update(ElementType const& element);
+
+ SoCoordinate3* coordinate3;
+ SoDrawStyle* lineStyle;
+ SoLineSet* lineSet;
+ SoMaterial* lineMaterial;
+ };
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/interface/ArViz/Elements.ice b/source/RobotAPI/interface/ArViz/Elements.ice
index 0adf0413b30d5985851b86dab12580d0a314acbe..1c00dd4e630208a79852ea3d18d96da8a501f7d0 100644
--- a/source/RobotAPI/interface/ArViz/Elements.ice
+++ b/source/RobotAPI/interface/ArViz/Elements.ice
@@ -100,6 +100,14 @@ module data
float lineWidth = 0.0f;
};
+ class ElementPath extends Element
+ {
+ Vector3fSeq points;
+
+ Color lineColor;
+ float lineWidth = 10.0f;
+ };
+
class ElementArrow extends Element
{
float length = 100.0f;
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 93b018801eb5e7f6b870e06b914798d511dc5780..d39e94c87ce0ea5d5ed5b21e252d31f1e2ca41f9 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -325,6 +325,7 @@ module armarx
{
void learnDMPFromFiles(Ice::StringSeq trajfiles);
void learnJointDMPFromFiles(string jointTrajFile, Ice::FloatSeq currentJVS);
+ void setUseNullSpaceJointDMP(bool enable);
bool isFinished();
void runDMP(Ice::DoubleSeq goals);
@@ -332,6 +333,7 @@ module armarx
void setViaPoints(double canVal, Ice::DoubleSeq point);
void setGoals(Ice::DoubleSeq goals);
+ void setDefaultNullSpaceJointValues(Eigen::VectorXf jointValues);
double getVirtualTime();
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index 65da4d862d7696bccaca2b68e5576777e722d54d..7e9acde0bdc18f3bd937f5972ce0081727aabc98 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -4,9 +4,12 @@
namespace armarx
{
- NJointControllerRegistration<NJointTaskSpaceImpedanceDMPController> registrationControllerNJointTaskSpaceImpedanceDMPController("NJointTaskSpaceImpedanceDMPController");
+ NJointControllerRegistration<NJointTaskSpaceImpedanceDMPController> registrationControllerNJointTaskSpaceImpedanceDMPController(
+ "NJointTaskSpaceImpedanceDMPController");
- NJointTaskSpaceImpedanceDMPController::NJointTaskSpaceImpedanceDMPController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ NJointTaskSpaceImpedanceDMPController::NJointTaskSpaceImpedanceDMPController(const RobotUnitPtr& robotUnit,
+ const armarx::NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&)
{
ARMARX_INFO << "creating impedance dmp controller";
cfg = NJointTaskSpaceImpedanceDMPControllerConfigPtr::dynamicCast(config);
@@ -37,8 +40,9 @@ namespace armarx
positionSensors.push_back(positionSensor);
};
- tcp = rns->getTCP();
- ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ tcp = rns->getTCP();
+ ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(),
+ VirtualRobot::JacobiProvider::eSVDDamped));
numOfJoints = targets.size();
// set DMP
TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
@@ -65,14 +69,15 @@ namespace armarx
isNullSpaceJointDMPLearned = false;
- defaultNullSpaceJointValues.resize(targets.size());
+ Eigen::VectorXf nullspaceValues(targets.size());
+
ARMARX_CHECK_EQUAL(cfg->defaultNullSpaceJointValues.size(), targets.size());
for (size_t i = 0; i < targets.size(); ++i)
{
- defaultNullSpaceJointValues(i) = cfg->defaultNullSpaceJointValues.at(i);
+ nullspaceValues(i) = cfg->defaultNullSpaceJointValues.at(i);
}
-
+ defaultNullSpaceJointValues.reinitAllBuffers(nullspaceValues);
Eigen::Vector3f kpos(cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2]);
Eigen::Vector3f dpos(cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2]);
@@ -126,7 +131,7 @@ namespace armarx
initData.targetPose = tcp->getPoseInRootFrame();
initData.targetVel.resize(6);
initData.targetVel.setZero();
- initData.desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ initData.desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
reinitTripleBuffer(initData);
@@ -154,8 +159,8 @@ namespace armarx
if (!started)
{
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ LockGuardType guard{controlDataMutex};
+ getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
getWriterControlStruct().targetVel.setZero(6);
getWriterControlStruct().targetPose = currentPose;
getWriterControlStruct().canVal = 1.0;
@@ -167,8 +172,8 @@ namespace armarx
if (stopped)
{
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ LockGuardType guard{controlDataMutex};
+ getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
getWriterControlStruct().targetVel.setZero(6);
getWriterControlStruct().targetPose = oldPose;
getWriterControlStruct().canVal = dmpCtrl->canVal;
@@ -180,7 +185,7 @@ namespace armarx
if (dmpCtrl->canVal < 1e-8)
{
finished = true;
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().targetVel.setZero();
writeControlStruct();
return;
@@ -192,7 +197,10 @@ namespace armarx
if (useNullSpaceJointDMP && isNullSpaceJointDMPLearned)
{
DMP::DVec targetJointState;
- currentJointState = nullSpaceJointDMPPtr->calculateDirectlyVelocity(currentJointState, dmpCtrl->canVal / timeDuration, deltaT / timeDuration, targetJointState);
+ currentJointState = nullSpaceJointDMPPtr->calculateDirectlyVelocity(currentJointState,
+ dmpCtrl->canVal / timeDuration,
+ deltaT / timeDuration,
+ targetJointState);
if (targetJointState.size() == jointNames.size())
{
@@ -203,15 +211,15 @@ namespace armarx
}
else
{
- desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
}
}
else
{
- desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
}
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().desiredNullSpaceJointValues = desiredNullSpaceJointValues;
getWriterControlStruct().targetVel = dmpCtrl->getTargetVelocity();
getWriterControlStruct().targetPose = dmpCtrl->getTargetPoseMat();
@@ -223,7 +231,8 @@ namespace armarx
}
}
- void NJointTaskSpaceImpedanceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void NJointTaskSpaceImpedanceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
double deltaT = timeSinceLastIteration.toSecondsDouble();
@@ -261,7 +270,7 @@ namespace armarx
firstRun = false;
targetPose = currentPose;
targetVel.setZero(6);
- desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
}
else
{
@@ -280,25 +289,28 @@ namespace armarx
Eigen::Vector3f targetTCPLinearVelocity;
targetTCPLinearVelocity << 0.001 * targetVel(0), 0.001 * targetVel(1), 0.001 * targetVel(2);
Eigen::Vector3f currentTCPLinearVelocity;
- currentTCPLinearVelocity << 0.001 * currentTwist(0), 0.001 * currentTwist(1), 0.001 * currentTwist(2);
+ currentTCPLinearVelocity << 0.001 * currentTwist(0), 0.001 * currentTwist(1), 0.001 * currentTwist(2);
Eigen::Vector3f currentTCPPosition = currentPose.block<3, 1>(0, 3);
Eigen::Vector3f desiredPosition = targetPose.block<3, 1>(0, 3);
- Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) + dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
+ Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) +
+ dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
Eigen::Vector3f currentTCPAngularVelocity;
- currentTCPAngularVelocity << currentTwist(3), currentTwist(4), currentTwist(5);
+ currentTCPAngularVelocity << currentTwist(3), currentTwist(4), currentTwist(5);
Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
- jointControlWrench << tcpDesiredForce, tcpDesiredTorque;
+ jointControlWrench << tcpDesiredForce, tcpDesiredTorque;
}
Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
- Eigen::VectorXf nullspaceTorque = knull.cwiseProduct(desiredNullSpaceJointValues - qpos) - dnull.cwiseProduct(qvel);
+ Eigen::VectorXf nullspaceTorque =
+ knull.cwiseProduct(desiredNullSpaceJointValues - qpos) - dnull.cwiseProduct(qvel);
Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
- Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
+ Eigen::VectorXf jointDesiredTorques =
+ jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
@@ -316,7 +328,7 @@ namespace armarx
desiredTorque = 0;
}
- desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
+ desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
debugOutputData.getWriteBuffer().desired_torques[jointNames[i]] = jointDesiredTorques(i);
@@ -325,7 +337,8 @@ namespace armarx
targets.at(i)->torque = desiredTorque;
if (!targets.at(i)->isValid())
{
- ARMARX_INFO << deactivateSpam(1) << "Torque controller target is invalid - setting to zero! set value: " << targets.at(i)->torque;
+ ARMARX_INFO << deactivateSpam(1) << "Torque controller target is invalid - setting to zero! set value: "
+ << targets.at(i)->torque;
targets.at(i)->torque = 0.0f;
}
}
@@ -371,7 +384,8 @@ namespace armarx
ARMARX_INFO << "Learned DMP ... ";
}
- void NJointTaskSpaceImpedanceDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint,
+ const Ice::Current&)
{
LockGuardType guard(controllerMutex);
ARMARX_INFO << "setting via points ";
@@ -385,9 +399,11 @@ namespace armarx
}
- void NJointTaskSpaceImpedanceDMPController::learnJointDMPFromFiles(const std::string& fileName, const Ice::FloatSeq& currentJVS, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::learnJointDMPFromFiles(const std::string& fileName,
+ const Ice::FloatSeq& currentJVS,
+ const Ice::Current&)
{
- DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
+ DMP::Vec<DMP::SampledTrajectoryV2> trajs;
DMP::DVec ratios;
DMP::SampledTrajectoryV2 traj;
traj.readFromCSVFile(fileName);
@@ -439,8 +455,14 @@ namespace armarx
stopped = false;
}
+ void NJointTaskSpaceImpedanceDMPController::setUseNullSpaceJointDMP(bool enable, const Ice::Current&)
+ {
+ useNullSpaceJointDMP = enable;
+ }
+
- void NJointTaskSpaceImpedanceDMPController::runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration,
+ const Ice::Current&)
{
firstRun = true;
while (firstRun)
@@ -490,8 +512,8 @@ namespace armarx
}
-
- void NJointTaskSpaceImpedanceDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
+ void NJointTaskSpaceImpedanceDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&,
+ const DebugObserverInterfacePrx& debugObs)
{
StringVariantBaseMap datafields;
auto values = debugOutputData.getUpToDateReadBuffer().desired_torques;
@@ -585,7 +607,7 @@ namespace armarx
void NJointTaskSpaceImpedanceDMPController::removeAllViaPoints(const Ice::Current&)
{
- LockGuardType guard {controllerMutex};
+ LockGuardType guard{controllerMutex};
ARMARX_INFO << "setting via points ";
dmpCtrl->removeAllViaPoints();
}
@@ -626,5 +648,14 @@ namespace armarx
}
+ void NJointTaskSpaceImpedanceDMPController::setDefaultNullSpaceJointValues(const Eigen::VectorXf& jointValues,
+ const Ice::Current&)
+ {
+ ARMARX_CHECK_EQUAL(jointValues.size(), targets.size());
+ defaultNullSpaceJointValues.getWriteBuffer() = jointValues;
+ defaultNullSpaceJointValues.commitWrite();
+
+ }
+
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
index 1b49c99634870147df440d88709f828c0e074872..ea8cef187de6296e07eeb270e4ca5ac2ad7b78f6 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -42,51 +42,53 @@ namespace armarx
NJointTaskSpaceImpedanceDMPController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
// NJointControllerInterface interface
- std::string getClassName(const Ice::Current&) const;
+ std::string getClassName(const Ice::Current&) const override;
// NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
// NJointTaskSpaceImpedanceDMPControllerInterface interface
- void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
- bool isFinished(const Ice::Current&)
+ void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) override;
+ bool isFinished(const Ice::Current&) override
{
return finished;
}
- void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
- void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
+ void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
+ void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;
- void learnJointDMPFromFiles(const std::string& fileName, const Ice::FloatSeq& currentJVS, const Ice::Current&);
- void runDMP(const Ice::DoubleSeq& goals, const Ice::Current&);
- void runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&);
+ void learnJointDMPFromFiles(const std::string& fileName, const Ice::FloatSeq& currentJVS, const Ice::Current&) override;
+ void runDMP(const Ice::DoubleSeq& goals, const Ice::Current&) override;
+ void runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&) override;
- Ice::Double getVirtualTime(const Ice::Current&)
+ Ice::Double getVirtualTime(const Ice::Current&) override
{
return dmpCtrl->canVal;
}
- void stopDMP(const Ice::Current&);
- void resumeDMP(const Ice::Current&);
- void resetDMP(const Ice::Current&);
+ void stopDMP(const Ice::Current&) override;
+ void resumeDMP(const Ice::Current&) override;
+ void resetDMP(const Ice::Current&) override;
- void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&);
- DoubleSeqSeq getMPWeights(const Ice::Current&);
+ void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&) override;
+ DoubleSeqSeq getMPWeights(const Ice::Current&) override;
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 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 setUseNullSpaceJointDMP(bool enable, const Ice::Current&) override;
+ void setDefaultNullSpaceJointValues(const Eigen::VectorXf& jointValues, const Ice::Current&) override;
protected:
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
+ virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
- void onInitNJointController();
- void onDisconnectNJointController();
+ void onInitNJointController() override;
+ void onDisconnectNJointController() override;
void controllerRun();
private:
@@ -190,11 +192,11 @@ namespace armarx
Eigen::VectorXf dnull;
int numOfJoints;
- bool useNullSpaceJointDMP;
+ std::atomic_bool useNullSpaceJointDMP;
bool isNullSpaceJointDMPLearned;
- Eigen::VectorXf defaultNullSpaceJointValues;
+ armarx::TripleBuffer<Eigen::VectorXf> defaultNullSpaceJointValues;
std::vector<std::string> jointNames;
mutable MutexType controllerMutex;
PeriodicTask<NJointTaskSpaceImpedanceDMPController>::pointer_type controllerTask;
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
index c8db83835ccc53f21e416b723745892a1455f801..5a3a4a0d4abd221dd9ce8385fbce98a747f36c09 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -284,14 +284,17 @@ namespace armarx::armem::common::robot_state::localization
std::vector<::armarx::armem::robot_state::Transform> transforms;
transforms.reserve(entity.history().size());
- const auto entitySnapshots = simox::alg::get_values(entity.history());
+ // const auto entitySnapshots = simox::alg::get_values(entity.history());
+
+ const std::vector<wm::EntitySnapshot> entitySnapshots = {entity.getLatestSnapshot()};
+
std::transform(
entitySnapshots.begin(),
entitySnapshots.end(),
std::back_inserter(transforms),
- [](const auto & entity)
+ [](const auto & entitySnapshot)
{
- return convertEntityToTransform(entity.getInstance(0));
+ return convertEntityToTransform(entitySnapshot.getInstance(0));
});
ARMARX_DEBUG << "obtaining transform";
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index f140f9512bcb990a82e50ed3eb271536817d4d88..9527835a6dd48107f5e11846ec882d30b056c54a 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -57,6 +57,11 @@ void CartesianVelocityController::calculateJacobis(VirtualRobot::IKSolver::Carte
_jacobiWithCosts(r, c) = jacobi(r, c) / _jointCosts(c);
}
}
+ auto svd = Eigen::JacobiSVD(_jacobiWithCosts, Eigen::ComputeThinU | Eigen::ComputeThinV);
+ auto sv = svd.singularValues();
+ double minSigVal = sv(sv.rows() - 1, sv.cols() - 1);
+ double damping = minSigVal < 1e-2 ? 1e-2 : 1e-8;
+ ik->setDampedSvdLambda(damping);
_inv = ik->computePseudoInverseJacobianMatrix(_jacobiWithCosts, ik->getJacobiRegularization(mode));
}
@@ -267,6 +272,11 @@ bool CartesianVelocityController::clampJacobiAtJointLimits(VirtualRobot::IKSolve
}
if (modifiedJacobi)
{
+ auto svd = Eigen::JacobiSVD(jacobi, Eigen::ComputeThinU | Eigen::ComputeThinV);
+ auto sv = svd.singularValues();
+ double minSigVal = sv(sv.rows() - 1, sv.cols() - 1);
+ double damping = minSigVal < 1e-2 ? 1e-2 : 1e-8;
+ ik->setDampedSvdLambda(damping);
inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
}