diff --git a/.gitignore b/.gitignore
index 1422f5933373b19b58e3759a11b1c1df41bb86c9..a0100e3665815b9ee90f2ce6b905e92ae436f47f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -68,3 +68,5 @@ data/RobotAPI/logs
# ArViz Recordings
data/RobotAPI/ArVizStorage/
+
+worktree/*
diff --git a/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h b/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
index cfba70de44efdf1ad9d873d33659eee2323413c7..101c2f0336b401fe96159c5099f90f221f604a7c 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
@@ -78,6 +78,22 @@ namespace armarx::viz
return this->position(position).orientation(orientation);
}
+ Eigen::Matrix4f pose() const
+ {
+ auto& p = data_->pose;
+ Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
+ m(0, 3) = p.x;
+ m(1, 3) = p.y;
+ m(2, 3) = p.z;
+ m.topLeftCorner<3, 3>() = Eigen::Quaternionf{p.qw, p.qx, p.qy, p.qz}.toRotationMatrix();
+ return m;
+ }
+
+ DerivedT& transformPose(Eigen::Matrix4f const& p)
+ {
+ return pose(p * pose());
+ }
+
DerivedT& color(Color color)
{
data_->color = color;
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
index 0e2ae813fbef99c1ce24e35c8be0be63deeeff8b..9b7d83c193f5665572320fd966034c1077d05721 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
@@ -137,9 +137,9 @@ namespace armarx::viz::coin
if (loadedDrawStyle & data::ModelDrawStyle::OVERRIDE_COLOR)
{
if (loadedColor.r != element.color.r
- || loadedColor.g != element.color.g
- || loadedColor.b != element.color.b
- || loadedColor.a != element.color.a)
+ || loadedColor.g != element.color.g
+ || loadedColor.b != element.color.b
+ || loadedColor.a != element.color.a)
{
int numChildren = node->getNumChildren();
for (int i = 0; i < numChildren; i++)
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
index a1c4fb5941d1108f8d5626f7995d690b6733f8e1..763bab62f2773dbb26e45efbc3e38ae7b4839561 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
@@ -182,6 +182,7 @@ namespace armarx::RobotUnitModule
const auto endInner = TimeUtil::GetTime(true);
timingMap["NJointControllerUpdates_" + nJointCtrl->getInstanceName()] = new TimedVariant {TimestampVariant{endInner - begInner}, lastControlThreadTimestamp};
}
+
robotUnitListenerBatchPrx->nJointControllerStatusChanged(allStatus);
debugObserverBatchPrx->ice_flushBatchRequests();
debugDrawerBatchPrx->ice_flushBatchRequests();
@@ -270,11 +271,15 @@ namespace armarx::RobotUnitModule
status.timestampUSec = lastControlThreadTimestamp.toMicroSeconds();
allStatus.emplace_back(status);
}
+
robotUnitListenerBatchPrx->controlDeviceStatusChanged(allStatus);
- if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
+ if (observerPublishControlTargets)
{
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->controlDevicesChannel, ctrlDevMap);
- robotUnitObserver->updateChannel(robotUnitObserver->controlDevicesChannel);
+ if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
+ {
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->controlDevicesChannel, ctrlDevMap);
+ robotUnitObserver->updateChannel(robotUnitObserver->controlDevicesChannel);
+ }
}
const auto end = TimeUtil::GetTime(true);
@@ -315,23 +320,27 @@ namespace armarx::RobotUnitModule
allStatus.emplace_back(status);
}
}
+
robotUnitListenerBatchPrx->sensorDeviceStatusChanged(allStatus);
- if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
+ if (observerPublishSensorValues)
{
- std::stringstream s;
- for (auto& pair : sensorDevMap)
- {
- s << pair.first << ": \t" << (pair.second ? pair.second->ice_id() + "\t with datatype \t" + Variant::typeToString(pair.second->getType()) : "NULL") << "\n";
- }
- ARMARX_DEBUG << deactivateSpam(spamdelay) << "Sensor Datafieldnames: " << ARMARX_STREAM_PRINTER
+ if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
{
+ std::stringstream s;
for (auto& pair : sensorDevMap)
{
- out << pair.first << ": " << (pair.second ? pair.second->ice_id() + Variant::typeToString(pair.second->getType()) : "NULL") << "\n";
+ s << pair.first << ": \t" << (pair.second ? pair.second->ice_id() + "\t with datatype \t" + Variant::typeToString(pair.second->getType()) : "NULL") << "\n";
}
- };
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->sensorDevicesChannel, sensorDevMap);
- robotUnitObserver->updateChannel(robotUnitObserver->sensorDevicesChannel);
+ ARMARX_DEBUG << deactivateSpam(spamdelay) << "Sensor Datafieldnames: " << ARMARX_STREAM_PRINTER
+ {
+ for (auto& pair : sensorDevMap)
+ {
+ out << pair.first << ": " << (pair.second ? pair.second->ice_id() + Variant::typeToString(pair.second->getType()) : "NULL") << "\n";
+ }
+ };
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->sensorDevicesChannel, sensorDevMap);
+ robotUnitObserver->updateChannel(robotUnitObserver->sensorDevicesChannel);
+ }
}
const auto end = TimeUtil::GetTime(true);
@@ -400,6 +409,8 @@ namespace armarx::RobotUnitModule
observerPublishSensorValues = getProperty<bool>("ObserverPublishSensorValues").getValue();
observerPublishControlTargets = getProperty<bool>("ObserverPublishControlTargets").getValue();
+ observerPublishTimingInformation = getProperty<bool>("ObserverPublishTimingInformation").getValue();
+ observerPublishAdditionalInformation = getProperty<bool>("ObserverPublishAdditionalInformation").getValue();
debugObserverSkipIterations = std::max(1ul, getProperty<std::uint64_t>("ObserverPrintEveryNthIterations").getValue());
publishPeriodMs = std::max(1ul, getProperty<std::size_t>("PublishPeriodMs").getValue());
@@ -428,6 +439,14 @@ namespace armarx::RobotUnitModule
{
observerPublishControlTargets = getProperty<bool>("ObserverPublishControlTargets").getValue();
}
+ if (changedProperties.count("ObserverPublishTimingInformation"))
+ {
+ observerPublishTimingInformation = getProperty<bool>("ObserverPublishTimingInformation").getValue();
+ }
+ if (changedProperties.count("ObserverPublishAdditionalInformation"))
+ {
+ observerPublishAdditionalInformation = getProperty<bool>("ObserverPublishAdditionalInformation").getValue();
+ }
if (changedProperties.count("ObserverPrintEveryNthIterations"))
{
debugObserverSkipIterations = getProperty<std::uint64_t>("ObserverPrintEveryNthIterations").getValue();
@@ -550,7 +569,10 @@ namespace armarx::RobotUnitModule
timingMap["ClassNameUpdates"] = publishNJointClassNames();
timingMap["RobotUnitListenerFlush"] = new TimedVariant
{
- ARMARX_STOPWATCH(TimestampVariant){robotUnitListenerBatchPrx->ice_flushBatchRequests();}, lastControlThreadTimestamp
+ ARMARX_STOPWATCH(TimestampVariant)
+ {
+ robotUnitListenerBatchPrx->ice_flushBatchRequests();
+ }, lastControlThreadTimestamp
};
@@ -559,10 +581,16 @@ namespace armarx::RobotUnitModule
timingMap["LastPublishLoop"] = new TimedVariant {TimestampVariant{lastPublishLoop}, lastControlThreadTimestamp};
if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
{
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->additionalChannel, additionalMap);
- robotUnitObserver->updateChannel(robotUnitObserver->additionalChannel);
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->timingChannel, timingMap);
- robotUnitObserver->updateChannel(robotUnitObserver->timingChannel);
+ if (observerPublishTimingInformation)
+ {
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->additionalChannel, additionalMap);
+ robotUnitObserver->updateChannel(robotUnitObserver->additionalChannel);
+ }
+ if (observerPublishAdditionalInformation)
+ {
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->timingChannel, timingMap);
+ robotUnitObserver->updateChannel(robotUnitObserver->timingChannel);
+ }
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
index 15b67d9fae37f0cd7b117759fb157136e76dafec..5844b7ba9efb2bbb4bdcd0f0370ad3def34cddd5 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
@@ -65,6 +65,12 @@ namespace armarx::RobotUnitModule
defineOptionalProperty<bool>(
"ObserverPublishControlTargets", true,
"Whether control targets are send to the observer", PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>(
+ "ObserverPublishTimingInformation", true,
+ "Whether timing information are send to the observer", PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>(
+ "ObserverPublishAdditionalInformation", true,
+ "Whether additional information are send to the observer", PropertyDefinitionBase::eModifiable);
defineOptionalProperty<std::string>(
"DebugObserverTopicName", "DebugObserver", "The topic where updates are send to");
defineOptionalProperty<std::uint64_t>(
@@ -258,6 +264,11 @@ namespace armarx::RobotUnitModule
std::atomic_bool observerPublishSensorValues;
/// @brief Whether \ref ControlTargetBase "ControlTargets" should be published to the observers
std::atomic_bool observerPublishControlTargets;
+ /// @brief Whether \ref Timing information should be published to the observers
+ std::atomic_bool observerPublishTimingInformation;
+ /// @brief Whether \ref Additional information should be published to the observers
+ std::atomic_bool observerPublishAdditionalInformation;
+
/// @brief How many iterations of \ref publish shold not publish data to the debug observer.
std::atomic<std::uint64_t> debugObserverSkipIterations;
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
index b0b65db9dad4ee9cd94742d27b542cb03719492a..9f48b80609903888caf23163cdefeb1c801ddf74 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
@@ -99,6 +99,9 @@ namespace armarx
void reportJointCurrents(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
void reportJointMotorTemperatures(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
void reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&) override {}
+
+ void setOffset(const ::Eigen::Matrix4f&, const ::Ice::Current& = ::Ice::emptyCurrent) override {}
+
// ManagedIceObject interface
protected:
void onInitComponent() override;
diff --git a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.h b/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.h
index 21fa9f38b5ffe50d1b2f4a560e90a2ef337669f8..1618ce6e6d047993ef1360d6ae7203cb79013296 100644
--- a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.h
+++ b/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.h
@@ -22,8 +22,8 @@
#pragma once
#include <RobotAPI/libraries/NJointControllerGuiPluginUtility/NJointControllerGuiPluginBase.h>
-#include <RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToVector.h>
-#include <RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToPose.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/SpinBoxToVector.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/SpinBoxToPose.h>
#include <RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.h>
#include <RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
diff --git a/source/RobotAPI/interface/components/TrajectoryPlayerInterface.ice b/source/RobotAPI/interface/components/TrajectoryPlayerInterface.ice
index 3e0f1eded9f05946c493584146891e8bc01632d0..7807f84419960a8219f5a809feccf66e08d6f76a 100644
--- a/source/RobotAPI/interface/components/TrajectoryPlayerInterface.ice
+++ b/source/RobotAPI/interface/components/TrajectoryPlayerInterface.ice
@@ -24,6 +24,8 @@
#include <RobotAPI/interface/core/Trajectory.ice>
#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.ice>
+#include <ArmarXCore/interface/serialization/Eigen.ice>
+#include <RobotAPI/interface/aron.ice>
module armarx
{
@@ -43,6 +45,8 @@ module armarx
bool setJointsInUse(string jointName, bool inUse);
void enableRobotPoseUnit(bool isRobotPose);
+ void setOffset(Eigen::Matrix4f offset);
+
double getCurrentTime();
double getEndTime();
double getTrajEndTime();
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 53cc507f90f49980a9a160b8bee0245750eca243..be015a4054ca736e124406dae0052241cd7c4aa4 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -643,6 +643,7 @@ module armarx
int frictionHorizon = 100;
// pid params
+ bool isForceCtrlInForceDir;
bool isForceControlEnabled;
bool isRotControlEnabled;
bool isTorqueControlEnabled;
diff --git a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/CMakeLists.txt b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/CMakeLists.txt
index b044f98be56f2ee7fe799cf3272aac3120d30ccb..94e91cdf9d477c838b7b18f47b56cbcfbb4a5be3 100644
--- a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/CMakeLists.txt
+++ b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/CMakeLists.txt
@@ -11,9 +11,8 @@ set(SOURCES
detail/NJointControllerGuiPluginBase.cpp
)
set(HEADERS
- SpinBoxToPose.h
- SpinBoxToVector.h
NJointControllerGuiPluginBase.h
+ detail/NJointControllerGuiPluginBase.h
)
set(GUI_MOC_HDRS detail/NJointControllerGuiPluginBase.h)
set(GUI_UIS)
diff --git a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToPose.h b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToPose.h
deleted file mode 100644
index c22b4b823b9788e44e2660c8d1f93d6677b1cd9a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToPose.h
+++ /dev/null
@@ -1,115 +0,0 @@
-/*
- * 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::NJointControllerGuiPluginUtility
- * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date 2020
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <SimoxUtility/math/convert.h>
-
-#include "SpinBoxToVector.h"
-
-namespace armarx
-{
- template<class Qwid = QDoubleSpinBox>
- class SpinBoxToPose
- {
- //set
- public:
- void setPos(const Eigen::Vector3f& pos)
- {
- _xyz.set(pos);
- }
- void setOri(const Eigen::Vector3f& rpy)
- {
- _rpy.set(rpy);
- }
- void setOri(const Eigen::Quaternionf& q)
- {
- setOri(simox::math::quat_to_rpy(q));
- }
- void setOri(const Eigen::Matrix3f& m)
- {
- setOri(simox::math::mat3f_to_rpy(m));
- }
- void setPose(const auto& pos, const auto& ori)
- {
- setPos(pos);
- setOri(ori);
- }
- void setPose(const Eigen::Matrix4f& m)
- {
- setPos(simox::math::mat4f_to_pos(m));
- setOri(simox::math::mat4f_to_rpy(m));
- }
- //get
- public:
- Eigen::Vector3f getPos() const
- {
- return _xyz.template get<Eigen::Vector3f>();
- }
- Eigen::Vector3f getRPY() const
- {
- return _rpy.template get<Eigen::Vector3f>();
- }
- Eigen::Quaternionf getQuat() const
- {
- return simox::math::rpy_to_quat(getRPY());
- }
- Eigen::Matrix3f getMat3() const
- {
- return simox::math::rpy_to_mat3f(getRPY());
- }
- Eigen::Matrix4f getMat4() const
- {
- return simox::math::pos_rpy_to_mat4f(getPos(), getRPY());
- }
- //set up
- public:
- void setXYZWidgets(Qwid* x, Qwid* y, Qwid* z)
- {
- _xyz.addWidget(x);
- _xyz.addWidget(y);
- _xyz.addWidget(z);
- }
- void setRPYWidgets(Qwid* r, Qwid* p, Qwid* y)
- {
- _rpy.addWidget(r);
- _rpy.addWidget(p);
- _rpy.addWidget(y);
- }
- SpinBoxToVector<Qwid, 3>& xyzWidgets()
- {
- return _xyz;
- }
- SpinBoxToVector<Qwid, 3>& rpyWidgets()
- {
- return _rpy;
- }
- void setDefaultLimits()
- {
- _xyz.setMinMax(-100'000, 100'000);
- _rpy.setMinMax(-9, 9);
- }
- private:
- SpinBoxToVector<Qwid, 3> _xyz;
- SpinBoxToVector<Qwid, 3> _rpy;
- };
-}
diff --git a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToVector.h b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToVector.h
deleted file mode 100644
index 3fe18f27c0fb2e32df2f44492cc6b71d1d162369..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToVector.h
+++ /dev/null
@@ -1,202 +0,0 @@
-/*
- * 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::NJointControllerGuiPluginUtility
- * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date 2020
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <array>
-#include <vector>
-
-#include <Eigen/Dense>
-
-#include <QDoubleSpinBox>
-
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/Nodes/RobotNode.h>
-
-#include <ArmarXCore/util/CPPUtility/trace.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-namespace armarx
-{
- template<class Qwid = QDoubleSpinBox, std::size_t Size = 0>
- class SpinBoxToVector
- {
- public:
- template<class Scalar, int Rows, int Cols>
- void get(Eigen::Matrix<Scalar, Rows, Cols>& m) const
- {
- ARMARX_TRACE;
- if constexpr(Size)
- {
- ARMARX_CHECK_EQUAL(Size, _widgets.size());
- }
- if constexpr(Rows == -1 && Cols == -1)
- {
- m.resize(_widgets.size(), 1);
- }
- else if constexpr(Rows == 1 && Cols == -1)
- {
- m.resize(_widgets.size());
- }
- else if constexpr(Rows == -1 && Cols == 1)
- {
- m.resize(_widgets.size());
- }
- else if constexpr(Cols == -1)
- {
- m.resize(Rows, _widgets.size() / Rows);
- }
- else if constexpr(Rows == -1)
- {
- m.resize(_widgets.size() / Cols, Cols);
- }
-
- ARMARX_TRACE;
- ARMARX_CHECK_EQUAL(static_cast<std::size_t>(m.size()), _widgets.size());
- for (std::size_t i = 0; i < _widgets.size(); ++i)
- {
- m.data()[i] = _widgets.at(i)->value();
- }
- }
- template<class Scalar>
- void get(std::vector<Scalar>& m) const
- {
- ARMARX_TRACE;
- if constexpr(Size)
- {
- ARMARX_CHECK_EQUAL(Size, _widgets.size());
- }
- m.resize(_widgets.size());
- ARMARX_CHECK_EQUAL(m.size(), _widgets.size());
- for (std::size_t i = 0; i < _widgets.size(); ++i)
- {
- m.at(i) = _widgets.at(i)->value();
- }
- }
- template<class T>
- T get() const
- {
- ARMARX_TRACE;
- if constexpr(Size)
- {
- ARMARX_CHECK_EQUAL(Size, _widgets.size());
- }
- T m;
- get(m);
- return m;
- }
- template<class Scalar, int Rows, int Cols>
- void set(const Eigen::Matrix<Scalar, Rows, Cols>& m)
- {
- ARMARX_TRACE;
- if constexpr(Size)
- {
- ARMARX_CHECK_EQUAL(Size, _widgets.size());
- }
- ARMARX_CHECK_EQUAL(m.size(), _widgets.size());
- for (int i = 0; i < _widgets.size(); ++i)
- {
- _widgets.at(i)->setValue(m.data()[i]);
- }
- }
- void set(const VirtualRobot::RobotNodeSetPtr& set)
- {
- ARMARX_TRACE;
- ARMARX_CHECK_NOT_NULL(set);
- ARMARX_CHECK_EQUAL(set->getSize(), _widgets.size());
- for (std::size_t i = 0; i < _widgets.size(); ++i)
- {
- ARMARX_CHECK_NOT_NULL(set->getNode(i));
- _widgets.at(i)->setValue(set->getNode(i)->getJointValue());
- }
- }
- void set(double d)
- {
- for (auto w : _widgets)
- {
- w->setValue(d);
- }
- }
- public:
- void addWidget(Qwid* s)
- {
- ARMARX_TRACE;
- ARMARX_CHECK_NOT_NULL(s);
- if constexpr(Size)
- {
- ARMARX_CHECK_LESS(_sz, Size);
- _widgets.at(_sz) = s;
- }
- else
- {
- _widgets.emplace_back(s);
- }
- ++_sz;
- }
- void clear()
- {
- ARMARX_TRACE;
- _sz = 0;
- if constexpr(!Size)
- {
- _widgets.clear();
- }
- }
- void visitWidgets(const auto& f)
- {
- for (auto w : _widgets)
- {
- f(w);
- }
- }
- void setMin(auto min)
- {
- for (auto w : _widgets)
- {
- w->setMinimum(min);
- }
- }
- void setMax(auto max)
- {
- for (auto w : _widgets)
- {
- w->setMaximum(max);
- }
- }
- void setMinMax(auto min, auto max)
- {
- setMin(min);
- setMax(max);
- }
- auto& widgets()
- {
- return _widgets;
- }
- private:
- unsigned _sz = 0;
- std::conditional_t <
- Size,
- std::array<Qwid*, Size>,
- std::vector<Qwid*>
- > _widgets;
- };
-}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
index f20e5cf17eebc0c4b8bf607571270b4d952f292e..e711da48631a7562fbc748ff2ef5d0d26d6fd84c 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
@@ -20,6 +20,7 @@
* GNU General Public License
*/
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
#include "RobotStateComponentPlugin.h"
@@ -272,6 +273,17 @@ namespace armarx::plugins
{
return _robotStateComponentName;
}
+
+ Eigen::Matrix4f RobotStateComponentPlugin::transformFromTo(
+ const std::string& from,
+ const std::string& to,
+ const VirtualRobot::RobotPtr& rob)
+ {
+ ARMARX_CHECK_NOT_NULL(rob);
+ armarx::FramedPose fp{Eigen::Matrix4f::Identity(), from, rob->getName()};
+ fp.changeFrame(rob, to);
+ return fp.toEigen();
+ }
}
namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
index ddbc64ab666aff4008c77434a86cdb0b62c5d1fe..80e96096b61881ff96df7b50d894cdf50f33d758 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
@@ -88,9 +88,14 @@ namespace armarx::plugins
RobotData getRobotData(const std::string& id) const;
void setRobotRNSAndNode(const std::string& id, const std::string& rnsName, const std::string& nodeName);
+ //querry
+ public:
const RobotStateComponentInterfacePrx& getRobotStateComponent() const;
-
const RobotNameHelper& getRobotNameHelper() const;
+ Eigen::Matrix4f transformFromTo(const std::string& from,
+ const std::string& to,
+ const VirtualRobot::RobotPtr& rob);
+
//sync
public:
bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.h b/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.h
index b29450d2c4afa1699169f06e7e0dde2156a55c38..acf84cb4ede9f5e56d8f278596d0b11c146af5a9 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.h
@@ -5,8 +5,8 @@
#include <VirtualRobot/RobotNodeSet.h>
-#include <RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToVector.h>
-#include <RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToPose.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/SpinBoxToVector.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/SpinBoxToPose.h>
#include <RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.h>
#include <RobotAPI/libraries/RobotAPINJointControllerWidgets/ui_CartesianImpedanceControllerConfigWidget.h>
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
index 6e6a64dd8cdee8bc0d773b7222dc1dca9af9a5f3..74e8c8dceb83a53134ff155e772fcad08b3bec1f 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
@@ -9,8 +9,7 @@ namespace armarx
NJointBimanualObjLevelController::NJointBimanualObjLevelController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
{
- ARMARX_INFO << "Preparing ... bimanual ";
- ARMARX_INFO << "I am here";
+ ARMARX_INFO << "Initializing Bimanual Object Level Controller";
useSynchronizedRtRobot();
cfg = NJointBimanualObjLevelControllerConfigPtr::dynamicCast(config);
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
index 2e3ec38e5e4cf64955a6ca6107b32e779aa1418e..657034a6bab019136ecc684ee2dd4ebf65264290 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
@@ -82,6 +82,7 @@ namespace armarx
kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
+ isForceCtrlInForceDir = cfg->isForceCtrlInForceDir;
isForceControlEnabled = cfg->isForceControlEnabled;
isRotControlEnabled = cfg->isRotControlEnabled;
isTorqueControlEnabled = cfg->isTorqueControlEnabled;
@@ -530,7 +531,14 @@ namespace armarx
/* -------------------------- Force Regulation and Torque PID Controller --------------------------------- */
- forcePID->update(deltaT, forceInToolFrame(2), targetForce);
+ if (isForceCtrlInForceDir)
+ {
+ forcePID->update(deltaT, forceInToolFrame.norm(), targetForce);
+ }
+ else
+ {
+ forcePID->update(deltaT, forceInToolFrame(2), targetForce);
+ }
torquePID->update(deltaT, torqueInToolFrame(1), 0.0);
/* -------------------------- Rotation PID Controller --------------------------------- */
@@ -665,9 +673,18 @@ namespace armarx
// }
if (isForceControlEnabled)
{
- // targetFTInToolFrame(2) -= (float)forcePID->getControlValue() * forceControlGate;
- targetVel(2) -= (float)forcePID->getControlValue();
- targetVel.head(3) = currentToolOri * targetVel.head(3);
+ if (isForceCtrlInForceDir)
+ {
+ float forcePIDVel = -(float)forcePID->getControlValue();
+ Eigen::Vector3f targetVelInTool = forceInToolFrame / forceInToolFrame.norm() * forcePIDVel;
+ targetVel.head(3) = currentToolOri * targetVelInTool;
+ }
+ else
+ {
+ // targetFTInToolFrame(2) -= (float)forcePID->getControlValue() * forceControlGate;
+ targetVel(2) -= (float)forcePID->getControlValue();
+ targetVel.head(3) = currentToolOri * targetVel.head(3);
+ }
}
if (isRotControlEnabled)
{
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
index 495d9ba47d28fd257f6a91c459a71adbd7c4a779..9d48c48a110c40c74d178c5438f030139708b58f 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
@@ -227,6 +227,7 @@ namespace armarx
const SensorValueForceTorque* forceSensor;
// pid controllers
+ bool isForceCtrlInForceDir;
bool isForceControlEnabled;
bool isRotControlEnabled;
bool isTorqueControlEnabled;
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
index a9ecec5e4f8965566355d6ef98f60bea206ee42e..6053d01967a38aeabd89f42645e2d2bbc3449a17 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
@@ -25,6 +25,8 @@
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/statechart/xmlstates/profiles/StatechartProfiles.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
@@ -32,7 +34,6 @@
namespace armarx
{
-
void RobotNameHelper::writeRobotInfoNode(
const RobotInfoNodePtr& n,
std::ostream& str,
@@ -225,6 +226,14 @@ namespace armarx
return select("HandModelPath");
}
+ std::string RobotNameHelper::Arm::getAbsoluteHandModelPath() const
+ {
+ ArmarXDataPath::FindPackageAndAddDataPath(getHandModelPackage());
+ auto path = getHandModelPath();
+ return ArmarXDataPath::getAbsolutePath(path, path) ?
+ path : "";
+ }
+
std::string RobotNameHelper::Arm::getHandModelPackage() const
{
ARMARX_TRACE;
@@ -289,7 +298,12 @@ namespace armarx
return tcp->getPoseInRootFrame().inverse() * hand->getPoseInRootFrame();
}
- RobotNameHelper::Arm RobotNameHelper::RobotArm::getArm() const
+ const VirtualRobot::RobotPtr& RobotNameHelper::RobotArm::getRobot() const
+ {
+ return robot;
+ }
+
+ const RobotNameHelper::Arm& RobotNameHelper::RobotArm::getArm() const
{
return arm;
}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
index 703c0ae83c286d4c3273d19e1dadd11044e1a71b..0a4fdd59a0fa75277ee69c13cd4ed1d619ecfd3d 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
@@ -74,6 +74,7 @@ namespace armarx
std::string getHandControllerName() const;
std::string getHandRootNode() const;
std::string getHandModelPath() const;
+ std::string getAbsoluteHandModelPath() const;
std::string getHandModelPackage() const;
RobotArm addRobot(const VirtualRobot::RobotPtr& robot) const;
@@ -104,7 +105,8 @@ namespace armarx
VirtualRobot::RobotNodePtr getTCP() const;
VirtualRobot::RobotNodePtr getHandRootNode() const;
Eigen::Matrix4f getTcp2HandRootTransform() const;
- Arm getArm() const;
+ const Arm& getArm() const;
+ const VirtualRobot::RobotPtr& getRobot() const;
RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot);
RobotArm() = default;
@@ -114,7 +116,7 @@ namespace armarx
RobotArm& operator=(const RobotArm&) = default;
private:
- const Arm arm;
+ Arm arm;
VirtualRobot::RobotPtr robot;
};
@@ -123,6 +125,10 @@ namespace armarx
static RobotNameHelperPtr Create(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile);
RobotNameHelper(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile = nullptr);
+ RobotNameHelper(RobotNameHelper&&) = default;
+ RobotNameHelper(const RobotNameHelper&) = default;
+ RobotNameHelper& operator=(RobotNameHelper&&) = default;
+ RobotNameHelper& operator=(const RobotNameHelper&) = default;
Arm getArm(const std::string& side) const;
RobotArm getRobotArm(const std::string& side, const VirtualRobot::RobotPtr& robot) const;