diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index bf445568148fca1e0a6ca7fc214c088467a1edd1..c90d353054348fc5de64387f5f1ee238d382b05c 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -13,6 +13,7 @@
<Variant baseType="::armarx::LinkedDirectionBase" dataType="::armarx::LinkedDirection" humanName="LinkedDirection" include="RobotAPI/libraries/core/LinkedPose.h" />
<Variant baseType="::armarx::OrientedPointBase" dataType="::armarx::OrientedPoint" humanName="OrientedPoint" include="RobotAPI/libraries/core/OrientedPoint.h" />
<Variant baseType="::armarx::FramedOrientedPointBase" dataType="::armarx::FramedOrientedPoint" humanName="FramedOrientedPoint" include="RobotAPI/libraries/core/FramedOrientedPoint.h" />
+ <Variant baseType="::armarx::CartesianPositionControllerConfigBase" dataType="::armarx::CartesianPositionControllerConfig" humanName="CartesianPositionControllerConfig" include="RobotAPI/libraries/core/CartesianPositionController.h" />
<Variant baseType="::armarx::TrajectoryBase" dataType="::armarx::Trajectory" humanName="Trajectory" include="RobotAPI/libraries/core/Trajectory.h"/>
</Lib>
<Lib name="RobotAPIInterfaces">
@@ -189,7 +190,11 @@
<onConnect>remoteRobot.reset(new RemoteRobot(robotStateComponent->getSynchronizedRobot()));</onConnect>
<stateMethod header="const VirtualRobot::RobotPtr getRobot() const">return %getContext%->getRobot();</stateMethod>
- <method header="const VirtualRobot::RobotPtr getLocalRobot() const">return robotPoolStructure->getRobot();</method>
+ <method header="const VirtualRobot::RobotPtr getLocalRobot(bool performInitialRobotSync = true) const">VirtualRobot::RobotPtr robot = robotPoolStructure->getRobot();
+ if(performInitialRobotSync) {
+ ::armarx::RemoteRobot::synchronizeLocalClone(robot, getRobotStateComponent());
+ }
+ return robot;</method>
<method header="const VirtualRobot::RobotPtr getLocalCollisionRobot() const">return robotPoolCollision->getRobot();</method>
<method header="const RobotNameHelperPtr getRobotNameHelper() const">return robotNameHelper;</method>
<member>VirtualRobot::RobotPtr localRobot;</member>
@@ -203,7 +208,7 @@
<onConnect>localCollisionRobot = RemoteRobot::createLocalCloneFromFile(robotStateComponent, VirtualRobot::RobotIO::eCollisionModel);</onConnect>
<onConnect>robotPoolCollision.reset(new RobotPool(localCollisionRobot,2));</onConnect>
<onConnect>robotNameHelper = RobotNameHelper::Create(robotStateComponent->getRobotInfo(), getSelectedStatechartProfile());</onConnect>
- <stateMethod header="const VirtualRobot::RobotPtr getLocalRobot() const">return %getContext%->getLocalRobot();</stateMethod>
+ <stateMethod header="const VirtualRobot::RobotPtr getLocalRobot(bool performInitialRobotSync = true) const">return %getContext%->getLocalRobot(performInitialRobotSync);</stateMethod>
<stateMethod header="const VirtualRobot::RobotPtr getLocalCollisionRobot() const">return %getContext%->getLocalCollisionRobot();</stateMethod>
<stateMethod header="const RobotNameHelperPtr getRobotNameHelper() const">return %getContext%->getRobotNameHelper();</stateMethod>
@@ -228,12 +233,23 @@
</Topic>
<Topic include="RobotAPI/interface/speech/SpeechInterface.h"
- humanName="Text to Speech"
+ humanName="Text to Speech Topic"
typeName="TextListenerInterfacePrx"
memberName="textToSpeech"
getterName="getTextToSpeech"
propertyName="TextToSpeechTopicName"
propertyIsOptional="true"
propertyDefaultValue="TextToSpeech" />
+
+
+ <Proxy include="RobotAPI/interface/observers/GraspCandidateObserverInterface.h"
+ humanName="Grasp Candidate Observer"
+ typeName="grasping::GraspCandidateObserverInterfacePrx"
+ memberName="graspCandidateObserver"
+ getterName="getGraspCandidateObserver"
+ propertyName="GraspCandidateObserverName"
+ propertyIsOptional="true"
+ propertyDefaultValue="GraspCandidateObserver" />
+
</Lib>
</VariantInfo>
diff --git a/source/RobotAPI/applications/CMakeLists.txt b/source/RobotAPI/applications/CMakeLists.txt
index 33562999950ec8e211693c9eac7867b4d01657b9..fcb12a119335518906e559c6e46ad070f13e9d5c 100644
--- a/source/RobotAPI/applications/CMakeLists.txt
+++ b/source/RobotAPI/applications/CMakeLists.txt
@@ -40,8 +40,10 @@ add_subdirectory(RobotNameService)
add_subdirectory(SpeechObserver)
add_subdirectory(DummyTextToSpeech)
add_subdirectory(KITProstheticHandUnit)
-add_subdirectory(ProsthesisObserver)
add_subdirectory(CyberGloveObserver)
add_subdirectory(RobotHealth)
add_subdirectory(RobotHealthDummy)
+add_subdirectory(GraspCandidateObserver)
+
+add_subdirectory(FrameTracking)
\ No newline at end of file
diff --git a/source/RobotAPI/applications/FrameTracking/CMakeLists.txt b/source/RobotAPI/applications/FrameTracking/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..69311ee8d5fb8e412d615b156eb93e9a1de31dd2
--- /dev/null
+++ b/source/RobotAPI/applications/FrameTracking/CMakeLists.txt
@@ -0,0 +1,11 @@
+armarx_component_set_name("FrameTrackingApp")
+set(COMPONENT_LIBS FrameTracking)
+armarx_add_component_executable(main.cpp)
+
+#find_package(MyLib QUIET)
+#armarx_build_if(MyLib_FOUND "MyLib not available")
+# all target_include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+#if(MyLib_FOUND)
+# target_include_directories(FrameTracking PUBLIC ${MyLib_INCLUDE_DIRS})
+#endif()
diff --git a/source/RobotAPI/applications/ProsthesisObserver/main.cpp b/source/RobotAPI/applications/FrameTracking/main.cpp
similarity index 73%
rename from source/RobotAPI/applications/ProsthesisObserver/main.cpp
rename to source/RobotAPI/applications/FrameTracking/main.cpp
index 1d3dfb4ca27ec76c18cfdbd9ac376e5a8b1109e3..d8a7c016d8a9ea8ba94638e1feaec9f55eb16e6f 100644
--- a/source/RobotAPI/applications/ProsthesisObserver/main.cpp
+++ b/source/RobotAPI/applications/FrameTracking/main.cpp
@@ -13,14 +13,14 @@
* 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::application::ProsthesisObserver
- * @author JuliaStarke ( julia dot starke at kit dot edu )
- * @date 2018
+ * @package RobotAPI::application::FrameTracking
+ * @author Adrian Knobloch ( adrian dot knobloch at student dot kit dot edu )
+ * @date 2019
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-#include <RobotAPI/components/ProsthesisObserver/ProsthesisObserver.h>
+#include <RobotAPI/components/FrameTracking/FrameTracking.h>
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
@@ -28,5 +28,5 @@
int main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp < armarx::ProsthesisObserver > (argc, argv, "ProsthesisObserver");
+ return armarx::runSimpleComponentApp < armarx::FrameTracking > (argc, argv, "FrameTracking");
}
diff --git a/source/RobotAPI/applications/GraspCandidateObserver/CMakeLists.txt b/source/RobotAPI/applications/GraspCandidateObserver/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9e4ac83c0d1ccfbd7251fc44ebdc25a4d7c13a62
--- /dev/null
+++ b/source/RobotAPI/applications/GraspCandidateObserver/CMakeLists.txt
@@ -0,0 +1,25 @@
+armarx_component_set_name("GraspCandidateObserverApp")
+
+
+find_package(Eigen3 QUIET)
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+
+if (Eigen3_FOUND)
+ include_directories(SYSTEM ${Eigen3_INCLUDE_DIR})
+endif()
+
+find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+if(Simox_FOUND)
+ include_directories(${Simox_INCLUDE_DIRS})
+endif()
+
+set(COMPONENT_LIBS
+ RobotAPIUnits
+ ArmarXCoreInterfaces
+ ArmarXCore
+)
+
+set(EXE_SOURCE main.cpp)
+
+armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/GraspCandidateObserver/main.cpp b/source/RobotAPI/applications/GraspCandidateObserver/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9de7e0e219db9e8d4a7fb289c9d09902fc8fabe6
--- /dev/null
+++ b/source/RobotAPI/applications/GraspCandidateObserver/main.cpp
@@ -0,0 +1,32 @@
+/*
+ * 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::application::GraspCandidateObserver
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <RobotAPI/components/units/GraspCandidateObserver.h>
+
+#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+ return armarx::runSimpleComponentApp < armarx::GraspCandidateObserver > (argc, argv, "GraspCandidateObserver");
+}
diff --git a/source/RobotAPI/applications/ProsthesisObserver/CMakeLists.txt b/source/RobotAPI/applications/ProsthesisObserver/CMakeLists.txt
deleted file mode 100644
index ea075e315bd3aab53d3abe4b5e951228a46bd71b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/applications/ProsthesisObserver/CMakeLists.txt
+++ /dev/null
@@ -1,20 +0,0 @@
-armarx_component_set_name("ProsthesisObserverApp")
-
-#find_package(MyLib QUIET)
-#armarx_build_if(MyLib_FOUND "MyLib not available")
-#
-# all include_directories must be guarded by if(Xyz_FOUND)
-# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
-#if(MyLib_FOUND)
-# include_directories(${MyLib_INCLUDE_DIRS})
-#endif()
-
-set(COMPONENT_LIBS
- ArmarXCoreInterfaces
- ArmarXCore
- ProsthesisObserver
-)
-
-set(EXE_SOURCE main.cpp)
-
-armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index a49e87af6ab414b169ba4a29a83a373443be1266..a158a9de601e92895908de099ef5c1f6534571d9 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -7,7 +7,8 @@ add_subdirectory(GamepadControlUnit)
add_subdirectory(RobotNameService)
add_subdirectory(DummyTextToSpeech)
add_subdirectory(KITProstheticHandUnit)
-add_subdirectory(ProsthesisObserver)
add_subdirectory(CyberGloveObserver)
add_subdirectory(RobotHealth)
+
+add_subdirectory(FrameTracking)
\ No newline at end of file
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index 4784feb5614234a69becf0be6c2f87d2ae8e76a2..eacd69f2afe16f464dc4486665e7c4e6674a7147 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -98,7 +98,8 @@ namespace armarx
DebugDrawerComponent::DebugDrawerComponent():
mutex(new RecursiveMutex()),
dataUpdateMutex(new RecursiveMutex()),
- topicMutex(new RecursiveMutex())
+ topicMutex(new RecursiveMutex()),
+ defaultLayerVisibility(true)
{
cycleTimeMS = 1000.0f / 30.0f;
timerSensor = NULL;
@@ -2327,6 +2328,11 @@ namespace armarx
removePointCloudVisu(layerName, i.first);
}
+ for (const auto& i : layer.addedColoredPointCloudVisualizations)
+ {
+ removeColoredPointCloudVisu(layerName, i.first);
+ }
+
for (const auto& i : layer.addedPolygonVisualizations)
{
removePolygonVisu(layerName, i.first);
@@ -2410,10 +2416,13 @@ namespace armarx
SoSeparator* mainNode = new SoSeparator {};
mainNode->ref();
- layerMainNode->addChild(mainNode);
layers[layerName] = Layer();
layers.at(layerName).mainNode = mainNode;
- layers.at(layerName).visible = true;
+ layers.at(layerName).visible = getDefaultLayerVisibility(layerName);
+ if (layers.at(layerName).visible)
+ {
+ layerMainNode->addChild(mainNode);
+ }
return layers.at(layerName);
}
@@ -2722,7 +2731,7 @@ namespace armarx
pclSep->addChild(new SoPointSet);
- ARMARX_INFO << d.name << "PointCloud Update " << pcl.size();
+ ARMARX_DEBUG << d.name << "PointCloud Update " << pcl.size();
layer.addedColoredPointCloudVisualizations[d.name] = pclSep;
layer.mainNode->addChild(pclSep);
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
index 6ed54f0450c85f234a23895e91771df5167d136e..2e27aed9ea0292e0161d8429301ccebb81eb6908 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -143,81 +143,81 @@ namespace armarx
}
/* Inherited from DebugDrawerInterface. */
- void exportScene(const std::string& filename, const ::Ice::Current& = ::Ice::Current()) override;
- void exportLayer(const std::string& filename, const std::string& layerName, const ::Ice::Current& = ::Ice::Current()) override;
-
- void setPoseVisu(const std::string& layerName, const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Current& = ::Ice::Current()) override;
- void setScaledPoseVisu(const std::string& layerName, const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current& = ::Ice::Current()) override;
- void setPoseDebugLayerVisu(const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Current& = ::Ice::Current()) override;
- void setScaledPoseDebugLayerVisu(const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current& = ::Ice::Current()) override;
- void removePoseVisu(const std::string& layerName, const std::string& poseName, const ::Ice::Current& = ::Ice::Current()) override;
- void removePoseDebugLayerVisu(const std::string& poseName, const ::Ice::Current& = ::Ice::Current()) override;
-
- void setLineVisu(const std::string& layerName, const std::string& lineName, const ::armarx::Vector3BasePtr& globalPosition1, const ::armarx::Vector3BasePtr& globalPosition2, float lineWidth, const ::armarx::DrawColor& color, const ::Ice::Current& = ::Ice::Current()) override;
- void setLineDebugLayerVisu(const std::string& lineName, const ::armarx::Vector3BasePtr& globalPosition1, const ::armarx::Vector3BasePtr& globalPosition2, float lineWidth, const ::armarx::DrawColor& color, const ::Ice::Current& = ::Ice::Current()) override;
- void removeLineVisu(const std::string& layerName, const std::string& lineName, const ::Ice::Current& = ::Ice::Current()) override;
- void removeLineDebugLayerVisu(const std::string& lineName, const ::Ice::Current& = ::Ice::Current()) override;
-
- void setLineSetVisu(const std::string& layerName, const std::string& lineSetName, const DebugDrawerLineSet& lineSet, const ::Ice::Current& = ::Ice::Current()) override;
- void setLineSetDebugLayerVisu(const std::string& lineSetName, const DebugDrawerLineSet& lineSet, const ::Ice::Current& = ::Ice::Current()) override;
- void removeLineSetVisu(const std::string& layerName, const std::string& lineSetName, const ::Ice::Current& = ::Ice::Current()) override;
- void removeLineSetDebugLayerVisu(const std::string& lineSetName, const ::Ice::Current& = ::Ice::Current()) override;
-
- void setBoxVisu(const std::string& layerName, const std::string& boxName, const ::armarx::PoseBasePtr& globalPose, const ::armarx::Vector3BasePtr& dimensions, const DrawColor& color, const ::Ice::Current& = ::Ice::Current()) override;
- void setBoxDebugLayerVisu(const std::string& boxName, const ::armarx::PoseBasePtr& globalPose, const ::armarx::Vector3BasePtr& dimensions, const DrawColor& color, const ::Ice::Current& = ::Ice::Current()) override;
- void removeBoxVisu(const std::string& layerName, const std::string& boxName, const ::Ice::Current& = ::Ice::Current()) override;
- void removeBoxDebugLayerVisu(const std::string& boxName, const ::Ice::Current& = ::Ice::Current()) override;
-
- void setTextVisu(const std::string& layerName, const std::string& textName, const std::string& text, const ::armarx::Vector3BasePtr& globalPosition, const DrawColor& color, int size, const ::Ice::Current& = ::Ice::Current()) override;
- void setTextDebugLayerVisu(const std::string& textName, const std::string& text, const ::armarx::Vector3BasePtr& globalPosition, const DrawColor& color, int size, const ::Ice::Current& = ::Ice::Current()) override;
- void removeTextVisu(const std::string& layerName, const std::string& textName, const ::Ice::Current& = ::Ice::Current()) override;
- void removeTextDebugLayerVisu(const std::string& textName, const ::Ice::Current& = ::Ice::Current()) override;
-
- void setSphereVisu(const std::string& layerName, const std::string& sphereName, const ::armarx::Vector3BasePtr& globalPosition, const DrawColor& color, float radius, const ::Ice::Current& = ::Ice::Current()) override;
- void setSphereDebugLayerVisu(const std::string& sphereName, const ::armarx::Vector3BasePtr& globalPosition, const DrawColor& color, float radius, const ::Ice::Current& = ::Ice::Current()) override;
- void removeSphereVisu(const std::string& layerName, const std::string& sphereName, const ::Ice::Current& = ::Ice::Current()) override;
- void removeSphereDebugLayerVisu(const std::string& sphereName, const ::Ice::Current& = ::Ice::Current()) override;
-
- void setCylinderVisu(const std::string& layerName, const std::string& cylinderName, const ::armarx::Vector3BasePtr& globalPosition, const ::armarx::Vector3BasePtr& direction, float length, float radius, const DrawColor& color, const ::Ice::Current& = ::Ice::Current()) override;
- void setCylinderDebugLayerVisu(const std::string& cylinderName, const ::armarx::Vector3BasePtr& globalPosition, const ::armarx::Vector3BasePtr& direction, float length, float radius, const DrawColor& color, const ::Ice::Current& = ::Ice::Current()) override;
- void removeCylinderVisu(const std::string& layerName, const std::string& cylinderName, const ::Ice::Current& = ::Ice::Current()) override;
- void removeCylinderDebugLayerVisu(const std::string& cylinderName, const ::Ice::Current& = ::Ice::Current()) override;
-
- void setPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawerPointCloud& pointCloud, const ::Ice::Current& = ::Ice::Current()) override;
- void setPointCloudDebugLayerVisu(const std::string& pointCloudName, const DebugDrawerPointCloud& pointCloud, const ::Ice::Current& = ::Ice::Current()) override;
- void removePointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const ::Ice::Current& = ::Ice::Current()) override;
- void removePointCloudDebugLayerVisu(const std::string& pointCloudName, const ::Ice::Current& = ::Ice::Current()) override;
-
- virtual void setColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const DebugDrawerColoredPointCloud& pointCloud, const ::Ice::Current& = ::Ice::Current());
- void setColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawerColoredPointCloud& pointCloud, const ::Ice::Current& = ::Ice::Current()) override;
- void removeColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const ::Ice::Current& = ::Ice::Current()) override;
- void removeColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const ::Ice::Current& = ::Ice::Current()) override;
-
- void set24BitColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const DebugDrawer24BitColoredPointCloud& pointCloud, const ::Ice::Current& = ::Ice::Current()) override;
- void set24BitColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawer24BitColoredPointCloud& pointCloud, const ::Ice::Current& = ::Ice::Current()) override;
- void remove24BitColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const ::Ice::Current& = ::Ice::Current()) override;
- void remove24BitColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const ::Ice::Current& = ::Ice::Current()) override;
-
-
- void setPolygonVisu(const std::string& layerName, const std::string& polygonName, const std::vector< ::armarx::Vector3BasePtr >& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, float lineWidth, const ::Ice::Current& = ::Ice::Current()) override;
- void setPolygonDebugLayerVisu(const std::string& polygonName, const std::vector< ::armarx::Vector3BasePtr >& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, float lineWidth, const ::Ice::Current& = ::Ice::Current()) override;
- void removePolygonVisu(const std::string& layerName, const std::string& polygonName, const ::Ice::Current& = ::Ice::Current()) override;
- void removePolygonDebugLayerVisu(const std::string& polygonName, const ::Ice::Current& = ::Ice::Current()) override;
-
- void setTriMeshVisu(const std::string& layerName, const std::string& triMeshName, const DebugDrawerTriMesh& triMesh, const ::Ice::Current& = ::Ice::Current()) override;
- void setTriMeshDebugLayerVisu(const std::string& triMeshName, const DebugDrawerTriMesh& triMesh, const ::Ice::Current& = ::Ice::Current()) override;
- void removeTriMeshVisu(const std::string& layerName, const std::string& triMeshName, const ::Ice::Current& = ::Ice::Current()) override;
- void removeTriMeshDebugLayerVisu(const std::string& triMeshName, const ::Ice::Current& = ::Ice::Current()) override;
-
- void setArrowVisu(const std::string& layerName, const std::string& arrowName, const ::armarx::Vector3BasePtr& position, const ::armarx::Vector3BasePtr& direction, const DrawColor& color, float length, float width, const ::Ice::Current& = ::Ice::Current()) override;
- void setArrowDebugLayerVisu(const std::string& arrowName, const ::armarx::Vector3BasePtr& position, const ::armarx::Vector3BasePtr& direction, const DrawColor& color, float length, float width, const ::Ice::Current& = ::Ice::Current()) override;
- void removeArrowVisu(const std::string& layerName, const std::string& arrowName, const ::Ice::Current& = ::Ice::Current()) override;
- void removeArrowDebugLayerVisu(const std::string& arrowName, const ::Ice::Current& = ::Ice::Current()) override;
-
- void setCircleArrowVisu(const std::string& layerName, const std::string& circleName, const Vector3BasePtr& globalPosition, const Vector3BasePtr& directionVec, Ice::Float radius, Ice::Float circleCompletion, Ice::Float width, const DrawColor& color, const Ice::Current& = ::Ice::Current()) override;
- void setCircleDebugLayerVisu(const std::string& circleName, const Vector3BasePtr& globalPosition, const Vector3BasePtr& directionVec, Ice::Float radius, Ice::Float circleCompletion, Ice::Float width, const DrawColor& color, const Ice::Current& = ::Ice::Current()) override;
- void removeCircleVisu(const std::string& layerName, const std::string& circleName, const Ice::Current& = ::Ice::Current()) override;
- void removeCircleDebugLayerVisu(const std::string& circleName, const Ice::Current& = ::Ice::Current()) override;
+ void exportScene(const std::string& filename, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void exportLayer(const std::string& filename, const std::string& layerName, const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void setPoseVisu(const std::string& layerName, const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setScaledPoseVisu(const std::string& layerName, const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setPoseDebugLayerVisu(const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setScaledPoseDebugLayerVisu(const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removePoseVisu(const std::string& layerName, const std::string& poseName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removePoseDebugLayerVisu(const std::string& poseName, const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void setLineVisu(const std::string& layerName, const std::string& lineName, const ::armarx::Vector3BasePtr& globalPosition1, const ::armarx::Vector3BasePtr& globalPosition2, float lineWidth, const ::armarx::DrawColor& color, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setLineDebugLayerVisu(const std::string& lineName, const ::armarx::Vector3BasePtr& globalPosition1, const ::armarx::Vector3BasePtr& globalPosition2, float lineWidth, const ::armarx::DrawColor& color, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeLineVisu(const std::string& layerName, const std::string& lineName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeLineDebugLayerVisu(const std::string& lineName, const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void setLineSetVisu(const std::string& layerName, const std::string& lineSetName, const DebugDrawerLineSet& lineSet, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setLineSetDebugLayerVisu(const std::string& lineSetName, const DebugDrawerLineSet& lineSet, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeLineSetVisu(const std::string& layerName, const std::string& lineSetName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeLineSetDebugLayerVisu(const std::string& lineSetName, const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void setBoxVisu(const std::string& layerName, const std::string& boxName, const ::armarx::PoseBasePtr& globalPose, const ::armarx::Vector3BasePtr& dimensions, const DrawColor& color, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setBoxDebugLayerVisu(const std::string& boxName, const ::armarx::PoseBasePtr& globalPose, const ::armarx::Vector3BasePtr& dimensions, const DrawColor& color, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeBoxVisu(const std::string& layerName, const std::string& boxName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeBoxDebugLayerVisu(const std::string& boxName, const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void setTextVisu(const std::string& layerName, const std::string& textName, const std::string& text, const ::armarx::Vector3BasePtr& globalPosition, const DrawColor& color, int size, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setTextDebugLayerVisu(const std::string& textName, const std::string& text, const ::armarx::Vector3BasePtr& globalPosition, const DrawColor& color, int size, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeTextVisu(const std::string& layerName, const std::string& textName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeTextDebugLayerVisu(const std::string& textName, const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void setSphereVisu(const std::string& layerName, const std::string& sphereName, const ::armarx::Vector3BasePtr& globalPosition, const DrawColor& color, float radius, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setSphereDebugLayerVisu(const std::string& sphereName, const ::armarx::Vector3BasePtr& globalPosition, const DrawColor& color, float radius, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeSphereVisu(const std::string& layerName, const std::string& sphereName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeSphereDebugLayerVisu(const std::string& sphereName, const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void setCylinderVisu(const std::string& layerName, const std::string& cylinderName, const ::armarx::Vector3BasePtr& globalPosition, const ::armarx::Vector3BasePtr& direction, float length, float radius, const DrawColor& color, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setCylinderDebugLayerVisu(const std::string& cylinderName, const ::armarx::Vector3BasePtr& globalPosition, const ::armarx::Vector3BasePtr& direction, float length, float radius, const DrawColor& color, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeCylinderVisu(const std::string& layerName, const std::string& cylinderName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeCylinderDebugLayerVisu(const std::string& cylinderName, const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void setPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawerPointCloud& pointCloud, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setPointCloudDebugLayerVisu(const std::string& pointCloudName, const DebugDrawerPointCloud& pointCloud, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removePointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removePointCloudDebugLayerVisu(const std::string& pointCloudName, const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ virtual void setColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const DebugDrawerColoredPointCloud& pointCloud, const ::Ice::Current& = Ice::emptyCurrent);
+ void setColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawerColoredPointCloud& pointCloud, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void set24BitColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const DebugDrawer24BitColoredPointCloud& pointCloud, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void set24BitColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawer24BitColoredPointCloud& pointCloud, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void remove24BitColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void remove24BitColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const ::Ice::Current& = Ice::emptyCurrent) override;
+
+
+ void setPolygonVisu(const std::string& layerName, const std::string& polygonName, const std::vector< ::armarx::Vector3BasePtr >& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, float lineWidth, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setPolygonDebugLayerVisu(const std::string& polygonName, const std::vector< ::armarx::Vector3BasePtr >& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, float lineWidth, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removePolygonVisu(const std::string& layerName, const std::string& polygonName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removePolygonDebugLayerVisu(const std::string& polygonName, const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void setTriMeshVisu(const std::string& layerName, const std::string& triMeshName, const DebugDrawerTriMesh& triMesh, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setTriMeshDebugLayerVisu(const std::string& triMeshName, const DebugDrawerTriMesh& triMesh, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeTriMeshVisu(const std::string& layerName, const std::string& triMeshName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeTriMeshDebugLayerVisu(const std::string& triMeshName, const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void setArrowVisu(const std::string& layerName, const std::string& arrowName, const ::armarx::Vector3BasePtr& position, const ::armarx::Vector3BasePtr& direction, const DrawColor& color, float length, float width, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setArrowDebugLayerVisu(const std::string& arrowName, const ::armarx::Vector3BasePtr& position, const ::armarx::Vector3BasePtr& direction, const DrawColor& color, float length, float width, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeArrowVisu(const std::string& layerName, const std::string& arrowName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeArrowDebugLayerVisu(const std::string& arrowName, const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void setCircleArrowVisu(const std::string& layerName, const std::string& circleName, const Vector3BasePtr& globalPosition, const Vector3BasePtr& directionVec, Ice::Float radius, Ice::Float circleCompletion, Ice::Float width, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
+ void setCircleDebugLayerVisu(const std::string& circleName, const Vector3BasePtr& globalPosition, const Vector3BasePtr& directionVec, Ice::Float radius, Ice::Float circleCompletion, Ice::Float width, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeCircleVisu(const std::string& layerName, const std::string& circleName, const Ice::Current& = Ice::emptyCurrent) override;
+ void removeCircleDebugLayerVisu(const std::string& circleName, const Ice::Current& = Ice::emptyCurrent) override;
/*!
@@ -228,50 +228,50 @@ namespace armarx
* \param DrawStyle Either FullModel ior CollisionModel.
* \param armarxProject Additional armarx project that should be used to search the robot. Must be locally available and accessible through the armarx cmake search procedure.
*/
- void setRobotVisu(const std::string& layerName, const std::string& robotName, const std::string& robotFile, const std::string& armarxProject, DrawStyle drawStyle, const ::Ice::Current& = ::Ice::Current()) override;
- void updateRobotPose(const std::string& layerName, const std::string& robotName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Current& = ::Ice::Current()) override;
- void updateRobotConfig(const std::string& layerName, const std::string& robotName, const std::map< std::string, float>& configuration, const ::Ice::Current& = ::Ice::Current()) override;
+ void setRobotVisu(const std::string& layerName, const std::string& robotName, const std::string& robotFile, const std::string& armarxProject, DrawStyle drawStyle, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void updateRobotPose(const std::string& layerName, const std::string& robotName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void updateRobotConfig(const std::string& layerName, const std::string& robotName, const std::map< std::string, float>& configuration, const ::Ice::Current& = Ice::emptyCurrent) override;
/*!
* \brief updateRobotColor Colorizes the robot visualization
* \param layerName The layer
* \param robotName The robot identifyer
* \param c The draw color, if all is set to 0, the colorization is disabled (i.e. the original vizaulization shows up)
*/
- void updateRobotColor(const std::string& layerName, const std::string& robotName, const armarx::DrawColor& c, const ::Ice::Current& = ::Ice::Current()) override;
- void updateRobotNodeColor(const std::string& layerName, const std::string& robotName, const std::string& robotNodeName, const armarx::DrawColor& c, const ::Ice::Current& = ::Ice::Current()) override;
- void removeRobotVisu(const std::string& layerName, const std::string& robotName, const ::Ice::Current& = ::Ice::Current()) override;
+ void updateRobotColor(const std::string& layerName, const std::string& robotName, const armarx::DrawColor& c, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void updateRobotNodeColor(const std::string& layerName, const std::string& robotName, const std::string& robotNodeName, const armarx::DrawColor& c, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeRobotVisu(const std::string& layerName, const std::string& robotName, const ::Ice::Current& = Ice::emptyCurrent) override;
- void clearAll(const ::Ice::Current& = ::Ice::Current()) override;
- void clearLayer(const std::string& layerName, const ::Ice::Current& = ::Ice::Current()) override;
- void clearDebugLayer(const ::Ice::Current& = ::Ice::Current()) override;
+ void clearAll(const ::Ice::Current& = Ice::emptyCurrent) override;
+ void clearLayer(const std::string& layerName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void clearDebugLayer(const ::Ice::Current& = Ice::emptyCurrent) override;
- bool hasLayer(const std::string& layerName, const ::Ice::Current& = ::Ice::Current()) override;
- void removeLayer(const std::string& layerName, const ::Ice::Current& = ::Ice::Current()) override;
+ bool hasLayer(const std::string& layerName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeLayer(const std::string& layerName, const ::Ice::Current& = Ice::emptyCurrent) override;
- void enableLayerVisu(const std::string& layerName, bool visible, const ::Ice::Current& = ::Ice::Current()) override;
- void enableDebugLayerVisu(bool visible, const ::Ice::Current& = ::Ice::Current()) override;
+ void enableLayerVisu(const std::string& layerName, bool visible, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void enableDebugLayerVisu(bool visible, const ::Ice::Current& = Ice::emptyCurrent) override;
- ::Ice::StringSeq layerNames(const ::Ice::Current& = ::Ice::Current()) override;
- ::armarx::LayerInformationSequence layerInformation(const ::Ice::Current& = ::Ice::Current()) override;
+ ::Ice::StringSeq layerNames(const ::Ice::Current& = Ice::emptyCurrent) override;
+ ::armarx::LayerInformationSequence layerInformation(const ::Ice::Current& = Ice::emptyCurrent) override;
- void disableAllLayers(const ::Ice::Current& = ::Ice::Current()) override;
- void enableAllLayers(const ::Ice::Current& = ::Ice::Current()) override;
+ void disableAllLayers(const ::Ice::Current& = Ice::emptyCurrent) override;
+ void enableAllLayers(const ::Ice::Current& = Ice::emptyCurrent) override;
// Methods for selection management
- void enableSelections(const std::string& layerName, const ::Ice::Current& = ::Ice::Current()) override;
- void disableSelections(const std::string& layerName, const ::Ice::Current& = ::Ice::Current()) override;
- void clearSelections(const std::string& layerName, const ::Ice::Current& = ::Ice::Current()) override;
- void select(const std::string& layerName, const std::string& elementName, const ::Ice::Current& = ::Ice::Current()) override;
- void deselect(const std::string& layerName, const std::string& elementName, const ::Ice::Current& = ::Ice::Current()) override;
+ void enableSelections(const std::string& layerName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void disableSelections(const std::string& layerName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void clearSelections(const std::string& layerName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void select(const std::string& layerName, const std::string& elementName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void deselect(const std::string& layerName, const std::string& elementName, const ::Ice::Current& = Ice::emptyCurrent) override;
- DebugDrawerSelectionList getSelections(const ::Ice::Current& = ::Ice::Current()) override;
+ DebugDrawerSelectionList getSelections(const ::Ice::Current& = Ice::emptyCurrent) override;
void selectionCallback();
void deselectionCallback();
void installSelectionCallbacks();
void removeSelectionCallbacks();
- void reportSelectionChanged(const DebugDrawerSelectionList& selectedElements, const ::Ice::Current& = ::Ice::Current()) override;
+ void reportSelectionChanged(const DebugDrawerSelectionList& selectedElements, const ::Ice::Current& = Ice::emptyCurrent) override;
/*!
* \brief getScopedLock If using the coin visualization it must be ensured that all rendering calls are protected with this mutex
@@ -288,6 +288,36 @@ namespace armarx
void setMutex(boost::shared_ptr<boost::recursive_mutex> m);
~DebugDrawerComponent() override;
+
+ void setDefaultLayerVisibility(bool defaultLayerVisibility)
+ {
+ this->defaultLayerVisibility = defaultLayerVisibility;
+ }
+ void setDefaultLayerVisibility(const std::string& layerName, bool defaultLayerVisibility)
+ {
+ defaultLayerVisibilityPerLayer[layerName] = defaultLayerVisibility;
+ }
+ void removeDefaultLayerVisibility(const std::string& layerName)
+ {
+ defaultLayerVisibilityPerLayer.erase(layerName);
+ }
+ bool getDefaultLayerVisibility() const
+ {
+ return defaultLayerVisibility;
+ }
+ bool getDefaultLayerVisibility(const std::string& layerName) const
+ {
+ auto elem = defaultLayerVisibilityPerLayer.find(layerName);
+ if (elem != defaultLayerVisibilityPerLayer.end())
+ {
+ return elem->second;
+ }
+ return defaultLayerVisibility;
+ }
+ std::map<std::string, bool> getAllDefaultLayerVisibilities() const
+ {
+ return defaultLayerVisibilityPerLayer;
+ }
protected:
static void updateVisualizationCB(void* data, SoSensor* sensor);
@@ -581,6 +611,9 @@ namespace armarx
std::map<std::string, VirtualRobot::RobotPtr> loadedRobots;
+ bool defaultLayerVisibility;
+ std::map<std::string, bool> defaultLayerVisibilityPerLayer;
+
};
using DebugDrawerComponentPtr = IceInternal::Handle<DebugDrawerComponent>;
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
index 833d4dbdfca4c8ad28964bf3e6de1b33a3108b29..d4d68c30e9a198e5a50ba7238774f2cfaf3cb9b5 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
@@ -30,72 +30,214 @@
using namespace math;
using namespace armarx;
+#define CHECK_AND_ADD(name,type) if (!enableVisu) {return;} addNewElement(name,type);
+//#define CHECK_AND_ADD(name,type) if (!enableVisu) {return;}
+
DebugDrawerHelper::DebugDrawerHelper(const DebugDrawerInterfacePrx& debugDrawerPrx, const std::string& layerName, const VirtualRobot::RobotPtr& robot)
- : debugDrawerPrx(debugDrawerPrx), robot(robot), rn(robot->getRootNode())
+ : debugDrawerPrx(debugDrawerPrx), layerName(layerName), robot(robot), rn(robot ? robot->getRootNode() : VirtualRobot::RobotNodePtr())
{
}
void DebugDrawerHelper::drawPose(const std::string& name, const Eigen::Matrix4f& pose)
{
+ CHECK_AND_ADD(name, DrawElementType::Pose)
debugDrawerPrx->setPoseVisu(layerName, name, makeGlobal(pose));
}
void DebugDrawerHelper::drawPose(const std::string& name, const Eigen::Matrix4f& pose, float scale)
{
+ CHECK_AND_ADD(name, DrawElementType::Pose)
debugDrawerPrx->setScaledPoseVisu(layerName, name, makeGlobal(pose), scale);
}
void DebugDrawerHelper::drawBox(const std::string& name, const Eigen::Vector3f& position, float size, const DrawColor& color)
{
+ CHECK_AND_ADD(name, DrawElementType::Box)
drawBox(name, Helpers::CreatePose(position, Eigen::Matrix3f::Identity()), Eigen::Vector3f(size, size, size), color);
}
void DebugDrawerHelper::drawBox(const std::string& name, const Eigen::Matrix4f& pose, float size, const DrawColor& color)
{
+ CHECK_AND_ADD(name, DrawElementType::Box)
drawBox(name, pose, Eigen::Vector3f(size, size, size), color);
}
void DebugDrawerHelper::drawBox(const std::string& name, const Eigen::Matrix4f& pose, const Eigen::Vector3f& size, const DrawColor& color)
{
+ CHECK_AND_ADD(name, DrawElementType::Box)
debugDrawerPrx->setBoxVisu(layerName, name, makeGlobal(pose), new Vector3(size), color);
}
void DebugDrawerHelper::drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float width, const DrawColor& color)
{
+ CHECK_AND_ADD(name, DrawElementType::Line)
debugDrawerPrx->setLineVisu(layerName, name, makeGlobal(p1), makeGlobal(p2), width, color);
}
void DebugDrawerHelper::drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2)
{
+ CHECK_AND_ADD(name, DrawElementType::Line)
drawLine(name, p1, p2, defaults.lineWidth, defaults.lineColor);
}
void DebugDrawerHelper::drawText(const std::string& name, const Eigen::Vector3f& p1, const std::string& text, const DrawColor& color, int size)
{
+ CHECK_AND_ADD(name, DrawElementType::Text)
debugDrawerPrx->setTextVisu(layerName, name, text, makeGlobal(p1), color, size);
}
void DebugDrawerHelper::drawArrow(const std::string& name, const Eigen::Vector3f& pos, const Eigen::Vector3f& direction, const DrawColor& color, float length, float width)
{
+ CHECK_AND_ADD(name, DrawElementType::Arrow)
debugDrawerPrx->setArrowVisu(layerName, name, makeGlobal(pos), makeGlobalDirection(direction), color, length, width);
}
+void DebugDrawerHelper::drawPointCloud(const std::string& name, const std::vector<Eigen::Vector3f>& points, float pointSize, const DrawColor& color)
+{
+ CHECK_AND_ADD(name, DrawElementType::ColoredPointCloud)
+ DebugDrawerColoredPointCloud pc;
+ pc.pointSize = pointSize;
+ for (const Eigen::Vector3f& p : points)
+ {
+ Eigen::Vector3f pg = rn ? rn->getGlobalPosition(p) : p;
+ DebugDrawerColoredPointCloudElement e;
+ e.x = pg(0);
+ e.y = pg(1);
+ e.z = pg(2);
+ e.color = color; //DrawColor24Bit {(Ice::Byte)(color.r * 255), (Ice::Byte)(color.g * 255), (Ice::Byte)(color.b * 255)};
+ pc.points.push_back(e);
+ }
+ debugDrawerPrx->setColoredPointCloudVisu(layerName, name, pc);
+}
+
+void armarx::DebugDrawerHelper::drawRobot(const std::string& name, const std::string& robotFile, const std::string& armarxProject, const Eigen::Matrix4f& pose, const armarx::DrawColor& color)
+{
+ CHECK_AND_ADD(name, DrawElementType::Robot)
+ debugDrawerPrx->setRobotVisu(layerName, name, robotFile, armarxProject, DrawStyle::FullModel);
+ debugDrawerPrx->updateRobotPose(layerName, name, makeGlobal(pose));
+ debugDrawerPrx->updateRobotColor(layerName, name, color);
+}
+
+void DebugDrawerHelper::setRobotConfig(const std::string& name, const std::map<std::string, float>& config)
+{
+ debugDrawerPrx->updateRobotConfig(layerName, name, config);
+}
+
+void DebugDrawerHelper::clearLayer()
+{
+ debugDrawerPrx->clearLayer(layerName);
+ oldElements.clear();
+ newElements.clear();
+}
+
+void DebugDrawerHelper::cyclicCleanup()
+{
+ /*
+ // debug code...
+ std::stringstream ss;
+ ss << "OLD: ";
+ for (const DrawElement& e : oldElements)
+ {
+ ss << e.name << ",";
+ }
+ ss << " NEW: ";
+ for (const DrawElement& e : newElements)
+ {
+ ss << e.name << ",";
+ }
+ ARMARX_IMPORTANT << ss.str();*/
+
+ for (const DrawElement& old : oldElements)
+ {
+ if (newElements.find(old) == newElements.end())
+ {
+ //ARMARX_IMPORTANT << "removing " << old.name;
+ removeElement(old.name, old.type);
+ }
+ }
+ oldElements = newElements;
+ newElements.clear();
+}
+
+void DebugDrawerHelper::setVisuEnabled(bool enableVisu)
+{
+ this->enableVisu = enableVisu;
+}
+
PosePtr DebugDrawerHelper::makeGlobal(const Eigen::Matrix4f& pose)
{
- return new Pose(rn->getGlobalPose(pose));
+ Eigen::Matrix4f gp = rn ? rn->getGlobalPose(pose) : pose;
+ return new Pose(gp);
}
Vector3Ptr DebugDrawerHelper::makeGlobal(const Eigen::Vector3f& position)
{
- return new Vector3(rn->getGlobalPosition(position));
+ Eigen::Vector3f gp = rn ? rn->getGlobalPosition(position) : position;
+ return new Vector3(gp);
}
Vector3Ptr DebugDrawerHelper::makeGlobalDirection(const Eigen::Vector3f& direction)
{
- return new Vector3(math::Helpers::TransformDirection(rn->getGlobalPose(), direction));
+ Eigen::Vector3f gd = rn ? math::Helpers::TransformDirection(rn->getGlobalPose(), direction) : direction;
+ return new Vector3(gd);
+}
+
+Eigen::Vector3f DebugDrawerHelper::makeGlobalEigen(const Eigen::Vector3f& position)
+{
+ return rn ? rn->getGlobalPosition(position) : position;
}
void armarx::DebugDrawerHelper::drawSphere(const std::string& name, const Eigen::Vector3f& position, float size, const armarx::DrawColor& color)
{
debugDrawerPrx->setSphereVisu(layerName, name, makeGlobal(position), color, size);
}
+
+void DebugDrawerHelper::removeElement(const std::string& name, DebugDrawerHelper::DrawElementType type)
+{
+ switch (type)
+ {
+ case DrawElementType::Pose:
+ debugDrawerPrx->removePoseVisu(layerName, name);
+ return;
+ case DrawElementType::Box:
+ debugDrawerPrx->removeBoxVisu(layerName, name);
+ return;
+ case DrawElementType::Line:
+ debugDrawerPrx->removeLineVisu(layerName, name);
+ return;
+ case DrawElementType::Text:
+ debugDrawerPrx->removeTextVisu(layerName, name);
+ return;
+ case DrawElementType::Arrow:
+ debugDrawerPrx->removeArrowVisu(layerName, name);
+ return;
+ case DrawElementType::ColoredPointCloud:
+ debugDrawerPrx->removeColoredPointCloudVisu(layerName, name);
+ return;
+ case DrawElementType::Robot:
+ debugDrawerPrx->removeRobotVisu(layerName, name);
+ return;
+ }
+}
+
+void DebugDrawerHelper::addNewElement(const std::string& name, DebugDrawerHelper::DrawElementType type)
+{
+ newElements.insert(DrawElement {name, type});
+}
+
+const DrawColor DebugDrawerHelper::Colors::Red = DrawColor {1, 0, 0, 1};
+const DrawColor DebugDrawerHelper::Colors::Green = DrawColor {0, 1, 0, 1};
+const DrawColor DebugDrawerHelper::Colors::Blue = DrawColor {0, 0, 1, 1};
+const DrawColor DebugDrawerHelper::Colors::Yellow = DrawColor {1, 1, 0, 1};
+const DrawColor DebugDrawerHelper::Colors::Orange = DrawColor {1, 0.5f, 0, 1};
+
+DrawColor DebugDrawerHelper::Colors::Lerp(const DrawColor& a, const DrawColor& b, float f)
+{
+ return DrawColor
+ {
+ math::Helpers::Lerp(a.r, b.r, f),
+ math::Helpers::Lerp(a.g, b.g, f),
+ math::Helpers::Lerp(a.b, b.b, f),
+ math::Helpers::Lerp(a.a, b.a, f)
+ };
+}
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
index 217e548fd28d252c3408f8aa1e59f9e002353b48..334fa002eaa8cc1c8be7023757a48edd9cf5e620 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
@@ -42,6 +42,30 @@ namespace armarx
float lineWidth = 2;
DrawColor lineColor = DrawColor {0, 1, 0, 1};
};
+ enum class DrawElementType
+ {
+ Pose, Box, Line, Text, Arrow, ColoredPointCloud, Robot
+ };
+ struct DrawElement
+ {
+ std::string name;
+ DrawElementType type;
+ bool operator<(const DrawElement& rhs) const
+ {
+ int cmp = name.compare(rhs.name);
+ return cmp < 0 || (cmp == 0 && type < rhs.type);
+ }
+ };
+ class Colors
+ {
+ public:
+ static const DrawColor Red;
+ static const DrawColor Green;
+ static const DrawColor Blue;
+ static const DrawColor Yellow;
+ static const DrawColor Orange;
+ static DrawColor Lerp(const DrawColor& a, const DrawColor& b, float f);
+ };
DebugDrawerHelper(const DebugDrawerInterfacePrx& debugDrawerPrx, const std::string& layerName, const VirtualRobot::RobotPtr& robot);
@@ -61,14 +85,32 @@ namespace armarx
void drawSphere(const std::string& name, const Eigen::Vector3f& position, float size, const DrawColor& color);
+ void drawPointCloud(const std::string& name, const std::vector<Eigen::Vector3f>& points, float pointSize, const DrawColor& color);
+
+ void drawRobot(const std::string& name, const std::string& robotFile, const std::string& armarxProject, const Eigen::Matrix4f& pose, const DrawColor& color);
+ void setRobotConfig(const std::string& name, const std::map<std::string, float>& config);
+
+ void clearLayer();
+ void cyclicCleanup();
+
+ void setVisuEnabled(bool enableVisu);
+ void removeElement(const std::string& name, DrawElementType type);
+
PosePtr makeGlobal(const Eigen::Matrix4f& pose);
Vector3Ptr makeGlobal(const Eigen::Vector3f& position);
Vector3Ptr makeGlobalDirection(const Eigen::Vector3f& direction);
+ Eigen::Vector3f makeGlobalEigen(const Eigen::Vector3f& position);
Defaults defaults;
private:
+ void addNewElement(const std::string& name, DrawElementType type);
+
+ private:
+ std::set<DrawElement> newElements;
+ std::set<DrawElement> oldElements;
+ bool enableVisu = true;
DebugDrawerInterfacePrx debugDrawerPrx;
std::string layerName;
VirtualRobot::RobotPtr robot;
diff --git a/source/RobotAPI/components/ProsthesisObserver/CMakeLists.txt b/source/RobotAPI/components/FrameTracking/CMakeLists.txt
similarity index 53%
rename from source/RobotAPI/components/ProsthesisObserver/CMakeLists.txt
rename to source/RobotAPI/components/FrameTracking/CMakeLists.txt
index a9767421b3dba96a35c7b1680d67c82af4c88def..54f4372df34157e13fb91d07b87d6806ec7aaadc 100644
--- a/source/RobotAPI/components/ProsthesisObserver/CMakeLists.txt
+++ b/source/RobotAPI/components/FrameTracking/CMakeLists.txt
@@ -1,26 +1,25 @@
-armarx_component_set_name("ProsthesisObserver")
+armarx_component_set_name("FrameTracking")
-#find_package(MyLib QUIET)
-#armarx_build_if(MyLib_FOUND "MyLib not available")
-#
-# all include_directories must be guarded by if(Xyz_FOUND)
-# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
-#if(MyLib_FOUND)
-# include_directories(${MyLib_INCLUDE_DIRS})
-#endif()
-
-set(COMPONENT_LIBS ArmarXCoreInterfaces ArmarXCore RobotAPICore ArmarXCoreObservers)
+set(COMPONENT_LIBS ArmarXCore RobotAPIInterfaces RobotAPICore RemoteGui ArmarXCoreObservers ArmarXGuiInterfaces RobotUnit)
set(SOURCES
-./ProsthesisObserver.cpp
+./FrameTracking.cpp
#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
)
set(HEADERS
-./ProsthesisObserver.h
+./FrameTracking.h
#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
)
armarx_add_component("${SOURCES}" "${HEADERS}")
+#find_package(MyLib QUIET)
+#armarx_build_if(MyLib_FOUND "MyLib not available")
+# all target_include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+#if(MyLib_FOUND)
+# target_include_directories(FrameTracking PUBLIC ${MyLib_INCLUDE_DIRS})
+#endif()
+
# add unit tests
add_subdirectory(test)
diff --git a/source/RobotAPI/components/FrameTracking/FrameTracking.cpp b/source/RobotAPI/components/FrameTracking/FrameTracking.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d57f358307e245e560117783481fb5993e9349f1
--- /dev/null
+++ b/source/RobotAPI/components/FrameTracking/FrameTracking.cpp
@@ -0,0 +1,544 @@
+/*
+ * 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::FrameTracking
+ * @author Adrian Knobloch ( adrian dot knobloch at student dot kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "FrameTracking.h"
+
+#include <ArmarXCore/interface/core/BasicVectorTypesHelpers.h>
+#include <ArmarXCore/observers/variant/DatafieldRef.h>
+
+#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
+
+#include <RobotAPI/components/units/KinematicUnitObserver.h>
+
+#include <time.h>
+
+
+using namespace armarx;
+
+
+void FrameTracking::onInitComponent()
+{
+ usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
+ usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
+ usingProxy(getProperty<std::string>("KinematicUnitObserverName").getValue());
+ if (!getProperty<std::string>("RemoteGuiName").getValue().empty())
+ {
+ usingProxy(getProperty<std::string>("RemoteGuiName").getValue());
+ }
+
+ enabled = false;
+ frameName = getProperty<std::string>("FrameOnStartup").getValue();
+
+ maxYawVelocity = getProperty<float>("MaxYawVelocity").getValue();
+ yawAcceleration = getProperty<float>("YawAcceleration").getValue();
+
+ maxPitchVelocity = getProperty<float>("MaxPitchVelocity").getValue();
+ pitchAcceleration = getProperty<float>("PitchAcceleration").getValue();
+}
+
+
+void FrameTracking::onConnectComponent()
+{
+ robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
+ kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
+ kinematicUnitObserverInterfacePrx = getProxy<KinematicUnitObserverInterfacePrx>(getProperty<std::string>("KinematicUnitObserverName").getValue());
+
+ localRobot = armarx::RemoteRobot::createLocalClone(robotStateComponent);
+ headYawJoint = localRobot->getRobotNode(getProperty<std::string>("HeadYawJoint").getValue());
+ if (!headYawJoint || !(headYawJoint->isRotationalJoint() || headYawJoint->isTranslationalJoint()))
+ {
+ ARMARX_ERROR << getProperty<std::string>("HeadYawJoint").getValue() << " is not a valid joint.";
+ getArmarXManager()->asyncShutdown();
+ }
+ headPitchJoint = localRobot->getRobotNode(getProperty<std::string>("HeadPitchJoint").getValue());
+ if (!headPitchJoint || !(headPitchJoint->isRotationalJoint() || headPitchJoint->isTranslationalJoint()))
+ {
+ ARMARX_ERROR << getProperty<std::string>("HeadPitchJoint").getValue() << " is not a valid joint.";
+ getArmarXManager()->asyncShutdown();
+ }
+ cameraNode = localRobot->getRobotNode(getProperty<std::string>("CameraNode").getValue());
+ if (!cameraNode)
+ {
+ ARMARX_ERROR << getProperty<std::string>("CameraNode").getValue() << " is not a valid node.";
+ getArmarXManager()->asyncShutdown();
+ }
+
+ processorTask = new PeriodicTask<FrameTracking>(this, &FrameTracking::process, 30);
+ _enableTracking(getProperty<bool>("EnableTrackingOnStartup").getValue());
+
+ if (!getProperty<std::string>("RemoteGuiName").getValue().empty())
+ {
+ _remoteGui = getProxy<RemoteGuiInterfacePrx>(getProperty<std::string>("RemoteGuiName").getValue());
+ RemoteGui::detail::VBoxLayoutBuilder rootLayoutBuilder = RemoteGui::makeVBoxLayout();
+
+ rootLayoutBuilder.addChild(
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel("Tracking: "))
+ .addChild(RemoteGui::makeTextLabel("Enabled"))
+ .addChild(RemoteGui::makeCheckBox("enabled").value(enabled))
+ .addChild(RemoteGui::makeTextLabel("Frame"))
+ .addChild(
+ RemoteGui::makeComboBox("tracking_frame")
+ .options(localRobot->getRobotNodeNames())
+ .addOptions({""})
+ .value(frameName)
+ )
+ );
+
+ rootLayoutBuilder.addChild(
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel("Look at frame: "))
+ .addChild(
+ RemoteGui::makeComboBox("frame_look")
+ .options(localRobot->getRobotNodeNames())
+ .value(frameName)
+ )
+ .addChild(RemoteGui::makeButton("button_look_at_frame").label("look at"))
+ );
+
+ rootLayoutBuilder.addChild(
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel("Look at global point: "))
+ .addChild(RemoteGui::makeFloatSpinBox("global_point_x").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("global_point_y").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("global_point_z").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeButton("button_look_at_global_point").label("look at"))
+ );
+
+ rootLayoutBuilder.addChild(
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel("Look at point in robot frame: "))
+ .addChild(RemoteGui::makeFloatSpinBox("robot_point_x").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("robot_point_y").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("robot_point_z").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeButton("button_look_at_robot_point").label("look at"))
+ );
+
+ rootLayoutBuilder.addChild(
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel("Scan: "))
+ .addChild(RemoteGui::makeTextLabel("yaw from "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_from").min(headYawJoint->getJointLimitLo()).max(headYawJoint->getJointLimitHi()).steps(static_cast<int>((headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo()) / 0.1)))
+ .addChild(RemoteGui::makeTextLabel("yaw to "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_to").min(headYawJoint->getJointLimitLo()).max(headYawJoint->getJointLimitHi()).steps(static_cast<int>((headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo()) / 0.1)))
+ .addChild(RemoteGui::makeTextLabel("pitch "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_pitch").min(headPitchJoint->getJointLimitLo()).max(headPitchJoint->getJointLimitHi()).steps(static_cast<int>((headPitchJoint->getJointLimitHi() - headPitchJoint->getJointLimitLo()) / 0.1)))
+ .addChild(RemoteGui::makeTextLabel("velocity "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_velocity").min(0).max(6).steps(static_cast<int>(6 / 0.1)).value(0.8f))
+ .addChild(RemoteGui::makeButton("button_scan_in_configuration_space").label("scan"))
+ );
+
+ rootLayoutBuilder.addChild(
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel("Scan: "))
+ .addChild(RemoteGui::makeTextLabel("from "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_x").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_y").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_z").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeTextLabel("to "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_x").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_y").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_z").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
+ .addChild(RemoteGui::makeTextLabel("velocity "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_velocity").min(0).max(6).steps(static_cast<int>(6 / 0.1)).value(0.8f))
+ .addChild(RemoteGui::makeTextLabel("acceleration "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_acceleration").min(0).max(8).steps(static_cast<int>(8 / 0.1)).value(4.0f))
+ .addChild(RemoteGui::makeButton("button_scan_in_workspace").label("scan"))
+ );
+
+ rootLayoutBuilder.addChild(new RemoteGui::VSpacer());
+
+ _guiTask = new SimplePeriodicTask<>([this]()
+ {
+ bool oldEnabledGui = _guiTab.getValue<bool>("enabled").get();
+ std::string oldFrameGui = _guiTab.getValue<std::string>("tracking_frame").get();
+
+ _guiTab.receiveUpdates();
+
+ if (oldEnabledGui == enabled)
+ {
+ // only apply changes of gui if not already changed by ice
+ _enableTracking(_guiTab.getValue<bool>("enabled").get());
+ }
+ _guiTab.getValue<bool>("enabled").set(enabled);
+
+ if (oldFrameGui == frameName && oldFrameGui != _guiTab.getValue<std::string>("tracking_frame").get())
+ {
+ // only apply changes of gui if not already changed by ice
+ setFrame(_guiTab.getValue<std::string>("tracking_frame").get());
+ }
+ _guiTab.getValue<std::string>("tracking_frame").set(frameName);
+
+ _guiTab.sendUpdates();
+
+ if (_guiTab.getButton("button_look_at_frame").clicked())
+ {
+ lookAtFrame(_guiTab.getValue<std::string>("frame_look").get());
+ }
+
+ if (_guiTab.getButton("button_look_at_global_point").clicked())
+ {
+ lookAtPointInGlobalFrame(Vector3f{_guiTab.getValue<float>("global_point_x").get(),
+ _guiTab.getValue<float>("global_point_y").get(),
+ _guiTab.getValue<float>("global_point_z").get()});
+ }
+
+ if (_guiTab.getButton("button_look_at_robot_point").clicked())
+ {
+ lookAtPointInRobotFrame(Vector3f{_guiTab.getValue<float>("robot_point_x").get(),
+ _guiTab.getValue<float>("robot_point_y").get(),
+ _guiTab.getValue<float>("robot_point_z").get()});
+ }
+
+ if (_guiTab.getButton("button_scan_in_configuration_space").clicked())
+ {
+ scanInConfigurationSpace(_guiTab.getValue<float>("scan_in_configuration_space_yaw_from").get(),
+ _guiTab.getValue<float>("scan_in_configuration_space_yaw_to").get(),
+ {_guiTab.getValue<float>("scan_in_configuration_space_pitch").get()},
+ _guiTab.getValue<float>("scan_in_configuration_space_velocity").get());
+ }
+
+ if (_guiTab.getButton("button_scan_in_workspace").clicked())
+ {
+ scanInWorkspace(
+ {
+ {
+ _guiTab.getValue<float>("scan_in_workspace_from_x").get(),
+ _guiTab.getValue<float>("scan_in_workspace_from_y").get(),
+ _guiTab.getValue<float>("scan_in_workspace_from_z").get()
+ },
+ {
+ _guiTab.getValue<float>("scan_in_workspace_to_x").get(),
+ _guiTab.getValue<float>("scan_in_workspace_to_y").get(),
+ _guiTab.getValue<float>("scan_in_workspace_to_z").get()
+ }
+ },
+ _guiTab.getValue<float>("scan_in_workspace_velocity").get(),
+ _guiTab.getValue<float>("scan_in_workspace_acceleration").get());
+ }
+ }, 10);
+
+ RemoteGui::WidgetPtr rootLayout = rootLayoutBuilder;
+
+ _remoteGui->createTab(getName(), rootLayout);
+ _guiTab = RemoteGui::TabProxy(_remoteGui, getName());
+
+ _guiTask->start();
+ }
+}
+
+
+void FrameTracking::onDisconnectComponent()
+{
+ _enableTracking(false);
+ if (_guiTask)
+ {
+ _guiTask->stop();
+ _guiTask = nullptr;
+ }
+}
+
+
+void FrameTracking::onExitComponent()
+{
+
+}
+
+armarx::PropertyDefinitionsPtr FrameTracking::createPropertyDefinitions()
+{
+ return armarx::PropertyDefinitionsPtr(new FrameTrackingPropertyDefinitions(
+ getConfigIdentifier()));
+}
+
+void FrameTracking::enableTracking(bool enable, const Ice::Current&)
+{
+ _enableTracking(enable);
+}
+
+void FrameTracking::setFrame(const std::string& frameName, const Ice::Current&)
+{
+ if (enabled)
+ {
+ ARMARX_WARNING << "Disable tracking to set new frame.";
+ return;
+ }
+ this->frameName = frameName;
+}
+
+std::string FrameTracking::getFrame(const Ice::Current&) const
+{
+ return frameName;
+}
+
+void FrameTracking::lookAtFrame(const std::string& frameName, const Ice::Current&)
+{
+ if (enabled)
+ {
+ ARMARX_WARNING << "Disable tracking to use lookAt functions.";
+ return;
+ }
+ if (!localRobot->hasRobotNode(frameName))
+ {
+ ARMARX_ERROR << frameName << " does not exist.";
+ return;
+ }
+ syncronizeLocalClone();
+ _lookAtFrame(frameName);
+}
+
+void FrameTracking::lookAtPointInGlobalFrame(const Vector3f& point, const Ice::Current&)
+{
+ if (enabled)
+ {
+ ARMARX_WARNING << "Disable tracking to use lookAt functions.";
+ return;
+ }
+ syncronizeLocalClone();
+ _lookAtPoint(localRobot->toLocalCoordinateSystemVec(ToEigen(point)));
+}
+
+void FrameTracking::lookAtPointInRobotFrame(const Vector3f& point, const Ice::Current&)
+{
+ if (enabled)
+ {
+ ARMARX_WARNING << "Disable tracking to use lookAt functions.";
+ return;
+ }
+ syncronizeLocalClone();
+ _lookAtPoint(ToEigen(point));
+}
+
+void FrameTracking::scanInConfigurationSpace(float yawFrom, float yawTo, const Ice::FloatSeq& pitchValues, float velocity, const Ice::Current&)
+{
+ if (enabled)
+ {
+ ARMARX_WARNING << "Disable tracking to use scan functions.";
+ return;
+ }
+ velocity = std::abs(velocity);
+
+ syncronizeLocalClone();
+ auto currentModes = kinematicUnitInterfacePrx->getControlModes();
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl}, {headPitchJoint->getName(), ControlMode::eVelocityControl}});
+
+ // to initial yaw
+ {
+ float yawVelocityToInit = (headYawJoint->getJointValue() > yawFrom) ? -velocity : velocity;
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), yawVelocityToInit}, {headPitchJoint->getName(), 0.f}});
+ while (std::abs(headYawJoint->getJointValue() - yawFrom) > static_cast<float>(M_PI / 180.))
+ {
+ syncronizeLocalClone();
+ }
+ }
+
+ for (const auto& p : pitchValues)
+ {
+ // to pitch value
+ float velocityPitch = headPitchJoint->getJointValue() < p ? velocity : -velocity;
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), velocityPitch}});
+ while (std::abs(headPitchJoint->getJointValue() - p) > static_cast<float>(M_PI / 180.))
+ {
+ syncronizeLocalClone();
+ }
+
+ // to yaw value
+ float velocityYaw = yawFrom < yawTo ? velocity : -velocity;
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), velocityYaw}, {headPitchJoint->getName(), 0.f}});
+ while (std::abs(headYawJoint->getJointValue() - yawTo) > static_cast<float>(M_PI / 180.))
+ {
+ syncronizeLocalClone();
+ }
+
+ std::swap(yawFrom, yawTo);
+ }
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]}, {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
+}
+
+void FrameTracking::scanInWorkspace(const Vector3fSeq& points, float maxVelocity, float acceleration, const Ice::Current&)
+{
+ if (enabled)
+ {
+ ARMARX_WARNING << "Disable tracking to use scan functions.";
+ return;
+ }
+ syncronizeLocalClone();
+ auto currentModes = kinematicUnitInterfacePrx->getControlModes();
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl}, {headPitchJoint->getName(), ControlMode::eVelocityControl}});
+ struct timespec req = {0, 30 * 1000000L};
+ for (const auto& p : points)
+ {
+ auto pEigen = localRobot->toLocalCoordinateSystemVec(ToEigen(p));
+ auto target = _calculateJointAngles(pEigen);
+ while (std::abs(target.currentYawPos - target.desiredYawPos) > static_cast<float>(M_PI / 180.) ||
+ std::abs(target.currentPitchPos - target.desiredPitchPos) > static_cast<float>(M_PI / 180.))
+ {
+ ARMARX_INFO << "yaw: " << target.currentYawPos << " - " << target.desiredYawPos << " pitch: " << target.currentPitchPos << " - " << target.desiredPitchPos;
+ syncronizeLocalClone();
+ target = _calculateJointAngles(pEigen);
+ _doVelocityControl(target, maxVelocity, acceleration, maxVelocity, acceleration);
+ // sleep for 30 milliseconds
+ nanosleep(&req, nullptr);
+ }
+ }
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]}, {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
+}
+
+void FrameTracking::process()
+{
+ if (!localRobot->hasRobotNode(frameName))
+ {
+ ARMARX_ERROR << frameName << " does not exist. Task will be disabled.";
+ std::thread([this]()
+ {
+ _enableTracking(false);
+ }).detach();
+ return;
+ }
+ syncronizeLocalClone();
+ _doVelocityControl(_calculateJointAnglesContinously(frameName), maxYawVelocity, yawAcceleration, maxPitchVelocity, pitchAcceleration);
+}
+
+void FrameTracking::syncronizeLocalClone()
+{
+ armarx::RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponent);
+}
+
+void FrameTracking::_lookAtFrame(const std::string& frameName)
+{
+ auto frame = localRobot->getRobotNode(frameName);
+ auto posInRobotFrame = localRobot->toLocalCoordinateSystemVec(frame->getGlobalPosition());
+ _lookAtPoint(posInRobotFrame);
+}
+
+void FrameTracking::_lookAtPoint(const Eigen::Vector3f& point)
+{
+ _doPositionControl(_calculateJointAngles(point));
+}
+
+FrameTracking::HeadState FrameTracking::_calculateJointAnglesContinously(const std::string& frameName)
+{
+ auto frame = localRobot->getRobotNode(frameName);
+ auto posInRobotFrame = localRobot->toLocalCoordinateSystemVec(frame->getGlobalPosition());
+ // do nothing if the robot works above his head
+ // he should already look upwards because if this component runs continously
+ if (std::sqrt(posInRobotFrame.x()*posInRobotFrame.x() + posInRobotFrame.y()*posInRobotFrame.y()) < 300.f)
+ {
+ return FrameTracking::HeadState{true, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f};
+ }
+ return _calculateJointAngles(posInRobotFrame);
+}
+
+FrameTracking::HeadState FrameTracking::_calculateJointAngles(const Eigen::Vector3f& point)
+{
+ float yaw = -std::atan2(point.x(), point.y());
+ // make shure the joint value satisfies the joint limits
+ yaw = std::max(headYawJoint->getJointLimitLo(), yaw);
+ yaw = std::min(headYawJoint->getJointLimitHi(), yaw);
+ // we dont want the robot to move from one limit to the other in one step
+ const float currentYaw = headYawJoint->getJointValue();
+ if (!headYawJoint->isLimitless() && std::abs(currentYaw - yaw) > headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo() - static_cast<float>(M_PI) / 8)
+ {
+ yaw = currentYaw;
+ }
+
+ const auto pointInPitchJointFrame = headPitchJoint->toLocalCoordinateSystemVec(localRobot->toGlobalCoordinateSystemVec(point));
+ const Eigen::Vector2f pj{pointInPitchJointFrame.y(), pointInPitchJointFrame.z()};
+ const float headHeightRealativeToPitchJoint = headPitchJoint->toLocalCoordinateSystemVec(cameraNode->getGlobalPosition()).z();
+ float pitch = headPitchJoint->getJointValue() - std::asin(pj.x() / pj.norm()) + std::asin(headHeightRealativeToPitchJoint / pj.norm());
+ // make shure the joint value satisfies the joint limits
+ pitch = std::max(headPitchJoint->getJointLimitLo(), pitch);
+ pitch = std::min(headPitchJoint->getJointLimitHi(), pitch);
+ const float currentPitch = headPitchJoint->getJointValue();
+
+ ARMARX_INFO << deactivateSpam(1.f, "FrameTracking") << "Looking at " << point << " using yaw=" << yaw << " and pitch=" << pitch;
+
+ const float currentYawVel = DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities", headYawJoint->getName()))->getFloat();
+ const float currentPitchVel = DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities", headPitchJoint->getName()))->getFloat();
+
+ FrameTracking::HeadState headState;
+ headState.currentYawPos = currentYaw;
+ headState.desiredYawPos = yaw;
+ headState.currentYawVel = currentYawVel;
+ headState.currentPitchPos = currentPitch;
+ headState.desiredPitchPos = pitch;
+ headState.currentPitchVel = currentPitchVel;
+ return headState;
+}
+
+void FrameTracking::_doVelocityControl(const FrameTracking::HeadState& headState, float maxYawVelocity, float yawAcceleration, float maxPitchVelocity, float pitchAcceleration)
+{
+ if (headState.stop)
+ {
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
+ return;
+ }
+
+ float desiredYawVelocity = positionThroughVelocityControlWithAccelerationBounds(
+ 30.f / 1000, 35.f / 1000,
+ headState.currentYawVel, maxYawVelocity,
+ yawAcceleration, yawAcceleration,
+ headState.currentYawPos, headState.desiredYawPos,
+ 1.f);
+ float desiredPitchVelocity = positionThroughVelocityControlWithAccelerationBounds(
+ 30.f / 1000, 35.f / 1000,
+ headState.currentPitchVel, maxPitchVelocity,
+ pitchAcceleration, pitchAcceleration,
+ headState.currentPitchPos, headState.desiredPitchPos,
+ 1.f);
+
+ // control mode is set when enable task
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), desiredYawVelocity}, {headPitchJoint->getName(), desiredPitchVelocity}});
+}
+
+void FrameTracking::_doPositionControl(const FrameTracking::HeadState& headState)
+{
+ auto currentModes = kinematicUnitInterfacePrx->getControlModes();
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::ePositionControl}, {headPitchJoint->getName(), ControlMode::ePositionControl}});
+ if (headState.stop)
+ {
+ return;
+ }
+ kinematicUnitInterfacePrx->setJointAngles({{headYawJoint->getName(), headState.desiredYawPos}, {headPitchJoint->getName(), headState.desiredPitchPos}});
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]}, {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
+}
+
+void FrameTracking::_enableTracking(bool enable)
+{
+ if (this->enabled == enable)
+ {
+ return;
+ }
+ this->enabled = enable;
+ if (enable)
+ {
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl}, {headPitchJoint->getName(), ControlMode::eVelocityControl}});
+ processorTask->start();
+ }
+ else
+ {
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
+ processorTask->stop();
+ }
+}
+
diff --git a/source/RobotAPI/components/FrameTracking/FrameTracking.h b/source/RobotAPI/components/FrameTracking/FrameTracking.h
new file mode 100644
index 0000000000000000000000000000000000000000..c8fc80e595ac71a6b55a96dfc435b3ce2a1e2091
--- /dev/null
+++ b/source/RobotAPI/components/FrameTracking/FrameTracking.h
@@ -0,0 +1,178 @@
+/*
+ * 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::FrameTracking
+ * @author Adrian Knobloch ( adrian dot knobloch at student dot kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+
+#include <RobotAPI/interface/components/FrameTrackingInterface.h>
+#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
+
+#include <ArmarXGui/interface/RemoteGuiInterface.h>
+#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
+
+#include <Eigen/Core>
+
+namespace armarx
+{
+ /**
+ * @class FrameTrackingPropertyDefinitions
+ * @brief
+ */
+ class FrameTrackingPropertyDefinitions:
+ public armarx::ComponentPropertyDefinitions
+ {
+ public:
+ FrameTrackingPropertyDefinitions(std::string prefix):
+ armarx::ComponentPropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the robot state component that should be used");
+ defineOptionalProperty<std::string>("KinematicUnitName", "KinematicUnit", "Name of the kinematic unit that should be used");
+ defineOptionalProperty<std::string>("KinematicUnitObserverName", "KinematicUnitObserver", "Name of the kinematic unit observer that should be used");
+ defineRequiredProperty<std::string>("HeadYawJoint", "Name of the yaw joint of the head.");
+ defineRequiredProperty<std::string>("HeadPitchJoint", "Name of the pitch joint of the head.");
+ defineRequiredProperty<std::string>("CameraNode", "Name of the camera node.");
+ defineOptionalProperty<bool>("EnableTrackingOnStartup", true, "Start Tracking on startup. This needs a frame to be defined.");
+ defineOptionalProperty<std::string>("FrameOnStartup", "", "Name of the frame that should be tracked.");
+ defineOptionalProperty<std::string>("RemoteGuiName", "RemoteGuiProvider", "Name of the remote gui. Remote gui is disabled if empty.");
+ defineOptionalProperty<float>("MaxYawVelocity", 0.8f, "The maximum velocity the yaw joint while tracking objects.");
+ defineOptionalProperty<float>("YawAcceleration", 4.f, "The acceleration of the yaw joint while tracking objects.");
+ defineOptionalProperty<float>("MaxPitchVelocity", 0.8f, "The maximum velocity the pitch joint while tracking objects.");
+ defineOptionalProperty<float>("PitchAcceleration", 4.f, "The acceleration of the pitch joint while tracking objects.");
+ }
+ };
+
+ /**
+ * @defgroup Component-FrameTracking FrameTracking
+ * @ingroup RobotAPI-Components
+ * A description of the component FrameTracking.
+ *
+ * @class FrameTracking
+ * @ingroup Component-FrameTracking
+ * @brief Brief description of class FrameTracking.
+ *
+ * Detailed description of class FrameTracking.
+ */
+ class FrameTracking :
+ virtual public armarx::Component,
+ public FrameTrackingInterface
+ {
+ private:
+ struct HeadState
+ {
+ bool stop = false;
+ float currentYawPos;
+ float currentYawVel;
+ float desiredYawPos;
+ float currentPitchPos;
+ float currentPitchVel;
+ float desiredPitchPos;
+ };
+ public:
+ /**
+ * @see armarx::ManagedIceObject::getDefaultName()
+ */
+ std::string getDefaultName() const override
+ {
+ return "FrameTracking";
+ }
+
+ protected:
+ /**
+ * @see armarx::ManagedIceObject::onInitComponent()
+ */
+ void onInitComponent() override;
+
+ /**
+ * @see armarx::ManagedIceObject::onConnectComponent()
+ */
+ void onConnectComponent() override;
+
+ /**
+ * @see armarx::ManagedIceObject::onDisconnectComponent()
+ */
+ void onDisconnectComponent() override;
+
+ /**
+ * @see armarx::ManagedIceObject::onExitComponent()
+ */
+ void onExitComponent() override;
+
+ /**
+ * @see PropertyUser::createPropertyDefinitions()
+ */
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ // FrameTrackingInterface interface
+ public:
+ void enableTracking(bool enable, const Ice::Current& = Ice::emptyCurrent) override;
+ void setFrame(const std::string& frameName, const Ice::Current& = Ice::emptyCurrent) override;
+ std::string getFrame(const Ice::Current& = Ice::emptyCurrent) const override;
+
+ void lookAtFrame(const std::string& frameName, const Ice::Current& = Ice::emptyCurrent) override;
+ void lookAtPointInGlobalFrame(const Vector3f& point, const Ice::Current& = Ice::emptyCurrent) override;
+ void lookAtPointInRobotFrame(const Vector3f& point, const Ice::Current& = Ice::emptyCurrent) override;
+
+ void scanInConfigurationSpace(float yawFrom, float yawTo, const ::Ice::FloatSeq& pitchValues, float velocity, const Ice::Current& = Ice::emptyCurrent) override;
+ void scanInWorkspace(const ::armarx::Vector3fSeq& points, float maxVelocity, float acceleration, const Ice::Current& = Ice::emptyCurrent) override;
+
+ protected:
+ void process();
+
+ void syncronizeLocalClone();
+ void _lookAtFrame(const std::string& frame);
+ void _lookAtPoint(const Eigen::Vector3f& point);
+ HeadState _calculateJointAnglesContinously(const std::string& frame);
+ HeadState _calculateJointAngles(const Eigen::Vector3f& point);
+ void _doVelocityControl(const HeadState& headstate, float maxYawVelocity, float yawAcceleration, float maxPitchVelocity, float pitchAcceleration);
+ void _doPositionControl(const HeadState& headstate);
+ void _enableTracking(bool enable);
+
+ RobotStateComponentInterfacePrx robotStateComponent;
+ KinematicUnitInterfacePrx kinematicUnitInterfacePrx;// send commands to kinematic unit
+ PeriodicTask<FrameTracking>::pointer_type processorTask;
+ KinematicUnitObserverInterfacePrx kinematicUnitObserverInterfacePrx;
+
+ std::atomic<bool> enabled;
+ std::string frameName;
+ float maxYawVelocity;
+ float yawAcceleration;
+ float maxPitchVelocity;
+ float pitchAcceleration;
+
+ VirtualRobot::RobotPtr localRobot;
+ VirtualRobot::RobotNodePtr headYawJoint;
+ VirtualRobot::RobotNodePtr headPitchJoint;
+ VirtualRobot::RobotNodePtr cameraNode;
+
+ RemoteGuiInterfacePrx _remoteGui;
+ SimplePeriodicTask<>::pointer_type _guiTask;
+ RemoteGui::TabProxy _guiTab;
+ };
+}
diff --git a/source/RobotAPI/components/FrameTracking/test/CMakeLists.txt b/source/RobotAPI/components/FrameTracking/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1d0500243d00099fe138fd0ccf1310dbd31c915d
--- /dev/null
+++ b/source/RobotAPI/components/FrameTracking/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore FrameTracking)
+
+armarx_add_test(FrameTrackingTest FrameTrackingTest.cpp "${LIBS}")
\ No newline at end of file
diff --git a/source/RobotAPI/components/ProsthesisObserver/test/ProsthesisObserverTest.cpp b/source/RobotAPI/components/FrameTracking/test/FrameTrackingTest.cpp
similarity index 72%
rename from source/RobotAPI/components/ProsthesisObserver/test/ProsthesisObserverTest.cpp
rename to source/RobotAPI/components/FrameTracking/test/FrameTrackingTest.cpp
index cd0633992dcf9ae131efbfe6c4e444bfbe95bd84..d7fbb0b3e6768337d4dd9084960f84e3f981f9ab 100644
--- a/source/RobotAPI/components/ProsthesisObserver/test/ProsthesisObserverTest.cpp
+++ b/source/RobotAPI/components/FrameTracking/test/FrameTrackingTest.cpp
@@ -13,25 +13,25 @@
* 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::ProsthesisObserver
- * @author JuliaStarke ( julia dot starke at kit dot edu )
- * @date 2018
+ * @package RobotAPI::ArmarXObjects::FrameTracking
+ * @author Adrian Knobloch ( adrian dot knobloch at student dot kit dot edu )
+ * @date 2019
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::ProsthesisObserver
+#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::FrameTracking
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/components/ProsthesisObserver/ProsthesisObserver.h>
+#include <RobotAPI/components/FrameTracking/FrameTracking.h>
#include <iostream>
BOOST_AUTO_TEST_CASE(testExample)
{
- armarx::ProsthesisObserver instance;
+ armarx::FrameTracking instance;
BOOST_CHECK_EQUAL(true, true);
}
diff --git a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
index 7d29b0a42699884b5f5d1aba07e5c534269030cf..f4e94fa036c7e0ac2c340d717802d71bd8644817 100644
--- a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
+++ b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
@@ -70,7 +70,14 @@ armarx::PropertyDefinitionsPtr GamepadControlUnit::createPropertyDefinitions()
void GamepadControlUnit::reportGamepadState(const std::string& device, const std::string& name, const GamepadData& data, const TimestampBasePtr& timestamp, const Ice::Current& c)
{
//scales are for the robot axis
- platformUnitPrx->move(data.leftStickY * scaleX, data. leftStickX * scaleY, data.rightStickX * scaleRotation);
+ if (data.rightTrigger > 0)
+ {
+ platformUnitPrx->move(data.leftStickY * scaleX, data. leftStickX * scaleY, data.rightStickX * scaleRotation);
+ }
+ else
+ {
+ platformUnitPrx->move(0, 0, 0);
+ }
//ARMARX_INFO << "sending targets" << data.leftStickX* scaleX << " " << data.leftStickY* scaleY << " " << data.rightStickX* scaleRotation;
}
diff --git a/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp b/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp
index 549629821483ac7a3060f9312c28e1d7180e6c3b..f7bc0e29d1593cb0c481acf61fd8c331ac62a166 100644
--- a/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp
+++ b/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp
@@ -25,6 +25,8 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
+
#include "KITProstheticHandUnit.h"
using namespace armarx;
@@ -50,6 +52,91 @@ void KITProstheticHandUnit::onInitHandUnit()
addShapeName("G6");
addShapeName("G7");
addShapeName("G8");
+
+ offeringTopic(getProperty<std::string>("DebugObserverName"));
+ if (!getProperty<std::string>("RemoteGuiName").getValue().empty())
+ {
+ usingProxy(getProperty<std::string>("RemoteGuiName"));
+ }
+}
+
+void KITProstheticHandUnit::onStartHandUnit()
+{
+ _debugObserver = getTopic<DebugObserverInterfacePrx>(getProperty<std::string>("DebugObserverName"));
+ if (!getProperty<std::string>("RemoteGuiName").getValue().empty())
+ {
+ _remoteGuiPrx = getProxy<RemoteGuiInterfacePrx>(getProperty<std::string>("RemoteGuiName").getValue());
+
+
+ RemoteGui::detail::VBoxLayoutBuilder rootLayoutBuilder = RemoteGui::makeVBoxLayout();
+
+ auto addFinger = [&](std::string name, float min, float max, float val, int steps)
+ {
+ rootLayoutBuilder.addChild(
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel(name))
+ .addChild(RemoteGui::makeTextLabel("min " + std::to_string(min)))
+ .addChild(RemoteGui::makeFloatSlider(name).min(min).max(max).value(val).steps(steps))
+ .addChild(RemoteGui::makeTextLabel("max " + std::to_string(max)))
+ );
+ rootLayoutBuilder.addChild(
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel(name + " Pos "))
+ .addChild(RemoteGui::makeLabel(name + "_pos").value("0"))
+ .addChild(new RemoteGui::HSpacer())
+ );
+ rootLayoutBuilder.addChild(
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel(name + " PWM "))
+ .addChild(RemoteGui::makeLabel(name + "_pwm").value("0"))
+ .addChild(new RemoteGui::HSpacer())
+ );
+ };
+
+ addFinger("Thumb", 0, 1, _lastGuiValueThumb, _driver->getMaxPosThumb());
+ addFinger("Fingers", 0, 1, _lastGuiValueFingers, _driver->getMaxPosFingers());
+
+ rootLayoutBuilder.addChild(new RemoteGui::VSpacer());
+
+ _guiTask = new SimplePeriodicTask<>([&]()
+ {
+ _guiTab.receiveUpdates();
+ _driver->getMaxPosThumb();
+ const float t = _guiTab.getValue<float>("Thumb").get();
+ const float f = _guiTab.getValue<float>("Fingers").get();
+
+ bool updateT = t != _lastGuiValueThumb;
+ bool updateF = f != _lastGuiValueFingers;
+ _lastGuiValueThumb = t;
+ _lastGuiValueFingers = f;
+
+ if (updateT && updateF)
+ {
+ setJointAngles({{"Thumb", t}, {"Fingers", f}});
+ }
+ else if (updateT)
+ {
+ setJointAngles({{"Thumb", t}});
+ }
+ else if (updateF)
+ {
+ setJointAngles({{"Fingers", f}});
+ }
+
+ _guiTab.getValue<std::string>("Thumb_pos").set(std::to_string(_driver->getThumbPos()));
+ _guiTab.getValue<std::string>("Thumb_pwm").set(std::to_string(_driver->getThumbPWM()));
+ _guiTab.getValue<std::string>("Fingers_pos").set(std::to_string(_driver->getFingerPos()));
+ _guiTab.getValue<std::string>("Fingers_pwm").set(std::to_string(_driver->getFingerPWM()));
+ _guiTab.sendUpdates();
+ }, 10);
+
+ RemoteGui::WidgetPtr rootLayout = rootLayoutBuilder;
+
+ _remoteGuiPrx->createTab("KITProstheticHandUnit", rootLayout);
+ _guiTab = RemoteGui::TabProxy(_remoteGuiPrx, "KITProstheticHandUnit");
+
+ _guiTask->start();
+ }
}
void KITProstheticHandUnit::onExitHandUnit()
@@ -65,7 +152,7 @@ void KITProstheticHandUnit::setJointAngles(const NameValueMap& targetJointAngles
{
if (pair.first == "Fingers")
{
- _driver->sendFingerPWM(200, 2999, static_cast<std::uint64_t>(pair.second * _driver->maxPosF()));
+ _driver->sendFingerPWM(200, 2999, static_cast<std::uint64_t>(pair.second * _driver->getMaxPosFingers()));
// fix until hw driver is fixed to handle multiple commands at the same time
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
@@ -85,7 +172,7 @@ void KITProstheticHandUnit::setJointAngles(const NameValueMap& targetJointAngles
NameValueMap KITProstheticHandUnit::getCurrentJointValues(const Ice::Current&)
{
NameValueMap jointValues;
- jointValues["Fingers"] = _driver->getFingerPos() * 1.f / _driver->maxPosF();
+ jointValues["Fingers"] = _driver->getFingerPos() * 1.f / _driver->getMaxPosFingers();
jointValues["Thumb"] = _driver->getThumbPos() * 1.f / _driver->getMaxPosThumb();
return jointValues;
}
diff --git a/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.h b/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.h
index 7b9c89a13d141076e1c8aabf8a1f66b6d0d5897e..1cba8b8eb3e34e163780298b3fda9af46c4f4985 100644
--- a/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.h
+++ b/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.h
@@ -22,8 +22,12 @@
#pragma once
-
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
+
+#include <ArmarXGui/interface/RemoteGuiInterface.h>
+#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
#include <RobotAPI/components/units/HandUnit.h>
#include <RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h>
@@ -42,7 +46,8 @@ namespace armarx
armarx::HandUnitPropertyDefinitions(prefix)
{
defineRequiredProperty<std::string>("MAC", "The KIT Prosthetic Hand's Bluetooth MAC");
- //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
+ defineOptionalProperty<std::string>("RemoteGuiName", "", "Name of the remote gui");
+ defineOptionalProperty<std::string>("DebugObserverName", "DebugObserver", "Name of the debug observer that should be used");
}
};
@@ -70,7 +75,7 @@ namespace armarx
}
void onInitHandUnit() override;
- void onStartHandUnit() override {}
+ void onStartHandUnit() override;
void onExitHandUnit() override;
void setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& = Ice::emptyCurrent) override;
NameValueMap getCurrentJointValues(const Ice::Current&) override;
@@ -90,5 +95,13 @@ namespace armarx
private:
std::unique_ptr<BLEProthesisInterface> _driver;
std::map<std::string, std::map<std::string, float>> _shapes;
+
+ DebugObserverInterfacePrx _debugObserver;
+ //gui
+ RemoteGuiInterfacePrx _remoteGuiPrx;
+ SimplePeriodicTask<>::pointer_type _guiTask;
+ RemoteGui::TabProxy _guiTab;
+ float _lastGuiValueThumb{0};
+ float _lastGuiValueFingers{0};
};
}
diff --git a/source/RobotAPI/components/ProsthesisObserver/ProsthesisObserver.cpp b/source/RobotAPI/components/ProsthesisObserver/ProsthesisObserver.cpp
deleted file mode 100644
index 45046d5d42b0b39ffacbd8001bf1e9bb7a9690d9..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ProsthesisObserver/ProsthesisObserver.cpp
+++ /dev/null
@@ -1,81 +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::ProsthesisObserver
- * @author JuliaStarke ( julia dot starke at kit dot edu )
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "ProsthesisObserver.h"
-#include <ArmarXCore/observers/variant/TimestampVariant.h>
-
-using namespace armarx;
-
-
-void ProsthesisObserver::onInitObserver()
-{
- usingTopic(getProperty<std::string>("ProsthesisTopicName").getValue());
- offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
-}
-
-
-void ProsthesisObserver::onConnectObserver()
-{
- debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
-}
-
-
-//void ProsthesisObserver::onDisconnectComponent()
-//{
-
-//}
-
-
-void ProsthesisObserver::onExitObserver()
-{
-
-}
-
-PropertyDefinitionsPtr ProsthesisObserver::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new ProsthesisObserverPropertyDefinitions(
- getConfigIdentifier()));
-}
-
-void ProsthesisObserver::reportMotorValues(const ProsthesisMotorValues& motorValues, const Ice::Current&)
-{
- ScopedLock lock(dataMutex);
- //TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
-
- std::string name = motorValues.name;
- if (!existsChannel(name))
- {
- offerChannel(name, "Prosthesis motor data");
- }
-
- offerOrUpdateDataField(name, "name", Variant(name), "Name of the prostesis");
- //offerOrUpdateDataField(name, "device", Variant(device), "Device name");
- offerOrUpdateDataField(name, "position1", Variant(motorValues.position1), "Position motor 2");
- offerOrUpdateDataField(name, "pwm1", Variant(motorValues.pwm1), "PWM motor 2");
- offerOrUpdateDataField(name, "position2", Variant(motorValues.position2), "Position motor 3");
- offerOrUpdateDataField(name, "pwm2", Variant(motorValues.pwm2), "PWM motor 3");
- //offerOrUpdateDataField(name, "timestamp", timestampPtr, "Timestamp");
- //offerOrUpdateDataField(name, "position", Vector2(position1, position2), "Position");
- //offerOrUpdateDataField(name, "pwm", Vector2(pwm1, pwm2), "PWM");
-
- updateChannel(name);
-}
diff --git a/source/RobotAPI/components/ProsthesisObserver/ProsthesisObserver.h b/source/RobotAPI/components/ProsthesisObserver/ProsthesisObserver.h
deleted file mode 100644
index 53f5109da36071e84fbf0838e9931abf6a21cdfd..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ProsthesisObserver/ProsthesisObserver.h
+++ /dev/null
@@ -1,112 +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::ProsthesisObserver
- * @author JuliaStarke ( julia dot starke at kit dot edu )
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-
-//#include <ArmarXCore/core/Component.h>
-//#include <RobotAPI/interface/units/ProsthesisInterface.h>
-#include <RobotAPI/interface/units/ProsthesisObserverInterface.h>
-#include <ArmarXCore/observers/Observer.h>
-#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
-
-namespace armarx
-{
- /**
- * @class ProsthesisObserverPropertyDefinitions
- * @brief
- */
- class ProsthesisObserverPropertyDefinitions:
- public armarx::ObserverPropertyDefinitions
- {
- public:
- ProsthesisObserverPropertyDefinitions(std::string prefix):
- armarx::ObserverPropertyDefinitions(prefix)
- {
- //defineRequiredProperty<std::string>("PropertyName", "Description");
- defineOptionalProperty<std::string>("ProsthesisTopicName", "ProsthesisMotorValues", "Name of the Prosthesis Topic");
- defineOptionalProperty<std::string>("DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
- }
- };
-
- /**
- * @defgroup Component-ProsthesisObserver ProsthesisObserver
- * @ingroup RobotAPI-Components
- * A description of the component ProsthesisObserver.
- *
- * @class ProsthesisObserver
- * @ingroup Component-ProsthesisObserver
- * @brief Brief description of class ProsthesisObserver.
- *
- * Detailed description of class ProsthesisObserver.
- */
- class ProsthesisObserver :
- virtual public Observer,
- virtual public ProsthesisObserverInterface
- {
- public:
- ProsthesisObserver() {}
-
- /**
- * @see armarx::ManagedIceObject::getDefaultName()
- */
- std::string getDefaultName() const override
- {
- return "ProsthesisObserver";
- }
-
- protected:
- /**
- * @see armarx::ManagedIceObject::onInitComponent()
- */
- void onInitObserver() override;
-
- /**
- * @see armarx::ManagedIceObject::onConnectComponent()
- */
- void onConnectObserver() override;
-
- /**
- * @see armarx::ManagedIceObject::onDisconnectComponent()
- */
- //void onDisconnectObserver() override;
-
- /**
- * @see armarx::ManagedIceObject::onExitComponent()
- */
- void onExitObserver() override;
-
- /**
- * @see PropertyUser::createPropertyDefinitions()
- */
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
-
- private:
-
- Mutex dataMutex;
- DebugDrawerInterfacePrx debugDrawerPrx;
-
- // ProsthesisListenerInterface interface
- public:
- void reportMotorValues(const ProsthesisMotorValues& motorValues, const Ice::Current&) override;
- };
-}
diff --git a/source/RobotAPI/components/ProsthesisObserver/test/CMakeLists.txt b/source/RobotAPI/components/ProsthesisObserver/test/CMakeLists.txt
deleted file mode 100644
index d89897a05dbb8e3df8051cf1d45bf8d7f3e9e8ed..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ProsthesisObserver/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore ProsthesisObserver)
-
-armarx_add_test(ProsthesisObserverTest ProsthesisObserverTest.cpp "${LIBS}")
\ No newline at end of file
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 697a5c50c01f83a36eb9de3108ae183809eedf93..de0dde42b8e8670e6c3953405ad59993eca07460 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -116,7 +116,7 @@ namespace armarx
{
ARMARX_VERBOSE << "Node: " << node->getName() << endl;
}
- usingTopic(robotNodeSetName + "State");
+ usingTopic(getProperty<std::string>("TopicPrefix").getValue() + robotNodeSetName + "State");
try
{
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index 817743c6d9c4ccc5e1ae79af641afc7a0c6df5dd..661b9d3375240cf2d83e51f1b57819315dd574f5 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -57,6 +57,8 @@ namespace armarx
defineOptionalProperty<std::string>("RobotStateReportingTopic", "RobotStateUpdates", "Name of the topic on which updates of the robot state are reported.");
defineOptionalProperty<int>("HistoryLength", 10000, "Number of entries in the robot state history");
defineOptionalProperty<float>("RobotModelScaling", 1.0f, "Scaling of the robot model");
+ defineOptionalProperty<std::string>("TopicPrefix", "", "Prefix for the sensor value topic name.");
+
}
};
@@ -164,14 +166,14 @@ namespace armarx
~RobotStateComponent() override;
// inherited from KinematicUnitInterface
- void reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current()) override;
- void reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current()) override;
- void reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current()) override;
- void reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current()) override;
+ void reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
void reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c) override;
- void reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current()) override;
- void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current()) override;
- void reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current()) override;
+ void reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
private:
void readRobotInfo(const std::string& robotFile);
diff --git a/source/RobotAPI/components/RobotState/SharedRobotServants.h b/source/RobotAPI/components/RobotState/SharedRobotServants.h
index 9d4d60cf2e36fe6caa9b091cf58ad80c0a4b6cb5..922588ddbdbf15100fa4a3e358a5abd232de3546 100644
--- a/source/RobotAPI/components/RobotState/SharedRobotServants.h
+++ b/source/RobotAPI/components/RobotState/SharedRobotServants.h
@@ -53,9 +53,9 @@ namespace armarx
virtual public SharedObjectInterface
{
public:
- void ref(const Ice::Current& c = Ice::Current()) override;
- void unref(const Ice::Current& c = Ice::Current()) override;
- void destroy(const Ice::Current& c = Ice::Current()) override;
+ void ref(const Ice::Current& c = Ice::emptyCurrent) override;
+ void unref(const Ice::Current& c = Ice::emptyCurrent) override;
+ void destroy(const Ice::Current& c = Ice::emptyCurrent) override;
SharedObjectBase();
private:
unsigned int _referenceCount;
@@ -74,32 +74,32 @@ namespace armarx
public SharedObjectBase
{
public:
- SharedRobotNodeServant(VirtualRobot::RobotNodePtr node /*,const Ice::Current & current = Ice::Current()*/);
+ SharedRobotNodeServant(VirtualRobot::RobotNodePtr node /*,const Ice::Current & current = Ice::emptyCurrent*/);
~SharedRobotNodeServant() override;
- float getJointValue(const Ice::Current& current = Ice::Current()) const override;
- std::string getName(const Ice::Current& current = Ice::Current()) const override;
+ float getJointValue(const Ice::Current& current = Ice::emptyCurrent) const override;
+ std::string getName(const Ice::Current& current = Ice::emptyCurrent) const override;
- PoseBasePtr getLocalTransformation(const Ice::Current& current = Ice::Current()) const override;
- FramedPoseBasePtr getGlobalPose(const Ice::Current& current = Ice::Current()) const override;
- FramedPoseBasePtr getPoseInRootFrame(const Ice::Current& current = Ice::Current()) const override;
+ PoseBasePtr getLocalTransformation(const Ice::Current& current = Ice::emptyCurrent) const override;
+ FramedPoseBasePtr getGlobalPose(const Ice::Current& current = Ice::emptyCurrent) const override;
+ FramedPoseBasePtr getPoseInRootFrame(const Ice::Current& current = Ice::emptyCurrent) const override;
- JointType getType(const Ice::Current& current = Ice::Current()) const override;
- Vector3BasePtr getJointTranslationDirection(const Ice::Current& current = Ice::Current()) const override;
- Vector3BasePtr getJointRotationAxis(const Ice::Current& current = Ice::Current()) const override;
+ JointType getType(const Ice::Current& current = Ice::emptyCurrent) const override;
+ Vector3BasePtr getJointTranslationDirection(const Ice::Current& current = Ice::emptyCurrent) const override;
+ Vector3BasePtr getJointRotationAxis(const Ice::Current& current = Ice::emptyCurrent) const override;
- bool hasChild(const std::string& name, bool recursive, const Ice::Current& current = Ice::Current()) const override;
- std::string getParent(const Ice::Current& current = Ice::Current()) const override;
- NameList getChildren(const Ice::Current& current = Ice::Current()) const override;
- NameList getAllParents(const std::string& name, const Ice::Current& current = Ice::Current()) const override;
+ bool hasChild(const std::string& name, bool recursive, const Ice::Current& current = Ice::emptyCurrent) const override;
+ std::string getParent(const Ice::Current& current = Ice::emptyCurrent) const override;
+ NameList getChildren(const Ice::Current& current = Ice::emptyCurrent) const override;
+ NameList getAllParents(const std::string& name, const Ice::Current& current = Ice::emptyCurrent) const override;
- float getJointValueOffest(const Ice::Current& current = Ice::Current()) const override;
- float getJointLimitHigh(const Ice::Current& current = Ice::Current()) const override;
- float getJointLimitLow(const Ice::Current& current = Ice::Current()) const override;
+ float getJointValueOffest(const Ice::Current& current = Ice::emptyCurrent) const override;
+ float getJointLimitHigh(const Ice::Current& current = Ice::emptyCurrent) const override;
+ float getJointLimitLow(const Ice::Current& current = Ice::emptyCurrent) const override;
- Vector3BasePtr getCoM(const Ice::Current& current = Ice::Current()) const override;
- Ice::FloatSeq getInertia(const Ice::Current& current = Ice::Current()) const override;
- float getMass(const Ice::Current& current = Ice::Current()) const override;
+ Vector3BasePtr getCoM(const Ice::Current& current = Ice::emptyCurrent) const override;
+ Ice::FloatSeq getInertia(const Ice::Current& current = Ice::emptyCurrent) const override;
+ float getMass(const Ice::Current& current = Ice::emptyCurrent) const override;
@@ -123,29 +123,29 @@ namespace armarx
SharedRobotServant(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStateComponent, RobotStateListenerInterfacePrx robotStateListenerPrx);
~SharedRobotServant() override;
void setRobotStateComponent(RobotStateComponentInterfacePrx robotStateComponent);
- SharedRobotNodeInterfacePrx getRobotNode(const std::string& name, const Ice::Current& current = Ice::Current()) override;
- SharedRobotNodeInterfacePrx getRootNode(const Ice::Current& current = Ice::Current()) override;
- bool hasRobotNode(const std::string& name, const Ice::Current& current = Ice::Current()) override;
+ SharedRobotNodeInterfacePrx getRobotNode(const std::string& name, const Ice::Current& current = Ice::emptyCurrent) override;
+ SharedRobotNodeInterfacePrx getRootNode(const Ice::Current& current = Ice::emptyCurrent) override;
+ bool hasRobotNode(const std::string& name, const Ice::Current& current = Ice::emptyCurrent) override;
- NameList getRobotNodes(const Ice::Current& current = Ice::Current()) override;
- RobotNodeSetInfoPtr getRobotNodeSet(const std::string& name, const Ice::Current& current = Ice::Current()) override;
- NameList getRobotNodeSets(const Ice::Current& current = Ice::Current()) override;
- bool hasRobotNodeSet(const std::string& name, const Ice::Current& current = Ice::Current()) override;
+ NameList getRobotNodes(const Ice::Current& current = Ice::emptyCurrent) override;
+ RobotNodeSetInfoPtr getRobotNodeSet(const std::string& name, const Ice::Current& current = Ice::emptyCurrent) override;
+ NameList getRobotNodeSets(const Ice::Current& current = Ice::emptyCurrent) override;
+ bool hasRobotNodeSet(const std::string& name, const Ice::Current& current = Ice::emptyCurrent) override;
- std::string getName(const Ice::Current& current = Ice::Current()) override;
- std::string getType(const Ice::Current& current = Ice::Current()) override;
+ std::string getName(const Ice::Current& current = Ice::emptyCurrent) override;
+ std::string getType(const Ice::Current& current = Ice::emptyCurrent) override;
- PoseBasePtr getGlobalPose(const Ice::Current& current = Ice::Current()) override;
- NameValueMap getConfig(const Ice::Current& current = Ice::Current()) override;
- NameValueMap getConfigAndPose(PoseBasePtr& globalPose, const Ice::Current& current = Ice::Current()) override;
- void setGlobalPose(const PoseBasePtr& pose, const Ice::Current& current = Ice::Current()) override;
+ PoseBasePtr getGlobalPose(const Ice::Current& current = Ice::emptyCurrent) override;
+ NameValueMap getConfig(const Ice::Current& current = Ice::emptyCurrent) override;
+ NameValueMap getConfigAndPose(PoseBasePtr& globalPose, const Ice::Current& current = Ice::emptyCurrent) override;
+ void setGlobalPose(const PoseBasePtr& pose, const Ice::Current& current = Ice::emptyCurrent) override;
- float getScaling(const Ice::Current& = Ice::Current()) override;
+ float getScaling(const Ice::Current& = Ice::emptyCurrent) override;
VirtualRobot::RobotPtr getRobot() const;
void setTimestamp(const IceUtil::Time& updateTime);
- TimestampBasePtr getTimestamp(const Ice::Current& = Ice::Current()) const override;
+ TimestampBasePtr getTimestamp(const Ice::Current& = Ice::emptyCurrent) const override;
RobotStateComponentInterfacePrx getRobotStateComponent(const Ice::Current&) const override;
protected:
VirtualRobot::RobotPtr _robot;
diff --git a/source/RobotAPI/components/ViewSelection/ViewSelection.h b/source/RobotAPI/components/ViewSelection/ViewSelection.h
index 414f566f4936a2536e3b82d0c3a72912cdd9d5cf..f8e6cbdc93a26500fef455bc989b2149bd1554b4 100644
--- a/source/RobotAPI/components/ViewSelection/ViewSelection.h
+++ b/source/RobotAPI/components/ViewSelection/ViewSelection.h
@@ -143,13 +143,13 @@ namespace armarx
*/
PropertyDefinitionsPtr createPropertyDefinitions() override;
- void addManualViewTarget(const FramedPositionBasePtr& target, const Ice::Current& c = Ice::Current()) override;
+ void addManualViewTarget(const FramedPositionBasePtr& target, const Ice::Current& c = Ice::emptyCurrent) override;
- void clearManualViewTargets(const Ice::Current& c = Ice::Current()) override;
+ void clearManualViewTargets(const Ice::Current& c = Ice::emptyCurrent) override;
- ViewTargetList getManualViewTargets(const Ice::Current& c = Ice::Current()) override;
+ ViewTargetList getManualViewTargets(const Ice::Current& c = Ice::emptyCurrent) override;
- void activateAutomaticViewSelection(const Ice::Current& c = Ice::Current()) override
+ void activateAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override
{
boost::mutex::scoped_lock lock(manualViewTargetsMutex);
@@ -159,7 +159,7 @@ namespace armarx
viewSelectionObserver->onActivateAutomaticViewSelection();
}
- void deactivateAutomaticViewSelection(const Ice::Current& c = Ice::Current()) override
+ void deactivateAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override
{
boost::mutex::scoped_lock lock(manualViewTargetsMutex);
@@ -169,22 +169,22 @@ namespace armarx
viewSelectionObserver->onDeactivateAutomaticViewSelection();
}
- bool isEnabledAutomaticViewSelection(const Ice::Current& c = Ice::Current()) override
+ bool isEnabledAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override
{
boost::mutex::scoped_lock lock(manualViewTargetsMutex);
return doAutomaticViewSelection;
}
- void updateSaliencyMap(const SaliencyMapBasePtr& map, const Ice::Current& c = ::Ice::Current()) override;
+ void updateSaliencyMap(const SaliencyMapBasePtr& map, const Ice::Current& c = Ice::emptyCurrent) override;
- void removeSaliencyMap(const std::string& name, const Ice::Current& c = ::Ice::Current()) override;
+ void removeSaliencyMap(const std::string& name, const Ice::Current& c = Ice::emptyCurrent) override;
- ::Ice::StringSeq getSaliencyMapNames(const Ice::Current& c = ::Ice::Current()) override;
+ ::Ice::StringSeq getSaliencyMapNames(const Ice::Current& c = Ice::emptyCurrent) override;
- void drawSaliencySphere(const ::Ice::StringSeq& names, const Ice::Current& c = ::Ice::Current()) override;
+ void drawSaliencySphere(const ::Ice::StringSeq& names, const Ice::Current& c = Ice::emptyCurrent) override;
- void clearSaliencySphere(const Ice::Current& c = ::Ice::Current()) override;
+ void clearSaliencySphere(const Ice::Current& c = Ice::emptyCurrent) override;
private:
diff --git a/source/RobotAPI/components/units/CMakeLists.txt b/source/RobotAPI/components/units/CMakeLists.txt
index a8fa129b5b5aa0da0aad734709048bd581094c9a..ce1deee815879ee57d182d4671d0e744ebff4d4e 100644
--- a/source/RobotAPI/components/units/CMakeLists.txt
+++ b/source/RobotAPI/components/units/CMakeLists.txt
@@ -24,6 +24,7 @@ set(LIB_HEADERS
GamepadUnitObserver.h
TCPControlUnit.h
TCPControlUnitObserver.h
+ GraspCandidateObserver.h
HandUnit.h
HandUnitSimulation.h
@@ -55,6 +56,7 @@ set(LIB_FILES
GamepadUnitObserver.cpp
TCPControlUnit.cpp
TCPControlUnitObserver.cpp
+ GraspCandidateObserver.cpp
HandUnit.cpp
HandUnitSimulation.cpp
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.cpp b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
index fe4074bbb71e9440ac1a914a014fa19f3baec52a..e896c60c2bc25183376659e1622cdb8905516d0e 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
@@ -131,7 +131,7 @@ void ForceTorqueObserver::visualizerFunction()
{
// if (localRobot->hasRobotNode(channel.first))
{
- DatafieldRefPtr field = DatafieldRefPtr::dynamicCast(getForceDatafield(channel.first, Ice::Current()));
+ DatafieldRefPtr field = DatafieldRefPtr::dynamicCast(getForceDatafield(channel.first, Ice::emptyCurrent));
FramedDirectionPtr value = field->getDataField()->get<FramedDirection>();
if (frameAlreadyReported.count(value->frame) > 0 || (value->frame != GlobalFrame && !value->frame.empty() && !localRobot->hasRobotNode(value->frame)))
@@ -153,7 +153,7 @@ void ForceTorqueObserver::visualizerFunction()
std::max(arrowLength * forceMag, 10.f),
3);
- field = DatafieldRefPtr::dynamicCast(getTorqueDatafield(channel.first, Ice::Current()));
+ field = DatafieldRefPtr::dynamicCast(getTorqueDatafield(channel.first, Ice::emptyCurrent));
value = field->getDataField()->get<FramedDirection>();
auto torque = value;
// ARMARX_INFO << deactivateSpam(1, torque->frame) << "Reporting for " << channel.first << " in frame " << torque->frame;
diff --git a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.h b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.h
index a52d64e37776d0d499af4099d7cd35f88a2e6b69..cc3115aa4d96c40baab5a7aed418abbe813db229 100644
--- a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.h
+++ b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.h
@@ -79,12 +79,12 @@ namespace armarx
/**
* \warning Not implemented yet
*/
- void setOffset(const FramedDirectionBasePtr& forceOffsets, const FramedDirectionBasePtr& torqueOffsets, const Ice::Current& c = ::Ice::Current()) override;
+ void setOffset(const FramedDirectionBasePtr& forceOffsets, const FramedDirectionBasePtr& torqueOffsets, const Ice::Current& c = Ice::emptyCurrent) override;
/**
* \warning Not implemented yet
*/
- void setToNull(const Ice::Current& c = ::Ice::Current()) override;
+ void setToNull(const Ice::Current& c = Ice::emptyCurrent) override;
/**
* \see PropertyUser::createPropertyDefinitions()
diff --git a/source/RobotAPI/components/units/GraspCandidateObserver.cpp b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f22b1dfde85d6a6b94a729face9ed4ccc272171d
--- /dev/null
+++ b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
@@ -0,0 +1,265 @@
+/**
+* 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 as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* 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 Lesser 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
+* @author Simon Ottenhaus
+* @copyright 2019 Humanoids Group, H2T, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "GraspCandidateObserver.h"
+
+//#include <ArmarXCore/core/checks/ConditionCheckEqualsPoseWithTolerance.h>
+#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
+#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
+#include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
+#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
+#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
+#include <RobotAPI/libraries/core/checks/ConditionCheckEqualsPoseWithTolerance.h>
+#include <RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#define TCP_POSE_CHANNEL "TCPPose"
+#define TCP_TRANS_VELOCITIES_CHANNEL "TCPVelocities"
+using namespace armarx;
+using namespace armarx::grasping;
+
+GraspCandidateObserver::GraspCandidateObserver()
+{
+}
+
+void GraspCandidateObserver::onInitObserver()
+{
+ usingTopic(getProperty<std::string>("GraspCandidatesTopicName").getValue());
+ offeringTopic(getProperty<std::string>("ConfigTopicName").getValue());
+
+
+}
+
+void GraspCandidateObserver::onConnectObserver()
+{
+ configTopic = getTopic<GraspCandidateProviderInterfacePrx>(getProperty<std::string>("ConfigTopicName").getValue());
+}
+
+PropertyDefinitionsPtr GraspCandidateObserver::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(new GraspCandidateObserverPropertyDefinitions(
+ getConfigIdentifier()));
+}
+
+bool GraspCandidateObserver::FilterMatches(const CandidateFilterConditionPtr& filter, const std::string& providerName, const GraspCandidatePtr& candidate)
+{
+ if (filter->providerName != "*" && filter->providerName != providerName)
+ {
+ return false;
+ }
+ if (filter->minimumSuccessProbability > candidate->graspSuccessProbability)
+ {
+ return false;
+ }
+ if (filter->approach != AnyApproach && (candidate->executionHints == 0 || filter->approach != candidate->executionHints->approach))
+ {
+ return false;
+ }
+ if (filter->preshape != AnyAperture && (candidate->executionHints == 0 || filter->preshape != candidate->executionHints->preshape))
+ {
+ return false;
+ }
+ if (filter->objectType != AnyObject && filter->objectType != candidate->objectType)
+ {
+ return false;
+ }
+ return true;
+}
+
+std::string GraspCandidateObserver::ObjectTypeToString(ObjectTypeEnum type)
+{
+ switch (type)
+ {
+ case grasping::AnyObject:
+ return "AnyObject";
+ case grasping::KnownObject:
+ return "KnownObject";
+ case grasping::UnknownObject:
+ return "UnknownObject";
+ default:
+ return "ERROR";
+ }
+}
+
+void GraspCandidateObserver::reportGraspCandidates(const std::string& providerName, const GraspCandidateSeq& candidates, const Ice::Current&)
+{
+ ScopedLock lock(dataMutex);
+ this->candidates[providerName] = candidates;
+ if (updateCounters.count(providerName) == 0)
+ {
+ updateCounters[providerName] = 0;
+ }
+ updateCounters[providerName]++;
+ if (providers.count(providerName) == 0)
+ {
+ providers[providerName] = new ProviderInfo();
+ }
+
+
+ if (!existsChannel(providerName))
+ {
+ offerChannel(providerName, "Channel of " + providerName);
+ }
+ offerOrUpdateDataField(providerName, "updateCounter", Variant(updateCounters[providerName]), "Counter that increases for each update");
+ offerOrUpdateDataField(providerName, "candidateCount", Variant((int)candidates.size()), "Number of provided candiates");
+
+}
+
+void GraspCandidateObserver::reportProviderInfo(const std::string& providerName, const ProviderInfoPtr& info, const Ice::Current&)
+{
+ ScopedLock lock(dataMutex);
+ providers[providerName] = info;
+ if (updateCounters.count(providerName) == 0)
+ {
+ updateCounters[providerName] = 0;
+ }
+
+
+ if (!existsChannel(providerName))
+ {
+ offerChannel(providerName, "Channel of " + providerName);
+ }
+ offerOrUpdateDataField(providerName, "objectType", ObjectTypeToString(info->objectType), "");
+}
+
+InfoMap GraspCandidateObserver::getAvailableProvidersWithInfo(const Ice::Current&)
+{
+ ScopedLock lock(dataMutex);
+ return providers;
+}
+
+StringSeq GraspCandidateObserver::getAvailableProviderNames(const Ice::Current&)
+{
+ ScopedLock lock(dataMutex);
+ return getAvailableProviderNames();
+}
+
+
+
+ProviderInfoPtr GraspCandidateObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
+{
+ ScopedLock lock(dataMutex);
+ checkHasProvider(providerName);
+ return providers[providerName];
+}
+
+bool GraspCandidateObserver::hasProvider(const std::string& providerName, const Ice::Current& c)
+{
+ ScopedLock lock(dataMutex);
+ return hasProvider(providerName);
+}
+
+
+
+GraspCandidateSeq GraspCandidateObserver::getAllCandidates(const Ice::Current&)
+{
+ ScopedLock lock(dataMutex);
+ GraspCandidateSeq all;
+ for (const std::pair<std::string, grasping::GraspCandidateSeq>& pair : candidates)
+ {
+ all.insert(all.end(), pair.second.begin(), pair.second.end());
+ }
+ return all;
+}
+
+GraspCandidateSeq GraspCandidateObserver::getCandidatesByProvider(const std::string& providerName, const Ice::Current& c)
+{
+ CandidateFilterConditionPtr filter = new CandidateFilterCondition();
+ filter->providerName = providerName;
+ return getCandidatesByFilter(filter, c);
+}
+
+GraspCandidateSeq GraspCandidateObserver::getCandidatesByFilter(const CandidateFilterConditionPtr& filter, const Ice::Current&)
+{
+ ScopedLock lock(dataMutex);
+ GraspCandidateSeq matching;
+ for (const std::pair<std::string, grasping::GraspCandidateSeq>& pair : candidates)
+ {
+ for (const grasping::GraspCandidatePtr& candidate : pair.second)
+ {
+ if (FilterMatches(filter, pair.first, candidate))
+ {
+ matching.push_back(candidate);
+ }
+ }
+ }
+ return matching;
+}
+
+Ice::Int GraspCandidateObserver::getUpdateCounterByProvider(const std::string& providerName, const Ice::Current&)
+{
+ ScopedLock lock(dataMutex);
+ checkHasProvider(providerName);
+ return updateCounters[providerName];
+}
+
+IntMap GraspCandidateObserver::getAllUpdateCounters(const Ice::Current& providerName)
+{
+ ScopedLock lock(dataMutex);
+ return updateCounters;
+}
+
+bool GraspCandidateObserver::setProviderConfig(const std::string& providerName, const StringVariantBaseMap& config, const Ice::Current&)
+{
+ ScopedLock lock(dataMutex);
+ if (providers.count(providerName) == 0)
+ {
+ return false;
+ }
+ configTopic->setServiceConfig(providerName, config);
+ return true;
+}
+
+void GraspCandidateObserver::setSelectedCandidates(const GraspCandidateSeq& candidates, const Ice::Current&)
+{
+ ScopedLock lock(selectedCandidatesMutex);
+ selectedCandidates = candidates;
+}
+
+GraspCandidateSeq GraspCandidateObserver::getSelectedCandidates(const Ice::Current&)
+{
+ ScopedLock lock(selectedCandidatesMutex);
+ return selectedCandidates;
+}
+
+
+bool GraspCandidateObserver::hasProvider(const std::string& providerName)
+{
+ return providers.count(providerName) > 0;
+}
+void GraspCandidateObserver::checkHasProvider(const std::string& providerName)
+{
+ if (!hasProvider(providerName))
+ {
+ throw LocalException("Unknown provider name '") << providerName << "'. Available providers: " << getAvailableProviderNames();
+ }
+}
+StringSeq GraspCandidateObserver::getAvailableProviderNames()
+{
+ StringSeq names;
+ for (const std::pair<std::string, ProviderInfoPtr>& pair : providers)
+ {
+ names.push_back(pair.first);
+ }
+ return names;
+}
diff --git a/source/RobotAPI/components/units/GraspCandidateObserver.h b/source/RobotAPI/components/units/GraspCandidateObserver.h
new file mode 100644
index 0000000000000000000000000000000000000000..7588252723921b1ce40903a6d7ccfbef6459ad01
--- /dev/null
+++ b/source/RobotAPI/components/units/GraspCandidateObserver.h
@@ -0,0 +1,114 @@
+/**
+* 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 as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* 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 Lesser 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
+* @author Simon Ottenhaus
+* @copyright 2019 Humanoids Group, H2T, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include <ArmarXCore/observers/Observer.h>
+#include <RobotAPI/interface/observers/GraspCandidateObserverInterface.h>
+
+namespace armarx
+{
+ /**
+ * \class GraspCandidateObserverPropertyDefinitions
+ * \brief
+ */
+ class GraspCandidateObserverPropertyDefinitions:
+ public ObserverPropertyDefinitions
+ {
+ public:
+ GraspCandidateObserverPropertyDefinitions(std::string prefix):
+ ObserverPropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<std::string>("GraspCandidatesTopicName", "GraspCandidatesTopic", "Name of the Grasp Candidate Topic");
+ defineOptionalProperty<std::string>("ConfigTopicName", "GraspCandidateProviderConfigTopic", "Name of the Grasp Candidate Provider Config Topic");
+ }
+ };
+
+ /**
+ * \class GraspCandidateObserver
+ * \ingroup RobotAPI-SensorActorUnits-observers
+ */
+ class GraspCandidateObserver :
+ virtual public Observer,
+ virtual public grasping::GraspCandidateObserverInterface
+ {
+ public:
+ GraspCandidateObserver();
+
+ // framework hooks
+ std::string getDefaultName() const override
+ {
+ return "GraspCandidateObserver";
+ }
+ void onInitObserver() override;
+ void onConnectObserver() override;
+
+ /**
+ * \see PropertyUser::createPropertyDefinitions()
+ */
+ PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ public:
+ static bool FilterMatches(const grasping::CandidateFilterConditionPtr& filter, const std::string& providerName, const grasping::GraspCandidatePtr& candidate);
+ static std::string ObjectTypeToString(grasping::ObjectTypeEnum type);
+
+ // GraspCandidateProviderListener interface
+ public:
+ void reportGraspCandidates(const std::string& providerName, const grasping::GraspCandidateSeq& candidates, const Ice::Current&) override;
+ void reportProviderInfo(const std::string& providerName, const grasping::ProviderInfoPtr& info, const Ice::Current&) override;
+
+ // GraspCandidateObserverInterface interface
+ public:
+ grasping::InfoMap getAvailableProvidersWithInfo(const Ice::Current&) override;
+ grasping::StringSeq getAvailableProviderNames(const Ice::Current&) override;
+ grasping::ProviderInfoPtr getProviderInfo(const std::string& providerName, const Ice::Current&) override;
+ bool hasProvider(const std::string& providerName, const Ice::Current& c) override;
+ grasping::GraspCandidateSeq getAllCandidates(const Ice::Current&) override;
+ grasping::GraspCandidateSeq getCandidatesByProvider(const std::string& providerName, const Ice::Current& c) override;
+ grasping::GraspCandidateSeq getCandidatesByFilter(const grasping::CandidateFilterConditionPtr& filter, const Ice::Current&) override;
+ Ice::Int getUpdateCounterByProvider(const std::string& providerName, const Ice::Current&) override;
+ grasping::IntMap getAllUpdateCounters(const Ice::Current& providerName) override;
+ bool setProviderConfig(const std::string& providerName, const StringVariantBaseMap& config, const Ice::Current&) override;
+
+ void setSelectedCandidates(const grasping::GraspCandidateSeq& candidates, const Ice::Current&) override;
+ grasping::GraspCandidateSeq getSelectedCandidates(const Ice::Current&) override;
+
+ private:
+ bool hasProvider(const std::string& providerName);
+ void checkHasProvider(const std::string& providerName);
+ grasping::StringSeq getAvailableProviderNames();
+ Mutex dataMutex;
+ std::map<std::string, grasping::GraspCandidateSeq> candidates;
+ grasping::InfoMap providers;
+ std::map<std::string, int> updateCounters;
+
+ grasping::GraspCandidateProviderInterfacePrx configTopic;
+
+ Mutex selectedCandidatesMutex;
+ grasping::GraspCandidateSeq selectedCandidates;
+
+
+ };
+
+}
+
diff --git a/source/RobotAPI/components/units/HandUnit.h b/source/RobotAPI/components/units/HandUnit.h
index 12d79c41b602ce701c2a473090edd8b0cb2479cb..6f7cc714ab242930d2a2a32d6022b77889a473bc 100644
--- a/source/RobotAPI/components/units/HandUnit.h
+++ b/source/RobotAPI/components/units/HandUnit.h
@@ -118,7 +118,7 @@ namespace armarx
/**
* Send command to the hand to form a specific shape position. The shapes are defined in the robot.xml file.
*/
- void setShape(const std::string& shapeName, const Ice::Current& c = ::Ice::Current()) override;
+ void setShape(const std::string& shapeName, const Ice::Current& c = Ice::emptyCurrent) override;
/**
* @brief setShapeWithObjectInstance Send command to the hand to form a specific shape position.
@@ -128,15 +128,15 @@ namespace armarx
* @param shapeName Name of the well known shape that the hand should form
* @param graspedObjectInstanceName name of the object instance which is used to check for collisions while setting the shape
*/
- void setShapeWithObjectInstance(const std::string& shapeName, const std::string& objectInstanceName, const Ice::Current& c = ::Ice::Current()) override;
+ void setShapeWithObjectInstance(const std::string& shapeName, const std::string& objectInstanceName, const Ice::Current& c = Ice::emptyCurrent) override;
/**
* \return a list of strings for shape positions which can be used together with HandUnit::shape().
*/
- SingleTypeVariantListBasePtr getShapeNames(const Ice::Current& c = ::Ice::Current()) override;
+ SingleTypeVariantListBasePtr getShapeNames(const Ice::Current& c = Ice::emptyCurrent) override;
- NameValueMap getShapeJointValues(const std::string& shapeName, const Ice::Current& c = ::Ice::Current()) override;
- NameValueMap getCurrentJointValues(const Ice::Current& c = ::Ice::Current()) override;
+ NameValueMap getShapeJointValues(const std::string& shapeName, const Ice::Current& c = Ice::emptyCurrent) override;
+ NameValueMap getCurrentJointValues(const Ice::Current& c = Ice::emptyCurrent) override;
void setObjectGrasped(const std::string& objectName, const Ice::Current&) override;
void setObjectReleased(const std::string& objectName, const Ice::Current&) override;
diff --git a/source/RobotAPI/components/units/HandUnitSimulation.h b/source/RobotAPI/components/units/HandUnitSimulation.h
index 1c02fd686c7a39512cd4ed6f732a8d879904a2ba..d5fff658f3e0c76a56669ef8e1b3fbddfd6a4d22 100644
--- a/source/RobotAPI/components/units/HandUnitSimulation.h
+++ b/source/RobotAPI/components/units/HandUnitSimulation.h
@@ -72,12 +72,12 @@ namespace armarx
*
* \warning Not implemented yet!
*/
- void setShape(const std::string& shapeName, const Ice::Current& c = ::Ice::Current()) override;
+ void setShape(const std::string& shapeName, const Ice::Current& c = Ice::emptyCurrent) override;
/**
* \warning Not implemented yet!
*/
- void setJointAngles(const NameValueMap& jointAngles, const Ice::Current& c = ::Ice::Current()) override;
+ void setJointAngles(const NameValueMap& jointAngles, const Ice::Current& c = Ice::emptyCurrent) override;
/**
* \see armarx::PropertyUser::createPropertyDefinitions()
diff --git a/source/RobotAPI/components/units/HapticObserver.h b/source/RobotAPI/components/units/HapticObserver.h
index e6b31b334d2b3b2bf6263d25c3e17918029810d8..5e75db5b05f74cf23cecb6ce3908f6f9ca1fd2c9 100644
--- a/source/RobotAPI/components/units/HapticObserver.h
+++ b/source/RobotAPI/components/units/HapticObserver.h
@@ -139,7 +139,7 @@ namespace armarx
void onConnectObserver() override;
void onExitObserver() override;
- void reportSensorValues(const ::std::string& device, const ::std::string& name, const ::armarx::MatrixFloatBasePtr& values, const ::armarx::TimestampBasePtr& timestamp, const ::Ice::Current& = ::Ice::Current()) override;
+ void reportSensorValues(const ::std::string& device, const ::std::string& name, const ::armarx::MatrixFloatBasePtr& values, const ::armarx::TimestampBasePtr& timestamp, const ::Ice::Current& = Ice::emptyCurrent) override;
/**
* \see PropertyUser::createPropertyDefinitions()
diff --git a/source/RobotAPI/components/units/HeadIKUnit.h b/source/RobotAPI/components/units/HeadIKUnit.h
index bbdd75e22205b8e8f4b966cac6543b3d2047a517..6783a2b2e1a7609b19bfae27a898e893879bc8f9 100644
--- a/source/RobotAPI/components/units/HeadIKUnit.h
+++ b/source/RobotAPI/components/units/HeadIKUnit.h
@@ -77,18 +77,18 @@ namespace armarx
}
// HeadIKUnitInterface interface
- void setCycleTime(Ice::Int milliseconds, const Ice::Current& c = Ice::Current()) override;
- void setHeadTarget(const std::string& robotNodeSetName, const FramedPositionBasePtr& targetPosition, const Ice::Current& c = Ice::Current()) override;
+ void setCycleTime(Ice::Int milliseconds, const Ice::Current& c = Ice::emptyCurrent) override;
+ void setHeadTarget(const std::string& robotNodeSetName, const FramedPositionBasePtr& targetPosition, const Ice::Current& c = Ice::emptyCurrent) override;
// UnitExecutionManagementInterface interface
- void init(const Ice::Current& c = Ice::Current()) override;
- void start(const Ice::Current& c = Ice::Current()) override;
- void stop(const Ice::Current& c = Ice::Current()) override;
- UnitExecutionState getExecutionState(const Ice::Current& c = Ice::Current()) override;
+ void init(const Ice::Current& c = Ice::emptyCurrent) override;
+ void start(const Ice::Current& c = Ice::emptyCurrent) override;
+ void stop(const Ice::Current& c = Ice::emptyCurrent) override;
+ UnitExecutionState getExecutionState(const Ice::Current& c = Ice::emptyCurrent) override;
// UnitResourceManagementInterface interface
- void request(const Ice::Current& c = Ice::Current()) override;
- void release(const Ice::Current& c = Ice::Current()) override;
+ void request(const Ice::Current& c = Ice::emptyCurrent) override;
+ void release(const Ice::Current& c = Ice::emptyCurrent) override;
// PropertyUser interface
PropertyDefinitionsPtr createPropertyDefinitions() override;
diff --git a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
index 3f387568df7d54c2b6c93ea27852269ff39ba562..e5ae05a54c63e5fd262a7c0025df9d54a7ffdf24 100644
--- a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
+++ b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
@@ -23,6 +23,7 @@
*/
#include "InertialMeasurementUnitObserver.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
@@ -66,7 +67,9 @@ void InertialMeasurementUnitObserver::onExitObserver()
}
}
-void InertialMeasurementUnitObserver::reportSensorValues(const std::string& device, const std::string& name, const IMUData& values, const TimestampBasePtr& timestamp, const Ice::Current& c)
+void InertialMeasurementUnitObserver::reportSensorValues(
+ const std::string& device, const std::string& name, const IMUData& values,
+ const TimestampBasePtr& timestamp, const Ice::Current&)
{
ScopedLock lock(dataMutex);
diff --git a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
index 1ceba2cf5f084d2f2f47e2ecf4fd0ec5e5a14679..4591228d92d457967b85106554898345895b6ee2 100644
--- a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
+++ b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
@@ -73,7 +73,7 @@ namespace armarx
void onConnectObserver() override;
void onExitObserver() override;
- void reportSensorValues(const std::string& device, const std::string& name, const IMUData& values, const TimestampBasePtr& timestamp, const Ice::Current& c = ::Ice::Current()) override;
+ void reportSensorValues(const std::string& device, const std::string& name, const IMUData& values, const TimestampBasePtr& timestamp, const Ice::Current& c = Ice::emptyCurrent) override;
/**
* @see PropertyUser::createPropertyDefinitions()
diff --git a/source/RobotAPI/components/units/KinematicUnit.cpp b/source/RobotAPI/components/units/KinematicUnit.cpp
index 085b96d83e2f8c67c9a522effc037d981096922f..0348c59145cf662d46e8d3b17bab1be247e8bb89 100644
--- a/source/RobotAPI/components/units/KinematicUnit.cpp
+++ b/source/RobotAPI/components/units/KinematicUnit.cpp
@@ -99,7 +99,7 @@ void KinematicUnit::onInitComponent()
robotNodes = robotNodeSetPtr->getAllRobotNodes();
// component dependencies
- listenerName = robotNodeSetName + "State";
+ listenerName = getProperty<std::string>("TopicPrefix").getValue() + robotNodeSetName + "State";
offeringTopic(listenerName);
this->onInitKinematicUnit();
diff --git a/source/RobotAPI/components/units/KinematicUnit.h b/source/RobotAPI/components/units/KinematicUnit.h
index 40be0bf28f23c852c778ea866405f3a84ee1e5a2..67b896fc0f4640ecf5c276afa68104184bc17b14 100644
--- a/source/RobotAPI/components/units/KinematicUnit.h
+++ b/source/RobotAPI/components/units/KinematicUnit.h
@@ -51,6 +51,7 @@ namespace armarx
defineRequiredProperty<std::string>("RobotNodeSetName", "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
defineRequiredProperty<std::string>("RobotFileName", "Robot file name, e.g. robot_model.xml");
defineOptionalProperty<std::string>("RobotFileNameProject", "", "Project in which the robot filename is located (if robot is loaded from an external project)");
+ defineOptionalProperty<std::string>("TopicPrefix", "", "Prefix for the sensor value topic name.");
}
};
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.cpp b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
index bdf8dc94e2068fb373f5f5984604d262bb78d1a1..ea9b8da615bdeaccbb5cd505caf8f528ef40241a 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
@@ -56,7 +56,7 @@ void KinematicUnitObserver::onInitObserver()
offerConditionCheck("larger", new ConditionCheckLarger());
offerConditionCheck("smaller", new ConditionCheckSmaller());
- usingTopic(robotNodeSetName + "State");
+ usingTopic(getProperty<std::string>("TopicPrefix").getValue() + robotNodeSetName + "State");
}
void KinematicUnitObserver::onConnectObserver()
@@ -91,11 +91,12 @@ void KinematicUnitObserver::onConnectObserver()
throw UserException("RobotNodeSet not defined");
}
- VirtualRobot::RobotNodeSetPtr robotNodeSetPtr = robot->getRobotNodeSet(robotNodeSetName);
+ auto robotNodeSetPtr = robot->getRobotNodeSet(robotNodeSetName);
std::vector< VirtualRobot::RobotNodePtr > robotNodes;
robotNodes = robotNodeSetPtr->getAllRobotNodes();
-
+ auto robotNodeNames = robotNodeSetPtr->getNodeNames();
+ this->robotNodes = std::set<std::string>(robotNodeNames.begin(), robotNodeNames.end());
// register all channels
offerChannel("jointangles", "Joint values of the " + robotNodeSetName + " kinematic chain");
offerChannel("jointvelocities", "Joint velocities of the " + robotNodeSetName + " kinematic chain");
@@ -298,14 +299,20 @@ void KinematicUnitObserver::nameValueMapToDataFields(const std::string& channelN
{
for (const auto& it : nameValueMap)
{
- map[it.first] = new Variant(it.second);
+ if (robotNodes.count(it.first))
+ {
+ map[it.first] = new Variant(it.second);
+ }
}
}
else
{
for (const auto& it : nameValueMap)
{
- map[it.first] = new TimedVariant(new Variant(it.second), IceUtil::Time::microSeconds(timestamp));
+ if (robotNodes.count(it.first))
+ {
+ map[it.first] = new TimedVariant(new Variant(it.second), IceUtil::Time::microSeconds(timestamp));
+ }
}
}
setDataFieldsFlatCopy(channelName, map);
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.h b/source/RobotAPI/components/units/KinematicUnitObserver.h
index 1f306d6863f85f203d9cd24db6486494ff15fd8f..92cf7f72095ebbe825087379ad1c662cc0e0cc7f 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.h
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.h
@@ -48,6 +48,8 @@ namespace armarx
defineRequiredProperty<std::string>("RobotNodeSetName", "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
defineRequiredProperty<std::string>("RobotFileName", "Robot file name, e.g. robot_model.xml");
defineOptionalProperty<std::string>("RobotFileNameProject", "", "Project in which the robot filename is located (if robot is loaded from an external project)");
+ defineOptionalProperty<std::string>("TopicPrefix", "", "Prefix for the sensor value topic name.");
+
}
};
@@ -75,14 +77,14 @@ namespace armarx
// slice interface implementation
- void reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current()) override;
- void reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current()) override;
- void reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current()) override;
- void reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current()) override;
+ void reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
void reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c) override;
- void reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current()) override;
- void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current()) override;
- void reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current()) override;
+ void reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
std::string getDefaultName() const override
{
@@ -155,6 +157,7 @@ namespace armarx
Mutex initializedChannelsMutex;
private:
std::string robotNodeSetName;
+ std::set<std::string> robotNodes;
};
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.h b/source/RobotAPI/components/units/KinematicUnitSimulation.h
index 7a04769c70c74fcfe8f679e7d9b272b84dcf70d7..cc3dae20ac7476572a4250c384a6395c7fd7d61b 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.h
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.h
@@ -148,26 +148,26 @@ namespace armarx
void simulationFunction();
// proxy implementation
- void requestJoints(const Ice::StringSeq& joints, const Ice::Current& c = ::Ice::Current()) override;
- void releaseJoints(const Ice::StringSeq& joints, const Ice::Current& c = ::Ice::Current()) override;
- void switchControlMode(const NameControlModeMap& targetJointModes, const Ice::Current& c = ::Ice::Current()) override;
- void setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& c = ::Ice::Current()) override;
- void setJointVelocities(const NameValueMap& targetJointVelocities, const Ice::Current& c = ::Ice::Current()) override;
- void setJointTorques(const NameValueMap& targetJointTorques, const Ice::Current& c = ::Ice::Current()) override;
+ void requestJoints(const Ice::StringSeq& joints, const Ice::Current& c = Ice::emptyCurrent) override;
+ void releaseJoints(const Ice::StringSeq& joints, const Ice::Current& c = Ice::emptyCurrent) override;
+ void switchControlMode(const NameControlModeMap& targetJointModes, const Ice::Current& c = Ice::emptyCurrent) override;
+ void setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& c = Ice::emptyCurrent) override;
+ void setJointVelocities(const NameValueMap& targetJointVelocities, const Ice::Current& c = Ice::emptyCurrent) override;
+ void setJointTorques(const NameValueMap& targetJointTorques, const Ice::Current& c = Ice::emptyCurrent) override;
- NameControlModeMap getControlModes(const Ice::Current& c = ::Ice::Current()) override;
+ NameControlModeMap getControlModes(const Ice::Current& c = Ice::emptyCurrent) override;
/**
* \warning Not implemented yet!
*/
- void setJointAccelerations(const NameValueMap& targetJointAccelerations, const Ice::Current& c = ::Ice::Current()) override;
+ void setJointAccelerations(const NameValueMap& targetJointAccelerations, const Ice::Current& c = Ice::emptyCurrent) override;
/**
* \warning Not implemented yet!
*/
- void setJointDecelerations(const NameValueMap& targetJointDecelerations, const Ice::Current& c = ::Ice::Current()) override;
+ void setJointDecelerations(const NameValueMap& targetJointDecelerations, const Ice::Current& c = Ice::emptyCurrent) override;
- void stop(const Ice::Current& c = Ice::Current()) override;
+ void stop(const Ice::Current& c = Ice::emptyCurrent) override;
/**
* \see PropertyUser::createPropertyDefinitions()
diff --git a/source/RobotAPI/components/units/OptoForceUnitObserver.h b/source/RobotAPI/components/units/OptoForceUnitObserver.h
index 1fcdb266986b47dc46f1dd135ffe9b964890961d..3bdc63e1a6b874cb9005260801c8c11869b009c7 100644
--- a/source/RobotAPI/components/units/OptoForceUnitObserver.h
+++ b/source/RobotAPI/components/units/OptoForceUnitObserver.h
@@ -72,7 +72,7 @@ namespace armarx
void onConnectObserver() override;
void onExitObserver() override;
- void reportSensorValues(const std::string& device, const std::string& name, float fx, float fy, float fz, const TimestampBasePtr& timestamp, const Ice::Current& c = ::Ice::Current()) override;
+ void reportSensorValues(const std::string& device, const std::string& name, float fx, float fy, float fz, const TimestampBasePtr& timestamp, const Ice::Current& c = Ice::emptyCurrent) override;
/**
* @see PropertyUser::createPropertyDefinitions()
diff --git a/source/RobotAPI/components/units/PlatformUnit.h b/source/RobotAPI/components/units/PlatformUnit.h
index 119f4a2249b94538ed25abb4dbb173432e595d5d..5bedf16ac7a782f2c6c17d4fad32792d3700e99f 100644
--- a/source/RobotAPI/components/units/PlatformUnit.h
+++ b/source/RobotAPI/components/units/PlatformUnit.h
@@ -105,9 +105,9 @@ namespace armarx
* The platform will move until it reaches the specified target with the specified accuracy.
*/
void moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation,
- Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c = ::Ice::Current()) override;
+ Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c = Ice::emptyCurrent) override;
- void stopPlatform(const Ice::Current& c = Ice::Current()) override {}
+ void stopPlatform(const Ice::Current& c = Ice::emptyCurrent) override {}
/**
* \see armarx::PropertyUser::createPropertyDefinitions()
*/
diff --git a/source/RobotAPI/components/units/PlatformUnitObserver.h b/source/RobotAPI/components/units/PlatformUnitObserver.h
index 47d86bbefffc0824ea357be9fdaf5a9af25e321c..412b9c57f942a48789c833773c01c5f9d6927331 100644
--- a/source/RobotAPI/components/units/PlatformUnitObserver.h
+++ b/source/RobotAPI/components/units/PlatformUnitObserver.h
@@ -78,9 +78,9 @@ namespace armarx
void onConnectObserver() override;
// slice interface implementation
- void reportPlatformPose(::Ice::Float currentPlatformPositionX, ::Ice::Float currentPlatformPositionY, ::Ice::Float currentPlatformRotation, const Ice::Current& c = ::Ice::Current()) override;
- void reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c = ::Ice::Current()) override;
- void reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c = ::Ice::Current()) override;
+ void reportPlatformPose(::Ice::Float currentPlatformPositionX, ::Ice::Float currentPlatformPositionY, ::Ice::Float currentPlatformRotation, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c = Ice::emptyCurrent) override;
void reportPlatformOdometryPose(Ice::Float x, Ice::Float y, Ice::Float angle, const Ice::Current&) override;
std::string getDefaultName() const override
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.h b/source/RobotAPI/components/units/PlatformUnitSimulation.h
index a4ba66acbd58aa3c740471b79bfcbf658953f5c6..8c7743a7f940c74860597157a46beb16b4de6794 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.h
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.h
@@ -83,16 +83,16 @@ namespace armarx
void simulationFunction();
// proxy implementation
- void moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c = ::Ice::Current()) override;
+ void moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c = Ice::emptyCurrent) override;
/**
* \warning Not yet implemented!
*/
- void move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation, const Ice::Current& c = Ice::Current()) override;
+ void move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation, const Ice::Current& c = Ice::emptyCurrent) override;
- void moveRelative(float targetPlatformOffsetX, float targetPlatformOffsetY, float targetPlatformOffsetRotation, float positionalAccuracy, float orientationalAccuracy, const Ice::Current& c = Ice::Current()) override;
- void setMaxVelocities(float positionalVelocity, float orientaionalVelocity, const Ice::Current& c = Ice::Current()) override;
- void stopPlatform(const Ice::Current& c = Ice::Current()) override;
+ void moveRelative(float targetPlatformOffsetX, float targetPlatformOffsetY, float targetPlatformOffsetRotation, float positionalAccuracy, float orientationalAccuracy, const Ice::Current& c = Ice::emptyCurrent) override;
+ void setMaxVelocities(float positionalVelocity, float orientaionalVelocity, const Ice::Current& c = Ice::emptyCurrent) override;
+ void stopPlatform(const Ice::Current& c = Ice::emptyCurrent) override;
/**
* \see PropertyUser::createPropertyDefinitions()
*/
diff --git a/source/RobotAPI/components/units/RobotPoseUnit.h b/source/RobotAPI/components/units/RobotPoseUnit.h
index d620765925628a8820355ab2c45dc7f8ec828b7b..fb8e12eaf71b99496001d626a461750e8fba2763 100644
--- a/source/RobotAPI/components/units/RobotPoseUnit.h
+++ b/source/RobotAPI/components/units/RobotPoseUnit.h
@@ -107,9 +107,9 @@ namespace armarx
* @param postionalAccuracy Robot stops translating if distance to target position gets lower than this threshhold.
* @param orientationalAccuracy Robot stops rotating if distance from current to target orientation gets lower than this threshhold.
**/
- virtual void moveTo(PoseBasePtr targetPose, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c = ::Ice::Current());
+ virtual void moveTo(PoseBasePtr targetPose, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c = Ice::emptyCurrent);
- void stopMovement(const Ice::Current& c = Ice::Current()) override {}
+ void stopMovement(const Ice::Current& c = Ice::emptyCurrent) override {}
/**
* \see armarx::PropertyUser::createPropertyDefinitions()
*/
diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
index 950db1b8cb86ce96affe1e0fec2c9d90be72d3d2..61115c4e124cf52f73255721f53f199df8bd52f0 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
@@ -26,7 +26,7 @@
#include <ArmarXCore/core/util/algorithm.h>
#include <ArmarXCore/core/logging/Logging.h>
-
+#include "util/CtrlUtil.h"
namespace armarx
{
float velocityControlWithAccelerationBounds(
@@ -353,6 +353,81 @@ namespace armarx
// return targetV; // TODO vel can't be larger than 3.14??
}
+ bool VelocityControllerWithRampedAcceleration::validParameters() const
+ {
+ return maxV > 0 &&
+ jerk > 0 &&
+ targetV <= maxV &&
+ targetV >= -maxV;
+ }
+
+ VelocityControllerWithRampedAcceleration::Output VelocityControllerWithRampedAcceleration::run() const
+ {
+ const double useddt = std::min(std::abs(dt), std::abs(maxDt));
+
+ //we can have 3 cases:
+ // 1. we directly set v and ignore acc/dec (if |curr - target| <= limit)
+ // 2. we need to accelerate (if curr and target v have same sign and |curr| < |target|)
+ // 3. we need to decelerate (other cases)
+ //handle case 1
+ const double clampedTargetV = math::MathUtils::LimitTo(targetV, maxV);
+ const double curverror = clampedTargetV - currentV;
+ if (std::abs(curverror) < directSetVLimit)
+ {
+ double nextAcc = (clampedTargetV - currentV) / useddt;
+ double nextJerk = (nextAcc - currentAcc) / useddt;
+ Output result {clampedTargetV, nextAcc, nextJerk};
+
+ return result;
+ }
+
+ //handle case 2 + 3
+ // const bool accelerate = sign(clampedTargetV) == sign(currentV) && // v in same direction
+ // std::abs(clampedTargetV) > std::abs(currentV); // currently to slow
+ const auto goalDir = math::MathUtils::Sign(clampedTargetV - currentV);
+ // calculate if we need to increase or decrease acc
+ bool increaseAcc = true;
+ if (goalDir == math::MathUtils::Sign(currentAcc))
+ {
+ // double t_to_v = ctrlutil::t_to_v(clampedTargetV - currentV, currentAcc, goalDir * jerk);
+ // double acc_at_t = ctrlutil::a(t_to_v, std::abs(currentAcc), -jerk);
+ // increaseJerk = acc_at_t < 0.0;
+
+ double t_to_zero_acc = std::abs(currentAcc) / jerk;
+ double v_at_t = ctrlutil::v(t_to_zero_acc, currentV, currentAcc, -goalDir * jerk);
+ increaseAcc = math::MathUtils::Sign(v_at_t - clampedTargetV) != goalDir ;
+ // ARMARX_INFO << VAROUT(t_to_zero_acc) << VAROUT(v_at_t) << VAROUT(increaseAcc);
+ }
+ // v = a*a/(2j)
+ const double adjustedJerk = std::abs(currentAcc * currentAcc / (2 * (clampedTargetV - currentV)));
+ double usedJerk = increaseAcc ? goalDir * jerk :
+ -goalDir *
+ ((std::abs(currentV) > 0.01 && !std::isnan(adjustedJerk)) ? adjustedJerk : jerk);
+ // double t_to_target_v = ctrlutil::t_to_v(clampedTargetV - currentV, currentAcc, usedJerk);
+ // double v_at_t2 = ctrlutil::v(t_to_target_v, currentV, currentAcc, usedJerk);
+ // ctrlutil::t_to_v(currentV, currentAcc, jerk);
+ // const double usedacc = currentAcc + useddt * usedJerk;
+ // const double maxDeltaVSimple = usedacc * useddt;
+
+ const double maxDeltaV = std::abs(ctrlutil::v(useddt, 0, currentAcc, usedJerk));
+ if (maxDeltaV >= std::abs(curverror))
+ {
+ //we change our v too much with the used acceleration
+ //but we could reach our target with a lower acceleration ->set the target
+ double nextAcc = (clampedTargetV - currentV) / useddt;
+ double nextJerk = (nextAcc - currentAcc) / useddt;
+ // ARMARX_INFO << "overshooting! " << VAROUT(clampedTargetV) << VAROUT(nextAcc) << VAROUT(nextJerk);
+ Output result {clampedTargetV, nextAcc, nextJerk};
+ return result;
+ }
+ // const double deltaVel = boost::algorithm::clamp(sign(currentV) * usedacc * useddt, -maxDeltaV, maxDeltaV);
+ const double nextV = ctrlutil::v(useddt, currentV, currentAcc, usedJerk);
+ const double nextAcc = ctrlutil::a(useddt, currentAcc, usedJerk);
+ // ARMARX_INFO << VAROUT(clampedTargetV) << VAROUT(nextV) << VAROUT(nextAcc) << VAROUT(usedJerk);
+ Output result {nextV, nextAcc, usedJerk};
+ return result;
+ }
+
float VelocityControllerWithAccelerationBounds::estimateTime() const
{
const float curverror = std::abs(targetV - currentV);
@@ -363,6 +438,8 @@ namespace armarx
return curverror / ((targetV > currentV) ? acceleration : deceleration);
}
+
+
bool VelocityControllerWithAccelerationAndPositionBounds::validParameters() const
{
return VelocityControllerWithAccelerationBounds::validParameters() &&
@@ -445,8 +522,23 @@ namespace armarx
#ifdef DEBUG_POS_CTRL
buffer = boost::circular_buffer<HelpStruct>(20);
#endif
+ pid.reset(new PIDController(1, 0, 0));
+ pid->threadSafe = false;
+ }
+
+ float PositionThroughVelocityControllerWithAccelerationBounds::calculateProportionalGain() const
+ {
+ /* s = v_0*v_0/(2*a) -> sqrt(s*2*a) = v_0;
+
+ K_p * error = v_0; -> v_0/error = K_p;
+ */
+ auto v_0 = std::sqrt(pControlPosErrorLimit * 2 * deceleration);
+ return v_0 / pControlPosErrorLimit;
}
+
+
+
bool PositionThroughVelocityControllerWithAccelerationBounds::validParameters() const
{
return maxV > 0 &&
@@ -454,7 +546,7 @@ namespace armarx
deceleration > 0 &&
// pControlPosErrorLimit > 0 &&
// pControlVelLimit > 0 &&
- p > 0;
+ pid->Kp > 0;
}
float PositionThroughVelocityControllerWithAccelerationBounds::run() const
@@ -470,7 +562,8 @@ namespace armarx
//handle case 1
const float positionError = targetPosition - currentPosition;
- float newTargetVelPController = positionError * p;
+ pid->update((double)useddt, (double)currentPosition, (double)targetPosition);
+ float newTargetVelPController = pid->getControlValue();
//handle case 2-3
const float brakingDistance = signV * currentV * currentV / 2.f / deceleration; //the braking distance points in the direction of the velocity
@@ -490,10 +583,15 @@ namespace armarx
const float usedacc = decelerate ? -usedDeceleration : acceleration;
const float deltaVel = signV * usedacc * useddt;
float newTargetVelRampCtrl = boost::algorithm::clamp(currentV + deltaVel, -maxV, maxV);
- currentlyPIDActive = std::abs(newTargetVelPController) < std::abs(newTargetVelRampCtrl)
- || std::abs(newTargetVelPController) < pControlVelLimit
- || std::abs(positionError) < pControlPosErrorLimit;
- float finalTargetVel = (currentlyPIDActive && p > 0.0f) ? newTargetVelPController : newTargetVelRampCtrl;
+ bool PIDActive = /*std::abs(newTargetVelPController) < std::abs(newTargetVelRampCtrl)
+ || std::abs(newTargetVelPController) < pControlVelLimit ||*/
+ std::abs(positionError) < pControlPosErrorLimit;
+ // if (currentlyPIDActive != PIDActive && PIDActive)
+ // {
+ // ARMARX_INFO << "Switching to PID mode: " << VAROUT(positionError) << VAROUT(newTargetVelPController) << VAROUT(newTargetVelRampCtrl);
+ // }
+ this->currentlyPIDActive = PIDActive;
+ float finalTargetVel = (currentlyPIDActive) ? newTargetVelPController : newTargetVelRampCtrl;
if (std::abs(positionError) < accuracy)
{
return 0.0;// if close to target set zero velocity to avoid oscillating around target
@@ -603,7 +701,6 @@ namespace armarx
// and the brakingDistance have the same sign and brakingDistance < e
// and currentVel <= maxV)
// 5. we need to decelerate (other cases)
-
//handle case 1
const float vsquared = currentV * currentV;
const float brakingDistance = signV * vsquared / 2.f / deceleration; //the braking distance points in the direction of the velocity
@@ -776,13 +873,690 @@ namespace armarx
return sub.run();
}
+ double PositionThroughVelocityControllerWithAccelerationRamps::getTargetPosition() const
+ {
+ return targetPosition;
+ }
+
bool PositionThroughVelocityControllerWithAccelerationBounds::getCurrentlyPIDActive() const
{
return currentlyPIDActive;
}
+ void PositionThroughVelocityControllerWithAccelerationRamps::setTargetPosition(double value)
+ {
+ if (std::abs(value - targetPosition) > 0.0001)
+ {
+ state = State::Unknown;
+ }
+ targetPosition = value;
+ }
+
+ PositionThroughVelocityControllerWithAccelerationRamps::PositionThroughVelocityControllerWithAccelerationRamps()
+ {
+
+ }
+
+ bool PositionThroughVelocityControllerWithAccelerationRamps::validParameters() const
+ {
+ return maxV > 0 &&
+ acceleration > 0 &&
+ deceleration > 0 &&
+ jerk > 0 &&
+ // pControlPosErrorLimit > 0 &&
+ // pControlVelLimit > 0 &&
+ p > 0;
+ }
+
+ PositionThroughVelocityControllerWithAccelerationRamps::Output PositionThroughVelocityControllerWithAccelerationRamps::run()
+ {
+ PositionThroughVelocityControllerWithAccelerationRamps::Output result;
+ const double useddt = std::min(std::abs(dt), std::abs(maxDt));
+ const double signV = sign(currentV);
+ //we can have 3 cases:
+ // 1. we use a p controller and ignore acc/dec (if |currentPosition - targetPosition| <= pControlPosErrorLimit AND |currentV| < pControlVelLimit)
+ // 2. we need to accelerate (or hold vel) (if e = (targetPosition - currentPosition)
+ // the brakingDistance have the same sign and brakingDistance < e
+ // and currentVel <= maxV)
+ // 3. we need to decelerate (other cases)
+
+ //handle case 1
+ const double positionError = targetPosition - currentPosition;
+ double newTargetVelPController = (positionError * p) * 0.5 + currentV * 0.5;
+
+ //handle case 2-3
+ auto brData = ctrlutil::braking_distance_with_wedge(currentV, currentAcc, jerk);
+ if (brData.dt2 < 0)
+ {
+ brData = ctrlutil::braking_distance_with_wedge(currentV, currentAcc, jerk * 10);
+ }
+ const auto goalDir = math::MathUtils::Sign(targetPosition - currentPosition);
+ // State currentState;
+ double usedAbsJerk;
+ State newState;
+ Output output;
+ std::tie(newState, output) = calcState();
+ usedAbsJerk = output.jerk;
+ // double newJerk = output.jerk;
+ // double newAcceleration = output.acceleration;
+ // ARMARX_INFO << "state: " << (int)state << " new state: " << (int)newState;
+ state = newState;
+ const double brakingDistance = brData.dt2 >= 0 ? brData.s_total : signV * currentV * currentV / 2.f / deceleration; //the braking distance points in the direction of the velocity
+ const double posIfBrakingNow = currentPosition + brakingDistance;
+ const double posErrorIfBrakingNow = targetPosition - posIfBrakingNow;
+ const bool hardBrakingNeeded = std::abs(brakingDistance) > std::abs(positionError);
+ // const double usedJerk = jerk;//currentState==State::DecAccDecJerk?std::abs(ctrlutil::jerk(ctrlutil::t_to_v(currentV, -signV*currentAcc, signV*jerk), positionError, currentV, currentAcc))
+ // :jerk;
+ const double jerkDir = (newState == State::DecAccIncJerk || newState == State::IncAccDecJerk) ? -1.0f : 1.0f;
+ const double usedJerk = goalDir * jerkDir * usedAbsJerk;
+ const double deltaAcc = usedJerk * useddt;
+ const double newAcceleration = (newState == State::Keep) ? currentAcc : (newState == State::DecAccDecJerk ? output.acceleration : currentAcc + deltaAcc);
+ result.acceleration = newAcceleration;
+ result.jerk = usedJerk;
+ const double usedDeceleration = hardBrakingNeeded ?
+ -std::abs(currentV * currentV / 2.f / math::MathUtils::LimitTo(positionError, 0.0001)) :
+ newAcceleration;
+ const bool decelerate =
+ std::abs(currentV) > maxV || // we need to slow down (to stay in [-maxV,maxV]
+ hardBrakingNeeded ||
+ sign(posErrorIfBrakingNow) != signV; // we are moving away from the target
+ const double deltaVel = ctrlutil::v(useddt, 0, newAcceleration, usedJerk);//signV * newAcceleration * useddt;
+ double newTargetVelRampCtrl = ctrlutil::v(useddt, currentV, currentAcc, usedJerk);//boost::algorithm::clamp(currentV + deltaVel, -maxV, maxV);
+ newTargetVelRampCtrl = boost::algorithm::clamp(newTargetVelRampCtrl, -maxV, maxV);
+ bool usePID = std::abs(newTargetVelPController) < std::abs(newTargetVelRampCtrl); //||
+ // std::abs(newTargetVelPController) < pControlVelLimit &&
+ //std::abs(positionError) < pControlPosErrorLimit;
+ usePID |= state == State::PCtrl;
+ usePID &= usePIDAtEnd;
+ if (usePID)
+ {
+ state = State::PCtrl;
+ }
+ double finalTargetVel = usePID ? newTargetVelPController : newTargetVelRampCtrl;
+ ARMARX_INFO << VAROUT(usePID) << VAROUT(result.jerk) << VAROUT(newTargetVelPController) << VAROUT(currentV);
+ // ARMARX_INFO << VAROUT(currentV) << VAROUT(currentAcc) << VAROUT(usedJerk) << VAROUT(newAcceleration) << VAROUT(deltaVel) << VAROUT(finalTargetVel) << " new distance: " << (targetPosition - ctrlutil::s(dt, currentPosition, currentV, currentAcc, usedJerk));
+ if (std::abs(positionError) < accuracy)
+ {
+ finalTargetVel = 0.0f; // if close to target set zero velocity to avoid oscillating around target
+ }
+
+#ifdef DEBUG_POS_CTRL
+ buffer.push_back({currentPosition, newTargetVelPController, newTargetVelRampCtrl, currentV, positionError, IceUtil::Time::now().toMicroSeconds()});
+
+ // if (PIDModeActive != usePID)
+ if (buffer.size() > 0 && sign(positionError) * sign(buffer[buffer.size() - 2].currentError) < 0 && eventHappeningCounter < 0)
+ {
+ eventHappeningCounter = 10;
+ ARMARX_IMPORTANT << "HIGH VELOCITY DETECTED";
+ }
+ if (eventHappeningCounter == 0)
+ {
+ ARMARX_IMPORTANT << "BUFFER CONTENT";
+ for (auto& elem : buffer)
+ {
+ ARMARX_INFO << VAROUT(elem.currentPosition) << VAROUT(elem.targetVelocityPID) << VAROUT(elem.targetVelocityRAMP) << VAROUT(elem.currentV) << VAROUT(elem.currentError) << VAROUT(elem.timestamp);
+ }
+ ARMARX_IMPORTANT << VAROUT(newTargetVelPController) << VAROUT(newTargetVelRampCtrl) << VAROUT(currentPosition) << VAROUT(usePID) << VAROUT(positionError);
+
+ }
+ if (eventHappeningCounter >= 0)
+ {
+ eventHappeningCounter--;
+ }
+
+ ARMARX_INFO << deactivateSpam(5) << VAROUT(newTargetVelPController) << VAROUT(newTargetVelRampCtrl) << VAROUT(currentPosition) << VAROUT(usePID) << VAROUT(positionError) << VAROUT(targetPosition) << VAROUT(currentV) << VAROUT(deltaVel) << VAROUT(useddt) << VAROUT(usedacc);
+ PIDModeActive = usePID;
+#endif
+ result.velocity = finalTargetVel;
+ return result;
+ }
+
+ double PositionThroughVelocityControllerWithAccelerationRamps::estimateTime() const
+ {
+ throw LocalException("NYI");
+ return 0;
+ }
+
+ double PositionThroughVelocityControllerWithAccelerationRamps::calculateProportionalGain() const
+ {
+ /* s = v_0*v_0/(2*a) -> sqrt(s*2*a) = v_0;
+ K_p * error = v_0; -> v_0/error = K_p;
+ */
+ auto v_0 = std::sqrt(pControlPosErrorLimit * 2 * deceleration);
+ return v_0 / pControlPosErrorLimit;
+ }
+ std::pair<PositionThroughVelocityControllerWithAccelerationRamps::State,
+ PositionThroughVelocityControllerWithAccelerationRamps::Output> PositionThroughVelocityControllerWithAccelerationRamps::calcState() const
+ {
+ // auto integratedChange = [](v0)
+
+ // double newJerk = this->jerk;
+ // double newAcc = currentAcc;
+ // double newVel = currentV;
+ State newState = state;
+ double a0 = currentAcc, newJerk = jerk, t;
+ Output result = {currentV, a0, newJerk};
+
+ const auto brData = ctrlutil::braking_distance_with_wedge(currentV, currentAcc, newJerk);
+ const auto goalDir = math::MathUtils::Sign(targetPosition - currentPosition);
+ const auto curVDir = math::MathUtils::Sign(currentV);
+ const auto straightBrakingDistance = ctrlutil::brakingDistance(currentV, currentAcc, newJerk);
+ const double brakingDistance = brData.dt2 < 0 ? straightBrakingDistance : brData.s_total;
+ const double distance = std::abs(targetPosition - currentPosition);
+ const double t_to_stop = ctrlutil::t_to_v(currentV, -curVDir * currentAcc, curVDir * newJerk);
+ const auto calculatedJerk = std::abs(ctrlutil::jerk(t_to_stop, distance, currentV, currentAcc));
+ double tmp_t, tmp_acc, tmp_jerk;
+ // std::tie( tmp_t, tmp_acc, tmp_jerk) = ctrlutil::calcAccAndJerk(targetPosition-(currentPosition+brData.s1), brData.v1);
+ std::tie(tmp_t, tmp_acc, tmp_jerk) = ctrlutil::calcAccAndJerk(targetPosition - currentPosition, currentV);
+ const double posWhenBrakingWithCustomJerk = ctrlutil::s(tmp_t, currentPosition + brData.s1, brData.v1, tmp_acc, curVDir * tmp_jerk);
+ // const double brakingDistanceWithCustomJerk = ctrlutil::s(tmp_t, 0, currentV, tmp_acc, curVDir * tmp_jerk);
+ const double posWhenBrakingWithFixedJerk = ctrlutil::s(brData.dt2, currentPosition + brData.s1, brData.v1, brData.a1, curVDir * jerk);
+
+ ARMARX_INFO << VAROUT((int)state) << VAROUT(distance) << VAROUT(brakingDistance);//VAROUT(straightBrakingDistance) << VAROUT(brData.dt1) << VAROUT(brData.dt2) << VAROUT(brData.s_total) << VAROUT(calculatedJerk);
+
+ // if(brData.dt2 < 0)
+ // {
+ // ARMARX_INFO << "distance with fixed jerk: " << ctrlutil::brakingDistance(currentV, currentAcc, newJerk)
+ // << " distance with custom jerk: " << ctrlutil::brakingDistance(currentV, currentAcc, calculatedJerk)
+ // << " vel after: " << ctrlutil::v(t_to_stop, currentV, currentAcc, -curVDir * newJerk) << " vel after next step: " << ctrlutil::v(dt, currentV, currentAcc, -curVDir * newJerk);
+ //// << " acc at stop: " << ctrlutil::a(t_to_stop, currentAcc, -curVDir *newJerk);
+ // jerk = 0.95*calculatedJerk;
+ // state = State::DecAccDecJerk;
+ // return;
+ // }
+ if (state < State::DecAccIncJerk &&
+ ((distance > brakingDistance && (math::MathUtils::Sign(currentAcc) == curVDir || std::abs(currentAcc) < 0.1)) // we are accelerating
+ || distance > brakingDistance * 2 // we are decelerating -> dont switch to accelerating to easily
+ || curVDir != goalDir)) // we are moving away from goal
+ {
+ if (std::abs(maxV - currentV) < 0.1 && std::abs(currentAcc) < 0.1)
+ {
+ newState = State::Keep;
+ newJerk = 0;
+ return std::make_pair(newState, result);
+ }
+ // calculate if we need to increase or decrease acc
+ double t_to_max_v = ctrlutil::t_to_v(goalDir * maxV - currentV, currentAcc, goalDir * newJerk);
+ double acc_at_t = ctrlutil::a(t_to_max_v, std::abs(currentAcc), -newJerk);
+ if (acc_at_t < 0.1)
+ {
+ newState = State::IncAccIncJerk;
+ return std::make_pair(newState, result);
+ }
+ else
+ {
+ newState = State::IncAccDecJerk;
+ return std::make_pair(newState, result);
+ }
+ }
+ else
+ {
+ // calculate if we need to increase or decrease acc
+ double t_to_0 = ctrlutil::t_to_v(currentV, -curVDir * currentAcc, curVDir * newJerk);
+ double tmpV = ctrlutil::v(t_to_0, 0, currentAcc, -curVDir * newJerk);
+ double vAfterBraking = ctrlutil::v(t_to_0, currentV, currentAcc, -curVDir * newJerk);
+ double accAfterBraking = ctrlutil::a(t_to_0, currentAcc, curVDir * newJerk);
+ const double posWhenBrakingNow = ctrlutil::s(t_to_0, currentPosition, currentV, currentAcc, -curVDir * newJerk);
+ const double simpleBrakingDistance = ctrlutil::s(t_to_0, 0, currentV, currentAcc, -curVDir * newJerk);
+ double acc_at_t = ctrlutil::a(t_to_0, std::abs(currentAcc), -newJerk);
+
+ std::tie(t, a0, newJerk) = ctrlutil::calcAccAndJerk(targetPosition - currentPosition, currentV);
+ double at = ctrlutil::a(t, a0, newJerk);
+ double vt = ctrlutil::v(t, currentV, a0, newJerk);
+ double st = ctrlutil::s(t, targetPosition - currentPosition, currentV, a0, newJerk);
+ if (this->state <= State::Keep)
+ {
+ newState = State::DecAccIncJerk;
+ return std::make_pair(newState, result);
+ }
+ else if ((state == State::DecAccIncJerk && acc_at_t > 1) || state == State::DecAccDecJerk)
+ {
+ result.jerk = newJerk;
+ result.acceleration = currentAcc + (a0 - currentAcc) * 0.5 + dt * newJerk;
+ newState = State::DecAccDecJerk;
+ return std::make_pair(newState, result);
+ }
+ else
+ {
+ return std::make_pair(newState, result);
+ }
+
+ }
+ throw LocalException();
+ }
+
+
+
+ void MinJerkPositionController::reset()
+ {
+ currentTime = 0;
+
+ fixedMinJerkState.reset();
+ // pid.reset();
+ currentP_Phase2 = -1;
+ currentP_Phase3 = -1;
+ }
+
+ double MinJerkPositionController::getTargetPosition() const
+ {
+ return targetPosition;
+ }
+
+ // MinJerkPositionController::Output MinJerkPositionController::run()
+ // {
+ // auto min_jerk_calc_jerk = [&](double tf, double s0, double v0, double a0, double xf = 0, double t0 = 0.0)
+ // {
+ // double D = tf - t0;
+ // return 60.0 / (D * D * D) * (xf - s0) - 36.0 / (D * D) * v0 - 9.0 / D * a0;
+ // };
+
+
+ // auto min_jerk = [&](double t, double tf, double s0, double v0, double a0, double xf = 0, double t0 = 0.0)
+ // {
+ // double D = tf - t0;
+ // BOOST_ASSERT(D != 0.0);
+ // double D2 = D * D;
+ // double tau = (t - t0) / D;
+ // double b0 = s0;
+ // double b1 = D * v0;
+ // double b2 = D2 * a0 / 2;
+
+ // double b3 = (-3 * D2) / 2 * a0 - 6 * D * v0 + 10 * (xf - s0);
+ // double b4 = (3 * D2) / 2 * a0 + 8 * D * v0 - 15 * (xf - s0);
+ // double b5 = (-D2) / 2 * a0 - 3 * D * v0 + 6 * (xf - s0);
+
+ // double tau2 = tau * tau;
+ // double tau3 = tau2 * tau;
+ // double tau4 = tau3 * tau;
+ // double tau5 = tau4 * tau;
+ // double st = b0 + b1 * tau + b2 * tau2 + b3 * tau3 + b4 * tau4 + b5 * tau5;
+ // double vt = b1 / D + 2 * b2 / D * tau + 3 * b3 / D * tau2 + 4 * b4 / D * tau3 + 5 * b5 / D * tau4;
+ // double at = 2 * b2 / D2 + 6 * b3 / D2 * tau + 12 * b4 / D2 * tau2 + 20 * b5 / D2 * tau3;
+ // return State{t, st, vt, at};
+ // };
+
+ // double remainingTime = finishTime - currentTime;
+ // double signedDistance = targetPosition - currentPosition;
+ // if (remainingTime <= 0.0)
+ // {
+ // // if(!pid)
+ // // {
+ // // double Kd = (currentV - distance * p) /(-currentV);
+ // // ARMARX_INFO << VAROUT(Kd);
+ // // pid.reset(new PIDController(1, 0, Kd));
+ // // }
+ // if (currentP_Phase3 < 0)
+ // {
+ // currentP_Phase3 = std::abs(currentV / signedDistance);
+ // ARMARX_DEBUG << "optimal P: " << currentP_Phase3;
+ // }
+
+
+ // currentP_Phase3 = currentP_Phase3 * p_adjust_ratio + p * (1.0 - p_adjust_ratio);
+ // double newVel = signedDistance * currentP_Phase3;
+ // double newAcc = (newVel - currentV) / dt;
+ // double newJerk = (newAcc - currentAcc) / dt;
+ // double newPos = currentPosition + newVel * dt;
+ // Output result {newPos, newVel, newAcc, newJerk};
+ // currentTime += dt;
+ // // State s = min_jerk(currentTime, finishTime, fixedMinJerkState->s0, fixedMinJerkState->v0, fixedMinJerkState->a0, targetPosition, fixedMinJerkState->t0);
+ // ARMARX_DEBUG << VAROUT(remainingTime) << VAROUT(signedDistance) << VAROUT(signedDistance * currentP_Phase3) << VAROUT(currentP_Phase3);// << VAROUT(s.s) << VAROUT(s.v) << VAROUT(s.a);
+ // return result;
+ // }
+
+ // if (std::abs(signedDistance) < phase2SwitchDistance && remainingTime < phase2SwitchMaxRemainingTime)
+ // {
+ // if (!fixedMinJerkState)
+ // {
+ // fixedMinJerkState.reset(new FixedMinJerkState{currentTime, currentPosition, currentV, currentAcc});
+ // currentP_Phase2 = 0;
+ // }
+ // // std::vector<State> states;
+ // // double t = currentTime;
+ // // while(t < finishTime)
+ // // {
+ // // State s = min_jerk(t, finishTime, fixedMinJerkState->s0, fixedMinJerkState->v0, fixedMinJerkState->a0, targetPosition, fixedMinJerkState->t0);
+ // // t += dt;
+ // // states.push_back(s);
+ // // }
+ // currentTime += dt;
+ // State s = min_jerk(currentTime, finishTime, fixedMinJerkState->s0, fixedMinJerkState->v0, fixedMinJerkState->a0, targetPosition, fixedMinJerkState->t0);
+ // currentP_Phase2 = currentP_Phase2 * p_adjust_ratio + p * (1.0 - p_adjust_ratio);
+ // double newVel = (s.s - currentPosition) * currentP_Phase2 + s.v;
+ // ARMARX_DEBUG << VAROUT(s.s) << VAROUT(s.v) << VAROUT(newVel);
+ // double newAcc = (newVel - currentV) / dt;
+ // double newJerk = (newAcc - currentAcc) / dt;
+ // double newPos = currentPosition + newVel * dt;
+ // Output result {newPos, newVel, newAcc, newJerk};
+ // return result;
+ // }
+
+
+
+
+ // double jerk = min_jerk_calc_jerk(remainingTime, currentPosition, currentV, currentAcc, targetPosition);
+ // double newAcc = currentAcc + jerk * dt;
+ // double newVelocity = ctrlutil::v(dt, currentV, currentAcc, jerk);
+ // double accAtEnd = ctrlutil::a(remainingTime, currentAcc, jerk);
+
+ // std::vector<State> states;
+ // std::vector<State> states2;
+ // //#define debug
+ //#ifdef debug
+ // {
+ // double dt = 0.001;
+ // double t = 1;
+ // double simS = currentPosition, simV = currentV, simAcc = currentAcc;
+ // while (t > 0)
+ // {
+ // double jerk = min_jerk_calc_jerk(t, simS, simV, simAcc, targetPosition);
+ // simAcc = simAcc + jerk * dt;
+ // simV = ctrlutil::v(dt, simV, simAcc, 0);
+ // simS += dt * simV;
+ // t -= dt;
+ // states.push_back(State{t, simS, simV, simAcc, jerk});
+ // }
+ // }
+ // {
+ // double dt = 0.001;
+
+ // double t = 1;
+ // double simS = currentPosition, simV = currentV, simAcc = currentAcc;
+ // while (t > 0)
+ // {
+ // double jerk = min_jerk_calc_jerk(t, simS, simV, simAcc, targetPosition);
+ // simS += ctrlutil::s(dt, 0, simV, simAcc, jerk);
+ // simV = ctrlutil::v(dt, simV, simAcc, jerk);
+ // simAcc = simAcc + jerk * dt;
+ // t -= dt;
+ // states2.push_back(State{t, simS, simV , simAcc, jerk});
+ // }
+ // }
+ //#endif
+ // ARMARX_DEBUG << VAROUT(currentTime) << VAROUT(remainingTime) << VAROUT(jerk) << VAROUT(accAtEnd) << VAROUT(signedDistance) << " delta acc: " << jerk* dt;
+ // currentTime += dt;
+
+
+ // double newPos = ctrlutil::s(dt, currentPosition, currentV, currentAcc, jerk);
+ // Output result { newPos, newVelocity, newAcc, jerk};
+ // return result;
+
+ // }
+
+ bool VelocityControllerWithRampedAccelerationAndPositionBounds::validParameters() const
+ {
+ return VelocityControllerWithRampedAcceleration::validParameters() &&
+ positionLimitHiSoft > positionLimitLoSoft &&
+ deceleration > 0;
+ }
+
+ VelocityControllerWithRampedAcceleration::Output VelocityControllerWithRampedAccelerationAndPositionBounds::run() const
+ {
+ const double useddt = std::min(std::abs(dt), std::abs(maxDt));
+
+ // if (currentPosition <= positionLimitLoHard || currentPosition >= positionLimitHiHard)
+ // {
+ // ARMARX_INFO << deactivateSpam(1) << "Hard limit violation. " << VAROUT(currentPosition) << VAROUT(positionLimitLoHard) << VAROUT(positionLimitHiHard);
+ // return std::nanf("1");
+ // }
+
+ double softLimitViolation = 0;
+ if (currentPosition <= positionLimitLoSoft)
+ {
+ softLimitViolation = -1;
+ }
+ if (currentPosition >= positionLimitHiSoft)
+ {
+ softLimitViolation = 1;
+ }
+
+ const double upperDt = std::max(std::abs(dt), std::abs(maxDt));
+
+ //we can have 4 cases:
+ // 1. we need to decelerate now or we will violate the position limits (maybe we still will violate them, e.g. if we violated them initially)
+ // 2. we directly set v and ignore acc/dec (if |curr - target| <= limit)
+ // 3. we need to accelerate (if curr and target v have same sign and |curr| < |target|)
+ // 4. we need to decelerate (other cases)
+ double nextv;
+ //handle case 1
+ const double vsquared = currentV * currentV;
+ const double brakingDist = sign(currentV) * vsquared / 2.f / std::abs(deceleration); //the braking distance points in the direction of the velocity
+
+ const double posIfBrakingNow = currentPosition + brakingDist;
+ // ARMARX_INFO << deactivateSpam(0.1) << VAROUT(currentPosition) << VAROUT(positionLimitLoSoft) << VAROUT(positionLimitHiSoft) << VAROUT(posIfBrakingNow) << VAROUT(currentV);
+ if ((posIfBrakingNow <= positionLimitLoSoft && currentV < 0) || (posIfBrakingNow >= positionLimitHiSoft && currentV > 0))
+ {
+ //case 1. -> brake now! (we try to have v=0 at the limit)
+ const auto limit = posIfBrakingNow <= positionLimitLoSoft ? positionLimitLoSoft : positionLimitHiSoft;
+ const double wayToGo = limit - currentPosition;
+
+ //decelerate!
+ // s = v²/(2a) <=> a = v²/(2s)
+ const double dec = std::abs(vsquared / 2.0 / wayToGo);
+ const double vel = currentV - sign(currentV) * dec * upperDt;
+ nextv = boost::algorithm::clamp(vel, -maxV, maxV);
+ // ARMARX_INFO /*<< deactivateSpam(0.1) */ << "clamped new Vel: " << VAROUT(nextv) << VAROUT(currentV);
+ if (sign(currentV) != sign(nextv))
+ {
+ // ARMARX_INFO << deactivateSpam(1) << "wrong sign: stopping"; //stop now
+ nextv = 0;
+ }
+ }
+ else
+ {
+ //handle 2-3
+ return VelocityControllerWithRampedAcceleration::run();
+ // ARMARX_INFO << deactivateSpam(1) << "Desired target Vel: " << targetV << " " << VAROUT(nextv);
+
+ }
+ if (softLimitViolation == sign(nextv) && nextv != 0)
+ {
+ //the area between soft and hard limits is sticky
+ //the controller can only move out of it (not further in)
+ // ARMARX_INFO << deactivateSpam(1) << "Soft limit violation. " << softLimitViolation << VAROUT(nextv);
+ nextv = 0;
+ }
+ double nextAcc = (nextv - currentV) / useddt; // doesnt work with calculated acc
+ double nextJerk = (nextAcc - currentAcc) / useddt;
+ //the next velocity will not violate the pos bounds harder than they are already
+ return Output {nextv, 0, 0};
+ }
+
+
+
+ void MinJerkPositionController::setTargetPosition(double newTargetPosition)
+ {
+ if (std::abs(targetPosition - newTargetPosition) > 0.0001)
+ {
+ reset();
+ double distance = newTargetPosition - currentPosition;
+ double rawFinishTime = 0.0;
+ double decelerationTime = std::abs(currentV / desiredDeceleration);
+ // calculate the time it should take to reach the goal (ToDo: find exact time)
+ if (math::MathUtils::Sign(currentV) == math::MathUtils::Sign(distance)
+ && std::abs(currentV) > 0.0
+ && ctrlutil::s(decelerationTime, 0, std::abs(currentV), -desiredDeceleration, 0) > std::abs(distance))
+ // && ctrlutil::brakingDistance(currentV, currentAcc, desiredJerk) < distance)
+ {
+ // ARMARX_INFO << "branch 1: " << VAROUT(currentV) << VAROUT(desiredDeceleration) << VAROUT(distance);
+ rawFinishTime = decelerationTime;
+ }
+ else
+ {
+ // ARMARX_INFO << "branch 2: " << VAROUT(currentV) << VAROUT(desiredDeceleration) << VAROUT(distance);
+ double accelerationTime = std::abs((math::MathUtils::Sign(distance) * maxV - currentV) / desiredDeceleration);
+ double decelerationTime = std::abs(maxV / desiredDeceleration);
+ rawFinishTime = std::max(decelerationTime + accelerationTime, std::abs(distance / (maxV * 0.75)));
+ }
+ double estimatedTime = ctrlutil::t_to_v_with_wedge_acc(std::abs(distance), std::abs(currentV), std::abs(desiredDeceleration));
+ double minTime = 0.8;
+ if (!std::isnan(estimatedTime))
+ {
+ finishTime = std::max(estimatedTime, minTime);
+ }
+ else
+ {
+ finishTime = std::max(rawFinishTime, minTime);
+ }
+ targetPosition = newTargetPosition;
+ // ARMARX_INFO << "New finish time: " << finishTime << " " << VAROUT(rawFinishTime) << VAROUT(estimatedTime);
+
+ }
+
+
+ }
+
+ MinJerkPositionController::Output MinJerkPositionController::run()
+ {
+ auto min_jerk_calc_jerk = [&](double tf, double s0, double v0, double a0, double xf = 0, double t0 = 0.0)
+ {
+ double D = tf - t0;
+ return 60.0 / (D * D * D) * (xf - s0) - 36.0 / (D * D) * v0 - 9.0 / D * a0;
+ };
+
+
+ auto min_jerk = [&](double t, double tf, double s0, double v0, double a0, double xf = 0, double t0 = 0.0)
+ {
+ double D = tf - t0;
+ BOOST_ASSERT(D != 0.0);
+ double D2 = D * D;
+ double tau = (t - t0) / D;
+ double b0 = s0;
+ double b1 = D * v0;
+ double b2 = D2 * a0 / 2;
+
+ double b3 = (-3 * D2) / 2 * a0 - 6 * D * v0 + 10 * (xf - s0);
+ double b4 = (3 * D2) / 2 * a0 + 8 * D * v0 - 15 * (xf - s0);
+ double b5 = (-D2) / 2 * a0 - 3 * D * v0 + 6 * (xf - s0);
+
+ double tau2 = tau * tau;
+ double tau3 = tau2 * tau;
+ double tau4 = tau3 * tau;
+ double tau5 = tau4 * tau;
+ double st = b0 + b1 * tau + b2 * tau2 + b3 * tau3 + b4 * tau4 + b5 * tau5;
+ double vt = b1 / D + 2 * b2 / D * tau + 3 * b3 / D * tau2 + 4 * b4 / D * tau3 + 5 * b5 / D * tau4;
+ double at = 2 * b2 / D2 + 6 * b3 / D2 * tau + 12 * b4 / D2 * tau2 + 20 * b5 / D2 * tau3;
+ return State {t, st, vt, at};
+ };
+
+ double remainingTime = finishTime - currentTime;
+ double signedDistance = targetPosition - currentPosition;
+ if (remainingTime <= 0.0)
+ {
+ // if(!pid)
+ // {
+ // double Kd = (currentV - distance * p) /(-currentV);
+ // ARMARX_INFO << VAROUT(Kd);
+ // pid.reset(new PIDController(1, 0, Kd));
+ // }
+ if (currentP_Phase3 < 0)
+ {
+ if (signedDistance == 0.0f)
+ {
+ currentP_Phase3 = p;
+ }
+ else
+ {
+ currentP_Phase3 = std::abs(currentV / signedDistance);
+ currentP_Phase3 = math::MathUtils::LimitTo(currentP_Phase3, p * 10);
+ }
+ // ARMARX_INFO << deactivateSpam(0.1) << "optimal P: " << currentP_Phase3 << VAROUT(currentV) << VAROUT(signedDistance);
+ }
+
+
+ currentP_Phase3 = currentP_Phase3 * p_adjust_ratio + p * (1.0 - p_adjust_ratio);
+ double newVel = signedDistance * currentP_Phase3;
+ double newAcc = (newVel - currentV) / dt;
+ double newJerk = (newAcc - currentAcc) / dt;
+ double newPos = currentPosition + newVel * dt;
+
+ Output result {newPos, newVel, newAcc, newJerk};
+ currentTime += dt;
+ // State s = min_jerk(currentTime, finishTime, fixedMinJerkState->s0, fixedMinJerkState->v0, fixedMinJerkState->a0, targetPosition, fixedMinJerkState->t0);
+ // ARMARX_INFO << deactivateSpam(0.1) << VAROUT(remainingTime) << VAROUT(signedDistance) << VAROUT(signedDistance * currentP_Phase3) << VAROUT(currentP_Phase3);// << VAROUT(s.s) << VAROUT(s.v) << VAROUT(s.a);
+ return result;
+ }
+
+ if (std::abs(signedDistance) < phase2SwitchDistance && remainingTime < phase2SwitchMaxRemainingTime)
+ {
+ if (!fixedMinJerkState)
+ {
+ fixedMinJerkState.reset(new FixedMinJerkState {currentTime, currentPosition, currentV, currentAcc});
+ currentP_Phase2 = 0;
+ }
+ // std::vector<State> states;
+ // double t = currentTime;
+ // while(t < finishTime)
+ // {
+ // State s = min_jerk(t, finishTime, fixedMinJerkState->s0, fixedMinJerkState->v0, fixedMinJerkState->a0, targetPosition, fixedMinJerkState->t0);
+ // t += dt;
+ // states.push_back(s);
+ // }
+ currentTime += dt;
+ State s = min_jerk(currentTime, finishTime, fixedMinJerkState->s0, fixedMinJerkState->v0, fixedMinJerkState->a0, targetPosition, fixedMinJerkState->t0);
+ double newVelocity = (s.s - (currentPosition + s.v * dt)) * currentP_Phase2 + s.v;
+ double newAcc = (newVelocity - currentV) / dt;
+ double newJerk = (newAcc - currentAcc) / dt;
+ double newPos = ctrlutil::s(dt, currentPosition, currentV, currentAcc, newJerk);//currentPosition + newVelocity * dt;
+ // ARMARX_INFO /*<< deactivateSpam(0.1) */ << VAROUT(currentP_Phase2) << VAROUT(currentPosition) << VAROUT(signedDistance) << VAROUT(s.s) << VAROUT(s.v) << VAROUT(newVelocity) << VAROUT(newAcc) << VAROUT(newJerk) << VAROUT(s.a);
+ currentP_Phase2 = currentP_Phase2 * p_adjust_ratio + p * (1.0 - p_adjust_ratio);
+ Output result {newPos, newVelocity, newAcc, newJerk};
+ return result;
+ }
+
+
+
+
+ double jerk = min_jerk_calc_jerk(remainingTime, currentPosition, currentV, currentAcc, targetPosition);
+ double newAcc = currentAcc + jerk * dt;
+ double newVelocity = ctrlutil::v(dt, currentV, currentAcc, jerk);
+ double accAtEnd = ctrlutil::a(remainingTime, currentAcc, jerk); // assumes that fixed jerk would be used, which is not the case here
+
+ std::vector<State> states;
+ std::vector<State> states2;
+ //#define debug
+#ifdef debug
+ {
+ double dt = 0.001;
+ double t = 1;
+ double simS = currentPosition, simV = currentV, simAcc = currentAcc;
+ while (t > 0)
+ {
+ double jerk = min_jerk_calc_jerk(t, simS, simV, simAcc, targetPosition);
+ simAcc = simAcc + jerk * dt;
+ simV = ctrlutil::v(dt, simV, simAcc, 0);
+ simS += dt * simV;
+ t -= dt;
+ states.push_back(State {t, simS, simV, simAcc, jerk});
+ }
+ }
+ {
+ double dt = 0.001;
+
+ double t = 1;
+ double simS = currentPosition, simV = currentV, simAcc = currentAcc;
+ while (t > 0)
+ {
+ double jerk = min_jerk_calc_jerk(t, simS, simV, simAcc, targetPosition);
+ simS += ctrlutil::s(dt, 0, simV, simAcc, jerk);
+ simV = ctrlutil::v(dt, simV, simAcc, jerk);
+ simAcc = simAcc + jerk * dt;
+ t -= dt;
+ states2.push_back(State {t, simS, simV, simAcc, jerk});
+ }
+ }
+#endif
+ // ARMARX_INFO << /*deactivateSpam(0.1) <<*/ VAROUT(currentTime) << VAROUT(remainingTime) << VAROUT(jerk) << VAROUT(accAtEnd) << VAROUT(signedDistance) << " new acc: " << newAcc << VAROUT(currentV);
+ currentTime += dt;
+
+
+ double newPos = ctrlutil::s(dt, currentPosition, currentV, currentAcc, jerk);
+ Output result {newPos, newVelocity, newAcc, jerk};
+ return result;
+
+ }
}
diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.h b/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
index e15f9a3cd8cacd07118ddfd33dc185b05613bdf2..b164554c2c35c4cc219aaab75b96e2c10c781632 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
@@ -26,11 +26,14 @@
#include <type_traits>
#include <ArmarXCore/core/util/algorithm.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
+#include <RobotAPI/libraries/core/PIDController.h>
// #define DEBUG_POS_CTRL
#ifdef DEBUG_POS_CTRL
#include <boost/circular_buffer.hpp>
#endif
+
+
namespace armarx
{
template<class T, class = typename std::enable_if<std:: is_floating_point<T>::value>::type >
@@ -142,7 +145,7 @@ namespace armarx
*/
inline float brakingDistance(float v0, float deceleration)
{
- return accelerateToVelocity(v0, deceleration, 0).dx;
+ return accelerateToVelocity(std::abs(v0), std::abs(deceleration), 0).dx;
}
inline float angleDistance(float angle1, float angle2)
@@ -166,6 +169,42 @@ namespace armarx
float estimateTime() const;
};
+ struct VelocityControllerWithRampedAcceleration
+ {
+
+ struct Output
+ {
+ double velocity;
+ double acceleration;
+ double jerk;
+ };
+ float dt;
+ float maxDt;
+ float currentV;
+ float currentAcc;
+ float targetV;
+ float maxV;
+ float jerk;
+ float directSetVLimit;
+
+
+ bool validParameters() const;
+ Output run() const;
+ };
+
+ struct VelocityControllerWithRampedAccelerationAndPositionBounds: VelocityControllerWithRampedAcceleration
+ {
+
+ double currentPosition;
+ double positionLimitLoSoft;
+ double positionLimitHiSoft;
+ // double positionLimitLoHard;
+ // double positionLimitHiHard;
+ double deceleration;
+ bool validParameters() const;
+ Output run() const;
+ };
+
float velocityControlWithAccelerationBounds(
float dt, float maxDt,
const float currentV, float targetV, float maxV,
@@ -195,6 +234,7 @@ namespace armarx
*/
struct VelocityControllerWithAccelerationAndPositionBounds: VelocityControllerWithAccelerationBounds
{
+
float currentPosition;
float positionLimitLoSoft;
float positionLimitHiSoft;
@@ -229,8 +269,10 @@ namespace armarx
float pControlPosErrorLimit = 0.01;
float pControlVelLimit = 0.002; // if target is below this threshold, PID controller will be used
float accuracy = 0.001;
- float p;
+ std::shared_ptr<PIDController> pid;
+ // float p;
PositionThroughVelocityControllerWithAccelerationBounds();
+ float calculateProportionalGain() const;
bool validParameters() const;
float run() const;
float estimateTime() const;
@@ -255,7 +297,76 @@ namespace armarx
private:
mutable bool currentlyPIDActive = false;
+
+ };
+
+ struct PositionThroughVelocityControllerWithAccelerationRamps
+ {
+ enum class State
+ {
+ Unknown = -1,
+ IncAccIncJerk,
+ IncAccDecJerk,
+ Keep,
+ DecAccIncJerk,
+ DecAccDecJerk,
+ PCtrl
+
+ };
+ struct Output
+ {
+ double velocity;
+ double acceleration;
+ double jerk;
+ };
+
+ double dt;
+ double maxDt;
+ double currentV;
+ double currentAcc;
+ double maxV;
+ double acceleration;
+ double deceleration;
+ double jerk;
+ double currentPosition;
+ protected:
+ double targetPosition;
+ public:
+ double getTargetPosition() const;
+ void setTargetPosition(double value);
+
+ double pControlPosErrorLimit = 0.01;
+ double pControlVelLimit = 0.002; // if target is below this threshold, PID controller will be used
+ double accuracy = 0.001;
+ double p;
+ bool usePIDAtEnd = true;
+ mutable State state = State::Unknown;
+ PositionThroughVelocityControllerWithAccelerationRamps();
+ bool validParameters() const;
+ Output run();
+ double estimateTime() const;
+ double calculateProportionalGain() const;
+
+
+ std::pair<State, Output> calcState() const;
+#ifdef DEBUG_POS_CTRL
+ mutable bool PIDModeActive = false;
+ struct HelpStruct
+ {
+ double currentPosition;
+ double targetVelocityPID;
+ double targetVelocityRAMP;
+ double currentV;
+ double currentError;
+ long timestamp;
+ };
+
+ mutable boost::circular_buffer<HelpStruct> buffer;
+ mutable int eventHappeningCounter = -1;
+#endif
+
};
+
float positionThroughVelocityControlWithAccelerationBounds(float dt, float maxDt,
float currentV, float maxV,
float acceleration, float deceleration,
@@ -290,5 +401,52 @@ namespace armarx
float pControlPosErrorLimit, float p,
float& direction,
float positionPeriodLo, float positionPeriodHi);
+
+ class MinJerkPositionController
+ {
+ public:
+ struct State
+ {
+ double t, s, v, a;
+ };
+ struct FixedMinJerkState
+ {
+ double t0, s0, v0, a0;
+ };
+ struct Output
+ {
+ double position;
+ double velocity;
+ double acceleration;
+ double jerk;
+ };
+
+ double dt;
+ double maxDt;
+ double currentPosition;
+ double currentV;
+ double currentAcc;
+ double maxV;
+ double desiredDeceleration;
+ double desiredJerk;
+ double p = 1.0;
+ double phase2SwitchDistance = 0.1;
+ double phase2SwitchMaxRemainingTime = 0.2; // seconds
+ double p_adjust_ratio = 0.8;
+ // PIDControllerPtr pid;
+ protected:
+ double currentP_Phase3 = -1;
+ double currentP_Phase2 = -1;
+ double targetPosition;
+ double finishTime = 0;
+ double currentTime = 0;
+ std::unique_ptr<FixedMinJerkState> fixedMinJerkState;
+ public:
+ void reset();
+ double getTargetPosition() const;
+ void setTargetPosition(double value);
+
+ Output run();
+ };
}
diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
old mode 100644
new mode 100755
index 397d386f44f8b8349526e92382a49b3fb60dad25..60ce76b94f2d47967c60ff5799dac1a864392c8c
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -13,6 +13,7 @@ set(LIBS
set(LIB_FILES
util.cpp
util/ControlThreadOutputBuffer.cpp
+ util/DynamicsHelper.cpp
util/introspection/DataFieldsInfo.cpp
SyntaxCheck.cpp
RobotUnit.cpp
@@ -42,6 +43,7 @@ set(LIB_FILES
NJointControllers/NJointKinematicUnitPassThroughController.cpp
NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
+ NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
NJointControllers/NJointTCPController.cpp
NJointControllers/NJointCartesianVelocityController.cpp
NJointControllers/NJointCartesianTorqueController.cpp
@@ -60,6 +62,7 @@ set(LIB_HEADERS
util/KeyValueVector.h
util/AtomicWrapper.h
util/Time.h
+ util/DynamicsHelper.h
util/ControlThreadOutputBuffer.h
util/JointAndNJointControllers.h
util/introspection/DataFieldsInfo.h
@@ -67,6 +70,7 @@ set(LIB_HEADERS
util/introspection/ClassMemberInfo.h
util/RtLogging.h
util/RtTiming.h
+ util/CtrlUtil.h
#robot unit modules need to be added to the list below (but not here)
RobotUnitModules/RobotUnitModuleBase.h
@@ -108,6 +112,7 @@ set(LIB_HEADERS
NJointControllers/NJointKinematicUnitPassThroughController.h
NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
NJointControllers/NJointHolonomicPlatformRelativePositionController.h
+ NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
NJointControllers/NJointTCPController.h
NJointControllers/NJointCartesianVelocityController.h
NJointControllers/NJointCartesianTorqueController.h
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
index d23148c615cd88770586a77d4b6e081deba09023..7b9994929a379228956f72a0f6de7f8efbdcf474 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
@@ -138,7 +138,7 @@ namespace armarx
// NJointCartesianVelocityControllerWithRampInterface interface
public:
- void setMaxAccelerations(float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration, const Ice::Current& = Ice::Current()) override;
+ void setMaxAccelerations(float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration, const Ice::Current& = Ice::emptyCurrent) override;
void setTargetVelocity(float vx, float vy, float vz, float vrx, float vry, float vrz, const Ice::Current&) override;
void setJointLimitAvoidanceScale(float jointLimitAvoidanceScale, const Ice::Current&) override;
void setKpJointLimitAvoidance(float KpJointLimitAvoidance, const Ice::Current&) override;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
index 6fd11bd486166177694ec16cee0b594202cda03f..86911d7e44b317bf7d04ea226a516853967947c4 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
@@ -252,7 +252,7 @@ namespace armarx
return RobotUnitModule::Devices::Instance().getControlDevice(deviceName);
}
- void NJointController::useSynchronizedRtRobot(bool updateCollisionModel)
+ const VirtualRobot::RobotPtr& NJointController::useSynchronizedRtRobot(bool updateCollisionModel)
{
ARMARX_CHECK_EXPRESSION_W_HINT(
NJointControllerRegistryEntry::ConstructorIsRunning(),
@@ -261,39 +261,27 @@ namespace armarx
ARMARX_CHECK_IS_NULL_W_HINT(rtRobot, "useSynchronizedRtRobot was already called");
rtRobot = RobotUnitModule::RobotData::Instance().cloneRobot(updateCollisionModel);
rtRobotNodes = rtRobot->getRobotNodes();
+ return rtRobot;
}
- ThreadPoolPtr NJointController::getThreadPool() const
+ void NJointController::onInitComponent()
{
- ARMARX_CHECK_EXPRESSION(Application::getInstance());
- return Application::getInstance()->getThreadPool();
+ onInitNJointController();
}
-
- const SensorValueBase* NJointController::useSensorValue(const std::string& deviceName) const
+ void NJointController::onConnectComponent()
{
- ARMARX_CHECK_EXPRESSION_W_HINT(
- NJointControllerRegistryEntry::ConstructorIsRunning(),
- "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController."
- );
- auto dev = peekSensorDevice(deviceName);
- if (!dev)
- {
- ARMARX_DEBUG << "No sensor device for " << deviceName;
- return nullptr;
- }
- return dev->getSensorValue();
+ onConnectNJointController();
}
- NJointController::NJointController() :
- robotUnit(RobotUnitModule::ModuleBase::Instance< ::armarx::RobotUnit>()),
- controlDeviceUsedBitmap(robotUnit.getNumberOfControlDevices(), 0)
+ void NJointController::onDisconnectComponent()
{
- controlDeviceUsedIndices.reserve(robotUnit.getNumberOfControlDevices());
+ onDisconnectNJointController();
}
- NJointController::~NJointController()
+ void NJointController::onExitComponent()
{
+ onExitNJointController();
// make sure the destructor of the handles does not throw an exception and triggers a termination of the process
ARMARX_DEBUG << "Deleting thread handles";
ScopedLock lock(threadHandlesMutex);
@@ -337,8 +325,39 @@ namespace armarx
}
threadHandles.clear();
+ }
+
+ ThreadPoolPtr NJointController::getThreadPool() const
+ {
+ ARMARX_CHECK_EXPRESSION(Application::getInstance());
+ return Application::getInstance()->getThreadPool();
+ }
+
+
+ const SensorValueBase* NJointController::useSensorValue(const std::string& deviceName) const
+ {
+ ARMARX_CHECK_EXPRESSION_W_HINT(
+ NJointControllerRegistryEntry::ConstructorIsRunning(),
+ "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController."
+ );
+ auto dev = peekSensorDevice(deviceName);
+ if (!dev)
+ {
+ ARMARX_DEBUG << "No sensor device for " << deviceName;
+ return nullptr;
+ }
+ return dev->getSensorValue();
+ }
+ NJointController::NJointController() :
+ robotUnit(RobotUnitModule::ModuleBase::Instance< ::armarx::RobotUnit>()),
+ controlDeviceUsedBitmap(robotUnit.getNumberOfControlDevices(), 0)
+ {
+ controlDeviceUsedIndices.reserve(robotUnit.getNumberOfControlDevices());
+ }
+ NJointController::~NJointController()
+ {
}
ControlTargetBase* NJointController::useControlTarget(const std::string& deviceName, const std::string& controlMode)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
index 40284b1c85821917abcb14768def1f369a86b699..3b2deaf46191bc48c46bdaba02d4707cc25e4452 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
@@ -643,7 +643,7 @@ namespace armarx
* @param updateCollisionModel Whether the robot's collision model should be updated
* @see rtGetRobot
*/
- void useSynchronizedRtRobot(bool updateCollisionModel = false);
+ const VirtualRobot::RobotPtr& useSynchronizedRtRobot(bool updateCollisionModel = false);
// //////////////////////////////////////////////////////////////////////////////////////// //
// ////////////////////////////////// Component interface ///////////////////////////////// //
@@ -652,9 +652,19 @@ namespace armarx
/// @see Component::getDefaultName
std::string getDefaultName() const override;
/// @see Component::onInitComponent
- void onInitComponent() override {}
+ void onInitComponent() final;
/// @see Component::onConnectComponent
- void onConnectComponent() override {}
+ void onConnectComponent() final;
+ /// @see Component::onDisconnectComponent
+ void onDisconnectComponent() final;
+ /// @see Component::onExitComponent
+ void onExitComponent() final;
+
+
+ virtual void onInitNJointController() {}
+ virtual void onConnectNJointController() {}
+ virtual void onDisconnectNJointController() {}
+ virtual void onExitNJointController() {}
// //////////////////////////////////////////////////////////////////////////////////////// //
// ////////////////////////////////// ThreadPool functionality///////////////////////////// //
@@ -666,7 +676,7 @@ namespace armarx
* @brief Executes a given task in a separate thread from the Application ThreadPool.
* @param taskName Descriptive name of this task to identify it on errors.
* @param task std::function object (or lambda) that is to be executed.
- * @note This task will be joined in the destructor of the NJointController. So make sure it terminates, when the
+ * @note This task will be joined in onExitComponent of the NJointController. So make sure it terminates, when the
* controller is deactivated or removed!
*
* @code{.cpp}
@@ -696,9 +706,10 @@ namespace armarx
ScopedLock lock(threadHandlesMutex);
ARMARX_CHECK_EXPRESSION(!taskName.empty());
ARMARX_CHECK_EXPRESSION(!threadHandles.count(taskName));
+ ARMARX_CHECK_EXPRESSION(getState() < eManagedIceObjectExiting);
ARMARX_VERBOSE << "Adding NJointController task named '" << taskName << "' - current available thread count: " << getThreadPool()->getAvailableTaskCount();
auto handlePtr = std::make_shared<ThreadPool::Handle>(getThreadPool()->runTask(task));
- ARMARX_CHECK_EXPRESSION_W_HINT(handlePtr->isValid(), "Could not add task (" << taskName << " - " << typeid(task).name() << " ) - available threads: " << getThreadPool()->getAvailableTaskCount());
+ ARMARX_CHECK_EXPRESSION_W_HINT(handlePtr->isValid(), "Could not add task (" << taskName << " - " << GetTypeString(task) << " ) - available threads: " << getThreadPool()->getAvailableTaskCount());
threadHandles[taskName] = handlePtr;
}
std::map<std::string, std::shared_ptr<ThreadPool::Handle>> threadHandles;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..222b7f42c475ca3645676bb4460a822f62328b48
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -0,0 +1,151 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 ArmarX
+ * @author Markus Grotz (markus.grotz at kit dot edu)
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#include "NJointHolonomicPlatformGlobalPositionController.h"
+
+namespace armarx
+{
+ NJointControllerRegistration<NJointHolonomicPlatformGlobalPositionController>
+ registrationNJointHolonomicPlatformGlobalPositionController("NJointHolonomicPlatformGlobalPositionController");
+
+
+ NJointHolonomicPlatformGlobalPositionController::NJointHolonomicPlatformGlobalPositionController(
+ const RobotUnitPtr&,
+ const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
+ const VirtualRobot::RobotPtr&) :
+ pid(cfg->p, cfg->i, cfg->d, cfg->maxVelocity, cfg->maxAcceleration)
+
+ {
+ const SensorValueBase* sv = useSensorValue(cfg->platformName);
+ this->sv = sv->asA<SensorValueHolonomicPlatform>();
+ target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)->asA<ControlTargetHolonomicPlatformVelocity>();
+ ARMARX_CHECK_EXPRESSION_W_HINT(target, "The actuator '" << cfg->platformName << "' has no control mode " << ControlModes::HolonomicPlatformVelocity);
+
+ pid.threadSafe = false;
+
+ oriCtrl.maxV = cfg->maxRotationVelocity;
+ oriCtrl.acceleration = cfg->maxRotationAcceleration;
+ oriCtrl.deceleration = cfg->maxRotationAcceleration;
+ oriCtrl.maxDt = 0.1;
+ oriCtrl.pid->Kp = cfg->p;
+ oriCtrl.positionPeriodLo = -M_PI;
+ oriCtrl.positionPeriodHi = M_PI;
+ pid.preallocate(2);
+
+ }
+
+ void NJointHolonomicPlatformGlobalPositionController::rtRun(const IceUtil::Time& currentTime, const IceUtil::Time& timeSinceLastIteration)
+ {
+ currentPosition << sv->relativePositionX, sv->relativePositionY;
+ currentOrientation = sv->relativePositionRotation;
+
+
+ if (rtGetControlStruct().newTargetSet)
+ {
+ pid.reset();
+ *const_cast<bool*>(&rtGetControlStruct().newTargetSet) = false;
+ }
+
+ if ((rtGetControlStruct().lastUpdate + IceUtil::Time::seconds(2)) < currentTime)
+ {
+
+ // ARMARX_RT_LOGF_WARNING << deactivateSpam(0.5) << "Waiting for global pos";
+
+ target->velocityX = 0;
+ target->velocityY = 0;
+
+ target->velocityRotation = 0;
+
+ return;
+ }
+
+ float relativeOrientation = currentOrientation - rtGetControlStruct().startOrientation;
+ Eigen::Vector2f relativeCurrentPosition = currentPosition - rtGetControlStruct().startPosition;
+
+ Eigen::Vector2f updatedPosition = rtGetControlStruct().globalPosition + relativeCurrentPosition;
+ float updatedOrientation = rtGetControlStruct().globalOrientation + relativeOrientation;
+ updatedOrientation = std::atan2(std::sin(updatedOrientation), std::cos(updatedOrientation));
+
+ pid.update(timeSinceLastIteration.toSecondsDouble(), updatedPosition, rtGetControlStruct().target);
+
+ oriCtrl.dt = timeSinceLastIteration.toSecondsDouble();
+ oriCtrl.accuracy = rtGetControlStruct().rotationAccuracy;
+ oriCtrl.currentPosition = updatedOrientation;
+ oriCtrl.targetPosition = getWriterControlStruct().targetOrientation;
+ oriCtrl.currentV = sv->velocityRotation;
+
+ Eigen::Vector2f localTargetVelocity = Eigen::Rotation2Df(-updatedOrientation) * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
+ target->velocityX = localTargetVelocity(0);
+ target->velocityY = localTargetVelocity(1);
+ target->velocityRotation = oriCtrl.run();
+
+ Eigen::Vector2f posError = pid.target.head(2) - pid.processValue.head(2);
+ if (posError.norm() < rtGetControlStruct().translationAccuracy)
+ {
+ // target->velocityX = 0;
+ // target->velocityY = 0;
+ }
+
+ float orientationError = oriCtrl.currentPosition - oriCtrl.targetPosition;
+ orientationError = std::atan2(std::sin(orientationError), std::cos(orientationError));
+ if (std::fabs(orientationError) < rtGetControlStruct().rotationAccuracy)
+ {
+ //target->velocityRotation = 0;
+ }
+ }
+
+ void NJointHolonomicPlatformGlobalPositionController::rtPreActivateController()
+ {
+ }
+
+ void NJointHolonomicPlatformGlobalPositionController::setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy)
+ {
+ // todo do we really need a recursive mutex?
+
+ std::lock_guard<std::recursive_mutex> lock(controlDataMutex);
+
+ getWriterControlStruct().target << x, y;
+ getWriterControlStruct().targetOrientation = std::atan2(std::sin(yaw), std::cos(yaw));
+ getWriterControlStruct().translationAccuracy = translationAccuracy;
+ getWriterControlStruct().rotationAccuracy = rotationAccuracy;
+ getWriterControlStruct().newTargetSet = true;
+ writeControlStruct();
+ }
+
+
+ void NJointHolonomicPlatformGlobalPositionController::setGlobalPos(float x, float y, float yaw, const IceUtil::Time& time = ::IceUtil::Time::now())
+ {
+ // ..todo:: lock :)
+
+ getWriterControlStruct().globalPosition << x, y;
+ getWriterControlStruct().globalOrientation = yaw;
+
+ getWriterControlStruct().startPosition = currentPosition;
+ getWriterControlStruct().startOrientation = currentOrientation;
+
+ getWriterControlStruct().lastUpdate = time;
+ writeControlStruct();
+ }
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
new file mode 100755
index 0000000000000000000000000000000000000000..b151f38024bbd08336f3aed67d89f98215bbad36
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
@@ -0,0 +1,122 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 ArmarX
+ * @author Markus Grotz (markus.grotz at kit dot edu)
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+
+#include <atomic>
+
+#include <VirtualRobot/Robot.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "NJointController.h"
+#include "../RobotUnit.h"
+#include "../SensorValues/SensorValueHolonomicPlatform.h"
+
+#include "../ControlTargets/ControlTarget1DoFActuator.h"
+#include <RobotAPI/libraries/core/PIDController.h>
+
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h>
+
+#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
+namespace armarx
+{
+
+ TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformGlobalPositionControllerConfig);
+ class NJointHolonomicPlatformGlobalPositionControllerConfig : virtual public NJointControllerConfig
+ {
+ public:
+ std::string platformName;
+ float p = 1.0f;
+ float i = 0.0f;
+ float d = 0.0f;
+ float maxVelocity = 300;
+ float maxAcceleration = 500;
+ float maxRotationVelocity = 0.5;
+ float maxRotationAcceleration = 0.5;
+ };
+
+ struct NJointHolonomicPlatformGlobalPositionControllerTarget
+ {
+ Eigen::Vector2f target; // x,y
+ float targetOrientation;
+ float translationAccuracy = 0.0f;
+ float rotationAccuracy = 0.0f;
+ bool newTargetSet = false;
+
+
+ Eigen::Vector2f startPosition;
+ float startOrientation;
+
+ Eigen::Vector2f globalPosition;
+ float globalOrientation;
+ IceUtil::Time lastUpdate = IceUtil::Time::seconds(0);
+ };
+
+ TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformGlobalPositionController);
+
+ /**
+ * @brief The NJointHolonomicPlatformGlobalPositionController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
+ class NJointHolonomicPlatformGlobalPositionController:
+ virtual public NJointControllerWithTripleBuffer<NJointHolonomicPlatformGlobalPositionControllerTarget>
+ {
+ public:
+ using ConfigPtrT = NJointHolonomicPlatformGlobalPositionControllerConfigPtr;
+
+ inline NJointHolonomicPlatformGlobalPositionController(
+ const RobotUnitPtr& robotUnit,
+ const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
+ const VirtualRobot::RobotPtr&);
+
+ inline virtual void rtRun(const IceUtil::Time&, const IceUtil::Time& timeSinceLastIteration) override;
+ inline virtual void rtPreActivateController() override;
+
+ //ice interface
+ inline virtual std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override
+ {
+ return "NJointHolonomicPlatformGlobalPositionController";
+ }
+
+ void setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy);
+
+ void setGlobalPos(float x, float y, float yaw, const IceUtil::Time&);
+
+
+ protected:
+ const SensorValueHolonomicPlatform* sv;
+ MultiDimPIDController pid;
+
+ PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition oriCtrl;
+ ControlTargetHolonomicPlatformVelocity* target;
+
+
+ Eigen::Vector2f currentPosition;
+ float currentOrientation;
+
+ };
+} // namespace armarx
+
+
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
index 15e6f325590e7fdfd02f5ec4305dddf2ce982459..3243b686d7e5ca71a3e992150693ebc241321b8d 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
@@ -46,7 +46,7 @@ namespace armarx
oriCtrl.acceleration = cfg->maxRotationAcceleration;
oriCtrl.deceleration = cfg->maxRotationAcceleration;
oriCtrl.maxDt = 0.1;
- oriCtrl.p = cfg->p;
+ oriCtrl.pid->Kp = cfg->p;
oriCtrl.positionPeriodLo = -M_PI;
oriCtrl.positionPeriodHi = M_PI;
pid.preallocate(2);
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
index 30805e93ea77a7d5488a2afaff986617b643379a..cf4e73188f070f801567d175b514703f728c9328 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
@@ -104,9 +104,6 @@ namespace armarx
Eigen::Vector2f startPose, currentPose;
float orientationOffset;
// float rad2MMFactor;
-
- virtual void onInitComponent() override {}
- virtual void onConnectComponent() override {}
};
} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
index 9c055b45e0f65150b309957bcb4b0f230a4e101a..e42356516b7645e998cf74a40c58e88c7931630e 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
@@ -29,7 +29,8 @@ using namespace armarx;
NJointHolonomicPlatformUnitVelocityPassThroughController::NJointHolonomicPlatformUnitVelocityPassThroughController(
RobotUnitPtr prov,
NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr
- cfg, const VirtualRobot::RobotPtr&)
+ cfg, const VirtualRobot::RobotPtr&) :
+ maxCommandDelay(IceUtil::Time::milliSeconds(500))
{
target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)->asA<ControlTargetHolonomicPlatformVelocity>();
ARMARX_CHECK_EXPRESSION_W_HINT(target, "The actuator " << cfg->platformName << " has no control mode " << ControlModes::HolonomicPlatformVelocity);
@@ -40,11 +41,21 @@ NJointHolonomicPlatformUnitVelocityPassThroughController::NJointHolonomicPlatfor
reinitTripleBuffer(initialSettings);
}
-void NJointHolonomicPlatformUnitVelocityPassThroughController::rtRun(const IceUtil::Time&, const IceUtil::Time&)
+void NJointHolonomicPlatformUnitVelocityPassThroughController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time&)
{
- target->velocityX = rtGetControlStruct().velocityX;
- target->velocityY = rtGetControlStruct().velocityY;
- target->velocityRotation = rtGetControlStruct().velocityRotation;
+ auto commandAge = sensorValuesTimestamp - rtGetControlStruct().commandTimestamp;
+
+ if (commandAge > maxCommandDelay && // command must be recent
+ (rtGetControlStruct().velocityX != 0.0f || rtGetControlStruct().velocityY != 0.0f || rtGetControlStruct().velocityRotation != 0.0f)) // only throw error if any command is not zero
+ {
+ throw LocalException("Platform target velocity was not set for a too long time: delay: ") << commandAge.toSecondsDouble() << " s, max allowed delay: " << maxCommandDelay.toSecondsDouble() << " s";
+ }
+ else
+ {
+ target->velocityX = rtGetControlStruct().velocityX;
+ target->velocityY = rtGetControlStruct().velocityY;
+ target->velocityRotation = rtGetControlStruct().velocityRotation;
+ }
}
void NJointHolonomicPlatformUnitVelocityPassThroughController::setVelocites(float velocityX, float velocityY,
@@ -54,8 +65,19 @@ void NJointHolonomicPlatformUnitVelocityPassThroughController::setVelocites(floa
getWriterControlStruct().velocityX = velocityX;
getWriterControlStruct().velocityY = velocityY;
getWriterControlStruct().velocityRotation = velocityRotation;
+ getWriterControlStruct().commandTimestamp = IceUtil::Time::now();
writeControlStruct();
}
+IceUtil::Time NJointHolonomicPlatformUnitVelocityPassThroughController::getMaxCommandDelay() const
+{
+ return maxCommandDelay;
+}
+
+void NJointHolonomicPlatformUnitVelocityPassThroughController::setMaxCommandDelay(const IceUtil::Time& value)
+{
+ maxCommandDelay = value;
+}
+
NJointControllerRegistration<NJointHolonomicPlatformUnitVelocityPassThroughController>
registrationNJointHolonomicPlatformUnitVelocityPassThroughController("NJointHolonomicPlatformUnitVelocityPassThroughController");
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
index 89cb892d3343854996e623b4bfe07748d587ba79..3c793d040a30b463b4419a3f168d389fa03b2f83 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
@@ -48,6 +48,7 @@ namespace armarx
float velocityX = 0;
float velocityY = 0;
float velocityRotation = 0;
+ IceUtil::Time commandTimestamp;
};
TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformUnitVelocityPassThroughController);
@@ -76,9 +77,11 @@ namespace armarx
{
return "NJointHolonomicPlatformUnitVelocityPassThroughController";
}
+ IceUtil::Time getMaxCommandDelay() const;
+ void setMaxCommandDelay(const IceUtil::Time& value);
+
protected:
- void onInitComponent() override {}
- void onConnectComponent() override {}
+ IceUtil::Time maxCommandDelay;
ControlTargetHolonomicPlatformVelocity* target;
NJointHolonomicPlatformUnitVelocityPassThroughControllerControlData initialSettings;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
index 2985febdcd46aa373e2dd12dfeaefee5fe988412..1d5b4d81faf87c0f5190b7124a3e0293f39b9e81 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
@@ -99,8 +99,5 @@ namespace armarx
};
float sensorToControlOnActivateFactor {0};
float resetZeroThreshold {0};
-
- void onInitComponent() override {}
- void onConnectComponent() override {}
};
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
index f735aba8fc065d888b69c59afe55bcef63f83dc9..3af83fb38da192c09309dd1a9f35eaa3f9a89ecd 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
@@ -169,7 +169,6 @@ void NJointTaskSpaceImpedanceController::rtRun(const IceUtil::Time& sensorValues
Eigen::VectorXf nullqerror = desiredJointPosition - qpos;
- // ARMARX_CHECK_EQUAL(nullqerror.rows(), targets.size());
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(nullqerror.rows()), targets.size());
for (int i = 0; i < nullqerror.rows(); ++i)
@@ -182,7 +181,6 @@ void NJointTaskSpaceImpedanceController::rtRun(const IceUtil::Time& sensorValues
Eigen::VectorXf nullspaceTorque = knull * nullqerror - dnull * qvel;
Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * tcpDesiredWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
- // ARMARX_CHECK_EQUAL(jointDesiredTorques.rows(), targets.size());
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(jointDesiredTorques.rows()), targets.size());
for (size_t i = 0; i < targets.size(); ++i)
@@ -257,6 +255,37 @@ void NJointTaskSpaceImpedanceController::setTarget(const Ice::FloatSeq& target,
writeControlStruct();
}
+void NJointTaskSpaceImpedanceController::setImpedanceParameters(const std::string& name, const Ice::FloatSeq& vals, const Ice::Current&)
+{
+ ARMARX_CHECK_EQUAL(vals.size(), 3);
+
+ Eigen::Vector3f vec;
+ for (size_t i = 0; i < 3; ++i)
+ {
+ vec(i) = vals.at(i);
+ }
+
+ LockGuardType guard {controlDataMutex};
+ if (name == "Kpos")
+ {
+ getWriterControlStruct().Kpos = vec;
+ }
+ else if (name == "Kori")
+ {
+ getWriterControlStruct().Kori = vec;
+ }
+ else if (name == "Dpos")
+ {
+ getWriterControlStruct().Dpos = vec;
+ }
+ else if (name == "Dori")
+ {
+ getWriterControlStruct().Dori = vec;
+ }
+ writeControlStruct();
+
+}
+
void armarx::NJointTaskSpaceImpedanceController::rtPreActivateController()
{
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
index 53462f8942496ac25c9e62e2e32dd2e12ffd0d6d..b93979d00f45293e4277b8cd3f7e7f7bd16a8846 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
@@ -39,6 +39,10 @@ namespace armarx
public:
Eigen::VectorXf desiredPose;
Eigen::VectorXf desiredTwist;
+ Eigen::Vector3f Kpos;
+ Eigen::Vector3f Dpos;
+ Eigen::Vector3f Kori;
+ Eigen::Vector3f Dori;
};
/**
* @defgroup Library-NJointTaskSpaceImpedanceController NJointTaskSpaceImpedanceController
@@ -71,6 +75,8 @@ namespace armarx
void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
void setTarget(const Ice::FloatSeq&, const Ice::Current&) override;
+ void setImpedanceParameters(const std::string&, const Ice::FloatSeq&, const Ice::Current&);
+
private:
Eigen::Vector3f desiredPosition;
Eigen::Quaternionf desiredQuaternion;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
index 612e0bc888507ff3a483fbdf33f445b255249465..b51bb59d3907675755f3a65c7278370ea6bb4027 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
@@ -44,12 +44,12 @@ namespace armarx
return "NJointTrajectoryController";
}
- void NJointTrajectoryController::onInitComponent()
+ void NJointTrajectoryController::onInitNJointController()
{
offeringTopic("StaticPlotter");
}
- void NJointTrajectoryController::onConnectComponent()
+ void NJointTrajectoryController::onConnectNJointController()
{
plotter = getTopic<StaticPlotterInterfacePrx>("StaticPlotter");
dbgObs = getTopic<DebugObserverInterfacePrx>("DebugObserver");
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
index e184ffb8f8bc3a691e842f0b7948b3752ce3257b..560f33100464eb9e2994069059f0262925579e88 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
@@ -32,8 +32,8 @@ namespace armarx
// NJointControllerInterface interface
std::string getClassName(const Ice::Current&) const override;
- void onInitComponent() override;
- void onConnectComponent() override;
+ void onInitNJointController() override;
+ void onConnectNJointController() override;
// NJointController interface
void rtPreActivateController() override;
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.h
index 618620785c39e76e3f775d16957b2a7a708fc47d..8c107ae88887a6ba8a98fd298efdd6a3bd520f26 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.h
@@ -32,6 +32,7 @@
#include <VirtualRobot/Robot.h>
+#include <ArmarXCore/util/CPPUtility/Pointer.h>
#include <ArmarXCore/core/Component.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
@@ -50,15 +51,6 @@ namespace armarx
TYPEDEF_PTRS_HANDLE(RobotUnit);
TYPEDEF_PTRS_HANDLE(NJointController);
- template<class T>
- T& CheckedDeref(T* ptr)
- {
- if (!ptr)
- {
- throw std::invalid_argument {"Ptr passed to CheckedDeref is NULL"};
- }
- return *ptr;
- }
template<class Targ, class Src>
Targ& CheckedCastAndDeref(Src* src)
{
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
index 5abecbc624674ca7b139339eb7aef3ab6be58215..8e7ddfa650cc87fd68b5a5a5a658eb42a249a1d7 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
@@ -24,6 +24,9 @@
#include <ArmarXCore/core/util/OnScopeExit.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h>
+#include <ArmarXCore/observers/filters/rtfilters/AverageFilter.h>
+#include <ArmarXCore/observers/filters/rtfilters/ButterworthFilter.h>
#include "RobotUnitModuleControlThread.h"
#include "../NJointControllers/NJointController.h"
@@ -368,9 +371,21 @@ namespace armarx
device->rtReadSensorValues(sensorValuesTimestamp, timeSinceLastIteration);
}
DevicesAttorneyForControlThread::UpdateRobotWithSensorValues(this, rtRobot, rtRobotNodes);
+ rtPostReadSensorDeviceValues(sensorValuesTimestamp, timeSinceLastIteration);
rtGetThreadTimingsSensorDevice().rtMarkRtReadSensorDeviceValuesEnd();
}
+ void ControlThread::rtPostReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ if (dynamicsHelpers)
+ {
+// auto start = IceUtil::Time::now();
+ dynamicsHelpers->update(sensorValuesTimestamp, timeSinceLastIteration);
+// auto end = IceUtil::Time::now();
+ // ARMARX_INFO << "Dynamics duration: " << (end-start).toMicroSeconds();
+ }
+ }
+
void ControlThread::rtRunJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
rtGetThreadTimingsSensorDevice().rtMarkRtRunJointControllersStart();
@@ -607,6 +622,33 @@ namespace armarx
preSwitchSetup_ActivatedNJointControllers.resize(rtGetActivatedNJointControllers().size());
postSwitchSetup_ActivatedNJointControllers.resize(rtGetActivatedNJointControllers().size());
+
+ // setup inverse dynamics
+ if (getProperty<bool>("EnableInverseDynamics").getValue())
+ {
+ RobotUnit* robotUnit = dynamic_cast<RobotUnit*>(this);
+ ARMARX_CHECK_EXPRESSION(robotUnit);
+ std::shared_ptr<DynamicsHelper> dynamicsHelpers(new DynamicsHelper(robotUnit));
+ auto bodySetName = getProperty<std::string>("InverseDynamicsRobotBodySet").getValue();
+ auto rtRobotBodySet = rtGetRobot()->getRobotNodeSet(bodySetName);
+ ARMARX_CHECK_EXPRESSION_W_HINT(rtRobotBodySet, "could not find robot node set with name: " << bodySetName << " - Check property InverseDynamicsRobotBodySet");
+ // rtfilters::RTFilterBasePtr accFilter(new rtfilters::ButterworthFilter(100, 1000, Lowpass, 1));
+ // rtfilters::RTFilterBasePtr velFilter(new rtfilters::ButterworthFilter(100, 1000, Lowpass, 1));
+ rtfilters::RTFilterBasePtr accFilter(new rtfilters::AverageFilter(30));
+ rtfilters::RTFilterBasePtr velFilter(new rtfilters::AverageFilter(30));
+
+ auto setList = armarx::Split(getProperty<std::string>("InverseDynamicsRobotJointSets").getValue(), ",", true, true);
+ for (auto& set : setList)
+ {
+ auto rns = rtGetRobot()->getRobotNodeSet(set);
+ ARMARX_CHECK_EXPRESSION_W_HINT(rns, "could not find robot node set with name: " << set << " - Check property InverseDynamicsRobotJointSets");
+ dynamicsHelpers->addRobotPart(rns, rtRobotBodySet, velFilter, accFilter);
+ ARMARX_VERBOSE << "Added nodeset " << set << " for inverse dynamics";
+ }
+
+ this->dynamicsHelpers = dynamicsHelpers;
+ }
+
}
void ControlThread::_preOnInitRobotUnit()
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
index 33e8a906f57c5ae4ed4ed71c964a06da73eee780..59644ac61b37fd7e2b303fddc109e59fc2d430cd 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
@@ -33,6 +33,7 @@
namespace armarx
{
+ class DynamicsHelper;
class RTThreadTimingsSensorDevice;
namespace RobotUnitModule
{
@@ -51,6 +52,18 @@ namespace armarx
"An Error will be printed, If the execution time in micro seconds of a NJointController "
"exceeds this parameter times the number of ControlDevices."
);
+ defineOptionalProperty<bool>("EnableInverseDynamics", false,
+ "If true, inverse dynamics are calculated for all joints given in the "
+ "InverseDynamicsRobotJointSets property during each loop of "
+ "the rt control thread.");
+ defineOptionalProperty<std::string>(
+ "InverseDynamicsRobotJointSets", "LeftArm,RightArm",
+ "Comma separated list of robot nodesets for which the inverse dynamics should be calculcated."
+ );
+ defineOptionalProperty<std::string>(
+ "InverseDynamicsRobotBodySet", "RobotCol",
+ "Robot nodeset of which the masses should be used for the inverse dynamics"
+ );
}
};
@@ -182,6 +195,12 @@ namespace armarx
* @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
*/
void rtReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ /**
+ * @brief Execute code after updating the sensor values. Default implementation calculates the inverse dynamics.
+ * @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
+ * @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
+ */
+ void virtual rtPostReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
/// @brief Deactivates all NJointControllers generating invalid targets.
void rtHandleInvalidTargets();
/// @brief Calls \ref JointController::rtResetTarget for all active Joint controllers
@@ -333,6 +352,9 @@ namespace armarx
std::vector<NJointController*> postSwitchSetup_ActivatedNJointControllers;
std::atomic<EmergencyStopStateRequest> emergencyStopStateRequest {EmergencyStopStateRequest::RequestNone};
+
+ std::shared_ptr<DynamicsHelper> dynamicsHelpers;
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// /////////////////////////////////////// Attorneys ////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
index df4a0f5503a21b05f1df52686ca34c42c90ae076..c24b433f99d08cad72c4d91b8893b4ce20f5987f 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
@@ -572,7 +572,7 @@ namespace armarx
return !jointCtrl;
}))
{
- ARMARX_WARNING << "armarx::RobotUnit::publish: Some activated JointControllers are reported to be nullptr! "
+ ARMARX_WARNING << deactivateSpam(5) << "armarx::RobotUnit::publish: Some activated JointControllers are reported to be nullptr! "
<< "(did you forget to call rtSwitchControllerSetup()?)";
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
index 94c7b9d8481c0a58a0e9de33cadabdc828e53a75..3be94c7a50619cfe95ff85046d544afd032e9888 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
@@ -41,6 +41,11 @@ namespace armarx
return robotPlatformName;
}
+ std::string RobotData::getRobotPlatformInstanceName() const
+ {
+ return robotPlatformInstanceName;
+ }
+
const std::string& RobotData::getRobotNodetSeName() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -82,6 +87,8 @@ namespace armarx
return clone;
}
+
+
void RobotData::_initVirtualRobot()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -94,6 +101,7 @@ namespace armarx
robotProjectName = getProperty<std::string>("RobotFileNameProject").getValue();
robotFileName = getProperty<std::string>("RobotFileName").getValue();
robotPlatformName = getProperty<std::string>("PlatformName").getValue();
+ robotPlatformInstanceName = getProperty<std::string>("PlatformInstanceName").getValue();
//load robot
{
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
index 0c0457b99f6b2242e82adefa656718ddcfb42c15..ff1e0637952cff7229a8ece8af322cfb205bd23f 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
@@ -55,7 +55,11 @@ namespace armarx
"Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
defineOptionalProperty<std::string>(
"PlatformName", "Platform",
- "Name of the platform (will publish values on PlatformName + 'State')");
+ "Name of the platform needs to correspond to a node in the virtual robot.");
+ defineOptionalProperty<std::string>(
+ "PlatformInstanceName", "Platform",
+ "Name of the platform instance (will publish values on PlatformInstanceName + 'State')");
+
}
};
@@ -114,6 +118,12 @@ namespace armarx
*/
std::string getRobotName() const;
+ /**
+ * @brief Returns the name of the robot platform instance. Used for the platform topic: RobotPlatformInstance + "State"
+ */
+ std::string getRobotPlatformInstanceName() const;
+
+
/**
* @brief Returns a clone of the robot's model
* @return A clone of the robot's model
@@ -122,6 +132,7 @@ namespace armarx
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////////////// Data ///////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
+
private:
/// @brief The name of the robot's RobotNodeSet
std::string robotNodeSetName;
@@ -131,6 +142,8 @@ namespace armarx
std::string robotFileName;
/// @brief The name of the robot's platform
std::string robotPlatformName;
+ /// @brief The name of the robot's platform instance
+ std::string robotPlatformInstanceName;
/// @brief The robot's model.
VirtualRobot::RobotPtr robot;
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index be11508013b7f3276c2b487476179136e9e557ba..27c760de9e0fbce031aa522e3f24b887ec17d25a 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -266,6 +266,8 @@ namespace armarx
properties->setProperty(confPre + "RobotNodeSetName", _module<RobotData>().getRobotNodetSeName());
properties->setProperty(confPre + "RobotFileName", _module<RobotData>().getRobotFileName());
properties->setProperty(confPre + "RobotFileNameProject", _module<RobotData>().getRobotProjectName());
+ properties->setProperty(confPre + "TopicPrefix", getProperty<std::string>("KinematicUnitNameTopicPrefix"));
+
ARMARX_DEBUG << "creating unit " << configName << " using these properties: " << properties->getPropertiesForPrefix("");
IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
@@ -317,7 +319,7 @@ namespace armarx
Ice::PropertiesPtr properties = getIceProperties()->clone();
//properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
//fill properties
- properties->setProperty(confPre + "PlatformName", _module<RobotData>().getRobotPlatformName());
+ properties->setProperty(confPre + "PlatformName", _module<RobotData>().getRobotPlatformInstanceName());
ARMARX_DEBUG << "creating unit " << configName << " using these properties: " << properties->getPropertiesForPrefix("");
IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
//config
@@ -352,6 +354,24 @@ namespace armarx
unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(_module<RobotData>().getRobotPlatformName());
//add
addUnit(std::move(unit));
+
+
+ NJointHolonomicPlatformGlobalPositionControllerConfigPtr configGlobalPositionCtrlCfg = new NJointHolonomicPlatformGlobalPositionControllerConfig;
+ configGlobalPositionCtrlCfg->platformName = _module<RobotData>().getRobotPlatformName();
+ auto ctrlGlobalPosition = NJointHolonomicPlatformGlobalPositionControllerPtr::dynamicCast(
+ _module<ControllerManagement>().createNJointController(
+ "NJointHolonomicPlatformGlobalPositionController",
+ getName() + "_" + configName + "_GlobalPositionContoller",
+ configGlobalPositionCtrlCfg, false, true
+ )
+ );
+ ARMARX_CHECK_EXPRESSION(ctrlGlobalPosition);
+ unit->pt = ctrl;
+ unit->globalPosCtrl = ctrlGlobalPosition;
+
+ unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(_module<RobotData>().getRobotPlatformName());
+ //add
+ addUnit(std::move(unit));
}
void Units::initializeForceTorqueUnit()
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
index 7978f56c700d39eda754f675f8889e013afacc2d..54d6c2dcc75ad6b33f9f1a865676da86d2d9833c 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
@@ -39,6 +39,10 @@ namespace armarx
defineOptionalProperty<std::string>(
"KinematicUnitName", "KinematicUnit",
"The name of the created kinematic unit");
+ defineOptionalProperty<std::string>(
+ "KinematicUnitNameTopicPrefix", "",
+ "Prefix for the kinematic sensor values topic");
+
defineOptionalProperty<std::string>(
"PlatformUnitName", "PlatformUnit",
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
index 7573abc1e5980a0762bfe5e9e098e21ff48a9c23..827b23913b7a840f512ec60ad5e55d7087de1cf2 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
@@ -101,6 +101,22 @@ namespace armarx
return svi;
}
};
+ class SensorValue1DoFInverseDynamicsTorque :
+ virtual public SensorValue1DoFGravityTorque
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ float inverseDynamicsTorque = 0.0f;
+ float inertiaTorque = 0.0f;
+ static SensorValueInfo<SensorValue1DoFInverseDynamicsTorque> GetClassMemberInfo()
+ {
+ SensorValueInfo<SensorValue1DoFInverseDynamicsTorque> svi;
+ svi.addBaseClass<SensorValue1DoFGravityTorque>();
+ svi.addMemberVariable(&SensorValue1DoFInverseDynamicsTorque::inverseDynamicsTorque, "inverseDynamicsTorque");
+ svi.addMemberVariable(&SensorValue1DoFInverseDynamicsTorque::inertiaTorque, "inertiaTorque");
+ return svi;
+ }
+ };
class SensorValue1DoFActuator :
@@ -108,7 +124,7 @@ namespace armarx
virtual public SensorValue1DoFActuatorVelocity,
virtual public SensorValue1DoFActuatorAcceleration,
virtual public SensorValue1DoFActuatorTorque,
- virtual public SensorValue1DoFGravityTorque
+ virtual public SensorValue1DoFInverseDynamicsTorque
{
public:
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
@@ -119,7 +135,7 @@ namespace armarx
svi.addBaseClass<SensorValue1DoFActuatorVelocity>();
svi.addBaseClass<SensorValue1DoFActuatorAcceleration>();
svi.addBaseClass<SensorValue1DoFActuatorTorque>();
- svi.addBaseClass<SensorValue1DoFGravityTorque>();
+ svi.addBaseClass<SensorValue1DoFInverseDynamicsTorque>();
return svi;
}
};
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
index 36b9181fd443f9dc1028efd53d0ef2b63bb6df5c..2e0408925c30f5ee0d8b9304b9efb220bf6bd057 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
@@ -20,6 +20,8 @@
* GNU General Public License
*/
#include "KinematicSubUnit.h"
+#include <VirtualRobot/RobotNodeSet.h>
+#include <RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.h>
#include <RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.h>
@@ -43,6 +45,9 @@ void armarx::KinematicSubUnit::setupData(
ARMARX_CHECK_EXPRESSION(!robot);
ARMARX_CHECK_EXPRESSION(rob);
robot = rob;
+ robot->setUpdateCollisionModel(false);
+ robot->setUpdateVisualization(false);
+ robot->setThreadsafe(false);
ARMARX_CHECK_EXPRESSION(!robotUnit);
ARMARX_CHECK_EXPRESSION(newRobotUnit);
@@ -55,6 +60,15 @@ void armarx::KinematicSubUnit::setupData(
devs = std::move(newDevs);
controlDeviceHardwareControlModeGroups = controlDeviceHardwareControlModeGrps;
controlDeviceHardwareControlModeGroupsMerged = controlDeviceHardwareControlModeGrpsMerged;
+
+ auto nodes = robot->getRobotNodes();
+ for (auto& node : nodes)
+ {
+ if ((node->isRotationalJoint() || node->isTranslationalJoint()) && !devs.count(node->getName()))
+ {
+ sensorLessJoints.push_back(node);
+ }
+ }
}
void armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc, const armarx::JointAndNJointControllers& c)
@@ -128,6 +142,20 @@ void armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc, const
{
ARMARX_WARNING << deactivateSpam(5) << "these actuators have no sensor!:\n" << actuatorsWithoutSensor;
}
+
+ // update the joint values of linked joints
+ robot->setJointValues(ang);
+ auto nodes = robot->getRobotNodes();
+ for (auto& node : nodes)
+ {
+ node->updatePose(false);
+ }
+ for (auto& node : sensorLessJoints)
+ {
+ ang[node->getName()] = node->getJointValue();
+ }
+
+
ARMARX_DEBUG << deactivateSpam(30) << "reporting updated data:\n"
<< ctrlModes.size() << " \tcontrol modes (updated = " << ctrlModesAValueChanged << ")\n"
<< ang.size() << " \tjoint angles (updated = " << angAValueChanged << ")\n"
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
index 01183607f958e2b53b67e39966846d4fdd33fa0c..65e8263cc44f4669dd43f0c4b084840f76b2b847 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
@@ -104,6 +104,7 @@ namespace armarx
std::vector<std::set<std::string>> controlDeviceHardwareControlModeGroups;
std::set<std::string> controlDeviceHardwareControlModeGroupsMerged;
IceReportSkipper reportSkipper;
+ std::vector<VirtualRobot::RobotNodePtr> sensorLessJoints;
template<class ValueT, class SensorValueT, ValueT SensorValueT::* Member>
static void UpdateNameValueMap(std::map<std::string, ValueT>& nvm, const std::string& name, const SensorValueBase* sv, bool& changeState)
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
index bac5190ed8f1a3f9a68b1cdfa304b8ea7fb00b61..428cc7c295396938bed6d9d6210296dc14ffe65d 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
@@ -32,6 +32,7 @@
#include <RobotAPI/components/units/PlatformUnit.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h>
#include <RobotAPI/libraries/core/Pose.h>
#include "RobotUnitSubUnit.h"
@@ -66,6 +67,7 @@ namespace armarx
NJointHolonomicPlatformUnitVelocityPassThroughControllerPtr pt;
NJointHolonomicPlatformRelativePositionControllerPtr relativePosCtrl;
+ NJointHolonomicPlatformGlobalPositionControllerPtr globalPosCtrl;
std::size_t platformSensorIndex;
protected:
Eigen::Matrix4f getRelativePose() const;
diff --git a/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp b/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp
index 1b856974e9a0f1d4be92c2125a62c03370054159..5f202a8f8da633a10380e0bc2f4e0f77d4ca179f 100644
--- a/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp
@@ -25,7 +25,8 @@
#define CREATE_LOGFILES
#include <random>
#include <iostream>
-
+#include <filesystem>
+#include "../util/CtrlUtil.h"
#include <boost/algorithm/clamp.hpp>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/algorithm.h>
@@ -34,7 +35,7 @@
using namespace armarx;
//params for random tests
const std::size_t tries = 1;
-const std::size_t ticks = 20000; // each tick is 0.75 to 1.25 ms
+const std::size_t ticks = 5000; // each tick is 0.75 to 1.25 ms
//helpers for logging
#ifdef CREATE_LOGFILES
#define LOG_CONTROLLER_DATA_WRITE_TO(name) change_logging_file(name)
@@ -43,7 +44,7 @@ const std::size_t ticks = 20000; // each tick is 0.75 to 1.25 ms
#include <boost/filesystem.hpp>
#include <fstream>
-static const boost::filesystem::path fpath
+static const std::filesystem::path fpath
{
"controller_logfiles/"
};
@@ -58,17 +59,17 @@ void change_logging_file(const std::string& name)
}
if (!isSetup)
{
- boost::filesystem::create_directories(fpath);
+ std::filesystem::create_directories(fpath);
//create the python evaluation file
- boost::filesystem::path tmppath(fpath / "eval.py");
+ std::filesystem::path tmppath(fpath / "eval.py");
f.open(tmppath.string());
#include "eval_script.inc"
isSetup = true;
f.close();
}
- boost::filesystem::path tmppath(fpath / name);
+ std::filesystem::path tmppath(fpath / (name + ".log"));
f.open(tmppath.string());
- std::cout << "now writing to: " << boost::filesystem::absolute(tmppath).string() << "\n";
+ std::cout << "now writing to: " << std::filesystem::absolute(tmppath).string() << "\n";
}
#else
@@ -88,36 +89,38 @@ static std::mt19937 gen {getSeed()};
struct Simulation
{
- float time = 0;
- float dt = 0;
- float maxDt = 0.001;
+ double time = 0;
+ double dt = 0;
+ double maxDt = 0.01;
+
+ double curpos = 0;
+ double oldpos = 0;
+ double targpos = 0;
+ double posHiHard = M_PI;
+ double posLoHard = -M_PI;
+ double posHi = M_PI;
+ double posLo = -M_PI;
- float curpos = 0;
- float oldpos = 0;
- float targpos = 0;
- float posHiHard = M_PI;
- float posLoHard = -M_PI;
- float posHi = M_PI;
- float posLo = -M_PI;
+ double curvel = 0;
+ double oldvel = 0;
+ double targvel = 0;
+ double maxvel = 10;
- float curvel = 0;
- float oldvel = 0;
- float targvel = 0;
- float maxvel = 10;
+ double curacc = 0;
+ double oldacc = 0;
+ double acc = 0;
+ double dec = 0;
- float curacc = 0;
- float oldacc = 0;
- float acc = 0;
- float dec = 0;
+ double jerk = 0;
- float brakingDist = 0;
- float posAfterBraking = 0;
+ double brakingDist = 0;
+ double posAfterBraking = 0;
- std::uniform_real_distribution<float> vDist;
- std::uniform_real_distribution<float> aDist;
- std::uniform_real_distribution<float> pDist;
- std::uniform_real_distribution<float> tDist;
+ std::uniform_real_distribution<double> vDist;
+ std::uniform_real_distribution<double> aDist;
+ std::uniform_real_distribution<double> pDist;
+ std::uniform_real_distribution<double> tDist;
void reset()
{
@@ -135,26 +138,27 @@ struct Simulation
oldacc = 0;
acc = 0;
dec = 0;
+ jerk = 0;
brakingDist = 0;
posAfterBraking = 0;
- vDist = std::uniform_real_distribution<float> { -maxvel, maxvel};
- aDist = std::uniform_real_distribution<float> {maxvel / 4, maxvel * 4 };
- tDist = std::uniform_real_distribution<float> {maxDt * 0.75f, maxDt * 1.25f};
- pDist = std::uniform_real_distribution<float> {posLoHard, posHiHard};
+ vDist = std::uniform_real_distribution<double> { -maxvel, maxvel};
+ aDist = std::uniform_real_distribution<double> {maxvel / 4, maxvel * 4 };
+ tDist = std::uniform_real_distribution<double> {maxDt * 0.75f, maxDt * 1.25f};
+ pDist = std::uniform_real_distribution<double> {posLoHard, posHiHard};
}
//updating
template<class FNC>
void tick(FNC& callee)
{
- dt = tDist(gen);
+ dt = maxDt;//tDist(gen);
callee();
log();
}
- void updateVel(float newvel)
+ void updateVel(double newvel)
{
BOOST_CHECK(std::isfinite(newvel));
//save old
@@ -164,8 +168,27 @@ struct Simulation
//update
curvel = newvel;
- curacc = std::abs(curvel - oldvel) / dt;
- curpos += curvel * dt;
+ curacc = (curvel - oldvel) / dt;
+ jerk = (curacc - oldacc) / dt;
+ curpos += ctrlutil::s(dt, 0, curvel, curacc, 0/*math::MathUtils::Sign(curacc-oldacc) * jerk*/);
+ time += dt;
+ brakingDist = brakingDistance(curvel, dec);
+ posAfterBraking = curpos + brakingDist;
+ }
+
+ void updatePos(double newPos)
+ {
+ BOOST_CHECK(std::isfinite(newPos));
+ //save old
+ oldvel = curvel;
+ oldpos = curpos;
+ oldacc = curacc;
+
+ //update
+ curvel = (newPos - oldpos) / dt;
+ curacc = (curvel - oldvel) / dt;
+ jerk = (curacc - oldacc) / dt;
+ curpos = newPos;
time += dt;
brakingDist = brakingDistance(curvel, dec);
posAfterBraking = curpos + brakingDist;
@@ -195,13 +218,13 @@ struct Simulation
if (std::abs(oldvel) > std::abs(curvel))
{
// deceleration check with PID controller for motion-finish does not work -> check disabled
- // //we decelerated
- // if (!(curacc < dec * 1.01))
- // {
- // std::cout << "Time[" << time << "] violated deceleration bound! vold " << oldvel << " / vnew " << curvel << " / dv " << std::abs(oldvel - curvel) << " / dt " << dt << " / dec " << dec
- // << " / (targetv " << targvel << ")\n";
- // }
- // BOOST_CHECK_LE(curacc, dec * 1.01);
+ //we decelerated
+ if (!(curacc < dec * 1.01))
+ {
+ std::cout << "Time[" << time << "] violated deceleration bound! vold " << oldvel << " / vnew " << curvel << " / dv " << std::abs(oldvel - curvel) << " / dt " << dt << " / dec " << dec
+ << " / (targetv " << targvel << ")\n";
+ }
+ BOOST_CHECK_LE(curacc, dec * 1.01);
}
else
{
@@ -219,182 +242,159 @@ struct Simulation
void writeHeader(const std::string& name)
{
LOG_CONTROLLER_DATA_WRITE_TO(name);
- LOG_CONTROLLER_DATA("time curpos targpos posHiHard posLoHard posHi posLo curvel targvel maxvel curacc acc dec brakingDist posAfterBraking");
+ LOG_CONTROLLER_DATA("time curpos targpos posHiHard posLoHard posHi posLo curvel targvel maxv curacc acc dec brakingDist posAfterBraking");
reset();
}
void log()
{
- //output a neg val for dec and a pos val for acc
- float outputacc;
- if (sign(curvel) != sign(oldvel))
- {
- // cant determine the current acceleration correctly (since we have an acceleration and deceleration phase)
- outputacc = 0;
- }
- else
- {
- outputacc = curacc;
- if (std::abs(oldvel) > std::abs(curvel))
- {
- //we decelerated -> negative sign
- outputacc *= -1;
- }
- }
+ // //output a neg val for dec and a pos val for acc
+ // double outputacc;
+ // if (sign(curvel) != sign(oldvel))
+ // {
+ // // cant determine the current acceleration correctly (since we have an acceleration and deceleration phase)
+ // outputacc = 0;
+ // }
+ // else
+ // {
+ // outputacc = curacc;
+ // if (std::abs(oldvel) > std::abs(curvel))
+ // {
+ // //we decelerated -> negative sign
+ // outputacc *= -1;
+ // }
+ // }
LOG_CONTROLLER_DATA(time << " " <<
curpos << " " << targpos << " " << posHiHard << " " << posLoHard << " " << posHi << " " << posLo << " " <<
curvel << " " << targvel << " " << maxvel << " " <<
- outputacc << " " << acc << " " << -dec << " " <<
+ curacc << " " << acc << " " << -dec << " " <<
brakingDist << " " << posAfterBraking);
}
};
-
-BOOST_AUTO_TEST_CASE(velocityControlWithAccelerationBoundsTest)
+BOOST_AUTO_TEST_CASE(VelocityControllerWithRampedAccelerationAndPositionBoundsTest)
{
- return;
- std::cout << "starting velocityControlWithAccelerationBoundsTest \n";
- Simulation s;
- s.posHi = 0;
- s.posLo = 0;
- s.posHiHard = 0;
- s.posLoHard = 0;
-
- float directSetVLimit = 0.005;
-
-
- auto testTick = [&]
- {
- VelocityControllerWithAccelerationBounds ctrl;
- ctrl.dt = s.dt;
- ctrl.maxDt = s.maxDt;
- ctrl.currentV = s.curvel;
- ctrl.targetV = s.targvel;
- ctrl.maxV = s.maxvel;
- ctrl.acceleration = s.acc;
- ctrl.deceleration = s.dec;
- ctrl.directSetVLimit = directSetVLimit;
- BOOST_CHECK(ctrl.validParameters());
- // s.updateVel(velocityControlWithAccelerationBounds(s.dt, s.maxDt, s.curvel, s.targvel, s.maxvel, s.acc, s.dec, directSetVLimit));
- s.updateVel(ctrl.run());
- s.checkVel();
- s.checkAcc();
- };
- std::cout << "random tests\n";
- for (std::size_t try_ = 0; try_ < tries; ++ try_)
- {
- s.writeHeader("velCtrl+acc_random_" + to_string(try_));
- s.curvel = s.vDist(gen);
- s.targvel = s.vDist(gen);
- s.acc = s.aDist(gen);
- s.dec = s.aDist(gen);
- directSetVLimit = (std::min(s.acc, s.dec) * s.maxDt * 0.75f / 2.f) * 0.99f; // vlimit = accmin*dtmin/2
-
- s.log();
- for (std::size_t tick = 0; tick < ticks; ++tick)
- {
- s.tick(testTick);
- }
- BOOST_CHECK_EQUAL(s.curvel, s.targvel);
- }
- std::cout << "bounds tests\n";
- std::cout << "TODO\n";
- ///TODO
-
- std::cout << "done velocityControlWithAccelerationBoundsTest \n";
-}
-
-BOOST_AUTO_TEST_CASE(velocityControlWithAccelerationAndPositionBoundsTest)
-{
- return;
- std::cout << "starting velocityControlWithAccelerationAndPositionBoundsTest \n";
+ std::cout << "starting VelocityControllerWithRampedAccelerationTest \n";
Simulation s;
+ // s.posHi = 0;
+ // s.posLo = 0;
- // const float positionSoftLimitViolationVelocity = 0.1;
- float directSetVLimit = 0.005;
- s.posLo = s.posLoHard * 0.99;
- s.posHi = s.posHiHard * 0.99;
+ auto distPosLo = std::uniform_real_distribution<double> { -M_PI, -M_PI_4};
+ auto distPosHi = std::uniform_real_distribution<double> { M_PI_4, M_PI};
+ s.posLo = distPosLo(gen);
+ s.posHi = distPosHi(gen);
+ double p = 20.5;
+ VelocityControllerWithRampedAccelerationAndPositionBounds ctrl;
auto testTick = [&]
{
- VelocityControllerWithAccelerationAndPositionBounds ctrl;
ctrl.dt = s.dt;
ctrl.maxDt = s.maxDt;
- ctrl.currentV = s.curvel;
- ctrl.targetV = s.targvel;
+ // ctrl.currentV = s.curvel;
+ // ctrl.currentAcc = s.curacc;
ctrl.maxV = s.maxvel;
- ctrl.acceleration = s.acc;
- ctrl.deceleration = s.dec;
- ctrl.directSetVLimit = directSetVLimit;
- ctrl.currentPosition = s.curpos;
- ctrl.positionLimitLoSoft = s.posLo;
+ ctrl.targetV = s.targvel;
ctrl.positionLimitHiSoft = s.posHi;
+ ctrl.positionLimitLoSoft = s.posLo;
+ ctrl.currentPosition = s.curpos;
+ ctrl.deceleration = 40;
+
+ // ctrl.accuracy = 0.00001;
+ // ctrl.pControlPosErrorLimit = pControlPosErrorLimit;
+ // ctrl.pControlVelLimit = pControlVelLimit;
+ // ctrl.p = p;//ctrl.calculateProportionalGain();
+ // ctrl.usePIDAtEnd = false;
+ // ARMARX_INFO << VAROUT(ctrl.p);
BOOST_CHECK(ctrl.validParameters());
- s.updateVel(ctrl.run());
- //s.updateVel(velocityControlWithAccelerationAndPositionBounds(
+ auto r = ctrl.run();
+ // ARMARX_INFO << "State: " << (int)ctrl.calcState() << " " << VAROUT(r.acceleration) << VAROUT(r.velocity) << VAROUT(r.jerk);
+ s.updateVel(r.velocity);
+ ctrl.currentV = r.velocity;
+ ctrl.currentAcc = r.acceleration;
+ //s.updateVel(positionThroughVelocityControlWithAccelerationBounds(
//s.dt, s.maxDt,
- //s.curvel, s.targvel, s.maxvel,
+ //s.curvel, s.maxvel,
//s.acc, s.dec,
- //directSetVLimit,
- //s.curpos,
- //s.posLo, s.posHi,
- //// positionSoftLimitViolationVelocity,
- //s.posLoHard, s.posHiHard
+ //s.curpos, s.targpos,
+ //pControlPosErrorLimit, pControlVelLimit, p
//));
- s.checkPos();
- s.checkVel();
+ // s.checkVel();
+ // s.checkAcc();
};
std::cout << "random tests\n";
for (std::size_t try_ = 0; try_ < tries; ++ try_)
{
- s.writeHeader("velCtrl+acc+pos_random_" + to_string(try_));
- s.curvel = s.vDist(gen);
+ ctrl = VelocityControllerWithRampedAccelerationAndPositionBounds();
+ s.writeHeader("RampedAccPositionLimitedVelCtrl+acc_random_" + to_string(try_));
+ s.maxvel = 5;//std::abs(s.vDist(gen)) + 1;
+ s.curvel = 5;//armarx::math::MathUtils::LimitTo(s.vDist(gen), s.maxvel);
+ ctrl.currentV = s.curvel;
s.curpos = s.pDist(gen);
- s.targvel = s.vDist(gen);
- s.acc = s.aDist(gen);
- s.dec = s.aDist(gen);
- directSetVLimit = (std::min(s.acc, s.dec) * s.maxDt * 0.75f / 2.f) * 0.99f; // vlimit = accmin*dtmin/2
+ ctrl.currentAcc = s.pDist(gen);
+ s.curacc = ctrl.currentAcc;
+ ARMARX_INFO << VAROUT(s.dt) << VAROUT(s.curpos) << VAROUT(s.targvel) << " " << VAROUT(s.acc) << VAROUT(s.dec) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
+ // p = ((std::min(s.acc,s.dec)*s.maxDt*0.75-pControlVelLimit)/pControlPosErrorLimit) * 0.99f; // sometimes <0
+ ctrl.jerk = 100;//(random() % 100) + 10;
s.log();
+ double stopTime = -1;
for (std::size_t tick = 0; tick < ticks; ++tick)
{
+ // std::cout << std::endl;
+ // s.targpos = tick > 900 ? 3 : 5;
+
s.tick(testTick);
+ ARMARX_INFO << /*deactivateSpam(0.01) <<*/ VAROUT(s.dt * tick) << VAROUT(s.curpos) << VAROUT(s.targvel) << VAROUT(s.curacc) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
+ if (std::abs(s.curvel - s.targvel) < 0.001 && stopTime < 0)
+ {
+ stopTime = s.dt * (tick + 20); // let it run for some more ticks to make sure the velocity is stable
+ }
+ if (stopTime > 0 && s.dt * tick > stopTime)
+ {
+ break;
+ }
}
+ BOOST_CHECK_LE(std::abs(s.curvel - s.targvel), 0.001); // allow error of 0.5729577951308232 deg
}
std::cout << "bounds tests\n";
std::cout << "TODO\n";
///TODO
- std::cout << "done velocityControlWithAccelerationAndPositionBoundsTest \n";
+ std::cout << "done positionThroughVelocityControlWithAccelerationBounds \n";
}
-BOOST_AUTO_TEST_CASE(positionThroughVelocityControlWithAccelerationBoundsTest)
+BOOST_AUTO_TEST_CASE(VelocityControllerWithRampedAccelerationTest)
{
- std::cout << "starting positionThroughVelocityControlWithAccelerationBounds \n";
+ return;
+ std::cout << "starting VelocityControllerWithRampedAccelerationTest \n";
Simulation s;
s.posHi = 0;
s.posLo = 0;
- float p = 20.5;
+ double p = 20.5;
+ VelocityControllerWithRampedAcceleration ctrl;
auto testTick = [&]
{
- PositionThroughVelocityControllerWithAccelerationBounds ctrl;
ctrl.dt = s.dt;
ctrl.maxDt = s.maxDt;
- ctrl.currentV = s.curvel;
+ // ctrl.currentV = s.curvel;
+ // ctrl.currentAcc = s.curacc;
ctrl.maxV = s.maxvel;
- ctrl.acceleration = s.acc;
- ctrl.deceleration = s.dec;
- ctrl.currentPosition = s.curpos;
- ctrl.targetPosition = s.targpos;
- ctrl.accuracy = 0.0;
+ ctrl.targetV = s.targvel;
+ // ctrl.accuracy = 0.00001;
// ctrl.pControlPosErrorLimit = pControlPosErrorLimit;
// ctrl.pControlVelLimit = pControlVelLimit;
- ctrl.p = p;
+ // ctrl.p = p;//ctrl.calculateProportionalGain();
+ // ctrl.usePIDAtEnd = false;
+ // ARMARX_INFO << VAROUT(ctrl.p);
BOOST_CHECK(ctrl.validParameters());
- s.updateVel(ctrl.run());
+ auto r = ctrl.run();
+ // ARMARX_INFO << "State: " << (int)ctrl.calcState() << " " << VAROUT(r.acceleration) << VAROUT(r.velocity) << VAROUT(r.jerk);
+ s.updateVel(r.velocity);
+ ctrl.currentV = r.velocity;
+ ctrl.currentAcc = r.acceleration;
//s.updateVel(positionThroughVelocityControlWithAccelerationBounds(
//s.dt, s.maxDt,
//s.curvel, s.maxvel,
@@ -402,33 +402,42 @@ BOOST_AUTO_TEST_CASE(positionThroughVelocityControlWithAccelerationBoundsTest)
//s.curpos, s.targpos,
//pControlPosErrorLimit, pControlVelLimit, p
//));
- s.checkVel();
- s.checkAcc();
+ // s.checkVel();
+ // s.checkAcc();
};
std::cout << "random tests\n";
for (std::size_t try_ = 0; try_ < tries; ++ try_)
{
- s.writeHeader("posViaVelCtrl+acc_random_" + to_string(try_));
+ ctrl = VelocityControllerWithRampedAcceleration();
+ s.writeHeader("RampedAccVelCtrl+acc_random_" + to_string(try_));
s.maxvel = std::abs(s.vDist(gen)) + 1;
s.curvel = armarx::math::MathUtils::LimitTo(s.vDist(gen), s.maxvel);
- s.curpos = s.pDist(gen);
- s.targpos = s.pDist(gen);
- s.acc = s.aDist(gen);
- s.dec = s.aDist(gen);
- ARMARX_INFO << VAROUT(s.curpos) << "TargetPos: " << s.targpos << VAROUT(s.acc) << VAROUT(s.dec) << VAROUT(s.maxvel) << VAROUT(s.curvel);
+ ctrl.currentV = s.curvel;
+ s.curpos = 1;//s.pDist(gen);
+ ctrl.currentAcc = s.pDist(gen);
+ ARMARX_INFO << VAROUT(s.dt) << VAROUT(s.curpos) << VAROUT(s.targvel) << " " << VAROUT(s.acc) << VAROUT(s.dec) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
// p = ((std::min(s.acc,s.dec)*s.maxDt*0.75-pControlVelLimit)/pControlPosErrorLimit) * 0.99f; // sometimes <0
+ ctrl.jerk = (random() % 100) + 10;
s.log();
+ double stopTime = -1;
for (std::size_t tick = 0; tick < ticks; ++tick)
{
+ // std::cout << std::endl;
+ // s.targpos = tick > 900 ? 3 : 5;
+
s.tick(testTick);
- ARMARX_INFO << deactivateSpam(0.01) << VAROUT(s.curpos) << "TargetPos: " << s.targpos << VAROUT(s.acc) << VAROUT(s.dec) << VAROUT(s.maxvel) << VAROUT(s.curvel);
- if (std::abs(s.curpos - s.targpos) < 0.01)
+ // ARMARX_INFO << /*deactivateSpam(0.01) <<*/ VAROUT(s.dt * tick) << VAROUT(s.curpos) << VAROUT(s.targvel) << VAROUT(s.curacc) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
+ if (std::abs(s.curvel - s.targvel) < 0.001 && stopTime < 0)
+ {
+ stopTime = s.dt * (tick + 20); // let it run for some more ticks to make sure the velocity is stable
+ }
+ if (stopTime > 0 && s.dt * tick > stopTime)
{
break;
}
}
- BOOST_CHECK_LE(std::abs(s.curpos - s.targpos), 0.01); // allow error of 0.5729577951308232 deg
+ BOOST_CHECK_LE(std::abs(s.curvel - s.targvel), 0.001); // allow error of 0.5729577951308232 deg
}
std::cout << "bounds tests\n";
std::cout << "TODO\n";
@@ -437,140 +446,505 @@ BOOST_AUTO_TEST_CASE(positionThroughVelocityControlWithAccelerationBoundsTest)
std::cout << "done positionThroughVelocityControlWithAccelerationBounds \n";
}
-BOOST_AUTO_TEST_CASE(positionThroughVelocityControlWithAccelerationAndPositionBoundsTest)
+
+//BOOST_AUTO_TEST_CASE(velocityControlWithAccelerationBoundsTest)
+//{
+// std::cout << "starting velocityControlWithAccelerationBoundsTest \n";
+// Simulation s;
+// s.posHi = 0;
+// s.posLo = 0;
+// s.posHiHard = 0;
+// s.posLoHard = 0;
+
+// double directSetVLimit = 0.005;
+
+
+// auto testTick = [&]
+// {
+// VelocityControllerWithAccelerationBounds ctrl;
+// ctrl.dt = s.dt;
+// ctrl.maxDt = s.maxDt;
+// ctrl.currentV = s.curvel;
+// ctrl.targetV = s.targvel;
+// ctrl.maxV = s.maxvel;
+// ctrl.acceleration = s.acc;
+// ctrl.deceleration = s.dec;
+// ctrl.directSetVLimit = directSetVLimit;
+// BOOST_CHECK(ctrl.validParameters());
+// // s.updateVel(velocityControlWithAccelerationBounds(s.dt, s.maxDt, s.curvel, s.targvel, s.maxvel, s.acc, s.dec, directSetVLimit));
+// s.updateVel(ctrl.run());
+// s.checkVel();
+// s.checkAcc();
+// };
+
+// std::cout << "random tests\n";
+// for (std::size_t try_ = 0; try_ < tries; ++ try_)
+// {
+// s.writeHeader("velCtrl+acc_random_" + to_string(try_));
+// s.curvel = s.vDist(gen);
+// s.targvel = s.vDist(gen);
+// s.acc = s.aDist(gen);
+// s.dec = s.aDist(gen);
+// directSetVLimit = (std::min(s.acc, s.dec) * s.maxDt * 0.75f / 2.f) * 0.99f; // vlimit = accmin*dtmin/2
+
+// s.log();
+// for (std::size_t tick = 0; tick < ticks; ++tick)
+// {
+// s.tick(testTick);
+// }
+// BOOST_CHECK(std::abs(s.curvel - s.targvel) < 0.001);
+// }
+// std::cout << "bounds tests\n";
+// std::cout << "TODO\n";
+// ///TODO
+
+// std::cout << "done velocityControlWithAccelerationBoundsTest \n";
+//}
+
+//BOOST_AUTO_TEST_CASE(velocityControlWithAccelerationAndPositionBoundsTest)
+//{
+// std::cout << "starting velocityControlWithAccelerationAndPositionBoundsTest \n";
+// Simulation s;
+
+// // const double positionSoftLimitViolationVelocity = 0.1;
+// double directSetVLimit = 0.005;
+// s.posLo = s.posLoHard * 0.99;
+// s.posHi = s.posHiHard * 0.99;
+
+// auto testTick = [&]
+// {
+// VelocityControllerWithAccelerationAndPositionBounds ctrl;
+// ctrl.dt = s.dt;
+// ctrl.maxDt = s.maxDt;
+// ctrl.currentV = s.curvel;
+// ctrl.targetV = s.targvel;
+// ctrl.maxV = s.maxvel;
+// ctrl.acceleration = s.acc;
+// ctrl.deceleration = s.dec;
+// ctrl.directSetVLimit = directSetVLimit;
+// ctrl.currentPosition = s.curpos;
+// ctrl.positionLimitLoSoft = s.posLo;
+// ctrl.positionLimitHiSoft = s.posHi;
+// BOOST_CHECK(ctrl.validParameters());
+// s.updateVel(ctrl.run());
+// //s.updateVel(velocityControlWithAccelerationAndPositionBounds(
+// //s.dt, s.maxDt,
+// //s.curvel, s.targvel, s.maxvel,
+// //s.acc, s.dec,
+// //directSetVLimit,
+// //s.curpos,
+// //s.posLo, s.posHi,
+// //// positionSoftLimitViolationVelocity,
+// //s.posLoHard, s.posHiHard
+// //));
+// s.checkPos();
+// s.checkVel();
+// };
+
+// std::cout << "random tests\n";
+// for (std::size_t try_ = 0; try_ < tries; ++ try_)
+// {
+// s.writeHeader("velCtrl+acc+pos_random_" + to_string(try_));
+// s.curvel = s.vDist(gen);
+// s.curpos = s.pDist(gen);
+// s.targvel = s.vDist(gen);
+// s.acc = s.aDist(gen);
+// s.dec = s.aDist(gen);
+// directSetVLimit = (std::min(s.acc, s.dec) * s.maxDt * 0.75f / 2.f) * 0.99f; // vlimit = accmin*dtmin/2
+// s.log();
+// for (std::size_t tick = 0; tick < ticks; ++tick)
+// {
+// s.tick(testTick);
+// }
+// }
+// std::cout << "bounds tests\n";
+// std::cout << "TODO\n";
+// ///TODO
+
+// std::cout << "done velocityControlWithAccelerationAndPositionBoundsTest \n";
+//}
+
+//BOOST_AUTO_TEST_CASE(positionThroughVelocityControlWithConstantJerkTest)
+//{
+// return;
+
+// std::cout << "starting positionThroughVelocityControlWithConstantJerk \n";
+// Simulation s;
+// s.posHi = 0;
+// s.posLo = 0;
+
+
+// double p = 20.5;
+// PositionThroughVelocityControllerWithAccelerationRamps ctrl;
+
+// auto testTick = [&]
+// {
+// ctrl.dt = s.dt;
+// ctrl.maxDt = s.maxDt;
+// // ctrl.currentV = s.curvel;
+// // ctrl.currentAcc = s.curacc;
+// ctrl.maxV = s.maxvel;
+// ctrl.acceleration = s.acc;
+// ctrl.deceleration = s.dec;
+// ctrl.currentPosition = s.curpos;
+// ctrl.setTargetPosition(s.targpos);
+// ctrl.accuracy = 0.00001;
+// ctrl.jerk = 100;
+// // ctrl.pControlPosErrorLimit = pControlPosErrorLimit;
+// // ctrl.pControlVelLimit = pControlVelLimit;
+// ctrl.p = p;//ctrl.calculateProportionalGain();
+// ctrl.usePIDAtEnd = false;
+// // ARMARX_INFO << VAROUT(ctrl.p);
+// BOOST_CHECK(ctrl.validParameters());
+// auto r = ctrl.run();
+// // ARMARX_INFO << "State: " << (int)ctrl.calcState() << " " << VAROUT(r.acceleration) << VAROUT(r.velocity) << VAROUT(r.jerk);
+// s.updateVel(r.velocity);
+// ctrl.currentV = r.velocity;
+// ctrl.currentAcc = r.acceleration;
+// //s.updateVel(positionThroughVelocityControlWithAccelerationBounds(
+// //s.dt, s.maxDt,
+// //s.curvel, s.maxvel,
+// //s.acc, s.dec,
+// //s.curpos, s.targpos,
+// //pControlPosErrorLimit, pControlVelLimit, p
+// //));
+// s.checkVel();
+// s.checkAcc();
+// };
+
+// std::cout << "random tests\n";
+// for (std::size_t try_ = 0; try_ < tries; ++ try_)
+// {
+// ctrl = PositionThroughVelocityControllerWithAccelerationRamps();
+// s.writeHeader("posViaVelCtrl+acc_random_" + to_string(try_));
+// s.maxvel = 1.49;//std::abs(s.vDist(gen)) + 1;
+// s.curvel = -1.49;//armarx::math::MathUtils::LimitTo(s.vDist(gen), s.maxvel);
+// ctrl.currentV = s.curvel;
+// s.curpos = 1;//s.pDist(gen);
+// // s.targpos = 5;//s.pDist(gen);
+// s.acc = 10;//s.aDist(gen);
+// s.dec = 20;//s.aDist(gen);
+//// ARMARX_INFO << VAROUT(s.dt) << VAROUT(s.curpos) << "TargetPos: " << s.targpos << " " << VAROUT(s.acc) << VAROUT(s.dec) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
+//// p = ((std::min(s.acc,s.dec)*s.maxDt*0.75-pControlVelLimit)/pControlPosErrorLimit) * 0.99f; // sometimes <0
+// s.log();
+// for (std::size_t tick = 0; tick < ticks; ++tick)
+// {
+//// std::cout << std::endl;
+// s.targpos = tick > 300 ? 3 : 5;
+
+// s.tick(testTick);
+//// ARMARX_INFO << /*deactivateSpam(0.01) <<*/ VAROUT(s.dt) << VAROUT(s.curpos) << "TargetPos: " << s.targpos << " " << VAROUT(s.curacc) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
+// if (std::abs(s.curpos - s.targpos) < 0.00001 /*|| s.curpos > s.targpos*/)
+// {
+// break;
+// }
+// }
+// BOOST_CHECK_LE(std::abs(s.curpos - s.targpos), 0.00001); // allow error of 0.5729577951308232 deg
+// }
+// std::cout << "bounds tests\n";
+// std::cout << "TODO\n";
+// ///TODO
+
+// std::cout << "done positionThroughVelocityControlWithAccelerationBounds \n";
+//}
+
+BOOST_AUTO_TEST_CASE(MinJerkPositionControllerTest)
{
- return;
- std::cout << "starting positionThroughVelocityControlWithAccelerationAndPositionBounds \n";
+
+ std::cout << "starting MinJerkPositionControllerTest \n";
Simulation s;
s.posHi = 0;
s.posLo = 0;
- float p = 0.5;
+ double p = 20.5;
+ MinJerkPositionController ctrl;
auto testTick = [&]
{
- PositionThroughVelocityControllerWithAccelerationAndPositionBounds ctrl;
ctrl.dt = s.dt;
ctrl.maxDt = s.maxDt;
- ctrl.currentV = s.curvel;
+ // ctrl.currentV = s.curvel;
+ // ctrl.currentAcc = s.curacc;
ctrl.maxV = s.maxvel;
- ctrl.acceleration = s.acc;
- ctrl.deceleration = s.dec;
+ ctrl.desiredDeceleration = s.dec;
ctrl.currentPosition = s.curpos;
- ctrl.targetPosition = s.targpos;
- ctrl.p = p;
- ctrl.positionLimitLo = s.posLoHard;
- ctrl.positionLimitHi = s.posHiHard;
- BOOST_CHECK(ctrl.validParameters());
- s.updateVel(ctrl.run());
- //s.updateVel(positionThroughVelocityControlWithAccelerationAndPositionBounds(
+ ctrl.setTargetPosition(s.targpos);
+ // ctrl.accuracy = 0.00001;
+ ctrl.desiredJerk = 100;
+ // ctrl.pControlPosErrorLimit = pControlPosErrorLimit;
+ // ctrl.pControlVelLimit = pControlVelLimit;
+ // ctrl.p = p;//ctrl.calculateProportionalGain();
+ // ctrl.usePIDAtEnd = false;
+ // ARMARX_INFO << VAROUT(ctrl.p);
+ // BOOST_CHECK(ctrl.validParameters());
+ auto r = ctrl.run();
+ // ARMARX_INFO << "State: " << (int)ctrl.calcState() << " " << VAROUT(r.acceleration) << VAROUT(r.velocity) << VAROUT(r.jerk);
+ // s.updateVel(r.velocity);
+ s.updatePos(r.position);
+ ctrl.currentV = r.velocity;
+ ctrl.currentAcc = r.acceleration;
+ //s.updateVel(positionThroughVelocityControlWithAccelerationBounds(
//s.dt, s.maxDt,
//s.curvel, s.maxvel,
//s.acc, s.dec,
//s.curpos, s.targpos,
- //pControlPosErrorLimit, pControlVelLimit, p,
- //s.posLoHard, s.posHiHard));
- s.checkPos();
+ //pControlPosErrorLimit, pControlVelLimit, p
+ //));
+ // s.checkVel();
+ // s.checkAcc();
};
std::cout << "random tests\n";
for (std::size_t try_ = 0; try_ < tries; ++ try_)
{
- s.writeHeader("posViaVelCtrl+acc+pos_random_" + to_string(try_));
- s.curvel = s.vDist(gen);
- s.curpos = s.pDist(gen);
- s.targpos = s.pDist(gen);
- s.acc = s.aDist(gen);
+ ctrl = MinJerkPositionController();
+ s.writeHeader("minJerkPosCtrl+acc_random_" + to_string(try_));
+ s.maxvel = std::abs(s.vDist(gen)) + 1;
+ s.curvel = armarx::math::MathUtils::LimitTo(s.vDist(gen), s.maxvel);
+ s.curacc = s.vDist(gen);
s.dec = s.aDist(gen);
+ ctrl.currentAcc = s.curacc;
+ ctrl.currentV = s.curvel;
+ s.curpos = 1;//s.pDist(gen);
+ ARMARX_INFO << VAROUT(s.dt) << VAROUT(s.curpos) << "TargetPos: " << s.targpos << " " << VAROUT(s.acc) << VAROUT(s.dec) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
// p = ((std::min(s.acc,s.dec)*s.maxDt*0.75-pControlVelLimit)/pControlPosErrorLimit) * 0.99f; // sometimes <0
s.log();
+ double stopTime = -1;
for (std::size_t tick = 0; tick < ticks; ++tick)
{
+ // std::cout << std::endl;
+ s.targpos = tick > 900 ? 3 : 5;
+
s.tick(testTick);
+ // ARMARX_INFO << /*deactivateSpam(0.01) <<*/ VAROUT(s.dt * tick) << VAROUT(s.curpos) << "TargetPos: " << s.targpos << " " << VAROUT(s.curacc) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
+ if (std::abs(s.curpos - s.targpos) < 0.01 && stopTime < 0)
+ {
+ stopTime = s.dt * (tick + 20); // let it run for some more ticks to make sure the velocity is stable
+ ARMARX_INFO << VAROUT(s.time) << VAROUT(stopTime);
+ }
+ if (stopTime > 0 && s.dt * tick > stopTime)
+ {
+ break;
+ }
}
- BOOST_CHECK_LE(std::abs(s.curpos - s.targpos), 0.05);
+ BOOST_CHECK_LE(std::abs(s.curpos - s.targpos), 0.01); // allow error of 0.5729577951308232 deg
}
std::cout << "bounds tests\n";
std::cout << "TODO\n";
///TODO
- std::cout << "done positionThroughVelocityControlWithAccelerationAndPositionBounds \n";
+ std::cout << "done positionThroughVelocityControlWithAccelerationBounds \n";
}
-//BOOST_AUTO_TEST_CASE(positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPositionTest)
+
+
+//BOOST_AUTO_TEST_CASE(positionThroughVelocityControlWithAccelerationBoundsTest)
//{
-// std::cout << "starting positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPosition \n";
+// return;
+// std::cout << "starting positionThroughVelocityControlWithAccelerationBounds \n";
// Simulation s;
// s.posHi = 0;
// s.posLo = 0;
-// // float p = 0.05;
-// // const float pControlPosErrorLimit = 0.02;
-// // const float pControlVelLimit = 0.02;
-// float p = 0.1;
-// const float pControlPosErrorLimit = 0.01;
-// const float pControlVelLimit = 0.01;
-// float direction;
+
+
+// double p = 0.5;
// auto testTick = [&]
// {
-// PositionThroughVelocityControllerWithAccelerationAndPositionBounds ctrl;
+// PositionThroughVelocityControllerWithAccelerationBounds ctrl;
// ctrl.dt = s.dt;
// ctrl.maxDt = s.maxDt;
// ctrl.currentV = s.curvel;
+// // ctrl.currentAcc = s.curacc;
// ctrl.maxV = s.maxvel;
// ctrl.acceleration = s.acc;
// ctrl.deceleration = s.dec;
// ctrl.currentPosition = s.curpos;
// ctrl.targetPosition = s.targpos;
-// ctrl.pControlPosErrorLimit = pControlPosErrorLimit;
-// ctrl.pControlVelLimit = pControlVelLimit;
-// ctrl.p = p;
-// ctrl.positionLimitLo = s.posLoHard;
-// ctrl.positionLimitHi = s.posHiHard;
+//// ctrl.accuracy = 0.0;
+// // ctrl.jerk = 100;
+// // ctrl.pControlPosErrorLimit = pControlPosErrorLimit;
+// // ctrl.pControlVelLimit = pControlVelLimit;
+// ctrl.pid->Kp = p;
// BOOST_CHECK(ctrl.validParameters());
-// s.updateVel(ctrl.run());
-// //s.updateVel(positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPosition(
+// auto result = ctrl.run();
+// s.updateVel(result);
+// //s.updateVel(positionThroughVelocityControlWithAccelerationBounds(
// //s.dt, s.maxDt,
// //s.curvel, s.maxvel,
// //s.acc, s.dec,
// //s.curpos, s.targpos,
-// //pControlPosErrorLimit, pControlVelLimit, p,
-// //direction,
-// //s.posLoHard, s.posHiHard));
+// //pControlPosErrorLimit, pControlVelLimit, p
+// //));
// s.checkVel();
// s.checkAcc();
-// s.curpos = periodicClamp(s.curpos, s.posLoHard, s.posHiHard);
// };
-// auto rngTestRuns = [&](int d)
+// std::cout << "random tests\n";
+// for (std::size_t try_ = 0; try_ < tries; ++ try_)
// {
-// std::cout << "random tests (dir=" << to_string(d) << "\n";
-// for (std::size_t try_ = 0; try_ < tries; ++ try_)
+// s.writeHeader("posViaVelCtrl+acc_random_" + to_string(try_));
+// s.maxvel = std::abs(s.vDist(gen)) + 1;
+// s.curvel = //armarx::math::MathUtils::LimitTo(s.vDist(gen), s.maxvel);
+// s.curpos = s.pDist(gen);
+// s.targpos = s.pDist(gen);
+// s.acc = s.aDist(gen);
+// s.dec = s.aDist(gen);
+// ARMARX_INFO << VAROUT(s.dt) << VAROUT(s.curpos) << "TargetPos: " << s.targpos << VAROUT(s.acc) << VAROUT(s.dec) << VAROUT(s.maxvel) << VAROUT(s.curvel);
+// // p = ((std::min(s.acc,s.dec)*s.maxDt*0.75-pControlVelLimit)/pControlPosErrorLimit) * 0.99f; // sometimes <0
+// s.log();
+// for (std::size_t tick = 0; tick < ticks; ++tick)
// {
-// s.writeHeader("posViaVelCtrl+acc+periodicPos+dir" + to_string(d) + "_random_" + to_string(try_));
-// direction = d;
-// s.curvel = s.vDist(gen);
-// s.curpos = periodicClamp(s.pDist(gen), s.posLoHard, s.posHiHard);
-// s.targpos = periodicClamp(s.pDist(gen), s.posLoHard, s.posHiHard);
-// s.acc = s.aDist(gen);
-// s.dec = s.aDist(gen);
-// s.log();
-// for (std::size_t tick = 0; tick < ticks; ++tick)
+// s.tick(testTick);
+//// ARMARX_INFO << /*deactivateSpam(0.01) <<*/ VAROUT(s.dt) << VAROUT(s.curpos) << "TargetPos: " << s.targpos << VAROUT(s.curacc) << VAROUT(s.maxvel) << VAROUT(s.curvel);
+// if (std::abs(s.curpos - s.targpos) < 0.01)
// {
-// s.tick(testTick);
+// break;
// }
-// BOOST_CHECK_LE(std::abs(s.curpos - s.targpos), 0.01);
-// BOOST_CHECK_LE(std::abs(s.curvel), 0.1);
// }
+// BOOST_CHECK_LE(std::abs(s.curpos - s.targpos), 0.01); // allow error of 0.5729577951308232 deg
+// }
+// std::cout << "bounds tests\n";
+// std::cout << "TODO\n";
+// ///TODO
+// std::cout << "done positionThroughVelocityControlWithAccelerationBounds \n";
+//}
+
+//BOOST_AUTO_TEST_CASE(positionThroughVelocityControlWithAccelerationAndPositionBoundsTest)
+//{
+// std::cout << "starting positionThroughVelocityControlWithAccelerationAndPositionBounds \n";
+// Simulation s;
+// s.posHi = 0;
+// s.posLo = 0;
+
+// double p = 0.5;
+
+
+// auto testTick = [&]
+// {
+// PositionThroughVelocityControllerWithAccelerationAndPositionBounds ctrl;
+// ctrl.dt = s.dt;
+// ctrl.maxDt = s.maxDt;
+// ctrl.currentV = s.curvel;
+// ctrl.maxV = s.maxvel;
+// ctrl.acceleration = s.acc;
+// ctrl.deceleration = s.dec;
+// ctrl.currentPosition = s.curpos;
+// ctrl.targetPosition = s.targpos;
+// ctrl.pid->Kp = p;
+// ctrl.positionLimitLo = s.posLoHard;
+// ctrl.positionLimitHi = s.posHiHard;
+// BOOST_REQUIRE(ctrl.validParameters());
+// s.updateVel(ctrl.run());
+// //s.updateVel(positionThroughVelocityControlWithAccelerationAndPositionBounds(
+// //s.dt, s.maxDt,
+// //s.curvel, s.maxvel,
+// //s.acc, s.dec,
+// //s.curpos, s.targpos,
+// //pControlPosErrorLimit, pControlVelLimit, p,
+// //s.posLoHard, s.posHiHard));
+// s.checkPos();
// };
-// rngTestRuns(0);
+// std::cout << "random tests\n";
+// for (std::size_t try_ = 0; try_ < tries; ++ try_)
+// {
+// s.writeHeader("posViaVelCtrl+acc+pos_random_" + to_string(try_));
+// s.curvel = s.vDist(gen);
+// s.curpos = s.pDist(gen);
+// s.targpos = s.pDist(gen);
+// s.acc = s.aDist(gen);
+// s.dec = s.aDist(gen);
+// // p = ((std::min(s.acc,s.dec)*s.maxDt*0.75-pControlVelLimit)/pControlPosErrorLimit) * 0.99f; // sometimes <0
+// s.log();
+// for (std::size_t tick = 0; tick < ticks; ++tick)
+// {
+// s.tick(testTick);
+// }
+// BOOST_CHECK_LE(std::abs(s.curpos - s.targpos), 0.05);
+// }
// std::cout << "bounds tests\n";
// std::cout << "TODO\n";
// ///TODO
-// std::cout << "done positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPosition \n";
+// std::cout << "done positionThroughVelocityControlWithAccelerationAndPositionBounds \n";
//}
+////BOOST_AUTO_TEST_CASE(positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPositionTest)
+////{
+//// std::cout << "starting positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPosition \n";
+//// Simulation s;
+//// s.posHi = 0;
+//// s.posLo = 0;
+//// // double p = 0.05;
+//// // const double pControlPosErrorLimit = 0.02;
+//// // const double pControlVelLimit = 0.02;
+//// double p = 0.1;
+//// const double pControlPosErrorLimit = 0.01;
+//// const double pControlVelLimit = 0.01;
+//// double direction;
+
+//// auto testTick = [&]
+//// {
+//// PositionThroughVelocityControllerWithAccelerationAndPositionBounds ctrl;
+//// ctrl.dt = s.dt;
+//// ctrl.maxDt = s.maxDt;
+//// ctrl.currentV = s.curvel;
+//// ctrl.maxV = s.maxvel;
+//// ctrl.acceleration = s.acc;
+//// ctrl.deceleration = s.dec;
+//// ctrl.currentPosition = s.curpos;
+//// ctrl.targetPosition = s.targpos;
+//// ctrl.pControlPosErrorLimit = pControlPosErrorLimit;
+//// ctrl.pControlVelLimit = pControlVelLimit;
+//// ctrl.p = p;
+//// ctrl.positionLimitLo = s.posLoHard;
+//// ctrl.positionLimitHi = s.posHiHard;
+//// BOOST_CHECK(ctrl.validParameters());
+//// s.updateVel(ctrl.run());
+//// //s.updateVel(positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPosition(
+//// //s.dt, s.maxDt,
+//// //s.curvel, s.maxvel,
+//// //s.acc, s.dec,
+//// //s.curpos, s.targpos,
+//// //pControlPosErrorLimit, pControlVelLimit, p,
+//// //direction,
+//// //s.posLoHard, s.posHiHard));
+//// s.checkVel();
+//// s.checkAcc();
+//// s.curpos = periodicClamp(s.curpos, s.posLoHard, s.posHiHard);
+//// };
+
+//// auto rngTestRuns = [&](int d)
+//// {
+//// std::cout << "random tests (dir=" << to_string(d) << "\n";
+//// for (std::size_t try_ = 0; try_ < tries; ++ try_)
+//// {
+//// s.writeHeader("posViaVelCtrl+acc+periodicPos+dir" + to_string(d) + "_random_" + to_string(try_));
+//// direction = d;
+//// s.curvel = s.vDist(gen);
+//// s.curpos = periodicClamp(s.pDist(gen), s.posLoHard, s.posHiHard);
+//// s.targpos = periodicClamp(s.pDist(gen), s.posLoHard, s.posHiHard);
+//// s.acc = s.aDist(gen);
+//// s.dec = s.aDist(gen);
+//// s.log();
+//// for (std::size_t tick = 0; tick < ticks; ++tick)
+//// {
+//// s.tick(testTick);
+//// }
+//// BOOST_CHECK_LE(std::abs(s.curpos - s.targpos), 0.01);
+//// BOOST_CHECK_LE(std::abs(s.curvel), 0.1);
+//// }
+
+//// };
+//// rngTestRuns(0);
+
+//// std::cout << "bounds tests\n";
+//// std::cout << "TODO\n";
+//// ///TODO
+
+//// std::cout << "done positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPosition \n";
+////}
+
diff --git a/source/RobotAPI/components/units/RobotUnit/test/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/test/CMakeLists.txt
index 6a3394ce80fb3c67a1670d555ae04fee7a0b0517..9c43b6455ece5f1448f25ac224d207a8593314b3 100644
--- a/source/RobotAPI/components/units/RobotUnit/test/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/test/CMakeLists.txt
@@ -1,5 +1,5 @@
set(LIBS ${LIBS} RobotUnit )
armarx_add_test(BasicControllerTest BasicControllerTest.cpp "${LIBS}")
-armarx_add_test(SingleBasicControllerTest SingleBasicControllerTest.cpp "${LIBS}")
+#armarx_add_test(SingleBasicControllerTest SingleBasicControllerTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/units/RobotUnit/test/eval_script.inc b/source/RobotAPI/components/units/RobotUnit/test/eval_script.inc
index 0c13aa8f3ad99cf5623697f3ba8a3c3778f74ed0..e6a9129a6847ff59514f00d32aed7d81bae94820 100644
--- a/source/RobotAPI/components/units/RobotUnit/test/eval_script.inc
+++ b/source/RobotAPI/components/units/RobotUnit/test/eval_script.inc
@@ -62,8 +62,8 @@ def consume_file(fname, save=True, show=True):
add(pos,'brakingDist',clr='orange', style=':')
add(pos,'posAfterBraking',clr='magenta', style=':')
- add(vel,'curvel',clr='teal')
- add(vel,'targvel',clr='teal', style='-.')
+ add(vel,'curvel',clr='blue')
+ add(vel,'targvel',clr='blue', style='-.')
add(vel,'maxv',clr='blue', style='--')
add(vel,'maxv',factor=-1,clr='blue', style='--')
@@ -72,7 +72,7 @@ def consume_file(fname, save=True, show=True):
add(acc,'dec',clr='darkgreen', style='--')
plt.draw()
- if save: plt.savefig(fname+'.png')
+ if save: plt.savefig(fname+'.svg')
if show: plt.show()
if not show: plt.close()
@@ -88,7 +88,8 @@ def handle_path(p, show=True):
show=False
for subdir, dirs, files in os.walk(p):
for f in files:
- handle_path(subdir+f,show=show)
+ if not ".svg" in f:
+ handle_path(subdir+f,show=show)
if len(sys.argv) >= 2:
handle_path(sys.argv[1])
diff --git a/source/RobotAPI/components/units/RobotUnit/util/CtrlUtil.h b/source/RobotAPI/components/units/RobotUnit/util/CtrlUtil.h
new file mode 100644
index 0000000000000000000000000000000000000000..16b1680121581308cbe687f344f21e5b057cf9fd
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/util/CtrlUtil.h
@@ -0,0 +1,186 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2019
+ * @copyright http{//www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+//pragma once
+#include <cmath>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
+#include <iostream>
+namespace armarx
+{
+ namespace ctrlutil
+ {
+ double s(double t, double s0, double v0, double a0, double j)
+ {
+ return s0 + v0 * t + 0.5 * a0 * t * t + 1.0 / 6.0 * j * t * t * t;
+ }
+ double v(double t, double v0, double a0, double j)
+ {
+ return v0 + a0 * t + 0.5 * j * t * t;
+ }
+ double a(double t, double a0, double j)
+ {
+ return a0 + j * t;
+ }
+
+
+ double t_to_v(double v, double a, double j)
+ {
+ // time to accelerate to v with jerk j starting at acceleration a0
+ //return 2*math.sqrt(a0**2+2*j*(v/2))-a0/j
+ // 0 = (-v + a0*t) /(0.5*j) + t*t
+ // 0 = -2v/j + 2a0t/j + t*t
+ // --> p = 2a0/j; q = -2v/j
+ // t = - a0/j +- sqrt((a0/j)**2 +2v/j)
+ double tmp = a / j;
+ return - a / j + std::sqrt(tmp * tmp + 2 * v / j);
+ }
+
+
+
+ double t_to_v_with_wedge_acc(double v, double a0, double j)
+ {
+ // ramp up and down of acceleration (i.e. a0=at); returns t to achieve delta v
+ return 2 * t_to_v(v / 2.0, a0, j);
+ }
+
+ struct BrakingData
+ {
+ double s_total, v1, v2, v3, dt1, dt2, dt3;
+ };
+
+ double brakingDistance(double v0, double a0, double j)
+ {
+ auto signV = math::MathUtils::Sign(v0);
+ auto t = t_to_v(v0, -signV * a0, signV * j);
+ return s(t, 0, v0, a0, -signV * j);
+ }
+
+ BrakingData brakingDistance(double goal, double s0, double v0, double a0, double v_max, double a_max, double j, double dec_max)
+ {
+ double d = math::MathUtils::Sign(goal - s0); // if v0 == 0.0 else abs(v0)/a_max
+ dec_max *= -d;
+ // std::cout << "dec max: " << (dec_max) << " d: " << d << std::endl;
+ double dt1 = std::abs((dec_max - a0) / (-j * d));
+ // double dt1 = t_to_v(v_max-v0, a0, j);
+ // double a1 = a(dt1, a0, -d * j);
+ double v1 = v(dt1, v0, a0, -d * j);
+ double acc_duration = std::abs(dec_max) / j;
+ double v2 = v(acc_duration, 0, 0, d * j);// # inverse of time to accelerate from 0 to max v
+ v2 = math::MathUtils::LimitTo(v2, v_max);
+ // v2 = v1 + dv2
+ double dt2 = d * ((v1 - v2) / dec_max);
+ // double a2 = a(dt2, a1, 0);
+
+ double dt3 = t_to_v(std::abs(v2), 0, j);
+ double s1 = s(dt1, 0, v0, a0, d * j);
+ double s2 = s(dt2, 0, v1, dec_max, 0);
+ double s3 = s(dt3, 0, v2, dec_max, d * j);
+ double v3 = v(dt3, v2, dec_max, d * j);
+ double s_total = s1 + s2 + s3;
+
+ if (dt2 < 0)// # we dont have a phase with a=a_max and need to reduce the a_max
+ {
+ double excess_time = t_to_v_with_wedge_acc(std::abs(v1), std::abs(dec_max), j);
+ double delta_a = a(excess_time / 2, 0, d * j);
+ a_max -= d * delta_a;
+ dec_max = std::abs(dec_max) - std::abs(delta_a);
+ dec_max *= -d;
+ return brakingDistance(goal, s0, v0, a0, v_max, a_max, j, dec_max);
+ }
+
+ return {s_total * d, v1, v2, v3, dt1, dt2, dt3};
+ // return (s_total, v1, v2, v3, dt1, dt2, dt3);
+ }
+
+ struct WedgeBrakingData
+ {
+ double s_total, s1, s2, v1, v2, a1, a2, t, dt1, dt2;
+ };
+ bool isPossibleToBrake(double v0, double a0, double j)
+ {
+ return j * v0 - a0 * a0 / 2 > 0.0f;
+ }
+
+ double jerk(double t, double s0, double v0, double a0)
+ {
+ return (6 * s0 - 3 * t * (a0 * t + 2 * v0)) / (t * t * t);
+ }
+
+ std::tuple<double, double, double> calcAccAndJerk(double s0, double v0)
+ {
+ s0 *= -1;
+ double t = - (3 * s0) / v0;
+ double a0 = - (2 * v0) / t;
+ double j = 2 * v0 / (t * t);
+ return std::make_tuple(t, a0, j);
+ }
+
+
+
+ WedgeBrakingData braking_distance_with_wedge(double v0, double a0, double j)
+ {
+ // # v0 = v1 + v2
+ // # v1 = t1 * a0 + 0.5*j*t1**2
+ // # v2 = 0.5*j*t2**2
+ // # a1 = t2 * j ^ a1 = a0 - t1 * j -> t2 * j = a0 - t1 * j
+ // # t2 = (a0 - t1 * j) / j
+ // # t2 = a0/j - t1
+ // # t1_1 = -(math.sqrt(2*j**2 * (a0**2 + 2*j*v0)) + 2*a0*j)/(2*j**2)
+ // # t1_2 = (math.sqrt(2*j**2 * (a0**2 + 2*j*v0)) - 2*a0*j)/(2*j**2)
+ // # t1 = t1_2
+ double d = math::MathUtils::Sign(v0);
+ v0 *= d;
+ a0 *= d;
+ // j *= d;
+ double part = j * v0 - a0 * a0 / 2.0;
+ if (part < 0)
+ {
+ WedgeBrakingData r;
+ r.s_total = -1;
+ r.dt2 = -1;
+ return r;
+ throw std::runtime_error("unsuitable parameters! : j: " + std::to_string(j) +
+ " a0: " + std::to_string(a0) +
+ " v0: " + std::to_string(v0));
+ }
+ double t1 = std::sqrt(part) / j;
+ double t2 = (a0 / j) + t1;
+ double v1 = v(t1, v0, a0, -j);
+ // double dv1 = v(t1, 0, a0, -j);
+ // double diff_v1 = v0 - v1;
+ double a1 = a(t1, -a0, -j);
+ // double a1_2 = a(t2, 0, j);
+ double v2 = v(t2, v1, a1, j);
+ // double dv2 = v(t2, 0, 0, j);
+ // double v_sum = abs(dv1)+dv2;
+ double a2 = a(t2, a1, j);
+ // assert(abs(v_sum - v0) < 0.001);
+ // assert(abs(v2) < 0.0001);
+ double s1 = s(t1, 0, v0, a0, -j);
+ double s2 = s(t2, 0, v1, a1, j);
+ double s_total = s1 + s2;
+ return {s_total, s1, s2, d * v1, d * v2, d * a1, d * a2, t1 + t2, t1, t2};
+ }
+ }
+
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f52c9276e2876c52264a09831c212d5b9db2ffd5
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp
@@ -0,0 +1,126 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#include "DynamicsHelper.h"
+
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/observers/filters/rtfilters/RTFilterBase.h>
+
+namespace armarx
+{
+
+ DynamicsHelper::DynamicsHelper(RobotUnit* robotUnit): robotUnit(robotUnit)
+ {
+
+ ARMARX_CHECK_EXPRESSION(robotUnit);
+ }
+
+ void DynamicsHelper::addRobotPart(VirtualRobot::RobotNodeSetPtr rnsJoints, VirtualRobot::RobotNodeSetPtr rnsBodies,
+ rtfilters::RTFilterBasePtr velocityFilter,
+ rtfilters::RTFilterBasePtr accelerationFilter)
+ {
+ DynamicsData data(rnsJoints, rnsBodies);
+ for (size_t i = 0; i < rnsJoints->getSize(); i++)
+ {
+ auto selectedJoint = robotUnit->getSensorDevice(rnsJoints->getNode(i)->getName());
+ if (selectedJoint)
+ {
+ auto sensorValue = const_cast<SensorValue1DoFActuator*>(selectedJoint->getSensorValue()->asA<SensorValue1DoFActuator>());
+ ARMARX_CHECK_EXPRESSION_W_HINT(sensorValue, rnsJoints->getNode(i)->getName());
+ ARMARX_DEBUG << "will calculate inverse dynamics for robot node " << rnsJoints->getNode(i)->getName();
+ data.nodeSensorVec.push_back(std::make_pair(rnsJoints->getNode(i), sensorValue));
+ }
+ else
+ {
+ ARMARX_INFO << "Joint " << rnsJoints->getNode(i)->getName() << " not found";
+ data.nodeSensorVec.push_back(std::make_pair(rnsJoints->getNode(i), nullptr));
+ }
+ if (velocityFilter)
+ {
+ data.velocityFilter.push_back(velocityFilter->clone());
+ }
+ if (accelerationFilter)
+ {
+ data.accelerationFilter.push_back(accelerationFilter->clone());
+ }
+ }
+ dataMap.emplace(std::make_pair(rnsJoints, data));
+ }
+
+ void DynamicsHelper::update(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ for (auto& pair : dataMap)
+ {
+ // first get current state of the robot (position, velocity, acceleration)
+ auto& data = pair.second;
+ auto& dynamics = data.dynamics;
+ {
+ size_t i = 0;
+ for (auto& node : data.nodeSensorVec)
+ {
+ ARMARX_CHECK_EXPRESSION_W_HINT(node.second, node.first->getName());
+ data.q(i) = node.second->position;
+ if (i < data.velocityFilter.size() && data.velocityFilter.at(i) && !std::isnan(node.second->velocity))
+ {
+ data.qDot(i) = data.velocityFilter.at(i)->update(sensorValuesTimestamp, node.second->velocity);
+ }
+ else
+ {
+ data.qDot(i) = node.second->velocity;
+ }
+ if (i < data.accelerationFilter.size() && data.accelerationFilter.at(i) && !std::isnan(node.second->acceleration))
+ {
+ data.qDDot(i) = data.accelerationFilter.at(i)->update(sensorValuesTimestamp, node.second->acceleration);
+ }
+ else
+ {
+ data.qDDot(i) = node.second->acceleration;
+ }
+ i++;
+ }
+ }
+ // calculate external torques (tau) applied to robot induced by dynamics
+ dynamics.getGravityMatrix(data.q, data.tauGravity);
+ dynamics.getInverseDynamics(data.q,
+ data.qDot,
+ data.qDDot,
+ data.tau);
+ // update virtual sensor values
+ size_t i = 0;
+ for (auto& node : data.nodeSensorVec)
+ {
+ auto torque = data.tau(i);
+ auto gravityTorque = data.tauGravity(i);
+ if (node.second)
+ {
+ node.second->inverseDynamicsTorque = -torque;
+ node.second->gravityTorque = -gravityTorque;
+ node.second->inertiaTorque = -(torque - gravityTorque);
+ }
+ i++;
+ }
+ }
+ }
+
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..4212b3ae5db79ac102f164860810a31483d1363d
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h
@@ -0,0 +1,69 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/Dynamics/Dynamics.h>
+#include <Eigen/Core>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+
+namespace armarx
+{
+ namespace rtfilters
+ {
+ class RTFilterBase;
+ using RTFilterBasePtr = std::shared_ptr<RTFilterBase>;
+ }
+ class RobotUnit;
+ class DynamicsHelper
+ {
+ public:
+ struct DynamicsData
+ {
+ DynamicsData(VirtualRobot::RobotNodeSetPtr rnsJoints, VirtualRobot::RobotNodeSetPtr rnsBodies) :
+ rnsJoints(rnsJoints), rnsBodies(rnsBodies),
+ dynamics(rnsJoints, rnsBodies)
+ {
+ q = qDot = qDDot = tau = tauGravity = Eigen::VectorXd::Zero(rnsJoints->getSize());
+ }
+ DynamicsData(const DynamicsData& r) = default;
+ VirtualRobot::RobotNodeSetPtr rnsJoints, rnsBodies;
+ VirtualRobot::Dynamics dynamics;
+ Eigen::VectorXd q, qDot, qDDot, tau, tauGravity;
+ std::vector<std::pair<VirtualRobot::RobotNodePtr, SensorValue1DoFActuator*>> nodeSensorVec;
+ std::vector<rtfilters::RTFilterBasePtr> velocityFilter;
+ std::vector<rtfilters::RTFilterBasePtr> accelerationFilter;
+ };
+
+ DynamicsHelper(RobotUnit* robotUnit);
+ ~DynamicsHelper() = default;
+ void addRobotPart(VirtualRobot::RobotNodeSetPtr rnsJoints, VirtualRobot::RobotNodeSetPtr rnsBodies,
+ rtfilters::RTFilterBasePtr velocityFilter = rtfilters::RTFilterBasePtr(),
+ rtfilters::RTFilterBasePtr accelerationFilter = rtfilters::RTFilterBasePtr());
+ void update(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ protected:
+ RobotUnit* robotUnit;
+ std::map<VirtualRobot::RobotNodeSetPtr, DynamicsData> dataMap;
+ };
+
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/util/KeyValueVector.h b/source/RobotAPI/components/units/RobotUnit/util/KeyValueVector.h
index 799baf84595d6a6eb042cbc5f7d6c52755b640ad..be82c776f26e326f76642dda211d662a2b9e13f9 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/KeyValueVector.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/KeyValueVector.h
@@ -20,224 +20,5 @@
* GNU General Public License
*/
#pragma once
+#include <ArmarXCore/util/CPPUtility/KeyValueVector.h>
-#include <ArmarXCore/core/util/TemplateMetaProgramming.h>
-#include <vector>
-#include <map>
-
-namespace armarx
-{
- /**
- * @brief This class is pretty much similar to a map.
- *
- * This class stores keys and values in two vectors and uses a map to map the keys to the index of the corresponding value
- * This enables map acces and index access.
- * The index of an inserted element never changes (this is different to a map).
- * Index access may be required in a high frequency case (e.g. an rt control loop)
- */
- template < class KeyT, class ValT,
- class KeyContT = std::vector<KeyT>,
- class ValContT = std::vector<ValT>,
- class IdxT = typename ValContT::size_type,
- class MapT = std::map<KeyT, IdxT >>
- class KeyValueVector
- {
- public:
- void add(KeyT key, ValT value);
- IdxT index(const KeyT& k) const;
- IdxT size() const;
-
- ValT& at(IdxT i);
- ValT const& at(IdxT i) const;
- ValT& at(const KeyT& k);
- ValT const& at(const KeyT& k) const;
- ValT& at(const KeyT& k, ValT& defaultVal);
- ValT const& at(const KeyT& k, const ValT& defaultVal) const;
-
- ValContT const& values() const;
- MapT const& indices() const;
- KeyContT const& keys() const;
- IdxT count(const KeyT& k) const;
- bool has(const KeyT& k) const;
- void clear();
-
- using iterator = typename ValContT::iterator;
- using const_iterator = typename ValContT::const_iterator;
-
- iterator begin();
- iterator end();
-
- const_iterator begin() const;
- const_iterator end() const;
-
- const_iterator cbegin() const;
- const_iterator cend() const;
-
-
- private:
- template<class T, class = typename std::enable_if<meta::HasToString<T>::value>::type>
- static std::string DescribeKey(const T* key);
- static std::string DescribeKey(...);
-
- MapT indices_;
- KeyContT keys_;
- ValContT values_;
- };
-}
-
-// implementation
-namespace armarx
-{
- template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT, class MapT>
- void KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::add(KeyT key, ValT value)
- {
- if (indices_.find(key) != indices_.end())
- {
- throw std::invalid_argument {"Already added a value for this key" + DescribeKey(&key)};
- }
- indices_.emplace(key, size());
- values_.emplace_back(std::move(value));
- keys_.emplace_back(std::move(key));
- }
-
- template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT, class MapT>
- IdxT KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::index(const KeyT& k) const
- {
- return indices_.at(k);
- }
-
- template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT, class MapT>
- ValT& KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::at(IdxT i)
- {
- return values_.at(i);
- }
-
- template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT, class MapT>
- const ValT& KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::at(IdxT i) const
- {
- return values_.at(i);
- }
-
- template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT, class MapT>
- ValT& KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::at(const KeyT& k)
- {
- return at(index(k));
- }
-
- template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT, class MapT>
- const ValT& KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::at(const KeyT& k) const
- {
- return at(index(k));
- }
-
- template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT, class MapT>
- ValT& KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::at(const KeyT& k, ValT& defaultVal)
- {
- return has(k) ? at(index(k)) : defaultVal;
- }
-
- template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT, class MapT>
- const ValT& KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::at(const KeyT& k, const ValT& defaultVal) const
- {
- return has(k) ? at(index(k)) : defaultVal;
- }
-
- template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT, class MapT>
- IdxT KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::size() const
- {
- return values_.size();
- }
-
- template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT, class MapT>
- const ValContT& KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::values() const
- {
- return values_;
- }
-
- template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT, class MapT>
- const MapT& KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::indices() const
- {
- return indices_;
- }
-
- template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT, class MapT>
- const KeyContT& KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::keys() const
- {
- return keys_;
- }
-
- template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT, class MapT>
- IdxT KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::count(const KeyT& k) const
- {
- return indices_.count(k);
- }
-
- template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT, class MapT>
- bool KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::has(const KeyT& k) const
- {
- return indices_.count(k);
- }
-
- template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT, class MapT>
- void KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::clear()
- {
- indices_.clear();
- keys_.clear();
- values_.clear();
- }
-
- template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT, class MapT>
- typename KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::iterator
- KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::begin()
- {
- return values_.begin();
- }
-
- template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT, class MapT>
- typename KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::iterator
- KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::end()
- {
- return values_.end();
- }
-
- template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT, class MapT>
- typename KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::const_iterator
- KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::begin() const
- {
- return values_.cbegin();
- }
-
- template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT, class MapT>
- typename KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::const_iterator
- KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::end() const
- {
- return values_.cend();
- }
-
- template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT, class MapT>
- typename KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::const_iterator
- KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::cbegin() const
- {
- return begin();
- }
-
- template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT, class MapT>
- typename KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::const_iterator
- KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::cend() const
- {
- return end();
- }
-
- template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT, class MapT>
- std::string KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::DescribeKey(...)
- {
- return "";
- }
-
- template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT, class MapT>
- template<class T, class>
- std::string KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::DescribeKey(const T* key)
- {
- return ": " + to_string(*key);
- }
-}
diff --git a/source/RobotAPI/components/units/SensorActorUnit.h b/source/RobotAPI/components/units/SensorActorUnit.h
index e41418d70f7f4a5beb3bcd4b847d0cc65c2322d3..bd6485fc64dc62047bbb84d6f4ac2a776b8bc70d 100644
--- a/source/RobotAPI/components/units/SensorActorUnit.h
+++ b/source/RobotAPI/components/units/SensorActorUnit.h
@@ -59,7 +59,7 @@ namespace armarx
*
* \param c Ice context provided by the Ice framework
*/
- void init(const Ice::Current& c = ::Ice::Current()) override;
+ void init(const Ice::Current& c = Ice::emptyCurrent) override;
/**
* Set execution state to eStarted
@@ -71,7 +71,7 @@ namespace armarx
*
* \param c Ice context provided by the Ice framework
*/
- void start(const Ice::Current& c = ::Ice::Current()) override;
+ void start(const Ice::Current& c = Ice::emptyCurrent) override;
/**
* Set execution state to eStopped
@@ -83,7 +83,7 @@ namespace armarx
*
* \param c Ice context provided by the Ice framework
*/
- void stop(const Ice::Current& c = ::Ice::Current()) override;
+ void stop(const Ice::Current& c = Ice::emptyCurrent) override;
/**
* Retrieve current execution state
@@ -91,21 +91,21 @@ namespace armarx
* \param c Ice context provided by the Ice framework
* \return current execution state
*/
- UnitExecutionState getExecutionState(const Ice::Current& c = ::Ice::Current()) override;
+ UnitExecutionState getExecutionState(const Ice::Current& c = Ice::emptyCurrent) override;
/**
* Request exclusive access to current unit. Throws ResourceUnavailableException on error.
*
* \param c Ice context provided by the Ice framework
*/
- void request(const Ice::Current& c = ::Ice::Current()) override;
+ void request(const Ice::Current& c = Ice::emptyCurrent) override;
/**
* Release exclusive access to current unit. Throws ResourceUnavailableException on error.
*
* \param c Ice context provided by the Ice framework
*/
- void release(const Ice::Current& c = ::Ice::Current()) override;
+ void release(const Ice::Current& c = Ice::emptyCurrent) override;
protected:
void onExitComponent() override;
diff --git a/source/RobotAPI/components/units/SpeechObserver.cpp b/source/RobotAPI/components/units/SpeechObserver.cpp
index 15b2766b4f07989833590f8a3e2a0aa3bbdd134a..26c215dbfa745af1213c3b1f52db4c5a7aa761d0 100644
--- a/source/RobotAPI/components/units/SpeechObserver.cpp
+++ b/source/RobotAPI/components/units/SpeechObserver.cpp
@@ -1,6 +1,6 @@
/*
* This file is part of ArmarX.
- *
+ *
* Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
* Karlsruhe Institute of Technology (KIT), all rights reserved.
*
@@ -22,6 +22,7 @@
*/
#include "SpeechObserver.h"
+#include <ArmarXCore/util/json/JSONObject.h>
using namespace armarx;
@@ -33,6 +34,8 @@ void SpeechObserver::onInitObserver()
{
usingTopic(getProperty<std::string>("TextToSpeechTopicName").getValue());
usingTopic(getProperty<std::string>("TextToSpeechStateTopicName").getValue());
+
+
}
void SpeechObserver::onConnectObserver()
@@ -42,12 +45,18 @@ void SpeechObserver::onConnectObserver()
offerDataFieldWithDefault("TextToSpeech", "TextChangeCounter", Variant(reportTextCounter), "Counter for text updates");
offerDataFieldWithDefault("TextToSpeech", "State", Variant(""), "Current TTS state");
- offerDataFieldWithDefault("TextToSpeech", "StateChangeCounter", Variant(reportStateCounter), "Counter for text updates");
+ offerDataFieldWithDefault("TextToSpeech", "StateChangeCounter", Variant(reportStateCounter), "Counter for state updates");
+
+ offerChannel("SpeechToText", "SpeechToText channel");
+ offerDataFieldWithDefault("SpeechToText", "RecognizedText", Variant(std::string()), "Text recognized by the SpeechRecogntion");
+
+ auto proxy = getObjectAdapter()->addWithUUID(new SpeechListenerImpl(this));
+ getIceManager()->subscribeTopic(proxy, "Speech_Commands");
}
std::string SpeechObserver::SpeechStateToString(TextToSpeechStateType state)
{
- switch(state)
+ switch (state)
{
case eIdle:
return "Idle";
@@ -70,7 +79,7 @@ void SpeechObserver::reportState(TextToSpeechStateType state, const Ice::Current
updateChannel("TextToSpeech");
}
-void SpeechObserver::reportText(const std::string& text, const Ice::Current&)
+void SpeechObserver::reportText(const std::string& text, const Ice::Current& c)
{
ScopedLock lock(dataMutex);
reportTextCounter++;
@@ -84,3 +93,24 @@ void SpeechObserver::reportTextWithParams(const std::string& text, const Ice::St
ScopedLock lock(dataMutex);
ARMARX_WARNING << "reportTextWithParams is not implemented";
}
+
+SpeechListenerImpl::SpeechListenerImpl(SpeechObserver* obs) :
+ obs(obs)
+{
+
+}
+
+
+void armarx::SpeechListenerImpl::reportText(const std::string& text, const Ice::Current&)
+{
+ ScopedLock lock(dataMutex);
+ JSONObject json;
+ json.fromString(text);
+ obs->setDataField("SpeechToText", "RecognizedText", Variant(json.getString("text")));
+}
+
+void armarx::SpeechListenerImpl::reportTextWithParams(const std::string&, const Ice::StringSeq&, const Ice::Current&)
+{
+ ScopedLock lock(dataMutex);
+ ARMARX_WARNING << "reportTextWithParams is not implemented";
+}
diff --git a/source/RobotAPI/components/units/SpeechObserver.h b/source/RobotAPI/components/units/SpeechObserver.h
index 769677e2759f0f7934ced448a3db51ef48db82e1..43e3bbc6fc37511b052d590fde29a9e8a09fabd9 100644
--- a/source/RobotAPI/components/units/SpeechObserver.h
+++ b/source/RobotAPI/components/units/SpeechObserver.h
@@ -1,6 +1,6 @@
/*
* This file is part of ArmarX.
- *
+ *
* Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
* Karlsruhe Institute of Technology (KIT), all rights reserved.
*
@@ -39,11 +39,25 @@ namespace armarx
defineOptionalProperty<std::string>("TextToSpeechStateTopicName", "TextToSpeechState", "Name of the TextToSpeechStateTopic");
}
};
+ class SpeechObserver;
+ class SpeechListenerImpl : public TextListenerInterface
+ {
+ public:
+ SpeechListenerImpl(SpeechObserver* obs);
+ protected:
+ SpeechObserver* obs;
+ Mutex dataMutex;
+ // TextListenerInterface interface
+ public:
+ void reportText(const std::string&, const Ice::Current&) override;
+ void reportTextWithParams(const std::string&, const Ice::StringSeq&, const Ice::Current&) override;
+ };
class SpeechObserver :
- virtual public Observer,
- virtual public SpeechObserverInterface
+ virtual public Observer,
+ virtual public SpeechObserverInterface
{
+ friend class SpeechListenerImpl;
public:
SpeechObserver();
@@ -58,9 +72,9 @@ namespace armarx
void onInitObserver() override;
void onConnectObserver() override;
- virtual void reportState(armarx::TextToSpeechStateType state, const Ice::Current& = Ice::Current()) override;
- virtual void reportText(const std::string& text, const Ice::Current& = Ice::Current()) override;
- virtual void reportTextWithParams(const std::string& text, const Ice::StringSeq& params, const Ice::Current& = Ice::Current()) override;
+ virtual void reportState(armarx::TextToSpeechStateType state, const Ice::Current& = Ice::emptyCurrent) override;
+ virtual void reportText(const std::string& text, const Ice::Current& = Ice::emptyCurrent) override;
+ virtual void reportTextWithParams(const std::string& text, const Ice::StringSeq& params, const Ice::Current& = Ice::emptyCurrent) override;
static std::string SpeechStateToString(TextToSpeechStateType state);
private:
diff --git a/source/RobotAPI/components/units/TCPControlUnit.h b/source/RobotAPI/components/units/TCPControlUnit.h
index ea2482acedb199503014992e36c7352f1080a379..6d254bba5031b1d1b3ddc3dfe2b0316a409f42e8 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.h
+++ b/source/RobotAPI/components/units/TCPControlUnit.h
@@ -93,7 +93,7 @@ namespace armarx
* \param milliseconds New cycle time.
* \param c Ice Context, leave blank.
*/
- void setCycleTime(Ice::Int milliseconds, const Ice::Current& c = Ice::Current()) override;
+ void setCycleTime(Ice::Int milliseconds, const Ice::Current& c = Ice::emptyCurrent) override;
/**
* \brief Sets the cartesian velocity of a node in a nodeset for translation and/or orientation.
@@ -110,32 +110,32 @@ namespace armarx
*
* \see request(), release()
*/
- void setTCPVelocity(const std::string& nodeSetName, const std::string& tcpName, const::armarx::FramedDirectionBasePtr& translationVelocity, const::armarx::FramedDirectionBasePtr& orientationVelocityRPY, const Ice::Current& c = Ice::Current()) override;
+ void setTCPVelocity(const std::string& nodeSetName, const std::string& tcpName, const::armarx::FramedDirectionBasePtr& translationVelocity, const::armarx::FramedDirectionBasePtr& orientationVelocityRPY, const Ice::Current& c = Ice::emptyCurrent) override;
// UnitExecutionManagementInterface interface
/**
* \brief Does not do anything at the moment.
* \param c
*/
- void init(const Ice::Current& c = Ice::Current()) override;
+ void init(const Ice::Current& c = Ice::emptyCurrent) override;
/**
* \brief Does not do anything at the moment.
* \param c
*/
- void start(const Ice::Current& c = Ice::Current()) override;
+ void start(const Ice::Current& c = Ice::emptyCurrent) override;
/**
* \brief Does not do anything at the moment.
* \param c
*/
- void stop(const Ice::Current& c = Ice::Current()) override;
- UnitExecutionState getExecutionState(const Ice::Current& c = Ice::Current()) override;
+ void stop(const Ice::Current& c = Ice::emptyCurrent) override;
+ UnitExecutionState getExecutionState(const Ice::Current& c = Ice::emptyCurrent) override;
// UnitResourceManagementInterface interface
/**
* \brief Triggers the calculation loop for using cartesian velocity. Call once before/after setting a tcp velocity with SetTCPVelocity.
* \param c Ice Context, leave blank.
*/
- void request(const Ice::Current& c = Ice::Current()) override;
+ void request(const Ice::Current& c = Ice::emptyCurrent) override;
/**
* \brief Releases and stops the recalculation and updating of joint velocities.
@@ -143,9 +143,9 @@ namespace armarx
* all node set will be deleted in this function.
* \param c Ice Context, leave blank.
*/
- void release(const Ice::Current& c = Ice::Current()) override;
+ void release(const Ice::Current& c = Ice::emptyCurrent) override;
- bool isRequested(const Ice::Current& c = Ice::Current()) override;
+ bool isRequested(const Ice::Current& c = Ice::emptyCurrent) override;
protected:
diff --git a/source/RobotAPI/components/units/TCPControlUnitObserver.h b/source/RobotAPI/components/units/TCPControlUnitObserver.h
index e62e4377e6b076d6da3adcd464952a97cc5bb01f..88cdf43c30f840a00c3b82857bf51b059aba8ad0 100644
--- a/source/RobotAPI/components/units/TCPControlUnitObserver.h
+++ b/source/RobotAPI/components/units/TCPControlUnitObserver.h
@@ -73,8 +73,8 @@ namespace armarx
public:
// TCPControlUnitListener interface
- void reportTCPPose(const FramedPoseBaseMap& poseMap, const Ice::Current& c = Ice::Current()) override;
- void reportTCPVelocities(const FramedDirectionMap& tcpTranslationVelocities, const FramedDirectionMap& tcpOrientationVelocities, const Ice::Current& c = Ice::Current()) override;
+ void reportTCPPose(const FramedPoseBaseMap& poseMap, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportTCPVelocities(const FramedDirectionMap& tcpTranslationVelocities, const FramedDirectionMap& tcpOrientationVelocities, const Ice::Current& c = Ice::emptyCurrent) override;
Mutex dataMutex;
};
diff --git a/source/RobotAPI/drivers/CMakeLists.txt b/source/RobotAPI/drivers/CMakeLists.txt
index c51339f3adc5b0c16a5e217c959ffaad43884ebc..731765e4bbcbdcb597b34f5fb6de5a5b9ae7b0ed 100644
--- a/source/RobotAPI/drivers/CMakeLists.txt
+++ b/source/RobotAPI/drivers/CMakeLists.txt
@@ -6,4 +6,4 @@ add_subdirectory(OrientedTactileSensor)
add_subdirectory(GamepadUnit)
add_subdirectory(MetaWearIMU)
add_subdirectory(KITProstheticHandDriver)
-
+add_subdirectory(KITProsthesisIceDriver)
diff --git a/source/RobotAPI/drivers/CyberGloveUnit/serial.tgz b/source/RobotAPI/drivers/CyberGloveUnit/serial.tgz
new file mode 100644
index 0000000000000000000000000000000000000000..5f224fa51a52d01c1a74e4d387a612e0ded79620
Binary files /dev/null and b/source/RobotAPI/drivers/CyberGloveUnit/serial.tgz differ
diff --git a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp
index a840e0c0663fdc3746f717712a8cc98edd73f423..436a81ed1097096b6ee905fd50e9f847af13d095 100644
--- a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp
+++ b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp
@@ -37,6 +37,25 @@ void GamepadUnit::onInitComponent()
void GamepadUnit::onConnectComponent()
{
topicPrx = getTopic<GamepadUnitListenerPrx>(getProperty<std::string>("GamepadTopicName").getValue());
+ sendTask = new SimplePeriodicTask<>([&]
+ {
+ ScopedLock lock(mutex);
+ if (!js.opened())
+ return;
+ auto age = IceUtil::Time::now() - dataTimestamp->toTime();
+ if (age.toMilliSeconds() < getProperty<int>("PublishTimeout").getValue())
+ topicPrx->reportGamepadState(deviceName, js.name, data, dataTimestamp);
+ else
+ {
+ ARMARX_INFO << deactivateSpam(100000, std::to_string(dataTimestamp->getTimestamp())) << "No new signal from gamepad for " << age.toMilliSecondsDouble() << " milliseconds. Not sending data. Timeout: " << getProperty<int>("PublishTimeout").getValue() << " ms";
+ }
+ }, 30);
+ sendTask->start();
+ openGamepadConnection();
+}
+
+bool GamepadUnit::openGamepadConnection()
+{
if (js.open(deviceName))
{
ARMARX_INFO << "opened a gamepad named " << js.name << " with " << js.numberOfAxis << " axis and " << js.numberOfButtons << " buttons.";
@@ -46,29 +65,37 @@ void GamepadUnit::onConnectComponent()
}
else
{
- ARMARX_WARNING << "this is not our trusty logitech gamepad you moron.";
+ ARMARX_WARNING << "this is not our trusty logitech gamepad.";
+ js.close();
+ return false;
}
}
else
{
ARMARX_WARNING << "Could not open gamepad device " << deviceName;
+ js.close();
+ return false;
}
+ return true;
}
void GamepadUnit::run()
{
while (readTask->isRunning())
{
- GamepadData data;
-
if (!js.pollEvent())
{
- ARMARX_WARNING << "failed to read gamepad data";
- break;
+ ARMARX_WARNING << "failed to read gamepad data - trying to reconnect";
+ js.close();
+ sleep(1);
+ while (readTask->isRunning() && !openGamepadConnection())
+ {
+ sleep(1);
+ }
}
-
+ ScopedLock lock(mutex);
IceUtil::Time now = IceUtil::Time::now();
- TimestampVariantPtr nowTimestamp = new TimestampVariant(now);
+ dataTimestamp = new TimestampVariant(now);
//mapping found with command line tool jstest <device file>
@@ -95,9 +122,7 @@ void GamepadUnit::run()
data.leftStickButton = js.buttonsPressed[9];
data.rightStickButton = js.buttonsPressed[10];
- ARMARX_IMPORTANT << "left x (integer): " << js.axis[0] << " left x (float): " << data.leftStickX;
-
- topicPrx->reportGamepadState(deviceName, js.name, data, nowTimestamp);
+ ARMARX_VERBOSE << "left x (integer): " << js.axis[0] << " left x (float): " << data.leftStickX << " right trigger: " << data.rightTrigger;
//usleep(1000); // 10ms
}
@@ -106,7 +131,14 @@ void GamepadUnit::run()
void GamepadUnit::onDisconnectComponent()
{
-
+ if (sendTask)
+ {
+ sendTask->stop();
+ }
+ if (readTask)
+ {
+ readTask->stop();
+ }
}
diff --git a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
index 1db681dc79cdb0b5a071390a63b0b72828692311..ee0e66a2e7e10ed62e1a634cc4787f48239db3de 100644
--- a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
+++ b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
@@ -25,10 +25,12 @@
#include<linux/joystick.h>
#include<sys/stat.h>
#include<fcntl.h>
+#include <ArmarXCore/observers/variant/TimestampVariant.h>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <RobotAPI/interface/units/GamepadUnit.h>
@@ -51,6 +53,7 @@ namespace armarx
//defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
defineOptionalProperty<std::string>("GamepadTopicName", "GamepadValues", "Name of the Gamepad Topic");
defineOptionalProperty<std::string>("GamepadDeviceName", "/dev/input/js2", "device that will be opened as a gamepad");
+ defineOptionalProperty<int>("PublishTimeout", 2000, "In Milliseconds. Timeout after which the gamepad data is not published after, if no new data was read from the gamepad");
}
};
@@ -103,12 +106,19 @@ namespace armarx
*/
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+ bool openGamepadConnection();
+
private:
GamepadUnitListenerPrx topicPrx;
RunningTask<GamepadUnit>::pointer_type readTask;
+ SimplePeriodicTask<>::pointer_type sendTask;
+
void run();
+ Mutex mutex;
std::string deviceName;
Joystick js;
+ GamepadData data;
+ TimestampVariantPtr dataTimestamp;
};
}
diff --git a/source/RobotAPI/drivers/GamepadUnit/Joystick.h b/source/RobotAPI/drivers/GamepadUnit/Joystick.h
index 132bb751dc2eb7d78c978532c3179b6644e3f2ca..1f39074b7a1449f9d58b643d42be68f2c123c445 100644
--- a/source/RobotAPI/drivers/GamepadUnit/Joystick.h
+++ b/source/RobotAPI/drivers/GamepadUnit/Joystick.h
@@ -62,6 +62,11 @@ namespace armarx
return fd != -1;
}
+ bool opened() const
+ {
+ return fd != -1;
+ }
+
bool pollEvent()
{
int bytes = read(fd, &event, sizeof(event));
@@ -87,6 +92,7 @@ namespace armarx
void close()
{
::close(fd);
+ fd = -1;
}
};
}
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
index c257210ebe454a3cbe88c405bb90aab5e0af1582..03b9f860ca100b7ae6a95b8a54ff39a2423f6ea0 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
@@ -23,6 +23,7 @@
#include "HokuyoLaserUnit.h"
#include <ArmarXCore/observers/variant/TimestampVariant.h>
+#include <sys/resource.h>
#include <boost/algorithm/string/split.hpp>
#include <HokuyoLaserScannerDriver/urg_utils.h>
@@ -32,6 +33,11 @@ using namespace armarx;
void HokuyoLaserUnit::onInitComponent()
{
+ // ARMARX_INFO << "Priority: " << getpriority(PRIO_PROCESS, LogSender::getProcessId());
+ // // setpriority(PRIO_PROCESS, LogSender::getProcessId(), -10);
+ // int prio = -20;
+ // auto ret = nice(prio);
+ // ARMARX_INFO << "Priority after: " << getpriority(PRIO_PROCESS, LogSender::getProcessId()) << " ret: " << ret;
offeringTopic(getProperty<std::string>("RobotHealthTopicName").getValue());
topicName = getProperty<std::string>("LaserScannerTopicName").getValue();
@@ -172,8 +178,12 @@ void HokuyoLaserUnit::updateScanData()
{
ARMARX_ERROR << "Device " << device.ip << " has too many consecutive errors!";
// assume dead
- device.connected = false;
-
+ if (device.connected)
+ {
+ ARMARX_INFO << "Disconnecting from laser scanner with IP " << device.ip;
+ urg_close(&device.urg);
+ device.connected = false;
+ }
ARMARX_WARNING << "Reconnecting to " << device.ip << ":" << device.port;
int ret = urg_open(&device.urg, URG_ETHERNET, device.ip.c_str(), device.port);
device.connected = (ret == 0);
@@ -227,7 +237,10 @@ void HokuyoLaserUnit::updateScanData()
device.errorCounter = 0;
topic->reportSensorValues(device.ip, device.frame, scan, now);
- robotHealthTopic->heartbeat(getName() + "_" + device.ip, RobotHealthHeartbeatArgs());
+ RobotHealthHeartbeatArgs args;
+ args.maximumCycleTimeWarningMS = 500;
+ args.maximumCycleTimeErrorMS = 800;
+ robotHealthTopic->heartbeat(getName() + "_" + device.ip, args);
}
}
}
diff --git a/source/RobotAPI/drivers/KITProsthesisIceDriver/KITProsthesisIceDriver.cpp b/source/RobotAPI/drivers/KITProsthesisIceDriver/KITProsthesisIceDriver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..25b5f9972efd5decb236f520eca28920ac741d19
--- /dev/null
+++ b/source/RobotAPI/drivers/KITProsthesisIceDriver/KITProsthesisIceDriver.cpp
@@ -0,0 +1,69 @@
+#include "KITProsthesisIceDriver.h"
+#include <iostream>
+#include <memory>
+
+using namespace KITProsthesis;
+
+void KITProsthesisIceDriver::sendGrasp(Ice::Int n, const Ice::Current&)
+{
+ iface.sendGrasp(n);
+}
+void KITProsthesisIceDriver::sendThumbPWM(const ProsthesisMotorValues& motorValues, const Ice::Current&)
+{
+ iface.sendThumbPWM(motorValues.v, motorValues.maxPWM, motorValues.pos);
+}
+
+void KITProsthesisIceDriver::sendFingerPWM(const ProsthesisMotorValues& motorValues, const Ice::Current&)
+{
+ iface.sendFingerPWM(motorValues.v, motorValues.maxPWM, motorValues.pos);
+}
+
+ProsthesisSensorValues KITProsthesisIceDriver::getSensorValues(const Ice::Current&)
+{
+ ProsthesisSensorValues value;
+ switch (iface.getState())
+ {
+ case BLEProthesisInterface::State::Created:
+ value.state = ProsthesisState::Created;
+ break;
+ case BLEProthesisInterface::State::DiscoveringDevices:
+ value.state = ProsthesisState::DiscoveringDevices;
+ break;
+ case BLEProthesisInterface::State::DiscoveringDevicesDone:
+ value.state = ProsthesisState::DiscoveringDevicesDone;
+ break;
+ case BLEProthesisInterface::State::Disconnected:
+ value.state = ProsthesisState::Disconnected;
+ break;
+ case BLEProthesisInterface::State::Connecting:
+ value.state = ProsthesisState::Connecting;
+ break;
+ case BLEProthesisInterface::State::ConnectingDone:
+ value.state = ProsthesisState::ConnectingDone;
+ break;
+ case BLEProthesisInterface::State::DiscoveringServices:
+ value.state = ProsthesisState::DiscoveringServices;
+ break;
+ case BLEProthesisInterface::State::DiscoveringServicesDone:
+ value.state = ProsthesisState::DiscoveringServicesDone;
+ break;
+ case BLEProthesisInterface::State::ConnectingService:
+ value.state = ProsthesisState::ConnectingService;
+ break;
+ case BLEProthesisInterface::State::Running:
+ value.state = ProsthesisState::Running;
+ break;
+ case BLEProthesisInterface::State::Killed:
+ value.state = ProsthesisState::Killed;
+ break;
+ }
+
+ value.thumbPWM = iface.getThumbPWM();
+ value.thumbPos = iface.getThumbPos();
+
+ value.fingerPWM = iface.getFingerPWM();
+ value.fingerPos = iface.getFingerPos();
+
+ //add IMU here
+ return value;
+}
diff --git a/source/RobotAPI/drivers/KITProsthesisIceDriver/KITProsthesisIceDriver.h b/source/RobotAPI/drivers/KITProsthesisIceDriver/KITProsthesisIceDriver.h
new file mode 100644
index 0000000000000000000000000000000000000000..3d8ec5dc028ef543d3f2aa8212c2f5620ab8c512
--- /dev/null
+++ b/source/RobotAPI/drivers/KITProsthesisIceDriver/KITProsthesisIceDriver.h
@@ -0,0 +1,52 @@
+/*
+ * 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::::drivers::KITProsthesisIceDriver
+ * @author Julia Starke ( julia dot starke at kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/units/KITProstheticHandInterface.h>
+#include <RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h>
+#include <Ice/Ice.h>
+#include <Ice/Object.h>
+#include <QCoreApplication>
+#include <iostream>
+#include <thread>
+
+[[maybe_unused]] static constexpr auto prosthesis = "CB:43:34:8F:3C:0A";
+[[maybe_unused]] static constexpr auto prosthesis_old = "DF:70:E8:81:DB:D6";
+
+class KITProsthesisIceDriver:
+ virtual public KITProsthesis::KITProstheticHandInterface
+{
+public:
+ KITProsthesisIceDriver(const std::string& mac = prosthesis_old) : iface{mac}
+ {}
+
+ // sender interface
+ void sendGrasp(Ice::Int, const Ice::Current& = Ice::emptyCurrent) override;
+ void sendThumbPWM(const KITProsthesis::ProsthesisMotorValues&, const ::Ice::Current& = ::Ice::emptyCurrent) override;
+ void sendFingerPWM(const KITProsthesis::ProsthesisMotorValues&, const ::Ice::Current& = ::Ice::emptyCurrent) override;
+
+ KITProsthesis::ProsthesisSensorValues getSensorValues(const Ice::Current&) override;
+private:
+ BLEProthesisInterface iface;
+
+};
diff --git a/source/RobotAPI/drivers/KITProsthesisIceDriver/example/CMakeLists.txt b/source/RobotAPI/drivers/KITProsthesisIceDriver/example/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8f8dd417c314d7e53badf1d85150f35dd4c47265
--- /dev/null
+++ b/source/RobotAPI/drivers/KITProsthesisIceDriver/example/CMakeLists.txt
@@ -0,0 +1,10 @@
+armarx_component_set_name("KITProsthesisIceDriverExampleNoArmarX")
+
+set(COMPONENT_LIBS
+ KITProsthesisIceDriver
+ RobotAPIInterfaces
+)
+
+set(EXE_SOURCE KITProsthesisIceDriverExample.cpp)
+
+armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/drivers/KITProsthesisIceDriver/example/KITProsthesisIceDriverExample.cpp b/source/RobotAPI/drivers/KITProsthesisIceDriver/example/KITProsthesisIceDriverExample.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..10928b88eb88b13a09239e5cda8226c2245c8d8b
--- /dev/null
+++ b/source/RobotAPI/drivers/KITProsthesisIceDriver/example/KITProsthesisIceDriverExample.cpp
@@ -0,0 +1,35 @@
+#include <Ice/Ice.h>
+#include <RobotAPI/drivers/KITProsthesisIceDriver/KITProsthesisIceDriver.h>
+#include <RobotAPI/interface/units/KITProstheticHandInterface.h>
+#include <QCoreApplication>
+#include <thread>
+int main(int argc, char* argv[])
+{
+ for (int i = 0; i < argc; ++i)
+ {
+ if (std::string(argv[i]) == "-h" ||
+ std::string(argv[i]) == "--print-options") // needed for ArmarX Doc generation
+ {
+ // TODO: print help
+ return 0;
+ }
+ }
+
+ QCoreApplication a(argc, argv);
+ try
+ {
+ Ice::CommunicatorHolder iceServer(argc, argv);
+ Ice::ObjectAdapterPtr adapter = iceServer->createObjectAdapterWithEndpoints("KITProsthesisControlAdapter", "default -h localhost -p 10000");
+ KITProsthesis::KITProstheticHandInterfacePtr object = new KITProsthesisIceDriver;
+ Ice::ObjectPrx prx = adapter->add(object, Ice::stringToIdentity("KITProsthesisControl"));
+ std::cout << prx->ice_toString() << std::endl;
+ adapter->activate();
+ iceServer->waitForShutdown();
+ }
+ catch (const std::exception& e)
+ {
+ std::cerr << e.what() << std::endl;
+ return 1;
+ }
+ return 0;
+}
diff --git a/source/RobotAPI/drivers/KITProsthesisIceDriver/example/KITProstheticHandInterface.ice b/source/RobotAPI/drivers/KITProsthesisIceDriver/example/KITProstheticHandInterface.ice
new file mode 120000
index 0000000000000000000000000000000000000000..a480161f79b731fbd1b7cb14308c378e93dbcffb
--- /dev/null
+++ b/source/RobotAPI/drivers/KITProsthesisIceDriver/example/KITProstheticHandInterface.ice
@@ -0,0 +1 @@
+../../../interface/units/KITProstheticHandInterface.ice
\ No newline at end of file
diff --git a/source/RobotAPI/drivers/KITProsthesisIceDriver/example/ProsthesisNetworkControl.py b/source/RobotAPI/drivers/KITProsthesisIceDriver/example/ProsthesisNetworkControl.py
new file mode 100755
index 0000000000000000000000000000000000000000..fb7a7d86f033118eed3bb43317c278f0151432f1
--- /dev/null
+++ b/source/RobotAPI/drivers/KITProsthesisIceDriver/example/ProsthesisNetworkControl.py
@@ -0,0 +1,69 @@
+#!/usr/bin/python
+import sys, traceback, Ice
+import IceStorm
+import time
+
+#from bmp280 import BMP280
+
+
+def main():
+ status = 0
+ ic = None
+ prosthesis = None
+ ########################################################## ICE part
+ try:
+ properties = Ice.createProperties(sys.argv)
+ properties.load("./default.generated.cfg")
+ properties.load("./default.cfg")
+
+ init_data = Ice.InitializationData()
+ init_data.properties = properties
+
+ ic = Ice.initialize(init_data)
+
+ Ice.loadSlice('-I%s %s -DMININTERFACE' % (Ice.getSliceDir(), 'KITProstheticHandInterface.ice'))
+
+ print("loadSlice")
+ import KITProsthesis
+
+
+ obj = ic.stringToProxy("IceStorm/TopicManager")
+
+ print("stringToProxy")
+ base = ic.stringToProxy("KITProsthesisControl -t -e 1.1:tcp -h localhost -p 10000 -t 60000")
+ print("base = ", base.ice_toString())
+ print("cast proxy")
+ prosthesis = KITProsthesis.KITProstheticHandInterfacePrx.checkedCast(base)
+ if not prosthesis:
+ raise RuntimeError("Invalid proxy")
+
+ except:
+ traceback.print_exc()
+ status = 1
+
+ if prosthesis is not None:
+ def graspAndStatus(grasp, sleep = 3):
+ prosthesis.sendGrasp(grasp)
+ val = prosthesis.getSensorValues()
+ print("state ", val.state)
+ print("thumbPWM ", val.thumbPWM)
+ print("thumbPos ", val.thumbPos)
+ print("fingerPWM ", val.fingerPWM)
+ print("fingerPos ", val.fingerPos)
+ time.sleep(sleep)
+
+ graspAndStatus(0)
+ graspAndStatus(1)
+ graspAndStatus(0)
+
+ if ic:
+ #clean up
+ try:
+ ic.destroy()
+ except:
+ traceback.print_exc()
+ status = 1
+
+ sys.exit(status)
+
+main()
diff --git a/source/RobotAPI/drivers/KITProsthesisIceDriver/example/default.cfg b/source/RobotAPI/drivers/KITProsthesisIceDriver/example/default.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..63cd81810cc833c4242a3acce28b189a1bc1b90a
--- /dev/null
+++ b/source/RobotAPI/drivers/KITProsthesisIceDriver/example/default.cfg
@@ -0,0 +1,3 @@
+Ice.Default.Locator=IceGrid/Locator:tcp -p 10000 -h localhost
+IceGrid.Registry.Client.Endpoints=tcp -p 10000
+
diff --git a/source/RobotAPI/drivers/KITProsthesisIceDriver/example/default.generated.cfg b/source/RobotAPI/drivers/KITProsthesisIceDriver/example/default.generated.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..36a1225e22ae5531d59066713e162ac7e1ce4a24
--- /dev/null
+++ b/source/RobotAPI/drivers/KITProsthesisIceDriver/example/default.generated.cfg
@@ -0,0 +1,16 @@
+# Do not edit this file!
+# This file is automatically generated and overwritten regularly.
+IceStormAdmin.TopicManager.Default=IceStorm/TopicManager
+
+Ice.ThreadPool.Server.SizeMax=20
+Ice.ThreadPool.Client.SizeMax=20
+Ice.PrintStackTraces=1
+Ice.MessageSizeMax=20000000
+
+# Server Deactivation Timeout & custom Process Facet instantiation
+# https://doc.zeroc.com/display/Ice34/IceGrid+and+the+Administrative+Facility#IceGridandtheAdministrativeFacility-DeactivatingaDeployedServer
+IceGrid.Node.WaitTime=10
+Ice.Admin.DelayCreation=1
+
+# comment out for Ice 3.4 to remove warning
+Ice.Default.EncodingVersion=1.0
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.cpp b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.cpp
index e8c024398fb5aa539b09cfa043fd28dea1e14cc3..fab724f4b1cc29732eed10bfa1c291237284c8ec 100644
--- a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.cpp
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.cpp
@@ -135,7 +135,7 @@ void BLEProthesisInterface::sendFingerPWM(uint64_t v, uint64_t mxPWM, uint64_t p
std::to_string(getMaxPWM()) + "]"
};
}
- if (pos > maxPosF())
+ if (pos > getMaxPosFingers())
{
throw std::invalid_argument
{
@@ -143,7 +143,7 @@ void BLEProthesisInterface::sendFingerPWM(uint64_t v, uint64_t mxPWM, uint64_t p
" , mxPWM = " + std::to_string(mxPWM) +
" , pos = " + std::to_string(pos) +
" ): The interval for pos is [0, " +
- std::to_string(maxPosF()) + "]"
+ std::to_string(getMaxPosFingers()) + "]"
};
}
std::stringstream str;
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h
index cf96515661f2e6497c1de67386010d0563fb83ac..51797fe9b5b1e2ee3300b53221ab43a8c9675052 100644
--- a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h
@@ -83,7 +83,7 @@ public:
{
return 100'000;
}
- static constexpr std::uint64_t maxPosF()
+ static constexpr std::uint64_t getMaxPosFingers()
{
return 200'000;
}
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/example/KITProstheticHandDriverExampleNoArmarX.cpp b/source/RobotAPI/drivers/KITProstheticHandDriver/example/KITProstheticHandDriverExampleNoArmarX.cpp
index 3b04f047f8524aa3e8a71733e92bdc6b7de1626f..a8a524a0dff47ce79912f1b25b80093cc7f45ae5 100644
--- a/source/RobotAPI/drivers/KITProstheticHandDriver/example/KITProstheticHandDriverExampleNoArmarX.cpp
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/example/KITProstheticHandDriverExampleNoArmarX.cpp
@@ -11,6 +11,15 @@
int main(int argc, char* argv[])
{
+ for (int i = 0; i < argc; ++i)
+ {
+ if (std::string(argv[i]) == "-h" ||
+ std::string(argv[i]) == "--print-options") // needed for ArmarX Doc generation
+ {
+ // TODO: print help
+ return 0;
+ }
+ }
QCoreApplication a(argc, argv);
{
BLEProthesisInterface iface{dlr};
diff --git a/source/RobotAPI/drivers/ProsthesisObserver/ProsthesisInterface.py b/source/RobotAPI/drivers/ProsthesisObserver/ProsthesisInterface.py
index be697e920434d1034dd11e9031974907fd810544..133b62afc9ab7cd136bb8355ab877b9265cb551c 100755
--- a/source/RobotAPI/drivers/ProsthesisObserver/ProsthesisInterface.py
+++ b/source/RobotAPI/drivers/ProsthesisObserver/ProsthesisInterface.py
@@ -2,14 +2,14 @@
# Example of interaction with a BLE UART device using a UART service
# implementation.
# Author: Tony DiCola
-import Adafruit_BluefruitLE
-from Adafruit_BluefruitLE.services import UART
+#import Adafruit_BluefruitLE
+#from Adafruit_BluefruitLE.services import UART
import re
import numpy as np
# Get the BLE provider for the current platform.
-ble = Adafruit_BluefruitLE.get_provider()
+#ble = Adafruit_BluefruitLE.get_provider()
import sys, traceback, Ice
import IceStorm
@@ -77,7 +77,7 @@ def main():
except:
traceback.print_exc()
status = 1
-
+ '''
############################################################## BLE part
motorData = np.zeros((2,2))
@@ -178,11 +178,15 @@ def main():
status = 1
sys.exit(status)
-
+ '''
# Initialize the BLE system. MUST be called before other BLE calls!
-ble.initialize()
+#ble.initialize()
# Start the mainloop to process BLE events, and run the provided function in
# a background thread. When the provided main function stops running, returns
# an integer status code, or throws an error the program will exit.
-ble.run_mainloop_with(main)
+#ble.run_mainloop_with(main)
+main()
+values = armarx.ProsthesisMotorValues(name = "motorValues", position1 = 0, pwm1 = 1, position2 = 2, pwm2 = 3)
+ #values.name = "Test"
+prosthesis.reportMotorValues(values)
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticUnit.cpp b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticUnit.cpp
index ad34a2dedd3f4368e8bbd6ad4b2bd4a15082e230..9cf656e32b1280b6ad3c60024094a4016cff08c6 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticUnit.cpp
+++ b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticUnit.cpp
@@ -26,7 +26,7 @@
#include "WeissHapticUnit.h"
#include <boost/regex.hpp>
-#include <boost/filesystem.hpp>
+#include <filesystem>
using namespace armarx;
@@ -58,12 +58,12 @@ std::vector< std::string > WeissHapticUnit::getDevices()
std::vector< std::string > files;
- boost::filesystem::directory_iterator end_itr; // Default ctor yields past-the-end
+ std::filesystem::directory_iterator end_itr; // Default ctor yields past-the-end
- for (boost::filesystem::directory_iterator i(target_path); i != end_itr; ++i)
+ for (std::filesystem::directory_iterator i(target_path); i != end_itr; ++i)
{
// Skip if not a file
- //if( !boost::filesystem::is_( i->status() ) ) continue;
+ //if( !std::filesystem::is_( i->status() ) ) continue;
boost::smatch what;
diff --git a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp
index 24bd2463237d88cc3623fca7e4434828597dd9fb..11d77b7f1ff415cc7391979947a6846ba8f23ea2 100644
--- a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp
@@ -264,6 +264,10 @@ QString DebugDrawerViewerWidgetController::makeLayerItemText(const LayerInformat
{
annotations.push_back("empty");
}
+ else
+ {
+ annotations.push_back(std::to_string(layer.elementCount));
+ }
if (!layer.visible)
{
annotations.push_back("hidden");
diff --git a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.h b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.h
index a3e46e54d5232bc381915311f15c2c512b7090c7..b8c58435fb792f97ef8ae3f88c518bb10fd407c2 100644
--- a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.h
+++ b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.h
@@ -22,7 +22,7 @@
#pragma once
-#include "ui_DebugDrawerViewerWidget.h"
+#include <RobotAPI/gui-plugins/DebugDrawerViewer/ui_DebugDrawerViewerWidget.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
diff --git a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.cpp b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.cpp
index 1c29574a9c4a0e85d4de52ed1528f5beba14066a..0a4d7031aad04aaf9cf42a044b1d1667b3c02ce3 100644
--- a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.cpp
@@ -21,7 +21,7 @@
*/
#include "HandUnitConfigDialog.h"
-#include "ui_HandUnitConfigDialog.h"
+#include <RobotAPI/gui-plugins/HandUnitPlugin/ui_HandUnitConfigDialog.h>
#include <RobotAPI/components/units/HandUnit.h>
#include <IceUtil/UUID.h>
diff --git a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp
index 4bd8d2d25bcbbf1652ec3d8611b851b03efe753a..8629a33ff0ae22a834ff13a08a3978c418f797fb 100644
--- a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp
@@ -23,7 +23,7 @@
*/
#include "HandUnitGuiPlugin.h"
#include "HandUnitConfigDialog.h"
-#include "ui_HandUnitConfigDialog.h"
+#include <RobotAPI/gui-plugins/HandUnitPlugin/ui_HandUnitConfigDialog.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
// Qt headers
diff --git a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.h
index d233f9ee9d3da3d0b6916656b32c9bdeac50b8f0..f57fd6d3ed15f50d5d0cc812b8b005f96ae45ec0 100644
--- a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.h
@@ -24,7 +24,7 @@
#pragma once
/* ArmarX headers */
-#include "ui_HandUnitGuiPlugin.h"
+#include <RobotAPI/gui-plugins/HandUnitPlugin/ui_HandUnitGuiPlugin.h>
#include <ArmarXCore/core/Component.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitConfigDialog.cpp b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitConfigDialog.cpp
index d450ab1607339c95e83fe9eea17976c5d1ab35b7..5b7f350afdc769c246cc6c8021953389859c0784 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitConfigDialog.cpp
@@ -21,7 +21,7 @@
*/
#include "HapticUnitConfigDialog.h"
-#include "ui_HapticUnitConfigDialog.h"
+#include <RobotAPI/gui-plugins/HapticUnitPlugin/ui_HapticUnitConfigDialog.h>
armarx::HapticUnitConfigDialog::HapticUnitConfigDialog(QWidget* parent) :
QDialog(parent),
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp
index f4ecec0573e109358f656efca57c7c04b25db56e..e5e60d26036935a8fc20bc902d7ad0de976178ca 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp
@@ -23,7 +23,7 @@
*/
#include "HapticUnitGuiPlugin.h"
#include "HapticUnitConfigDialog.h"
-#include "ui_HapticUnitConfigDialog.h"
+#include <RobotAPI/gui-plugins/HapticUnitPlugin/ui_HapticUnitConfigDialog.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.h
index 760874e360336bd3cd9d93a5cb90a1f841e9c547..44b2f33227d22d45fd5c14af1abb579a6ceb0774 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.h
@@ -24,7 +24,7 @@
#pragma once
/* ArmarX headers */
-#include "ui_HapticUnitGuiPlugin.h"
+#include <RobotAPI/gui-plugins/HapticUnitPlugin/ui_HapticUnitGuiPlugin.h>
#include <ArmarXCore/core/Component.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.cpp b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.cpp
index 9ea47061f22f74cd68895bbc8b27132a41dc1be5..8f5bc0d16dad87c97d243453e4c53f1645390dc5 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.cpp
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.cpp
@@ -22,7 +22,7 @@
* GNU General Public License
*/
#include "MatrixDisplayWidget.h"
-#include "ui_MatrixDisplayWidget.h"
+#include <RobotAPI/gui-plugins/HapticUnitPlugin/ui_MatrixDisplayWidget.h>
#include <QPainter>
#include <pthread.h>
diff --git a/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorWidgetController.h b/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorWidgetController.h
index f58937e8f8de83e025ed7cd41cd2b2154605fd36..8c726c50144ee12b5b1b31e2bfeacd77f8cafa20 100644
--- a/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorWidgetController.h
+++ b/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorWidgetController.h
@@ -22,7 +22,7 @@
#pragma once
-#include "ui_HomogeneousMatrixCalculatorWidget.h"
+#include <RobotAPI/gui-plugins/HomogeneousMatrixCalculator/ui_HomogeneousMatrixCalculatorWidget.h>
#include <VirtualRobot/MathTools.h>
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp
index 72cf349da009f7070eec4dd01b62b6a3abb708f9..f224f04b1ec1de9895a94315e2b1bdd24595d7cc 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp
@@ -21,7 +21,7 @@
*/
#include "KinematicUnitConfigDialog.h"
-#include "ui_KinematicUnitConfigDialog.h"
+#include <RobotAPI/gui-plugins/KinematicUnitPlugin/ui_KinematicUnitConfigDialog.h>
#include <QTimer>
#include <QPushButton>
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index 5263f02f8bc2439f9454aefc2b6cabdda3173770..5d362e12b6615b34fe0831d73249bee23ca2a490 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -61,7 +61,7 @@
#include <iostream>
#include <cmath>
-#include <boost/filesystem.hpp>
+#include <filesystem>
//#define KINEMATIC_UNIT_FILE_DEFAULT std::string("RobotAPI/robots/Armar3/ArmarIII.xml")
@@ -337,6 +337,7 @@ void KinematicUnitWidgetController::loadSettings(QSettings* settings)
{
kinematicUnitName = settings->value("kinematicUnitName", KINEMATIC_UNIT_NAME_DEFAULT).toString().toStdString();
enableValueValidator = settings->value("enableValueValidator", true).toBool();
+ viewerEnabled = settings->value("viewerEnabled", true).toBool();
historyTime = settings->value("historyTime", 100).toInt() * 1000;
currentValueMax = settings->value("currentValueMax", 5.0).toFloat();
}
@@ -345,6 +346,7 @@ void KinematicUnitWidgetController::saveSettings(QSettings* settings)
{
settings->setValue("kinematicUnitName", QString::fromStdString(kinematicUnitName));
settings->setValue("enableValueValidator", enableValueValidator);
+ settings->setValue("viewerEnabled", viewerEnabled);
assert(historyTime % 1000 == 0);
settings->setValue("historyTime", (int)(historyTime / 1000));
settings->setValue("currentValueMax", currentValueMax);
@@ -1392,33 +1394,13 @@ void KinematicUnitWidgetController::reportJointStatuses(const NameStatusMap& joi
void KinematicUnitWidgetController::updateModel()
{
-
-
// ARMARX_INFO << "updateModel()" << flush;
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
if (!robotNodeSet)
{
return;
}
- std::vector< RobotNodePtr > rn2 = robotNodeSet->getAllRobotNodes();
-
- std::vector< RobotNodePtr > usedNodes;
- std::vector< float > jv;
-
- for (unsigned int i = 0; i < rn2.size(); i++)
- {
- VirtualRobot::RobotNodePtr node = robot->getRobotNode(rn2[i]->getName());
- NameValueMap::const_iterator it;
- it = reportedJointAngles.find(node->getName());
-
- if (it != reportedJointAngles.end())
- {
- usedNodes.push_back(node);
- jv.push_back(it->second);
- }
- }
-
- robot->setJointValues(usedNodes, jv);
+ robot->setJointValues(reportedJointAngles);
}
void KinematicUnitWidgetController::highlightCriticalValues()
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/kinematicunitguiplugin.ui b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/kinematicunitguiplugin.ui
index 3eaba382cc0c67a5e92dfbcda4b3d40cafa70c5c..ece04b7de3fedbf030a0354c6442c6a487589b11 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/kinematicunitguiplugin.ui
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/kinematicunitguiplugin.ui
@@ -206,7 +206,7 @@
<number>3</number>
</property>
<property name="maximum">
- <double>1.000000000000000</double>
+ <double>10.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.cpp b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.cpp
index 8683057dd13c94900addaf91f665bb8e6428d5d1..cd15a2e3a54a9010ee38c1486986b8f3c890da11 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.cpp
@@ -21,7 +21,7 @@
*/
#include "PlatformUnitConfigDialog.h"
-#include "ui_PlatformUnitConfigDialog.h"
+#include <RobotAPI/gui-plugins/PlatformUnitPlugin/ui_PlatformUnitConfigDialog.h>
#include <IceUtil/UUID.h>
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
index 95d957f85249fe874ac6229a0b20177ce69712a2..f23423f6cfaa89711123bfbfa612269105d8e123 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
@@ -23,7 +23,7 @@
*/
#include "PlatformUnitGuiPlugin.h"
#include "PlatformUnitConfigDialog.h"
-#include "ui_PlatformUnitConfigDialog.h"
+#include <RobotAPI/gui-plugins/PlatformUnitPlugin/ui_PlatformUnitConfigDialog.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
// Qt headers
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h
index 9e63f997a91b9d6b1b21382f93f41d6ffcac88bd..775e254616cf8dfd62f6abdb14bf7da3d361c00d 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h
@@ -24,7 +24,7 @@
#pragma once
/* ArmarX headers */
-#include "ui_PlatformUnitGuiPlugin.h"
+#include <RobotAPI/gui-plugins/PlatformUnitPlugin/ui_PlatformUnitGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/JoystickControlWidget.h>
#include <ArmarXCore/core/Component.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
@@ -118,9 +118,9 @@ namespace armarx
void onExitComponent() override;
// slice interface implementation
- void reportPlatformPose(::Ice::Float currentPlatformPositionX, ::Ice::Float currentPlatformPositionY, ::Ice::Float currentPlatformRotation, const Ice::Current& c = ::Ice::Current()) override;
- void reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c = ::Ice::Current()) override;
- void reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c = ::Ice::Current()) override;
+ void reportPlatformPose(::Ice::Float currentPlatformPositionX, ::Ice::Float currentPlatformPositionY, ::Ice::Float currentPlatformRotation, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c = Ice::emptyCurrent) override;
void reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&) override;
// inherited of ArmarXWidget
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/ControlDevicesWidget.h b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/ControlDevicesWidget.h
index b06897427de80b4cfd60e6283ddde502770752a1..2754c47b43f49c728a59ca1c112cf1bb06c430b0 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/ControlDevicesWidget.h
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/ControlDevicesWidget.h
@@ -41,7 +41,7 @@
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include "RobotUnitWidgetBase.h"
-#include "ui_ControlDevicesWidget.h"
+#include <RobotAPI/gui-plugins/RobotUnitPlugin/ui_ControlDevicesWidget.h>
namespace armarx
{
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
index 400d2884deb8652f7b09750c3f4ead94a5f1bcdf..b0bb952eab4ac87d74aa687e47d7c66ce33ce6a9 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
@@ -22,7 +22,7 @@
#include <thread>
-#include <boost/filesystem.hpp>
+#include <filesystem>
#include "NJointControllerClassesWidget.h"
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
@@ -44,7 +44,7 @@ namespace armarx
//get package hints
{
- using namespace boost::filesystem;
+ using namespace std::filesystem;
std::string homeDir = QDir::homePath().toStdString();
path p = path {homeDir} / ".cmake" / "packages";
if (is_directory(p))
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.h b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.h
index 21e44e0a86287660b5eeb842563ae5e5327f4073..47057c1f8381aed2c7cdb71a707401f5d36d9cb7 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.h
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.h
@@ -41,7 +41,7 @@
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include "RobotUnitWidgetBase.h"
-#include "ui_NJointControllerClassesWidget.h"
+#include <RobotAPI/gui-plugins/RobotUnitPlugin/ui_NJointControllerClassesWidget.h>
namespace armarx
{
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllersWidget.h b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllersWidget.h
index d6abed0dafda4047a269018ec7386438700de496..0db9a473257734bd978fc1864ae583de5fee318e 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllersWidget.h
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllersWidget.h
@@ -42,7 +42,7 @@
#include <RobotAPI/interface/units/RobotUnit/NJointController.h>
#include "RobotUnitWidgetBase.h"
-#include "ui_NJointControllersWidget.h"
+#include <RobotAPI/gui-plugins/RobotUnitPlugin/ui_NJointControllersWidget.h>
namespace armarx
{
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/SensorDevicesWidget.h b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/SensorDevicesWidget.h
index bccbbda1f8e57b5562e477634812d0ca66522a0a..7f55ab7298bf72d3d6e78c625deb381640948731 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/SensorDevicesWidget.h
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/SensorDevicesWidget.h
@@ -40,7 +40,7 @@
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include "RobotUnitWidgetBase.h"
-#include "ui_SensorDevicesWidget.h"
+#include <RobotAPI/gui-plugins/RobotUnitPlugin/ui_SensorDevicesWidget.h>
namespace armarx
{
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp
index db997c5d2ac8006245a44ddadb27c775c23836e1..8fdc20d0a779d19dd37c410d025c24f6f7aa03e6 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp
@@ -25,7 +25,7 @@
#include <string>
-#include <boost/filesystem.hpp>
+#include <filesystem>
#include <QDir>
#include <QSortFilterProxyModel>
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.h b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.h
index 03b8671112b067a5cee7037b1494e6cbced15b7e..fa9a52c448997605b83c88dfbcca7c344543b6a4 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.h
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.h
@@ -22,7 +22,7 @@
#pragma once
-#include "ui_RobotUnitPluginWidget.h"
+#include <RobotAPI/gui-plugins/RobotUnitPlugin/ui_RobotUnitPluginWidget.h>
#include <mutex>
#include <map>
diff --git a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
index 3b463172e9858b9524901a193017cf82abaa31db..8d116f2d67e90fcc60b1415701599be945688a85 100644
--- a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
@@ -53,7 +53,7 @@
#include <iostream>
#include <cmath>
-#include <boost/filesystem.hpp>
+#include <filesystem>
#define TIMER_MS 33
#define ROBOTSTATE_NAME_DEFAULT "RobotStateComponent"
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.h b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.h
index 20e4166eab415e086fb844cf93c46800e60927da..e3a78e725f07cf73ac8fc10d478c3abf914ee7f6 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.h
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.h
@@ -24,7 +24,7 @@
/** ArmarX headers **/
#include "TCPMoverConfigDialog.h"
-#include "ui_TCPMover.h"
+#include <RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ui_TCPMover.h>
// ArmarX includes
#include <RobotAPI/interface/units/TCPMoverUnitInterface.h>
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.cpp b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.cpp
index 4ec8ad2b5b6d31bf2091cb013c5d1eadded6e607..2f906a864348c1ea87b97f5b79a78d7a984c5252 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.cpp
@@ -21,7 +21,7 @@
*/
#include "TCPMoverConfigDialog.h"
-#include "ui_TCPMoverConfigDialog.h"
+#include <RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ui_TCPMoverConfigDialog.h>
#include <IceUtil/UUID.h>
diff --git a/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionConfigDialog.cpp b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionConfigDialog.cpp
index 248c260f25cb20e117a8698785a357bdc6ad8d94..d221e2ff267cbcbf7768d2be6bdf5e65872c81e6 100644
--- a/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionConfigDialog.cpp
@@ -1,5 +1,5 @@
#include "ViewSelectionConfigDialog.h"
-#include "ui_ViewSelectionConfigDialog.h"
+#include <RobotAPI/gui-plugins/ViewSelection/ui_ViewSelectionConfigDialog.h>
#include <RobotAPI/interface/components/ViewSelectionInterface.h>
diff --git a/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidgetController.h b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidgetController.h
index 1b5f87d8b5223ceed8b5ea0aea043f92c7f4e066..d95095edc7862d2386ca1d188aa252a339398871 100644
--- a/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidgetController.h
+++ b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidgetController.h
@@ -22,7 +22,7 @@
#pragma once
-#include "ui_ViewSelectionWidget.h"
+#include <RobotAPI/gui-plugins/ViewSelection/ui_ViewSelectionWidget.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
@@ -87,9 +87,9 @@ namespace armarx
void configured() override;
- void onActivateAutomaticViewSelection(const Ice::Current& c = Ice::Current()) override;
- void onDeactivateAutomaticViewSelection(const Ice::Current& c = Ice::Current()) override;
- void nextViewTarget(Ice::Long, const Ice::Current& c = Ice::Current()) override;
+ void onActivateAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override;
+ void onDeactivateAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override;
+ void nextViewTarget(Ice::Long, const Ice::Current& c = Ice::emptyCurrent) override;
/**
* Returns the Widget name displayed in the ArmarXGui to create an
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 32308c8a90c3ed3bb0a5f85e6de9df50c2354586..772a12aff4eb357a993a40db93c7ff121fcb332d 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -8,6 +8,7 @@ set(SLICE_FILES
observers/KinematicUnitObserverInterface.ice
observers/PlatformUnitObserverInterface.ice
observers/ObserverFilters.ice
+ observers/GraspCandidateObserverInterface.ice
core/PoseBase.ice
core/OrientedPoint.ice
@@ -17,11 +18,14 @@ set(SLICE_FILES
core/RobotStateObserverInterface.ice
core/Trajectory.ice
core/CartesianSelection.ice
+ core/CartesianWaypointControllerConfig.ice
+ core/CartesianPositionControllerConfig.ice
selflocalisation/SelfLocalisationProcess.ice
speech/SpeechInterface.ice
observers/SpeechObserverInterface.ice
+ observers/GraspCandidateObserverInterface.ice
units/ForceTorqueUnit.ice
units/InertialMeasurementUnit.ice
@@ -42,10 +46,10 @@ set(SLICE_FILES
units/GamepadUnit.ice
units/MetaWearIMUInterface.ice
units/MetaWearIMU.ice
- units/ProsthesisInterface.ice
- units/ProsthesisObserverInterface.ice
units/CyberGloveInterface.ice
units/CyberGloveObserverInterface.ice
+ units/GraspCandidateProviderInterface.ice
+ units/KITProstheticHandInterface.ice
units/RobotUnit/NJointController.ice
units/RobotUnit/NJointTrajectoryController.ice
units/RobotUnit/NJointCartesianVelocityController.ice
@@ -61,11 +65,14 @@ set(SLICE_FILES
units/RobotUnit/NJointTaskSpaceDMPController.ice
units/RobotUnit/NJointBimanualForceMPController.ice
+ units/RobotUnit/DSControllerBase.ice
+
units/RobotUnit/TaskSpaceActiveImpedanceControl.ice
components/ViewSelectionInterface.ice
components/TrajectoryPlayerInterface.ice
components/RobotNameServiceInterface.ice
components/RobotHealthInterface.ice
+ components/FrameTrackingInterface.ice
visualization/DebugDrawerInterface.ice
@@ -83,3 +90,6 @@ set(SLICE_FILES_ADDITIONAL_SOURCES
# generate the interface library
armarx_interfaces_generate_library(RobotAPI "${ROBOTAPI_INTERFACE_DEPEND}")
+
+target_link_libraries(RobotAPIInterfaces PUBLIC ArmarXCore)
+
diff --git a/source/RobotAPI/interface/units/ProsthesisObserverInterface.ice b/source/RobotAPI/interface/components/FrameTrackingInterface.ice
similarity index 50%
rename from source/RobotAPI/interface/units/ProsthesisObserverInterface.ice
rename to source/RobotAPI/interface/components/FrameTrackingInterface.ice
index 8f5b3e34415855379be9f12b40f96856aeec6e25..a892a5aecfff626ef9de02d2a811be89cd8da696 100644
--- a/source/RobotAPI/interface/units/ProsthesisObserverInterface.ice
+++ b/source/RobotAPI/interface/components/FrameTrackingInterface.ice
@@ -1,8 +1,6 @@
/*
* This file is part of ArmarX.
*
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
* 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.
@@ -15,27 +13,32 @@
* 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
- * @author Peter Kaiser <peter dot kaiser at kit dot edu>
- * @date 2014
+ * @author Adrian Knobloch ( adrian dot knobloch at student dot kit dot edu )
+ * @date 2019
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-#ifndef _ARMARX_ROBOTAPI_PROSTHESIS_OBSERVER_INTERFACE_SLICE_
-#define _ARMARX_ROBOTAPI_PROSTHESIS_OBSERVER_INTERFACE_SLICE_
-#include <ArmarXCore/interface/observers/ObserverInterface.ice>
-#include <RobotAPI/interface/units/ProsthesisInterface.ice>
+#pragma once
+
+#include <ArmarXCore/interface/core/BasicVectorTypes.ice>
module armarx
-{
- /**
- * Implements an interface to an OptoForceUnitObserver.
- **/
- interface ProsthesisObserverInterface extends ObserverInterface, ProsthesisListenerInterface
+{
+ interface FrameTrackingInterface
{
+ void enableTracking(bool enable);
+ void setFrame(string frameName);
+ ["cpp:const"]
+ idempotent
+ string getFrame();
+
+ void lookAtFrame(string frameName);
+ void lookAtPointInGlobalFrame(Vector3f point);
+ void lookAtPointInRobotFrame(Vector3f point);
+
+ void scanInConfigurationSpace(float yawFrom, float yawTo, ::Ice::FloatSeq pitchValues, float velocity);
+ void scanInWorkspace(::armarx::Vector3fSeq points, float maxVelocity, float acceleration);
};
};
-
-#endif
diff --git a/source/RobotAPI/interface/core/CartesianPositionControllerConfig.ice b/source/RobotAPI/interface/core/CartesianPositionControllerConfig.ice
new file mode 100644
index 0000000000000000000000000000000000000000..0c1418a257091bead62d86a0629967ba8bfde77b
--- /dev/null
+++ b/source/RobotAPI/interface/core/CartesianPositionControllerConfig.ice
@@ -0,0 +1,49 @@
+/*
+* 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 ArmarX::RobotAPI
+* @author Martin Miller
+* @copyright 2015 Humanoids Group, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include <ArmarXCore/interface/observers/VariantBase.ice>
+#include <ArmarXCore/interface/observers/Filters.ice>
+
+module armarx
+{
+ /**
+ * [CartesianPositionControllerConfigBase] defines the config for CartesianPositionController
+ */
+ class CartesianPositionControllerConfigBase extends VariantDataClass
+ {
+ float KpPos = 1;
+ float KpOri = 1;
+ float maxPosVel = -1;
+ float maxOriVel = -1;
+
+ float thresholdOrientationNear = -1;
+ float thresholdOrientationReached = -1;
+ float thresholdPositionNear = -1;
+ float thresholdPositionReached = -1;
+ };
+
+
+
+};
+
diff --git a/source/RobotAPI/interface/core/CartesianWaypointControllerConfig.ice b/source/RobotAPI/interface/core/CartesianWaypointControllerConfig.ice
new file mode 100644
index 0000000000000000000000000000000000000000..1e2a1a8e9ea01ee32c0b6d229eeada60cda1c0aa
--- /dev/null
+++ b/source/RobotAPI/interface/core/CartesianWaypointControllerConfig.ice
@@ -0,0 +1,44 @@
+/*
+* 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 ArmarX::RobotAPI
+* @author Raphael Grimm
+* @copyright 2019 Humanoids Group, H2T, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+module armarx
+{
+ struct CartesianWaypointControllerConfig
+ {
+ float kpJointLimitAvoidance = 1;
+ float jointLimitAvoidanceScale = 2;
+
+ float thresholdOrientationNear = 0.1;
+ float thresholdOrientationReached = 0.05;
+ float thresholdPositionNear = 50;
+ float thresholdPositionReached = 5;
+
+ float maxOriVel = 1;
+ float maxPosVel = 80;
+ float kpOri = 1;
+ float kpPos = 1;
+ };
+};
+
+
diff --git a/source/RobotAPI/interface/observers/GraspCandidateObserverInterface.ice b/source/RobotAPI/interface/observers/GraspCandidateObserverInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..4f500a4520a8fc0cc7b85a424d2fcfd1b79874c1
--- /dev/null
+++ b/source/RobotAPI/interface/observers/GraspCandidateObserverInterface.ice
@@ -0,0 +1,67 @@
+/**
+* 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 as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* 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 Lesser 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
+* @author Simon Ottenhaus
+* @copyright 2019 Humanoids Group, H2T, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+
+#include <ArmarXCore/interface/observers/ObserverInterface.ice>
+#include <RobotAPI/interface/units/GraspCandidateProviderInterface.ice>
+
+module armarx
+{
+ module grasping
+ {
+ class CandidateFilterCondition
+ {
+ string providerName = "*";
+ float minimumSuccessProbability = 0;
+ ObjectTypeEnum objectType = AnyObject;
+ ApertureType preshape = AnyAperture;
+ ApproachType approach = AnyApproach;
+
+ };
+
+ sequence<string> StringSeq;
+ dictionary<string, int> IntMap;
+
+ interface GraspCandidateObserverInterface extends ObserverInterface, GraspCandidatesTopicInterface
+ {
+ InfoMap getAvailableProvidersWithInfo();
+ StringSeq getAvailableProviderNames();
+ ProviderInfo getProviderInfo(string providerName);
+ bool hasProvider(string providerName);
+ GraspCandidateSeq getAllCandidates();
+ GraspCandidateSeq getCandidatesByProvider(string providerName);
+ GraspCandidateSeq getCandidatesByFilter(CandidateFilterCondition filter);
+ int getUpdateCounterByProvider(string providerName);
+ IntMap getAllUpdateCounters();
+ bool setProviderConfig(string providerName, StringVariantBaseMap config);
+
+ // for execution
+ void setSelectedCandidates(GraspCandidateSeq candidates);
+ GraspCandidateSeq getSelectedCandidates();
+ };
+ };
+
+};
+
diff --git a/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice b/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..153810cd5c7e0a4d2bfd27a3e5238ef716d63adf
--- /dev/null
+++ b/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
@@ -0,0 +1,122 @@
+/**
+* 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 as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* 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 Lesser 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
+* @author Simon Ottenhaus
+* @copyright 2019 Humanoids Group, H2T, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <RobotAPI/interface/core/FramedPoseBase.ice>
+#include <ArmarXCore/interface/observers/VariantBase.ice>
+#include <ArmarXCore/interface/observers/RequestableService.ice>
+
+module armarx
+{
+ module grasping
+ {
+ enum ObjectTypeEnum {
+ AnyObject, KnownObject, UnknownObject
+ };
+ enum ApproachType {
+ AnyApproach, TopApproach, SideApproach
+ };
+ enum ApertureType {
+ AnyAperture, OpenAperture, PreshapedAperture
+ };
+
+ class BoundingBox
+ {
+ Vector3Base center;
+ Vector3Base ha1; // HalfAxis1 : longest half axis
+ Vector3Base ha2; // HalfAxis2 : middle half axis
+ Vector3Base ha3; // HalfAxis3 : shortest half axis
+ };
+
+ class GraspCandidateSourceInfo
+ {
+ PoseBase referenceObjectPose;
+ string referenceObjectName;
+ int segmentationLabelID = -1;
+ BoundingBox bbox;
+ };
+ class GraspCandidateExecutionHints
+ {
+ ApertureType preshape;
+ ApproachType approach;
+ string graspTrajectoryName;
+ };
+ class GraspCandidateReachabilityInfo
+ {
+ bool reachable = false;
+ float minimumJointLimitMargin = -1;
+ Ice::FloatSeq jointLimitMargins;
+ float maxPosError = 0;
+ float maxOriError = 0;
+ };
+
+
+
+ class GraspCandidate
+ {
+ PoseBase graspPose;
+ PoseBase robotPose;
+ Vector3Base approachVector;
+
+ string sourceFrame; // frame where graspPose is located
+ string targetFrame; // frame which should be moved to graspPose
+ string side;
+
+ float graspSuccessProbability;
+
+ ObjectTypeEnum objectType;
+ int groupNr = -1; // used to match candidates that belog together, e.g. from the same object or point cloud segment
+ string providerName;
+
+ GraspCandidateSourceInfo sourceInfo; // (optional)
+ GraspCandidateExecutionHints executionHints; // (optional)
+ GraspCandidateReachabilityInfo reachabilityInfo; // (optional)
+ };
+
+
+
+ class ProviderInfo
+ {
+ ObjectTypeEnum objectType = AnyObject;
+ StringVariantBaseMap currentConfig;
+ };
+
+ sequence<GraspCandidate> GraspCandidateSeq;
+ dictionary<string, ProviderInfo> InfoMap;
+
+
+ interface GraspCandidatesTopicInterface
+ {
+ void reportProviderInfo(string providerName, ProviderInfo info);
+ void reportGraspCandidates(string providerName, GraspCandidateSeq candidates);
+ };
+
+ interface GraspCandidateProviderInterface extends RequestableServiceListenerInterface
+ {
+ };
+ };
+
+};
+
diff --git a/source/RobotAPI/interface/units/KITProstheticHandInterface.ice b/source/RobotAPI/interface/units/KITProstheticHandInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..1babfdfe0b919ab38cba8b3f379db26260282f04
--- /dev/null
+++ b/source/RobotAPI/interface/units/KITProstheticHandInterface.ice
@@ -0,0 +1,76 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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
+ * @author Julia Starke <julia dot starke at kit dot edu>
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+module KITProsthesis
+{
+ module ProsthesisState
+ {
+ //the enclosing namespace scopes the c-style enum values
+ enum State
+ {
+ Created,
+ DiscoveringDevices,
+ DiscoveringDevicesDone,
+ Disconnected,
+ Connecting,
+ ConnectingDone,
+ DiscoveringServices,
+ DiscoveringServicesDone,
+ ConnectingService,
+ Running,
+ Killed
+ };
+ };
+
+ struct ProsthesisMotorValues
+ {
+ long v;
+ long maxPWM;
+ long pos;
+ };
+
+ struct ProsthesisSensorValues
+ {
+ ProsthesisState::State state;
+
+ long thumbPWM;
+ long thumbPos;
+
+ long fingerPWM;
+ long fingerPos;
+
+ //add IMU here
+ };
+
+ interface KITProstheticHandInterface
+ {
+ void sendGrasp(int n);
+ void sendThumbPWM(ProsthesisMotorValues motorValues);
+ void sendFingerPWM(ProsthesisMotorValues motorValues);
+
+ ProsthesisSensorValues getSensorValues();
+ };
+};
diff --git a/source/RobotAPI/interface/units/ProsthesisInterface.ice b/source/RobotAPI/interface/units/ProsthesisInterface.ice
deleted file mode 100644
index 28219519e85357d13747a234352dd594d6f4c805..0000000000000000000000000000000000000000
--- a/source/RobotAPI/interface/units/ProsthesisInterface.ice
+++ /dev/null
@@ -1,51 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * 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
- * @author Peter Kaiser <peter dot kaiser at kit dot edu>
- * @date 2014
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#ifndef _ARMARX_ROBOTAPI_SIMPLE_HAPTIC_INTERFACE_SLICE_
-#define _ARMARX_ROBOTAPI_SIMPLE_HAPTIC_INTERFACE_SLICE_
-
-module armarx
-{
- struct ProsthesisMotorValues
- {
- string name;
- float position1;
- float pwm1;
- float position2;
- float pwm2;
- };
-
- interface ProsthesisListenerInterface
- {
- void reportMotorValues(ProsthesisMotorValues motorValues);
- /*void reportMotorValues(string name, float position1, float pwm1, float position2, float pwm2);*/
- };
-
- interface ProsthesisInterface
- {
- string getTopicName();
- };
-};
-
-#endif
diff --git a/source/RobotAPI/interface/units/RobotUnit/DSControllerBase.ice b/source/RobotAPI/interface/units/RobotUnit/DSControllerBase.ice
new file mode 100644
index 0000000000000000000000000000000000000000..1ae23199e5281780ec263c0ec62b0dbf3e6faabf
--- /dev/null
+++ b/source/RobotAPI/interface/units/RobotUnit/DSControllerBase.ice
@@ -0,0 +1,137 @@
+/*
+ * 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::NJointControllerInterface
+ * @author Mirko Waechter ( mirko dot waechter at kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
+
+module armarx
+{
+ class DSControllerConfig extends NJointControllerConfig
+ {
+ float posiKp = 5;
+ float v_max = 0.15;
+ float posiDamping = 10;
+
+ float oriDamping;
+ float oriKp;
+
+ float filterTimeConstant = 0.01;
+ float torqueLimit = 1;
+
+
+ string nodeSetName = "";
+ string tcpName = "";
+
+ Ice::FloatSeq desiredPosition;
+ Ice::FloatSeq desiredQuaternion;
+
+ Ice::FloatSeq qnullspaceVec;
+
+ float nullspaceKp;
+ float nullspaceDamping;
+
+
+ Ice::StringSeq gmmParamsFiles;
+ float positionErrorTolerance;
+
+ float dsAdaptorEpsilon;
+
+ };
+
+ interface DSControllerInterface extends NJointControllerInterface
+ {
+
+ };
+
+ class DSRTBimanualControllerConfig extends NJointControllerConfig
+ {
+ float posiKp = 5;
+ float v_max = 0.15;
+ Ice::FloatSeq posiDamping;
+ float couplingStiffness = 10;
+ float couplingForceLimit = 50;
+
+ float forwardGain = 1;
+
+ float oriDamping;
+ float oriKp;
+
+ float filterTimeConstant = 0.01;
+ float torqueLimit = 1;
+ float NullTorqueLimit = 0.2;
+
+// string tcpName = "";
+
+// Ice::FloatSeq desiredPosition;
+
+ Ice::FloatSeq left_desiredQuaternion;
+ Ice::FloatSeq right_desiredQuaternion;
+
+ Ice::FloatSeq leftarm_qnullspaceVec;
+ Ice::FloatSeq rightarm_qnullspaceVec;
+
+ float nullspaceKp;
+ float nullspaceDamping;
+
+
+
+ string gmmParamsFile = "";
+ float positionErrorTolerance;
+
+ float contactForce;
+ float guardTargetZUp;
+ float guardTargetZDown;
+ float loweringForce;
+ float liftingForce;
+ bool guardDesiredDirection;
+ float highPassFilterFactor;
+
+
+ Ice::FloatSeq left_oriUp;
+ Ice::FloatSeq left_oriDown;
+ Ice::FloatSeq right_oriUp;
+ Ice::FloatSeq right_oriDown;
+ float forceFilterCoeff;
+ float forceErrorZDisturbance;
+ float forceErrorZThreshold;
+ float forceErrorZGain;
+ float desiredTorqueDisturbance;
+ float TorqueFilterConstant;
+ float leftForceOffsetZ;
+ float rightForceOffsetZ;
+ float contactDistanceTolerance;
+ float mountingDistanceTolerance;
+ float mountingCorrectionFilterFactor;
+
+ Ice::FloatSeq forceLeftOffset;
+ Ice::FloatSeq forceRightOffset;
+ };
+
+
+ interface DSBimanualControllerInterface extends NJointControllerInterface
+ {
+ void setToDefaultTarget();
+
+ };
+
+};
diff --git a/source/RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.ice b/source/RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.ice
index 04cc5fae69b1c830c4d64e172a97e40314462f11..34e01876aff6c4d9cdb0e02d3ed79fde22c5d300 100644
--- a/source/RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.ice
@@ -48,6 +48,7 @@ module armarx
interface NJointTaskSpaceImpedanceControlInterface extends NJointControllerInterface
{
void setTarget(Ice::FloatSeq target);
+ void setImpedanceParameters(string paraName, Ice::FloatSeq vals);
};
};
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractData.h b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractData.h
index f4a0e8c3162f72ec60777e220ec1e233cf6cbd37..50207f96b4df5d3cfc58d302564c6ca188dacd97 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractData.h
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractData.h
@@ -48,16 +48,29 @@ namespace armarx
* @param node
* @param defaultValue
* @param offsetBeforeFactor if true the offset is added before multiplying with factor. If false: the other way around.
+ * @param nameForDebugging This name is printend in case an error is encountered (Its only purpose is to ease debugging)
*/
- void init(T* raw, const DefaultRapidXmlReaderNode& node, float defaultValue = std::nan("1"), bool offsetBeforeFactor = true)
+ void init(T* raw, const DefaultRapidXmlReaderNode& node, float defaultValue = std::nan("1"), bool offsetBeforeFactor = true, const char* nameForDebugging = "")
{
float factor = node.attribute_as_float("factor");
float offset = node.attribute_as_float("offset");
- init(raw, factor, offset, defaultValue, offsetBeforeFactor);
+ init(raw, factor, offset, defaultValue, offsetBeforeFactor, nameForDebugging);
}
- void init(T* raw, float factor, float offset, float defaultValue = std::nan("1"), bool offsetBeforeFactor = true)
+ void init(T* raw, float factor, float offset, float defaultValue = std::nan("1"), bool offsetBeforeFactor = true, const char* nameForDebugging = "")
{
+ const auto rawAsInt = reinterpret_cast<std::uint64_t>(raw);
+ ARMARX_CHECK_EXPRESSION_W_HINT(
+ (rawAsInt % alignof(T)) == 0,
+ "\nThe alignment is wrong!\nIt has to be "
+ << alignof(T) << ", but the data is aligned with "
+ << rawAsInt % alignof(std::max_align_t)
+ << "!\nThis is an offset of " << (rawAsInt % alignof(T))
+ << " bytes!\nThe datatype is " << GetTypeString<T>()
+ << "\nIts size is " << sizeof(T)
+ << "\nraw = " << raw
+ << " bytes\nThe name is " << nameForDebugging
+ );
this->offsetBeforeFactor = offsetBeforeFactor;
this->factor = factor;
this->offset = offset;
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.h b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.h
index ffb694ab40b87caf32c05d0f761f0bc1e50dd7fe..fb8868b0d0044514b29702b9d144af9e75209531 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.h
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.h
@@ -110,7 +110,7 @@ namespace armarx
public:
using AbstractSlave::AbstractSlave;
- void setInputPDO(void *ptr) override
+ void setInputPDO(void* ptr) override
{
const auto ptrAsInt = reinterpret_cast<std::uint64_t>(ptr);
ARMARX_CHECK_EXPRESSION_W_HINT(
@@ -125,7 +125,7 @@ namespace armarx
);
inputs = static_cast<InputT*>(ptr);
}
- void setOutputPDO(void *ptr) override
+ void setOutputPDO(void* ptr) override
{
const auto ptrAsInt = reinterpret_cast<std::uint64_t>(ptr);
ARMARX_CHECK_EXPRESSION_W_HINT(
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.cpp
index 792583d9d314e3b6135e226d028b2da7d1367b85..d85ca3490aa355e853dc7b65ec873a0710b4e668 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.cpp
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.cpp
@@ -119,4 +119,9 @@ namespace armarx
return returnList;
}
+ std::vector<AbstractFunctionalDevicePtr> DeviceContainer::getAllFunctionalDevices() const
+ {
+ return devices;
+ }
+
} // namespace armarx
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.h b/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.h
index 2ae55ba257f5e88c426d715ad74e342b7e0763a3..52288ccc8797644cf80038d71f2eacfd97d02cb1 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.h
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.h
@@ -52,7 +52,7 @@ namespace armarx
}
std::vector<AbstractFunctionalDevicePtr> getAllInitializedFunctionalDevices() const;
std::vector<AbstractFunctionalDevicePtr> getAllUninitializedFunctionalDevices() const;
- virtual std::vector<AbstractFunctionalDevicePtr> getAllFunctionalDevices() const = 0;
+ virtual std::vector<AbstractFunctionalDevicePtr> getAllFunctionalDevices() const;
protected:
std::vector<AbstractFunctionalDevicePtr> devices;
};
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
index a522aa295ee936e907aec5da8d3c4b24252e1444..4fffde8837104f7ecc90c71ad39d0e84d9ab1b79 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
@@ -21,13 +21,20 @@
#include "DeviceContainer.h"
#include "EtherCATDeviceFactory.h"
//include all soem stuff here so no one should be able to use soem functions unintentionally because he has to include some first
+#define EC_VER1
+#include <ethercattype.h>
+#include <nicdrv.h>
+#include <osal.h>
+#include <osal_defs.h>
+#include <ethercatmain.h>
+#include <ethercatbase.h>
+#include <ArmarXCore/core/util/OnScopeExit.h>
//EtherCAT definitions only used for the ethercat stuff
#define SDO_READ_TIMEOUT 100000
#define SDO_WRITE_TIMEOUT 50000
#define EC_TIMEOUTMON 2000
-
using namespace armarx;
@@ -65,6 +72,7 @@ EtherCAT::EtherCAT() : expectedWKC(-1),
deviceContainerPtr(nullptr),
mainUnitPtr(nullptr)
{
+ checkErrorCountersOnWKCError = false;
//writing zeros to the IOMap
for (size_t i = 0; i < IOmapSize; ++i)
{
@@ -183,6 +191,7 @@ bool EtherCAT::startBus(bool createDevices)
{
slave->prepareForSafeOp();
}
+ ARMARX_INFO << "Finishing Preparing for safe op now";
for (auto slave : slaves)
{
slave->finishPreparingForSafeOp();
@@ -190,10 +199,11 @@ bool EtherCAT::startBus(bool createDevices)
osal_usleep(500000);
-
///// going to SAFE_OP
//do the mapping
+ ARMARX_INFO << "Mapping....";
actualMappedSize = ec_config_map(IOmap.data());
+ ARMARX_INFO << "Going to safe op now";
//wait to get all slaves to SAFE-OP
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4);
ARMARX_INFO << "IOmapping done, size: " << actualMappedSize << " - all Slaves are in SAFE-OP now\n";
@@ -231,7 +241,7 @@ bool EtherCAT::startBus(bool createDevices)
/// setting the data pointer in the functional devices
for (AbstractFunctionalDevicePtr& device : functionalDevices)
{
- ARMARX_INFO << "init for " << device->getClassName();
+ ARMARX_INFO << "init for device type '" << device->getClassName() << "'";
device->initData();
}
@@ -366,8 +376,10 @@ bool EtherCAT::setPDOMappings()
int byteSum = 0;
for (std::shared_ptr<AbstractSlave>& slave : slaves)
{
+ ARMARX_INFO << "Checking mapping for slave " << slave->getSlaveNumber() << " name: " << slave->getSlaveIdentifier().humanName;
if (!slave->hasPDOMapping())
{
+ ARMARX_INFO << "No mapping needed for " << slave->getSlaveIdentifier().humanName;
continue;
}
byteSum += ec_slave[slave->getSlaveNumber()].Obytes + ec_slave[slave->getSlaveNumber()].Ibytes;
@@ -419,6 +431,7 @@ std::pair<std::vector<ControlDevicePtr>, std::vector<SensorDevicePtr>> EtherCAT:
}
auto etherCATFactoryNames = EtherCATDeviceFactory::getAvailableClasses();
ARMARX_INFO << "PPP: " << etherCATFactoryNames;
+ std::map<std::string, std::vector<uint16_t>> devicesOfType;
for (uint16_t currentSlaveIndex = 1; currentSlaveIndex <= ec_slavecount; currentSlaveIndex++)
{
const std::string messageSlaveIdentifier = "[Slave index: " + std::to_string(currentSlaveIndex) + "] ";
@@ -439,10 +452,13 @@ std::pair<std::vector<ControlDevicePtr>, std::vector<SensorDevicePtr>> EtherCAT:
auto device = EtherCATDeviceFactory::fromName(facName, std::make_tuple(this, currentSlaveIndex, deviceContainerPtr));
if (device)
{
- ARMARX_INFO << "Found device of type: " << device->getClassName();
- ARMARX_CHECK_GREATER(device->getSlaves().size(), 0);
- currentSlaveIndex += device->getSlaves().size() - 1;
- slaves.insert(slaves.end(), device->getSlaves().begin(), device->getSlaves().end());
+ devicesOfType[device->getClassName()].emplace_back(currentSlaveIndex);
+
+ auto newSlaves = device->getSlaves();
+ ARMARX_INFO << "Found device of type: " << device->getClassName() << " with " << newSlaves.size() << " slaves";
+ ARMARX_CHECK_GREATER(newSlaves.size(), 0);
+ currentSlaveIndex += newSlaves.size() - 1;
+ slaves.insert(slaves.end(), newSlaves.begin(), newSlaves.end());
ctrlDevs.insert(device->getCtrlDevs().begin(), device->getCtrlDevs().end());
sensDevs.insert(device->getSensDevs().begin(), device->getSensDevs().end());
foundDevice = true;
@@ -458,12 +474,24 @@ std::pair<std::vector<ControlDevicePtr>, std::vector<SensorDevicePtr>> EtherCAT:
}
if (!foundDevice)
{
+ devicesOfType["<NO FACTORY FOUND>"].emplace_back(currentSlaveIndex);
ARMARX_ERROR << "Could not find a corresponding factory for " << messageSlaveIdentifier << " product id: " << (int)ec_slave[currentSlaveIndex].eep_id <<
"vendor id: " << (int)ec_slave[currentSlaveIndex].eep_man;
}
}
-
- ARMARX_INFO << slaves.size() << " usable slave devices are initialized!" << std::endl;
+ ARMARX_INFO << "Summary of devices and factory types:\n" << ARMARX_STREAM_PRINTER
+ {
+ for (const auto& [factoryName, ids] : devicesOfType)
+ {
+ out << "---- " << factoryName << ": #" << ids.size() << " (ids:";
+ for (auto id : ids)
+ {
+ out << ' ' << id;
+ }
+ out << ")\n";
+ }
+ }
+ << '\n' << slaves.size() << " usable slave devices are initialized!" << std::endl;
return {std::vector<ControlDevicePtr>(ctrlDevs.begin(), ctrlDevs.end()), std::vector<SensorDevicePtr>(sensDevs.begin(), sensDevs.end())};
}
@@ -496,7 +524,10 @@ bool EtherCAT::updateBus(bool doErrorHandling)
}
else if (switchON_OFF == OFF)
{
- ARMARX_WARNING << "Could not update bus because it switched off - closing bus (again?)";
+ if (!isSDOWorking)
+ {
+ ARMARX_WARNING << "Could not update bus because it switched off - closing bus (again?)";
+ }
closeBus();
return false;
}
@@ -590,7 +621,7 @@ void EtherCAT::switchBusOFF()
ARMARX_VERBOSE << deactivateSpam(1) << "Bus is already switched off";
return;
}
- ARMARX_VERBOSE << "Switching off bus";
+ ARMARX_INFO << "Switching off bus";
switchON_OFF = OFF;
}
@@ -689,20 +720,54 @@ void EtherCAT::errorHandling()
uint16 slave;
//if the bus is in already in safe op then we have an error and don't need any more stuff to do
error = ec_slave[0].state == EC_STATE_SAFE_OP;
- if ((lastWorkCounter == expectedWKC))
+
+ if (lastWorkCounter == expectedWKC)
{
noErrorIterations++;
-
}
+ ARMARX_ON_SCOPE_EXIT
+ {
+ firstErrorCheck = false;
+ };
if (((lastWorkCounter < expectedWKC) || ec_group[currentGroup].docheckstate) && !error)
{
- ARMARX_RT_LOGF_WARN("RECOVERY - Wkc: %s - %d, state: %s - iterations without error: %d", ((lastWorkCounter < expectedWKC) ? "NOT OK" : "OK"),
- lastWorkCounter, (ec_group[currentGroup].docheckstate ? "NOT OK" : "OK"), noErrorIterations);
- noErrorIterations = 0;
+
+ ARMARX_RT_LOGF_WARN("RECOVERY - Wkc: %s - %d/%d, state: %s - iterations without error: %d", ((lastWorkCounter < expectedWKC) ? "NOT OK" : "OK"),
+ lastWorkCounter, expectedWKC, (ec_group[currentGroup].docheckstate ? "NOT OK" : "OK"), noErrorIterations);
+
//actually there is something wrong so we have an error lets see if we can find an fix it
error = true;
+ if (checkErrorCountersOnWKCError || (!firstErrorCheck && noErrorIterations == 0))
+ {
+ IceUtil::Time start = IceUtil::Time::now(IceUtil::Time::Monotonic);
+ for (uint16 slaveIndex = 1; slaveIndex <= *(ecx_context.slavecount); slaveIndex++)
+ {
+ auto ADPh = (uint16)(1 - slaveIndex);
+ uint16_t configAddr = ecx_APRDw(ecx_context.port, ADPh, ECT_REG_STADR, 100000);
+ for (int i = 0; i < 4; i++)
+ {
+
+
+ uint16_t rxErrorField = ecx_APRDw(ecx_context.port, ADPh, 0x300 + i * 2, 100000);
+ uint16_t forwardedRxErrorField = ecx_APRDw(ecx_context.port, ADPh, 0x308 + i, 100000);
+ uint8_t invalidFrameCounter = rxErrorField >> 8;
+ uint8_t rxError = rxErrorField;
+ if (rxError > 0 || invalidFrameCounter > 0 || forwardedRxErrorField > 0)
+ {
+ ARMARX_INFO << "non zero frame error counter found: Slavenumber " << slaveIndex << " config addr: " << (int)configAddr << ": port " << i << ": " << VAROUT((int)invalidFrameCounter) << VAROUT((int)rxError) << VAROUT((int)forwardedRxErrorField);
+ }
+ }
+ if ((IceUtil::Time::now(IceUtil::Time::Monotonic) - start).toMilliSeconds() > 10)
+ {
+ updatePDO();
+ ARMARX_RT_LOGF_INFO("Updated BUS to prevent timeout").deactivateSpam(1);
+ start = IceUtil::Time::now(IceUtil::Time::Monotonic);
+ }
+ }
+ }
+ noErrorIterations = 0;
/* one ore more slaves are not responding */
//This is hard SOEM code not the easy stuff, but works...
@@ -1428,4 +1493,13 @@ bool EtherCAT::isEmergencyStopActive() const
}
+bool EtherCAT::getCheckErrorCountersOnWKCError() const
+{
+ return checkErrorCountersOnWKCError;
+}
+
+void EtherCAT::setCheckErrorCountersOnWKCError(bool value)
+{
+ checkErrorCountersOnWKCError = value;
+}
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h
index e0a231dce019a0e72b4f99482a6ce94416e339bc..20b5bf9ee8aafb71837cb024bda2ed56b9521579 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h
@@ -147,6 +147,10 @@ namespace armarx
std::vector<SensorDevicePtr> getSensDevs() const;
+
+ bool getCheckErrorCountersOnWKCError() const;
+ void setCheckErrorCountersOnWKCError(bool value);
+
private:
//Hiding the constructor to avoid illegal creation of the Bus (singelton pattern)
EtherCAT();
@@ -193,7 +197,7 @@ namespace armarx
//! Socketfiledescriptor on which the ethercat connection is running
int socketFileDescriptor = -1;
/** @brief IOmap the IO map where the process data are mapped in */
- std::array<char, IOmapSize> IOmap;
+ alignas(alignof(std::max_align_t)) std::array<char, IOmapSize> IOmap;
/** @brief switchON_OFF this flag is used to switch the bus off an close it */
std::atomic_bool switchON_OFF;
/** current Bus group we are working on */
@@ -208,6 +212,7 @@ namespace armarx
/** the working counter from the last transmission */
int lastWorkCounter;
int noErrorIterations = 0;
+ bool firstErrorCheck = true;
/** List of all functional/internal Units */
std::vector<AbstractFunctionalDevicePtr> functionalDevices;
@@ -215,6 +220,8 @@ namespace armarx
int actualMappedSize;
+ std::atomic_bool checkErrorCountersOnWKCError;
+
/** Indicates if the bus got broken and is not recoverable, so we don't need to switch it down correct it already went away...*/
std::atomic_bool isBusDead;
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
index 705f140e22df531b583bacc3f5bfaa7b7fe8ad73..548d42be36a4a4832493c259e8f9b48d3ec0ed52 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
@@ -31,7 +31,6 @@
#include <sys/mman.h>
#include <sys/stat.h>
-#include <boost/filesystem/path.hpp>
#include <boost/algorithm/clamp.hpp>
#include <VirtualRobot/Tools/Gravity.h>
@@ -78,12 +77,20 @@ EtherCATRTUnit::EtherCATRTUnit() :
}
-
+void EtherCATRTUnit::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
+{
+ if (changedProperties.count("checkErrorCountersOnEtherCATError"))
+ {
+ auto val = getProperty<bool>("checkErrorCountersOnEtherCATError").getValue();
+ ARMARX_INFO << "Changing checkErrorCountersOnEtherCATError to " << val;
+ EtherCAT::getBus().setCheckErrorCountersOnWKCError(val);
+ }
+}
void EtherCATRTUnit::onInitRobotUnit()
{
-
+ rtWarningFactor = getProperty<float>("RTLoopTimingCheckToleranceFactor").getValue();
rtLoopTime = getProperty<int>("RTLoopFrequency").getValue();
@@ -100,7 +107,7 @@ void EtherCATRTUnit::onInitRobotUnit()
memset(dummy, 0, MAX_SAFE_STACK);
ARMARX_INFO << "EtherCATRTUnit initializing now";
- robot = rtGetRobot()->clone();
+ publishRobot = rtGetRobot()->clone();
rtRobotJointSet = rtGetRobot()->getRobotNodeSet("RobotJoints");
rtRobotBodySet = rtGetRobot()->getRobotNodeSet("RobotCol");
for (auto& node : *rtRobotJointSet)
@@ -209,8 +216,8 @@ void EtherCATRTUnit::joinControlThread()
{
ARMARX_INFO << "stopping control task";
taskRunning = false;
- EtherCAT& bus = EtherCAT::getBus();
- bus.switchBusOFF();
+ // EtherCAT& bus = EtherCAT::getBus();
+ // bus.switchBusOFF();
rtTask.join();
ARMARX_INFO << "rt task stopped";
}
@@ -219,40 +226,40 @@ void EtherCATRTUnit::publish(StringVariantBaseMap debugObserverMap, StringVarian
{
RobotUnit::publish(std::move(debugObserverMap), std::move(timingMap));
- for (auto& name : getSensorDeviceNames())
- {
- auto data = getSensorDevice(name)->getSensorValue()->asA<SensorValue1DoFActuatorPosition>();
- if (!data)
- {
- continue;
- }
- robot->getRobotNode(name)->setJointValueNoUpdate(data->position);
- }
- robot->applyJointValues();
- for (auto& name : getSensorDeviceNames())
- {
- auto data = getSensorDevice(name)->getSensorValue()->asA<SensorValue1DoFActuatorTorque>();
- if (!data)
- {
- continue;
- }
- auto node = robot->getRobotNode(name);
- ARMARX_CHECK_EXPRESSION(node);
- auto joint = boost::dynamic_pointer_cast<VirtualRobot::RobotNodeRevolute>(node);
- ARMARX_CHECK_EXPRESSION(joint);
- Vector3Ptr pos = new Vector3(joint->getGlobalPosition());
- Vector3Ptr axis = new Vector3(joint->getJointRotationAxis());
- float percent = data->torque / 4.0f;
- if (std::abs(percent) > 0.1)
- {
- getDebugDrawerProxy()->setCircleArrowVisu("TorqueEstimation", name, pos, axis, 100, percent, 5, DrawColor{0, 1, 0, 1});
- }
- else
- {
- getDebugDrawerProxy()->removeCircleVisu("TorqueEstimation", name);
- }
-
- }
+ // for (auto& name : getSensorDeviceNames())
+ // {
+ // auto data = getSensorDevice(name)->getSensorValue()->asA<SensorValue1DoFActuatorPosition>();
+ // if (!data)
+ // {
+ // continue;
+ // }
+ // publishRobot->getRobotNode(name)->setJointValueNoUpdate(data->position);
+ // }
+ // publishRobot->applyJointValues();
+ // for (auto& name : getSensorDeviceNames())
+ // {
+ // auto data = getSensorDevice(name)->getSensorValue()->asA<SensorValue1DoFActuatorTorque>();
+ // if (!data)
+ // {
+ // continue;
+ // }
+ // auto node = publishRobot->getRobotNode(name);
+ // ARMARX_CHECK_EXPRESSION(node);
+ // auto joint = boost::dynamic_pointer_cast<VirtualRobot::RobotNodeRevolute>(node);
+ // ARMARX_CHECK_EXPRESSION(joint);
+ // Vector3Ptr pos = new Vector3(joint->getGlobalPosition());
+ // Vector3Ptr axis = new Vector3(joint->getJointRotationAxis());
+ // float percent = data->torque / 4.0f;
+ // if (std::abs(percent) > 0.1)
+ // {
+ // getDebugDrawerProxy()->setCircleArrowVisu("TorqueEstimation", name, pos, axis, 100, percent, 5, DrawColor {0, 1, 0, 1});
+ // }
+ // else
+ // {
+ // getDebugDrawerProxy()->removeCircleVisu("TorqueEstimation", name);
+ // }
+
+ // }
}
armarx::PropertyDefinitionsPtr EtherCATRTUnit::createPropertyDefinitions()
@@ -274,9 +281,19 @@ void EtherCATRTUnit::rtRun()
ARMARX_ON_SCOPE_EXIT
{
+ ARMARX_INFO << "Leaving rtRun scope";
//switching off the bus and be sure that the bus will receive
bus.switchBusOFF();
bus.updateBus();
+ if (getProperty<int>("SocketFileDescriptor").isSet() && getProperty<int>("SocketFileDescriptor").getValue() > 0)
+ {
+ ARMARX_INFO << "Closing raw socket with FD " << getProperty<int>("SocketFileDescriptor").getValue();
+ int ret = close(getProperty<int>("SocketFileDescriptor").getValue());
+ if (ret)
+ {
+ ARMARX_INFO << "Failed to close raw socket file handle: " << ret << " errno: " << strerror(errno);
+ }
+ }
};
@@ -408,9 +425,9 @@ void EtherCATRTUnit::rtRun()
// unitInitTask.join();
controlLoopRTThread();
- //switching off the bus and be sure that the bus will receive
- bus.switchBusOFF();
- bus.updateBus();
+ // //switching off the bus and be sure that the bus will receive
+ // bus.switchBusOFF();
+ // bus.updateBus();
while (getObjectScheduler() && !getObjectScheduler()->isTerminationRequested())
{
@@ -425,24 +442,24 @@ void EtherCATRTUnit::icePropertiesInitialized()
}
-MultiNodeRapidXMLReader EtherCATRTUnit::readHardwareConfigFile(std::string busConfigFilePath)
+MultiNodeRapidXMLReader EtherCATRTUnit::ReadHardwareConfigFile(std::string busConfigFilePath, std::string rootNodeName)
{
if (!ArmarXDataPath::getAbsolutePath(busConfigFilePath, busConfigFilePath))
{
throw LocalException("could not find BusConfigFilePath: ") << busConfigFilePath;
}
- ARMARX_DEBUG << "read the config";
+ ARMARX_DEBUG_S << "read the config";
//reading the config for the Bus and create all the robot objects in the robot container
- auto busConfigFilePathDir = boost::filesystem::path(busConfigFilePath).parent_path();
+ auto busConfigFilePathDir = std::filesystem::path(busConfigFilePath).parent_path();
auto rapidXmlReaderPtr = RapidXmlReader::FromFile(busConfigFilePath);
- auto rootNode = rapidXmlReaderPtr->getRoot("Armar6");
+ auto rootNode = rapidXmlReaderPtr->getRoot(rootNodeName.c_str());
MultiNodeRapidXMLReader multiNode({rootNode});
for (RapidXmlReaderNode& includeNode : rootNode.nodes("include"))
{
auto relPath = includeNode.attribute_value("file");
- boost::filesystem::path filepath = (busConfigFilePathDir / relPath);
- if (!boost::filesystem::exists(filepath))
+ std::filesystem::path filepath = (busConfigFilePathDir / relPath);
+ if (!std::filesystem::exists(filepath))
{
std::string absPath;
if (!ArmarXDataPath::getAbsolutePath(relPath, absPath))
@@ -450,7 +467,7 @@ MultiNodeRapidXMLReader EtherCATRTUnit::readHardwareConfigFile(std::string busCo
throw LocalException("Could not find config file at path ") << relPath;
}
}
- multiNode.addNode(RapidXmlReader::FromFile(filepath.string())->getRoot("Armar6"));
+ multiNode.addNode(RapidXmlReader::FromFile(filepath.string())->getRoot(rootNodeName.c_str()));
}
return multiNode;
}
@@ -540,21 +557,21 @@ void EtherCATRTUnit::controlLoopRTThread()
RT_TIMING_START(RunControllers);
RT_TIMING_START(SwitchControllers);
rtSwitchControllerSetup();
- RT_TIMING_CEND(SwitchControllers, 0.3);
+ RT_TIMING_CEND(SwitchControllers, 0.3 * rtWarningFactor);
RT_TIMING_START(ResettingTargets);
rtResetAllTargets();
- RT_TIMING_CEND(ResettingTargets, 0.3);
+ RT_TIMING_CEND(ResettingTargets, 0.3 * rtWarningFactor);
RT_TIMING_START(RunNJointControllers);
rtRunNJointControllers(currentPDOUpdateTimestamp, cappedDeltaT);
- RT_TIMING_CEND(RunNJointControllers, 0.3);
+ RT_TIMING_CEND(RunNJointControllers, 0.3 * rtWarningFactor);
RT_TIMING_START(CheckInvalidTargets);
rtHandleInvalidTargets();
- RT_TIMING_CEND(CheckInvalidTargets, 0.3);
+ RT_TIMING_CEND(CheckInvalidTargets, 0.3 * rtWarningFactor);
RT_TIMING_START(RunJointControllers);
rtRunJointControllers(currentPDOUpdateTimestamp, cappedDeltaT);
- RT_TIMING_CEND(RunJointControllers, 0.3);
- RT_TIMING_CEND(RunControllers, 0.3);
+ RT_TIMING_CEND(RunJointControllers, 0.3 * rtWarningFactor);
+ RT_TIMING_CEND(RunControllers, 0.3 * rtWarningFactor);
}
//bus update
@@ -577,20 +594,20 @@ void EtherCATRTUnit::controlLoopRTThread()
rtSetEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
}
}
- RT_TIMING_CEND(updateBus, 0.7);
+ RT_TIMING_CEND(updateBus, 0.7 * rtWarningFactor);
rtGetRTThreadTimingsSensorDevice().rtMarkRtBusSendReceiveEnd();
RT_TIMING_START(ReadSensorValues);
rtReadSensorDeviceValues(currentPDOUpdateTimestamp, cappedDeltaT);
- RT_TIMING_CEND(ReadSensorValues, 0.7);
+ RT_TIMING_CEND(ReadSensorValues, 0.7 * rtWarningFactor);
lastMonoticTimestamp = currentMonotonicTimestamp;
currentMonotonicTimestamp = armarx::rtNow();
RT_TIMING_START(Publish);
rtUpdateSensorAndControlBuffer(currentPDOUpdateTimestamp, cappedDeltaT);
- RT_TIMING_CEND(Publish, 0.15);
+ RT_TIMING_CEND(Publish, 0.15 * rtWarningFactor);
RT_TIMING_START(ComputeGravityTorques);
gravity.computeGravityTorque(gravityValues);
@@ -605,7 +622,7 @@ void EtherCATRTUnit::controlLoopRTThread()
i++;
}
- RT_TIMING_CEND(ComputeGravityTorques, 0.2);
+ RT_TIMING_CEND(ComputeGravityTorques, 0.2 * rtWarningFactor);
// copyEtherCATBufferOut();
@@ -614,11 +631,11 @@ void EtherCATRTUnit::controlLoopRTThread()
const auto loopPreSleepTime = armarx::rtNow();
RT_TIMING_START(RTLoopWaiting);
cycleKeeper.waitForCycleDuration();
- RT_TIMING_CEND(RTLoopWaiting, rtLoopTime * 1.3);
+ RT_TIMING_CEND(RTLoopWaiting, rtLoopTime * 1.3 * rtWarningFactor);
const auto loopPostSleepTime = armarx::rtNow();
const auto loopDuration = armarx::rtNow() - loopStartTime;
- if (loopDuration.toMicroSeconds() > rtLoopTime + 500)
+ if (loopDuration.toMicroSeconds() > (rtLoopTime + 500) * rtWarningFactor)
{
const SensorValueRTThreadTimings* sval = rtGetRTThreadTimingsSensorDevice().getSensorValue()->asA<SensorValueRTThreadTimings>();
ARMARX_CHECK_NOT_NULL(sval);
@@ -714,6 +731,7 @@ void EtherCATRTUnit::controlLoopRTThread()
ARMARX_FATAL << "exception in control thread!" << std::flush;
//TODO emergency stop
}
+ ARMARX_INFO << "Leaving control loop function";
}
DeviceContainerPtr EtherCATRTUnit::getDeviceContainerPtr() const
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.h b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.h
index faf3d505d9bb5596faa045e8cad5bff4052256d0..1619c7c1708301607ebf7588a99363bca23f1d35 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.h
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.h
@@ -68,7 +68,10 @@ namespace armarx
defineOptionalProperty<bool>("UseTorqueVelocityModeAsDefault", false, "If true, the KinematicUnit will use TorqueVelocity mode for velocity mode");
defineOptionalProperty<int>("RTLoopFrequency", 1000, "Desired frequency of real-time control loop");
+ defineOptionalProperty<float>("RTLoopTimingCheckToleranceFactor", 1.0f, "Factor by which the timing checks are multiplied. Higher value -> less warning outputs");
+ defineOptionalProperty<bool>("checkErrorCountersOnEtherCATError", false, "If true, the EtherCAT bus will be checked for receive errors on bus error. This is slow; it should be active in normal usage.", PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>("VisualizeTorques", false, "If true, EtherCATRTUnit::publish will draw joint torques on the debug drawer");
}
};
@@ -123,9 +126,10 @@ namespace armarx
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void icePropertiesInitialized() override;
- protected:
+ void componentPropertiesUpdated(const std::set<std::string>& changedProperties) override;
- MultiNodeRapidXMLReader readHardwareConfigFile(std::string hardwareConfigFilePath);
+ protected:
+ static MultiNodeRapidXMLReader ReadHardwareConfigFile(std::string hardwareConfigFilePath, std::string rootNodeName);
//all the stuff to run the rt Thread
void startRTThread();
@@ -138,23 +142,24 @@ namespace armarx
bool initBusRTThread();
void controlLoopRTThread();
+
+ void computeInertiaTorque();
DebugDrawerInterfacePrx dd;
std::thread rtTask;
std::atomic_bool taskRunning {false};
- std::atomic_int rtLoopTime;
+ std::atomic_int rtLoopTime{1000};
+ float rtWarningFactor{1};
+
//timestamps for the pdo updates
IceUtil::Time currentPDOUpdateTimestamp;
-
- VirtualRobot::RobotPtr robot;
+ VirtualRobot::RobotPtr publishRobot;
DeviceContainerPtr deviceContainerPtr;
-
VirtualRobot::RobotNodeSetPtr rtRobotJointSet, rtRobotBodySet;
std::vector<std::pair<VirtualRobot::RobotNodePtr, SensorValue1DoFGravityTorque*>> nodeJointDataVec;
-
int latency_target_fd = -1;
void set_latency_target(int32_t latency_target_value = 0);
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index d5d62998ce906e8e70412f60e13afb01478f16c3..e6c6c6e3be9facfce693eaf70fe28707c49686a0 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -8,5 +8,9 @@ add_subdirectory(RobotAPIVariantWidget)
add_subdirectory(RobotAPINJointControllers)
add_subdirectory(DMPController)
+# disabled until Mathlib include is fixed...
+add_subdirectory(DSControllers)
add_subdirectory(RobotStatechartHelpers)
+
+add_subdirectory(DebugDrawerConfigWidget)
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
index 03da7f366f5f612ae660cedfd55489f66c6d0504..25c74c7958249e38595a64e6c67df2aa8de11f8b 100644
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
+++ b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
@@ -22,29 +22,33 @@
#include "TaskSpaceDMPController.h"
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+
using namespace armarx;
void TaskSpaceDMPController::flow(double deltaT, const Eigen::Matrix4f& currentPose, const Eigen::VectorXf& twist)
+{
+ canVal = flow(canVal, deltaT, currentPose, twist);
+}
+
+double TaskSpaceDMPController::flow(double canVal, double deltaT, const Eigen::Matrix4f& currentPose, const Eigen::VectorXf& twist)
{
if (paused)
{
targetVel.setZero();
return;
}
- if (canVal < 1e-8)
+ if (canVal < 0.1 && config.DMPStyle == "Periodic")
{
- if (config.DMPStyle == "Periodic")
- {
- prepareExecution(eigen4f2vec(currentPose), goalPoseVec);
- }
- else
- {
- targetVel.setZero();
- return;
- }
+ canVal = config.motionTimeDuration;
+ }
+ if (canVal < 1e-8 && config.DMPStyle == "Discrete")
+ {
+ targetVel.setZero();
+ return canVal;
}
Eigen::Vector3f currentPosition;
@@ -81,24 +85,20 @@ void TaskSpaceDMPController::flow(double deltaT, const Eigen::Matrix4f& currentP
isDisturbance = false;
}
- double tau = dmpPtr->getTemporalFactor();
double timeDuration = config.motionTimeDuration;
- canVal -= deltaT * 1 / (1 + phaseStop) ;
+ canVal -= tau * deltaT * 1;// / (1 + phaseStop) ;
+
+ DMP::Vec<DMP::DMPState > temporalState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / timeDuration, deltaT / timeDuration, targetPoseVec);
- for (size_t i = 0; i < 7; ++i)
+ // scale translation velocity
+ for (size_t i = 0; i < 3; ++i)
{
- currentState[i].vel = currentState[i].vel * config.DMPAmplitude;
+ currentState[i].vel = tau * temporalState[i].vel * config.DMPAmplitude / timeDuration;
+ currentState[i].pos += deltaT * currentState[i].vel;
}
- currentState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / (tau * timeDuration), deltaT / (tau * timeDuration), targetPoseVec);
-
- // for (size_t i = 0; i < 7; ++i)
- // {
- // targetPoseVec[i] = currentState[i].pos;
- // }
-
-
+ // define the translation velocity
if (isPhaseStopControl)
{
float vel0, vel1;
@@ -107,7 +107,7 @@ void TaskSpaceDMPController::flow(double deltaT, const Eigen::Matrix4f& currentP
linearVel << twist(0), twist(1), twist(2);
for (size_t i = 0; i < 3; i++)
{
- vel0 = currentState[i].vel / (timeDuration * tau);
+ vel0 = currentState[i].vel;
vel1 = config.phaseStopParas.Kpos * (targetPoseVec[i] - currentPosition(i)) - config.phaseStopParas.Dpos * linearVel(i);
targetVel(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
}
@@ -116,15 +116,18 @@ void TaskSpaceDMPController::flow(double deltaT, const Eigen::Matrix4f& currentP
{
for (size_t i = 0; i < 3; i++)
{
- targetVel(i) = currentState[i].vel / (timeDuration * tau);
+ targetVel(i) = currentState[i].vel;
}
}
+
+
+ // define the rotation velocity
Eigen::Quaterniond dmpQuaternionVel;
- dmpQuaternionVel.w() = currentState[3].vel;
- dmpQuaternionVel.x() = currentState[4].vel;
- dmpQuaternionVel.y() = currentState[5].vel;
- dmpQuaternionVel.z() = currentState[6].vel;
+ dmpQuaternionVel.w() = temporalState[3].vel;
+ dmpQuaternionVel.x() = temporalState[4].vel;
+ dmpQuaternionVel.y() = temporalState[5].vel;
+ dmpQuaternionVel.z() = temporalState[6].vel;
Eigen::Quaterniond dmpQuaternionPosi;
dmpQuaternionPosi.w() = currentState[3].pos;
@@ -154,33 +157,50 @@ void TaskSpaceDMPController::flow(double deltaT, const Eigen::Matrix4f& currentP
}
oldDMPAngularVelocity = angularVel0;
- Eigen::Vector3f currentAngularVel;
- currentAngularVel << twist(3), twist(4), twist(5);
+ // scale orientation velocity
+ angularVel0.w() = 0;
+ angularVel0.x() = tau * angularVel0.x() * config.oriAmplitude / timeDuration;
+ angularVel0.y() = tau * angularVel0.y() * config.oriAmplitude / timeDuration;
+ angularVel0.z() = tau * angularVel0.z() * config.oriAmplitude / timeDuration;
- Eigen::Quaternionf targetQuaternionf;
- targetQuaternionf.w() = targetPoseVec[3];
- targetQuaternionf.x() = targetPoseVec[4];
- targetQuaternionf.y() = targetPoseVec[5];
- targetQuaternionf.z() = targetPoseVec[6];
-
- Eigen::Matrix3f desiredMat(targetQuaternionf);
- Eigen::Matrix3f currentMat = currentPose.block<3, 3>(0, 0);
- Eigen::Matrix3f diffMat = desiredMat * currentMat.inverse();
- Eigen::Vector3f diffRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
- Eigen::Vector3f angularVel1 = config.phaseStopParas.Kori * diffRPY - config.phaseStopParas.Dori * currentAngularVel;
+ // Eigen::Quaterniond scaledQuat = (angularVel0 * dmpQuaternionPosi);
+ // currentState[3].vel = 0.5 * scaledQuat.w();
+ // currentState[4].vel = 0.5 * scaledQuat.x();
+ // currentState[6].vel = 0.5 * scaledQuat.z();
+ // currentState[5].vel = 0.5 * scaledQuat.y();
+ for (size_t i = 3; i < 7; ++i)
+ {
+ currentState[i].vel = tau * temporalState[i].vel * config.oriAmplitude / timeDuration;
+ currentState[i].pos += currentState[i].vel * deltaT;
+ }
if (isPhaseStopControl)
{
- targetVel(3) = angularVel0.x() / (timeDuration * tau);
- targetVel(4) = angularVel0.y() / (timeDuration * tau);
- targetVel(5) = angularVel0.z() / (timeDuration * tau);
+ Eigen::Vector3f currentAngularVel;
+ currentAngularVel << twist(3), twist(4), twist(5);
+
+ Eigen::Quaternionf targetQuaternionf;
+ targetQuaternionf.w() = targetPoseVec[3];
+ targetQuaternionf.x() = targetPoseVec[4];
+ targetQuaternionf.y() = targetPoseVec[5];
+ targetQuaternionf.z() = targetPoseVec[6];
+
+ Eigen::Matrix3f desiredMat(targetQuaternionf);
+ Eigen::Matrix3f currentMat = currentPose.block<3, 3>(0, 0);
+ Eigen::Matrix3f diffMat = desiredMat * currentMat.inverse();
+ Eigen::Vector3f diffRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
+ Eigen::Vector3f angularVel1 = config.phaseStopParas.Kori * diffRPY - config.phaseStopParas.Dori * currentAngularVel;
+
+ targetVel(3) = mpcFactor * angularVel0.x() / timeDuration + (1 - mpcFactor) * angularVel1(0);
+ targetVel(4) = mpcFactor * angularVel0.y() / timeDuration + (1 - mpcFactor) * angularVel1(1);
+ targetVel(5) = mpcFactor * angularVel0.z() / timeDuration + (1 - mpcFactor) * angularVel1(2);
}
else
{
- targetVel(3) = mpcFactor * angularVel0.x() / (timeDuration * tau) + (1 - mpcFactor) * angularVel1(0);
- targetVel(4) = mpcFactor * angularVel0.y() / (timeDuration * tau) + (1 - mpcFactor) * angularVel1(1);
- targetVel(5) = mpcFactor * angularVel0.z() / (timeDuration * tau) + (1 - mpcFactor) * angularVel1(2);
+ targetVel(3) = angularVel0.x() ;
+ targetVel(4) = angularVel0.y();
+ targetVel(5) = angularVel0.z();
}
debugData.canVal = canVal;
@@ -188,6 +208,8 @@ void TaskSpaceDMPController::flow(double deltaT, const Eigen::Matrix4f& currentP
debugData.posiError = posiError;
debugData.mpcFactor = mpcFactor;
debugData.poseError = poseError;
+
+ return canVal;
}
void TaskSpaceDMPController::learnDMPFromFiles(const std::vector<std::string>& fileNames, const std::vector<double>& ratios)
@@ -205,7 +227,9 @@ void TaskSpaceDMPController::learnDMPFromFiles(const std::vector<std::string>& f
for (size_t i = 0; i < fileNames.size(); ++i)
{
DMP::SampledTrajectoryV2 traj;
- traj.readFromCSVFile(fileNames.at(i));
+ std::string absPath;
+ ArmarXDataPath::getAbsolutePath(fileNames.at(i), absPath);
+ traj.readFromCSVFile(absPath);
traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
trajs.push_back(traj);
@@ -248,6 +272,28 @@ void TaskSpaceDMPController::learnDMPFromFiles(const std::vector<std::string>& f
learnDMPFromFiles(fileNames, ratios);
}
+void TaskSpaceDMPController::learnDMPFromTrajectory(const TrajectoryPtr& traj)
+{
+ ARMARX_CHECK_EQUAL(traj->dim(), 7);
+ DMP::SampledTrajectoryV2 dmpTraj;
+
+ DMP::DVec timestamps(traj->getTimestamps());
+ for (size_t i = 0; i < traj->dim(); ++i)
+ {
+ DMP::DVec dimData(traj->getDimensionData(i, 0));
+ dmpTraj.addDimension(timestamps, dimData);
+ }
+
+ DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
+
+ dmpTraj = DMP::SampledTrajectoryV2::normalizeTimestamps(dmpTraj, 0, 1);
+ trajs.push_back(dmpTraj);
+ DMP::DVec ratiosVec;
+ ratiosVec.push_back(1.0);
+ dmpPtr->learnFromTrajectories(trajs);
+ dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratiosVec);
+}
+
void TaskSpaceDMPController::setViaPose(double canVal, const Eigen::Matrix4f& viaPose)
{
@@ -281,6 +327,7 @@ void TaskSpaceDMPController::prepareExecution(const std::vector<double>& current
ARMARX_CHECK_EQUAL(currentPoseVec.size(), 7);
ARMARX_CHECK_EQUAL(goalPoseVec.size(), 7);
+ ARMARX_IMPORTANT << "prepareExecution: currentPoseVec: " << currentPoseVec;
for (size_t i = 0; i < currentPoseVec.size(); ++i)
{
currentState[i].pos = currentPoseVec.at(i);
@@ -298,12 +345,21 @@ void TaskSpaceDMPController::prepareExecution(const std::vector<double>& current
void TaskSpaceDMPController::setSpeed(double times)
{
- if (times == 0)
+ if (times <= 0)
{
- return;
+ ARMARX_WARNING << "TaskSpaceDMPController: cannot set non-positive speed times";
}
- config.motionTimeDuration /= times;
+ tau = times;
+}
+
+void TaskSpaceDMPController::setAmplitude(double amp)
+{
+ if (amp <= 0)
+ {
+ ARMARX_WARNING << "TaskSpaceDMPController: cannot set non-positive amplitude";
+ }
+ config.DMPAmplitude = amp;
}
std::vector<double> TaskSpaceDMPController::eigen4f2vec(const Eigen::Matrix4f& pose)
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
index eb8d5c541cb09936b664701d75ae09b4b23d339e..81e87e88794a0ad1cdfdafed8fdd5467a89a107a 100644
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
+++ b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
@@ -30,7 +30,7 @@
#include <VirtualRobot/RobotNodeSet.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
+#include <RobotAPI/libraries/core/Trajectory.h>
namespace armarx
{
@@ -54,6 +54,7 @@ namespace armarx
std::string DMPMode = "Linear";
std::string DMPStyle = "Discrete";
float DMPAmplitude = 1;
+ float oriAmplitude = 1;
float motionTimeDuration = 10;
PhaseStopParams phaseStopParas;
};
@@ -107,6 +108,7 @@ namespace armarx
this->isPhaseStopControl = isPhaseStopControl;
dmpName = name;
this->paused = false;
+ tau = 1;
}
std::string getName()
@@ -116,6 +118,7 @@ namespace armarx
void flow(double deltaT, const Eigen::Matrix4f& currentPose, const Eigen::VectorXf& twist);
+ double flow(double canVal, double deltaT, const Eigen::Matrix4f& currentPose, const Eigen::VectorXf& twist);
Eigen::VectorXf getTargetVelocity()
{
@@ -137,9 +140,24 @@ namespace armarx
return res;
}
+ Eigen::Matrix4f getIntegratedPoseMat()
+ {
+ Eigen::Matrix4f res = VirtualRobot::MathTools::quat2eigen4f(currentState.at(4).pos,
+ currentState.at(5).pos,
+ currentState.at(6).pos,
+ currentState.at(3).pos);
+ res(0, 3) = currentState.at(0).pos;
+ res(1, 3) = currentState.at(1).pos;
+ res(2, 3) = currentState.at(2).pos;
+
+ return res;
+ }
+
void learnDMPFromFiles(const std::vector<std::string>& fileNames, const std::vector<double>& ratios);
void learnDMPFromFiles(const std::vector<std::string>& fileNames);
+ void learnDMPFromTrajectory(const TrajectoryPtr& traj);
+
void setViaPose(double canVal, const Eigen::Matrix4f& viaPose);
void setViaPose(double canVal, const std::vector<double>& viaPoseWithQuaternion);
@@ -149,6 +167,7 @@ namespace armarx
void prepareExecution(const std::vector<double>& currentPoseVec, const std::vector<double>& goalPoseVec, double tau = 1);
void setSpeed(double times);
+ void setAmplitude(double amp);
void setGoalPose(const Eigen::Matrix4f& goalPose)
{
@@ -185,7 +204,7 @@ namespace armarx
private:
-
+ double tau;
DMP::DVec goalPoseVec;
Eigen::VectorXf targetVel;
diff --git a/source/RobotAPI/libraries/DSControllers/CMakeLists.txt b/source/RobotAPI/libraries/DSControllers/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8708dc17072abcadda62797f3167d7b0fa82de4f
--- /dev/null
+++ b/source/RobotAPI/libraries/DSControllers/CMakeLists.txt
@@ -0,0 +1,55 @@
+set(LIB_NAME DSControllers)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+find_package(Eigen3 QUIET)
+find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
+find_package(MATHLIB QUIET)
+
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+armarx_build_if(MATHLIB_FOUND "MATHLIB not available")
+
+if (Eigen3_FOUND AND Simox_FOUND AND MATHLIB_FOUND)
+ include_directories(${Simox_INCLUDE_DIRS})
+ include_directories(SYSTEM ${Eigen3_INCLUDE_DIR})
+ include_directories(${MATHLIB_INCLUDE_DIRS})
+endif()
+
+
+
+
+message(STATUS "mathlib is ${MATHLIB_LIBS}")
+set(LIBS ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreEigen3Variants
+ VirtualRobot
+ Saba
+ SimDynamics
+ RobotUnit
+ RobotAPIUnits
+ RobotAPICore
+ RobotAPIInterfaces
+ ${MATHLIB_LIB}
+)
+
+
+
+set(LIB_FILES
+./DSRTBimanualController.cpp
+./DSRTController.cpp
+./GMRDynamics.cpp
+./Gaussians.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+set(LIB_HEADERS
+./DSRTBimanualController.h
+./DSRTController.h
+./GMRDynamics.h
+./Gaussians.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+)
+
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+# add unit tests
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..249b951e8fc383b76d616b78f6f4255b62b24655
--- /dev/null
+++ b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp
@@ -0,0 +1,1009 @@
+/*
+ * 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 DSController::ArmarXObjects::DSRTBimanualController
+ * @author Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "DSRTBimanualController.h"
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+using namespace armarx;
+
+NJointControllerRegistration<DSRTBimanualController> registrationControllerDSRTBimanualController("DSRTBimanualController");
+
+void DSRTBimanualController::onInitNJointController()
+{
+ ARMARX_INFO << "init ...";
+ controllerStopRequested = false;
+ controllerRunFinished = false;
+ runTask("DSRTBimanualControllerTask", [&]
+ {
+ CycleUtil c(1);
+ getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
+ while (getState() == eManagedIceObjectStarted && !controllerStopRequested)
+ {
+ ARMARX_INFO << deactivateSpam(1) << "control fct";
+ if (isControllerActive())
+ {
+ controllerRun();
+ }
+ c.waitForCycleDuration();
+ }
+ controllerRunFinished = true;
+ ARMARX_INFO << "Control Fct finished";
+ });
+
+
+}
+
+void DSRTBimanualController::onDisconnectNJointController()
+{
+ ARMARX_INFO << "disconnect";
+ controllerStopRequested = true;
+ while (!controllerRunFinished)
+ {
+ ARMARX_INFO << deactivateSpam(1) << "waiting for run function";
+ usleep(10000);
+ }
+}
+
+
+DSRTBimanualController::DSRTBimanualController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+{
+ cfg = DSRTBimanualControllerConfigPtr::dynamicCast(config);
+ useSynchronizedRtRobot();
+
+ VirtualRobot::RobotNodeSetPtr left_ns = rtGetRobot()->getRobotNodeSet("LeftArm");
+ VirtualRobot::RobotNodeSetPtr right_ns = rtGetRobot()->getRobotNodeSet("RightArm");
+
+ left_jointNames.clear();
+ right_jointNames.clear();
+
+ ARMARX_CHECK_EXPRESSION_W_HINT(left_ns, "LeftArm");
+ ARMARX_CHECK_EXPRESSION_W_HINT(right_ns, "RightArm");
+
+ // for left arm
+ for (size_t i = 0; i < left_ns->getSize(); ++i)
+ {
+ std::string jointName = left_ns->getNode(i)->getName();
+ left_jointNames.push_back(jointName);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ ARMARX_CHECK_EXPRESSION(ct);
+ const SensorValueBase* sv = useSensorValue(jointName);
+ ARMARX_CHECK_EXPRESSION(sv);
+ auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
+ ARMARX_CHECK_EXPRESSION(casted_ct);
+ left_torque_targets.push_back(casted_ct);
+
+ const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+ if (!torqueSensor)
+ {
+ ARMARX_WARNING << "No Torque sensor available for " << jointName;
+ }
+ if (!gravityTorqueSensor)
+ {
+ ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
+ }
+
+ left_torqueSensors.push_back(torqueSensor);
+ left_gravityTorqueSensors.push_back(gravityTorqueSensor);
+ left_velocitySensors.push_back(velocitySensor);
+ left_positionSensors.push_back(positionSensor);
+ };
+
+ // for right arm
+ for (size_t i = 0; i < right_ns->getSize(); ++i)
+ {
+ std::string jointName = right_ns->getNode(i)->getName();
+ right_jointNames.push_back(jointName);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ ARMARX_CHECK_EXPRESSION(ct);
+ const SensorValueBase* sv = useSensorValue(jointName);
+ ARMARX_CHECK_EXPRESSION(sv);
+ auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
+ ARMARX_CHECK_EXPRESSION(casted_ct);
+ right_torque_targets.push_back(casted_ct);
+
+ const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+ if (!torqueSensor)
+ {
+ ARMARX_WARNING << "No Torque sensor available for " << jointName;
+ }
+ if (!gravityTorqueSensor)
+ {
+ ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
+ }
+
+ right_torqueSensors.push_back(torqueSensor);
+ right_gravityTorqueSensors.push_back(gravityTorqueSensor);
+ right_velocitySensors.push_back(velocitySensor);
+ right_positionSensors.push_back(positionSensor);
+ };
+
+
+ const SensorValueBase* svlf = useSensorValue("FT L");
+ leftForceTorque = svlf->asA<SensorValueForceTorque>();
+ const SensorValueBase* svrf = useSensorValue("FT R");
+ rightForceTorque = svrf->asA<SensorValueForceTorque>();
+
+ ARMARX_INFO << "Initialized " << left_torque_targets.size() << " targets for the left arm";
+ ARMARX_INFO << "Initialized " << right_torque_targets.size() << " targets for the right arm";
+
+ left_arm_tcp = left_ns->getTCP();
+ right_arm_tcp = right_ns->getTCP();
+
+ left_sensor_frame = left_ns->getRobot()->getRobotNode("ArmL8_Wri2");
+ right_sensor_frame = right_ns->getRobot()->getRobotNode("ArmR8_Wri2");
+
+ left_ik.reset(new VirtualRobot::DifferentialIK(left_ns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ right_ik.reset(new VirtualRobot::DifferentialIK(right_ns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+
+
+ // ?? not sure about this
+ DSRTBimanualControllerSensorData initSensorData;
+
+ initSensorData.left_tcpPose = left_arm_tcp->getPoseInRootFrame();
+ initSensorData.currentTime = 0;
+ controllerSensorData.reinitAllBuffers(initSensorData);
+
+ initSensorData.right_tcpPose = right_arm_tcp->getPoseInRootFrame();
+ initSensorData.currentTime = 0;
+ controllerSensorData.reinitAllBuffers(initSensorData);
+
+
+ left_oldPosition = left_arm_tcp->getPositionInRootFrame();
+ left_oldOrientation = left_arm_tcp->getPoseInRootFrame().block<3, 3>(0, 0);
+
+ right_oldPosition = right_arm_tcp->getPositionInRootFrame();
+ right_oldOrientation = right_arm_tcp->getPoseInRootFrame().block<3, 3>(0, 0);
+
+
+ std::vector<float> left_desiredQuaternionVec = cfg->left_desiredQuaternion;
+ ARMARX_CHECK_EQUAL(left_desiredQuaternionVec.size(), 4);
+
+ std::vector<float> right_desiredQuaternionVec = cfg->right_desiredQuaternion;
+ ARMARX_CHECK_EQUAL(right_desiredQuaternionVec.size(), 4);
+
+ left_desiredQuaternion.w() = left_desiredQuaternionVec.at(0);
+ left_desiredQuaternion.x() = left_desiredQuaternionVec.at(1);
+ left_desiredQuaternion.y() = left_desiredQuaternionVec.at(2);
+ left_desiredQuaternion.z() = left_desiredQuaternionVec.at(3);
+
+ right_desiredQuaternion.w() = right_desiredQuaternionVec.at(0);
+ right_desiredQuaternion.x() = right_desiredQuaternionVec.at(1);
+ right_desiredQuaternion.y() = right_desiredQuaternionVec.at(2);
+ right_desiredQuaternion.z() = right_desiredQuaternionVec.at(3);
+
+
+ // set initial joint velocities filter
+
+ left_jointVelocity_filtered.resize(left_torque_targets.size());
+ left_jointVelocity_filtered.setZero();
+ right_jointVelocity_filtered.resize(left_torque_targets.size());
+ right_jointVelocity_filtered.setZero();
+
+ // do we need to duplicate this?
+ DSRTBimanualControllerControlData initData;
+ for (size_t i = 0; i < 3; ++i)
+ {
+ initData.left_tcpDesiredLinearVelocity(i) = 0;
+ initData.right_tcpDesiredLinearVelocity(i) = 0;
+
+ }
+
+ for (size_t i = 0; i < 3; ++i)
+ {
+ initData.left_tcpDesiredAngularError(i) = 0;
+ initData.right_tcpDesiredAngularError(i) = 0;
+
+ }
+ reinitTripleBuffer(initData);
+
+
+ // initial filter
+ left_desiredTorques_filtered.resize(left_torque_targets.size());
+ left_desiredTorques_filtered.setZero();
+ right_desiredTorques_filtered.resize(right_torque_targets.size());
+ right_desiredTorques_filtered.setZero();
+
+
+ left_currentTCPLinearVelocity_filtered.setZero();
+ right_currentTCPLinearVelocity_filtered.setZero();
+
+ left_currentTCPAngularVelocity_filtered.setZero();
+ right_currentTCPAngularVelocity_filtered.setZero();
+
+ left_tcpDesiredTorque_filtered.setZero();
+ right_tcpDesiredTorque_filtered.setZero();
+
+
+ smooth_startup = 0;
+
+
+ filterTimeConstant = cfg->filterTimeConstant;
+ posiKp = cfg->posiKp;
+ v_max = cfg->v_max;
+ Damping = cfg->posiDamping;
+ Coupling_stiffness = cfg->couplingStiffness;
+ Coupling_force_limit = cfg->couplingForceLimit;
+ forward_gain = cfg->forwardGain;
+ torqueLimit = cfg->torqueLimit;
+ null_torqueLimit = cfg->NullTorqueLimit;
+ oriKp = cfg->oriKp;
+ oriDamping = cfg->oriDamping;
+ contactForce = cfg->contactForce;
+
+ left_oriUp.w() = cfg->left_oriUp[0];
+ left_oriUp.x() = cfg->left_oriUp[1];
+ left_oriUp.y() = cfg->left_oriUp[2];
+ left_oriUp.z() = cfg->left_oriUp[3];
+
+ left_oriDown.w() = cfg->left_oriDown[0];
+ left_oriDown.x() = cfg->left_oriDown[1];
+ left_oriDown.y() = cfg->left_oriDown[2];
+ left_oriDown.z() = cfg->left_oriDown[3];
+
+ right_oriUp.w() = cfg->right_oriUp[0];
+ right_oriUp.x() = cfg->right_oriUp[1];
+ right_oriUp.y() = cfg->right_oriUp[2];
+ right_oriUp.z() = cfg->right_oriUp[3];
+
+ right_oriDown.w() = cfg->right_oriDown[0];
+ right_oriDown.x() = cfg->right_oriDown[1];
+ right_oriDown.y() = cfg->right_oriDown[2];
+ right_oriDown.z() = cfg->right_oriDown[3];
+
+
+ guardTargetZUp = cfg->guardTargetZUp;
+ guardTargetZDown = cfg->guardTargetZDown;
+ guardDesiredZ = guardTargetZUp;
+ guard_mounting_correction_z = 0;
+
+ guardXYHighFrequency = 0;
+ left_force_old.setZero();
+ right_force_old.setZero();
+
+ left_contactForceZ_correction = 0;
+ right_contactForceZ_correction = 0;
+
+
+ std::vector<float> leftarm_qnullspaceVec = cfg->leftarm_qnullspaceVec;
+ std::vector<float> rightarm_qnullspaceVec = cfg->rightarm_qnullspaceVec;
+
+ left_qnullspace.resize(leftarm_qnullspaceVec.size());
+ right_qnullspace.resize(rightarm_qnullspaceVec.size());
+
+ for (size_t i = 0; i < leftarm_qnullspaceVec.size(); ++i)
+ {
+ left_qnullspace(i) = leftarm_qnullspaceVec[i];
+ right_qnullspace(i) = rightarm_qnullspaceVec[i];
+ }
+
+ nullspaceKp = cfg->nullspaceKp;
+ nullspaceDamping = cfg->nullspaceDamping;
+
+
+ //set GMM parameters ...
+ if (cfg->gmmParamsFile == "")
+ {
+ ARMARX_ERROR << "gmm params file cannot be empty string ...";
+ }
+ gmmMotionGenerator.reset(new BimanualGMMMotionGen(cfg->gmmParamsFile));
+ positionErrorTolerance = cfg->positionErrorTolerance;
+ forceFilterCoeff = cfg->forceFilterCoeff;
+ for (size_t i = 0 ; i < 3; ++i)
+ {
+ leftForceOffset[i] = cfg->forceLeftOffset.at(i);
+ rightForceOffset[i] = cfg->forceRightOffset.at(i);
+ }
+ isDefaultTarget = false;
+ ARMARX_INFO << "Initialization done";
+}
+
+void DSRTBimanualController::controllerRun()
+{
+ if (!controllerSensorData.updateReadBuffer())
+ {
+ return;
+ }
+
+
+ // receive the measurements
+ Eigen::Matrix4f left_currentTCPPose = controllerSensorData.getReadBuffer().left_tcpPose;
+ Eigen::Matrix4f right_currentTCPPose = controllerSensorData.getReadBuffer().right_tcpPose;
+
+ Eigen::Vector3f left_force = controllerSensorData.getReadBuffer().left_force;
+ Eigen::Vector3f right_force = controllerSensorData.getReadBuffer().right_force;
+ Eigen::Vector3f both_arm_force_ave = - 0.5 * (left_force + right_force); // negative sign for the direction
+
+
+ float left_force_z = left_force(2);
+ float right_force_z = right_force(2);
+
+ Eigen::Vector3f left_currentTCPPositionInMeter;
+ Eigen::Vector3f right_currentTCPPositionInMeter;
+
+ left_currentTCPPositionInMeter << left_currentTCPPose(0, 3), left_currentTCPPose(1, 3), left_currentTCPPose(2, 3);
+ right_currentTCPPositionInMeter << right_currentTCPPose(0, 3), right_currentTCPPose(1, 3), right_currentTCPPose(2, 3);
+
+ left_currentTCPPositionInMeter = 0.001 * left_currentTCPPositionInMeter;
+ right_currentTCPPositionInMeter = 0.001 * right_currentTCPPositionInMeter;
+
+
+ // updating the desired height for the guard based on the interaction forces
+ float both_arm_height_z_ave = 0.5 * (left_currentTCPPositionInMeter(2) + right_currentTCPPositionInMeter(2));
+
+
+ float adaptive_ratio = 1;
+ float force_error_z = 0;
+ float guard_mounting_correction_x = 0;
+ float guard_mounting_correction_y = 0;
+
+
+ if (cfg->guardDesiredDirection)
+ {
+ adaptive_ratio = 1;
+ force_error_z = both_arm_force_ave(2) - adaptive_ratio * cfg->liftingForce;
+
+ // meausures the high-frequency components of the interaction forces
+ float diff_norm = (left_force - left_force_old).squaredNorm() + (right_force - right_force_old).squaredNorm();
+ // diff_norm = deadzone(diff_norm,0.1,2);
+ guardXYHighFrequency = cfg->highPassFilterFactor * guardXYHighFrequency + 0.1 * diff_norm;
+
+ guardXYHighFrequency = (guardXYHighFrequency > 10) ? 10 : guardXYHighFrequency;
+
+ left_force_old = left_force;
+ right_force_old = right_force;
+
+ if (fabs(both_arm_height_z_ave - guardTargetZUp) < cfg->mountingDistanceTolerance)
+ {
+ guard_mounting_correction_z = (1 - cfg->mountingCorrectionFilterFactor) * guard_mounting_correction_z + cfg->mountingCorrectionFilterFactor * (- 0.1 * (guardXYHighFrequency - 8));
+ guard_mounting_correction_z = deadzone(guard_mounting_correction_z, 0, 0.1);
+
+
+ guard_mounting_correction_x = guard_mounting_correction_x - cfg->forceErrorZGain * deadzone(both_arm_force_ave(0), 1, 3);
+ guard_mounting_correction_y = guard_mounting_correction_y - cfg->forceErrorZGain * deadzone(both_arm_force_ave(1), 1, 3);
+
+ guard_mounting_correction_x = deadzone(guard_mounting_correction_x, 0, 0.1);
+ guard_mounting_correction_y = deadzone(guard_mounting_correction_y, 0, 0.1);
+
+
+ }
+
+ }
+ else
+ {
+ adaptive_ratio = (0.5 * (both_arm_height_z_ave - guardTargetZDown) / (guardTargetZUp - guardTargetZDown) + 0.5);
+ force_error_z = both_arm_force_ave(2) - adaptive_ratio * cfg->loweringForce;
+
+ }
+
+
+
+ force_error_z = deadzone(force_error_z, cfg->forceErrorZDisturbance, cfg->forceErrorZThreshold);
+ guardDesiredZ = guardDesiredZ - cfg->forceErrorZGain * (force_error_z);
+
+ guardDesiredZ = (guardDesiredZ > guardTargetZUp) ? guardTargetZUp : guardDesiredZ;
+ guardDesiredZ = (guardDesiredZ < guardTargetZDown) ? guardTargetZDown : guardDesiredZ;
+
+ if (isDefaultTarget)
+ {
+ guardDesiredZ = guardTargetZDown;
+ }
+
+ if (!cfg->guardDesiredDirection && guardDesiredZ < 1.5)
+ {
+ guard_mounting_correction_y = -0.1;
+ }
+
+ // update the desired velocity
+ gmmMotionGenerator->updateDesiredVelocity(
+ left_currentTCPPositionInMeter,
+ right_currentTCPPositionInMeter,
+ positionErrorTolerance,
+ guardDesiredZ,
+ guard_mounting_correction_x,
+ guard_mounting_correction_y,
+ guard_mounting_correction_z);
+
+ // ARMARX_INFO << "tcpDesiredLinearVelocity: " << gmmMotionGenerator->left_DS_DesiredVelocity;
+ // ARMARX_INFO << "tcpDesiredLinearVelocity: " << gmmMotionGenerator->right_DS_DesiredVelocity;
+
+
+ Eigen::Vector3f left_tcpDesiredAngularError;
+ Eigen::Vector3f right_tcpDesiredAngularError;
+
+ left_tcpDesiredAngularError << 0, 0, 0;
+ right_tcpDesiredAngularError << 0, 0, 0;
+
+
+
+ Eigen::Matrix3f left_currentRotMat = left_currentTCPPose.block<3, 3>(0, 0);
+ Eigen::Matrix3f right_currentRotMat = right_currentTCPPose.block<3, 3>(0, 0);
+
+
+ float lqratio = (left_currentTCPPositionInMeter(2) - guardTargetZDown) / (guardTargetZUp - guardTargetZDown);
+ float rqratio = (right_currentTCPPositionInMeter(2) - guardTargetZDown) / (guardTargetZUp - guardTargetZDown);
+
+ lqratio = (lqratio > 1) ? 1 : lqratio;
+ lqratio = (lqratio < 0) ? 0 : lqratio;
+ rqratio = (rqratio > 1) ? 1 : rqratio;
+ rqratio = (rqratio < 0) ? 0 : rqratio;
+
+
+ Eigen::Quaternionf left_desiredQuaternion = quatSlerp(lqratio, left_oriDown, left_oriUp);
+ Eigen::Quaternionf right_desiredQuaternion = quatSlerp(rqratio, right_oriDown, right_oriUp);
+
+
+ Eigen::Matrix3f left_desiredRotMat = left_desiredQuaternion.normalized().toRotationMatrix();
+ Eigen::Matrix3f right_desiredRotMat = right_desiredQuaternion.normalized().toRotationMatrix();
+
+ Eigen::Matrix3f left_orientationError = left_currentRotMat * left_desiredRotMat.inverse();
+ Eigen::Matrix3f right_orientationError = right_currentRotMat * right_desiredRotMat.inverse();
+
+ Eigen::Quaternionf left_diffQuaternion(left_orientationError);
+ Eigen::Quaternionf right_diffQuaternion(right_orientationError);
+
+ Eigen::AngleAxisf left_angleAxis(left_diffQuaternion);
+ Eigen::AngleAxisf right_angleAxis(right_diffQuaternion);
+
+ left_tcpDesiredAngularError = left_angleAxis.angle() * left_angleAxis.axis();
+ right_tcpDesiredAngularError = right_angleAxis.angle() * right_angleAxis.axis();
+
+
+ // writing to the buffer
+ getWriterControlStruct().left_tcpDesiredLinearVelocity = gmmMotionGenerator->left_DS_DesiredVelocity;
+ getWriterControlStruct().right_tcpDesiredLinearVelocity = gmmMotionGenerator->right_DS_DesiredVelocity;
+ getWriterControlStruct().left_right_distanceInMeter = gmmMotionGenerator->left_right_position_errorInMeter;
+
+ getWriterControlStruct().left_tcpDesiredAngularError = left_tcpDesiredAngularError;
+ getWriterControlStruct().right_tcpDesiredAngularError = right_tcpDesiredAngularError;
+
+ writeControlStruct();
+ debugCtrlDataInfo.getWriteBuffer().desredZ = guardDesiredZ;
+ debugCtrlDataInfo.getWriteBuffer().force_error_z = force_error_z;
+ debugCtrlDataInfo.getWriteBuffer().guardXYHighFrequency = guardXYHighFrequency;
+ debugCtrlDataInfo.getWriteBuffer().guard_mounting_correction_x = guard_mounting_correction_x;
+ debugCtrlDataInfo.getWriteBuffer().guard_mounting_correction_y = guard_mounting_correction_y;
+ debugCtrlDataInfo.getWriteBuffer().guard_mounting_correction_z = guard_mounting_correction_z;
+ debugCtrlDataInfo.commitWrite();
+
+}
+
+
+void DSRTBimanualController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+{
+ // reading the control objectives from the other threads (desired velocities)
+ Eigen::Vector3f left_tcpDesiredLinearVelocity = rtGetControlStruct().left_tcpDesiredLinearVelocity;
+ Eigen::Vector3f right_tcpDesiredLinearVelocity = rtGetControlStruct().right_tcpDesiredLinearVelocity;
+ Eigen::Vector3f left_tcpDesiredAngularError = rtGetControlStruct().left_tcpDesiredAngularError;
+ Eigen::Vector3f right_tcpDesiredAngularError = rtGetControlStruct().right_tcpDesiredAngularError;
+ Eigen::Vector3f left_right_distanceInMeter = rtGetControlStruct().left_right_distanceInMeter;
+
+ double deltaT = timeSinceLastIteration.toSecondsDouble();
+
+
+ // measure the sesor data for the other threads
+
+ Eigen::Matrix4f leftSensorFrame = left_sensor_frame->getPoseInRootFrame();
+ Eigen::Matrix4f rightSensorFrame = right_sensor_frame->getPoseInRootFrame();
+
+ // ARMARX_IMPORTANT << "left force offset: " << leftForceOffset;
+ // ARMARX_IMPORTANT << "right force offset: " << rightForceOffset;
+
+ Eigen::Vector3f left_forceInRoot = leftSensorFrame.block<3, 3>(0, 0) * (leftForceTorque->force - leftForceOffset);
+ Eigen::Vector3f right_forceInRoot = rightSensorFrame.block<3, 3>(0, 0) * (rightForceTorque->force - rightForceOffset);
+ // Eigen::Vector3f left_torqueInRoot = leftSensorFrame.block<3, 3>(0, 0) * leftForceTorque->torque;
+ // Eigen::Vector3f right_torqueInRoot = rightSensorFrame.block<3, 3>(0, 0) * rightForceTorque->torque;
+ left_forceInRoot(2) = left_forceInRoot(2) + cfg->leftForceOffsetZ; // - 4 + 8.5;
+ right_forceInRoot(2) = right_forceInRoot(2) + cfg->rightForceOffsetZ; // + 30 - 5.2;
+
+
+ Eigen::Matrix4f left_currentTCPPose = left_arm_tcp->getPoseInRootFrame();
+ Eigen::Matrix4f right_currentTCPPose = right_arm_tcp->getPoseInRootFrame();
+
+
+ Eigen::MatrixXf left_jacobi = left_ik->getJacobianMatrix(left_arm_tcp, VirtualRobot::IKSolver::CartesianSelection::All);
+ Eigen::MatrixXf right_jacobi = right_ik->getJacobianMatrix(right_arm_tcp, VirtualRobot::IKSolver::CartesianSelection::All);
+
+ Eigen::VectorXf left_qpos;
+ Eigen::VectorXf left_qvel;
+
+ Eigen::VectorXf right_qpos;
+ Eigen::VectorXf right_qvel;
+
+ left_qpos.resize(left_positionSensors.size());
+ left_qvel.resize(left_velocitySensors.size());
+
+ right_qpos.resize(right_positionSensors.size());
+ right_qvel.resize(right_velocitySensors.size());
+
+ int jointDim = left_positionSensors.size();
+
+ for (size_t i = 0; i < left_velocitySensors.size(); ++i)
+ {
+ left_qpos(i) = left_positionSensors[i]->position;
+ left_qvel(i) = left_velocitySensors[i]->velocity;
+
+ right_qpos(i) = right_positionSensors[i]->position;
+ right_qvel(i) = right_velocitySensors[i]->velocity;
+ }
+
+ Eigen::VectorXf left_tcptwist = left_jacobi * left_qvel;
+ Eigen::VectorXf right_tcptwist = right_jacobi * right_qvel;
+
+ Eigen::Vector3f left_currentTCPLinearVelocity;
+ Eigen::Vector3f right_currentTCPLinearVelocity;
+ Eigen::Vector3f left_currentTCPAngularVelocity;
+ Eigen::Vector3f right_currentTCPAngularVelocity;
+ left_currentTCPLinearVelocity << 0.001 * left_tcptwist(0), 0.001 * left_tcptwist(1), 0.001 * left_tcptwist(2);
+ right_currentTCPLinearVelocity << 0.001 * right_tcptwist(0), 0.001 * right_tcptwist(1), 0.001 * right_tcptwist(2);
+ left_currentTCPAngularVelocity << left_tcptwist(3), left_tcptwist(4), left_tcptwist(5);
+ right_currentTCPAngularVelocity << right_tcptwist(3), right_tcptwist(4), right_tcptwist(5);
+ double filterFactor = deltaT / (filterTimeConstant + deltaT);
+ left_currentTCPLinearVelocity_filtered = (1 - filterFactor) * left_currentTCPLinearVelocity_filtered + filterFactor * left_currentTCPLinearVelocity;
+ right_currentTCPLinearVelocity_filtered = (1 - filterFactor) * right_currentTCPLinearVelocity_filtered + filterFactor * right_currentTCPLinearVelocity;
+ left_currentTCPAngularVelocity_filtered = (1 - filterFactor) * left_currentTCPAngularVelocity_filtered + left_currentTCPAngularVelocity;
+ right_currentTCPAngularVelocity_filtered = (1 - filterFactor) * right_currentTCPAngularVelocity_filtered + right_currentTCPAngularVelocity;
+ left_jointVelocity_filtered = (1 - filterFactor) * left_jointVelocity_filtered + filterFactor * left_qvel;
+ right_jointVelocity_filtered = (1 - filterFactor) * right_jointVelocity_filtered + filterFactor * right_qvel;
+
+ // writing into the bufffer for other threads
+ controllerSensorData.getWriteBuffer().left_tcpPose = left_currentTCPPose;
+ controllerSensorData.getWriteBuffer().right_tcpPose = right_currentTCPPose;
+ controllerSensorData.getWriteBuffer().left_force = left_forceInRoot;
+ controllerSensorData.getWriteBuffer().right_force = right_forceInRoot;
+ controllerSensorData.getWriteBuffer().currentTime += deltaT;
+ controllerSensorData.commitWrite();
+
+
+
+
+ // inverse dynamics:
+
+
+ // reading the tcp orientation
+
+
+
+
+ // computing the task-specific forces
+ Eigen::Vector3f left_DS_force = -(1.2 * left_currentTCPLinearVelocity_filtered - forward_gain * left_tcpDesiredLinearVelocity);
+ Eigen::Vector3f right_DS_force = -(1.2 * right_currentTCPLinearVelocity_filtered - forward_gain * right_tcpDesiredLinearVelocity);
+ for (int i = 0; i < 3; ++i)
+ {
+ left_DS_force(i) = left_DS_force(i) * Damping[i];
+ right_DS_force(i) = right_DS_force(i) * Damping[i];
+
+ left_DS_force(i) = deadzone(left_DS_force(i), 0.1, 100);
+ right_DS_force(i) = deadzone(right_DS_force(i), 0.1, 100);
+
+ }
+
+ // computing coupling forces
+ Eigen::Vector3f coupling_force = - Coupling_stiffness * left_right_distanceInMeter;
+ float coupling_force_norm = coupling_force.norm();
+
+ if (coupling_force_norm > Coupling_force_limit)
+ {
+ coupling_force = (Coupling_force_limit / coupling_force_norm) * coupling_force;
+ }
+
+ coupling_force(0) = deadzone(coupling_force(0), 0.1, 100);
+ coupling_force(1) = deadzone(coupling_force(1), 0.1, 100);
+ coupling_force(2) = deadzone(coupling_force(2), 0.1, 100);
+
+
+
+
+ double v_both = left_currentTCPLinearVelocity_filtered.norm() + right_currentTCPLinearVelocity_filtered.norm();
+ float force_contact_switch = 0;
+ float left_height = left_currentTCPPose(2, 3) * 0.001;
+ float right_height = right_currentTCPPose(2, 3) * 0.001;
+
+ float disTolerance = cfg->contactDistanceTolerance;
+ bool isUp = fabs(left_height - guardTargetZUp) < disTolerance && fabs(right_height - guardTargetZUp) < disTolerance;
+ if (v_both < disTolerance && isUp)
+ {
+ force_contact_switch = 1;
+ }
+ else if (v_both < 0.05 && isUp)
+ {
+ force_contact_switch = (0.05 - v_both) / (0.05 - disTolerance);
+ }
+ else if (v_both >= 0.05)
+ {
+ force_contact_switch = 0;
+ }
+
+ // computing for contact forces
+ float left_force_error = force_contact_switch * (-left_forceInRoot(2) - contactForce);
+ float right_force_error = force_contact_switch * (-right_forceInRoot(2) - contactForce);
+
+ // float left_force_error_limited = left_force_error;
+ // float right_force_error_limited = right_force_error;
+
+ // left_force_error_limited = (left_force_error_limited > 2) ? 2 : left_force_error_limited;
+ // left_force_error_limited = (left_force_error_limited < -2) ? -2 : left_force_error_limited;
+
+ // right_force_error_limited = (right_force_error_limited > 2) ? 2 : right_force_error_limited;
+ // right_force_error_limited = (right_force_error_limited < -2) ? -2 : right_force_error_limited;
+
+
+ left_force_error = deadzone(left_force_error, 0.5, 2);
+ right_force_error = deadzone(right_force_error, 0.5, 2);
+
+ left_contactForceZ_correction = left_contactForceZ_correction - forceFilterCoeff * left_force_error;
+ right_contactForceZ_correction = right_contactForceZ_correction - forceFilterCoeff * right_force_error;
+
+ left_contactForceZ_correction = (left_contactForceZ_correction > 30) ? 30 : left_contactForceZ_correction;
+ right_contactForceZ_correction = (right_contactForceZ_correction > 30) ? 30 : right_contactForceZ_correction;
+
+ left_contactForceZ_correction = (left_contactForceZ_correction < -30) ? -contactForce : left_contactForceZ_correction;
+ right_contactForceZ_correction = (right_contactForceZ_correction < -30) ? -contactForce : right_contactForceZ_correction;
+
+ Eigen::Vector3f left_tcpDesiredForce = left_DS_force + coupling_force;
+ Eigen::Vector3f right_tcpDesiredForce = right_DS_force - coupling_force;
+
+ left_tcpDesiredForce(2) += force_contact_switch * (contactForce + left_contactForceZ_correction);
+ right_tcpDesiredForce(2) += force_contact_switch * (contactForce + right_contactForceZ_correction);
+
+
+
+ Eigen::Vector3f left_tcpDesiredTorque = - oriKp * left_tcpDesiredAngularError - oriDamping * left_currentTCPAngularVelocity_filtered;
+ Eigen::Vector3f right_tcpDesiredTorque = - oriKp * right_tcpDesiredAngularError - oriDamping * right_currentTCPAngularVelocity_filtered;
+
+ // for (int i = 0; i < left_tcpDesiredTorque.size(); ++i)
+ // {
+ // left_tcpDesiredTorque(i) = deadzone(left_tcpDesiredTorque(i), )
+
+ // }
+
+
+ left_tcpDesiredTorque_filtered = (1 - filterFactor) * left_tcpDesiredTorque_filtered + filterFactor * left_tcpDesiredTorque;
+ right_tcpDesiredTorque_filtered = (1 - filterFactor) * right_tcpDesiredTorque_filtered + filterFactor * right_tcpDesiredTorque;
+
+
+ Eigen::Vector6f left_tcpDesiredWrench;
+ Eigen::Vector6f right_tcpDesiredWrench;
+
+ left_tcpDesiredWrench << 0.001 * left_tcpDesiredForce, left_tcpDesiredTorque_filtered;
+ right_tcpDesiredWrench << 0.001 * right_tcpDesiredForce, right_tcpDesiredTorque_filtered;
+
+
+ // calculate the null-spcae torque
+ Eigen::MatrixXf I = Eigen::MatrixXf::Identity(jointDim, jointDim);
+
+ float lambda = 0.2;
+ Eigen::MatrixXf left_jtpinv = left_ik->computePseudoInverseJacobianMatrix(left_jacobi.transpose(), lambda);
+ Eigen::MatrixXf right_jtpinv = right_ik->computePseudoInverseJacobianMatrix(right_jacobi.transpose(), lambda);
+
+
+ Eigen::VectorXf left_nullqerror = left_qpos - left_qnullspace;
+ Eigen::VectorXf right_nullqerror = right_qpos - right_qnullspace;
+
+ for (int i = 0; i < left_nullqerror.size(); ++i)
+ {
+ if (fabs(left_nullqerror(i)) < 0.09)
+ {
+ left_nullqerror(i) = 0;
+ }
+
+ if (fabs(right_nullqerror(i)) < 0.09)
+ {
+ right_nullqerror(i) = 0;
+ }
+ }
+
+
+ Eigen::VectorXf left_nullspaceTorque = - nullspaceKp * left_nullqerror - nullspaceDamping * left_jointVelocity_filtered;
+ Eigen::VectorXf right_nullspaceTorque = - nullspaceKp * right_nullqerror - nullspaceDamping * right_jointVelocity_filtered;
+
+ Eigen::VectorXf left_projectedNullTorque = (I - left_jacobi.transpose() * left_jtpinv) * left_nullspaceTorque;
+ Eigen::VectorXf right_projectedNullTorque = (I - right_jacobi.transpose() * right_jtpinv) * right_nullspaceTorque;
+
+ float left_nulltorque_norm = left_projectedNullTorque.norm();
+ float right_nulltorque_norm = right_projectedNullTorque.norm();
+
+ if (left_nulltorque_norm > null_torqueLimit)
+ {
+ left_projectedNullTorque = (null_torqueLimit / left_nulltorque_norm) * left_projectedNullTorque;
+ }
+
+ if (right_nulltorque_norm > null_torqueLimit)
+ {
+ right_projectedNullTorque = (null_torqueLimit / right_nulltorque_norm) * right_projectedNullTorque;
+ }
+
+ Eigen::VectorXf left_jointDesiredTorques = left_jacobi.transpose() * left_tcpDesiredWrench + left_projectedNullTorque;
+ Eigen::VectorXf right_jointDesiredTorques = right_jacobi.transpose() * right_tcpDesiredWrench + right_projectedNullTorque;
+
+
+ right_desiredTorques_filtered = (1 - cfg->TorqueFilterConstant) * right_desiredTorques_filtered + cfg->TorqueFilterConstant * right_jointDesiredTorques;
+
+
+ for (size_t i = 0; i < left_torque_targets.size(); ++i)
+ {
+ float desiredTorque = smooth_startup * left_jointDesiredTorques(i);
+ desiredTorque = deadzone(desiredTorque, cfg->desiredTorqueDisturbance, torqueLimit);
+ left_desiredTorques_filtered(i) = (1 - cfg->TorqueFilterConstant) * left_desiredTorques_filtered(i) + cfg->TorqueFilterConstant * desiredTorque;
+
+ std::string names = left_jointNames[i] + "_desiredTorques";
+ debugDataInfo.getWriteBuffer().desired_torques[names] = desiredTorque;
+ names = names + "_filtered";
+ debugDataInfo.getWriteBuffer().desired_torques[names] = left_desiredTorques_filtered(i);
+
+ if (fabs(left_desiredTorques_filtered(i)) > torqueLimit)
+ {
+ left_torque_targets.at(i)->torque = 0;
+ }
+ else
+ {
+ left_torque_targets.at(i)->torque = left_desiredTorques_filtered(i); // targets.at(i)->velocity = desiredVelocity;
+ }
+ }
+
+ for (size_t i = 0; i < right_torque_targets.size(); ++i)
+ {
+ float desiredTorque = smooth_startup * right_jointDesiredTorques(i);
+ desiredTorque = deadzone(desiredTorque, cfg->desiredTorqueDisturbance, torqueLimit);
+ right_desiredTorques_filtered(i) = (1 - cfg->TorqueFilterConstant) * right_desiredTorques_filtered(i) + cfg->TorqueFilterConstant * desiredTorque;
+
+ std::string names = right_jointNames[i] + "_desiredTorques";
+ debugDataInfo.getWriteBuffer().desired_torques[names] = desiredTorque;
+ names = names + "_filtered";
+ debugDataInfo.getWriteBuffer().desired_torques[names] = right_desiredTorques_filtered(i);
+
+ if (fabs(right_desiredTorques_filtered(i)) > torqueLimit)
+ {
+ right_torque_targets.at(i)->torque = 0;
+ }
+ else
+ {
+ right_torque_targets.at(i)->torque = right_desiredTorques_filtered(i); // targets.at(i)->velocity = desiredVelocity;
+ }
+ }
+
+ smooth_startup = smooth_startup + 0.001 * (1.1 - smooth_startup);
+ smooth_startup = (smooth_startup > 1) ? 1 : smooth_startup;
+ smooth_startup = (smooth_startup < 0) ? 0 : smooth_startup;
+
+
+
+ debugDataInfo.getWriteBuffer().left_realForce_x = left_forceInRoot(0);
+ debugDataInfo.getWriteBuffer().left_realForce_y = left_forceInRoot(1);
+ debugDataInfo.getWriteBuffer().left_realForce_z = left_forceInRoot(2);
+ debugDataInfo.getWriteBuffer().right_realForce_x = right_forceInRoot(0);
+ debugDataInfo.getWriteBuffer().right_realForce_y = right_forceInRoot(1);
+ debugDataInfo.getWriteBuffer().right_realForce_z = right_forceInRoot(2);
+
+ debugDataInfo.getWriteBuffer().left_force_error = left_force_error;
+ debugDataInfo.getWriteBuffer().right_force_error = right_force_error;
+
+
+ debugDataInfo.getWriteBuffer().left_tcpDesiredTorque_x = left_tcpDesiredTorque_filtered(0);
+ debugDataInfo.getWriteBuffer().left_tcpDesiredTorque_y = left_tcpDesiredTorque_filtered(1);
+ debugDataInfo.getWriteBuffer().left_tcpDesiredTorque_z = left_tcpDesiredTorque_filtered(2);
+ debugDataInfo.getWriteBuffer().right_tcpDesiredTorque_x = right_tcpDesiredTorque_filtered(0);
+ debugDataInfo.getWriteBuffer().right_tcpDesiredTorque_y = right_tcpDesiredTorque_filtered(1);
+ debugDataInfo.getWriteBuffer().right_tcpDesiredTorque_z = right_tcpDesiredTorque_filtered(2);
+ // debugDataInfo.getWriteBuffer().desiredForce_x = tcpDesiredForce(0);
+ // debugDataInfo.getWriteBuffer().desiredForce_y = tcpDesiredForce(1);
+ // debugDataInfo.getWriteBuffer().desiredForce_z = tcpDesiredForce(2);
+
+
+ // debugDataInfo.getWriteBuffer().tcpDesiredTorque_x = tcpDesiredTorque(0);
+ // debugDataInfo.getWriteBuffer().tcpDesiredTorque_y = tcpDesiredTorque(1);
+ // debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
+
+ // debugDataInfo.getWriteBuffer().tcpDesiredAngularError_x = tcpDesiredAngularError(0);
+ // debugDataInfo.getWriteBuffer().tcpDesiredAngularError_y = tcpDesiredAngularError(1);
+ // debugDataInfo.getWriteBuffer().tcpDesiredAngularError_z = tcpDesiredAngularError(2);
+
+ // debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_x = currentTCPAngularVelocity(0);
+ // debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_y = currentTCPAngularVelocity(1);
+ // debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_z = currentTCPAngularVelocity(2);
+
+ // debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_x = currentTCPLinearVelocity_filtered(0);
+ // debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_y = currentTCPLinearVelocity_filtered(1);
+ // debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_z = currentTCPLinearVelocity_filtered(2);
+
+ debugDataInfo.commitWrite();
+
+ // }
+ // else
+ // {
+ // for (size_t i = 0; i < targets.size(); ++i)
+ // {
+ // targets.at(i)->torque = 0;
+
+ // }
+ // }
+
+
+
+}
+
+float DSRTBimanualController::deadzone(float input, float disturbance, float threshold)
+{
+ if (input > 0)
+ {
+ input = input - disturbance;
+ input = (input < 0) ? 0 : input;
+ input = (input > threshold) ? threshold : input;
+ }
+ else if (input < 0)
+ {
+ input = input + disturbance;
+ input = (input > 0) ? 0 : input;
+ input = (input < -threshold) ? -threshold : input;
+ }
+
+
+ return input;
+}
+
+Eigen::Quaternionf DSRTBimanualController::quatSlerp(double t, const Eigen::Quaternionf& q0, const Eigen::Quaternionf& q1)
+{
+ double cosHalfTheta = q0.w() * q1.w() + q0.x() * q1.x() + q0.y() * q1.y() + q0.z() * q1.z();
+
+
+ Eigen::Quaternionf q1x = q1;
+ if (cosHalfTheta < 0)
+ {
+ q1x.w() = -q1.w();
+ q1x.x() = -q1.x();
+ q1x.y() = -q1.y();
+ q1x.z() = -q1.z();
+ }
+
+
+ if (fabs(cosHalfTheta) >= 1.0)
+ {
+ return q0;
+ }
+
+ double halfTheta = acos(cosHalfTheta);
+ double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);
+
+
+ Eigen::Quaternionf result;
+ if (fabs(sinHalfTheta) < 0.001)
+ {
+ result.w() = (1 - t) * q0.w() + t * q1x.w();
+ result.x() = (1 - t) * q0.x() + t * q1x.x();
+ result.y() = (1 - t) * q0.y() + t * q1x.y();
+ result.z() = (1 - t) * q0.z() + t * q1x.z();
+
+ }
+ else
+ {
+ double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta;
+ double ratioB = sin(t * halfTheta) / sinHalfTheta;
+
+ result.w() = ratioA * q0.w() + ratioB * q1x.w();
+ result.x() = ratioA * q0.x() + ratioB * q1x.x();
+ result.y() = ratioA * q0.y() + ratioB * q1x.y();
+ result.z() = ratioA * q0.z() + ratioB * q1x.z();
+ }
+
+ return result;
+}
+
+void DSRTBimanualController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
+{
+
+ StringVariantBaseMap datafields;
+ auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
+ for (auto& pair : values)
+ {
+ datafields[pair.first] = new Variant(pair.second);
+ }
+
+ // std::string nameJacobi = "jacobiori";
+ // std::string nameJacobipos = "jacobipos";
+
+ // std::vector<float> jacobiVec = debugDataInfo.getUpToDateReadBuffer().jacobiVec;
+ // std::vector<float> jacobiPos = debugDataInfo.getUpToDateReadBuffer().jacobiPos;
+
+ // for (size_t i = 0; i < jacobiVec.size(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string name = nameJacobi + ss.str();
+ // datafields[name] = new Variant(jacobiVec[i]);
+ // std::string namepos = nameJacobipos + ss.str();
+ // datafields[namepos] = new Variant(jacobiPos[i]);
+
+ // }
+
+
+ datafields["left_tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_tcpDesiredTorque_x);
+ datafields["left_tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_tcpDesiredTorque_y);
+ datafields["left_tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_tcpDesiredTorque_z);
+ datafields["right_tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_tcpDesiredTorque_x);
+ datafields["right_tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_tcpDesiredTorque_y);
+ datafields["right_tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_tcpDesiredTorque_z);
+
+ datafields["left_realForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_realForce_x);
+ datafields["left_realForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_realForce_y);
+ datafields["left_realForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_realForce_z);
+ datafields["right_realForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_realForce_x);
+ datafields["right_realForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_realForce_y);
+ datafields["right_realForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_realForce_z);
+
+ datafields["left_force_error"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_force_error);
+ datafields["right_force_error"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_force_error);
+
+
+ datafields["Desired_Guard_Z"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().desredZ);
+ datafields["Force_Error_Z"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().force_error_z);
+ datafields["guardXYHighFrequency"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guardXYHighFrequency);
+ datafields["guard_mounting_correction_x"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guard_mounting_correction_x);
+ datafields["guard_mounting_correction_y"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guard_mounting_correction_y);
+ datafields["guard_mounting_correction_z"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guard_mounting_correction_z);
+
+
+
+ // datafields["desiredForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_x);
+ // datafields["desiredForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_y);
+ // datafields["desiredForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_z);
+
+ // datafields["tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_x);
+ // datafields["tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_y);
+ // datafields["tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_z);
+
+ // datafields["tcpDesiredAngularError_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_x);
+ // datafields["tcpDesiredAngularError_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_y);
+ // datafields["tcpDesiredAngularError_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_z);
+
+ // datafields["currentTCPAngularVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_x);
+ // datafields["currentTCPAngularVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_y);
+ // datafields["currentTCPAngularVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_z);
+
+
+ // datafields["currentTCPLinearVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_x);
+ // datafields["currentTCPLinearVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_y);
+ // datafields["currentTCPLinearVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_z);
+
+
+ debugObs->setDebugChannel("DSBimanualControllerOutput", datafields);
+
+}
+
+void DSRTBimanualController::rtPreActivateController()
+{
+
+}
+
+void DSRTBimanualController::rtPostDeactivateController()
+{
+
+}
+
+void armarx::DSRTBimanualController::setToDefaultTarget(const Ice::Current&)
+{
+ isDefaultTarget = true;
+}
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.h b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.h
new file mode 100644
index 0000000000000000000000000000000000000000..02fc3f8ab609b6cbcdd6529256e6955e126467a8
--- /dev/null
+++ b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.h
@@ -0,0 +1,548 @@
+/*
+ * 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 DSController::ArmarXObjects::DSRTBimanualController
+ * @author Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_DSController_DSRTBimanualController_H
+#define _ARMARX_LIB_DSController_DSRTBimanualController_H
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <VirtualRobot/Robot.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Tools/Gravity.h>
+
+#include <RobotAPI/interface/units/RobotUnit/DSControllerBase.h>
+#include "GMRDynamics.h"
+#include <ArmarXCore/util/json/JSONObject.h>
+
+#include "MathLib.h"
+//#include <ArmarXGui/libraries/StructuralJson/JPathNavigator.h>
+//#include <ArmarXGui/libraries/StructuralJson/StructuralJsonParser.h>
+
+
+namespace armarx
+{
+ class DSRTBimanualControllerControlData
+ {
+ public:
+ Eigen::Vector3f left_tcpDesiredLinearVelocity;
+ Eigen::Vector3f left_tcpDesiredAngularError;
+
+ Eigen::Vector3f right_tcpDesiredLinearVelocity;
+ Eigen::Vector3f right_tcpDesiredAngularError;
+
+ Eigen::Vector3f left_right_distanceInMeter;
+
+ };
+
+ typedef boost::shared_ptr<GMRDynamics> BimanualGMMPtr;
+
+ struct BimanualGMRParameters
+ {
+ int K_gmm_;
+ int dim_;
+ std::vector<double> Priors_;
+ std::vector<double> Mu_;
+ std::vector<double> Sigma_;
+ std::vector<double> attractor_;
+ double dt_;
+ };
+
+
+ class BimanualGMMMotionGen
+ {
+ public:
+ BimanualGMMMotionGen() {}
+
+ BimanualGMMMotionGen(const std::string& fileName)
+ {
+ getGMMParamsFromJsonFile(fileName);
+ }
+
+ BimanualGMMPtr leftarm_gmm;
+ BimanualGMMPtr rightarm_gmm;
+
+ BimanualGMRParameters leftarm_gmmParas;
+ BimanualGMRParameters rightarm_gmmParas;
+
+ Eigen::Vector3f leftarm_Target;
+ Eigen::Vector3f rightarm_Target;
+
+ Eigen::Vector3f left_DS_DesiredVelocity;
+ Eigen::Vector3f right_DS_DesiredVelocity;
+ Eigen::Vector3f left_right_position_errorInMeter;
+
+
+
+ float scaling;
+ float v_max;
+
+ void getGMMParamsFromJsonFile(const std::string& fileName)
+ {
+ std::ifstream infile { fileName };
+ std::string objDefs = { std::istreambuf_iterator<char>(infile), std::istreambuf_iterator<char>() };
+ // StructuralJsonParser jsonParser(objDefs);
+ // jsonParser.parse();
+ // JPathNavigator jpnav(jsonParser.parsedJson);
+ // float k = jpnav.selectSingleNode("left/K").asFloat();
+ // ARMARX_INFO << "k: " << k ;
+ // jpnav.selectSingleNode("left")
+ JSONObjectPtr json = new JSONObject();
+ json->fromString(objDefs);
+
+ // leftarm_gmmParas.K_gmm_ = AbstractObjectSerializerPtr::dynamicCast<JSONObjectPtr>(json->getElement("left"))->getInt("K");
+ // leftarm_gmmParas.dim_ = json->getElement("left")->getInt("dim");
+ // boost::dynamic_pointer_cast<JSONObjectPtr>(json->getElement("left"))->getArray<double>("Priors", leftarm_gmmParas.Priors_);
+
+ // json->getElement("left")->getArray<double>("Mu", leftarm_gmmParas.Mu_);
+ // json->getElement("left")->getArray<double>("attractor", leftarm_gmmParas.attractor_);
+ // json->getElement("left")->getArray<double>("Sigma", leftarm_gmmParas.Sigma_);
+
+ // rightarm_gmmParas.K_gmm_ = json->getElement("right")->getInt("K");
+ // rightarm_gmmParas.dim_ = json->getElement("right")->getInt("dim");
+ // json->getElement("right")->getArray<double>("Priors", rightarm_gmmParas.Priors_);
+ // json->getElement("right")->getArray<double>("Mu", rightarm_gmmParas.Mu_);
+ // json->getElement("right")->getArray<double>("attractor", rightarm_gmmParas.attractor_);
+ // json->getElement("right")->getArray<double>("Sigma", rightarm_gmmParas.Sigma_);
+
+
+ leftarm_gmmParas.K_gmm_ = json->getInt("leftK");
+ leftarm_gmmParas.dim_ = json->getInt("leftDim");
+ json->getArray<double>("leftPriors", leftarm_gmmParas.Priors_);
+ json->getArray<double>("leftMu", leftarm_gmmParas.Mu_);
+ json->getArray<double>("leftAttractor", leftarm_gmmParas.attractor_);
+ json->getArray<double>("leftSigma", leftarm_gmmParas.Sigma_);
+
+
+ rightarm_gmmParas.K_gmm_ = json->getInt("rightK");
+ rightarm_gmmParas.dim_ = json->getInt("rightDim");
+ json->getArray<double>("rightPriors", rightarm_gmmParas.Priors_);
+ json->getArray<double>("rightMu", rightarm_gmmParas.Mu_);
+ json->getArray<double>("rightAttractor", rightarm_gmmParas.attractor_);
+ json->getArray<double>("rightSigma", rightarm_gmmParas.Sigma_);
+
+
+ scaling = json->getDouble("Scaling");
+ v_max = json->getDouble("MaxVelocity");
+
+ leftarm_gmm.reset(new GMRDynamics(leftarm_gmmParas.K_gmm_, leftarm_gmmParas.dim_, leftarm_gmmParas.dt_, leftarm_gmmParas.Priors_, leftarm_gmmParas.Mu_, leftarm_gmmParas.Sigma_));
+ leftarm_gmm->initGMR(0, 2, 3, 5);
+
+ rightarm_gmm.reset(new GMRDynamics(rightarm_gmmParas.K_gmm_, rightarm_gmmParas.dim_, rightarm_gmmParas.dt_, rightarm_gmmParas.Priors_, rightarm_gmmParas.Mu_, rightarm_gmmParas.Sigma_));
+ rightarm_gmm->initGMR(0, 2, 3, 5);
+
+
+ // if (leftarm_gmmParas.attractor_.size() < 3)
+ // {
+ // ARMARX_ERROR << "attractor in json file for the left arm should be 6 dimension vector ... ";
+ // }
+
+ // if (rightarm_gmmParas.attractor_.size() < 3)
+ // {
+ // ARMARX_ERROR << "attractor in json file for the left arm should be 6 dimension vector ... ";
+ // }
+
+ std::cout << "line 162." << std::endl;
+
+
+ for (int i = 0; i < 3; ++i)
+ {
+ leftarm_Target(i) = leftarm_gmmParas.attractor_.at(i);
+ rightarm_Target(i) = rightarm_gmmParas.attractor_.at(i);
+ }
+
+ std::cout << "Finished GMM." << std::endl;
+
+ }
+
+
+ void updateDesiredVelocity(
+ const Eigen::Vector3f& leftarm_PositionInMeter,
+ const Eigen::Vector3f& rightarm_PositionInMeter,
+ float positionErrorToleranceInMeter,
+ float desiredZ,
+ float correction_x,
+ float correction_y,
+ float correction_z)
+ {
+ MathLib::Vector position_error;
+ position_error.Resize(3);
+
+ MathLib::Vector desired_vel;
+ desired_vel.Resize(3);
+
+
+
+ Eigen::Vector3f leftarm_Target_corrected = leftarm_Target;
+ Eigen::Vector3f rightarm_Target_corrected = rightarm_Target;
+
+ leftarm_Target_corrected(0) += correction_x;
+ rightarm_Target_corrected(0) += correction_x;
+
+ leftarm_Target_corrected(1) += correction_y;
+ rightarm_Target_corrected(1) += correction_y;
+
+ leftarm_Target_corrected(2) = desiredZ + correction_z;
+ rightarm_Target_corrected(2) = desiredZ + correction_z;
+
+
+
+ // for the left arm
+ Eigen::Vector3f leftarm_PositionError = leftarm_PositionInMeter - leftarm_Target_corrected;
+ if (leftarm_PositionError.norm() < positionErrorToleranceInMeter)
+ {
+ leftarm_PositionError.setZero();
+ }
+
+ for (int i = 0; i < 3; ++i)
+ {
+ position_error(i) = leftarm_PositionError(i);
+ }
+
+ desired_vel = leftarm_gmm->getVelocity(position_error);
+
+ Eigen::Vector3f leftarm_desired_vel;
+ leftarm_desired_vel << desired_vel(0), desired_vel(1), desired_vel(2);
+
+ leftarm_desired_vel = scaling * leftarm_desired_vel;
+
+ float lenVec = leftarm_desired_vel.norm();
+
+ if (std::isnan(lenVec))
+ {
+ leftarm_desired_vel.setZero();
+ }
+
+ if (desiredZ < 1.5)
+ {
+ v_max = 0.2;
+ }
+ else
+ {
+ v_max = 0.5;
+ }
+
+ if (lenVec > v_max)
+ {
+ leftarm_desired_vel = (v_max / lenVec) * leftarm_desired_vel;
+ }
+
+ left_DS_DesiredVelocity = leftarm_desired_vel;
+
+
+ // for the right arm
+ Eigen::Vector3f rightarm_PositionError = rightarm_PositionInMeter - rightarm_Target_corrected;
+ if (rightarm_PositionError.norm() < positionErrorToleranceInMeter)
+ {
+ rightarm_PositionError.setZero();
+ }
+
+ for (int i = 0; i < 3; ++i)
+ {
+ position_error(i) = rightarm_PositionError(i);
+ }
+
+ desired_vel = rightarm_gmm->getVelocity(position_error);
+
+ Eigen::Vector3f rightarm_desired_vel;
+ rightarm_desired_vel << desired_vel(0), desired_vel(1), desired_vel(2);
+
+ rightarm_desired_vel = scaling * rightarm_desired_vel;
+
+ lenVec = rightarm_desired_vel.norm();
+ if (std::isnan(lenVec))
+ {
+ rightarm_desired_vel.setZero();
+ }
+
+ if (lenVec > v_max)
+ {
+ rightarm_desired_vel = (v_max / lenVec) * rightarm_desired_vel;
+ }
+
+ right_DS_DesiredVelocity = rightarm_desired_vel;
+
+ left_right_position_errorInMeter = leftarm_PositionError - rightarm_PositionError;
+
+ }
+
+
+
+ };
+
+ typedef boost::shared_ptr<BimanualGMMMotionGen> BimanualGMMMotionGenPtr;
+
+
+
+
+ /**
+ * @defgroup Library-DSRTBimanualController DSRTBimanualController
+ * @ingroup DSController
+ * A description of the library DSRTBimanualController.
+ *
+ * @class DSRTBimanualController
+ * @ingroup Library-DSRTBimanualController
+ * @brief Brief description of class DSRTBimanualController.
+ *
+ * Detailed description of class DSRTBimanualController.
+ */
+
+ class DSRTBimanualController : public NJointControllerWithTripleBuffer<DSRTBimanualControllerControlData>, public DSBimanualControllerInterface
+ {
+
+ // ManagedIceObject interface
+ protected:
+ void onInitNJointController();
+ void onDisconnectNJointController();
+
+
+ void controllerRun();
+
+
+
+ // NJointControllerInterface interface
+ public:
+ using ConfigPtrT = DSRTBimanualControllerConfigPtr;
+
+ DSRTBimanualController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+
+ std::string getClassName(const Ice::Current&) const
+ {
+ return "DSRTBimanualController";
+ }
+
+ // NJointController interface
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+
+ void setToDefaultTarget(const Ice::Current&);
+ private:
+
+ float deadzone(float currentValue, float targetValue, float threshold);
+ Eigen::Quaternionf quatSlerp(double t, const Eigen::Quaternionf& q0, const Eigen::Quaternionf& q1);
+
+ // PeriodicTask<DSRTBimanualController>::pointer_type controllerTask;
+ BimanualGMMMotionGenPtr gmmMotionGenerator;
+ struct DSRTBimanualControllerSensorData
+ {
+ Eigen::Matrix4f left_tcpPose;
+ Eigen::Matrix4f right_tcpPose;
+
+ // Eigen::Vector3f left_linearVelocity;
+ // Eigen::Vector3f right_linearVelocity;
+
+ Eigen::Vector3f left_force;
+ Eigen::Vector3f right_force;
+
+
+ double currentTime;
+
+
+ };
+ TripleBuffer<DSRTBimanualControllerSensorData> controllerSensorData;
+
+ struct DSCtrlDebugInfo
+ {
+ float desredZ;
+ float force_error_z;
+ float guardXYHighFrequency;
+ float guard_mounting_correction_x;
+ float guard_mounting_correction_y;
+ float guard_mounting_correction_z;
+ };
+ TripleBuffer<DSCtrlDebugInfo> debugCtrlDataInfo;
+
+ struct DSRTDebugInfo
+ {
+ StringFloatDictionary desired_torques;
+ float desiredForce_x;
+ float desiredForce_y;
+ float desiredForce_z;
+ float tcpDesiredTorque_x;
+ float tcpDesiredTorque_y;
+ float tcpDesiredTorque_z;
+
+ float tcpDesiredAngularError_x;
+ float tcpDesiredAngularError_y;
+ float tcpDesiredAngularError_z;
+
+ float currentTCPAngularVelocity_x;
+ float currentTCPAngularVelocity_y;
+ float currentTCPAngularVelocity_z;
+
+ float currentTCPLinearVelocity_x;
+ float currentTCPLinearVelocity_y;
+ float currentTCPLinearVelocity_z;
+
+ // force torque sensor in root
+ float left_realForce_x;
+ float left_realForce_y;
+ float left_realForce_z;
+ float right_realForce_x;
+ float right_realForce_y;
+ float right_realForce_z;
+ float left_force_error;
+ float right_force_error;
+
+ float left_tcpDesiredTorque_x;
+ float left_tcpDesiredTorque_y;
+ float left_tcpDesiredTorque_z;
+ float right_tcpDesiredTorque_x;
+ float right_tcpDesiredTorque_y;
+ float right_tcpDesiredTorque_z;
+
+ };
+ TripleBuffer<DSRTDebugInfo> debugDataInfo;
+
+
+ std::vector<const SensorValue1DoFActuatorTorque*> left_torqueSensors;
+ std::vector<const SensorValue1DoFGravityTorque*> left_gravityTorqueSensors;
+ std::vector<const SensorValue1DoFActuatorVelocity*> left_velocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> left_positionSensors;
+
+ std::vector<const SensorValue1DoFActuatorTorque*> right_torqueSensors;
+ std::vector<const SensorValue1DoFGravityTorque*> right_gravityTorqueSensors;
+ std::vector<const SensorValue1DoFActuatorVelocity*> right_velocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> right_positionSensors;
+
+ const SensorValueForceTorque* leftForceTorque;
+ const SensorValueForceTorque* rightForceTorque;
+
+ std::vector<ControlTarget1DoFActuatorTorque*> left_torque_targets;
+ std::vector<ControlTarget1DoFActuatorTorque*> right_torque_targets;
+
+
+ VirtualRobot::RobotNodePtr left_arm_tcp;
+ VirtualRobot::RobotNodePtr right_arm_tcp;
+
+ VirtualRobot::RobotNodePtr left_sensor_frame;
+ VirtualRobot::RobotNodePtr right_sensor_frame;
+
+ VirtualRobot::DifferentialIKPtr left_ik;
+ Eigen::MatrixXf left_jacobip;
+ Eigen::MatrixXf left_jacobio;
+
+ VirtualRobot::DifferentialIKPtr right_ik;
+ Eigen::MatrixXf right_jacobip;
+ Eigen::MatrixXf right_jacobio;
+
+ Eigen::Quaternionf left_desiredQuaternion;
+ Eigen::Vector3f left_oldPosition;
+ Eigen::Matrix3f left_oldOrientation;
+
+ Eigen::Quaternionf right_desiredQuaternion;
+ Eigen::Vector3f right_oldPosition;
+ Eigen::Matrix3f right_oldOrientation;
+
+ Eigen::Vector3f left_currentTCPLinearVelocity_filtered;
+ Eigen::Vector3f left_currentTCPAngularVelocity_filtered;
+
+ Eigen::VectorXf left_jointVelocity_filtered;
+ Eigen::VectorXf right_jointVelocity_filtered;
+
+ Eigen::VectorXf left_desiredTorques_filtered;
+ Eigen::VectorXf right_desiredTorques_filtered;
+
+
+ Eigen::Vector3f left_tcpDesiredTorque_filtered;
+ Eigen::Vector3f right_tcpDesiredTorque_filtered;
+
+ Eigen::Vector3f leftForceOffset;
+ Eigen::Vector3f rightForceOffset;
+
+ float left_contactForceZ_correction;
+ float right_contactForceZ_correction;
+
+ float smooth_startup;
+
+ DSRTBimanualControllerConfigPtr cfg;
+
+ Eigen::Vector3f right_currentTCPLinearVelocity_filtered;
+ Eigen::Vector3f right_currentTCPAngularVelocity_filtered;
+
+ float filterTimeConstant;
+
+ std::vector<std::string> left_jointNames;
+ std::vector<std::string> right_jointNames;
+
+ float posiKp;
+ float v_max;
+ std::vector<float> Damping;
+ float torqueLimit;
+ float null_torqueLimit;
+
+ float Coupling_stiffness;
+ float Coupling_force_limit;
+
+ float forward_gain;
+
+ float oriKp;
+ float oriDamping;
+
+ float nullspaceKp;
+ float nullspaceDamping;
+
+ float contactForce;
+
+ float guardTargetZUp;
+ float guardTargetZDown;
+ float guardDesiredZ;
+ float guard_mounting_correction_z;
+
+ float guardXYHighFrequency;
+ Eigen::Vector3f left_force_old;
+ Eigen::Vector3f right_force_old;
+
+ Eigen::VectorXf left_qnullspace;
+ Eigen::VectorXf right_qnullspace;
+
+ Eigen::Quaternionf left_oriUp;
+ Eigen::Quaternionf left_oriDown;
+ Eigen::Quaternionf right_oriUp;
+ Eigen::Quaternionf right_oriDown;
+
+ // std::vector<BimanualGMMMotionGenPtr> BimanualGMMMotionGenList;
+
+
+ float forceFilterCoeff;
+
+ float positionErrorTolerance;
+ bool controllerStopRequested = false;
+ bool controllerRunFinished = false;
+
+ bool isDefaultTarget = true;
+
+ // NJointController interface
+ protected:
+ void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
+
+ // NJointController interface
+ protected:
+ void rtPreActivateController();
+ void rtPostDeactivateController();
+ };
+
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTController.cpp b/source/RobotAPI/libraries/DSControllers/DSRTController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..efe9bf5231456b0b743ac46c8b830d3958e79812
--- /dev/null
+++ b/source/RobotAPI/libraries/DSControllers/DSRTController.cpp
@@ -0,0 +1,442 @@
+/*
+ * 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 DSController::ArmarXObjects::DSRTController
+ * @author Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "DSRTController.h"
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+using namespace armarx;
+
+NJointControllerRegistration<DSRTController> registrationControllerDSRTController("DSRTController");
+
+void DSRTController::onInitNJointController()
+{
+ ARMARX_INFO << "init ...";
+
+ runTask("DSRTControllerTask", [&]
+ {
+ CycleUtil c(1);
+ getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
+ while (getState() == eManagedIceObjectStarted)
+ {
+ if (isControllerActive())
+ {
+ controllerRun();
+ }
+ c.waitForCycleDuration();
+ }
+ });
+
+
+}
+
+void DSRTController::onDisconnectNJointController()
+{
+
+}
+
+
+DSRTController::DSRTController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+{
+ DSControllerConfigPtr cfg = DSControllerConfigPtr::dynamicCast(config);
+ useSynchronizedRtRobot();
+ VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
+
+ jointNames.clear();
+ ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName);
+ for (size_t i = 0; i < rns->getSize(); ++i)
+ {
+ std::string jointName = rns->getNode(i)->getName();
+ jointNames.push_back(jointName);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ ARMARX_CHECK_EXPRESSION(ct);
+ const SensorValueBase* sv = useSensorValue(jointName);
+ ARMARX_CHECK_EXPRESSION(sv);
+ auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
+ ARMARX_CHECK_EXPRESSION(casted_ct);
+ targets.push_back(casted_ct);
+
+ const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+ if (!torqueSensor)
+ {
+ ARMARX_WARNING << "No Torque sensor available for " << jointName;
+ }
+ if (!gravityTorqueSensor)
+ {
+ ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
+ }
+
+ torqueSensors.push_back(torqueSensor);
+ gravityTorqueSensors.push_back(gravityTorqueSensor);
+ velocitySensors.push_back(velocitySensor);
+ positionSensors.push_back(positionSensor);
+ };
+ ARMARX_INFO << "Initialized " << targets.size() << " targets";
+ tcp = rns->getTCP();
+ ARMARX_CHECK_EXPRESSION_W_HINT(tcp, cfg->tcpName);
+
+
+ ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+
+ DSRTControllerSensorData initSensorData;
+ initSensorData.tcpPose = tcp->getPoseInRootFrame();
+ initSensorData.currentTime = 0;
+ controllerSensorData.reinitAllBuffers(initSensorData);
+
+
+ oldPosition = tcp->getPositionInRootFrame();
+ oldOrientation = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
+
+ std::vector<float> desiredPositionVec = cfg->desiredPosition;
+ for (size_t i = 0; i < 3; ++i)
+ {
+ desiredPosition(i) = desiredPositionVec[i];
+ }
+ ARMARX_INFO << "ik reseted ";
+
+ std::vector<float> desiredQuaternionVec = cfg->desiredQuaternion;
+ ARMARX_CHECK_EQUAL(desiredQuaternionVec.size(), 4);
+
+ desiredQuaternion.x() = desiredQuaternionVec.at(0);
+ desiredQuaternion.y() = desiredQuaternionVec.at(1);
+ desiredQuaternion.z() = desiredQuaternionVec.at(2);
+ desiredQuaternion.w() = desiredQuaternionVec.at(3);
+
+
+
+ DSRTControllerControlData initData;
+ for (size_t i = 0; i < 3; ++i)
+ {
+ initData.tcpDesiredLinearVelocity(i) = 0;
+ }
+
+ for (size_t i = 0; i < 3; ++i)
+ {
+ initData.tcpDesiredAngularError(i) = 0;
+ }
+ reinitTripleBuffer(initData);
+
+ // initial filter
+ currentTCPLinearVelocity_filtered.setZero();
+ currentTCPAngularVelocity_filtered.setZero();
+ filterTimeConstant = cfg->filterTimeConstant;
+ posiKp = cfg->posiKp;
+ v_max = cfg->v_max;
+ posiDamping = cfg->posiDamping;
+ torqueLimit = cfg->torqueLimit;
+ oriKp = cfg->oriKp;
+ oriDamping = cfg->oriDamping;
+
+
+ std::vector<float> qnullspaceVec = cfg->qnullspaceVec;
+
+ qnullspace.resize(qnullspaceVec.size());
+
+ for (size_t i = 0; i < qnullspaceVec.size(); ++i)
+ {
+ qnullspace(i) = qnullspaceVec[i];
+ }
+
+ nullspaceKp = cfg->nullspaceKp;
+ nullspaceDamping = cfg->nullspaceDamping;
+
+
+ //set GMM parameters ...
+ std::vector<std::string> gmmParamsFiles = cfg->gmmParamsFiles;
+ if (gmmParamsFiles.size() == 0)
+ {
+ ARMARX_ERROR << "no gmm found ... ";
+ }
+
+ gmmMotionGenList.clear();
+ float sumBelief = 0;
+ for (size_t i = 0; i < gmmParamsFiles.size(); ++i)
+ {
+ gmmMotionGenList.push_back(GMMMotionGenPtr(new GMMMotionGen(gmmParamsFiles.at(i))));
+ sumBelief += gmmMotionGenList[i]->belief;
+ }
+ ARMARX_CHECK_EQUAL(gmmMotionGenList.size(), 2);
+
+ dsAdaptorPtr.reset(new DSAdaptor(gmmMotionGenList, cfg->dsAdaptorEpsilon));
+ positionErrorTolerance = cfg->positionErrorTolerance;
+
+
+ ARMARX_INFO << "Initialization done";
+}
+
+
+void DSRTController::controllerRun()
+{
+ if (!controllerSensorData.updateReadBuffer())
+ {
+ return;
+ }
+
+
+ Eigen::Matrix4f currentTCPPose = controllerSensorData.getReadBuffer().tcpPose;
+ Eigen::Vector3f realVelocity = controllerSensorData.getReadBuffer().linearVelocity;
+
+ Eigen::Vector3f currentTCPPositionInMeter;
+ currentTCPPositionInMeter << currentTCPPose(0, 3), currentTCPPose(1, 3), currentTCPPose(2, 3);
+ currentTCPPositionInMeter = 0.001 * currentTCPPositionInMeter;
+
+ dsAdaptorPtr->updateDesiredVelocity(currentTCPPositionInMeter, positionErrorTolerance);
+ Eigen::Vector3f tcpDesiredLinearVelocity = dsAdaptorPtr->totalDesiredVelocity;
+ dsAdaptorPtr->updateBelief(realVelocity);
+
+
+
+ float vecLen = tcpDesiredLinearVelocity.norm();
+ if (vecLen > v_max)
+ {
+
+ tcpDesiredLinearVelocity = tcpDesiredLinearVelocity / vecLen * v_max;
+ }
+
+ ARMARX_INFO << "tcpDesiredLinearVelocity: " << tcpDesiredLinearVelocity;
+
+ debugDataInfo.getWriteBuffer().belief0 = dsAdaptorPtr->task0_belief;
+ debugDataInfo.getWriteBuffer().belief1 = gmmMotionGenList[0]->belief;
+ debugDataInfo.getWriteBuffer().belief2 = gmmMotionGenList[1]->belief;
+ debugDataInfo.commitWrite();
+
+ Eigen::Vector3f tcpDesiredAngularError;
+ tcpDesiredAngularError << 0, 0, 0;
+
+ Eigen::Matrix3f currentOrientation = currentTCPPose.block<3, 3>(0, 0);
+ Eigen::Matrix3f desiredOrientation = desiredQuaternion.normalized().toRotationMatrix();
+ Eigen::Matrix3f orientationError = currentOrientation * desiredOrientation.inverse();
+ Eigen::Quaternionf diffQuaternion(orientationError);
+ Eigen::AngleAxisf angleAxis(diffQuaternion);
+ tcpDesiredAngularError = angleAxis.angle() * angleAxis.axis();
+
+ getWriterControlStruct().tcpDesiredLinearVelocity = tcpDesiredLinearVelocity;
+ getWriterControlStruct().tcpDesiredAngularError = tcpDesiredAngularError;
+
+ writeControlStruct();
+}
+
+
+void DSRTController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+{
+ double deltaT = timeSinceLastIteration.toSecondsDouble();
+ if (deltaT != 0)
+ {
+
+ Eigen::Matrix4f currentTCPPose = tcp->getPoseInRootFrame();
+
+
+ Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
+
+ Eigen::VectorXf qpos;
+ Eigen::VectorXf qvel;
+ qpos.resize(positionSensors.size());
+ qvel.resize(velocitySensors.size());
+
+ int jointDim = positionSensors.size();
+
+ for (size_t i = 0; i < velocitySensors.size(); ++i)
+ {
+ qpos(i) = positionSensors[i]->position;
+ qvel(i) = velocitySensors[i]->velocity;
+ }
+
+ Eigen::VectorXf tcptwist = jacobi * qvel;
+
+ Eigen::Vector3f currentTCPLinearVelocity;
+ currentTCPLinearVelocity << 0.001 * tcptwist(0), 0.001 * tcptwist(1), 0.001 * tcptwist(2);
+ double filterFactor = deltaT / (filterTimeConstant + deltaT);
+ currentTCPLinearVelocity_filtered = (1 - filterFactor) * currentTCPLinearVelocity_filtered + filterFactor * currentTCPLinearVelocity;
+
+ controllerSensorData.getWriteBuffer().tcpPose = currentTCPPose;
+ controllerSensorData.getWriteBuffer().currentTime += deltaT;
+ controllerSensorData.getWriteBuffer().linearVelocity = currentTCPLinearVelocity_filtered;
+ controllerSensorData.commitWrite();
+
+
+ Eigen::Vector3f currentTCPAngularVelocity;
+ currentTCPAngularVelocity << tcptwist(3), tcptwist(4), tcptwist(5);
+ // // calculate linear velocity
+ // Eigen::Vector3f currentTCPPosition;
+ // currentTCPPosition << currentTCPPose(0, 3), currentTCPPose(1, 3), currentTCPPose(2, 3);
+ // Eigen::Vector3f currentTCPLinearVelocity_raw = (currentTCPPosition - oldPosition) / deltaT;
+ // oldPosition = currentTCPPosition;
+
+ // // calculate angular velocity
+ // Eigen::Matrix3f currentTCPOrientation = currentTCPPose.block<3, 3>(0, 0);
+ // Eigen::Matrix3f currentTCPDiff = currentTCPOrientation * oldOrientation.inverse();
+ // Eigen::AngleAxisf currentAngleAxisDiff(currentTCPDiff);
+ // Eigen::Vector3f currentTCPAngularVelocity_raw = currentAngleAxisDiff.angle() * currentAngleAxisDiff.axis();
+ // oldOrientation = currentTCPOrientation;
+ // currentTCPAngularVelocity_filtered = (1 - filterFactor) * currentTCPAngularVelocity_filtered + filterFactor * currentTCPAngularVelocity_raw;
+
+
+ Eigen::Vector3f tcpDesiredLinearVelocity = rtGetControlStruct().tcpDesiredLinearVelocity;
+ Eigen::Vector3f tcpDesiredAngularError = rtGetControlStruct().tcpDesiredAngularError;
+
+ // calculate desired tcp force
+ Eigen::Vector3f tcpDesiredForce = -posiDamping * (currentTCPLinearVelocity_filtered - tcpDesiredLinearVelocity);
+
+ // calculate desired tcp torque
+ Eigen::Vector3f tcpDesiredTorque = - oriKp * tcpDesiredAngularError - oriDamping * currentTCPAngularVelocity;
+
+ Eigen::Vector6f tcpDesiredWrench;
+ tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
+
+ // calculate desired joint torque
+ Eigen::MatrixXf I = Eigen::MatrixXf::Identity(jointDim, jointDim);
+
+ float lambda = 2;
+ Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), lambda);
+
+ Eigen::VectorXf nullqerror = qpos - qnullspace;
+
+ for (int i = 0; i < nullqerror.rows(); ++i)
+ {
+ if (fabs(nullqerror(i)) < 0.09)
+ {
+ nullqerror(i) = 0;
+ }
+ }
+ Eigen::VectorXf nullspaceTorque = - nullspaceKp * nullqerror - nullspaceDamping * qvel;
+
+ Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * tcpDesiredWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
+
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ float desiredTorque = jointDesiredTorques(i);
+
+ desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
+ desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
+
+ debugDataInfo.getWriteBuffer().desired_torques[jointNames[i]] = jointDesiredTorques(i);
+
+ targets.at(i)->torque = desiredTorque; // targets.at(i)->velocity = desiredVelocity;
+ }
+
+
+ debugDataInfo.getWriteBuffer().desiredForce_x = tcpDesiredForce(0);
+ debugDataInfo.getWriteBuffer().desiredForce_y = tcpDesiredForce(1);
+ debugDataInfo.getWriteBuffer().desiredForce_z = tcpDesiredForce(2);
+
+
+ debugDataInfo.getWriteBuffer().tcpDesiredTorque_x = tcpDesiredTorque(0);
+ debugDataInfo.getWriteBuffer().tcpDesiredTorque_y = tcpDesiredTorque(1);
+ debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
+
+ debugDataInfo.getWriteBuffer().tcpDesiredAngularError_x = tcpDesiredAngularError(0);
+ debugDataInfo.getWriteBuffer().tcpDesiredAngularError_y = tcpDesiredAngularError(1);
+ debugDataInfo.getWriteBuffer().tcpDesiredAngularError_z = tcpDesiredAngularError(2);
+
+ debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_x = currentTCPAngularVelocity(0);
+ debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_y = currentTCPAngularVelocity(1);
+ debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_z = currentTCPAngularVelocity(2);
+
+ debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_x = currentTCPLinearVelocity_filtered(0);
+ debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_y = currentTCPLinearVelocity_filtered(1);
+ debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_z = currentTCPLinearVelocity_filtered(2);
+
+ debugDataInfo.commitWrite();
+
+ }
+ else
+ {
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ targets.at(i)->torque = 0;
+
+ }
+ }
+
+
+
+}
+
+void DSRTController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
+{
+
+ StringVariantBaseMap datafields;
+ auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
+ for (auto& pair : values)
+ {
+ datafields[pair.first] = new Variant(pair.second);
+ }
+
+ // std::string nameJacobi = "jacobiori";
+ // std::string nameJacobipos = "jacobipos";
+
+ // std::vector<float> jacobiVec = debugDataInfo.getUpToDateReadBuffer().jacobiVec;
+ // std::vector<float> jacobiPos = debugDataInfo.getUpToDateReadBuffer().jacobiPos;
+
+ // for (size_t i = 0; i < jacobiVec.size(); ++i)
+ // {
+ // std::stringstream ss;
+ // ss << i;
+ // std::string name = nameJacobi + ss.str();
+ // datafields[name] = new Variant(jacobiVec[i]);
+ // std::string namepos = nameJacobipos + ss.str();
+ // datafields[namepos] = new Variant(jacobiPos[i]);
+
+ // }
+
+
+
+ datafields["desiredForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_x);
+ datafields["desiredForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_y);
+ datafields["desiredForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_z);
+
+ datafields["tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_x);
+ datafields["tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_y);
+ datafields["tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_z);
+
+ datafields["tcpDesiredAngularError_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_x);
+ datafields["tcpDesiredAngularError_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_y);
+ datafields["tcpDesiredAngularError_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_z);
+
+ datafields["currentTCPAngularVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_x);
+ datafields["currentTCPAngularVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_y);
+ datafields["currentTCPAngularVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_z);
+
+
+ datafields["currentTCPLinearVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_x);
+ datafields["currentTCPLinearVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_y);
+ datafields["currentTCPLinearVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_z);
+
+ datafields["belief_0"] = new Variant(debugDataInfo.getUpToDateReadBuffer().belief0);
+ datafields["belief_1"] = new Variant(debugDataInfo.getUpToDateReadBuffer().belief1);
+ datafields["belief_2"] = new Variant(debugDataInfo.getUpToDateReadBuffer().belief2);
+
+ debugObs->setDebugChannel("DSControllerOutput", datafields);
+
+}
+
+void DSRTController::rtPreActivateController()
+{
+
+}
+
+void DSRTController::rtPostDeactivateController()
+{
+
+}
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTController.h b/source/RobotAPI/libraries/DSControllers/DSRTController.h
new file mode 100644
index 0000000000000000000000000000000000000000..d60552cef0a5bb6bf563b3d161a93185c0560220
--- /dev/null
+++ b/source/RobotAPI/libraries/DSControllers/DSRTController.h
@@ -0,0 +1,491 @@
+/*
+ * 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 DSController::ArmarXObjects::DSRTController
+ * @author Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_DSController_DSRTController_H
+#define _ARMARX_LIB_DSController_DSRTController_H
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <VirtualRobot/Robot.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Tools/Gravity.h>
+
+#include <RobotAPI/interface/units/RobotUnit/DSControllerBase.h>
+#include "GMRDynamics.h"
+#include <ArmarXCore/util/json/JSONObject.h>
+
+#include "MathLib.h"
+
+namespace armarx
+{
+ class DSRTControllerControlData
+ {
+ public:
+ Eigen::Vector3f tcpDesiredLinearVelocity;
+ Eigen::Vector3f tcpDesiredAngularError;
+ };
+
+ typedef boost::shared_ptr<GMRDynamics> GMMPtr;
+
+ struct GMRParameters
+ {
+ int K_gmm_;
+ int dim_;
+ std::vector<double> Priors_;
+ std::vector<double> Mu_;
+ std::vector<double> Sigma_;
+ std::vector<double> attractor_;
+ double dt_;
+ };
+
+
+ class GMMMotionGen
+ {
+ public:
+ GMMMotionGen() {}
+
+ GMMMotionGen(const std::string& fileName)
+ {
+ getGMMParamsFromJsonFile(fileName);
+ }
+
+ GMMPtr gmm;
+ GMRParameters gmmParas;
+ Eigen::Vector3f desiredDefaultTarget;
+ float scaling;
+
+ float belief;
+ float v_max;
+
+ void getGMMParamsFromJsonFile(const std::string& fileName)
+ {
+ std::ifstream infile { fileName };
+ std::string objDefs = { std::istreambuf_iterator<char>(infile), std::istreambuf_iterator<char>() };
+ JSONObjectPtr json = new JSONObject();
+ json->fromString(objDefs);
+
+
+ gmmParas.K_gmm_ = json->getInt("K");
+ gmmParas.dim_ = json->getInt("dim");
+ json->getArray<double>("Priors", gmmParas.Priors_);
+ json->getArray<double>("Mu", gmmParas.Mu_);
+ json->getArray<double>("attractor", gmmParas.attractor_);
+ json->getArray<double>("Sigma", gmmParas.Sigma_);
+
+ scaling = json->getDouble("Scaling");
+ belief = json->getDouble("InitBelief");
+ belief = 0;
+ v_max = json->getDouble("MaxVelocity");
+
+ gmm.reset(new GMRDynamics(gmmParas.K_gmm_, gmmParas.dim_, gmmParas.dt_, gmmParas.Priors_, gmmParas.Mu_, gmmParas.Sigma_));
+ gmm->initGMR(0, 2, 3, 5);
+
+ if (gmmParas.attractor_.size() < 3)
+ {
+ ARMARX_ERROR << "attractor in json file should be 6 dimension vector ... ";
+ }
+
+ for (int i = 0; i < 3; ++i)
+ {
+ desiredDefaultTarget(i) = gmmParas.attractor_.at(i);
+ }
+ }
+
+ void updateDesiredVelocity(const Eigen::Vector3f& currentPositionInMeter, float positionErrorToleranceInMeter)
+ {
+ Eigen::Vector3f PositionError = currentPositionInMeter - desiredDefaultTarget;
+ if (PositionError.norm() < positionErrorToleranceInMeter)
+ {
+ PositionError.setZero();
+ }
+
+ MathLib::Vector position_error;
+ position_error.Resize(3);
+
+ for (int i = 0; i < 3; ++i)
+ {
+ position_error(i) = PositionError(i);
+ }
+
+ MathLib::Vector desired_vel;
+ desired_vel.Resize(3);
+ desired_vel = gmm->getVelocity(position_error);
+
+ Eigen::Vector3f tcpDesiredLinearVelocity;
+ tcpDesiredLinearVelocity << desired_vel(0), desired_vel(1), desired_vel(2);
+
+ currentDesiredVelocity = scaling * tcpDesiredLinearVelocity;
+
+
+ float lenVec = tcpDesiredLinearVelocity.norm();
+ if (std::isnan(lenVec))
+ {
+ tcpDesiredLinearVelocity.setZero();
+ }
+
+ if (lenVec > v_max)
+ {
+ tcpDesiredLinearVelocity = (v_max / lenVec) * tcpDesiredLinearVelocity;
+ }
+ }
+
+
+
+ Eigen::Vector3f currentDesiredVelocity;
+ };
+
+ typedef boost::shared_ptr<GMMMotionGen> GMMMotionGenPtr;
+
+ class DSAdaptor
+ {
+ public:
+ float task0_belief;
+ float epsilon;
+ DSAdaptor() {}
+
+ DSAdaptor(std::vector<GMMMotionGenPtr> gmmMotionGenList, float epsilon)
+ {
+ task0_belief = 1;
+ this->gmmMotionGenList = gmmMotionGenList;
+
+ ARMARX_INFO << "epsilon: " << epsilon;
+ this->epsilon = epsilon;
+
+ totalDesiredVelocity.setZero();
+ }
+
+ Eigen::Vector3f totalDesiredVelocity;
+ std::vector<GMMMotionGenPtr> gmmMotionGenList;
+
+
+ void updateDesiredVelocity(const Eigen::Vector3f& currentPositionInMeter, float positionErrorToleranceInMeter)
+ {
+ totalDesiredVelocity.setZero();
+ for (size_t i = 0; i < gmmMotionGenList.size(); ++i)
+ {
+ gmmMotionGenList[i]->updateDesiredVelocity(currentPositionInMeter, positionErrorToleranceInMeter);
+ totalDesiredVelocity += gmmMotionGenList[i]->belief * gmmMotionGenList[i]->currentDesiredVelocity;
+ }
+ }
+
+ void updateBelief(const Eigen::Vector3f& realVelocity)
+ {
+ std::vector<float> beliefUpdate;
+ beliefUpdate.resize(gmmMotionGenList.size());
+
+ float nullInnerSimilarity = 0;
+ for (size_t i = 0; i < gmmMotionGenList.size(); ++i)
+ {
+
+ GMMMotionGenPtr currentGMM = gmmMotionGenList[i];
+
+ float belief = currentGMM->belief;
+ Eigen::Vector3f OtherTasks = totalDesiredVelocity - belief * currentGMM->currentDesiredVelocity;
+ float innerSimilarity = 2 * OtherTasks.dot(currentGMM->currentDesiredVelocity);
+ float outerDisSimilarity = (realVelocity - currentGMM->currentDesiredVelocity).squaredNorm();
+
+ if (innerSimilarity > nullInnerSimilarity)
+ {
+ nullInnerSimilarity = innerSimilarity;
+ }
+
+ beliefUpdate[i] = - outerDisSimilarity - innerSimilarity;
+
+
+ }
+
+
+
+
+ float nullOuterSimilarity = realVelocity.squaredNorm();
+ float zeroTaskRawBeliefUpdate = - nullInnerSimilarity - nullOuterSimilarity;
+
+ if (zeroTaskRawBeliefUpdate < 0.2)
+ {
+ zeroTaskRawBeliefUpdate -= 1000;
+ }
+
+
+ beliefUpdate.insert(beliefUpdate.begin(), zeroTaskRawBeliefUpdate);
+
+ WinnerTakeAll(beliefUpdate);
+ task0_belief += epsilon * beliefUpdate[0];
+ if (task0_belief > 1)
+ {
+ task0_belief = 1;
+ }
+ else if (task0_belief < 0)
+ {
+ task0_belief = 0;
+ }
+
+ float beliefSum = task0_belief;
+
+ for (size_t i = 0; i < gmmMotionGenList.size(); ++i)
+ {
+ gmmMotionGenList[i]->belief += epsilon * beliefUpdate[i + 1];
+
+
+ if (gmmMotionGenList[i]->belief > 1)
+ {
+ gmmMotionGenList[i]->belief = 1;
+ }
+ else if (gmmMotionGenList[i]->belief < 0)
+ {
+ gmmMotionGenList[i]->belief = 0;
+ }
+
+ beliefSum += gmmMotionGenList[i]->belief;
+
+ }
+
+ for (size_t i = 0; i < gmmMotionGenList.size(); ++i)
+ {
+ gmmMotionGenList[i]->belief /= beliefSum;
+ }
+
+ task0_belief /= beliefSum;
+ }
+
+ private:
+
+ void WinnerTakeAll(std::vector<float>& UpdateBeliefs_)
+ {
+ // std::fill(UpdateBeliefs_.begin(), UpdateBeliefs_.end(), 0);
+
+ size_t winner_index = 0;
+
+ for (size_t i = 1; i < UpdateBeliefs_.size(); i++)
+ {
+ if (UpdateBeliefs_[i] > UpdateBeliefs_[winner_index])
+ {
+ winner_index = i;
+ }
+ }
+
+ float winner_belief = task0_belief;
+
+ if (winner_index != 0)
+ {
+ winner_belief = gmmMotionGenList[winner_index - 1]->belief;
+ }
+
+ if (winner_belief == 1)
+ {
+ return;
+ }
+
+ int runnerUp_index = 0;
+
+ if (winner_index == 0)
+ {
+ runnerUp_index = 1;
+ }
+
+ for (size_t i = 0; i < UpdateBeliefs_.size(); i++)
+ {
+ if (i == winner_index)
+ {
+ continue;
+ }
+
+ if (UpdateBeliefs_[i] > UpdateBeliefs_[runnerUp_index])
+ {
+ runnerUp_index = i;
+ }
+ }
+
+ float offset = 0.5 * (UpdateBeliefs_[winner_index] + UpdateBeliefs_[runnerUp_index]);
+
+ for (size_t i = 0; i < UpdateBeliefs_.size(); i++)
+ {
+ UpdateBeliefs_[i] -= offset;
+ }
+
+ float UpdateSum = 0;
+
+ for (size_t i = 0; i < UpdateBeliefs_.size(); i++)
+ {
+ float belief = task0_belief;
+ if (i != 0)
+ {
+ belief = gmmMotionGenList[i - 1]->belief;
+ }
+
+ if (belief != 0 || UpdateBeliefs_[i] > 0)
+ {
+ UpdateSum += UpdateBeliefs_[i];
+ }
+ }
+
+ UpdateBeliefs_[winner_index] -= UpdateSum;
+ }
+ };
+
+ typedef boost::shared_ptr<DSAdaptor> DSAdaptorPtr;
+
+
+
+ /**
+ * @defgroup Library-DSRTController DSRTController
+ * @ingroup DSController
+ * A description of the library DSRTController.
+ *
+ * @class DSRTController
+ * @ingroup Library-DSRTController
+ * @brief Brief description of class DSRTController.
+ *
+ * Detailed description of class DSRTController.
+ */
+
+ class DSRTController : public NJointControllerWithTripleBuffer<DSRTControllerControlData>, public DSControllerInterface
+ {
+
+ // ManagedIceObject interface
+ protected:
+ void onInitNJointController();
+ void onDisconnectNJointController();
+
+
+ void controllerRun();
+
+
+
+ // NJointControllerInterface interface
+ public:
+ using ConfigPtrT = DSControllerConfigPtr;
+
+ DSRTController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+
+ std::string getClassName(const Ice::Current&) const
+ {
+ return "DSRTController";
+ }
+
+ // NJointController interface
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+
+ private:
+ // PeriodicTask<DSRTController>::pointer_type controllerTask;
+
+ struct DSRTControllerSensorData
+ {
+ Eigen::Matrix4f tcpPose;
+ double currentTime;
+
+ Eigen::Vector3f linearVelocity;
+
+ };
+ TripleBuffer<DSRTControllerSensorData> controllerSensorData;
+
+ struct DSRTDebugInfo
+ {
+ StringFloatDictionary desired_torques;
+ float desiredForce_x;
+ float desiredForce_y;
+ float desiredForce_z;
+ float tcpDesiredTorque_x;
+ float tcpDesiredTorque_y;
+ float tcpDesiredTorque_z;
+
+ float tcpDesiredAngularError_x;
+ float tcpDesiredAngularError_y;
+ float tcpDesiredAngularError_z;
+
+ float currentTCPAngularVelocity_x;
+ float currentTCPAngularVelocity_y;
+ float currentTCPAngularVelocity_z;
+
+ float currentTCPLinearVelocity_x;
+ float currentTCPLinearVelocity_y;
+ float currentTCPLinearVelocity_z;
+
+ float belief0;
+ float belief1;
+ float belief2;
+ };
+ TripleBuffer<DSRTDebugInfo> debugDataInfo;
+
+ std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
+ std::vector<const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
+ std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
+
+ std::vector<ControlTarget1DoFActuatorTorque*> targets;
+
+
+ VirtualRobot::RobotNodePtr tcp;
+
+ VirtualRobot::DifferentialIKPtr ik;
+ Eigen::MatrixXf jacobip;
+ Eigen::MatrixXf jacobio;
+
+ Eigen::Vector3f desiredPosition;
+
+ Eigen::Quaternionf desiredQuaternion;
+ Eigen::Vector3f oldPosition;
+
+ Eigen::Matrix3f oldOrientation;
+
+ Eigen::Vector3f currentTCPLinearVelocity_filtered;
+ Eigen::Vector3f currentTCPAngularVelocity_filtered;
+
+ float filterTimeConstant;
+
+ std::vector<std::string> jointNames;
+
+ float posiKp;
+ float v_max;
+ float posiDamping;
+ float torqueLimit;
+
+ float oriKp;
+ float oriDamping;
+
+ float nullspaceKp;
+ float nullspaceDamping;
+
+ Eigen::VectorXf qnullspace;
+
+ std::vector<GMMMotionGenPtr> gmmMotionGenList;
+
+ DSAdaptorPtr dsAdaptorPtr;
+
+ float positionErrorTolerance;
+
+
+ // NJointController interface
+ protected:
+ void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
+
+ // NJointController interface
+ protected:
+ void rtPreActivateController();
+ void rtPostDeactivateController();
+ };
+
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/DSControllers/GMRDynamics.cpp b/source/RobotAPI/libraries/DSControllers/GMRDynamics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1941fd2b8dda74b2fe4124f2786629eff8c526df
--- /dev/null
+++ b/source/RobotAPI/libraries/DSControllers/GMRDynamics.cpp
@@ -0,0 +1,132 @@
+/*
+ * GMRDynamics.cpp
+ *
+ * Created on: Nov 20, 2011
+ * Author: Seungsu KIM
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include "GMRDynamics.h"
+
+
+GMRDynamics::GMRDynamics(int nStates, int nVar, double delta_t, const char* f_mu, const char* f_sigma, const char* f_prio)
+{
+ this->delta_t = delta_t;
+ GMM = new Gaussians(nStates, nVar, f_mu, f_sigma, f_prio);
+}
+
+GMRDynamics::GMRDynamics(int nStates, int nVar, double delta_t, const vector<double> pri_vec, const vector<double> mu_vec, const vector<double> sig_vec)
+{
+ this->delta_t = delta_t;
+ GMM = new Gaussians(nStates, nVar, pri_vec, mu_vec, sig_vec);
+}
+
+
+void GMRDynamics::initGMR(int first_inindex, int last_inindex, int first_outindex, int last_outindex)
+{
+ GMM->InitFastGMR(first_inindex, last_inindex, first_outindex, last_outindex);
+
+ gDim = last_inindex - first_inindex + 1;
+ if (gDim != static_cast<unsigned int>(last_outindex - first_outindex + 1))
+ {
+ cout << "dynamics dimension is not matching" << endl;
+ }
+
+ gXi.Resize(gDim);
+ target.Resize(gDim);
+
+ gXi.Zero();
+ target.Zero();
+}
+
+void GMRDynamics::setStateTarget(Vector state, Vector target)
+{
+ setTarget(target);
+ setState(state);
+}
+
+void GMRDynamics::setTarget(Vector target, double target_t)
+{
+ this->target_t = target_t;
+
+ //gXi += (this->target - target);
+ this->target = target;
+}
+
+Vector GMRDynamics::getTarget(void)
+{
+ return target;
+}
+
+double GMRDynamics::getTargetT(void)
+{
+ return target_t;
+}
+
+void GMRDynamics::setState(Vector state)
+{
+ gXi = state;
+}
+
+Vector GMRDynamics::getState(void)
+{
+ return gXi;
+}
+
+void GMRDynamics::setCurrentTime(double current_t)
+{
+ this->current_t = current_t;
+}
+
+double GMRDynamics::getCurrentTime(void)
+{
+ return current_t;
+}
+
+Vector GMRDynamics::getVelocity(Vector x)
+{
+ return GMM->Regression(x);
+
+
+}
+
+
+Vector GMRDynamics::getNextState(void)
+{
+ return getNextState(1.0);
+}
+
+Vector GMRDynamics::getNextState(double lamda)
+{
+ // target time
+ target_t -= (delta_t* lamda);
+
+ gXi += (getVelocity(gXi - target) * (delta_t* lamda));
+
+ return gXi;
+}
+
+double GMRDynamics::getReachingTime(double lamda)
+{
+ unsigned int frame = 0;
+ unsigned int li = 0;
+ Vector xi(3);
+ xi.Set(gXi);
+
+ for (frame = 0; frame < REACHING_ITERATION_MAX; frame++)
+ {
+ for (li = 0; li < INTEGRATION_L; li++)
+ {
+ xi += getVelocity(xi - target) * delta_t / (double)INTEGRATION_L * lamda;
+
+ if ((xi - target).Norm() < GMR_ERROR_TOLERANCE)
+ {
+ return (double)(frame * INTEGRATION_L + li) * delta_t / (double)INTEGRATION_L;
+ }
+ }
+ }
+ return (double)(frame * INTEGRATION_L + li) * delta_t / (double)INTEGRATION_L;
+}
+
diff --git a/source/RobotAPI/libraries/DSControllers/GMRDynamics.h b/source/RobotAPI/libraries/DSControllers/GMRDynamics.h
new file mode 100644
index 0000000000000000000000000000000000000000..de826ca48963846616753dd86912f5c28d4c962b
--- /dev/null
+++ b/source/RobotAPI/libraries/DSControllers/GMRDynamics.h
@@ -0,0 +1,56 @@
+/*
+ * GMRDynamics.h
+ *
+ * Created on: Nov 20, 2011
+ * Author: Seungsu KIM
+ */
+
+#ifndef __GMRDYNAMICS_H__
+#define __GMRDYNAMICS_H__
+
+#include "Gaussians.h"
+
+#define GMR_ERROR_TOLERANCE 0.001
+#define INTEGRATION_L 5
+#define REACHING_ITERATION_MAX 500
+#define REAL_MIN (1e-30)
+
+// GMR Dynamics
+class GMRDynamics
+{
+private:
+ Gaussians* GMM;
+
+ double delta_t;
+ double target_t;
+ double current_t;
+
+ Vector gXi;
+ Vector target;
+ unsigned int gDim;
+
+public:
+ GMRDynamics(int nStates, int nVar, double delta_t, const char* f_mu, const char* f_sigma, const char* f_prio);
+ GMRDynamics(int nStates, int nVar, double delta_t, const vector<double> pri_vec, const vector<double> mu_vec, const vector<double> sig_vec);
+
+ void initGMR(int first_inindex, int last_inindex, int first_outindex, int last_outindex);
+
+ void setStateTarget(Vector state, Vector target);
+ void setTarget(Vector target, double target_t = -1.0);
+ Vector getTarget(void);
+ double getTargetT(void);
+ void setState(Vector state);
+ Vector getState(void);
+ void setCurrentTime(double current_t);
+ double getCurrentTime(void);
+
+ Vector getVelocity(Vector x);
+
+ Vector getNextState(void);
+ Vector getNextState(double lamda);
+ double getReachingTime(double lamda);
+};
+
+
+
+#endif //__GMRDYNAMICS_H__
diff --git a/source/RobotAPI/libraries/DSControllers/Gaussians.cpp b/source/RobotAPI/libraries/DSControllers/Gaussians.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..200605424145ced271fc45acb4bea5044e2b3d38
--- /dev/null
+++ b/source/RobotAPI/libraries/DSControllers/Gaussians.cpp
@@ -0,0 +1,518 @@
+/*
+ * Gaussians.cpp
+ *
+ * Created on: Nov 19, 2011
+ * Author: Seungsu KIM
+ */
+
+#include <math.h>
+#include <iostream>
+#include <fstream>
+
+#include "Gaussians.h"
+
+using namespace std;
+/*
+Gaussians::Gaussians(GMMs *model)
+{
+ this->model.nbStates = model->nbStates;
+ this->model.nbDim = model->nbDim;
+
+ this->model.States = (GMMState *)malloc(model->nbStates*sizeof(GMMState) );
+
+ for(int s=0; s<model->nbStates; s++ ){
+ this->model.States[s].Mu = model->GMMState[s].Mu;
+ this->model.States[s].Sigma = model->GMMState[s].Sigma;
+ this->model.States[s].Prio = model->GMMState[s].Prio;
+ }
+}
+*/
+
+Gaussians::Gaussians(const char *f_mu, const char *f_sigma, const char *f_prio)
+{
+ int s, i, j;
+ int nbStates;
+ int nbDim;
+
+ Matrix fMatrix;
+ fMatrix.Load(f_prio);
+ if ( fMatrix(0, fMatrix.ColumnSize() - 1) > 0.0 )
+ {
+ nbStates = fMatrix.ColumnSize();
+ }
+ else
+ {
+ nbStates = fMatrix.ColumnSize() - 1;
+ }
+
+ for ( s = 0; s < nbStates; s++ ) {
+ model.States[s].Prio = fMatrix(0, s);
+ }
+
+ fMatrix.Load(f_mu);
+ nbDim = fMatrix.RowSize();
+ model.nbStates = nbStates;
+ model.nbDim = nbDim;
+
+
+ for ( s = 0; s < nbStates; s++ ) {
+ model.States[s].Mu.Resize(nbDim);
+ model.States[s].Sigma.Resize(nbDim, nbDim );
+ }
+
+ for ( s = 0; s < nbStates; s++ ) {
+ model.States[s].Mu = fMatrix.GetColumn(s);
+ }
+
+ fMatrix.Load(f_sigma);
+ j = 0;
+ for ( s = 0; s < nbStates; s++ ) {
+ for ( i = 0; i < nbDim; i++ ) {
+ model.States[s].Sigma.SetRow(fMatrix.GetRow(j), i);
+ j++;
+ }
+ }
+}
+
+Gaussians::Gaussians(int nbStates, int nbDim, const char *f_mu, const char *f_sigma, const char *f_prio)
+{
+
+ int s, i, j;
+
+ model.nbStates = nbStates;
+ model.nbDim = nbDim;
+
+ for ( s = 0; s < nbStates; s++ ) {
+ model.States[s].Mu.Resize(nbDim);
+ model.States[s].Sigma.Resize(nbDim, nbDim );
+ }
+
+ Matrix fMatrix(nbDim, nbStates);
+ fMatrix.Load(f_mu);
+ for ( s = 0; s < nbStates; s++ ) {
+ model.States[s].Mu = fMatrix.GetColumn(s);
+ }
+
+ fMatrix.Resize(nbStates * nbDim, nbDim);
+ fMatrix.Load(f_sigma);
+ j = 0;
+ for ( s = 0; s < nbStates; s++ ) {
+ for ( i = 0; i < nbDim; i++ ) {
+ model.States[s].Sigma.SetRow(fMatrix.GetRow(j), i);
+ j++;
+ }
+ }
+
+ fMatrix.Resize(1, nbStates);
+ fMatrix.Load(f_prio);
+ Vector fVector(nbStates);
+ for ( s = 0; s < nbStates; s++ ) {
+ model.States[s].Prio = fMatrix(0, s);
+ }
+
+}
+
+Gaussians::Gaussians(const int nbStates, const int nbDim, const vector<double> pri_vec, const vector<double> mu_vec, const vector<double> sig_vec) {
+
+ model.nbStates = nbStates;
+ model.nbDim = nbDim;
+
+ for ( int s = 0; s < nbStates; s++ ) {
+ model.States[s].Mu.Resize(nbDim);
+ model.States[s].Sigma.Resize(nbDim, nbDim );
+ }
+
+ for ( int s = 0; s < nbStates; s++ ) {
+ model.States[s].Prio = pri_vec[s];
+ }
+ // cout << endl << "Printing the constructed Priors" << endl;
+ // for ( int s = 0; s < nbStates; s++ ) {
+ // cout << model.States[s].Prio << "\t";
+ // }
+ // cout << endl;
+
+
+ for ( int s = 0; s < nbStates; s++ ) {
+ for (int d = 0; d < nbDim; d++) {
+ model.States[s].Mu[d] = mu_vec[s * nbDim + d];
+ }
+ }
+
+
+ // cout << endl << "Printing the constructed Mu" << endl;
+ // for ( int s = 0; s < nbStates; s++ ) {
+ // for (int d = 0; d < nbDim; d++) {
+ // cout << model.States[s].Mu[d] << "\t";
+ // }
+ // cout << endl;
+ // }
+
+ for ( int s = 0; s < nbStates; s++ ) {
+ for (int row = 0; row < nbDim; row++) {
+ for (int col = 0; col < nbDim; col++) {
+ int ind = s * nbDim * nbDim + row * nbDim + col;
+ model.States[s].Sigma(row, col) = sig_vec[ind];
+ }
+ }
+ }
+
+
+ // cout << endl << "Printing the constructed Sigma" << endl;
+ // for ( int s = 0; s < nbStates; s++ ) {
+ // for (int row = 0; row < nbDim; row++) {
+ // for (int col = 0; col < nbDim; col++) {
+ // cout << model.States[s].Sigma(row, col) << "\t";
+ // }
+ // cout <<endl;
+ // }
+ // cout << endl;
+ // }
+
+
+
+
+}
+
+
+
+
+void Gaussians::setGMMs(GMMs *model)
+{
+ for (unsigned int s = 0; s < model->nbStates; s++ ) {
+ this->model.States[s].Mu = model->States[s].Mu;
+ this->model.States[s].Sigma = model->States[s].Sigma;
+ this->model.States[s].Prio = model->States[s].Prio;
+ }
+}
+
+
+void Gaussians::InitFastGaussians(int first_inindex, int last_inindex)
+{
+ double det;
+ int nbIN = last_inindex - first_inindex + 1;
+
+ for (unsigned int s = 0; s < model.nbStates; s++ ) {
+ gmmpinv[s].MuI.Resize(nbIN);
+ gmmpinv[s].SigmaII.Resize(nbIN, nbIN);
+ gmmpinv[s].SigmaIIInv.Resize(nbIN, nbIN);
+ }
+
+ for (unsigned int s = 0; s < model.nbStates; s++ ) {
+ for ( int i = first_inindex; i <= last_inindex; i++ ) gmmpinv[s].MuI(i - first_inindex) = model.States[s].Mu(i);
+ for ( int i = first_inindex; i <= last_inindex; i++ )
+ for ( int j = first_inindex; j <= last_inindex; j++ )
+ gmmpinv[s].SigmaII(i - first_inindex, j - first_inindex) = model.States[s].Sigma(i, j);
+
+ gmmpinv[s].SigmaII.Inverse(gmmpinv[s].SigmaIIInv, &det);
+ //gmmpinv[s].SigmaIIInv = gmmpinv[s].SigmaII.Inverse();
+ //gmmpinv[s].SigmaII.Inverse(&det);
+ if (det < 0) det = 1e-30;
+ gmmpinv[s].detSigmaII = det;
+
+ }
+
+ nbDimI = last_inindex - first_inindex + 1;
+ gfDiff.Resize(nbDimI);
+ gfDiffp.Resize(nbDimI);
+ gDer.Resize(nbDimI);
+
+}
+
+double Gaussians::GaussianPDFFast(int state, Vector x)
+{
+ double p;
+ gfDiff = x - gmmpinv[state].MuI;
+ gfDiffp = gmmpinv[state].SigmaIIInv * gfDiff;
+
+ p = exp(-0.5 * gfDiff.Dot(gfDiffp)) / sqrt(pow(2.0 * PI, nbDimI) * ( gmmpinv[state].detSigmaII + 1e-30));
+
+ return p;
+}
+
+double Gaussians::GaussianProbFast(Vector x)
+{
+ double totalP = 0;
+ for (unsigned int s = 0; s < model.nbStates; s++ ) {
+ totalP += model.States[s].Prio * GaussianPDFFast(s, x);
+ }
+ return totalP;
+}
+
+Vector Gaussians::GaussianDerProbFast(Vector x)
+{
+ gDer.Zero();
+ for (unsigned int s = 0; s < model.nbStates; s++ ) {
+ gDer += (gmmpinv[s].SigmaIIInv * (x - gmmpinv[s].MuI)) * model.States[s].Prio * GaussianPDFFast(s, x);
+ }
+ return -gDer;
+}
+
+void Gaussians::InitFastGMR(int first_inindex, int last_inindex, int first_outindex, int last_outindex)
+{
+ double det;
+ int nbIN = last_inindex - first_inindex + 1;
+ int nbOUT = last_outindex - first_outindex + 1;
+
+ gPdf.Resize(model.nbStates);
+
+ for (unsigned int s = 0; s < model.nbStates; s++ ) {
+ gmmpinv[s].MuI.Resize(nbIN);
+ gmmpinv[s].SigmaII.Resize(nbIN, nbIN);
+ gmmpinv[s].SigmaIIInv.Resize(nbIN, nbIN);
+
+ gmmpinv[s].muO.Resize(nbOUT);
+ gmmpinv[s].SigmaIO.Resize(nbIN, nbOUT);
+ gmmpinv[s].SigmaIOInv.Resize(nbOUT, nbOUT);
+ }
+
+ for (unsigned int s = 0; s < model.nbStates; s++ ) {
+ for ( int i = first_inindex; i <= last_inindex; i++ ) {
+ gmmpinv[s].MuI(i - first_inindex) = model.States[s].Mu(i);
+
+ for ( int j = first_inindex; j <= last_inindex; j++ ) {
+ gmmpinv[s].SigmaII(i - first_inindex, j - first_inindex) = model.States[s].Sigma(i, j);
+ }
+ for ( int j = first_outindex; j <= last_outindex; j++ ) {
+ gmmpinv[s].SigmaIO(i - first_inindex, j - first_outindex) = model.States[s].Sigma(i, j);
+ }
+ }
+
+ for ( int i = first_outindex; i <= last_outindex; i++ ) {
+ gmmpinv[s].muO(i - first_outindex) = model.States[s].Mu(i);
+ }
+
+ gmmpinv[s].SigmaII.Inverse(gmmpinv[s].SigmaIIInv, &det);
+ if (det < 0) det = 1e-30;
+ gmmpinv[s].detSigmaII = det;
+ (gmmpinv[s].SigmaIO).Transpose().Inverse(gmmpinv[s].SigmaIOInv, &det);
+ }
+
+ nbDimI = last_inindex - first_inindex + 1;
+ gfDiff.Resize(nbDimI);
+ gfDiffp.Resize(nbDimI);
+ gDer.Resize(nbDimI);
+
+}
+
+void Gaussians::Regression(const Vector & indata, Vector & outdata, Matrix & derGMR)
+{
+ Regression(indata, outdata);
+ cout << "derivative is not implemented yet " << endl;
+}
+
+void Gaussians::Regression(const Vector & indata, Vector & outdata)
+{
+ double pdfall;
+ Vector h(model.nbStates);
+ Vector r_diff(outdata.Size());
+
+ for (unsigned int s = 0; s < model.nbStates; s++) {
+ gPdf(s) = model.States[s].Prio * GaussianPDFFast(s, indata);
+ }
+ pdfall = gPdf.Sum();
+
+ outdata.Zero();
+ for (unsigned int s = 0; s < model.nbStates; s++) {
+ //h(s) = gPdf(s)/(pdfall + 1e-30 );
+ h(s) = gPdf(s) / (pdfall );
+ r_diff = gmmpinv[s].SigmaIO.Transpose() * gmmpinv[s].SigmaIIInv * (indata - gmmpinv[s].MuI);
+
+ for (unsigned int i = 0; i < r_diff.Size(); i++ ) {
+ outdata(i) += h(s) * (r_diff(i) + gmmpinv[s].muO(i));
+ }
+ }
+}
+
+Vector Gaussians::Regression(const Vector & indata)
+{
+ Vector outdata(indata.Size());
+ Regression(indata, outdata);
+ return outdata;
+}
+
+
+/*
+#include <math.h>
+#include "Gaussians.h"
+
+#include "armadillo"
+
+using namespace arma;
+using namespace std;
+
+Gaussians::Gaussians(GMMs *model)
+{
+ this->model.nbStates = model->nbStates;
+ this->model.nbDim = model->nbDim;
+
+ this->model.States = (GMMState *)malloc(model->nbStates*sizeof(GMMState) );
+
+ for(int s=0; s<model->nbStates; s++ ){
+ this->model.States[s].Mu = model->GMMState[s].Mu;
+ this->model.States[s].Sigma = model->GMMState[s].Sigma;
+ this->model.States[s].Prio = model->GMMState[s].Prio;
+ }
+}
+
+Gaussians::Gaussians(int nbStates, int nbDim, char *f_mu, char *f_sigma, char *f_prio)
+{
+
+ int s, i, j;
+
+ model.nbStates = nbStates;
+ model.nbDim = nbDim;
+ model.States = (GMMState *)malloc(nbStates*sizeof(GMMState) );
+
+ for( s=0; s<nbStates; s++ ){
+ model.States[s].Mu = zeros<vec>(nbDim);
+ model.States[s].Sigma = zeros<mat>(nbDim, nbDim );
+ }
+
+ // f_mu
+ ifstream fin;
+ fin.open(f_mu);
+ for( i=0; i<nbDim; i++ ){
+ for( s=0; s<nbStates; s++ ){
+ fin >> model.States[s].Mu(i);
+ }
+ }
+ fin.close();
+
+ // f_sigma
+ fin.open(f_sigma);
+ for( s=0; s<nbStates; s++ ){
+ for( i=0; i<nbDim; i++ ){
+ for( j=0; j<nbDim; j++ ){
+ fin >> model.States[s].Sigma(i,j);
+ }
+ }
+ }
+ fin.close();
+
+ // f_prio
+ fin.open(f_prio);
+ for( s=0; s<nbStates; s++ ){
+ fin >>model.States[s].Prio;
+ }
+ fin.close();
+}
+
+void Gaussians::setGMMs(GMMs *model)
+{
+ for(int s=0; s<model->nbStates; s++ ){
+ this->model.States[s].Mu = model->GMMState[s].Mu;
+ this->model.States[s].Sigma = model->GMMState[s].Sigma;
+ this->model.States[s].Prio = model->GMMState[s].Prio;
+ }
+}
+
+
+void Gaussians::InitFastGaussians(int first_inindex, int last_inindex)
+{
+ gmmpinv = (GMMStateP *)malloc(model.nbStates*sizeof(GMMStateP) );
+
+ for(int s=0; s<model.nbStates; s++ ){
+ gmmpinv[s].MuI = model.States[s].Mu.subvec(first_inindex, last_inindex);
+ gmmpinv[s].SigmaII = model.States[s].Sigma.submat(first_inindex, first_inindex, last_inindex, last_inindex);
+ gmmpinv[s].SigmaIIInv = pinv(gmmpinv[s].SigmaII);
+ gmmpinv[s].detSigmaII = det(gmmpinv[s].SigmaII);
+ }
+
+ nbDimI = last_inindex - first_inindex +1;
+ gfDiff = zeros<vec>(nbDimI);
+ gfDiffp = zeros<vec>(nbDimI);
+ gDer = zeros<vec>(nbDimI);
+}
+
+double Gaussians::GaussianPDFFast(int state, vec x)
+{
+ double p;
+ gfDiff = x - gmmpinv[state].MuI;
+ gfDiffp = gmmpinv[state].SigmaIIInv * gfDiff;
+
+ p = exp(-0.5*dot(gfDiff, gfDiffp)) / sqrt(pow(2.0*math::pi(), nbDimI)*( gmmpinv[state].detSigmaII +1e-30));
+
+ return p;
+}
+
+double Gaussians::GaussianProbFast(vec x)
+{
+ double totalP=0;
+ for(int s=0; s<model.nbStates; s++ ){
+ totalP += model.States[s].Prio*GaussianPDFFast(s,x);
+ }
+ return totalP;
+}
+
+vec Gaussians::GaussianDerProbFast(vec x)
+{
+ gDer.zeros();
+ for(int s=0; s<model.nbStates; s++ ){
+ gDer += model.States[s].Prio * gmmpinv[s].SigmaIIInv *(x-gmmpinv[s].MuI)*GaussianPDFFast(s,x);
+ }
+ return -gDer;
+}
+
+//-------------------------------------------------------------------------------------------------------
+void AllocateGMMs(GMMs *model, int nbDim, int nbStates)
+{
+ model->nbDim = nbDim;
+ model->nbStates = nbStates;
+ model->GMMState = (GMMState *)malloc(nbStates*sizeof(GMMState) );
+
+ for(int s=0; s<nbStates; s++ ){
+ model->GMMState[s].Mu = zeros<vec>(nbDim);
+ model->GMMState[s].Sigma = zeros<mat>(nbDim, nbDim );
+ }
+}
+
+
+double GaussianPDF(vec x, vec Mu, mat Sigma)
+{
+ double p;
+ vec diff = x - Mu;
+ vec diffp = pinv( Sigma ) * diff;
+ int nbDim = x.size();
+
+ p = exp(-0.5*dot(diff, diffp)) / sqrt(pow(2.0*math::pi(), nbDim)*( abs(det(Sigma)) +1e-30));
+
+ if(p < 1e-30){
+ return 1e-30;
+ }
+ else{
+ return p;
+ }
+}
+
+void GaussianMux(GMMs *modelK, GMMs *modelL, GMMs *modelOut)
+{
+ int k,l,j;
+ int K = modelK->nbStates;
+ int L = modelL->nbStates;
+ int J = K*L;
+
+ //modelOut->nbDim = modelK->nbDim;
+ //modelOut->nbStates = J;
+ //modelOut->GMMState = (GMMState *)malloc(J*sizeof(GMMState) );
+
+ j=0;
+ for(k=0; k<K; k++){
+ for(l=0; l<L; l++){
+ modelOut->GMMState[j].Sigma = pinv( pinv(modelK->GMMState[k].Sigma) + pinv( modelL->GMMState[l].Sigma) );
+ modelOut->GMMState[j].Mu = modelOut->GMMState[j].Sigma *( pinv(modelK->GMMState[k].Sigma) * modelK->GMMState[k].Mu + pinv(modelL->GMMState[l].Sigma) * modelL->GMMState[l].Mu );
+ modelOut->GMMState[j].Prio = modelK->GMMState[k].Prio* modelL->GMMState[l].Prio * GaussianPDF( modelK->GMMState[k].Mu, modelL->GMMState[l].Mu, modelK->GMMState[k].Sigma + modelL->GMMState[l].Sigma);
+ j++;
+ }
+ }
+}
+
+void GaussianRotate(GMMs *model, arma::vec P, arma::mat R, GMMs *modelOut)
+{
+ for(int s=0; s<model->nbStates; s++){
+ modelOut->GMMState[s].Mu = R*model->GMMState[s].Mu + P;
+ modelOut->GMMState[s].Sigma = R*model->GMMState[s].Sigma*trans(R);
+ }
+ //modelOut->nbDim = model->nbDim;
+ //modelOut->nbStates = model->nbStates;
+}
+*/
diff --git a/source/RobotAPI/libraries/DSControllers/Gaussians.h b/source/RobotAPI/libraries/DSControllers/Gaussians.h
new file mode 100644
index 0000000000000000000000000000000000000000..b72890a832d7f81b619a24655b236daa869acbad
--- /dev/null
+++ b/source/RobotAPI/libraries/DSControllers/Gaussians.h
@@ -0,0 +1,83 @@
+/*
+ * Gaussians.h
+ *
+ * Created on: Nov 19, 2011
+ * Author: Seungsu KIM
+ */
+
+#ifndef __GAUSSIANSM_H__
+#define __GAUSSIANSM_H__
+
+#include "MathLib.h"
+
+#define GAUSSIAN_MAXIMUM_NUMBER 50
+
+using namespace MathLib;
+
+struct GMMState
+{
+ Vector Mu;
+ Matrix Sigma;
+ double Prio;
+};
+
+struct GMMStateP
+{
+ Vector MuI;
+ Matrix SigmaII;
+ Matrix SigmaIIInv;
+ double detSigmaII;
+
+ // for GMR
+ Vector muO;
+ Matrix SigmaIO;
+ Matrix SigmaIOInv;
+};
+
+struct GMMs
+{
+ unsigned int nbStates;
+ unsigned int nbDim;
+
+ GMMState States[GAUSSIAN_MAXIMUM_NUMBER];
+};
+
+class Gaussians
+{
+private:
+ GMMStateP gmmpinv[GAUSSIAN_MAXIMUM_NUMBER];
+
+public:
+ GMMs model;
+
+ Gaussians(const char* f_mu, const char* f_sigma, const char* f_prio);
+ Gaussians(int nbStates, int nbDim, const char* f_mu, const char* f_sigma, const char* f_prio);
+ Gaussians(const int nbStates, const int nbDim, const vector<double> pri_vec, const vector<double> mu_vec, const vector<double> sig_vec);
+ Gaussians(GMMs* model);
+
+ void setGMMs(GMMs* model);
+
+ // For fast computation of GaussianPDF
+ Vector gfDiff, gfDiffp;
+ Vector gDer;
+ Vector gPdf;
+ int nbDimI;
+
+
+ void InitFastGaussians(int first_inindex, int last_inindex);
+ double GaussianPDFFast(int state, Vector x);
+ double GaussianProbFast(Vector x);
+ Vector GaussianDerProbFast(Vector x);
+
+ void InitFastGMR(int first_inindex, int last_inindex, int first_outindex, int last_outindex);
+ void Regression(const Vector& indata, Vector& outdata, Matrix& derGMR);
+ void Regression(const Vector& indata, Vector& outdata);
+ Vector Regression(const Vector& indata);
+
+};
+/*
+void GaussianMux(GMMs *modelK, GMMs *modelL, GMMs *modelOut);
+void GaussianRotate(GMMs *model, Vector P, Matrix R, GMMs *modelOut);
+*/
+
+#endif //__GAUSSIANS_H__
diff --git a/source/RobotAPI/libraries/DebugDrawerConfigWidget/CMakeLists.txt b/source/RobotAPI/libraries/DebugDrawerConfigWidget/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..02e56dcab691c35572a94ac66ed4822e231d57a6
--- /dev/null
+++ b/source/RobotAPI/libraries/DebugDrawerConfigWidget/CMakeLists.txt
@@ -0,0 +1,35 @@
+set(LIB_NAME DebugDrawerConfigWidget)
+
+armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
+
+armarx_find_qt(QtCore QtGui QtDesigner)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+set(LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ DebugDrawer
+ ${QT_LIBRARIES}
+)
+
+set(SOURCES
+./DebugDrawerConfigWidget.cpp
+)
+set(HEADERS
+./DebugDrawerConfigWidget.h
+)
+set(GUI_MOC_HDRS
+./DebugDrawerConfigWidget.h
+)
+set(GUI_UIS
+./DebugDrawerConfigWidget.ui
+)
+
+if(ArmarXGui_FOUND)
+ armarx_gui_library("${LIB_NAME}" "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${LIBS}")
+
+ # add unit tests
+ add_subdirectory(test)
+endif()
diff --git a/source/RobotAPI/libraries/DebugDrawerConfigWidget/DebugDrawerConfigWidget.cpp b/source/RobotAPI/libraries/DebugDrawerConfigWidget/DebugDrawerConfigWidget.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..504409aee28f308fddce88de06155340af1b527e
--- /dev/null
+++ b/source/RobotAPI/libraries/DebugDrawerConfigWidget/DebugDrawerConfigWidget.cpp
@@ -0,0 +1,160 @@
+/*
+ * 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::DebugDrawerConfigWidget
+ * @author Adrian Knobloch ( adrian dot knobloch at student dot kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "DebugDrawerConfigWidget.h"
+#include <QInputDialog>
+
+namespace armarx
+{
+
+ DebugDrawerConfigWidget::DebugDrawerConfigWidget(const DebugDrawerComponentPtr& debugDrawer, QWidget* parent)
+ : QWidget(parent),
+ debugDrawer(debugDrawer)
+ {
+ ui.setupUi(this);
+ refresh();
+
+ connect(ui.pushButtonRefresh, &QPushButton::released, this, &DebugDrawerConfigWidget::refresh);
+ connect(ui.listWidgetLayerVisibility, &QListWidget::itemChanged, this, &DebugDrawerConfigWidget::onVisibilityChanged);
+ connect(ui.checkBoxDefaultLayerVisibility, &QCheckBox::stateChanged, this, &DebugDrawerConfigWidget::onDefaultLayerVisibilityChanged);
+ connect(ui.listWidgetDefaultLayerVisibility, &QListWidget::itemChanged, this, &DebugDrawerConfigWidget::onDefaultLayerVisibilityPerLayerChanged);
+ connect(ui.pushButtonDefaultLayerVisibilityAdd, &QPushButton::released, this, &DebugDrawerConfigWidget::onAddDefaultLayerVisibility);
+ connect(ui.pushButtonDefaultLayerVisibilityRemove, &QPushButton::released, this, &DebugDrawerConfigWidget::onRemoveDefaultLayerVisibility);
+ }
+
+ void DebugDrawerConfigWidget::loadSettings(QSettings* settings)
+ {
+ ui.checkBoxDefaultLayerVisibility->setCheckState(settings->value(QString::fromStdString("DefaultLayerVisibility")).toBool() ? Qt::Checked : Qt::Unchecked);
+ settings->beginGroup("layer");
+ for (const auto& key : settings->allKeys())
+ {
+ QListWidgetItem* item = new QListWidgetItem(key, ui.listWidgetDefaultLayerVisibility);
+ item->setCheckState(settings->value(key).toBool() ? Qt::Checked : Qt::Unchecked);
+ ui.listWidgetDefaultLayerVisibility->addItem(item);
+ }
+ settings->endGroup();
+ }
+
+ void DebugDrawerConfigWidget::saveSettings(QSettings* settings)
+ {
+ settings->setValue(QString::fromStdString("DefaultLayerVisibility"), ui.checkBoxDefaultLayerVisibility->checkState() == Qt::Checked);
+ settings->beginGroup("layer");
+ for (const auto& layerInfo : debugDrawer->getAllDefaultLayerVisibilities())
+ {
+ settings->setValue(QString::fromStdString(layerInfo.first), layerInfo.second);
+ }
+ settings->endGroup();
+ }
+
+ void DebugDrawerConfigWidget::setDebugDrawer(const DebugDrawerComponentPtr& debugDrawer)
+ {
+ this->debugDrawer = debugDrawer;
+ onDefaultLayerVisibilityChanged(ui.checkBoxDefaultLayerVisibility->checkState());
+ for (int i = 0; i < ui.listWidgetDefaultLayerVisibility->count(); ++i)
+ {
+ onDefaultLayerVisibilityPerLayerChanged(ui.listWidgetDefaultLayerVisibility->item(i));
+ }
+ refresh();
+ }
+
+ DebugDrawerComponentPtr DebugDrawerConfigWidget::getDebugDrawer() const
+ {
+ return debugDrawer;
+ }
+
+ void DebugDrawerConfigWidget::refresh()
+ {
+ if (!debugDrawer)
+ {
+ return;
+ }
+ ui.listWidgetLayerVisibility->clear();
+ for (const auto& layerInfo : debugDrawer->layerInformation())
+ {
+ QListWidgetItem* item = new QListWidgetItem(QString::fromStdString(layerInfo.layerName), ui.listWidgetLayerVisibility);
+ item->setCheckState(layerInfo.visible ? Qt::Checked : Qt::Unchecked);
+ ui.listWidgetLayerVisibility->addItem(item);
+ }
+ ui.checkBoxDefaultLayerVisibility->setCheckState(debugDrawer->getDefaultLayerVisibility() ? Qt::Checked : Qt::Unchecked);
+ ui.listWidgetDefaultLayerVisibility->clear();
+ for (const auto& layerInfo : debugDrawer->getAllDefaultLayerVisibilities())
+ {
+ QListWidgetItem* item = new QListWidgetItem(QString::fromStdString(layerInfo.first), ui.listWidgetDefaultLayerVisibility);
+ item->setCheckState(layerInfo.second ? Qt::Checked : Qt::Unchecked);
+ ui.listWidgetDefaultLayerVisibility->addItem(item);
+ }
+ }
+
+ void DebugDrawerConfigWidget::onVisibilityChanged(QListWidgetItem* item)
+ {
+ if (!debugDrawer)
+ {
+ return;
+ }
+ debugDrawer->enableLayerVisu(item->text().toStdString(), item->checkState() == Qt::Checked);
+ }
+
+ void DebugDrawerConfigWidget::onDefaultLayerVisibilityChanged(int checkState)
+ {
+ if (!debugDrawer)
+ {
+ return;
+ }
+ debugDrawer->setDefaultLayerVisibility(checkState != Qt::Unchecked);
+ }
+
+ void DebugDrawerConfigWidget::onDefaultLayerVisibilityPerLayerChanged(QListWidgetItem* item)
+ {
+ if (!debugDrawer)
+ {
+ return;
+ }
+ debugDrawer->setDefaultLayerVisibility(item->text().toStdString(), item->checkState() == Qt::Checked);
+ }
+
+ void DebugDrawerConfigWidget::onAddDefaultLayerVisibility()
+ {
+ bool ok;
+ QString text = QInputDialog::getText(this, "Layer name",
+ "Layer name:", QLineEdit::Normal,
+ ui.listWidgetLayerVisibility->selectedItems().empty() ? "" : ui.listWidgetLayerVisibility->selectedItems().front()->text(), &ok);
+ if (ok && !text.isEmpty())
+ {
+ QListWidgetItem* item = new QListWidgetItem(text, ui.listWidgetDefaultLayerVisibility);
+ item->setCheckState(debugDrawer->getDefaultLayerVisibility() ? Qt::Checked : Qt::Unchecked);
+ ui.listWidgetDefaultLayerVisibility->addItem(item);
+ }
+ }
+
+ void DebugDrawerConfigWidget::onRemoveDefaultLayerVisibility()
+ {
+ for (const auto& selected : ui.listWidgetDefaultLayerVisibility->selectedItems())
+ {
+ if (debugDrawer)
+ {
+ debugDrawer->removeDefaultLayerVisibility(selected->text().toStdString());
+ }
+ ui.listWidgetDefaultLayerVisibility->takeItem(ui.listWidgetDefaultLayerVisibility->row(selected));
+ }
+ }
+
+}
diff --git a/source/RobotAPI/libraries/DebugDrawerConfigWidget/DebugDrawerConfigWidget.h b/source/RobotAPI/libraries/DebugDrawerConfigWidget/DebugDrawerConfigWidget.h
new file mode 100644
index 0000000000000000000000000000000000000000..6680464000af1a9706c765955979dc18ea8d297c
--- /dev/null
+++ b/source/RobotAPI/libraries/DebugDrawerConfigWidget/DebugDrawerConfigWidget.h
@@ -0,0 +1,73 @@
+/*
+ * 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::DebugDrawerConfigWidget
+ * @author Adrian Knobloch ( adrian dot knobloch at student dot kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
+
+#include <RobotAPI/libraries/DebugDrawerConfigWidget/ui_DebugDrawerConfigWidget.h>
+
+#include <QWidget>
+#include <QSettings>
+
+
+namespace armarx
+{
+ /**
+ * @defgroup Library-DebugDrawerConfigWidget DebugDrawerConfigWidget
+ * @ingroup RobotAPI
+ * A description of the library DebugDrawerConfigWidget.
+ *
+ * @class DebugDrawerConfigWidget
+ * @ingroup Library-DebugDrawerConfigWidget
+ * @brief Brief description of class DebugDrawerConfigWidget.
+ *
+ * Detailed description of class DebugDrawerConfigWidget.
+ */
+ class DebugDrawerConfigWidget : public QWidget
+ {
+ public:
+ DebugDrawerConfigWidget(const DebugDrawerComponentPtr& debugDrawer, QWidget* parent = nullptr);
+
+ void loadSettings(QSettings* settings);
+ void saveSettings(QSettings* settings);
+
+ void setDebugDrawer(const DebugDrawerComponentPtr& debugDrawer);
+ DebugDrawerComponentPtr getDebugDrawer() const;
+
+ public slots:
+ void refresh();
+
+ void onVisibilityChanged(QListWidgetItem* item);
+
+ void onDefaultLayerVisibilityChanged(int checkState);
+ void onDefaultLayerVisibilityPerLayerChanged(QListWidgetItem* item);
+
+ void onAddDefaultLayerVisibility();
+ void onRemoveDefaultLayerVisibility();
+
+ protected:
+ DebugDrawerComponentPtr debugDrawer;
+ Ui::DebugDrawerConfigWidget ui;
+ };
+
+}
diff --git a/source/RobotAPI/libraries/DebugDrawerConfigWidget/DebugDrawerConfigWidget.ui b/source/RobotAPI/libraries/DebugDrawerConfigWidget/DebugDrawerConfigWidget.ui
new file mode 100644
index 0000000000000000000000000000000000000000..0fe625be17d0592a87153ce1b8873307bdf722f9
--- /dev/null
+++ b/source/RobotAPI/libraries/DebugDrawerConfigWidget/DebugDrawerConfigWidget.ui
@@ -0,0 +1,85 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>DebugDrawerConfigWidget</class>
+ <widget class="QWidget" name="DebugDrawerConfigWidget">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>421</width>
+ <height>236</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Form</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="0" column="0">
+ <widget class="QGroupBox" name="groupBox">
+ <property name="styleSheet">
+ <string notr="true">QGroupBox{border:2px solid gray;border-radius:5px;margin-top: 1ex;} QGroupBox::title{subcontrol-origin: margin;subcontrol-position:top center;padding:0 3px;}</string>
+ </property>
+ <property name="title">
+ <string>DebugDrawer configuration</string>
+ </property>
+ <layout class="QFormLayout" name="formLayout_2">
+ <item row="3" column="0">
+ <widget class="QLabel" name="labelLayerVisibility">
+ <property name="text">
+ <string>Layer visibility</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="0">
+ <widget class="QLabel" name="labelDefaultLayerVisibility">
+ <property name="text">
+ <string>Default layer visibility</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QListWidget" name="listWidgetLayerVisibility"/>
+ </item>
+ <item row="4" column="1">
+ <widget class="QCheckBox" name="checkBoxDefaultLayerVisibility">
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QPushButton" name="pushButtonRefresh">
+ <property name="text">
+ <string>Refresh</string>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="1">
+ <widget class="QListWidget" name="listWidgetDefaultLayerVisibility"/>
+ </item>
+ <item row="5" column="1">
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <widget class="QPushButton" name="pushButtonDefaultLayerVisibilityAdd">
+ <property name="text">
+ <string>Add</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="pushButtonDefaultLayerVisibilityRemove">
+ <property name="text">
+ <string>Remove</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/source/RobotAPI/libraries/DebugDrawerConfigWidget/test/CMakeLists.txt b/source/RobotAPI/libraries/DebugDrawerConfigWidget/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..dc326fdc7c93ba3e1bb38bd4f2316ee4aa4bc530
--- /dev/null
+++ b/source/RobotAPI/libraries/DebugDrawerConfigWidget/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore DebugDrawerConfigWidget)
+
+armarx_add_test(DebugDrawerConfigWidgetTest DebugDrawerConfigWidgetTest.cpp "${LIBS}")
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/DebugDrawerConfigWidget/test/DebugDrawerConfigWidgetTest.cpp b/source/RobotAPI/libraries/DebugDrawerConfigWidget/test/DebugDrawerConfigWidgetTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e10bd7c4b3ba362cb6ef5dab600b0660351f54d8
--- /dev/null
+++ b/source/RobotAPI/libraries/DebugDrawerConfigWidget/test/DebugDrawerConfigWidgetTest.cpp
@@ -0,0 +1,36 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::DebugDrawerConfigWidget
+ * @author Adrian Knobloch ( adrian dot knobloch at student dot kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::DebugDrawerConfigWidget
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include "../DebugDrawerConfigWidget.h"
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+
+ BOOST_CHECK_EQUAL(true, true);
+}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp
index 6ed80f25a5b54fe6de3e8d7d0e871971f82aa7cc..3bbf673c04a8e6bb1fdaf9d921ad713eeaa65bfa 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp
@@ -6,10 +6,21 @@ namespace armarx
JointKITGripperEmergencyStopController::JointKITGripperEmergencyStopController(ActorDataPtr dataPtr) :
dataPtr(dataPtr)
{
-
+ pid.reset(new PIDController(0, 5000, 0));
+ pid->maxIntegral = 0.1;
}
void JointKITGripperEmergencyStopController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
+ pid->update(timeSinceLastIteration.toSecondsDouble(), dataPtr->getVelocity(), 0.0);
+ float targetPwm = pid->getControlValue();
+ if (std::isnan(targetPwm))
+ {
+ targetPwm = 0.0f;
+ }
+ // dataPtr->setTargetPWM(targetPwm);
+ lastPWM *= 0.9997;
+ // ARMARX_RT_LOGF_INFO("old pwm %d, new pwm: %.2f", dataPtr->getTargetPWM(), lastPWM);
+ dataPtr->setTargetPWM(lastPWM);
}
@@ -21,7 +32,8 @@ namespace armarx
void JointKITGripperEmergencyStopController::rtPreActivateController()
{
// ARMARX_INFO << "Stopping gripper!";
- dataPtr->setTargetPWM(0);
+ // dataPtr->setTargetPWM(0);
+ lastPWM = math::MathUtils::LimitTo(dataPtr->getTargetPWM(), 500);
}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.h
index 043b63f39628f90f50d68ba4599403350b6c389c..df1c76a4a52997ba35bddfdd15dc7a539b537b20 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.h
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.h
@@ -1,6 +1,7 @@
#pragma once
#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include <RobotAPI/libraries/core/PIDController.h>
#include "../KITGripperBasisBoardData.h"
@@ -28,6 +29,8 @@ namespace armarx
private:
DummyControlTargetEmergencyStop target;
ActorDataPtr dataPtr;
+ PIDControllerPtr pid;
+ float lastPWM = 0.0f;
};
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5e5441eaaf750affbce122d8f65a24fe94a7262f
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.cpp
@@ -0,0 +1,29 @@
+#include "JointKITGripperPWMPassThroughController.h"
+#include <boost/algorithm/clamp.hpp>
+#include "../KITGripperBasisBoard.h"
+
+namespace armarx
+{
+ ControlTargetBase* JointKITGripperPWMPassThroughController::getControlTarget()
+ {
+ return ⌖
+ }
+
+ JointKITGripperPWMPassThroughController::JointKITGripperPWMPassThroughController(const std::string deviceName, ActorDataPtr jointData) :
+ jointData(jointData)
+ {}
+
+ void JointKITGripperPWMPassThroughController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ if (target.isValid())
+ {
+ jointData->setTargetPWM(target.current * 1000);
+ }
+ }
+
+ void JointKITGripperPWMPassThroughController::rtPreActivateController()
+ {
+ jointData->setTargetPWM(0);
+
+ }
+}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.h
new file mode 100644
index 0000000000000000000000000000000000000000..9a367bfec1e2af2d9d4223b0246e2f91d35c734f
--- /dev/null
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.h
@@ -0,0 +1,31 @@
+#pragma once
+
+#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
+#include "../KITGripperBasisBoardData.h"
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+
+namespace armarx
+{
+
+ class JointKITGripperPWMPassThroughController;
+ typedef std::shared_ptr<JointKITGripperPWMPassThroughController> JointKITGripperPWMPassThroughControllerPtr;
+
+ class JointKITGripperPWMPassThroughController : public armarx::JointController
+ {
+ public:
+ JointKITGripperPWMPassThroughController(const std::string deviceName, ActorDataPtr jointData);
+
+ // JointController interface
+ ControlTargetBase* getControlTarget() override;
+ protected:
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ private:
+ ControlTarget1DoFActuatorCurrent target;
+ ActorDataPtr jointData;
+
+ // JointController interface
+ protected:
+ void rtPreActivateController() override;
+ };
+
+}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
index 2daabdda37d71cd46eb11e48fcbadddebfd432f7..e4bf73b8cad298cb7909024d8d2b18f6452e90b7 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
@@ -5,7 +5,7 @@
#include "../KITGripperBasisBoard.h"
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
#include <ArmarXCore/core/ManagedIceObject.h>
-
+#include <boost/algorithm/clamp.hpp>
#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
@@ -14,17 +14,21 @@
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
+#include <ArmarXCore/observers/filters/rtfilters/AverageFilter.h>
+
using namespace armarx;
JointPWMPositionController::JointPWMPositionController(const std::string deviceName, KITGripperBasisBoardPtr board,
ActorDataPtr jointData,
- PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr) : JointController(),
- config(velocityControllerConfigDataPtr),
- controller(velocityControllerConfigDataPtr),
+ PWMPositionControllerConfigurationCPtr positionControllerConfigDataPtr) : JointController(),
+ config(positionControllerConfigDataPtr),
+ // controller(positionControllerConfigDataPtr),
target(), board(board), deviceName(deviceName)
{
actorIndex = board->getActorIndex(deviceName);
+ sensorValue = board->getDevices().at(actorIndex)->getSensorValue()->asA<SensorValue1DoFActuator>();
+ ARMARX_CHECK_EXPRESSION_W_HINT(sensorValue, deviceName);
dataPtr = jointData;
// add limitless joint controller to solve the flipping sensor value problem
@@ -43,18 +47,25 @@ JointPWMPositionController::JointPWMPositionController(const std::string deviceN
// ARMARX_INFO << "maxDt: " << limitlessController.maxDt << ", maxV: " << limitlessController.maxV;
// ARMARX_INFO << "acceleration: " << limitlessController.acceleration << ", deceleration: " << limitlessController.deceleration;
- posController.acceleration = velocityControllerConfigDataPtr->maxAccelerationRad;
- posController.deceleration = velocityControllerConfigDataPtr->maxDecelerationRad;
- posController.maxDt = velocityControllerConfigDataPtr->maxDt;
- posController.maxV = velocityControllerConfigDataPtr->maxVelocityRad;
+ posController.desiredDeceleration = positionControllerConfigDataPtr->maxDecelerationRad;
+ posController.desiredJerk = 100;
+ posController.maxDt = positionControllerConfigDataPtr->maxDt;
+ posController.maxV = positionControllerConfigDataPtr->maxVelocityRad;
+ posController.p = 4;
+ posController.phase2SwitchDistance = 0.1;
ARMARX_CHECK_GREATER_EQUAL(jointData->getSoftLimitHi(), jointData->getSoftLimitLo());
// controller.positionLimitHiHard = dataPtr->getHardLimitHi();
- posController.positionLimitHi = jointData->getSoftLimitHi();
+ // posController.positionLimitHi = jointData->getSoftLimitHi();
// controller.positionLimitLoHard = dataPtr->getHardLimitLo();
- posController.positionLimitLo = jointData->getSoftLimitLo();
- posController.p = velocityControllerConfigDataPtr->posControlP; //3.0f;
+ // posController.positionLimitLo = jointData->getSoftLimitLo();
+ // posController.pControlPosErrorLimit = 0.02f;
+ // posController.pid->Kp = posController.calculateProportionalGain();
+ // ARMARX_IMPORTANT << "position Kp " << posController.pid->Kp;
+
this->isLimitless = jointData->isLimitless();
- // ARMARX_INFO << "limitless: " << this->isLimitless;
+ pidPosController.reset(new PIDController(positionControllerConfigDataPtr->p, positionControllerConfigDataPtr->i, positionControllerConfigDataPtr->d));
+ pidPosController->maxIntegral = positionControllerConfigDataPtr->maxIntegral;
+ pidPosController->differentialFilter.reset(new rtfilters::AverageFilter(10));
}
JointPWMPositionController::~JointPWMPositionController() noexcept(true)
@@ -88,13 +99,16 @@ void JointPWMPositionController::rtRun(const IceUtil::Time& sensorValuesTimestam
}
else
{
- posController.currentPosition = currentPosition;
- posController.currentV = lastTargetVelocity;
- posController.dt = timeSinceLastIteration.toSecondsDouble();
- posController.targetPosition = target.position;
- newVel = posController.run();
- }
-
+ // posController.currentPosition = currentPosition;
+ posController.currentV = lastTargetVelocity;
+ posController.dt = timeSinceLastIteration.toSecondsDouble();
+ posController.setTargetPosition(target.position);
+ // ARMARX_CHECK_EXPRESSION(posController.validParameters());
+ auto r = posController.run();
+ posController.currentPosition = r.position;
+ posController.currentAcc = r.acceleration;
+ newVel = r.velocity;
+ }
// double newVel = posController.p * (posController.targetPosition - posController.currentPosition);
// newVel = math::MathUtils::LimitTo(newVel, posController.maxV);
// ARMARX_INFO << deactivateSpam(1) << VAROUT(newVel);
@@ -102,20 +116,42 @@ void JointPWMPositionController::rtRun(const IceUtil::Time& sensorValuesTimestam
{
newVel = 0;
}
+ pidPosController->update(timeSinceLastIteration.toSecondsDouble(), currentPosition, r.position);
+ auto targetPWM = pidPosController->getControlValue();
+ // auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration, dataPtr->getVelocity(), newVel));
+ newVel = math::MathUtils::LimitTo(newVel, posController.maxV);
+
+ float torqueFF = config->feedforwardTorqueToPWMFactor * -sensorValue->gravityTorque;
+ targetPWM += torqueFF;
+ targetPWM += newVel * config->feedforwardVelocityToPWMFactor;
- auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration, dataPtr->getVelocity(), newVel));
+ // ARMARX_INFO << deactivateSpam(0.1) << VAROUT(pidPosController->previousError) << VAROUT(r.acceleration)
+ // << VAROUT(target.position) << VAROUT(targetPWM) << VAROUT(pidPosController->Kp) << VAROUT(pidPosController->Ki)
+ // << VAROUT(torqueFF);
if (std::isnan(targetPWM))
{
- throw LocalException() << "Target PWM of " << getParent().getDeviceName() << " is NaN!";
+ ARMARX_ERROR << deactivateSpam(1) << "Target PWM of " << getParent().getDeviceName() << " is NaN!";
+ targetPWM = 0.0f;
+ }
+ float updateRatio = 0.3;
+ this->targetPWM = (1.0 - updateRatio) * this->targetPWM + updateRatio * targetPWM;
+ float pwmDiff = std::abs(dataPtr->getTargetPWM() - targetPWM);
+ // ARMARX_INFO << deactivateSpam(0.3) << VAROUT(pwmDiff) << VAROUT(dataPtr->getTargetPWM()) << VAROUT(targetPWM) << VAROUT(dataPtr->getVelocity());
+ if (pwmDiff > 5 || dataPtr->getVelocity() > 0.01) // avoid jittering when standing still
+ {
+ // ARMARX_INFO << deactivateSpam(0.0, std::to_string(targetPWM)) << "Setting new targetPWM to" << targetPWM << " diff: " << pwmDiff << " vel: " << dataPtr->getVelocity();
+ dataPtr->setTargetPWM(targetPWM);
}
- dataPtr->setTargetPWM(targetPWM);
+ this->targetPWM = targetPWM;
lastTargetVelocity = newVel;
// auto name = getParent().getDeviceName().c_str();
// ARMARX_RT_LOGF_INFO("%s: position: %.f, target position: %.f, targetvelocity: %.f, target PWM: %d", name,
- // currentPosition, target.position, newVel, targetPWM).deactivateSpam(1);
- // ARMARX_INFO << deactivateSpam(1) << VAROUT(name) << VAROUT(currentPosition) << VAROUT(target.position) << VAROUT(newVel) << VAROUT(targetPWM);
+ // currentPosition, targetPosition, newVel, targetPWM).deactivateSpam(1);
+ // ARMARX_INFO << deactivateSpam(1) << VAROUT(name) << VAROUT(currentPosition) << VAROUT(targetPosition) << VAROUT(newVel) << VAROUT(targetPWM);
+
+
}
else
{
@@ -130,14 +166,19 @@ ControlTargetBase* JointPWMPositionController::getControlTarget()
void JointPWMPositionController::rtPreActivateController()
{
+ targetPWM = 0.0f;
lastTargetVelocity = dataPtr->getVelocity();
- controller.reset(dataPtr->getVelocity());
+ posController.currentAcc = dataPtr->getAcceleration();
+ posController.currentPosition = dataPtr->getPosition();
+ posController.currentV = dataPtr->getVelocity();
+ pidPosController->reset();
+ // controller.reset(dataPtr->getVelocity());
}
void JointPWMPositionController::rtPostDeactivateController()
{
- ARMARX_RT_LOGF_INFO("Setting PWM to 0");
- dataPtr->setTargetPWM(0);
+ // ARMARX_RT_LOGF_INFO("Setting PWM to 0");
+ // dataPtr->setTargetPWM(0);
}
StringVariantBaseMap JointPWMPositionController::publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const
@@ -145,118 +186,132 @@ StringVariantBaseMap JointPWMPositionController::publish(const DebugDrawerInterf
if (!remoteGui)
{
- // threadHandle = Application::getInstance()->getThreadPool()->runTask([this]
- // {
- // std::string guiTabName;
- // while (!stopRequested)
- // {
- // ManagedIceObjectPtr object;
- // ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
- // try
- // {
- // object = ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
- // ARMARX_CHECK_EXPRESSION(object);
- // remoteGui = object->getProxy<RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
- // if (!remoteGui)
- // {
- // continue;
- // }
- // ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
- // guiTabName = getParent().getDeviceName() + getControlMode();
- // break;
- // }
- // catch (...)
- // {
- // sleep(1);
- // }
-
- // }
- // if (remoteGui)
- // {
- // ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
- // using namespace RemoteGui;
-
-
-
- // auto vLayout = makeVBoxLayout();
-
- // {
- // WidgetPtr KpLabel = makeTextLabel("Kp: ");
-
- // WidgetPtr KiSlider = makeFloatSlider("KpSlider")
- // .min(0.0f).max(5000.0f)
- // .value(config->p);
- // WidgetPtr line = makeHBoxLayout()
- // .children({KpLabel, KiSlider});
-
- // vLayout.addChild(line);
-
- // }
-
-
- // {
- // WidgetPtr KiLabel = makeTextLabel("Ki: ");
- // WidgetPtr KiSlider = makeFloatSlider("KiSlider")
- // .min(0.0f).max(50000.0f)
- // .value(config->i);
-
- // WidgetPtr line = makeHBoxLayout()
- // .children({KiLabel, KiSlider});
-
- // vLayout.addChild(line);
-
- // }
-
- // {
- // WidgetPtr KdLabel = makeTextLabel("Kd: ");
- // WidgetPtr KdSlider = makeFloatSlider("KdSlider")
- // .min(0.0f).max(50.0f)
- // .steps(100)
- // .value(config->d);
-
- // WidgetPtr line = makeHBoxLayout()
- // .children({KdLabel, KdSlider});
-
- // vLayout.addChild(line);
- // vLayout.addChild(new VSpacer);
- // }
-
- // // WidgetPtr spin = makeFloatSpinBox("KpSpin")
- // // .min(0.0f).max(2.0f)
- // // .steps(20).decimals(2)
- // // .value(0.4f);
-
-
-
-
- // WidgetPtr groupBox = makeGroupBox("GroupBox")
- // .label("Group")
- // .child(vLayout);
-
- // remoteGui->createTab(guiTabName, groupBox);
-
- // while (!stopRequested)
- // {
- // RemoteGui::TabProxy tab(remoteGui, guiTabName);
- // tab.receiveUpdates();
- // this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
- // this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
- // this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
- // usleep(100000);
- // }
- // }
-
- // });
+ threadHandle = Application::getInstance()->getThreadPool()->runTask([this]
+ {
+ std::string guiTabName;
+ while (!stopRequested)
+ {
+ ManagedIceObjectPtr object;
+ ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
+ try
+ {
+ object = ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
+ ARMARX_CHECK_EXPRESSION(object);
+ remoteGui = object->getProxy<RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
+ if (!remoteGui)
+ {
+ continue;
+ }
+ ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
+ guiTabName = getParent().getDeviceName() + getControlMode();
+ break;
+ }
+ catch (...)
+ {
+ sleep(1);
+ }
+
+ }
+ if (remoteGui)
+ {
+ ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
+ using namespace RemoteGui;
+
+
+
+ auto vLayout = makeVBoxLayout();
+
+ {
+ WidgetPtr KpLabel = makeTextLabel("Kp: ");
+
+ WidgetPtr KpSlider = makeFloatSlider("KpSlider")
+ .min(0.0f).max(pidPosController->Kp * 5)
+ .value(pidPosController->Kp);
+ WidgetPtr KpLabelValue = makeTextLabel(std::to_string(pidPosController->Kp * 5));
+ WidgetPtr line = makeHBoxLayout()
+ .children({KpLabel, KpSlider, KpLabelValue});
+
+ vLayout.addChild(line);
+
+ }
+
+
+ {
+ WidgetPtr KiLabel = makeTextLabel("Ki: ");
+ WidgetPtr KiSlider = makeFloatSlider("KiSlider")
+ .min(0.0f).max(pidPosController->Ki * 5)
+ .value(pidPosController->Ki);
+ WidgetPtr KiLabelValue = makeTextLabel(std::to_string(pidPosController->Ki * 5));
+
+ WidgetPtr line = makeHBoxLayout()
+ .children({KiLabel, KiSlider, KiLabelValue});
+
+ vLayout.addChild(line);
+
+ }
+
+ {
+ WidgetPtr KdLabel = makeTextLabel("Kd: ");
+ WidgetPtr KdSlider = makeFloatSlider("KdSlider")
+ .min(-10.0f * pidPosController->Kd).max(10.0f * pidPosController->Kd)
+ .steps(1000)
+ .value(pidPosController->Kd);
+ WidgetPtr KdLabelValue = makeTextLabel(std::to_string(pidPosController->Kd * 10));
+
+ WidgetPtr line = makeHBoxLayout()
+ .children({KdLabel, KdSlider, KdLabelValue});
+
+ vLayout.addChild(line);
+ vLayout.addChild(new VSpacer);
+ }
+
+ // WidgetPtr spin = makeFloatSpinBox("KpSpin")
+ // .min(0.0f).max(2.0f)
+ // .steps(20).decimals(2)
+ // .value(0.4f);
+
+
+
+
+ WidgetPtr groupBox = makeGroupBox("GroupBox")
+ .label("Group")
+ .child(vLayout);
+
+ remoteGui->createTab(guiTabName, groupBox);
+
+ while (!stopRequested)
+ {
+ RemoteGui::TabProxy tab(remoteGui, guiTabName);
+ tab.receiveUpdates();
+ // this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
+ // this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
+ // this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
+ pidPosController->Kp = tab.getValue<float>("KpSlider").get();
+ pidPosController->Ki = tab.getValue<float>("KiSlider").get();
+ pidPosController->Kd = tab.getValue<float>("KdSlider").get();
+ usleep(100000);
+ }
+ }
+
+ });
}
return {{"lastTargetVelocity", new Variant(lastTargetVelocity.load())},
- {"targetPosition", new Variant(posController.targetPosition)},
- {"posError", new Variant(posController.targetPosition - posController.currentPosition)},
- {"pidError", new Variant(controller.pid->previousError)},
- {"filteredVelocity", new Variant(controller.lastActualVelocity.load())},
- {"pidIntegralCV", new Variant(controller.pid->integral * controller.pid->Ki)},
- {"pidPropCV", new Variant(controller.pid->previousError * controller.pid->Kp)},
- {"pidDiffCV", new Variant(controller.pid->derivative * controller.pid->Kd)},
- {"pidUsed", new Variant(posController.getCurrentlyPIDActive())}
+ {"targetPosition", new Variant(posController.currentPosition)}, // position of profile generator is target position
+ {"posError", new Variant(posController.getTargetPosition() - posController.currentPosition)},
+ {"pidError", new Variant(pidPosController->previousError)},
+ // {"filteredVelocity", new Variant(controller.lastActualVelocity.load())},
+ {"pidIntegralCV", new Variant(pidPosController->integral * pidPosController->Ki)},
+ {"pidIntegral", new Variant(pidPosController->integral)},
+ {"pidPropCV", new Variant(pidPosController->previousError * pidPosController->Kp)},
+ {"pidDiffCV", new Variant(pidPosController->derivative * pidPosController->Kd)},
+ // {"pospidIntegralCV", new Variant(posController.pid->integral * posController.pid->Ki)},
+ // {"pospidIntegral", new Variant(posController.pid->integral)},
+ // {"pospidPropCV", new Variant(posController.pid->previousError * posController.pid->Kp)},
+ // {"pospidDiffCV", new Variant(posController.pid->derivative * posController.pid->Kd)},
+ // {"pidUsed", new Variant(posController.getCurrentlyPIDActive())},
+ {"desiredPWM", new Variant(targetPWM.load())}
+
+
};
}
@@ -264,3 +319,25 @@ StringVariantBaseMap JointPWMPositionController::publish(const DebugDrawerInterf
+
+PWMPositionControllerConfigurationCPtr PWMPositionControllerConfiguration::CreatePWMPositionControllerConfigDataFromXml(DefaultRapidXmlReaderNode node)
+{
+ PWMPositionControllerConfiguration configData;
+
+ configData.maxVelocityRad = node.first_node("maxVelocityRad").value_as_float();
+ configData.maxAccelerationRad = node.first_node("maxAccelerationRad").value_as_float();
+ configData.maxDecelerationRad = node.first_node("maxDecelerationRad").value_as_float();
+ configData.maxDt = node.first_node("maxDt").value_as_float();
+ configData.p = node.first_node("p").value_as_float();
+ configData.i = node.first_node("i").value_as_float();
+ configData.d = node.first_node("d").value_as_float();
+ configData.maxIntegral = node.first_node("maxIntegral").value_as_float();
+ configData.feedforwardVelocityToPWMFactor = node.first_node("feedforwardVelocityToPWMFactor").value_as_float();
+ configData.feedforwardTorqueToPWMFactor = node.first_node("feedforwardTorqueToPWMFactor").value_as_float();
+ configData.PWMDeadzone = node.first_node("PWMDeadzone").value_as_float();
+ configData.velocityUpdatePercent = node.first_node("velocityUpdatePercent").value_as_float();
+ configData.conditionalIntegralErrorTreshold = node.first_node("conditionalIntegralErrorTreshold").value_as_float();
+ configData.feedForwardMode = node.first_node("FeedForwardMode").value_as_bool("1", "0");
+
+ return std::make_shared<PWMPositionControllerConfiguration>(configData);
+}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.h
index 62818578f3ba3db58f0158fc3e330d2d31f67a36..d409d57690acf87aca4164892b02b4888a4ec2eb 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.h
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.h
@@ -19,6 +19,30 @@
namespace armarx
{
using KITGripperBasisBoardPtr = std::shared_ptr<class KITGripperBasisBoard>;
+ typedef std::shared_ptr<class PWMPositionControllerConfiguration> PWMPositionControllerConfigurationPtr;
+ typedef std::shared_ptr<const PWMPositionControllerConfiguration> PWMPositionControllerConfigurationCPtr;
+
+
+ class PWMPositionControllerConfiguration
+ {
+ public:
+ PWMPositionControllerConfiguration() {}
+ static PWMPositionControllerConfigurationCPtr CreatePWMPositionControllerConfigDataFromXml(DefaultRapidXmlReaderNode node);
+ float maxVelocityRad;
+ float maxAccelerationRad;
+ float maxDecelerationRad;
+ float maxDt;
+ float p;
+ float i;
+ float d;
+ float maxIntegral;
+ float feedforwardVelocityToPWMFactor;
+ float feedforwardTorqueToPWMFactor;
+ float PWMDeadzone;
+ float velocityUpdatePercent;
+ float conditionalIntegralErrorTreshold;
+ bool feedForwardMode;
+ };
class JointPWMPositionController;
@@ -27,7 +51,7 @@ namespace armarx
class JointPWMPositionController : public JointController
{
public:
- JointPWMPositionController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData, PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr);
+ JointPWMPositionController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData, PWMPositionControllerConfigurationCPtr positionControllerConfigDataPtr);
~JointPWMPositionController() noexcept(true);
void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
@@ -35,14 +59,15 @@ namespace armarx
void rtPreActivateController() override;
protected:
- PWMVelocityControllerConfigurationCPtr config;
- PWMVelocityController controller;
- PositionThroughVelocityControllerWithAccelerationAndPositionBounds posController;
+ PWMPositionControllerConfigurationCPtr config;
+ // PWMVelocityController controller;
+ // PositionThroughVelocityControllerWithAccelerationAndPositionBounds posController;
+ MinJerkPositionController posController;
+ PIDControllerPtr pidPosController;
PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition limitlessController;
-
ControlTarget1DoFActuatorPosition target;
- std::atomic<double> lastTargetVelocity;
+ std::atomic<double> lastTargetVelocity, targetPWM;
bool isLimitless;
ActorDataPtr dataPtr;
@@ -52,6 +77,7 @@ namespace armarx
mutable RemoteGuiInterfacePrx remoteGui;
bool stopRequested = false;
mutable ThreadPool::Handle threadHandle;
+ const SensorValue1DoFActuator* sensorValue;
// JointController interface
protected:
void rtPostDeactivateController() override;
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp
index 6f289e055d87d9a8894ac8e2ad9c945f16b0fba6..67da106534cb1255002b9d88197ea2f4854ace72 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp
@@ -24,10 +24,13 @@ JointPWMVelocityController::JointPWMVelocityController(const std::string deviceN
target(), board(board), deviceName(deviceName)
{
actorIndex = board->getActorIndex(deviceName);
+ sensorValue = board->getDevices().at(actorIndex)->getSensorValue()->asA<SensorValue1DoFActuator>();
+ ARMARX_CHECK_EXPRESSION_W_HINT(sensorValue, deviceName);
dataPtr = jointData;
- velController.acceleration = velocityControllerConfigDataPtr->maxAccelerationRad;
+ // velController.acceleration = velocityControllerConfigDataPtr->maxAccelerationRad;
velController.deceleration = velocityControllerConfigDataPtr->maxDecelerationRad;
+ velController.jerk = 30;
velController.maxDt = velocityControllerConfigDataPtr->maxDt;
velController.maxV = velocityControllerConfigDataPtr->maxVelocityRad;
velController.directSetVLimit = velocityControllerConfigDataPtr->directSetVLimit;
@@ -69,27 +72,31 @@ void JointPWMVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestam
velController.currentPosition = currentPosition;
}
velController.currentV = lastTargetVelocity;
+ velController.currentAcc = lastTargetAcceleration;
velController.dt = timeSinceLastIteration.toSecondsDouble();
velController.targetV = target.velocity;
- double newVel = velController.run();
- ARMARX_DEBUG << deactivateSpam(1) << VAROUT(newVel) << VAROUT(target.velocity);
+ auto r = velController.run();
+ double newVel = r.velocity;
+ double newAcc = r.acceleration;
if (std::isnan(newVel))
{
newVel = 0;
+ newAcc = 0;
}
if ((currentPosition > velController.positionLimitHiSoft && target.velocity > 0)
|| (currentPosition < velController.positionLimitLoSoft && target.velocity < 0))
{
newVel = 0;
+ newAcc = 0;
ARMARX_INFO << deactivateSpam(1) << "Breaking now at " << dataPtr->getPosition() << " pwm: " << dataPtr->getTargetPWM();
}
- auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration, dataPtr->getVelocity(), newVel));
- ARMARX_DEBUG << deactivateSpam(1) << "Target PWM: " << targetPWM;
+ auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration, dataPtr->getVelocity(), newVel, sensorValue->gravityTorque));
dataPtr->setTargetPWM(targetPWM);
lastTargetVelocity = newVel;
+ lastTargetAcceleration = newAcc;
// ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, max acc: %.3f",
// target.velocity, dataPtr->getVelocity(), targetPWM, pid->Kp, pid->Ki, pid->Kd, controller.acceleration).deactivateSpam(1);
@@ -110,13 +117,14 @@ ControlTargetBase* JointPWMVelocityController::getControlTarget()
void JointPWMVelocityController::rtPreActivateController()
{
lastTargetVelocity = dataPtr->getVelocity();
+ lastTargetAcceleration = dataPtr->getAcceleration();
controller.reset(dataPtr->getVelocity());
}
void JointPWMVelocityController::rtPostDeactivateController()
{
- ARMARX_RT_LOGF_INFO("Setting PWM to 0");
- dataPtr->setTargetPWM(0);
+ // ARMARX_RT_LOGF_INFO("Setting PWM to 0");
+ // dataPtr->setTargetPWM(0);
}
StringVariantBaseMap JointPWMVelocityController::publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const
@@ -124,111 +132,114 @@ StringVariantBaseMap JointPWMVelocityController::publish(const DebugDrawerInterf
if (!remoteGui && !threadHandle.isValid())
{
- // threadHandle = Application::getInstance()->getThreadPool()->runTask([this]
- // {
- // std::string guiTabName;
- // while (!stopRequested)
- // {
- // ManagedIceObjectPtr object;
- // ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
- // try
- // {
- // object = ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
- // ARMARX_CHECK_EXPRESSION(object);
- // remoteGui = object->getProxy<RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
- // if (!remoteGui)
- // {
- // return;
- // }
- // ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
- // guiTabName = getParent().getDeviceName() + getControlMode();
- // break;
- // }
- // catch (...)
- // {
- // handleExceptions();
- // sleep(1);
- // }
-
- // }
- // if (remoteGui)
- // {
- // ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
- // using namespace RemoteGui;
-
-
-
- // auto vLayout = makeVBoxLayout();
-
- // {
- // WidgetPtr KpLabel = makeTextLabel("Kp: ");
-
- // WidgetPtr KiSlider = makeFloatSlider("KpSlider")
- // .min(0.0f).max(5000.0f)
- // .value(config->p);
- // WidgetPtr line = makeHBoxLayout()
- // .children({KpLabel, KiSlider});
-
- // vLayout.addChild(line);
-
- // }
-
-
- // {
- // WidgetPtr KiLabel = makeTextLabel("Ki: ");
- // WidgetPtr KiSlider = makeFloatSlider("KiSlider")
- // .min(0.0f).max(50000.0f)
- // .value(config->i);
-
- // WidgetPtr line = makeHBoxLayout()
- // .children({KiLabel, KiSlider});
-
- // vLayout.addChild(line);
-
- // }
-
- // {
- // WidgetPtr KdLabel = makeTextLabel("Kd: ");
- // WidgetPtr KdSlider = makeFloatSlider("KdSlider")
- // .min(0.0f).max(50.0f)
- // .steps(100)
- // .value(config->d);
-
- // WidgetPtr line = makeHBoxLayout()
- // .children({KdLabel, KdSlider});
-
- // vLayout.addChild(line);
- // vLayout.addChild(new VSpacer);
- // }
-
- // // WidgetPtr spin = makeFloatSpinBox("KpSpin")
- // // .min(0.0f).max(2.0f)
- // // .steps(20).decimals(2)
- // // .value(0.4f);
-
-
-
-
- // WidgetPtr groupBox = makeGroupBox("GroupBox")
- // .label("Group")
- // .child(vLayout);
-
- // remoteGui->createTab(guiTabName, groupBox);
-
- // while (!stopRequested)
- // {
- // RemoteGui::TabProxy tab(remoteGui, guiTabName);
- // tab.receiveUpdates();
- // this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
- // this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
- // this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
- // usleep(100000);
- // }
- // }
-
- // });
+ threadHandle = Application::getInstance()->getThreadPool()->runTask([this]
+ {
+ std::string guiTabName;
+ while (!stopRequested)
+ {
+ ManagedIceObjectPtr object;
+ ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
+ try
+ {
+ object = ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
+ ARMARX_CHECK_EXPRESSION(object);
+ remoteGui = object->getProxy<RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
+ if (!remoteGui)
+ {
+ return;
+ }
+ ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
+ guiTabName = getParent().getDeviceName() + getControlMode();
+ break;
+ }
+ catch (...)
+ {
+ handleExceptions();
+ sleep(1);
+ }
+
+ }
+ if (remoteGui)
+ {
+ ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
+ using namespace RemoteGui;
+
+
+
+ auto vLayout = makeVBoxLayout();
+
+ {
+ WidgetPtr KpLabel = makeTextLabel("Kp: ");
+
+ WidgetPtr KiSlider = makeFloatSlider("KpSlider")
+ .min(0.0f).max(5000.0f)
+ .value(config->p);
+ WidgetPtr line = makeHBoxLayout()
+ .children({KpLabel, KiSlider});
+
+ vLayout.addChild(line);
+
+ }
+
+
+ {
+ WidgetPtr KiLabel = makeTextLabel("Ki: ");
+ WidgetPtr KiSlider = makeFloatSlider("KiSlider")
+ .min(0.0f).max(50000.0f)
+ .value(config->i);
+
+ WidgetPtr line = makeHBoxLayout()
+ .children({KiLabel, KiSlider});
+
+ vLayout.addChild(line);
+
+ }
+
+ {
+ WidgetPtr KdLabel = makeTextLabel("Kd: ");
+ WidgetPtr KdSlider = makeFloatSlider("KdSlider")
+ .min(0.0f).max(50.0f)
+ .steps(100)
+ .value(config->d);
+
+ WidgetPtr line = makeHBoxLayout()
+ .children({KdLabel, KdSlider});
+
+ vLayout.addChild(line);
+ vLayout.addChild(new VSpacer);
+ }
+
+ // WidgetPtr spin = makeFloatSpinBox("KpSpin")
+ // .min(0.0f).max(2.0f)
+ // .steps(20).decimals(2)
+ // .value(0.4f);
+
+
+
+
+ WidgetPtr groupBox = makeGroupBox("GroupBox")
+ .label("Group")
+ .child(vLayout);
+
+ remoteGui->createTab(guiTabName, groupBox);
+
+ while (!stopRequested)
+ {
+ RemoteGui::TabProxy tab(remoteGui, guiTabName);
+ tab.receiveUpdates();
+ this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
+ this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
+ this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
+ usleep(100000);
+ }
+ }
+
+ });
}
- return {{"lastTargetVelocity", new Variant(lastTargetVelocity.load())},
+ return
+ {
+ {"lastTargetVelocity", new Variant(lastTargetVelocity.load())},
+ {"lastTargetAcceleration", new Variant(lastTargetAcceleration.load())},
{"filteredVelocity", new Variant(controller.lastActualVelocity.load())},
{"pidIntegralCV", new Variant(controller.pid->integral * controller.pid->Ki)},
{"pidPropCV", new Variant(controller.pid->previousError * controller.pid->Kp)},
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.h
index 34312460d248e73648bd1d4f94b5f51ffe595932..d54e3b7dfa70b014c529c9e4b19b43b6c81e2ba2 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.h
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.h
@@ -37,11 +37,11 @@ namespace armarx
protected:
PWMVelocityControllerConfigurationCPtr config;
PWMVelocityController controller;
- VelocityControllerWithAccelerationAndPositionBounds velController;
+ VelocityControllerWithRampedAccelerationAndPositionBounds velController;
ControlTarget1DoFActuatorVelocity target;
- std::atomic<double> lastTargetVelocity;
+ std::atomic<double> lastTargetVelocity, lastTargetAcceleration;
bool isLimitless;
ActorDataPtr dataPtr;
@@ -51,6 +51,7 @@ namespace armarx
mutable RemoteGuiInterfacePrx remoteGui;
bool stopRequested = false;
mutable ThreadPool::Handle threadHandle;
+ const SensorValue1DoFActuator* sensorValue;
// JointController interface
protected:
void rtPostDeactivateController() override;
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.cpp
index d5451bbd65d0a5acd6a761d891304e364c895223..2b83bac93f9102bce75baec4b05850ee8c91da4f 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.cpp
@@ -22,7 +22,9 @@
* GNU General Public License
*/
#include "PWMVelocityController.h"
+
#include <ArmarXCore/observers/filters/rtfilters/AverageFilter.h>
+
namespace armarx
{
@@ -35,13 +37,14 @@ namespace armarx
pid.reset(new PIDController(velocityControllerConfigDataPtr->p,
velocityControllerConfigDataPtr->i,
velocityControllerConfigDataPtr->d));
+ pid->pdOutputFilter.reset(new rtfilters::AverageFilter(10));
pid->maxIntegral = velocityControllerConfigDataPtr->maxIntegral;
pid->differentialFilter.reset(new rtfilters::AverageFilter(130));
pid->conditionalIntegralErrorTreshold = velocityControllerConfigDataPtr->conditionalIntegralErrorTreshold;
pid->threadSafe = false;
}
- double PWMVelocityController::run(IceUtil::Time const& deltaT, double currentVelocity, double targetVelocity)
+ double PWMVelocityController::run(IceUtil::Time const& deltaT, double currentVelocity, double targetVelocity, double gravityTorque)
{
double targetPWM = 0;
if (!this->config->feedForwardMode)
@@ -50,13 +53,15 @@ namespace armarx
pid->update(deltaT.toSecondsDouble(), lastActualVelocity, targetVelocity);
targetPWM = pid->getControlValue();
}
- // TODO: add feedforward term based on gravity
+ float torqueFF = config->feedforwardTorqueToPWMFactor * -gravityTorque;
+ // ARMARX_INFO << deactivateSpam(1) << VAROUT(torqueFF);
+ targetPWM += torqueFF;
//feed forward
- if (std::abs(targetVelocity) > 0.001)
+ if (std::abs(targetVelocity) > 0.001 && std::abs(currentVelocity) < 0.0001f)
{
- // targetPWM += config->PWMDeadzone * math::MathUtils::Sign(targetVelocity); // deadzone
+ targetPWM += config->PWMDeadzone * math::MathUtils::Sign(targetVelocity); // deadzone
}
targetPWM += config->feedforwardVelocityToPWMFactor * targetVelocity; // approx. feedforward vel
@@ -85,13 +90,13 @@ namespace armarx
configData.maxAccelerationRad = node.first_node("maxAccelerationRad").value_as_float();
configData.maxDecelerationRad = node.first_node("maxDecelerationRad").value_as_float();
configData.maxDt = node.first_node("maxDt").value_as_float();
- configData.maxDt = node.first_node("directSetVLimit").value_as_float();
- configData.posControlP = node.first_node("posControlP").value_as_float();
+ configData.directSetVLimit = node.first_node("directSetVLimit").value_as_float();
configData.p = node.first_node("p").value_as_float();
configData.i = node.first_node("i").value_as_float();
configData.d = node.first_node("d").value_as_float();
configData.maxIntegral = node.first_node("maxIntegral").value_as_float();
configData.feedforwardVelocityToPWMFactor = node.first_node("feedforwardVelocityToPWMFactor").value_as_float();
+ configData.feedforwardTorqueToPWMFactor = node.first_node("feedforwardTorqueToPWMFactor").value_as_float();
configData.PWMDeadzone = node.first_node("PWMDeadzone").value_as_float();
configData.velocityUpdatePercent = node.first_node("velocityUpdatePercent").value_as_float();
configData.conditionalIntegralErrorTreshold = node.first_node("conditionalIntegralErrorTreshold").value_as_float();
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.h
index 2dc6b7e80eb99a73be932434f6b7303532238c30..f85e19c84520e8647fabefc61cd23bc3652c3d13 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.h
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.h
@@ -54,6 +54,7 @@ namespace armarx
float d;
float maxIntegral;
float feedforwardVelocityToPWMFactor;
+ float feedforwardTorqueToPWMFactor;
float PWMDeadzone;
float velocityUpdatePercent;
float conditionalIntegralErrorTreshold;
@@ -64,7 +65,7 @@ namespace armarx
{
public:
PWMVelocityController(PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr);
- double run(IceUtil::Time const& deltaT, double currentVelocity, double targetVelocity);
+ double run(IceUtil::Time const& deltaT, double currentVelocity, double targetVelocity, double gravityTorque);
void reset(double currentVelocity);
PWMVelocityControllerConfigurationCPtr config;
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp
index 969260fe52c7fdf638867de452f3738dba5b159d..24acd829759500298faa531292bd6a917367e784 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp
@@ -20,8 +20,8 @@ using namespace armarx;
ParallelGripperPositionController::ParallelGripperPositionController(const std::string deviceName, KITGripperBasisBoardPtr board,
ActorDataPtr jointData,
- PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr) :
- JointPWMPositionController(deviceName, board, jointData, velocityControllerConfigDataPtr)
+ PWMPositionControllerConfigurationCPtr positionControllerConfigDataPtr) :
+ JointPWMPositionController(deviceName, board, jointData, positionControllerConfigDataPtr)
{
linkedJointConnectorIndex = jointData->getSiblingControlActorIndex();
}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.h
index ab776fa914b63e0212410c47aabe7f43e4e9606d..b645f126bc7c0e6815bcdf0c461dde2efdb70f16 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.h
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.h
@@ -28,7 +28,7 @@ namespace armarx
class ParallelGripperPositionController : public JointPWMPositionController
{
public:
- ParallelGripperPositionController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData, PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr);
+ ParallelGripperPositionController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData, PWMPositionControllerConfigurationCPtr positionControllerConfigDataPtr);
~ParallelGripperPositionController() noexcept(true);
void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp
index c40b666578f08e78283124a9193f653dd9ff62b9..834674e21c541026e61538d2ebb9be7a2b04119a 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp
@@ -31,6 +31,7 @@
#include "JointController/JointKITGripperEmergencyStopController.h"
#include "JointController/JointKITGripperStopMovementController.h"
#include "JointController/JointPWMVelocityController.h"
+#include "JointController/JointKITGripperPWMPassThroughController.h"
#include "JointController/ParallelGripperPositionController.h"
#include "JointController/ParallelGripperVelocityController.h"
namespace armarx
@@ -117,6 +118,7 @@ namespace armarx
dataPtr->rtReadSensorValues(sensorValuesTimestamp, timeSinceLastIteration);
// TODO: read IMU
+ sensorValue.IMUTemperature = dataPtr->getIMUTemperature();
}
// void KITGripperBasisBoard::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
@@ -167,6 +169,10 @@ namespace armarx
addJointController(emergencyController.get());
stopMovementController.reset(new JointKITGripperStopMovementController(actorDataPtr));
addJointController(stopMovementController.get());
+ auto positionControllerCfg = PWMPositionControllerConfiguration::CreatePWMPositionControllerConfigDataFromXml(
+ defaultConfigurationNode.first_node("JointPWMPositionControllerDefaultConfiguration")
+ .add_node_at_end(configNode.first_node("JointPWMPositionControllerConfig")));
+
auto velocityControllerCfg = PWMVelocityControllerConfiguration::CreatePWMVelocityControllerConfigDataFromXml(
defaultConfigurationNode.first_node("JointPWMVelocityControllerDefaultConfiguration")
.add_node_at_end(configNode.first_node("JointPWMVelocityControllerConfig")));
@@ -188,6 +194,16 @@ namespace armarx
// add_node_at_end(configNode);
// parallelGripperDecouplingFactor = tempConfigNode.first_node("ParallelGripperDecoupplingFactor").value_as_float();
+ pwmController.reset(new JointKITGripperPWMPassThroughController(getDeviceName(), actorDataPtr));
+ addJointController(pwmController.get());
+ // ARMARX_CHECK_EQUAL_W_HINT(
+ // configNode.has_node("ParallelGripperDecoupplingFactor"),
+ // configNode.has_node("SiblingConnectorId"),
+ // "Either both or none have to be set.");
+ // auto tempConfigNode = defaultConfigurationNode.first_node("KITGripperActorDefaultConfiguration").
+ // add_node_at_end(configNode);
+ // parallelGripperDecouplingFactor = tempConfigNode.first_node("ParallelGripperDecoupplingFactor").value_as_float();
+
// if (configNode.has_node("ParallelGripperDecoupplingFactor"))
// {
// const int siblingConnectorId = configNode.first_node("SiblingConnectorId").value_as_int32();
@@ -221,7 +237,7 @@ namespace armarx
}
if (actorDataPtr->getPositionControlEnabled())
{
- positionController.reset(new ParallelGripperPositionController(getDeviceName(), dev, actorDataPtr, velocityControllerCfg));
+ positionController.reset(new ParallelGripperPositionController(getDeviceName(), dev, actorDataPtr, positionControllerCfg));
addJointController(positionController.get());
}
else
@@ -248,7 +264,7 @@ namespace armarx
addJointController(zeroTorqueController.get());
if (actorDataPtr->getPositionControlEnabled())
{
- positionController.reset(new JointPWMPositionController(getDeviceName(), dev, actorDataPtr, velocityControllerCfg));
+ positionController.reset(new JointPWMPositionController(getDeviceName(), dev, actorDataPtr, positionControllerCfg));
addJointController(positionController.get());
}
else
@@ -282,7 +298,7 @@ namespace armarx
sensorValue.position = actorDataPtr->getPosition();
sensorValue.relativePosition = actorDataPtr->getRelativePosition();
sensorValue.velocity = actorDataPtr->getVelocity();
- // TODO add new entry for acceleration
+ sensorValue.acceleration = actorDataPtr->getAcceleration();
sensorValue.absoluteEncoderVelocity = actorDataPtr->getAbsoluteEncoderVelocity();
sensorValue.targetPWM = actorDataPtr->getTargetPWM();
sensorValue.motorCurrent = actorDataPtr->getTargetPWM();
@@ -300,6 +316,7 @@ namespace armarx
void KITGripperBasisBoard::ActorRobotUnitDevice::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
+
}
} // namespace
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.h
index a345164e85ff9b8c512ae766a0898a52764bc541..6ce753478c4b190efe3f05f4507bb89c3318f5c8 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.h
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.h
@@ -43,6 +43,7 @@ namespace armarx
using JointPWMPositionControllerPtr = std::shared_ptr<class JointPWMPositionController>;
using JointPWMTorqueControllerPtr = std::shared_ptr<class JointPWMTorqueController>;
using JointPWMZeroTorqueControllerPtr = std::shared_ptr<class JointPWMZeroTorqueController>;
+ using JointKITGripperPWMPassThroughControllerPtr = std::shared_ptr<class JointKITGripperPWMPassThroughController>;
using ParallelGripperPositionControllerPtr = std::shared_ptr<class ParallelGripperPositionController>;
using ParallelGripperVelocityControllerPtr = std::shared_ptr<class ParallelGripperVelocityController>;
using PWMVelocityControllerConfigurationPtr = std::shared_ptr<class PWMVelocityControllerConfiguration>;
@@ -84,6 +85,7 @@ namespace armarx
JointPWMPositionControllerPtr positionController;
JointPWMTorqueControllerPtr torqueController;
JointPWMZeroTorqueControllerPtr zeroTorqueController;
+ JointKITGripperPWMPassThroughControllerPtr pwmController;
/// The data object for copying to non-rt part
KITGripperActorSensorData sensorValue;
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
index 6b31205aa9679cec06ed5d5baa05837e788f9c96..8f85a09ffc983f917e0070220b3783257272dc6b 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
@@ -115,6 +115,7 @@ namespace armarx
void KITGripperBasisBoardData::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
int i = 0;
+ double dt = timeSinceLastIteration.toSecondsDouble();
for (auto& ptr : actorData)
{
if (!ptr)
@@ -230,12 +231,14 @@ namespace armarx
// d.torque.read();
d.currentMaxPWM = std::round(*d.velocityTicks * d.currentPWMBoundGradient + d.currentPWMBoundOffset);
d.currentMinPWM = std::round(*d.velocityTicks * d.currentPWMBoundGradient - d.currentPWMBoundOffset);
+ d.acceleration = (d.velocity.value - oldVelocity) / dt;
// d.acceleration.read();
// d.gravityTorque.read();
// d.motorCurrent.read();
// d.motorTemperature.read();
i++;
}
+
}
void KITGripperBasisBoardData::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
@@ -253,6 +256,21 @@ namespace armarx
// i++;
// }
+
+ for (auto& ptr : actorData)
+ {
+ if (!ptr)
+ {
+
+ continue;
+ }
+ ActorData& d = *ptr;
+ if (this->getIMUTemperature() > 87)
+ {
+ ARMARX_RT_LOGF_WARNING("IMU Temperature of a gripper board is too high! %d degree celcius").deactivateSpam(5);
+ d.setTargetPWM(0);
+ }
+ }
}
ActorDataPtr& KITGripperBasisBoardData::getActorData(size_t actorIndex)
@@ -263,7 +281,15 @@ namespace armarx
}
ActorData::ActorData(int32_t filterWindowSize):
- velocityFilter(filterWindowSize)
+ velocityFilter(filterWindowSize){}
+
+ int8_t KITGripperBasisBoardData::getIMUTemperature() const
+ {
+ return sensorOUT->IMUTemperature;
+ }
+
+ ActorData::ActorData() :
+ velocityFilter(10)
{
}
@@ -303,6 +329,11 @@ namespace armarx
return rawVelocity;
}
+ float ActorData::getAcceleration() const
+ {
+ return acceleration;
+ }
+
float ActorData::getTorque() const
{
return 0; //torque.value;
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h
index e43e9fc38aa2fd9312577a6153363e8c8826cd10..31dd7c18f2b831582522553935c87151550c90ed 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h
@@ -38,10 +38,15 @@ namespace armarx
{
public:
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+
+ int IMUTemperature;
+
+
static SensorValueInfo<SensorValueKITGripperBasisBoard> GetClassMemberInfo()
{
SensorValueInfo<SensorValueKITGripperBasisBoard> svi;
// svi.addBaseClass<SensorValue1DoFActuatorMotorTemperature>();
+ svi.addMemberVariable(&SensorValueKITGripperBasisBoard::IMUTemperature, "IMUTemperature");
svi.addBaseClass<SensorValueIMU>();
return svi;
}
@@ -96,11 +101,13 @@ namespace armarx
class ActorData
{
public:
- ActorData(int32_t filterWindowSize);
- // position data function
+ ActorData();
+ void setTargetPWM(int32_t targetPWM);
float getPosition() const;
float getRelativePosition() const;
float getCompensatedRelativePosition() const;
+ float getVelocity() const;
+ float getAcceleration() const;
int32_t getRelativePositionSensorOnly() const;
bool getPositionControlEnabled() const;
float getSoftLimitHi() const;
@@ -124,6 +131,7 @@ namespace armarx
int32_t getTargetPWM() const;
int32_t getCurrentMinPWM() const;
int32_t getCurrentMaxPWM() const;
+
float getCurrentPWMBoundGradient() const;
int32_t getCurrentPWMBoundOffset() const;
std::string getMotorModelTypeIndex() const;
@@ -134,6 +142,10 @@ namespace armarx
int32_t getSiblingControlActorIndex() const;
float getParallelGripperDecouplingFactor() const;
+ float getParallelGripperDecouplingFactor() const;
+
+ float getAbsoluteEncoderVelocity() const;
+
private:
// parallel gripper controller configuration
float parallelGripperDecouplingFactor = std::nanf("");
@@ -145,6 +157,7 @@ namespace armarx
LinearConvertedValue<int32_t> relativePosition;
float adjustedRelativePosition;
float relativePositionOffset = std::nan("");
+ float parallelGripperDecouplingFactor = std::nanf("");
LinearConvertedValue<u_int32_t> position;
float sanitizedAbsolutePosition;
float lastAbsolutePosition = std::nanf("");
@@ -157,6 +170,7 @@ namespace armarx
// int32_t velocityFilterWindowSize = 10;
rtfilters::AverageFilter velocityFilter;
float absoluteEncoderVelocity = 0.0f;
+ float acceleration;
int32_t* velocityTicks;
int32_t useABSEncoderForVelocity = -1;
@@ -194,6 +208,8 @@ namespace armarx
void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
ActorDataPtr& getActorData(size_t actorIndex);
+ int8_t getIMUTemperature() const;
+
private:
KITGripperBasisBoardOUT_t* sensorOUT;
KITGripperBasisBoardIN_t* sensorIN;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
index b7a74584a68d152ab97b9e6cca4c67300d628873..a31bc7d02144f5b1f39730cadf15cc4431492162 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
@@ -33,7 +33,10 @@ set(LIB_HEADERS
DMPController/NJointBimanualCCDMPController.h
DMPController/NJointTaskSpaceImpedanceDMPController.h
DMPController/NJointBimanualForceMPController.h
-# DMPController/NJointTSMultiMPController.h
+ DMPController/NJointPeriodicTSDMPForwardVelController.h
+ DMPController/NJointPeriodicTSDMPCompliantController.h
+ DMPController/NJointAdaptiveWipingController.h
+
)
list(APPEND LIB_FILES
DMPController/NJointJointSpaceDMPController.cpp
@@ -43,7 +46,10 @@ set(LIB_HEADERS
DMPController/NJointBimanualCCDMPController.cpp
DMPController/NJointTaskSpaceImpedanceDMPController.cpp
DMPController/NJointBimanualForceMPController.cpp
-# DMPController/NJointTSMultiMPController.cpp
+ DMPController/NJointPeriodicTSDMPForwardVelController.cpp
+ DMPController/NJointPeriodicTSDMPCompliantController.cpp
+ DMPController/NJointAdaptiveWipingController.cpp
+
)
list(APPEND LIBS ${DMP_LIBRARIES} DMPController)
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d19f4e5856fd718aae44d98650d787c8d7efead8
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp
@@ -0,0 +1,704 @@
+#include "NJointAdaptiveWipingController.h"
+
+namespace armarx
+{
+ NJointControllerRegistration<NJointAdaptiveWipingController> registrationControllerNJointAdaptiveWipingController("NJointAdaptiveWipingController");
+
+ NJointAdaptiveWipingController::NJointAdaptiveWipingController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ {
+ useSynchronizedRtRobot();
+ cfg = NJointAdaptiveWipingControllerConfigPtr::dynamicCast(config);
+ ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
+ VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
+
+ ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName);
+ for (size_t i = 0; i < rns->getSize(); ++i)
+ {
+ std::string jointName = rns->getNode(i)->getName();
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
+ const SensorValueBase* sv = useSensorValue(jointName);
+ targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
+
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+
+ velocitySensors.push_back(velocitySensor);
+ positionSensors.push_back(positionSensor);
+ };
+
+ tcp = rns->getTCP();
+ // set tcp controller
+ nodeSetName = cfg->nodeSetName;
+ ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+
+ TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
+ taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
+ taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
+ taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
+ taskSpaceDMPConfig.DMPMode = "Linear";
+ taskSpaceDMPConfig.DMPStyle = "Periodic";
+ taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
+ taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
+ taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
+ taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
+ taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
+ taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
+ taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
+ taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
+ taskSpaceDMPConfig.phaseStopParas.Dori = 0;
+
+
+
+ dmpCtrl.reset(new TaskSpaceDMPController("periodicDMP", taskSpaceDMPConfig, false));
+
+ NJointAdaptiveWipingControllerControlData initData;
+ initData.targetTSVel.resize(6);
+ for (size_t i = 0; i < 6; ++i)
+ {
+ initData.targetTSVel(i) = 0;
+ }
+ reinitTripleBuffer(initData);
+
+ firstRun = true;
+ dmpRunning = false;
+
+
+ ARMARX_CHECK_EQUAL(cfg->Kpos.size(), 3);
+ ARMARX_CHECK_EQUAL(cfg->Dpos.size(), 3);
+ ARMARX_CHECK_EQUAL(cfg->Kori.size(), 3);
+ ARMARX_CHECK_EQUAL(cfg->Dori.size(), 3);
+
+ kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
+ dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
+ kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
+ dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
+
+ kpf = cfg->Kpf;
+ 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);
+
+ }
+
+ nullSpaceJointsVec.resize(cfg->desiredNullSpaceJointValues.size());
+ for (size_t i = 0; i < cfg->desiredNullSpaceJointValues.size(); ++i)
+ {
+ nullSpaceJointsVec(i) = cfg->desiredNullSpaceJointValues.at(i);
+ }
+
+
+ const SensorValueBase* svlf = robUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
+ forceSensor = svlf->asA<SensorValueForceTorque>();
+
+
+ ARMARX_CHECK_EQUAL(cfg->ftOffset.size(), 6);
+
+ currentForceOffset.setZero();
+ forceOffset << cfg->ftOffset[0], cfg->ftOffset[1], cfg->ftOffset[2];
+ torqueOffset << cfg->ftOffset[3], cfg->ftOffset[4], cfg->ftOffset[5];
+
+ handMass = cfg->handMass;
+ handCOM << cfg->handCOM[0], cfg->handCOM[1], cfg->handCOM[2];
+
+
+ filteredForce.setZero();
+ filteredTorque.setZero();
+
+ filteredForceInRoot.setZero();
+ filteredTorqueInRoot.setZero();
+
+ UserToRTData initUserData;
+ initUserData.targetForce = 0;
+ user2rtData.reinitAllBuffers(initUserData);
+
+ oriToolDir << 0, 0, 1;
+ gravityInRoot << 0, 0, -9.8;
+
+ qvel_filtered.setZero(targets.size());
+
+ ARMARX_CHECK_EQUAL(cfg->ws_x.size(), 2);
+ ARMARX_CHECK_EQUAL(cfg->ws_y.size(), 2);
+ ARMARX_CHECK_EQUAL(cfg->ws_z.size(), 2);
+ // only for ARMAR-6 (safe-guard)
+ ARMARX_CHECK_LESS(cfg->ws_x[0], cfg->ws_x[1]);
+ ARMARX_CHECK_LESS(cfg->ws_x[0], 1000);
+ ARMARX_CHECK_LESS(-200, cfg->ws_x[1]);
+
+ ARMARX_CHECK_LESS(cfg->ws_y[0], cfg->ws_y[1]);
+ ARMARX_CHECK_LESS(cfg->ws_y[0], 1200);
+ ARMARX_CHECK_LESS(0, cfg->ws_y[1]);
+
+ ARMARX_CHECK_LESS(cfg->ws_z[0], cfg->ws_z[1]);
+ ARMARX_CHECK_LESS(cfg->ws_z[0], 1800);
+ ARMARX_CHECK_LESS(300, cfg->ws_z[1]);
+
+ adaptK = kpos;
+ lastDiff = 0;
+ changeTimer = 0;
+ }
+
+ void NJointAdaptiveWipingController::onInitNJointController()
+ {
+ ARMARX_INFO << "init ...";
+
+
+ RTToControllerData initSensorData;
+ initSensorData.deltaT = 0;
+ initSensorData.currentTime = 0;
+ initSensorData.currentPose = tcp->getPoseInRootFrame();
+ initSensorData.currentTwist.setZero();
+ initSensorData.isPhaseStop = false;
+ rt2CtrlData.reinitAllBuffers(initSensorData);
+
+ RTToUserData initInterfaceData;
+ initInterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
+ initInterfaceData.waitTimeForCalibration = 0;
+ rt2UserData.reinitAllBuffers(initInterfaceData);
+
+ started = false;
+
+ runTask("NJointAdaptiveWipingController", [&]
+ {
+ CycleUtil c(1);
+ getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
+ while (getState() == eManagedIceObjectStarted)
+ {
+ if (isControllerActive())
+ {
+ controllerRun();
+ }
+ c.waitForCycleDuration();
+ }
+ });
+
+ }
+
+ std::string NJointAdaptiveWipingController::getClassName(const Ice::Current&) const
+ {
+ return "NJointAdaptiveWipingController";
+ }
+
+ void NJointAdaptiveWipingController::controllerRun()
+ {
+ if (!started)
+ {
+ return;
+ }
+
+ if (!dmpCtrl)
+ {
+ return;
+ }
+
+ Eigen::VectorXf targetVels(6);
+ bool isPhaseStop = rt2CtrlData.getUpToDateReadBuffer().isPhaseStop;
+ if (isPhaseStop)
+ {
+ targetVels.setZero();
+ }
+ else
+ {
+
+ double deltaT = rt2CtrlData.getUpToDateReadBuffer().deltaT;
+ Eigen::Matrix4f currentPose = rt2CtrlData.getUpToDateReadBuffer().currentPose;
+ Eigen::VectorXf currentTwist = rt2CtrlData.getUpToDateReadBuffer().currentTwist;
+
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->flow(deltaT, currentPose, currentTwist);
+
+ targetVels = dmpCtrl->getTargetVelocity();
+
+ debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
+ debugOutputData.getWriteBuffer().currentPose["currentPose_x"] = currentPose(0, 3);
+ debugOutputData.getWriteBuffer().currentPose["currentPose_y"] = currentPose(1, 3);
+ debugOutputData.getWriteBuffer().currentPose["currentPose_z"] = currentPose(2, 3);
+ VirtualRobot::MathTools::Quaternion currentQ = VirtualRobot::MathTools::eigen4f2quat(currentPose);
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qw"] = currentQ.w;
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
+ debugOutputData.getWriteBuffer().currentCanVal = dmpCtrl->debugData.canVal;
+ debugOutputData.getWriteBuffer().mpcFactor = dmpCtrl->debugData.mpcFactor;
+ debugOutputData.getWriteBuffer().error = dmpCtrl->debugData.poseError;
+ debugOutputData.getWriteBuffer().posError = dmpCtrl->debugData.posiError;
+ debugOutputData.getWriteBuffer().oriError = dmpCtrl->debugData.oriError;
+ debugOutputData.getWriteBuffer().deltaT = deltaT;
+ debugOutputData.commitWrite();
+ }
+
+ getWriterControlStruct().targetTSVel = targetVels;
+ writeControlStruct();
+
+ dmpRunning = true;
+ }
+
+
+ void NJointAdaptiveWipingController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ double deltaT = timeSinceLastIteration.toSecondsDouble();
+
+ Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
+ rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
+ rt2UserData.getWriteBuffer().waitTimeForCalibration += deltaT;
+ rt2UserData.commitWrite();
+
+ Eigen::Vector3f currentToolDir;
+ currentToolDir.setZero();
+ Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
+
+ Eigen::VectorXf qpos(positionSensors.size());
+ Eigen::VectorXf qvel(velocitySensors.size());
+ for (size_t i = 0; i < positionSensors.size(); ++i)
+ {
+ qpos(i) = positionSensors[i]->position;
+ qvel(i) = velocitySensors[i]->velocity;
+ }
+
+ qvel_filtered = (1 - cfg->velFilter) * qvel_filtered + cfg->velFilter * qvel;
+ Eigen::VectorXf currentTwist = jacobi * qvel_filtered;
+
+ Eigen::VectorXf targetVel(6);
+ targetVel.setZero();
+ if (firstRun || !dmpRunning)
+ {
+ lastPosition = currentPose.block<2, 1>(0, 3);
+ targetPose = currentPose;
+ firstRun = false;
+ filteredForce.setZero();
+ Eigen::Vector3f currentForce = forceSensor->force - forceOffset;
+ Eigen::Matrix3f forceFrameOri = rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame().block<3, 3>(0, 0);
+ Eigen::Vector3f gravityInForceFrame = forceFrameOri.transpose() * gravityInRoot;
+ Eigen::Vector3f handGravity = handMass * gravityInForceFrame;
+ currentForce = currentForce - handGravity;
+ currentForceOffset = 0.1 * currentForceOffset + 0.9 * currentForce;
+
+ origHandOri = currentPose.block<3, 3>(0, 0);
+ toolTransform = origHandOri.transpose();
+ targetVel.setZero();
+ }
+ else
+ {
+ // communicate with DMP controller
+ rtUpdateControlStruct();
+ targetVel = rtGetControlStruct().targetTSVel;
+ targetVel(2) = 0;
+
+ // calculate force
+ Eigen::Vector3f currentForce = forceSensor->force - forceOffset - currentForceOffset;
+
+ Eigen::Matrix3f forceFrameOri = rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame().block<3, 3>(0, 0);
+ Eigen::Vector3f gravityInForceFrame = forceFrameOri.transpose() * gravityInRoot;
+ Eigen::Vector3f handGravity = handMass * gravityInForceFrame;
+
+ currentForce = currentForce - handGravity;
+ filteredForce = (1 - cfg->forceFilter) * filteredForce + cfg->forceFilter * currentForce;
+
+ // Eigen::Vector3f currentTorque = forceSensor->torque - torqueOffset;
+ // Eigen::Vector3f handTorque = handCOM.cross(gravityInForceFrame);
+ // currentTorque = currentTorque - handTorque;
+
+ for (size_t i = 0; i < 3; ++i)
+ {
+ if (fabs(filteredForce(i)) > cfg->forceDeadZone)
+ {
+ filteredForce(i) -= (filteredForce(i) / fabs(filteredForce(i))) * cfg->forceDeadZone;
+ }
+ else
+ {
+ filteredForce(i) = 0;
+ }
+ }
+
+ filteredForceInRoot = forceFrameOri * filteredForce;
+ float targetForce = user2rtData.getUpToDateReadBuffer().targetForce;
+
+ Eigen::Matrix3f currentHandOri = currentPose.block<3, 3>(0, 0);
+ Eigen::Matrix3f currentToolOri = currentHandOri * toolTransform;
+
+ Eigen::Vector3f forceInToolFrame = currentToolOri.transpose() * filteredForceInRoot;
+ float desiredZVel = kpf * (targetForce - forceInToolFrame(2));
+ targetVel(2) -= desiredZVel;
+ targetVel.block<3, 1>(0, 0) = currentToolOri * targetVel.block<3, 1>(0, 0);
+
+ currentToolDir = currentToolOri * oriToolDir;
+
+ for (int i = 3; i < 6; ++i)
+ {
+ targetVel(i) = 0;
+ }
+
+ // rotation changes
+
+ if (filteredForceInRoot.norm() > fabs(cfg->minimumReactForce))
+ {
+ Eigen::Vector3f desiredToolDir = filteredForceInRoot.normalized();// / filteredForceInRoot.norm();
+ currentToolDir.normalize();
+
+ Eigen::Vector3f axis = currentToolDir.cross(desiredToolDir);
+ float angle = acosf(currentToolDir.dot(desiredToolDir));
+
+ if (fabs(angle) < M_PI / 2)
+ {
+ Eigen::AngleAxisf desiredToolRot(angle, axis);
+ Eigen::Matrix3f desiredRotMat = desiredToolRot * Eigen::Matrix3f::Identity();
+
+ Eigen::Vector3f angularDiff = VirtualRobot::MathTools::eigen3f2rpy(desiredRotMat);
+
+ targetVel.tail(3) = cfg->angularKp * angularDiff;
+
+ Eigen::Vector3f desiredRPY = VirtualRobot::MathTools::eigen3f2rpy(desiredRotMat);
+ Eigen::Vector3f checkedToolDir = desiredRotMat * currentToolDir;
+ checkedToolDir.normalize();
+ }
+
+ }
+
+
+ // integrate for targetPose
+ }
+
+ bool isPhaseStop = false;
+
+ float diff = (targetPose.block<2, 1>(0, 3) - currentPose.block<2, 1>(0, 3)).norm();
+ if (diff > cfg->phaseDist0)
+ {
+ isPhaseStop = true;
+ }
+
+ float dTf = (float)deltaT;
+
+
+ if (filteredForceInRoot.block<2, 1>(0, 0).norm() > cfg->dragForceDeadZone)
+ {
+ Eigen::Vector2f dragForce = filteredForceInRoot.block<2, 1>(0, 0) - cfg->dragForceDeadZone * filteredForceInRoot.block<2, 1>(0, 0) / filteredForceInRoot.block<2, 1>(0, 0).norm();
+ adaptK(0) = fmax(adaptK(0) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
+ adaptK(1) = fmax(adaptK(1) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
+ lastDiff = diff;
+ }
+ else
+ {
+ adaptK(0) = fmin(adaptK(0) + fabs(dTf * cfg->adaptCoeff), kpos(0));
+ adaptK(1) = fmin(adaptK(1) + fabs(dTf * cfg->adaptCoeff), kpos(1));
+ }
+ adaptK(2) = kpos(2);
+
+ // adaptive control with distance
+
+
+
+
+ targetPose.block<3, 1>(0, 3) = targetPose.block<3, 1>(0, 3) + dTf * targetVel.block<3, 1>(0, 0);
+ Eigen::Matrix3f rotMat = VirtualRobot::MathTools::rpy2eigen3f(dTf * targetVel(3), dTf * targetVel(4), dTf * targetVel(5));
+ targetPose.block<3, 3>(0, 0) = rotMat * targetPose.block<3, 3>(0, 0);
+
+ if (isPhaseStop)
+ {
+ Eigen::Vector2f currentXY = currentPose.block<2, 1>(0, 3);
+ if ((lastPosition - currentXY).norm() < cfg->changePositionTolerance)
+ {
+ changeTimer += deltaT;
+ }
+ else
+ {
+ lastPosition = currentPose.block<2, 1>(0, 3);
+ changeTimer = 0;
+ }
+
+ if (changeTimer > cfg->changeTimerThreshold)
+ {
+ targetPose(0, 3) = currentPose(0, 3);
+ targetPose(1, 3) = currentPose(1, 3);
+ isPhaseStop = false;
+ changeTimer = 0;
+ }
+ }
+ else
+ {
+ changeTimer = 0;
+ }
+
+
+ targetPose(0, 3) = targetPose(0, 3) > cfg->ws_x[0] ? targetPose(0, 3) : cfg->ws_x[0];
+ targetPose(0, 3) = targetPose(0, 3) < cfg->ws_x[1] ? targetPose(0, 3) : cfg->ws_x[1];
+
+ targetPose(1, 3) = targetPose(1, 3) > cfg->ws_y[0] ? targetPose(1, 3) : cfg->ws_y[0];
+ targetPose(1, 3) = targetPose(1, 3) < cfg->ws_y[1] ? targetPose(1, 3) : cfg->ws_y[1];
+
+ targetPose(2, 3) = targetPose(2, 3) > cfg->ws_z[0] ? targetPose(2, 3) : cfg->ws_z[0];
+ targetPose(2, 3) = targetPose(2, 3) < cfg->ws_z[1] ? targetPose(2, 3) : cfg->ws_z[1];
+
+
+
+ debugRT.getWriteBuffer().currentToolDir = tcp->getRobot()->getRootNode()->toGlobalCoordinateSystemVec(currentToolDir);
+ debugRT.getWriteBuffer().targetPose = targetPose;
+ debugRT.getWriteBuffer().globalPose = tcp->getRobot()->getRootNode()->toGlobalCoordinateSystem(targetPose);
+ debugRT.getWriteBuffer().currentPose = currentPose;
+ debugRT.getWriteBuffer().filteredForceInRoot = filteredForceInRoot;
+ debugRT.getWriteBuffer().filteredForce = filteredForce;
+ debugRT.getWriteBuffer().globalFilteredForce = tcp->getRobot()->getRootNode()->toGlobalCoordinateSystemVec(filteredForceInRoot);
+ debugRT.getWriteBuffer().targetVel = targetVel;
+ debugRT.getWriteBuffer().adaptK = adaptK;
+ debugRT.getWriteBuffer().isPhaseStop = isPhaseStop;
+ debugRT.commitWrite();
+
+ rt2CtrlData.getWriteBuffer().currentPose = currentPose;
+ rt2CtrlData.getWriteBuffer().currentTwist = currentTwist;
+ rt2CtrlData.getWriteBuffer().deltaT = deltaT;
+ rt2CtrlData.getWriteBuffer().currentTime += deltaT;
+ rt2CtrlData.getWriteBuffer().isPhaseStop = isPhaseStop;
+ rt2CtrlData.commitWrite();
+
+ // Eigen::Matrix3f rotVel = VirtualRobot::MathTools::rpy2eigen3f(targetVel(3) * dTf, targetVel(4) * dTf, targetVel(5) * dTf);
+ // targetPose.block<3, 3>(0, 0) = rotVel * targetPose.block<3, 3>(0, 0);
+
+ // inverse dynamic controller
+ jacobi.block<3, 8>(0, 0) = 0.001 * jacobi.block<3, 8>(0, 0); // convert mm to m
+
+
+
+
+ Eigen::Vector6f jointControlWrench;
+ {
+ 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);
+ Eigen::Vector3f currentTCPPosition = currentPose.block<3, 1>(0, 3);
+ Eigen::Vector3f desiredPosition = targetPose.block<3, 1>(0, 3);
+
+ Eigen::Vector3f linearVel = adaptK.cwiseProduct(desiredPosition - currentTCPPosition);
+
+ // if (isPhaseStop)
+ // {
+ // linearVel = ((float)cfg->phaseKpPos) * (desiredPosition - currentTCPPosition);
+ // for (size_t i = 0; i < 3; ++i)
+ // {
+ // linearVel(i) = fmin(cfg->maxLinearVel, linearVel(i));
+ // }
+ // }
+ // else
+ // {
+ // linearVel = kpos.cwiseProduct(desiredPosition - currentTCPPosition);
+ // }
+ Eigen::Vector3f tcpDesiredForce = 0.001 * linearVel + dpos.cwiseProduct(- currentTCPLinearVelocity);
+
+ Eigen::Vector3f currentTCPAngularVelocity;
+ 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;
+ }
+
+
+
+ Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
+ Eigen::VectorXf nullspaceTorque = knull.cwiseProduct(nullSpaceJointsVec - qpos) - dnull.cwiseProduct(qvel);
+ Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
+ Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
+
+ // torque filter (maybe)
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ targets.at(i)->torque = jointDesiredTorques(i);
+
+ if (!targets.at(i)->isValid())
+ {
+ targets.at(i)->torque = 0.0f;
+ }
+ else
+ {
+ if (fabs(targets.at(i)->torque) > fabs(cfg->maxJointTorque))
+ {
+ targets.at(i)->torque = fabs(cfg->maxJointTorque) * (targets.at(i)->torque / fabs(targets.at(i)->torque));
+ }
+ }
+ }
+
+
+ }
+
+
+ void NJointAdaptiveWipingController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
+ {
+ ARMARX_INFO << "Learning DMP ... ";
+
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->learnDMPFromFiles(fileNames);
+
+ }
+
+ void NJointAdaptiveWipingController::learnDMPFromTrajectory(const TrajectoryBasePtr& trajectory, const Ice::Current&)
+ {
+ ARMARX_INFO << "Learning DMP ... ";
+ ARMARX_CHECK_EXPRESSION(trajectory);
+ TrajectoryPtr dmpTraj = TrajectoryPtr::dynamicCast(trajectory);
+ ARMARX_CHECK_EXPRESSION(dmpTraj);
+
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->learnDMPFromTrajectory(dmpTraj);
+
+ }
+
+ void NJointAdaptiveWipingController::setSpeed(Ice::Double times, const Ice::Current&)
+ {
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->setSpeed(times);
+ }
+
+
+ void NJointAdaptiveWipingController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
+ {
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->setGoalPoseVec(goals);
+ }
+
+ void NJointAdaptiveWipingController::setAmplitude(Ice::Double amp, const Ice::Current&)
+ {
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->setAmplitude(amp);
+ }
+
+ void NJointAdaptiveWipingController::setTargetForceInRootFrame(float targetForce, const Ice::Current&)
+ {
+ LockGuardType guard {controlDataMutex};
+ user2rtData.getWriteBuffer().targetForce = targetForce;
+ user2rtData.commitWrite();
+ }
+
+ void NJointAdaptiveWipingController::runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&)
+ {
+ firstRun = true;
+ while (firstRun || rt2UserData.getUpToDateReadBuffer().waitTimeForCalibration < cfg->waitTimeForCalibration)
+ {
+ usleep(100);
+ }
+
+
+ Eigen::Matrix4f pose = rt2UserData.getUpToDateReadBuffer().currentTcpPose;
+
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
+ dmpCtrl->setSpeed(tau);
+
+ ARMARX_IMPORTANT << "run DMP";
+ started = true;
+ dmpRunning = false;
+ }
+
+
+ void NJointAdaptiveWipingController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx& debugDrawer, const DebugObserverInterfacePrx& debugObs)
+ {
+ std::string datafieldName;
+ std::string debugName = "Periodic";
+ StringVariantBaseMap datafields;
+
+ Eigen::Matrix4f targetPoseDebug = debugRT.getUpToDateReadBuffer().targetPose;
+ datafields["target_x"] = new Variant(targetPoseDebug(0, 3));
+ datafields["target_y"] = new Variant(targetPoseDebug(1, 3));
+ datafields["target_z"] = new Variant(targetPoseDebug(2, 3));
+
+ Eigen::Matrix4f currentPoseDebug = debugRT.getUpToDateReadBuffer().currentPose;
+ datafields["current_x"] = new Variant(currentPoseDebug(0, 3));
+ datafields["current_y"] = new Variant(currentPoseDebug(1, 3));
+ datafields["current_z"] = new Variant(currentPoseDebug(2, 3));
+
+ Eigen::Vector3f filteredForce = debugRT.getUpToDateReadBuffer().filteredForce;
+ datafields["filteredforce_x"] = new Variant(filteredForce(0));
+ datafields["filteredforce_y"] = new Variant(filteredForce(1));
+ datafields["filteredforce_z"] = new Variant(filteredForce(2));
+
+ Eigen::Vector3f filteredForceInRoot = debugRT.getUpToDateReadBuffer().filteredForceInRoot;
+ datafields["filteredForceInRoot_x"] = new Variant(filteredForceInRoot(0));
+ datafields["filteredForceInRoot_y"] = new Variant(filteredForceInRoot(1));
+ datafields["filteredForceInRoot_z"] = new Variant(filteredForceInRoot(2));
+
+ Eigen::Vector3f reactForce = debugRT.getUpToDateReadBuffer().reactForce;
+ datafields["reactForce_x"] = new Variant(reactForce(0));
+ datafields["reactForce_y"] = new Variant(reactForce(1));
+ datafields["reactForce_z"] = new Variant(reactForce(2));
+
+ Eigen::VectorXf targetVel = debugRT.getUpToDateReadBuffer().targetVel;
+ datafields["targetVel_x"] = new Variant(targetVel(0));
+ datafields["targetVel_y"] = new Variant(targetVel(1));
+ datafields["targetVel_z"] = new Variant(targetVel(2));
+
+ Eigen::Vector3f adaptK = debugRT.getUpToDateReadBuffer().adaptK;
+ datafields["adaptK_x"] = new Variant(adaptK(0));
+ datafields["adaptK_y"] = new Variant(adaptK(1));
+
+ datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
+ datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
+
+ datafields["PhaseStop"] = new Variant(debugRT.getUpToDateReadBuffer().isPhaseStop);
+
+
+ // datafields["targetVel_rx"] = new Variant(targetVel(3));
+ // datafields["targetVel_ry"] = new Variant(targetVel(4));
+ // datafields["targetVel_rz"] = new Variant(targetVel(5));
+
+ // auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
+ // for (auto& pair : values)
+ // {
+ // datafieldName = pair.first + "_" + debugName;
+ // datafields[datafieldName] = new Variant(pair.second);
+ // }
+
+ // auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
+ // for (auto& pair : currentPose)
+ // {
+ // datafieldName = pair.first + "_" + debugName;
+ // datafields[datafieldName] = new Variant(pair.second);
+ // }
+
+ // datafieldName = "canVal_" + debugName;
+ // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
+ // datafieldName = "mpcFactor_" + debugName;
+ // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
+ // datafieldName = "error_" + debugName;
+ // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
+ // datafieldName = "posError_" + debugName;
+ // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
+ // datafieldName = "oriError_" + debugName;
+ // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
+ // datafieldName = "deltaT_" + debugName;
+ // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
+
+ datafieldName = "PeriodicDMP";
+ debugObs->setDebugChannel(datafieldName, datafields);
+
+
+ // draw force;
+ Eigen::Matrix4f globalPose = debugRT.getUpToDateReadBuffer().globalPose;
+ Eigen::Vector3f handPosition = globalPose.block<3, 1>(0, 3);
+ Eigen::Vector3f forceDir = debugRT.getUpToDateReadBuffer().globalFilteredForce;
+
+ debugDrawer->setArrowVisu("Force", "currentForce", new Vector3(handPosition), new Vector3(forceDir), DrawColor {0, 0, 1, 1}, 10 * forceDir.norm(), 3);
+
+ // draw direction of the tool
+ Eigen::Vector3f currentToolDir = debugRT.getUpToDateReadBuffer().currentToolDir;
+ debugDrawer->setArrowVisu("Tool", "Tool", new Vector3(handPosition), new Vector3(currentToolDir), DrawColor {1, 0, 0, 1}, 100, 3);
+ debugDrawer->setPoseVisu("target", "targetPose", new Pose(globalPose));
+
+ }
+
+
+
+ void NJointAdaptiveWipingController::onDisconnectNJointController()
+ {
+ ARMARX_INFO << "stopped ...";
+ }
+
+
+
+}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.h
new file mode 100644
index 0000000000000000000000000000000000000000..9bfd26e64ad5551f5997634189034bd158e9b43e
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.h
@@ -0,0 +1,209 @@
+
+#pragma once
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <VirtualRobot/Robot.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+
+#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
+#include <RobotAPI/libraries/core/Trajectory.h>
+
+namespace armarx
+{
+
+
+ TYPEDEF_PTRS_HANDLE(NJointAdaptiveWipingController);
+ TYPEDEF_PTRS_HANDLE(NJointAdaptiveWipingControllerControlData);
+
+ class NJointAdaptiveWipingControllerControlData
+ {
+ public:
+ Eigen::VectorXf targetTSVel;
+ };
+
+ class pidController
+ {
+ public:
+ float Kp = 0, Kd = 0;
+ float lastError = 0;
+ float update(float dt, float error)
+ {
+ float derivative = (error - lastError) / dt;
+ float retVal = Kp * error + Kd * derivative;
+ lastError = error;
+ return retVal;
+ }
+ };
+
+ /**
+ * @brief The NJointAdaptiveWipingController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
+ class NJointAdaptiveWipingController :
+ public NJointControllerWithTripleBuffer<NJointAdaptiveWipingControllerControlData>,
+ public NJointAdaptiveWipingControllerInterface
+ {
+ public:
+ using ConfigPtrT = NJointAdaptiveWipingControllerConfigPtr;
+ NJointAdaptiveWipingController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+ // NJointControllerInterface interface
+ std::string getClassName(const Ice::Current&) const;
+
+ // NJointController interface
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+
+ // NJointAdaptiveWipingControllerInterface interface
+ void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
+ void learnDMPFromTrajectory(const TrajectoryBasePtr& trajectory, const Ice::Current&);
+ bool isFinished(const Ice::Current&)
+ {
+ return false;
+ }
+
+ void setSpeed(Ice::Double times, const Ice::Current&);
+ void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
+ void setAmplitude(Ice::Double amp, const Ice::Current&);
+ void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
+ void setTargetForceInRootFrame(Ice::Float force, const Ice::Current&);
+ double getCanVal(const Ice::Current&)
+ {
+ return dmpCtrl->canVal;
+ }
+
+ protected:
+ virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
+
+ void onInitNJointController();
+ void onDisconnectNJointController();
+ void controllerRun();
+
+ private:
+ struct DebugBufferData
+ {
+ StringFloatDictionary latestTargetVelocities;
+ StringFloatDictionary currentPose;
+ double currentCanVal;
+ double mpcFactor;
+ double error;
+ double phaseStop;
+ double posError;
+ double oriError;
+ double deltaT;
+ };
+
+ TripleBuffer<DebugBufferData> debugOutputData;
+
+
+ struct DebugRTData
+ {
+ Eigen::Matrix4f targetPose;
+ Eigen::Vector3f filteredForce;
+ Eigen::Vector3f filteredForceInRoot;
+
+ Eigen::Vector3f reactForce;
+ Eigen::Vector3f adaptK;
+ Eigen::VectorXf targetVel;
+ Eigen::Matrix4f currentPose;
+ bool isPhaseStop;
+
+ Eigen::Matrix4f globalPose;
+ Eigen::Vector3f globalFilteredForce;
+ Eigen::Vector3f currentToolDir;
+ };
+ TripleBuffer<DebugRTData> debugRT;
+
+
+
+
+ struct RTToControllerData
+ {
+ double currentTime;
+ double deltaT;
+ Eigen::Matrix4f currentPose;
+ Eigen::VectorXf currentTwist;
+ bool isPhaseStop;
+ };
+ TripleBuffer<RTToControllerData> rt2CtrlData;
+
+ struct RTToUserData
+ {
+ Eigen::Matrix4f currentTcpPose;
+ float waitTimeForCalibration;
+ };
+ TripleBuffer<RTToUserData> rt2UserData;
+
+ struct UserToRTData
+ {
+ float targetForce;
+ };
+ TripleBuffer<UserToRTData> user2rtData;
+
+
+ TaskSpaceDMPControllerPtr dmpCtrl;
+
+ std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
+ std::vector<ControlTarget1DoFActuatorTorque*> targets;
+
+ // velocity ik controller parameters
+ std::string nodeSetName;
+
+ bool started;
+ bool firstRun;
+ bool dmpRunning;
+
+ VirtualRobot::DifferentialIKPtr ik;
+ VirtualRobot::RobotNodePtr tcp;
+
+ NJointAdaptiveWipingControllerConfigPtr cfg;
+ mutable MutexType controllerMutex;
+ PeriodicTask<NJointAdaptiveWipingController>::pointer_type controllerTask;
+ Eigen::Matrix4f targetPose;
+
+ Eigen::Vector3f kpos;
+ Eigen::Vector3f dpos;
+ Eigen::Vector3f kori;
+ Eigen::Vector3f dori;
+ Eigen::VectorXf knull;
+ Eigen::VectorXf dnull;
+ float kpf;
+
+ Eigen::VectorXf nullSpaceJointsVec;
+ const SensorValueForceTorque* forceSensor;
+
+ Eigen::Vector3f filteredForce;
+ Eigen::Vector3f filteredTorque;
+ Eigen::Vector3f forceOffset;
+ Eigen::Vector3f currentForceOffset;
+ Eigen::Vector3f torqueOffset;
+ float handMass;
+ Eigen::Vector3f handCOM;
+ Eigen::Vector3f gravityInRoot;
+
+ Eigen::Vector3f filteredForceInRoot;
+ Eigen::Vector3f filteredTorqueInRoot;
+
+ Eigen::Matrix3f toolTransform;
+ Eigen::Vector3f oriToolDir;
+ Eigen::Matrix3f origHandOri;
+ Eigen::VectorXf qvel_filtered;
+
+ Eigen::Vector3f adaptK;
+ float lastDiff;
+ Eigen::Vector2f lastPosition;
+ double changeTimer;
+
+ };
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
index c0d0f4c5011b1c5432a22f59f06700cb4d9b6d77..d61b8fa3f02fb55eb0311332c289c6df6febcae3 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
@@ -833,13 +833,13 @@ namespace armarx
debugObs->setDebugChannel("DMPController", datafields);
}
- void NJointBimanualCCDMPController::onInitComponent()
+ void NJointBimanualCCDMPController::onInitNJointController()
{
ARMARX_INFO << "init ...";
controllerTask = new PeriodicTask<NJointBimanualCCDMPController>(this, &NJointBimanualCCDMPController::controllerRun, 0.3);
}
- void NJointBimanualCCDMPController::onDisconnectComponent()
+ void NJointBimanualCCDMPController::onDisconnectNJointController()
{
controllerTask->stop();
ARMARX_INFO << "stopped ...";
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
index 4f4f8bf0f5d4fadaf22434a56934759e95791fb2..2bc75223f0578d3607704e65d0fbe5691d9104b7 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
@@ -87,8 +87,8 @@ namespace armarx
virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
- void onInitComponent() override;
- void onDisconnectComponent() override;
+ void onInitNJointController() override;
+ void onDisconnectNJointController() override;
void controllerRun();
private:
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp
index eeb9ebe8f80df2f0775ad22d8b1156e913dc9d3e..7d1a8f2c012aa7da744dd39281b6327c376e8b41 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp
@@ -479,13 +479,13 @@ namespace armarx
debugObs->setDebugChannel(datafieldName, datafields);
}
- void NJointBimanualForceMPController::onInitComponent()
+ void NJointBimanualForceMPController::onInitNJointController()
{
ARMARX_INFO << "init ...";
controllerTask = new PeriodicTask<NJointBimanualForceMPController>(this, &NJointBimanualForceMPController::controllerRun, 0.3);
}
- void NJointBimanualForceMPController::onDisconnectComponent()
+ void NJointBimanualForceMPController::onDisconnectNJointController()
{
ARMARX_INFO << "stopped ...";
controllerTask->stop();
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h
index db73f4f8c74b281f2e10dd0e6676101185e5deeb..29f765ded0010d57043dafbb876c4771d5c904ae 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h
@@ -68,8 +68,8 @@ namespace armarx
protected:
virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
- void onInitComponent() override;
- void onDisconnectComponent() override;
+ void onInitNJointController() override;
+ void onDisconnectNJointController() override;
void controllerRun();
private:
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
index 6b38de60d12a6bce7ea37f9f8cfcef81aa7242cf..c9fa7a03af5c54636240a9374efce3ed78fee0e1 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
@@ -189,14 +189,14 @@ namespace armarx
double timeDuration = timeDurations[i];
std::string dmpType = dmpTypes[i];
- // double amplitude = 1.0;
+ //double amplitude = 1.0;
if (dmpType == "Periodic")
{
if (canVals[i] <= 1e-8)
{
canVals[i] = timeDuration;
}
- // amplitude = amplitudes[i];
+ //amplitude = amplitudes[i];
}
if (canVals[i] > 1e-8)
@@ -570,13 +570,13 @@ namespace armarx
debugObs->setDebugChannel("DMPController", datafields);
}
- void NJointCCDMPController::onInitComponent()
+ void NJointCCDMPController::onInitNJointController()
{
ARMARX_INFO << "init ...";
controllerTask = new PeriodicTask<NJointCCDMPController>(this, &NJointCCDMPController::controllerRun, 0.3);
}
- void NJointCCDMPController::onDisconnectComponent()
+ void NJointCCDMPController::onDisconnectNJointController()
{
controllerTask->stop();
ARMARX_INFO << "stopped ...";
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
index cd0c4b0dbcd57285433dceda0a16f3e2f87b2021..2fd838d91be90490ad57d3f50f6e6586048caaba 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
@@ -90,8 +90,8 @@ namespace armarx
VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
- void onInitComponent() override;
- void onDisconnectComponent() override;
+ void onInitNJointController() override;
+ void onDisconnectNJointController() override;
void controllerRun();
private:
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
index fd33f782b3f3e90d99d10c639a8237d93b485928..0a8ba174454a5f6c0b80f40aea0fc4caf75967f1 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
@@ -261,13 +261,13 @@ namespace armarx
}
- void NJointJSDMPController::onInitComponent()
+ void NJointJSDMPController::onInitNJointController()
{
ARMARX_INFO << "init ...";
controllerTask = new PeriodicTask<NJointJSDMPController>(this, &NJointJSDMPController::controllerRun, 0.2);
}
- void NJointJSDMPController::onDisconnectComponent()
+ void NJointJSDMPController::onDisconnectNJointController()
{
controllerTask->stop();
ARMARX_INFO << "stopped ...";
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
index 210dd98621f43aa2820c30b1d0388c6107f50921..f593d4943bcaae6286a654d8dcfb46823abe3915 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
@@ -123,8 +123,8 @@ namespace armarx
// ManagedIceObject interface
protected:
void controllerRun();
- void onInitComponent() override;
- void onDisconnectComponent() override;
+ void onInitNJointController() override;
+ void onDisconnectNJointController() override;
};
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b6b58637cb064bd03c43befb53ddceac9e7ee97a
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
@@ -0,0 +1,667 @@
+#include "NJointPeriodicTSDMPCompliantController.h"
+
+namespace armarx
+{
+ NJointControllerRegistration<NJointPeriodicTSDMPCompliantController> registrationControllerNJointPeriodicTSDMPCompliantController("NJointPeriodicTSDMPCompliantController");
+
+ NJointPeriodicTSDMPCompliantController::NJointPeriodicTSDMPCompliantController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ {
+ useSynchronizedRtRobot();
+ cfg = NJointPeriodicTSDMPCompliantControllerConfigPtr::dynamicCast(config);
+ ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
+ VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
+
+ ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName);
+ for (size_t i = 0; i < rns->getSize(); ++i)
+ {
+ std::string jointName = rns->getNode(i)->getName();
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
+ const SensorValueBase* sv = useSensorValue(jointName);
+ targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
+
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+
+ velocitySensors.push_back(velocitySensor);
+ positionSensors.push_back(positionSensor);
+ };
+
+ tcp = rns->getTCP();
+ // set tcp controller
+ nodeSetName = cfg->nodeSetName;
+ ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+
+ TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
+ taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
+ taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
+ taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
+ taskSpaceDMPConfig.DMPMode = "Linear";
+ taskSpaceDMPConfig.DMPStyle = "Periodic";
+ taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
+ taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
+ taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
+ taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
+ taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
+ taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
+ taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
+ taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
+ taskSpaceDMPConfig.phaseStopParas.Dori = 0;
+
+
+
+ dmpCtrl.reset(new TaskSpaceDMPController("periodicDMP", taskSpaceDMPConfig, false));
+
+ NJointPeriodicTSDMPCompliantControllerControlData initData;
+ initData.targetTSVel.resize(6);
+ for (size_t i = 0; i < 6; ++i)
+ {
+ initData.targetTSVel(i) = 0;
+ }
+ reinitTripleBuffer(initData);
+
+ firstRun = true;
+ dmpRunning = false;
+
+
+ ARMARX_CHECK_EQUAL(cfg->Kpos.size(), 3);
+ ARMARX_CHECK_EQUAL(cfg->Dpos.size(), 3);
+ ARMARX_CHECK_EQUAL(cfg->Kori.size(), 3);
+ ARMARX_CHECK_EQUAL(cfg->Dori.size(), 3);
+
+ kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
+ dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
+ kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
+ dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
+
+ kpf = cfg->Kpf;
+ knull = cfg->Knull;
+ dnull = cfg->Dnull;
+
+
+
+ nullSpaceJointsVec.resize(cfg->desiredNullSpaceJointValues.size());
+ for (size_t i = 0; i < cfg->desiredNullSpaceJointValues.size(); ++i)
+ {
+ nullSpaceJointsVec(i) = cfg->desiredNullSpaceJointValues.at(i);
+ }
+
+
+ const SensorValueBase* svlf = robUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
+ forceSensor = svlf->asA<SensorValueForceTorque>();
+
+ forceOffset.setZero();
+ filteredForce.setZero();
+ filteredForceInRoot.setZero();
+
+
+ UserToRTData initUserData;
+ initUserData.targetForce = 0;
+ user2rtData.reinitAllBuffers(initUserData);
+
+ oriToolDir << 0, 0, 1;
+
+ qvel_filtered.setZero(targets.size());
+
+ ARMARX_CHECK_EQUAL(cfg->ws_x.size(), 2);
+ ARMARX_CHECK_EQUAL(cfg->ws_y.size(), 2);
+ ARMARX_CHECK_EQUAL(cfg->ws_z.size(), 2);
+ // only for ARMAR-6 (safe-guard)
+ ARMARX_CHECK_LESS(cfg->ws_x[0], cfg->ws_x[1]);
+ ARMARX_CHECK_LESS(cfg->ws_x[0], 1000);
+ ARMARX_CHECK_LESS(-200, cfg->ws_x[1]);
+
+ ARMARX_CHECK_LESS(cfg->ws_y[0], cfg->ws_y[1]);
+ ARMARX_CHECK_LESS(cfg->ws_y[0], 1200);
+ ARMARX_CHECK_LESS(0, cfg->ws_y[1]);
+
+ ARMARX_CHECK_LESS(cfg->ws_z[0], cfg->ws_z[1]);
+ ARMARX_CHECK_LESS(cfg->ws_z[0], 1800);
+ ARMARX_CHECK_LESS(300, cfg->ws_z[1]);
+
+ adaptK = kpos;
+ lastDiff = 0;
+ changeTimer = 0;
+ }
+
+ void NJointPeriodicTSDMPCompliantController::onInitNJointController()
+ {
+ ARMARX_INFO << "init ...";
+
+
+ RTToControllerData initSensorData;
+ initSensorData.deltaT = 0;
+ initSensorData.currentTime = 0;
+ initSensorData.currentPose = tcp->getPoseInRootFrame();
+ initSensorData.currentTwist.setZero();
+ initSensorData.isPhaseStop = false;
+ rt2CtrlData.reinitAllBuffers(initSensorData);
+
+ RTToUserData initInterfaceData;
+ initInterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
+ initInterfaceData.waitTimeForCalibration = 0;
+ rt2UserData.reinitAllBuffers(initInterfaceData);
+
+
+ ARMARX_IMPORTANT << "read force sensor ...";
+
+ forceOffset = forceSensor->force;
+
+ ARMARX_IMPORTANT << "force offset: " << forceOffset;
+
+ started = false;
+
+ runTask("NJointPeriodicTSDMPCompliantController", [&]
+ {
+ CycleUtil c(1);
+ getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
+ while (getState() == eManagedIceObjectStarted)
+ {
+ if (isControllerActive())
+ {
+ controllerRun();
+ }
+ c.waitForCycleDuration();
+ }
+ });
+
+ ARMARX_IMPORTANT << "started controller ";
+
+ }
+
+ std::string NJointPeriodicTSDMPCompliantController::getClassName(const Ice::Current&) const
+ {
+ return "NJointPeriodicTSDMPCompliantController";
+ }
+
+ void NJointPeriodicTSDMPCompliantController::controllerRun()
+ {
+ if (!started)
+ {
+ return;
+ }
+
+ if (!dmpCtrl)
+ {
+ return;
+ }
+
+ Eigen::VectorXf targetVels(6);
+ bool isPhaseStop = rt2CtrlData.getUpToDateReadBuffer().isPhaseStop;
+ if (isPhaseStop)
+ {
+ targetVels.setZero();
+ }
+ else
+ {
+
+ double deltaT = rt2CtrlData.getUpToDateReadBuffer().deltaT;
+ Eigen::Matrix4f currentPose = rt2CtrlData.getUpToDateReadBuffer().currentPose;
+ Eigen::VectorXf currentTwist = rt2CtrlData.getUpToDateReadBuffer().currentTwist;
+
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->flow(deltaT, currentPose, currentTwist);
+
+ targetVels = dmpCtrl->getTargetVelocity();
+
+ debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
+ debugOutputData.getWriteBuffer().currentPose["currentPose_x"] = currentPose(0, 3);
+ debugOutputData.getWriteBuffer().currentPose["currentPose_y"] = currentPose(1, 3);
+ debugOutputData.getWriteBuffer().currentPose["currentPose_z"] = currentPose(2, 3);
+ VirtualRobot::MathTools::Quaternion currentQ = VirtualRobot::MathTools::eigen4f2quat(currentPose);
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qw"] = currentQ.w;
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
+ debugOutputData.getWriteBuffer().currentCanVal = dmpCtrl->debugData.canVal;
+ debugOutputData.getWriteBuffer().mpcFactor = dmpCtrl->debugData.mpcFactor;
+ debugOutputData.getWriteBuffer().error = dmpCtrl->debugData.poseError;
+ debugOutputData.getWriteBuffer().posError = dmpCtrl->debugData.posiError;
+ debugOutputData.getWriteBuffer().oriError = dmpCtrl->debugData.oriError;
+ debugOutputData.getWriteBuffer().deltaT = deltaT;
+ debugOutputData.commitWrite();
+ }
+
+ getWriterControlStruct().targetTSVel = targetVels;
+ writeControlStruct();
+
+ dmpRunning = true;
+ }
+
+
+ void NJointPeriodicTSDMPCompliantController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ double deltaT = timeSinceLastIteration.toSecondsDouble();
+
+ Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
+ rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
+ rt2UserData.getWriteBuffer().waitTimeForCalibration += deltaT;
+ rt2UserData.commitWrite();
+
+
+ Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
+
+ Eigen::VectorXf qpos(positionSensors.size());
+ Eigen::VectorXf qvel(velocitySensors.size());
+ for (size_t i = 0; i < positionSensors.size(); ++i)
+ {
+ qpos(i) = positionSensors[i]->position;
+ qvel(i) = velocitySensors[i]->velocity;
+ }
+
+ qvel_filtered = (1 - cfg->velFilter) * qvel_filtered + cfg->velFilter * qvel;
+ Eigen::VectorXf currentTwist = jacobi * qvel_filtered;
+
+ Eigen::VectorXf targetVel(6);
+ targetVel.setZero();
+ if (firstRun || !dmpRunning)
+ {
+ lastPosition = currentPose.block<2, 1>(0, 3);
+ targetPose = currentPose;
+ firstRun = false;
+ filteredForce.setZero();
+
+ origHandOri = currentPose.block<3, 3>(0, 0);
+ toolTransform = origHandOri.transpose();
+ // force calibration
+ if (!dmpRunning)
+ {
+ forceOffset = (1 - cfg->forceFilter) * forceOffset + cfg->forceFilter * forceSensor->force;
+ }
+
+ targetVel.setZero();
+ }
+ else
+ {
+ // communicate with DMP controller
+ rtUpdateControlStruct();
+ targetVel = rtGetControlStruct().targetTSVel;
+ targetVel(2) = 0;
+
+ // force detection
+ filteredForce = (1 - cfg->forceFilter) * filteredForce + cfg->forceFilter * (forceSensor->force - forceOffset);
+
+ for (size_t i = 0; i < 3; ++i)
+ {
+ if (fabs(filteredForce(i)) > cfg->forceDeadZone)
+ {
+ filteredForce(i) -= (filteredForce(i) / fabs(filteredForce(i))) * cfg->forceDeadZone;
+ }
+ else
+ {
+ filteredForce(i) = 0;
+ }
+ }
+ Eigen::Matrix4f forceFrameInRoot = rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame();
+ filteredForceInRoot = forceFrameInRoot.block<3, 3>(0, 0) * filteredForce;
+ float targetForce = user2rtData.getUpToDateReadBuffer().targetForce;
+
+ // Eigen::Matrix3f currentHandOri = currentPose.block<3, 3>(0, 0);
+ // Eigen::Matrix3f currentToolOri = toolTransform * currentHandOri;
+ float desiredZVel = kpf * (targetForce - filteredForceInRoot(2));
+ targetVel(2) -= desiredZVel;
+ // targetVel.block<3, 1>(0, 0) = currentToolOri * targetVel.block<3, 1>(0, 0);
+
+
+ // Eigen::Vector3f currentToolDir = currentToolOri * oriToolDir;
+
+ for (int i = 3; i < 6; ++i)
+ {
+ targetVel(i) = 0;
+ }
+
+ // rotation changes
+
+ // if (filteredForceInRoot.norm() > fabs(cfg->minimumReactForce))
+ // {
+ // Eigen::Vector3f desiredToolDir = filteredForceInRoot / filteredForceInRoot.norm();
+ // currentToolDir = currentToolDir / currentToolDir.norm();
+ // Eigen::Vector3f axis = currentToolDir.cross(desiredToolDir);
+ // float angle = acosf(currentToolDir.dot(desiredToolDir));
+
+ // if (fabs(angle) < M_PI / 2)
+ // {
+ // Eigen::AngleAxisf desiredToolRot(angle, axis);
+ // Eigen::Matrix3f desiredRotMat = desiredToolRot * Eigen::Matrix3f::Identity();
+
+ // targetPose.block<3, 3>(0, 0) = desiredRotMat * currentHandOri;
+
+ // Eigen::Vector3f desiredRPY = VirtualRobot::MathTools::eigen3f2rpy(desiredRotMat);
+ // Eigen::Vector3f checkedToolDir = desiredRotMat * currentToolDir;
+ // ARMARX_IMPORTANT << "axis: " << axis;
+ // ARMARX_IMPORTANT << "angle: " << angle * 180 / 3.1415;
+ // ARMARX_IMPORTANT << "desiredRotMat: " << desiredRotMat;
+
+ // ARMARX_IMPORTANT << "desiredToolDir: " << desiredToolDir;
+ // ARMARX_IMPORTANT << "currentToolDir: " << currentToolDir;
+ // ARMARX_IMPORTANT << "checkedToolDir: " << checkedToolDir;
+ // }
+
+ // }
+
+
+ // integrate for targetPose
+
+
+
+
+
+ }
+
+ bool isPhaseStop = false;
+
+ float diff = (targetPose.block<2, 1>(0, 3) - currentPose.block<2, 1>(0, 3)).norm();
+ if (diff > cfg->phaseDist0)
+ {
+ isPhaseStop = true;
+ }
+
+ float dTf = (float)deltaT;
+
+
+ if (filteredForceInRoot.block<2, 1>(0, 0).norm() > cfg->dragForceDeadZone)
+ {
+ Eigen::Vector2f dragForce = filteredForceInRoot.block<2, 1>(0, 0) - cfg->dragForceDeadZone * filteredForceInRoot.block<2, 1>(0, 0) / filteredForceInRoot.block<2, 1>(0, 0).norm();
+ adaptK(0) = fmax(adaptK(0) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
+ adaptK(1) = fmax(adaptK(1) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
+ lastDiff = diff;
+ }
+ else
+ {
+ adaptK(0) = fmin(adaptK(0) + fabs(dTf * cfg->adaptCoeff), kpos(0));
+ adaptK(1) = fmin(adaptK(1) + fabs(dTf * cfg->adaptCoeff), kpos(1));
+ }
+ adaptK(2) = kpos(2);
+
+ // adaptive control with distance
+
+
+
+
+ targetPose.block<3, 1>(0, 3) = targetPose.block<3, 1>(0, 3) + dTf * targetVel.block<3, 1>(0, 0);
+
+ if (isPhaseStop)
+ {
+ Eigen::Vector2f currentXY = currentPose.block<2, 1>(0, 3);
+ if ((lastPosition - currentXY).norm() < cfg->changePositionTolerance)
+ {
+ changeTimer += deltaT;
+ }
+ else
+ {
+ lastPosition = currentPose.block<2, 1>(0, 3);
+ changeTimer = 0;
+ }
+
+ if (changeTimer > cfg->changeTimerThreshold)
+ {
+ targetPose(0, 3) = currentPose(0, 3);
+ targetPose(1, 3) = currentPose(1, 3);
+ isPhaseStop = false;
+ changeTimer = 0;
+ }
+ }
+ else
+ {
+ changeTimer = 0;
+ }
+
+
+ targetPose(0, 3) = targetPose(0, 3) > cfg->ws_x[0] ? targetPose(0, 3) : cfg->ws_x[0];
+ targetPose(0, 3) = targetPose(0, 3) < cfg->ws_x[1] ? targetPose(0, 3) : cfg->ws_x[1];
+
+ targetPose(1, 3) = targetPose(1, 3) > cfg->ws_y[0] ? targetPose(1, 3) : cfg->ws_y[0];
+ targetPose(1, 3) = targetPose(1, 3) < cfg->ws_y[1] ? targetPose(1, 3) : cfg->ws_y[1];
+
+ targetPose(2, 3) = targetPose(2, 3) > cfg->ws_z[0] ? targetPose(2, 3) : cfg->ws_z[0];
+ targetPose(2, 3) = targetPose(2, 3) < cfg->ws_z[1] ? targetPose(2, 3) : cfg->ws_z[1];
+
+
+
+
+ debugRT.getWriteBuffer().targetPose = targetPose;
+ debugRT.getWriteBuffer().currentPose = currentPose;
+ debugRT.getWriteBuffer().filteredForce = filteredForceInRoot;
+ debugRT.getWriteBuffer().targetVel = targetVel;
+ debugRT.getWriteBuffer().adaptK = adaptK;
+ debugRT.getWriteBuffer().isPhaseStop = isPhaseStop;
+ debugRT.commitWrite();
+
+ rt2CtrlData.getWriteBuffer().currentPose = currentPose;
+ rt2CtrlData.getWriteBuffer().currentTwist = currentTwist;
+ rt2CtrlData.getWriteBuffer().deltaT = deltaT;
+ rt2CtrlData.getWriteBuffer().currentTime += deltaT;
+ rt2CtrlData.getWriteBuffer().isPhaseStop = isPhaseStop;
+ rt2CtrlData.commitWrite();
+
+ // Eigen::Matrix3f rotVel = VirtualRobot::MathTools::rpy2eigen3f(targetVel(3) * dTf, targetVel(4) * dTf, targetVel(5) * dTf);
+ // targetPose.block<3, 3>(0, 0) = rotVel * targetPose.block<3, 3>(0, 0);
+
+ // inverse dynamic controller
+ jacobi.block<3, 8>(0, 0) = 0.001 * jacobi.block<3, 8>(0, 0); // convert mm to m
+
+
+
+
+ Eigen::Vector6f jointControlWrench;
+ {
+ 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);
+ Eigen::Vector3f currentTCPPosition = currentPose.block<3, 1>(0, 3);
+ Eigen::Vector3f desiredPosition = targetPose.block<3, 1>(0, 3);
+
+ Eigen::Vector3f linearVel = adaptK.cwiseProduct(desiredPosition - currentTCPPosition);
+
+ // if (isPhaseStop)
+ // {
+ // linearVel = ((float)cfg->phaseKpPos) * (desiredPosition - currentTCPPosition);
+ // for (size_t i = 0; i < 3; ++i)
+ // {
+ // linearVel(i) = fmin(cfg->maxLinearVel, linearVel(i));
+ // }
+ // }
+ // else
+ // {
+ // linearVel = kpos.cwiseProduct(desiredPosition - currentTCPPosition);
+ // }
+ Eigen::Vector3f tcpDesiredForce = 0.001 * linearVel + dpos.cwiseProduct(- currentTCPLinearVelocity);
+
+ Eigen::Vector3f currentTCPAngularVelocity;
+ 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;
+ }
+
+
+
+ Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
+ Eigen::VectorXf nullspaceTorque = knull * (nullSpaceJointsVec - qpos) - dnull * qvel;
+ Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
+ Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
+
+ // torque filter (maybe)
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ targets.at(i)->torque = jointDesiredTorques(i);
+
+ if (!targets.at(i)->isValid())
+ {
+ targets.at(i)->torque = 0.0f;
+ }
+ else
+ {
+ if (fabs(targets.at(i)->torque) > fabs(cfg->maxJointTorque))
+ {
+ targets.at(i)->torque = fabs(cfg->maxJointTorque) * (targets.at(i)->torque / fabs(targets.at(i)->torque));
+ }
+ }
+ }
+
+
+ }
+
+
+ void NJointPeriodicTSDMPCompliantController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
+ {
+ ARMARX_INFO << "Learning DMP ... ";
+
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->learnDMPFromFiles(fileNames);
+
+ }
+
+ void NJointPeriodicTSDMPCompliantController::learnDMPFromTrajectory(const TrajectoryBasePtr& trajectory, const Ice::Current&)
+ {
+ ARMARX_INFO << "Learning DMP ... ";
+ ARMARX_CHECK_EXPRESSION(trajectory);
+ TrajectoryPtr dmpTraj = TrajectoryPtr::dynamicCast(trajectory);
+ ARMARX_CHECK_EXPRESSION(dmpTraj);
+
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->learnDMPFromTrajectory(dmpTraj);
+
+ }
+
+ void NJointPeriodicTSDMPCompliantController::setSpeed(Ice::Double times, const Ice::Current&)
+ {
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->setSpeed(times);
+ }
+
+
+ void NJointPeriodicTSDMPCompliantController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
+ {
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->setGoalPoseVec(goals);
+ }
+
+ void NJointPeriodicTSDMPCompliantController::setAmplitude(Ice::Double amp, const Ice::Current&)
+ {
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->setAmplitude(amp);
+ }
+
+ void NJointPeriodicTSDMPCompliantController::setTargetForceInRootFrame(float targetForce, const Ice::Current&)
+ {
+ LockGuardType guard {controlDataMutex};
+ user2rtData.getWriteBuffer().targetForce = targetForce;
+ user2rtData.commitWrite();
+ }
+
+ void NJointPeriodicTSDMPCompliantController::runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&)
+ {
+ firstRun = true;
+ while (firstRun || rt2UserData.getUpToDateReadBuffer().waitTimeForCalibration < cfg->waitTimeForCalibration)
+ {
+ usleep(100);
+ }
+
+
+ Eigen::Matrix4f pose = rt2UserData.getUpToDateReadBuffer().currentTcpPose;
+
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
+ dmpCtrl->setSpeed(tau);
+
+ ARMARX_IMPORTANT << "run DMP";
+ started = true;
+ dmpRunning = false;
+ }
+
+
+ void NJointPeriodicTSDMPCompliantController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
+ {
+ std::string datafieldName;
+ std::string debugName = "Periodic";
+ StringVariantBaseMap datafields;
+
+ Eigen::Matrix4f targetPoseDebug = debugRT.getUpToDateReadBuffer().targetPose;
+ datafields["target_x"] = new Variant(targetPoseDebug(0, 3));
+ datafields["target_y"] = new Variant(targetPoseDebug(1, 3));
+ datafields["target_z"] = new Variant(targetPoseDebug(2, 3));
+
+ Eigen::Matrix4f currentPoseDebug = debugRT.getUpToDateReadBuffer().currentPose;
+ datafields["current_x"] = new Variant(currentPoseDebug(0, 3));
+ datafields["current_y"] = new Variant(currentPoseDebug(1, 3));
+ datafields["current_z"] = new Variant(currentPoseDebug(2, 3));
+
+ Eigen::Vector3f filteredForce = debugRT.getUpToDateReadBuffer().filteredForce;
+ datafields["filteredforce_x"] = new Variant(filteredForce(0));
+ datafields["filteredforce_y"] = new Variant(filteredForce(1));
+ datafields["filteredforce_z"] = new Variant(filteredForce(2));
+
+
+ Eigen::Vector3f reactForce = debugRT.getUpToDateReadBuffer().reactForce;
+ datafields["reactForce_x"] = new Variant(reactForce(0));
+ datafields["reactForce_y"] = new Variant(reactForce(1));
+ datafields["reactForce_z"] = new Variant(reactForce(2));
+
+ Eigen::VectorXf targetVel = debugRT.getUpToDateReadBuffer().targetVel;
+ datafields["targetVel_x"] = new Variant(targetVel(0));
+ datafields["targetVel_y"] = new Variant(targetVel(1));
+ datafields["targetVel_z"] = new Variant(targetVel(2));
+
+ Eigen::Vector3f adaptK = debugRT.getUpToDateReadBuffer().adaptK;
+ datafields["adaptK_x"] = new Variant(adaptK(0));
+ datafields["adaptK_y"] = new Variant(adaptK(1));
+
+ datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
+ datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
+
+ datafields["PhaseStop"] = new Variant(debugRT.getUpToDateReadBuffer().isPhaseStop);
+
+
+ // datafields["targetVel_rx"] = new Variant(targetVel(3));
+ // datafields["targetVel_ry"] = new Variant(targetVel(4));
+ // datafields["targetVel_rz"] = new Variant(targetVel(5));
+
+ // auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
+ // for (auto& pair : values)
+ // {
+ // datafieldName = pair.first + "_" + debugName;
+ // datafields[datafieldName] = new Variant(pair.second);
+ // }
+
+ // auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
+ // for (auto& pair : currentPose)
+ // {
+ // datafieldName = pair.first + "_" + debugName;
+ // datafields[datafieldName] = new Variant(pair.second);
+ // }
+
+ // datafieldName = "canVal_" + debugName;
+ // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
+ // datafieldName = "mpcFactor_" + debugName;
+ // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
+ // datafieldName = "error_" + debugName;
+ // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
+ // datafieldName = "posError_" + debugName;
+ // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
+ // datafieldName = "oriError_" + debugName;
+ // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
+ // datafieldName = "deltaT_" + debugName;
+ // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
+
+ datafieldName = "PeriodicDMP";
+ debugObs->setDebugChannel(datafieldName, datafields);
+ }
+
+
+
+ void NJointPeriodicTSDMPCompliantController::onDisconnectNJointController()
+ {
+ ARMARX_INFO << "stopped ...";
+ }
+
+
+
+}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
new file mode 100644
index 0000000000000000000000000000000000000000..8b59a968731f1404f917d1a3f68e6a81dadd43ed
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
@@ -0,0 +1,192 @@
+
+#pragma once
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <VirtualRobot/Robot.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+
+#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
+#include <RobotAPI/libraries/core/Trajectory.h>
+
+namespace armarx
+{
+
+
+ TYPEDEF_PTRS_HANDLE(NJointPeriodicTSDMPCompliantController);
+ TYPEDEF_PTRS_HANDLE(NJointPeriodicTSDMPCompliantControllerControlData);
+
+ class NJointPeriodicTSDMPCompliantControllerControlData
+ {
+ public:
+ Eigen::VectorXf targetTSVel;
+ };
+
+ class pidController
+ {
+ public:
+ float Kp = 0, Kd = 0;
+ float lastError = 0;
+ float update(float dt, float error)
+ {
+ float derivative = (error - lastError) / dt;
+ float retVal = Kp * error + Kd * derivative;
+ lastError = error;
+ return retVal;
+ }
+ };
+
+ /**
+ * @brief The NJointPeriodicTSDMPCompliantController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
+ class NJointPeriodicTSDMPCompliantController :
+ public NJointControllerWithTripleBuffer<NJointPeriodicTSDMPCompliantControllerControlData>,
+ public NJointPeriodicTSDMPCompliantControllerInterface
+ {
+ public:
+ using ConfigPtrT = NJointPeriodicTSDMPCompliantControllerConfigPtr;
+ NJointPeriodicTSDMPCompliantController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+ // NJointControllerInterface interface
+ std::string getClassName(const Ice::Current&) const;
+
+ // NJointController interface
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+
+ // NJointPeriodicTSDMPCompliantControllerInterface interface
+ void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
+ void learnDMPFromTrajectory(const TrajectoryBasePtr& trajectory, const Ice::Current&);
+ bool isFinished(const Ice::Current&)
+ {
+ return false;
+ }
+
+ void setSpeed(Ice::Double times, const Ice::Current&);
+ void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
+ void setAmplitude(Ice::Double amp, const Ice::Current&);
+ void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
+ void setTargetForceInRootFrame(Ice::Float force, const Ice::Current&);
+ double getCanVal(const Ice::Current&)
+ {
+ return dmpCtrl->canVal;
+ }
+
+ protected:
+ virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
+
+ void onInitNJointController();
+ void onDisconnectNJointController();
+ void controllerRun();
+
+ private:
+ struct DebugBufferData
+ {
+ StringFloatDictionary latestTargetVelocities;
+ StringFloatDictionary currentPose;
+ double currentCanVal;
+ double mpcFactor;
+ double error;
+ double phaseStop;
+ double posError;
+ double oriError;
+ double deltaT;
+ };
+
+ TripleBuffer<DebugBufferData> debugOutputData;
+
+
+ struct DebugRTData
+ {
+ Eigen::Matrix4f targetPose;
+ Eigen::Vector3f filteredForce;
+ Eigen::Vector3f reactForce;
+ Eigen::Vector3f adaptK;
+ Eigen::VectorXf targetVel;
+ Eigen::Matrix4f currentPose;
+ bool isPhaseStop;
+ };
+ TripleBuffer<DebugRTData> debugRT;
+
+ struct RTToControllerData
+ {
+ double currentTime;
+ double deltaT;
+ Eigen::Matrix4f currentPose;
+ Eigen::VectorXf currentTwist;
+ bool isPhaseStop;
+ };
+ TripleBuffer<RTToControllerData> rt2CtrlData;
+
+ struct RTToUserData
+ {
+ Eigen::Matrix4f currentTcpPose;
+ float waitTimeForCalibration;
+ };
+ TripleBuffer<RTToUserData> rt2UserData;
+
+ struct UserToRTData
+ {
+ float targetForce;
+ };
+ TripleBuffer<UserToRTData> user2rtData;
+
+
+ TaskSpaceDMPControllerPtr dmpCtrl;
+
+ std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
+ std::vector<ControlTarget1DoFActuatorTorque*> targets;
+
+ // velocity ik controller parameters
+ std::string nodeSetName;
+
+ bool started;
+ bool firstRun;
+ bool dmpRunning;
+
+ VirtualRobot::DifferentialIKPtr ik;
+ VirtualRobot::RobotNodePtr tcp;
+
+ NJointPeriodicTSDMPCompliantControllerConfigPtr cfg;
+ mutable MutexType controllerMutex;
+ PeriodicTask<NJointPeriodicTSDMPCompliantController>::pointer_type controllerTask;
+ Eigen::Matrix4f targetPose;
+
+ Eigen::Vector3f kpos;
+ Eigen::Vector3f dpos;
+ Eigen::Vector3f kori;
+ Eigen::Vector3f dori;
+ float knull;
+ float dnull;
+ float kpf;
+
+ Eigen::VectorXf nullSpaceJointsVec;
+ const SensorValueForceTorque* forceSensor;
+
+ Eigen::Vector3f filteredForce;
+ Eigen::Vector3f forceOffset;
+ Eigen::Vector3f filteredForceInRoot;
+
+ Eigen::Matrix3f toolTransform;
+ Eigen::Vector3f oriToolDir;
+ Eigen::Matrix3f origHandOri;
+ Eigen::VectorXf qvel_filtered;
+
+ Eigen::Vector3f adaptK;
+ float lastDiff;
+ Eigen::Vector2f lastPosition;
+ double changeTimer;
+
+ };
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8d892d2c7ddf40dbef0b3bace0835d6890b848b7
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp
@@ -0,0 +1,427 @@
+#include "NJointPeriodicTSDMPForwardVelController.h"
+
+namespace armarx
+{
+ NJointControllerRegistration<NJointPeriodicTSDMPForwardVelController> registrationControllerNJointPeriodicTSDMPForwardVelController("NJointPeriodicTSDMPForwardVelController");
+
+ NJointPeriodicTSDMPForwardVelController::NJointPeriodicTSDMPForwardVelController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ {
+ useSynchronizedRtRobot();
+ cfg = NJointPeriodicTSDMPControllerConfigPtr::dynamicCast(config);
+ ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
+ VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
+
+ ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName);
+ for (size_t i = 0; i < rns->getSize(); ++i)
+ {
+ std::string jointName = rns->getNode(i)->getName();
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ const SensorValueBase* sv = useSensorValue(jointName);
+ targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
+
+ const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+
+ velocitySensors.push_back(velocitySensor);
+ positionSensors.push_back(positionSensor);
+ };
+
+ tcp = rns->getTCP();
+ // set tcp controller
+ tcpController.reset(new CartesianVelocityController(rns, tcp));
+ nodeSetName = cfg->nodeSetName;
+ ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+
+
+
+ TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
+ taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
+ taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
+ taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
+
+ taskSpaceDMPConfig.DMPMode = "Linear";
+ taskSpaceDMPConfig.DMPStyle = "Periodic";
+ taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
+ taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
+ taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
+ taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
+ taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
+ taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
+ taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
+ taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
+ taskSpaceDMPConfig.phaseStopParas.Dori = 0;
+
+
+ dmpCtrl.reset(new TaskSpaceDMPController("periodicDMP", taskSpaceDMPConfig, false));
+
+ NJointPeriodicTSDMPForwardVelControllerControlData initData;
+ initData.targetPose = tcp->getPoseInRootFrame();
+ initData.targetTSVel.resize(6);
+ for (size_t i = 0; i < 6; ++i)
+ {
+ initData.targetTSVel(i) = 0;
+ }
+ reinitTripleBuffer(initData);
+
+ finished = false;
+ firstRun = true;
+
+
+ const SensorValueBase* svlf = robUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
+ forceSensor = svlf->asA<SensorValueForceTorque>();
+
+ forceOffset.setZero();
+ filteredForce.setZero();
+
+ UserToRTData initUserData;
+ initUserData.targetForce = 0;
+ user2rtData.reinitAllBuffers(initUserData);
+
+ oriToolDir << 0, 0, 1;
+
+ kpf = cfg->Kpf;
+
+ }
+
+ std::string NJointPeriodicTSDMPForwardVelController::getClassName(const Ice::Current&) const
+ {
+ return "NJointPeriodicTSDMPForwardVelController";
+ }
+
+ void NJointPeriodicTSDMPForwardVelController::controllerRun()
+ {
+ if (!started)
+ {
+ return;
+ }
+
+ if (!rt2CtrlData.updateReadBuffer() || !dmpCtrl)
+ {
+ return;
+ }
+
+ double deltaT = rt2CtrlData.getReadBuffer().deltaT;
+ Eigen::Matrix4f currentPose = rt2CtrlData.getReadBuffer().currentPose;
+ Eigen::VectorXf currentTwist = rt2CtrlData.getReadBuffer().currentTwist;
+
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->flow(deltaT, currentPose, currentTwist);
+
+ Eigen::VectorXf targetVels = dmpCtrl->getTargetVelocity();
+ Eigen::Matrix4f targetPose = dmpCtrl->getIntegratedPoseMat();
+
+ debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
+ debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
+ debugOutputData.getWriteBuffer().currentPose["currentPose_x"] = currentPose(0, 3);
+ debugOutputData.getWriteBuffer().currentPose["currentPose_y"] = currentPose(1, 3);
+ debugOutputData.getWriteBuffer().currentPose["currentPose_z"] = currentPose(2, 3);
+ VirtualRobot::MathTools::Quaternion currentQ = VirtualRobot::MathTools::eigen4f2quat(currentPose);
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qw"] = currentQ.w;
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
+ debugOutputData.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
+ debugOutputData.getWriteBuffer().currentCanVal = dmpCtrl->debugData.canVal;
+ debugOutputData.getWriteBuffer().mpcFactor = dmpCtrl->debugData.mpcFactor;
+ debugOutputData.getWriteBuffer().error = dmpCtrl->debugData.poseError;
+ debugOutputData.getWriteBuffer().posError = dmpCtrl->debugData.posiError;
+ debugOutputData.getWriteBuffer().oriError = dmpCtrl->debugData.oriError;
+ debugOutputData.getWriteBuffer().deltaT = deltaT;
+ debugOutputData.commitWrite();
+
+ getWriterControlStruct().targetTSVel = targetVels;
+ getWriterControlStruct().targetPose = targetPose;
+ writeControlStruct();
+
+ dmpRunning = true;
+ }
+
+
+ void NJointPeriodicTSDMPForwardVelController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ double deltaT = timeSinceLastIteration.toSecondsDouble();
+
+ Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
+ rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
+ rt2UserData.getWriteBuffer().waitTimeForCalibration += deltaT;
+ rt2UserData.commitWrite();
+
+ if (firstRun || !dmpRunning)
+ {
+ targetPose = currentPose;
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ targets.at(i)->velocity = 0.0f;
+ }
+ firstRun = false;
+ filteredForce.setZero();
+
+ Eigen::Matrix3f currentHandOri = currentPose.block<3, 3>(0, 0);
+ toolTransform = currentHandOri.transpose();
+ // force calibration
+ if (!dmpRunning)
+ {
+ forceOffset = (1 - cfg->forceFilter) * forceOffset + cfg->forceFilter * forceSensor->force;
+ }
+ }
+ else
+ {
+
+ Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
+
+ Eigen::VectorXf qvel;
+ qvel.resize(velocitySensors.size());
+ for (size_t i = 0; i < velocitySensors.size(); ++i)
+ {
+ qvel(i) = velocitySensors[i]->velocity;
+ }
+
+ Eigen::VectorXf tcptwist = jacobi * qvel;
+
+ rt2CtrlData.getWriteBuffer().currentPose = currentPose;
+ rt2CtrlData.getWriteBuffer().currentTwist = tcptwist;
+ rt2CtrlData.getWriteBuffer().deltaT = deltaT;
+ rt2CtrlData.getWriteBuffer().currentTime += deltaT;
+ rt2CtrlData.commitWrite();
+
+
+ // forward controller
+ rtUpdateControlStruct();
+ Eigen::VectorXf targetVel = rtGetControlStruct().targetTSVel;
+ // Eigen::Matrix4f targetPose = rtGetControlStruct().targetPose;
+
+ // force detection
+ // filteredForce = (1 - cfg->forceFilter) * filteredForce + cfg->forceFilter * (forceSensor->force - forceOffset);
+ // Eigen::Vector3f filteredForceInRoot = rtGetRobot()->getRobotNode(cfg->forceFrameName)->toGlobalCoordinateSystemVec(filteredForce);
+ // filteredForceInRoot = rtGetRobot()->getRootNode()->toLocalCoordinateSystemVec(filteredForceInRoot);
+ // float targetForce = user2rtData.getUpToDateReadBuffer().targetForce;
+
+ // Eigen::Matrix3f currentHandOri = currentPose.block<3, 3>(0, 0);
+ // Eigen::Matrix3f currentToolOri = toolTransform * currentHandOri;
+ // targetVel.block<3, 1>(0, 0) = currentToolOri * targetVel.block<3, 1>(0, 0);
+ // Eigen::Vector3f currentToolDir = currentToolOri * oriToolDir;
+
+ // ARMARX_IMPORTANT << "original force: " << forceSensor->force;
+ // ARMARX_IMPORTANT << "filteredForce: " << filteredForce;
+ // ARMARX_IMPORTANT << "filteredForceInRoot: " << filteredForceInRoot;
+ // ARMARX_IMPORTANT << "forceOffset: " << forceOffset;
+ // ARMARX_IMPORTANT << "currentToolOri: " << currentToolOri;
+
+ for (size_t i = 3; i < 6; ++i)
+ {
+ targetVel(i) = 0;
+ }
+
+ // float forceCtrl = kpf * (targetForce - filteredForceInRoot.norm());
+ // Eigen::Vector3f desiredZVel = - forceCtrl * (currentToolDir / currentToolDir.norm());
+ // targetVel.block<3, 1>(0, 0) += desiredZVel;
+
+ // dead zone for force
+ // if (filteredForceInRoot.norm() > cfg->minimumReactForce)
+ // {
+ // // rotation changes
+ // Eigen::Vector3f axis = oriToolDir.cross(filteredForceInRoot);
+ // float angle = oriToolDir.dot(filteredForceInRoot);
+ // Eigen::AngleAxisf desiredToolOri(angle, axis);
+ // Eigen::Matrix3f desiredHandOri = toolTransform.transpose() * desiredToolOri;
+ // Eigen::Matrix3f desiredRotMat = desiredHandOri * currentHandOri.transpose();
+ // Eigen::Vector3f desiredRPY = VirtualRobot::MathTools::eigen3f2rpy(desiredRotMat);
+ // for (size_t i = 3; i < 6; ++i)
+ // {
+ // targetVel(i) = desiredRPY(i - 3);
+ // }
+ // }}
+
+ // ARMARX_IMPORTANT << "targetVel: " << targetVel;
+ // ARMARX_IMPORTANT << "targetPose: " << targetPose;
+
+ // targetPose.block<3, 1>(0, 3) += deltaT * targetVel.block<3, 1>(0, 0);
+ // Eigen::Matrix3f rotVel = VirtualRobot::MathTools::rpy2eigen3f(targetVel(3) * deltaT, targetVel(4) * deltaT, targetVel(5) * deltaT);
+ // targetPose.block<3, 3>(0, 0) = rotVel * targetPose.block<3, 3>(0, 0);
+
+ float dTf = (float)deltaT;
+ targetPose.block<3, 1>(0, 3) = targetPose.block<3, 1>(0, 3) + dTf * targetVel.block<3, 1>(0, 0);
+ Eigen::Matrix3f rotVel = VirtualRobot::MathTools::rpy2eigen3f(targetVel(3) * dTf, targetVel(4) * dTf, targetVel(5) * dTf);
+ targetPose.block<3, 3>(0, 0) = rotVel * targetPose.block<3, 3>(0, 0);
+
+ ARMARX_IMPORTANT << "targetVel: " << targetVel.block<3, 1>(0, 0);
+ ARMARX_IMPORTANT << "targetPose: " << targetPose;
+ ARMARX_IMPORTANT << "deltaT: " << deltaT;
+
+ Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).inverse();
+ Eigen::Vector3f errorRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
+
+ Eigen::VectorXf rtTargetVel = targetVel;
+ rtTargetVel.block<3, 1>(0, 0) += cfg->Kpos * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3));
+ rtTargetVel.block<3, 1>(3, 0) += cfg->Kori * errorRPY;
+
+ float normLinearVelocity = rtTargetVel.block<3, 1>(0, 0).norm();
+ if (normLinearVelocity > fabs(cfg->maxLinearVel))
+ {
+ rtTargetVel.block<3, 1>(0, 0) = fabs(cfg->maxLinearVel) * rtTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
+ }
+
+ float normAngularVelocity = rtTargetVel.block<3, 1>(3, 0).norm();
+ if (normAngularVelocity > fabs(cfg->maxAngularVel))
+ {
+ rtTargetVel.block<3, 1>(3, 0) = fabs(cfg->maxAngularVel) * rtTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
+ }
+
+
+ // cartesian vel controller
+
+ Eigen::VectorXf jnv = Eigen::VectorXf::Zero(tcpController->rns->getSize());
+ if (cfg->avoidJointLimitsKp > 0)
+ {
+ jnv += cfg->avoidJointLimitsKp * tcpController->calculateJointLimitAvoidance();
+ }
+
+ Eigen::VectorXf jointTargetVelocities = tcpController->calculate(rtTargetVel, jnv, VirtualRobot::IKSolver::CartesianSelection::All);
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ targets.at(i)->velocity = jointTargetVelocities(i);
+ if (!targets.at(i)->isValid())
+ {
+ targets.at(i)->velocity = 0.0f;
+ }
+ else
+ {
+ if (fabs(targets.at(i)->velocity) > fabs(cfg->maxJointVel))
+ {
+ targets.at(i)->velocity = fabs(cfg->maxJointVel) * (targets.at(i)->velocity / fabs(targets.at(i)->velocity));
+ }
+ }
+ }
+ }
+
+ }
+
+
+ void NJointPeriodicTSDMPForwardVelController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
+ {
+ ARMARX_INFO << "Learning DMP ... ";
+
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->learnDMPFromFiles(fileNames);
+
+ }
+
+ void NJointPeriodicTSDMPForwardVelController::setSpeed(Ice::Double times, const Ice::Current&)
+ {
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->setSpeed(times);
+ }
+
+
+ void NJointPeriodicTSDMPForwardVelController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
+ {
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->setGoalPoseVec(goals);
+ }
+
+ void NJointPeriodicTSDMPForwardVelController::setAmplitude(Ice::Double amp, const Ice::Current&)
+ {
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->setAmplitude(amp);
+ }
+
+ void NJointPeriodicTSDMPForwardVelController::runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&)
+ {
+ firstRun = true;
+ while (firstRun || rt2UserData.getUpToDateReadBuffer().waitTimeForCalibration < cfg->waitTimeForCalibration)
+ {
+ usleep(100);
+ }
+
+ Eigen::Matrix4f pose = rt2UserData.getUpToDateReadBuffer().currentTcpPose;
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
+ dmpCtrl->setSpeed(tau);
+ finished = false;
+
+ ARMARX_INFO << "run DMP";
+ started = true;
+ dmpRunning = false;
+ }
+
+
+ void NJointPeriodicTSDMPForwardVelController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
+ {
+ std::string datafieldName;
+ std::string debugName = "Periodic";
+ StringVariantBaseMap datafields;
+ auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
+ for (auto& pair : values)
+ {
+ datafieldName = pair.first + "_" + debugName;
+ datafields[datafieldName] = new Variant(pair.second);
+ }
+
+ auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
+ for (auto& pair : currentPose)
+ {
+ datafieldName = pair.first + "_" + debugName;
+ datafields[datafieldName] = new Variant(pair.second);
+ }
+
+ datafieldName = "canVal_" + debugName;
+ datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
+ datafieldName = "mpcFactor_" + debugName;
+ datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
+ datafieldName = "error_" + debugName;
+ datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
+ datafieldName = "posError_" + debugName;
+ datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
+ datafieldName = "oriError_" + debugName;
+ datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
+ datafieldName = "deltaT_" + debugName;
+ datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
+ datafieldName = "DMPController_" + debugName;
+
+ debugObs->setDebugChannel(datafieldName, datafields);
+ }
+
+ void NJointPeriodicTSDMPForwardVelController::onInitNJointController()
+ {
+ ARMARX_INFO << "init ...";
+
+ RTToControllerData initSensorData;
+ initSensorData.deltaT = 0;
+ initSensorData.currentTime = 0;
+ initSensorData.currentPose = tcp->getPoseInRootFrame();
+ initSensorData.currentTwist.setZero();
+ rt2CtrlData.reinitAllBuffers(initSensorData);
+
+ RTToUserData initInterfaceData;
+ initInterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
+ rt2UserData.reinitAllBuffers(initInterfaceData);
+
+
+ started = false;
+ runTask("NJointPeriodicTSDMPForwardVelController", [&]
+ {
+ CycleUtil c(1);
+ getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
+ while (getState() == eManagedIceObjectStarted)
+ {
+ if (isControllerActive())
+ {
+ controllerRun();
+ }
+ c.waitForCycleDuration();
+ }
+ });
+
+ }
+
+ void NJointPeriodicTSDMPForwardVelController::onDisconnectNJointController()
+ {
+ ARMARX_INFO << "stopped ...";
+ }
+
+
+
+}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp.F24458 b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp.F24458
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.h
new file mode 100644
index 0000000000000000000000000000000000000000..351a9451c601ca2c61fc0416d016f41ecd4a3b73
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.h
@@ -0,0 +1,166 @@
+
+#pragma once
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <VirtualRobot/Robot.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+
+#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
+
+namespace armarx
+{
+
+
+ TYPEDEF_PTRS_HANDLE(NJointPeriodicTSDMPForwardVelController);
+ TYPEDEF_PTRS_HANDLE(NJointPeriodicTSDMPForwardVelControllerControlData);
+
+ class NJointPeriodicTSDMPForwardVelControllerControlData
+ {
+ public:
+ Eigen::VectorXf targetTSVel;
+ Eigen::Matrix4f targetPose;
+ };
+
+ class pidController
+ {
+ public:
+ float Kp = 0, Kd = 0;
+ float lastError = 0;
+ float update(float dt, float error)
+ {
+ float derivative = (error - lastError) / dt;
+ float retVal = Kp * error + Kd * derivative;
+ lastError = error;
+ return retVal;
+ }
+ };
+
+ /**
+ * @brief The NJointPeriodicTSDMPForwardVelController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
+ class NJointPeriodicTSDMPForwardVelController :
+ public NJointControllerWithTripleBuffer<NJointPeriodicTSDMPForwardVelControllerControlData>,
+ public NJointPeriodicTSDMPControllerInterface
+ {
+ public:
+ using ConfigPtrT = NJointPeriodicTSDMPControllerConfigPtr;
+ NJointPeriodicTSDMPForwardVelController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+ // NJointControllerInterface interface
+ std::string getClassName(const Ice::Current&) const;
+
+ // NJointController interface
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+
+ // NJointPeriodicTSDMPForwardVelControllerInterface interface
+ void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
+ bool isFinished(const Ice::Current&)
+ {
+ return finished;
+ }
+
+ void runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&);
+ void setSpeed(Ice::Double times, const Ice::Current&);
+ void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
+ void setAmplitude(Ice::Double amp, const Ice::Current&);
+
+
+ double getCanVal(const Ice::Current&)
+ {
+ return dmpCtrl->canVal;
+ }
+
+ protected:
+ virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
+
+ void onInitNJointController();
+ void onDisconnectNJointController();
+ void controllerRun();
+
+ private:
+ struct DebugBufferData
+ {
+ StringFloatDictionary latestTargetVelocities;
+ StringFloatDictionary currentPose;
+ double currentCanVal;
+ double mpcFactor;
+ double error;
+ double phaseStop;
+ double posError;
+ double oriError;
+ double deltaT;
+ };
+
+ TripleBuffer<DebugBufferData> debugOutputData;
+
+ struct RTToControllerData
+ {
+ double currentTime;
+ double deltaT;
+ Eigen::Matrix4f currentPose;
+ Eigen::VectorXf currentTwist;
+ };
+ TripleBuffer<RTToControllerData> rt2CtrlData;
+
+ struct RTToUserData
+ {
+ Eigen::Matrix4f currentTcpPose;
+ float waitTimeForCalibration;
+ };
+ TripleBuffer<RTToUserData> rt2UserData;
+
+
+ TaskSpaceDMPControllerPtr dmpCtrl;
+
+ std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
+ std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
+ std::vector<ControlTarget1DoFActuatorVelocity*> targets;
+
+ // velocity ik controller parameters
+ CartesianVelocityControllerPtr tcpController;
+ std::string nodeSetName;
+
+ // dmp parameters
+ bool finished;
+ bool started;
+ bool dmpRunning;
+ bool firstRun;
+
+ VirtualRobot::DifferentialIKPtr ik;
+ VirtualRobot::RobotNodePtr tcp;
+
+ NJointPeriodicTSDMPControllerConfigPtr cfg;
+ mutable MutexType controllerMutex;
+ PeriodicTask<NJointPeriodicTSDMPForwardVelController>::pointer_type controllerTask;
+
+ Eigen::Matrix4f targetPose;
+
+
+ const SensorValueForceTorque* forceSensor;
+ Eigen::Vector3f filteredForce;
+ Eigen::Vector3f forceOffset;
+ Eigen::Matrix3f toolTransform;
+ Eigen::Vector3f oriToolDir;
+ struct UserToRTData
+ {
+ float targetForce;
+ };
+ TripleBuffer<UserToRTData> user2rtData;
+ float kpf;
+
+ // NJointPeriodicTSDMPControllerInterface interface
+
+ };
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
index b24cb76fb61f8be9726f42bb58e9094bf68e4842..25e5943508f959ae1b505e248e833c51e0c7ad1a 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
@@ -59,19 +59,29 @@ namespace armarx
taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
- taskSpaceDMPController.reset(new TaskSpaceDMPController("default", taskSpaceDMPConfig, false));
+ dmpCtrl.reset(new TaskSpaceDMPController("default", taskSpaceDMPConfig, false));
// initialize tcp position and orientation
- NJointTSDMPControllerSensorData initSensorData;
+ RTToControllerData initSensorData;
initSensorData.deltaT = 0;
initSensorData.currentTime = 0;
initSensorData.currentPose.setZero();
initSensorData.currentTwist.setZero();
- controllerSensorData.reinitAllBuffers(initSensorData);
+ rt2CtrlData.reinitAllBuffers(initSensorData);
+ targetVels.resize(6);
+ NJointTSDMPControllerControlData initData;
+ initData.targetTSVel.resize(6);
+ for (size_t i = 0; i < 6; ++i)
+ {
+ initData.targetTSVel(i) = 0;
+ targetVels(i) = 0;
+ }
+ initData.nullspaceJointVelocities.resize(tcpController->rns->getSize(), 0);
+ reinitTripleBuffer(initData);
debugName = cfg->debugName;
@@ -105,6 +115,9 @@ namespace armarx
NJointTSDMPControllerInterfaceData initInterfaceData;
initInterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
interfaceData.reinitAllBuffers(initInterfaceData);
+ RTToUserData initInterfaceData2;
+ initInterfaceData2.currentTcpPose = Eigen::Matrix4f::Identity();
+ rt2UserData.reinitAllBuffers(initInterfaceData2);
}
std::string NJointTSDMPController::getClassName(const Ice::Current&) const
@@ -119,31 +132,25 @@ namespace armarx
return;
}
- if (!controllerSensorData.updateReadBuffer() || !taskSpaceDMPController)
+ if (!rt2CtrlData.updateReadBuffer() || !dmpCtrl)
{
return;
}
- double deltaT = controllerSensorData.getReadBuffer().deltaT;
- Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
- Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
+ double deltaT = rt2CtrlData.getReadBuffer().deltaT;
+ Eigen::Matrix4f currentPose = rt2CtrlData.getReadBuffer().currentPose;
+ Eigen::VectorXf currentTwist = rt2CtrlData.getReadBuffer().currentTwist;
LockGuardType guard {controllerMutex};
- taskSpaceDMPController->flow(deltaT, currentPose, currentTwist);
+ dmpCtrl->flow(deltaT, currentPose, currentTwist);
-
- if (taskSpaceDMPController->canVal < 1e-8)
+ if (dmpCtrl->canVal < 1e-8)
{
finished = true;
}
- targetVels = taskSpaceDMPController->getTargetVelocity();
- targetPose = taskSpaceDMPController->getTargetPoseMat();
- std::vector<double> targetState = taskSpaceDMPController->getTargetPose();
-
- // ARMARX_IMPORTANT << "CanVal: " << taskSpaceDMPController->canVal;
- // ARMARX_IMPORTANT << "currentPose: " << currentPose;
- // ARMARX_IMPORTANT << "targetVels: " << targetVels;
- // ARMARX_IMPORTANT << "targetPose: " << targetPose;
+ targetVels = dmpCtrl->getTargetVelocity();
+ targetPose = dmpCtrl->getTargetPoseMat();
+ std::vector<double> targetState = dmpCtrl->getTargetPose();
debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
@@ -167,20 +174,21 @@ namespace armarx
debugOutputData.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
debugOutputData.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
debugOutputData.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
- debugOutputData.getWriteBuffer().currentCanVal = taskSpaceDMPController->debugData.canVal;
- debugOutputData.getWriteBuffer().mpcFactor = taskSpaceDMPController->debugData.mpcFactor;
- debugOutputData.getWriteBuffer().error = taskSpaceDMPController->debugData.poseError;
- debugOutputData.getWriteBuffer().posError = taskSpaceDMPController->debugData.posiError;
- debugOutputData.getWriteBuffer().oriError = taskSpaceDMPController->debugData.oriError;
+ debugOutputData.getWriteBuffer().currentCanVal = dmpCtrl->debugData.canVal;
+ debugOutputData.getWriteBuffer().mpcFactor = dmpCtrl->debugData.mpcFactor;
+ debugOutputData.getWriteBuffer().error = dmpCtrl->debugData.poseError;
+ debugOutputData.getWriteBuffer().posError = dmpCtrl->debugData.posiError;
+ debugOutputData.getWriteBuffer().oriError = dmpCtrl->debugData.oriError;
debugOutputData.getWriteBuffer().deltaT = deltaT;
debugOutputData.commitWrite();
+
getWriterControlStruct().targetTSVel = targetVels;
getWriterControlStruct().targetPose = targetPose;
-
writeControlStruct();
+
}
@@ -223,13 +231,14 @@ namespace armarx
filtered_qvel = (1 - vel_filter_factor) * filtered_qvel + vel_filter_factor * qvel;
Eigen::VectorXf tcptwist = jacobi * filtered_qvel;
- controllerSensorData.getWriteBuffer().currentPose = currentPose;
- controllerSensorData.getWriteBuffer().currentTwist = tcptwist;
- controllerSensorData.getWriteBuffer().deltaT = deltaT;
- controllerSensorData.getWriteBuffer().currentTime += deltaT;
- controllerSensorData.commitWrite();
-
+ rt2CtrlData.getWriteBuffer().currentPose = currentPose;
+ rt2CtrlData.getWriteBuffer().currentTwist = tcptwist;
+ rt2CtrlData.getWriteBuffer().deltaT = deltaT;
+ rt2CtrlData.getWriteBuffer().currentTime += deltaT;
+ rt2CtrlData.commitWrite();
+ rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
+ rt2UserData.commitWrite();
Eigen::VectorXf targetVel = rtGetControlStruct().targetTSVel;
Eigen::Matrix4f targetPose = rtGetControlStruct().targetPose;
@@ -273,38 +282,10 @@ namespace armarx
// cartesian vel controller
Eigen::VectorXf x;
- auto mode = rtGetControlStruct().mode;
-
- if (mode == VirtualRobot::IKSolver::All)
+ x.resize(6);
+ for (size_t i = 0; i < 6; i++)
{
- x.resize(6);
- for (size_t i = 0; i < 6; i++)
- {
- x(i) = rtTargetVel(i);
- }
- }
- else if (mode == VirtualRobot::IKSolver::Position)
- {
- x.resize(3);
- for (size_t i = 0; i < 3; i++)
- {
- x(i) = rtTargetVel(i);
- }
-
- }
- else if (mode == VirtualRobot::IKSolver::Orientation)
- {
- x.resize(3);
- for (size_t i = 0; i < 3; i++)
- {
- x(i) = rtTargetVel(i + 3);
- }
- }
- else
- {
-
- // No mode has been set
- return;
+ x(i) = rtTargetVel(i);
}
Eigen::VectorXf jnv = Eigen::VectorXf::Zero(tcpController->rns->getSize());
@@ -318,7 +299,7 @@ namespace armarx
jnv(i) += rtGetControlStruct().nullspaceJointVelocities.at(i);
}
- Eigen::VectorXf jointTargetVelocities = tcpController->calculate(x, jnv, mode);
+ Eigen::VectorXf jointTargetVelocities = tcpController->calculate(x, jnv, VirtualRobot::IKSolver::CartesianSelection::All);
ARMARX_CHECK_EXPRESSION(!targets.empty());
ARMARX_CHECK_LESS(targets.size(), 1000);
for (size_t i = 0; i < targets.size(); ++i)
@@ -327,35 +308,36 @@ namespace armarx
if (!targets.at(i)->isValid() || fabs(targets.at(i)->velocity) > cfg->maxJointVelocity)
{
- ARMARX_IMPORTANT << deactivateSpam(1, std::to_string(i))
- << "Velocity controller target is invalid - setting to zero! set value: " << targets.at(i)->velocity;
+ //ARMARX_IMPORTANT << deactivateSpam(1, std::to_string(i))
+ // << "Velocity controller target is invalid - setting to zero! set value: " << targets.at(i)->velocity;
targets.at(i)->velocity = 0.0f;
+
}
}
-
}
void NJointTSDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
{
- taskSpaceDMPController->learnDMPFromFiles(fileNames);
+ ARMARX_INFO << "Learning DMP ... ";
+
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->learnDMPFromFiles(fileNames);
- ARMARX_INFO << "Learned DMP ... ";
}
void NJointTSDMPController::setSpeed(Ice::Double times, const Ice::Current&)
{
LockGuardType guard {controllerMutex};
- taskSpaceDMPController->setSpeed(times);
+ dmpCtrl->setSpeed(times);
}
void NJointTSDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
{
LockGuardType guard {controllerMutex};
- ARMARX_INFO << "setting via points ";
- taskSpaceDMPController->setViaPose(u, viapoint);
+ dmpCtrl->setViaPose(u, viapoint);
}
void NJointTSDMPController::removeAllViaPoints(const Ice::Current&)
@@ -368,54 +350,8 @@ namespace armarx
void NJointTSDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
{
- taskSpaceDMPController->setGoalPoseVec(goals);
- }
-
- void NJointTSDMPController::setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current&)
- {
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().avoidJointLimitsKp = avoidJointLimitsKp;
- getWriterControlStruct().mode = ModeFromIce(mode);
- writeControlStruct();
- }
-
- VirtualRobot::IKSolver::CartesianSelection NJointTSDMPController::ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode)
- {
- if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::ePosition)
- {
- return VirtualRobot::IKSolver::CartesianSelection::Position;
- }
- if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::eOrientation)
- {
- return VirtualRobot::IKSolver::CartesianSelection::Orientation;
- }
- if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::eAll)
- {
- return VirtualRobot::IKSolver::CartesianSelection::All;
- }
- ARMARX_ERROR_S << "invalid mode " << mode;
- return (VirtualRobot::IKSolver::CartesianSelection)0;
- }
-
-
- void NJointTSDMPController::setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&)
- {
- LockGuardType guard {controlDataMutex};
- for (size_t i = 0; i < tcpController->rns->getSize(); i++)
- {
- getWriterControlStruct().torqueKp.at(i) = torqueKp.at(tcpController->rns->getNode(i)->getName());
- }
- writeControlStruct();
- }
-
- void NJointTSDMPController::setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&)
- {
- LockGuardType guard {controlDataMutex};
- for (size_t i = 0; i < tcpController->rns->getSize(); i++)
- {
- getWriterControlStruct().nullspaceJointVelocities.at(i) = nullspaceJointVelocities.at(tcpController->rns->getNode(i)->getName());
- }
- writeControlStruct();
+ LockGuardType guard {controllerMutex};
+ dmpCtrl->setGoalPoseVec(goals);
}
void NJointTSDMPController::pauseDMP(const Ice::Current&)
@@ -450,16 +386,16 @@ namespace armarx
{
usleep(100);
}
- while (!interfaceData.updateReadBuffer())
+ while (!rt2UserData.updateReadBuffer())
{
usleep(100);
}
- Eigen::Matrix4f pose = interfaceData.getReadBuffer().currentTcpPose;
- ARMARX_INFO << "current pose: " << pose;
- // Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
+ Eigen::Matrix4f pose = rt2UserData.getReadBuffer().currentTcpPose;
+
LockGuardType guard {controllerMutex};
- taskSpaceDMPController->prepareExecution(taskSpaceDMPController->eigen4f2vec(pose), goals, tau);
+ // Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
+ dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
finished = false;
ARMARX_INFO << "run DMP";
@@ -520,9 +456,10 @@ namespace armarx
debugObs->setDebugChannel(datafieldName, datafields);
}
- void NJointTSDMPController::onInitComponent()
+ void NJointTSDMPController::onInitNJointController()
{
ARMARX_INFO << "init ...";
+/*
targetVels.resize(6);
NJointTSDMPControllerControlData initData;
initData.targetTSVel.resize(6);
@@ -537,19 +474,28 @@ namespace armarx
initData.torqueKd.resize(tcpController->rns->getSize(), 0);
initData.mode = ModeFromIce(cfg->mode);
reinitTripleBuffer(initData);
+*/
-
-
- controllerTask = new PeriodicTask<NJointTSDMPController>(this, &NJointTSDMPController::controllerRun, 0.3);
- controllerTask->start();
-
+ started = false;
+ runTask("NJointTSDMPController", [&]
+ {
+ CycleUtil c(1);
+ getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
+ while (getState() == eManagedIceObjectStarted)
+ {
+ if (isControllerActive())
+ {
+ controllerRun();
+ }
+ c.waitForCycleDuration();
+ }
+ });
}
- void NJointTSDMPController::onDisconnectComponent()
+ void NJointTSDMPController::onDisconnectNJointController()
{
ARMARX_INFO << "stopped ...";
- controllerTask->stop();
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
index 9eea8c9bbe2f40204b22feced8784250527d1602..75cf0f9067823c869b61e4ec18585c7f83464cac 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
@@ -12,6 +12,8 @@
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+
using namespace DMP;
namespace armarx
@@ -31,9 +33,6 @@ namespace armarx
// cartesian velocity control data
std::vector<float> nullspaceJointVelocities;
float avoidJointLimitsKp = 0;
- std::vector<float> torqueKp;
- std::vector<float> torqueKd;
- VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All;
};
class pidController
@@ -91,9 +90,10 @@ namespace armarx
void resetDMP(const Ice::Current&) override;
void stopDMP(const Ice::Current&) override;
+
double getCanVal(const Ice::Current&) override
{
- return taskSpaceDMPController->canVal / cfg->timeDuration;
+ return dmpCtrl->canVal;
}
protected:
@@ -102,8 +102,8 @@ namespace armarx
VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
- void onInitComponent() override;
- void onDisconnectComponent() override;
+ void onInitNJointController() override;
+ void onDisconnectNJointController() override;
void controllerRun();
private:
@@ -123,21 +123,26 @@ namespace armarx
TripleBuffer<DebugBufferData> debugOutputData;
- struct NJointTSDMPControllerSensorData
+ struct RTToControllerData
{
double currentTime;
double deltaT;
Eigen::Matrix4f currentPose;
Eigen::VectorXf currentTwist;
};
- TripleBuffer<NJointTSDMPControllerSensorData> controllerSensorData;
+ TripleBuffer<RTToControllerData> rt2CtrlData;
- struct NJointTSDMPControllerInterfaceData
+ struct RTToUserData
{
Eigen::Matrix4f currentTcpPose;
+
};
+ TripleBuffer<RTToUserData> rt2UserData;
+
+
+ TaskSpaceDMPControllerPtr dmpCtrl;
+
- TripleBuffer<NJointTSDMPControllerInterfaceData> interfaceData;
std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
std::vector<const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
@@ -146,7 +151,6 @@ namespace armarx
std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
- TaskSpaceDMPControllerPtr taskSpaceDMPController;
// velocity ik controller parameters
std::vector<pidController> torquePIDs;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index dfb40405adbc4a8613233833a252145fbc8d2c57..5675efd9521c6092afb4e3e84f1cbb784f58584a 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -8,7 +8,7 @@ namespace armarx
{
cfg = NJointTaskSpaceImpedanceDMPControllerConfigPtr::dynamicCast(config);
useSynchronizedRtRobot();
- VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
+ rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName);
for (size_t i = 0; i < rns->getSize(); ++i)
@@ -36,7 +36,7 @@ namespace armarx
tcp = rns->getTCP();
ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
+ numOfJoints = targets.size();
// set DMP
TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
@@ -75,8 +75,19 @@ namespace armarx
dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
- knull = cfg->Knull;
- dnull = cfg->Dnull;
+
+
+ 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);
+ }
torqueLimit = cfg->torqueLimit;
timeDuration = cfg->timeDuration;
@@ -91,6 +102,8 @@ namespace armarx
initControllerSensorData.deltaT = 0;
initControllerSensorData.currentTwist.setZero();
controllerSensorData.reinitAllBuffers(initControllerSensorData);
+
+ firstRun = true;
}
std::string NJointTaskSpaceImpedanceDMPController::getClassName(const Ice::Current&) const
@@ -115,63 +128,99 @@ namespace armarx
void NJointTaskSpaceImpedanceDMPController::controllerRun()
{
- if (!controllerSensorData.updateReadBuffer())
+
+
+ if (!dmpCtrl)
{
return;
}
- if (dmpCtrl->canVal < 1e-8)
+ if (!controllerSensorData.updateReadBuffer())
{
- finished = true;
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().targetVel.setZero();
- writeControlStruct();
return;
}
+
double deltaT = 0.001; //controllerSensorData.getReadBuffer().deltaT;
Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
- dmpCtrl->flow(deltaT, currentPose, currentTwist);
-
- Eigen::VectorXf desiredNullSpaceJointValues(jointNames.size());
- if (useNullSpaceJointDMP && isNullSpaceJointDMPLearned)
+ if (!started)
{
- DMP::DVec targetJointState;
- currentJointState = nullSpaceJointDMPPtr->calculateDirectlyVelocity(currentJointState, dmpCtrl->canVal / timeDuration, deltaT / timeDuration, targetJointState);
-
- if (targetJointState.size() == jointNames.size())
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ getWriterControlStruct().targetVel.setZero(6);
+ getWriterControlStruct().targetPose = currentPose;
+ getWriterControlStruct().canVal = 1.0;
+ getWriterControlStruct().mpcFactor = 0.0;
+ writeControlStruct();
+ }
+ else
+ {
+ if (stopped)
{
- for (size_t i = 0; i < targetJointState.size(); ++i)
- {
- desiredNullSpaceJointValues(i) = targetJointState[i];
- }
+
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ getWriterControlStruct().targetVel.setZero(6);
+ getWriterControlStruct().targetPose = oldPose;
+ getWriterControlStruct().canVal = dmpCtrl->canVal;
+ getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
+ writeControlStruct();
}
else
{
- desiredNullSpaceJointValues = defaultNullSpaceJointValues;
- }
- }
- else
- {
- desiredNullSpaceJointValues = defaultNullSpaceJointValues;
- }
+ if (dmpCtrl->canVal < 1e-8)
+ {
+ finished = true;
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().targetVel.setZero();
+ writeControlStruct();
+ return;
+ }
+
+ dmpCtrl->flow(deltaT, currentPose, currentTwist);
+
+ Eigen::VectorXf desiredNullSpaceJointValues(jointNames.size());
+ if (useNullSpaceJointDMP && isNullSpaceJointDMPLearned)
+ {
+ DMP::DVec targetJointState;
+ currentJointState = nullSpaceJointDMPPtr->calculateDirectlyVelocity(currentJointState, dmpCtrl->canVal / timeDuration, deltaT / timeDuration, targetJointState);
+
+ if (targetJointState.size() == jointNames.size())
+ {
+ for (size_t i = 0; i < targetJointState.size(); ++i)
+ {
+ desiredNullSpaceJointValues(i) = targetJointState[i];
+ }
+ }
+ else
+ {
+ desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ }
+ }
+ else
+ {
+ desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ }
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().desiredNullSpaceJointValues = desiredNullSpaceJointValues;
- getWriterControlStruct().targetVel = dmpCtrl->getTargetVelocity();
- getWriterControlStruct().targetPose = dmpCtrl->getTargetPoseMat();
- getWriterControlStruct().canVal = dmpCtrl->canVal;
- getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().desiredNullSpaceJointValues = desiredNullSpaceJointValues;
+ getWriterControlStruct().targetVel = dmpCtrl->getTargetVelocity();
+ getWriterControlStruct().targetPose = dmpCtrl->getTargetPoseMat();
+ getWriterControlStruct().canVal = dmpCtrl->canVal;
+ getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
- writeControlStruct();
+ writeControlStruct();
+ }
+ }
}
void NJointTaskSpaceImpedanceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
double deltaT = timeSinceLastIteration.toSecondsDouble();
+
Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
Eigen::VectorXf qpos(positionSensors.size());
@@ -194,11 +243,25 @@ namespace armarx
interfaceData.getWriteBuffer().currentTcpPose = currentPose;
interfaceData.commitWrite();
- jacobi.block<3, 8>(0, 0) = 0.001 * jacobi.block<3, 8>(0, 0); // convert mm to m
+ jacobi.block(0, 0, 3, numOfJoints) = 0.001 * jacobi.block(0, 0, 3, numOfJoints); // convert mm to m
- Eigen::Matrix4f targetPose = rtGetControlStruct().targetPose;
- Eigen::VectorXf targetVel = rtGetControlStruct().targetVel;
+ Eigen::Matrix4f targetPose;
+ Eigen::VectorXf targetVel;
+ Eigen::VectorXf desiredNullSpaceJointValues;
+ if (firstRun)
+ {
+ firstRun = false;
+ targetPose = currentPose;
+ targetVel.setZero(6);
+ desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+ }
+ else
+ {
+ targetPose = rtGetControlStruct().targetPose;
+ targetVel = rtGetControlStruct().targetVel;
+ desiredNullSpaceJointValues = rtGetControlStruct().desiredNullSpaceJointValues;
+ }
Eigen::Vector6f jointControlWrench;
{
Eigen::Vector3f targetTCPLinearVelocity;
@@ -207,7 +270,7 @@ namespace armarx
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(- currentTCPLinearVelocity);
+ Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) + dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
Eigen::Vector3f currentTCPAngularVelocity;
currentTCPAngularVelocity << currentTwist(3), currentTwist(4), currentTwist(5);
@@ -220,8 +283,7 @@ namespace armarx
Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
- Eigen::VectorXf desiredNullSpaceJointValues = rtGetControlStruct().desiredNullSpaceJointValues;
- Eigen::VectorXf nullspaceTorque = knull * (desiredNullSpaceJointValues - qpos) - dnull * 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;
@@ -263,8 +325,8 @@ namespace armarx
debugOutputData.getWriteBuffer().forceDesired_ry = jointControlWrench(4);
debugOutputData.getWriteBuffer().forceDesired_rz = jointControlWrench(5);
- debugOutputData.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
- debugOutputData.getWriteBuffer().mpcfactor = rtGetControlStruct().mpcFactor;
+ // debugOutputData.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
+ // debugOutputData.getWriteBuffer().mpcfactor = rtGetControlStruct().mpcFactor;
debugOutputData.getWriteBuffer().targetPose_x = targetPose(0, 3);
debugOutputData.getWriteBuffer().targetPose_y = targetPose(1, 3);
@@ -298,7 +360,7 @@ namespace armarx
void NJointTaskSpaceImpedanceDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
{
- // LockGuardType guard(controllerMutex);
+ LockGuardType guard(controllerMutex);
ARMARX_INFO << "setting via points ";
dmpCtrl->setViaPose(u, viapoint);
@@ -310,7 +372,7 @@ namespace armarx
}
- void NJointTaskSpaceImpedanceDMPController::learnJointDMPFromFiles(const std::string& fileName, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::learnJointDMPFromFiles(const std::string& fileName, const Ice::FloatSeq& currentJVS, const Ice::Current&)
{
DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
DMP::DVec ratios;
@@ -330,7 +392,7 @@ namespace armarx
for (size_t i = 0; i < goal.size(); ++i)
{
goal.at(i) = traj.rbegin()->getPosition(i);
- currentJointState.at(i).pos = traj.begin()->getPosition(i);
+ currentJointState.at(i).pos = currentJVS.at(i);
currentJointState.at(i).vel = 0;
}
@@ -343,15 +405,61 @@ namespace armarx
isNullSpaceJointDMPLearned = true;
}
- void NJointTaskSpaceImpedanceDMPController::runDMP(const Ice::DoubleSeq& goals, const Ice::Current&)
+
+ void NJointTaskSpaceImpedanceDMPController::resetDMP(const Ice::Current&)
+ {
+ if (started)
+ {
+ ARMARX_INFO << "Cannot reset running DMP";
+ }
+ firstRun = true;
+ }
+
+ void NJointTaskSpaceImpedanceDMPController::stopDMP(const Ice::Current&)
+ {
+ oldPose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
+ stopped = true;
+ }
+
+ void NJointTaskSpaceImpedanceDMPController::resumeDMP(const Ice::Current&)
+ {
+ stopped = false;
+ }
+
+
+ void NJointTaskSpaceImpedanceDMPController::runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
{
+ firstRun = true;
+ while (firstRun)
+ {
+ usleep(100);
+ }
- while (!interfaceData.updateReadBuffer())
+ Eigen::Matrix4f pose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
+ dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
+
+ dmpCtrl->canVal = timeDuration;
+ finished = false;
+
+ if (isNullSpaceJointDMPLearned && useNullSpaceJointDMP)
+ {
+ ARMARX_INFO << "Using Null Space Joint DMP";
+ }
+
+ started = true;
+ stopped = false;
+ // controllerTask->start();
+ }
+
+ void NJointTaskSpaceImpedanceDMPController::runDMP(const Ice::DoubleSeq& goals, const Ice::Current&)
+ {
+ firstRun = true;
+ while (firstRun)
{
usleep(100);
}
- Eigen::Matrix4f pose = interfaceData.getReadBuffer().currentTcpPose;
+ Eigen::Matrix4f pose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
finished = false;
@@ -361,7 +469,9 @@ namespace armarx
ARMARX_INFO << "Using Null Space Joint DMP";
}
- controllerTask->start();
+ started = true;
+ stopped = false;
+ // controllerTask->start();
}
@@ -412,16 +522,28 @@ namespace armarx
debugObs->setDebugChannel(channelName, datafields);
}
- void NJointTaskSpaceImpedanceDMPController::onInitComponent()
+ void NJointTaskSpaceImpedanceDMPController::onInitNJointController()
{
ARMARX_INFO << "init ...";
- controllerTask = new PeriodicTask<NJointTaskSpaceImpedanceDMPController>(this, &NJointTaskSpaceImpedanceDMPController::controllerRun, 1);
+ // controllerTask = new PeriodicTask<NJointTaskSpaceImpedanceDMPController>(this, &NJointTaskSpaceImpedanceDMPController::controllerRun, 1);
+ runTask("NJointTaskSpaceImpedanceDMPController", [&]
+ {
+ CycleUtil c(1);
+ getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
+ while (getState() == eManagedIceObjectStarted)
+ {
+ if (isControllerActive())
+ {
+ controllerRun();
+ }
+ c.waitForCycleDuration();
+ }
+ });
}
- void NJointTaskSpaceImpedanceDMPController::onDisconnectComponent()
+ void NJointTaskSpaceImpedanceDMPController::onDisconnectNJointController()
{
- controllerTask->stop();
- ARMARX_INFO << "stopped ...";
+ // controllerTask->stop();
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
index 143d752d358edf14a5abd5497649679b4aef8589..49a104b540537279487adb1ad50b86bc7da9ccb5 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -10,6 +10,7 @@
#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
#include <dmp/representation/dmp/umidmp.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
using namespace DMP;
@@ -46,34 +47,39 @@ namespace armarx
NJointTaskSpaceImpedanceDMPController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
// NJointControllerInterface interface
- std::string getClassName(const Ice::Current&) const override;
+ std::string getClassName(const Ice::Current&) const;
// NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
// NJointTaskSpaceImpedanceDMPControllerInterface interface
- void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) override;
- bool isFinished(const Ice::Current&) override
+ void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
+ bool isFinished(const Ice::Current&)
{
return finished;
}
- void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
- void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;
+ void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
+ void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
- void learnJointDMPFromFiles(const std::string& fileName, const Ice::Current&) override;
- void runDMP(const Ice::DoubleSeq& goals, const Ice::Current&) override;
- Ice::Double getVirtualTime(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&);
+
+ Ice::Double getVirtualTime(const Ice::Current&)
{
return dmpCtrl->canVal;
}
+ void stopDMP(const Ice::Current&);
+ void resumeDMP(const Ice::Current&);
+ void resetDMP(const Ice::Current&);
protected:
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
+ virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
- void onInitComponent() override;
- void onDisconnectComponent() override;
+ void onInitNJointController();
+ void onDisconnectNJointController();
void controllerRun();
private:
@@ -142,6 +148,7 @@ namespace armarx
// dmp parameters
double timeDuration;
bool finished;
+ VirtualRobot::RobotNodeSetPtr rns;
// phaseStop parameters
double phaseL;
@@ -161,9 +168,9 @@ namespace armarx
Eigen::Vector3f kori;
Eigen::Vector3f dpos;
Eigen::Vector3f dori;
- float knull;
- float dnull;
-
+ Eigen::VectorXf knull;
+ Eigen::VectorXf dnull;
+ int numOfJoints;
bool useNullSpaceJointDMP;
bool isNullSpaceJointDMPLearned;
@@ -173,11 +180,14 @@ namespace armarx
std::vector<std::string> jointNames;
mutable MutexType controllerMutex;
PeriodicTask<NJointTaskSpaceImpedanceDMPController>::pointer_type controllerTask;
+ bool firstRun;
+ bool started = false;
+ bool stopped = false;
-
+ Eigen::Matrix4f oldPose;
// NJointController interface
protected:
- void rtPreActivateController() override;
+ void rtPreActivateController();
};
} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
index a54cf2fe86393f94a679b1d09d45cb5a7d48f292..a54284c3a03b39a3406b74d83cebae127856a815 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
@@ -51,12 +51,34 @@ PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNode
}
}
+void PositionControllerHelper::readConfig(const CartesianPositionControllerConfigBasePtr& config)
+{
+ thresholdOrientationNear = config->thresholdOrientationNear;
+ thresholdOrientationReached = config->thresholdOrientationReached;
+ thresholdPositionNear = config->thresholdPositionNear;
+ thresholdPositionReached = config->thresholdPositionReached;
+ posController.KpOri = config->KpOri;
+ posController.KpPos = config->KpPos;
+ posController.maxOriVel = config->maxOriVel;
+ posController.maxPosVel = config->maxPosVel;
+}
+
void PositionControllerHelper::update()
+{
+ updateRead();
+ updateWrite();
+}
+
+void PositionControllerHelper::updateRead()
{
if (!isLastWaypoint() && isCurrentTargetNear())
{
currentWaypointIndex++;
}
+}
+
+void PositionControllerHelper::updateWrite()
+{
Eigen::VectorXf cv = posController.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All);
velocityControllerHelper->setTargetVelocity(cv + feedForwardVelocity);
if (autoClearFeedForward)
@@ -109,6 +131,15 @@ void PositionControllerHelper::clearFeedForwardVelocity()
feedForwardVelocity = Eigen::Vector6f::Zero();
}
+void PositionControllerHelper::immediateHardStop(bool clearTargets)
+{
+ velocityControllerHelper->controller->immediateHardStop();
+ if (clearTargets)
+ {
+ setTarget(posController.getTcp()->getPoseInRootFrame());
+ }
+}
+
float PositionControllerHelper::getPositionError() const
{
return posController.getPositionError(getCurrentTarget());
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
index c38c2be4452781a2b53667d538f4199725836abc..4f380eeb34eaebe736b7fa8281baa7a2a4bb732d 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
@@ -51,7 +51,15 @@ namespace armarx
PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<Eigen::Matrix4f>& waypoints);
PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<PosePtr>& waypoints);
+ void readConfig(const CartesianPositionControllerConfigBasePtr& config);
+
+ // read data and write targets, call this if you are unsure, anywhere in your control loop.
+ // use updateRead and updateWrite for better performance
void update();
+ // read data, call this after robot sync
+ void updateRead();
+ // write targets, call this at the end of your control loop, before the sleep
+ void updateWrite();
void setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints);
void setNewWaypoints(const std::vector<PosePtr>& waypoints);
@@ -60,6 +68,7 @@ namespace armarx
void setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity);
void setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri);
void clearFeedForwardVelocity();
+ void immediateHardStop(bool clearTargets = true);
float getPositionError() const;
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
index 5bf8b9eaa04605ebb19483a147b675a7e1b337d1..9d0551330671517b37dc0872135e302cd268d00b 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
@@ -121,6 +121,11 @@ void RobotNameHelper::Node::checkValid()
}
+std::string RobotNameHelper::Arm::getSide() const
+{
+ return side;
+}
+
std::string RobotNameHelper::Arm::getKinematicChain() const
{
return select("KinematicChain");
@@ -156,6 +161,21 @@ std::string RobotNameHelper::Arm::getHandControllerName() const
return select("HandControllerName");
}
+std::string RobotNameHelper::Arm::getHandRootNode() const
+{
+ return select("HandRootNode");
+}
+
+std::string RobotNameHelper::Arm::getHandModelPath() const
+{
+ return select("HandModelPath");
+}
+
+std::string RobotNameHelper::Arm::getHandModelPackage() const
+{
+ return select("HandModelPackage");
+}
+
RobotNameHelper::RobotArm RobotNameHelper::Arm::addRobot(const VirtualRobot::RobotPtr& robot) const
{
return RobotArm(*this, robot);
@@ -172,6 +192,11 @@ std::string RobotNameHelper::Arm::select(const std::string& path) const
return rnh->select(side + "Arm/" + path);
}
+std::string RobotNameHelper::RobotArm::getSide() const
+{
+ return arm.getSide();
+}
+
VirtualRobot::RobotNodeSetPtr RobotNameHelper::RobotArm::getKinematicChain() const
{
return robot->getRobotNodeSet(arm.getKinematicChain());
@@ -187,6 +212,21 @@ VirtualRobot::RobotNodePtr RobotNameHelper::RobotArm::getTCP() const
return robot->getRobotNode(arm.getTCP());
}
+VirtualRobot::RobotNodePtr RobotNameHelper::RobotArm::getHandRootNode() const
+{
+ return robot->getRobotNode(arm.getHandRootNode());
+}
+
+Eigen::Matrix4f RobotNameHelper::RobotArm::getTcp2HandRootTransform() const
+{
+ return getTCP()->getPoseInRootFrame().inverse() * getHandRootNode()->getPoseInRootFrame();
+}
+
+RobotNameHelper::Arm RobotNameHelper::RobotArm::getArm() const
+{
+ return arm;
+}
+
RobotNameHelper::RobotArm::RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot)
: arm(arm), robot(robot)
{
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
index c4e34fd0f60852c242db1d6d1424396065ca5cb7..bd95565df108c2b30086bb83aae8a2588cb9cc86 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
@@ -61,6 +61,7 @@ namespace armarx
friend class RobotNameHelper;
public:
+ std::string getSide() const;
std::string getKinematicChain() const;
std::string getTorsoKinematicChain() const;
std::string getTCP() const;
@@ -68,6 +69,9 @@ namespace armarx
std::string getEndEffector() const;
std::string getMemoryHandName() const;
std::string getHandControllerName() const;
+ std::string getHandRootNode() const;
+ std::string getHandModelPath() const;
+ std::string getHandModelPackage() const;
RobotArm addRobot(const VirtualRobot::RobotPtr& robot) const;
private:
@@ -83,9 +87,13 @@ namespace armarx
friend class RobotNameHelper;
friend class Arm;
public:
+ std::string getSide() const;
VirtualRobot::RobotNodeSetPtr getKinematicChain() const;
VirtualRobot::RobotNodeSetPtr getTorsoKinematicChain() const;
VirtualRobot::RobotNodePtr getTCP() const;
+ VirtualRobot::RobotNodePtr getHandRootNode() const;
+ Eigen::Matrix4f getTcp2HandRootTransform() const;
+ Arm getArm() const;
private:
RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot);
diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index 8ecf0ea00336d937225cb97c95603a62374441c5..249994acdaa5b4e1dd955f3619adec22879c8232 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/CMakeLists.txt
@@ -8,7 +8,10 @@ armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
set(LIB_NAME RobotAPICore)
set(LIBS RobotAPIInterfaces ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreEigen3Variants
- VirtualRobot Saba)
+ VirtualRobot
+ Saba
+ SimDynamics
+ )
set(LIB_FILES
PIDController.cpp
@@ -33,10 +36,15 @@ set(LIB_FILES
math/TimeSeriesUtils.cpp
CartesianVelocityController.cpp
CartesianPositionController.cpp
+ CartesianWaypointController.cpp
CartesianFeedForwardPositionController.cpp
CartesianVelocityRamp.cpp
JointVelocityRamp.cpp
CartesianVelocityControllerWithRamp.cpp
+ SimpleDiffIK.cpp
+
+ visualization/DebugDrawerTopic.cpp
+ visualization/GlasbeyLUT.cpp
)
set(LIB_HEADERS
@@ -76,11 +84,16 @@ set(LIB_HEADERS
math/TimeSeriesUtils.h
CartesianVelocityController.h
CartesianPositionController.h
+ CartesianWaypointController.h
CartesianFeedForwardPositionController.h
CartesianVelocityRamp.h
JointVelocityRamp.h
CartesianVelocityControllerWithRamp.h
+ SimpleDiffIK.h
EigenHelpers.h
+
+ visualization/DebugDrawerTopic.h
+ visualization/GlasbeyLUT.h
)
add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.cpp b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
index 4559e139c7d93e803f1ff7d070a77698396863a0..60c62092f4e34302e825e530436e150204f91fe5 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
@@ -100,3 +100,64 @@ Eigen::Vector3f CartesianPositionController::getOrientationDiff(const Eigen::Mat
Eigen::AngleAxisf aa(oriDir);
return aa.axis() * aa.angle();
}
+
+VirtualRobot::RobotNodePtr CartesianPositionController::getTcp() const
+{
+ return tcp;
+}
+
+
+
+#define SS_OUT(x) ss << #x << " = " << x << "\n"
+#define SET_FLT(x) obj->setFloat(#x, x)
+#define GET_FLT(x) x = obj->getFloat(#x);
+
+CartesianPositionControllerConfig::CartesianPositionControllerConfig()
+{
+}
+
+std::string CartesianPositionControllerConfig::output(const Ice::Current& c) const
+{
+ std::stringstream ss;
+
+ SS_OUT(KpPos);
+ SS_OUT(KpOri);
+ SS_OUT(maxPosVel);
+ SS_OUT(maxOriVel);
+ SS_OUT(thresholdOrientationNear);
+ SS_OUT(thresholdOrientationReached);
+ SS_OUT(thresholdPositionNear);
+ SS_OUT(thresholdPositionReached);
+
+ return ss.str();
+}
+
+void CartesianPositionControllerConfig::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
+{
+ AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+
+ SET_FLT(KpPos);
+ SET_FLT(KpOri);
+ SET_FLT(maxPosVel);
+ SET_FLT(maxOriVel);
+ SET_FLT(thresholdOrientationNear);
+ SET_FLT(thresholdOrientationReached);
+ SET_FLT(thresholdPositionNear);
+ SET_FLT(thresholdPositionReached);
+
+}
+
+void CartesianPositionControllerConfig::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
+{
+ AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+
+ GET_FLT(KpPos);
+ GET_FLT(KpOri);
+ GET_FLT(maxPosVel);
+ GET_FLT(maxOriVel);
+ GET_FLT(thresholdOrientationNear);
+ GET_FLT(thresholdOrientationReached);
+ GET_FLT(thresholdPositionNear);
+ GET_FLT(thresholdPositionReached);
+
+}
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.h b/source/RobotAPI/libraries/core/CartesianPositionController.h
index 392ba95c03b8dd8e3c5242ae23bcbf26a2a5f6b7..e5f2c832edb3640ef649e3d8550fa94c7daa1654 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.h
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.h
@@ -28,6 +28,10 @@
#include <VirtualRobot/Robot.h>
#include <Eigen/Dense>
+#include <RobotAPI/interface/core/CartesianPositionControllerConfig.h>
+#include <ArmarXCore/observers/variant/Variant.h>
+#include <ArmarXCore/observers/AbstractObjectSerializer.h>
+
namespace armarx
{
class CartesianPositionController;
@@ -39,12 +43,16 @@ namespace armarx
public:
CartesianPositionController(const VirtualRobot::RobotNodePtr& tcp);
+ CartesianPositionController(CartesianPositionController&&) = default;
+ CartesianPositionController& operator=(CartesianPositionController&&) = default;
+
Eigen::VectorXf calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode) const;
float getPositionError(const Eigen::Matrix4f& targetPose) const;
float getOrientationError(const Eigen::Matrix4f& targetPose) const;
Eigen::Vector3f getPositionDiff(const Eigen::Matrix4f& targetPose) const;
Eigen::Vector3f getOrientationDiff(const Eigen::Matrix4f& targetPose) const;
+ VirtualRobot::RobotNodePtr getTcp() const;
float KpPos = 1;
float KpOri = 1;
@@ -55,5 +63,50 @@ namespace armarx
private:
VirtualRobot::RobotNodePtr tcp;
};
+
+ namespace VariantType
+ {
+ // variant types
+ const VariantTypeId CartesianPositionControllerConfig = Variant::addTypeName("::armarx::CartesianPositionControllerConfig");
+ }
+
+ class CartesianPositionControllerConfig : virtual public CartesianPositionControllerConfigBase
+ {
+ public:
+ CartesianPositionControllerConfig();
+
+
+ // inherited from VariantDataClass
+ Ice::ObjectPtr ice_clone() const override
+ {
+ return this->clone();
+ }
+ VariantDataClassPtr clone(const Ice::Current& = ::Ice::Current()) const override
+ {
+ return new CartesianPositionControllerConfig(*this);
+ }
+ std::string output(const Ice::Current& c = ::Ice::Current()) const override;
+ VariantTypeId getType(const Ice::Current& = ::Ice::Current()) const override
+ {
+ return VariantType::CartesianPositionControllerConfig;
+ }
+ bool validate(const Ice::Current& = ::Ice::Current()) override
+ {
+ return true;
+ }
+
+ friend std::ostream& operator<<(std::ostream& stream, const CartesianPositionControllerConfig& rhs)
+ {
+ stream << "CartesianPositionControllerConfig: " << std::endl << rhs.output() << std::endl;
+ return stream;
+ }
+
+ public: // serialization
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) override;
+
+ };
+
+ typedef IceInternal::Handle<CartesianPositionControllerConfig> CartesianPositionControllerConfigPtr;
}
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index 3ec8849c620e0abbc05a548f625a771b76839199..0f84e0a0a2e54981497c4723009f297800e2bc02 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -119,6 +119,7 @@ Eigen::VectorXf CartesianVelocityController::calculateJointLimitAvoidance()
{
float f = math::MathUtils::ILerp(rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
r(i) = cos(f * M_PI);
+ //r(i) = math::MathUtils::Lerp(1.f, -1.f, f);
}
}
return r;
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.h b/source/RobotAPI/libraries/core/CartesianVelocityController.h
index c0bc793a75603caf9becceffb8a13c0d75aebf16..9447e901e4bf6c927c78b56984bc22c6dc27324f 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.h
@@ -41,6 +41,9 @@ namespace armarx
const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod = VirtualRobot::JacobiProvider::eSVDDamped,
bool considerJointLimits = true);
+ CartesianVelocityController(CartesianVelocityController&&) = default;
+ CartesianVelocityController& operator=(CartesianVelocityController&&) = default;
+
Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, float KpJointLimitAvoidanceScale, VirtualRobot::IKSolver::CartesianSelection mode);
Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, VirtualRobot::IKSolver::CartesianSelection mode);
Eigen::VectorXf calculateJointLimitAvoidance();
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
index e1d5154f4e51f7b959b1c891c63cdccbaca4f23b..bd336320b84558e883136633cfa90876f5c0379c 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
@@ -35,7 +35,7 @@ CartesianVelocityControllerWithRamp::CartesianVelocityControllerWithRamp(const V
setCurrentJointVelocity(currentJointVelocity);
}
-void CartesianVelocityControllerWithRamp::setCurrentJointVelocity(const Eigen::VectorXf& currentJointVelocity)
+void CartesianVelocityControllerWithRamp::setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity)
{
Eigen::MatrixXf jacobi = controller.ik->getJacobianMatrix(controller.tcp, mode);
Eigen::MatrixXf inv = controller.ik->computePseudoInverseJacobianMatrix(jacobi, controller.ik->getJacobiRegularization(mode));
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
index cca4d79339de53704b8d2a7a19aec6babb568c5d..3f0932ab997767a6ce03dba72e49a5417946a75c 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
@@ -46,7 +46,10 @@ namespace armarx
float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration,
const VirtualRobot::RobotNodePtr& tcp = nullptr);
- void setCurrentJointVelocity(const Eigen::VectorXf& currentJointVelocity);
+ CartesianVelocityControllerWithRamp(CartesianVelocityControllerWithRamp&&) = default;
+ CartesianVelocityControllerWithRamp& operator=(CartesianVelocityControllerWithRamp&&) = default;
+
+ void setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity);
Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, float jointLimitAvoidanceScale, float dt);
Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, float dt);
diff --git a/source/RobotAPI/libraries/core/CartesianWaypointController.cpp b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..de563c4f306b4cc1245ec978a33f345efbd3212e
--- /dev/null
+++ b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
@@ -0,0 +1,221 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 Raphael Grimm (raphael dot grimm at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <VirtualRobot/math/Helpers.h>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+
+#include "CartesianWaypointController.h"
+
+namespace armarx
+{
+ CartesianWaypointController::CartesianWaypointController(
+ const VirtualRobot::RobotNodeSetPtr& rns,
+ const Eigen::VectorXf& currentJointVelocity,
+ float maxPositionAcceleration,
+ float maxOrientationAcceleration,
+ float maxNullspaceAcceleration,
+ const VirtualRobot::RobotNodePtr& tcp
+ ) :
+ ctrlCartesianVelWithRamps
+ {
+ rns,
+ currentJointVelocity,
+ VirtualRobot::IKSolver::CartesianSelection::All,
+ maxPositionAcceleration,
+ maxOrientationAcceleration,
+ maxNullspaceAcceleration,
+ tcp ? tcp : rns->getTCP()
+ },
+ ctrlCartesianPos2Vel(tcp ? tcp : rns->getTCP()),
+ currentWaypointIndex(0)
+ {
+ ARMARX_CHECK_NOT_NULL(rns);
+ _out = Eigen::VectorXf::Zero(rns->getSize());
+ _jnv = Eigen::VectorXf::Zero(rns->getSize());
+ }
+
+ const Eigen::VectorXf& CartesianWaypointController::calculate(float dt)
+ {
+ //calculate cartesian velocity + some management stuff
+ if (!isLastWaypoint() && isCurrentTargetNear())
+ {
+ currentWaypointIndex++;
+ }
+ cartesianVelocity = ctrlCartesianPos2Vel.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All) + feedForwardVelocity;
+
+ if (autoClearFeedForward)
+ {
+ clearFeedForwardVelocity();
+ }
+
+ //calculate joint velocity
+ if (nullSpaceControlEnabled)
+ {
+ //avoid joint limits
+ _jnv = KpJointLimitAvoidance * ctrlCartesianVelWithRamps.controller.calculateJointLimitAvoidance() +
+ ctrlCartesianVelWithRamps.controller.calculateNullspaceVelocity(
+ cartesianVelocity,
+ jointLimitAvoidanceScale,
+ VirtualRobot::IKSolver::All
+ );
+ }
+ else
+ {
+ //don't avoid joint limits
+ _jnv *= 0;
+ }
+ _out = ctrlCartesianVelWithRamps.calculate(cartesianVelocity, _jnv, dt);
+ return _out;
+ }
+
+ void CartesianWaypointController::setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints)
+ {
+ this->waypoints = waypoints;
+ currentWaypointIndex = 0;
+ }
+
+ void CartesianWaypointController::addWaypoint(const Eigen::Matrix4f& waypoint)
+ {
+ this->waypoints.push_back(waypoint);
+ }
+
+ void CartesianWaypointController::setTarget(const Eigen::Matrix4f& target)
+ {
+ waypoints.clear();
+ waypoints.push_back(target);
+ currentWaypointIndex = 0;
+ }
+
+ void CartesianWaypointController::setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity)
+ {
+ this->feedForwardVelocity = feedForwardVelocity;
+ }
+
+ void CartesianWaypointController::setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri)
+ {
+ feedForwardVelocity.block<3, 1>(0, 0) = feedForwardVelocityPos;
+ feedForwardVelocity.block<3, 1>(3, 0) = feedForwardVelocityOri;
+ }
+
+ void CartesianWaypointController::clearFeedForwardVelocity()
+ {
+ feedForwardVelocity = Eigen::Vector6f::Zero();
+ }
+
+ float CartesianWaypointController::getPositionError() const
+ {
+ return ctrlCartesianPos2Vel.getPositionError(getCurrentTarget());
+ }
+
+ float CartesianWaypointController::getOrientationError() const
+ {
+ return ctrlCartesianPos2Vel.getOrientationError(getCurrentTarget());
+ }
+
+ bool CartesianWaypointController::isCurrentTargetReached() const
+ {
+ return getPositionError() < thresholdPositionReached && getOrientationError() < thresholdOrientationReached;
+ }
+
+ bool CartesianWaypointController::isCurrentTargetNear() const
+ {
+ return getPositionError() < thresholdPositionNear && getOrientationError() < thresholdOrientationNear;
+ }
+
+ bool CartesianWaypointController::isFinalTargetReached() const
+ {
+ return isLastWaypoint() && isCurrentTargetReached();
+ }
+
+ bool CartesianWaypointController::isFinalTargetNear() const
+ {
+ return isLastWaypoint() && isCurrentTargetNear();
+ }
+
+ bool CartesianWaypointController::isLastWaypoint() const
+ {
+ return currentWaypointIndex + 1 >= waypoints.size();
+ }
+
+ const Eigen::Matrix4f& CartesianWaypointController::getCurrentTarget() const
+ {
+ return waypoints.at(currentWaypointIndex);
+ }
+
+ const Eigen::Vector3f CartesianWaypointController::getCurrentTargetPosition() const
+ {
+ return math::Helpers::GetPosition(waypoints.at(currentWaypointIndex));
+ }
+
+ size_t CartesianWaypointController::skipToClosestWaypoint(float rad2mmFactor)
+ {
+ float dist = FLT_MAX;
+ size_t minIndex = 0;
+ for (size_t i = 0; i < waypoints.size(); i++)
+ {
+ float posErr = ctrlCartesianPos2Vel.getPositionError(waypoints.at(i));
+ float oriErr = ctrlCartesianPos2Vel.getOrientationError(waypoints.at(i));
+ float d = posErr + oriErr * rad2mmFactor;
+ if (d < dist)
+ {
+ minIndex = i;
+ dist = d;
+ }
+ }
+ currentWaypointIndex = minIndex;
+ return currentWaypointIndex;
+ }
+
+ void CartesianWaypointController::setNullSpaceControl(bool enabled)
+ {
+ nullSpaceControlEnabled = enabled;
+ }
+
+ std::string CartesianWaypointController::getStatusText()
+ {
+ std::stringstream ss;
+ ss.precision(2);
+ ss << std::fixed << "Waypoint: " << (currentWaypointIndex + 1) << "/" << waypoints.size() << " distance: " << getPositionError() << " mm " << VirtualRobot::MathTools::rad2deg(getOrientationError()) << " deg";
+ return ss.str();
+ }
+
+ void CartesianWaypointController::setConfig(const CartesianWaypointControllerConfig& cfg)
+ {
+ KpJointLimitAvoidance = cfg.kpJointLimitAvoidance;
+ jointLimitAvoidanceScale = cfg.jointLimitAvoidanceScale;
+
+ thresholdOrientationNear = cfg.thresholdOrientationNear;
+ thresholdOrientationReached = cfg.thresholdOrientationReached;
+ thresholdPositionNear = cfg.thresholdPositionNear;
+ thresholdPositionReached = cfg.thresholdPositionReached;
+
+ ctrlCartesianPos2Vel.maxOriVel = cfg.maxOriVel;
+ ctrlCartesianPos2Vel.maxPosVel = cfg.maxPosVel;
+ ctrlCartesianPos2Vel.KpOri = cfg.kpOri;
+ ctrlCartesianPos2Vel.KpPos = cfg.kpPos;
+ }
+}
diff --git a/source/RobotAPI/libraries/core/CartesianWaypointController.h b/source/RobotAPI/libraries/core/CartesianWaypointController.h
new file mode 100644
index 0000000000000000000000000000000000000000..95bef4948f192eebbeb526bec591640499eda419
--- /dev/null
+++ b/source/RobotAPI/libraries/core/CartesianWaypointController.h
@@ -0,0 +1,121 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 Raphael Grimm (raphael dot grimm at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <memory>
+
+#include <Eigen/Dense>
+
+#include <VirtualRobot/Robot.h>
+
+#include <RobotAPI/libraries/core/CartesianPositionController.h>
+#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/interface/core/CartesianWaypointControllerConfig.h>
+
+#include "CartesianVelocityControllerWithRamp.h"
+
+namespace Eigen
+{
+ using Vector6f = Matrix<float, 6, 1>;
+}
+
+
+namespace armarx
+{
+ class CartesianWaypointController;
+ using CartesianWaypointControllerPtr = std::shared_ptr<CartesianWaypointController>;
+
+ class CartesianWaypointController
+ {
+ public:
+ CartesianWaypointController(const VirtualRobot::RobotNodeSetPtr& rns,
+ const Eigen::VectorXf& currentJointVelocity,
+ float maxPositionAcceleration,
+ float maxOrientationAcceleration,
+ float maxNullspaceAcceleration,
+ const VirtualRobot::RobotNodePtr& tcp = nullptr
+ );
+
+
+ void setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity)
+ {
+ ctrlCartesianVelWithRamps.setCurrentJointVelocity(currentJointVelocity);
+ }
+
+ const Eigen::VectorXf& calculate(float dt);
+
+ void setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints);
+ void addWaypoint(const Eigen::Matrix4f& waypoint);
+ void setTarget(const Eigen::Matrix4f& target);
+ void setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity);
+ void setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri);
+ void clearFeedForwardVelocity();
+
+ float getPositionError() const;
+
+ float getOrientationError() const;
+
+ bool isCurrentTargetReached() const;
+ bool isCurrentTargetNear() const;
+ bool isFinalTargetReached() const;
+ bool isFinalTargetNear() const;
+
+ bool isLastWaypoint() const;
+
+ const Eigen::Matrix4f& getCurrentTarget() const;
+ const Eigen::Vector3f getCurrentTargetPosition() const;
+
+ size_t skipToClosestWaypoint(float rad2mmFactor);
+
+ void setNullSpaceControl(bool enabled);
+
+ std::string getStatusText();
+
+ void setConfig(const CartesianWaypointControllerConfig& cfg);
+
+ CartesianVelocityControllerWithRamp ctrlCartesianVelWithRamps;
+ CartesianPositionController ctrlCartesianPos2Vel;
+
+ std::vector<Eigen::Matrix4f> waypoints;
+ size_t currentWaypointIndex = 0.f;
+
+ float thresholdPositionReached = 0.f;
+ float thresholdOrientationReached = 0.f;
+ float thresholdPositionNear = 0.f;
+ float thresholdOrientationNear = 0.f;
+
+ Eigen::Vector6f feedForwardVelocity = Eigen::Vector6f::Zero();
+ Eigen::Vector6f cartesianVelocity = Eigen::Vector6f::Zero();
+ bool autoClearFeedForward = true;
+
+
+ bool nullSpaceControlEnabled = true;
+ float jointLimitAvoidanceScale = 0.f;
+ float KpJointLimitAvoidance = 0.f;
+
+ private:
+ Eigen::VectorXf _out;
+ Eigen::VectorXf _jnv;
+ };
+}
diff --git a/source/RobotAPI/libraries/core/FramedOrientedPoint.h b/source/RobotAPI/libraries/core/FramedOrientedPoint.h
index 387ab6f4ce53f697362cc880da5e299dda3dcf47..7f2231c17bc80c39acb745a15ebdb672393a9a72 100644
--- a/source/RobotAPI/libraries/core/FramedOrientedPoint.h
+++ b/source/RobotAPI/libraries/core/FramedOrientedPoint.h
@@ -79,16 +79,16 @@ namespace armarx
{
return this->clone();
}
- VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const override
+ VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
{
return new FramedOrientedPoint(*this);
}
- std::string output(const Ice::Current& c = ::Ice::Current()) const override;
- VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const override
+ std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
+ VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
{
return VariantType::FramedOrientedPoint;
}
- bool validate(const Ice::Current& c = ::Ice::Current()) override
+ bool validate(const Ice::Current& c = Ice::emptyCurrent) override
{
return true;
}
@@ -100,8 +100,8 @@ namespace armarx
}
public: // serialization
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) override;
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
};
}
diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h
index e847e63497dfa5b2929649c77e1a6da2af5a1436..390396d228ebe51250dd0941019f30c661478cb3 100644
--- a/source/RobotAPI/libraries/core/FramedPose.h
+++ b/source/RobotAPI/libraries/core/FramedPose.h
@@ -110,16 +110,16 @@ namespace armarx
{
return this->clone();
}
- VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const override
+ VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
{
return new FramedDirection(*this);
}
- std::string output(const Ice::Current& c = ::Ice::Current()) const override;
- VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const override
+ std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
+ VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
{
return VariantType::FramedDirection;
}
- bool validate(const Ice::Current& c = ::Ice::Current()) override
+ bool validate(const Ice::Current& c = Ice::emptyCurrent) override
{
return true;
}
@@ -131,8 +131,8 @@ namespace armarx
}
public: // serialization
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) override;
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
private:
static Eigen::Matrix4f __GetRotationBetweenFrames(const std::string& oldFrame, const std::string& newFrame, VirtualRobot::RobotPtr robotState);
@@ -194,16 +194,16 @@ namespace armarx
{
return this->clone();
}
- VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const override
+ VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
{
return new FramedPosition(*this);
}
- std::string output(const Ice::Current& c = ::Ice::Current()) const override;
- VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const override
+ std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
+ VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
{
return VariantType::FramedPosition;
}
- bool validate(const Ice::Current& c = ::Ice::Current()) override
+ bool validate(const Ice::Current& c = Ice::emptyCurrent) override
{
return true;
}
@@ -215,8 +215,8 @@ namespace armarx
};
public: // serialization
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) override;
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
};
class FramedOrientation;
@@ -246,16 +246,16 @@ namespace armarx
{
return this->clone();
}
- VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const override
+ VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
{
return new FramedOrientation(*this);
}
- std::string output(const Ice::Current& c = ::Ice::Current()) const override;
- VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const override
+ std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
+ VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
{
return VariantType::FramedOrientation;
}
- bool validate(const Ice::Current& c = ::Ice::Current()) override
+ bool validate(const Ice::Current& c = Ice::emptyCurrent) override
{
return true;
}
@@ -280,8 +280,8 @@ namespace armarx
};
public: // serialization
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) override;
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
};
@@ -314,19 +314,19 @@ namespace armarx
return this->clone();
}
- VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const override
+ VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
{
return new FramedPose(*this);
}
- std::string output(const Ice::Current& c = ::Ice::Current()) const override;
+ std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
- VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const override
+ VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
{
return VariantType::FramedPose;
}
- bool validate(const Ice::Current& c = ::Ice::Current()) override
+ bool validate(const Ice::Current& c = Ice::emptyCurrent) override
{
return true;
}
@@ -355,8 +355,8 @@ namespace armarx
static VirtualRobot::LinkedCoordinate createLinkedCoordinate(const VirtualRobot::RobotPtr& virtualRobot, const FramedPositionPtr& position, const FramedOrientationPtr& orientation);
public:
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) override;
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
};
diff --git a/source/RobotAPI/libraries/core/LinkedPose.h b/source/RobotAPI/libraries/core/LinkedPose.h
index d6cb56ea4cbdac46b288629fb350da4cfb3e7e22..414309200bb28472dc03cbee856c42c01e8243b0 100644
--- a/source/RobotAPI/libraries/core/LinkedPose.h
+++ b/source/RobotAPI/libraries/core/LinkedPose.h
@@ -81,15 +81,15 @@ namespace armarx
// inherited from VariantDataClass
Ice::ObjectPtr ice_clone() const override;
- VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const override;
+ VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override;
- std::string output(const Ice::Current& c = ::Ice::Current()) const override;
+ std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
- VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const override;
+ VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override;
- bool validate(const Ice::Current& c = ::Ice::Current()) override;
+ bool validate(const Ice::Current& c = Ice::emptyCurrent) override;
- void changeFrame(const std::string& newFrame, const Ice::Current& c = ::Ice::Current()) override;
+ void changeFrame(const std::string& newFrame, const Ice::Current& c = Ice::emptyCurrent) override;
void changeToGlobal();
LinkedPosePtr toGlobal() const;
@@ -99,8 +99,8 @@ namespace armarx
return stream;
};
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) override;
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
protected:
void ice_postUnmarshal() override;
@@ -126,7 +126,7 @@ namespace armarx
~LinkedDirection() override;
- void changeFrame(const std::string& newFrame, const Ice::Current& c = Ice::Current()) override;
+ void changeFrame(const std::string& newFrame, const Ice::Current& c = Ice::emptyCurrent) override;
// inherited from VariantDataClass
Ice::ObjectPtr ice_clone() const override
@@ -134,24 +134,24 @@ namespace armarx
return this->clone();
}
- VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const override
+ VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
{
return new LinkedDirection(*this);
}
- std::string output(const Ice::Current& c = ::Ice::Current()) const override
+ std::string output(const Ice::Current& c = Ice::emptyCurrent) const override
{
std::stringstream s;
s << FramedDirection::toEigen() << std::endl << "reference robot: " << referenceRobot;
return s.str();
}
- VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const override
+ VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
{
return VariantType::LinkedDirection;
}
- bool validate(const Ice::Current& c = ::Ice::Current()) override
+ bool validate(const Ice::Current& c = Ice::emptyCurrent) override
{
return true;
}
@@ -163,8 +163,8 @@ namespace armarx
return stream;
};
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) override;
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
protected:
diff --git a/source/RobotAPI/libraries/core/OrientedPoint.h b/source/RobotAPI/libraries/core/OrientedPoint.h
index 6d1ed992b1de2adfbe4050c9d6a294a1b16de451..4bb8955e8cf9ddb85933c498c5861655fd453ab3 100644
--- a/source/RobotAPI/libraries/core/OrientedPoint.h
+++ b/source/RobotAPI/libraries/core/OrientedPoint.h
@@ -55,16 +55,16 @@ namespace armarx
{
return this->clone();
}
- VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const override
+ VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
{
return new OrientedPoint(*this);
}
- std::string output(const Ice::Current& c = ::Ice::Current()) const override;
- VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const override
+ std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
+ VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
{
return VariantType::OrientedPoint;
}
- bool validate(const Ice::Current& c = ::Ice::Current()) override
+ bool validate(const Ice::Current& c = Ice::emptyCurrent) override
{
return true;
}
@@ -76,8 +76,8 @@ namespace armarx
}
public: // serialization
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) override;
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
};
using OrientedPointPtr = IceInternal::Handle<OrientedPoint>;
diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp
index c9f70d3db8b12fe00a364d484990e13387d59219..5a4700dc5ee7b1a618227e03f97b0260b213bd00 100644
--- a/source/RobotAPI/libraries/core/PIDController.cpp
+++ b/source/RobotAPI/libraries/core/PIDController.cpp
@@ -59,6 +59,14 @@ void PIDController::reset()
previousError = 0;
integral = 0;
lastUpdateTime = TimeUtil::GetTime();
+ if (pdOutputFilter)
+ {
+ pdOutputFilter->reset();
+ }
+ if (differentialFilter)
+ {
+ differentialFilter->reset();
+ }
}
ScopedRecursiveLockPtr PIDController::getLock() const
@@ -146,7 +154,12 @@ void PIDController::update(double deltaSec, double measuredValue, double targetV
firstRun = false;
double oldControlValue = controlValue;
- controlValue = Kp * error + Ki * integral + Kd * derivative;
+ double pdControlValue = Kp * error + Kd * derivative;
+ if (pdOutputFilter)
+ {
+ pdControlValue = pdOutputFilter->update(deltaSec, pdControlValue);
+ }
+ controlValue = pdControlValue + Ki * integral;
if (deltaSec > 0.0)
{
double deriv = (controlValue - oldControlValue) / deltaSec;
diff --git a/source/RobotAPI/libraries/core/PIDController.h b/source/RobotAPI/libraries/core/PIDController.h
index 87bca749ae5f1ab06c9d04b425b2a2a37e653151..a1c6b006029187f32a71b56d7a2fb41cd58edbb1 100644
--- a/source/RobotAPI/libraries/core/PIDController.h
+++ b/source/RobotAPI/libraries/core/PIDController.h
@@ -68,6 +68,7 @@ namespace armarx
bool limitless;
bool threadSafe = true;
rtfilters::RTFilterBasePtr differentialFilter;
+ rtfilters::RTFilterBasePtr pdOutputFilter;
private:
ScopedRecursiveLockPtr getLock() const;
mutable RecursiveMutex mutex;
diff --git a/source/RobotAPI/libraries/core/Pose.cpp b/source/RobotAPI/libraries/core/Pose.cpp
index 7a99d4b6b7682be748d9d8cc2a63f764fda2318e..400b2839c8c90d726a8c780720b08190a20da1f5 100644
--- a/source/RobotAPI/libraries/core/Pose.cpp
+++ b/source/RobotAPI/libraries/core/Pose.cpp
@@ -29,6 +29,9 @@
#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/VirtualRobot.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
using namespace Eigen;
template class ::IceInternal::Handle<::armarx::Pose>;
@@ -198,13 +201,7 @@ namespace armarx
Matrix3f Quaternion::toEigen() const
{
- Matrix3f rot;
- rot = Quaternionf(
- this->qw,
- this->qx,
- this->qy,
- this->qz);
- return rot;
+ return toEigenQuaternion().toRotationMatrix();
}
Eigen::Quaternionf Quaternion::toEigenQuaternion() const
diff --git a/source/RobotAPI/libraries/core/Pose.h b/source/RobotAPI/libraries/core/Pose.h
index 848eaa099195dc63a0546e697d088a29dc3d4f7e..acd23ad090153f7b7a93a69992e0e0b405ceeb51 100644
--- a/source/RobotAPI/libraries/core/Pose.h
+++ b/source/RobotAPI/libraries/core/Pose.h
@@ -28,7 +28,6 @@
#include <ArmarXCore/observers/variant/Variant.h>
#include <ArmarXCore/observers/AbstractObjectSerializer.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
@@ -70,16 +69,16 @@ namespace armarx
{
return this->clone();
}
- VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const override
+ VariantDataClassPtr clone(const Ice::Current& = Ice::emptyCurrent) const override
{
return new Vector2(*this);
}
- std::string output(const Ice::Current& c = ::Ice::Current()) const override;
- VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const override
+ std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
+ VariantTypeId getType(const Ice::Current& = Ice::emptyCurrent) const override
{
return VariantType::Vector2;
}
- bool validate(const Ice::Current& c = ::Ice::Current()) override
+ bool validate(const Ice::Current& = Ice::emptyCurrent) override
{
return true;
}
@@ -91,8 +90,8 @@ namespace armarx
}
public: // serialization
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) override;
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
};
@@ -122,16 +121,16 @@ namespace armarx
{
return this->clone();
}
- VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const override
+ VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
{
return new Vector3(*this);
}
- std::string output(const Ice::Current& c = ::Ice::Current()) const override;
- VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const override
+ std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
+ VariantTypeId getType(const Ice::Current& = Ice::emptyCurrent) const override
{
return VariantType::Vector3;
}
- bool validate(const Ice::Current& c = ::Ice::Current()) override
+ bool validate(const Ice::Current& = Ice::emptyCurrent) override
{
return true;
}
@@ -144,8 +143,8 @@ namespace armarx
}
public: // serialization
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) override;
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
};
using Vector3Ptr = IceInternal::Handle<Vector3>;
@@ -161,6 +160,8 @@ namespace armarx
virtual public QuaternionBase
{
public:
+
+ /// Construct an identity quaternion.
Quaternion();
Quaternion(const Eigen::Matrix4f&);
Quaternion(const Eigen::Matrix3f&);
@@ -178,16 +179,16 @@ namespace armarx
{
return this->clone();
}
- VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const override
+ VariantDataClassPtr clone(const Ice::Current& = Ice::emptyCurrent) const override
{
return new Quaternion(*this);
}
- std::string output(const Ice::Current& c = ::Ice::Current()) const override;
- VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const override
+ std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
+ VariantTypeId getType(const Ice::Current& = Ice::emptyCurrent) const override
{
return VariantType::Quaternion;
}
- bool validate(const Ice::Current& c = ::Ice::Current()) override
+ bool validate(const Ice::Current& = Ice::emptyCurrent) override
{
return true;
}
@@ -200,8 +201,8 @@ namespace armarx
}
public: // serialization
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) override;
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
private:
void init(const Eigen::Matrix3f&);
@@ -235,16 +236,16 @@ namespace armarx
{
return this->clone();
}
- VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const override
+ VariantDataClassPtr clone(const Ice::Current& = Ice::emptyCurrent) const override
{
return new Pose(*this);
}
- std::string output(const Ice::Current& c = ::Ice::Current()) const override;
- VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const override
+ std::string output(const Ice::Current& = Ice::emptyCurrent) const override;
+ VariantTypeId getType(const Ice::Current& = Ice::emptyCurrent) const override
{
return VariantType::Pose;
}
- bool validate(const Ice::Current& c = ::Ice::Current()) override
+ bool validate(const Ice::Current& = Ice::emptyCurrent) override
{
return true;
}
@@ -256,8 +257,8 @@ namespace armarx
}
public: // serialization
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) override;
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
protected:
void init();
diff --git a/source/RobotAPI/libraries/core/RobotAPIObjectFactories.cpp b/source/RobotAPI/libraries/core/RobotAPIObjectFactories.cpp
index e2d22fa0e321fc0ce6c3699466fc24b5002755c0..4305b29ceb6166b81d238e2309c799db83ce1e66 100644
--- a/source/RobotAPI/libraries/core/RobotAPIObjectFactories.cpp
+++ b/source/RobotAPI/libraries/core/RobotAPIObjectFactories.cpp
@@ -36,6 +36,7 @@
#include <RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.h>
#include <RobotAPI/libraries/core/OrientedPoint.h>
#include <RobotAPI/libraries/core/FramedOrientedPoint.h>
+#include <RobotAPI/libraries/core/CartesianPositionController.h>
using namespace armarx;
@@ -58,6 +59,7 @@ ObjectFactoryMap RobotAPIObjectFactories::getFactories()
add<armarx::OrientedPointBase, armarx::OrientedPoint>(map);
add<armarx::FramedOrientedPointBase, armarx::FramedOrientedPoint>(map);
+ add<armarx::CartesianPositionControllerConfigBase, armarx::CartesianPositionControllerConfig>(map);
add<armarx::PoseMedianFilterBase, armarx::filters::PoseMedianFilter>(map);
add<armarx::PoseAverageFilterBase, armarx::filters::PoseAverageFilter>(map);
diff --git a/source/RobotAPI/libraries/core/SimpleDiffIK.cpp b/source/RobotAPI/libraries/core/SimpleDiffIK.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2eb0226c18833da6139d8b236defadaef3f0bbf7
--- /dev/null
+++ b/source/RobotAPI/libraries/core/SimpleDiffIK.cpp
@@ -0,0 +1,101 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 SecondHands Demo (shdemo at armar6)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "CartesianPositionController.h"
+#include "CartesianVelocityController.h"
+#include "SimpleDiffIK.h"
+
+using namespace armarx;
+
+
+
+SimpleDiffIK::Result SimpleDiffIK::CalculateDiffIK(const Eigen::Matrix4f targetPose, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, Parameters params)
+{
+ tcp = tcp ? tcp : rns->getTCP();
+ CartesianVelocityController velocityController(rns);
+ CartesianPositionController positionController(tcp);
+
+ for (size_t i = 0; i <= params.stepsInitial + params.stepsFineTune; i++)
+ {
+ Eigen::Vector3f posDiff = positionController.getPositionDiff(targetPose);
+ Eigen::Vector3f oriDiff = positionController.getOrientationDiff(targetPose);
+
+ //ARMARX_IMPORTANT << VAROUT(posDiff) << VAROUT(oriDiff);
+
+ Eigen::VectorXf cartesialVel(6);
+ cartesialVel << posDiff(0), posDiff(1), posDiff(2), oriDiff(0), oriDiff(1), oriDiff(2);
+ Eigen::VectorXf jnv = params.jointLimitAvoidanceKp * velocityController.calculateJointLimitAvoidance();
+ Eigen::VectorXf jv = velocityController.calculate(cartesialVel, jnv, VirtualRobot::IKSolver::All);
+
+
+ float stepLength = i < params.stepsInitial ? params.ikStepLengthInitial : params.ikStepLengthFineTune;
+ jv = jv * stepLength;
+
+ for (size_t n = 0; n < rns->getSize(); n++)
+ {
+ rns->getNode(n)->setJointValue(rns->getNode(n)->getJointValue() + jv(n));
+ }
+ }
+
+ Result result;
+ result.jointValues = rns->getJointValuesEigen();
+ result.posDiff = positionController.getPositionDiff(targetPose);
+ result.oriDiff = positionController.getOrientationDiff(targetPose);
+ result.posError = positionController.getPositionError(targetPose);
+ result.oriError = positionController.getOrientationError(targetPose);
+ result.reached = result.posError < params.maxPosError && result.oriError < params.maxOriError;
+
+ result.jointLimitMargins = Eigen::VectorXf::Zero(rns->getSize());
+ result.minimumJointLimitMargin = FLT_MAX;
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ VirtualRobot::RobotNodePtr rn = rns->getNode(i);
+ if (rn->isLimitless())
+ {
+ result.jointLimitMargins(i) = M_PI;
+ }
+ else
+ {
+ result.jointLimitMargins(i) = std::min(rn->getJointValue() - rn->getJointLimitLo(), rn->getJointLimitHi() - rn->getJointValue());
+ result.minimumJointLimitMargin = std::min(result.minimumJointLimitMargin, result.jointLimitMargins(i));
+ }
+ }
+
+ return result;
+}
+
+SimpleDiffIK::Reachability SimpleDiffIK::CalculateReachability(const std::vector<Eigen::Matrix4f> targets, const Eigen::VectorXf& initialJV, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, SimpleDiffIK::Parameters params)
+{
+ Reachability r;
+ rns->setJointValues(initialJV);
+ for (const Eigen::Matrix4f& target : targets)
+ {
+ Result res = CalculateDiffIK(target, rns, tcp, params);
+ r.aggregate(res);
+ }
+ if (!r.reachable)
+ {
+ r.minimumJointLimitMargin = -1;
+ }
+ return r;
+}
diff --git a/source/RobotAPI/libraries/core/SimpleDiffIK.h b/source/RobotAPI/libraries/core/SimpleDiffIK.h
new file mode 100644
index 0000000000000000000000000000000000000000..23f6e9ae0c81676fa449c48ee086b244e61e64ae
--- /dev/null
+++ b/source/RobotAPI/libraries/core/SimpleDiffIK.h
@@ -0,0 +1,92 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
+
+namespace armarx
+{
+ typedef boost::shared_ptr<class SimpleDiffIK> SimpleDiffIKPtr;
+
+ class SimpleDiffIK
+ {
+ public:
+ struct Parameters
+ {
+ Parameters() {}
+ // IK params
+ float ikStepLengthInitial = 0.2f;
+ float ikStepLengthFineTune = 0.5f;
+ size_t stepsInitial = 25;
+ size_t stepsFineTune = 5;
+ float maxPosError = 10.f;
+ float maxOriError = 0.05f;
+ float jointLimitAvoidanceKp = 2.0f;
+ };
+ struct Result
+ {
+ Eigen::VectorXf jointValues;
+ Eigen::Vector3f posDiff;
+ Eigen::Vector3f oriDiff;
+ float posError;
+ float oriError;
+ bool reached;
+ Eigen::VectorXf jointLimitMargins;
+ float minimumJointLimitMargin;
+ };
+ struct Reachability
+ {
+
+ bool reachable = true;
+ float minimumJointLimitMargin = M_PI;
+ Eigen::VectorXf jointLimitMargins;
+ float maxPosError = 0;
+ float maxOriError = 0;
+
+ void aggregate(const Result& result)
+ {
+ reachable = reachable && result.reached;
+ minimumJointLimitMargin = std::min(minimumJointLimitMargin, result.minimumJointLimitMargin);
+ if (jointLimitMargins.rows() == 0)
+ {
+ jointLimitMargins = result.jointLimitMargins;
+ }
+ else
+ {
+ jointLimitMargins = jointLimitMargins.cwiseMin(result.jointLimitMargins);
+ }
+ maxPosError = std::max(maxPosError, result.posError);
+ maxOriError = std::max(maxOriError, result.oriError);
+ }
+ };
+
+ static Result CalculateDiffIK(const Eigen::Matrix4f targetPose, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), Parameters params = Parameters());
+
+ static Reachability CalculateReachability(const std::vector<Eigen::Matrix4f> targets, const Eigen::VectorXf& initialJV, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), Parameters params = Parameters());
+
+ };
+}
diff --git a/source/RobotAPI/libraries/core/SimpleGridReachability.cpp b/source/RobotAPI/libraries/core/SimpleGridReachability.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c13ea798af6433f002e02832de70ac8093834d4f
--- /dev/null
+++ b/source/RobotAPI/libraries/core/SimpleGridReachability.cpp
@@ -0,0 +1,44 @@
+#include "CartesianVelocityController.h"
+#include "SimpleGridReachability.h"
+
+using namespace armarx;
+
+
+SimpleGridReachability::Result SimpleGridReachability::calculateDiffIK(const Eigen::Matrix4f targetPose, const Parameters& params)
+{
+ VirtualRobot::RobotNodePtr rns = params.nodeSet;
+ VirtualRobot::RobotNodeSetPtr tcp = params.tcp ? params.tcp : rns->getTCP();
+ CartesianVelocityController velocityController(rns);
+ CartesianPositionController positionController(tcp);
+
+ Eigen::VectorXf initialJV = rns->getJointValuesEigen();
+ for (size_t i = 0; i <= params.stepsInitial + params.stepsFineTune; i++)
+ {
+ Eigen::Vector3f posDiff = positionController.getPositionDiff(targetPose);
+ Eigen::Vector3f oriDiff = positionController.getOrientationDiff(targetPose);
+
+ //ARMARX_IMPORTANT << VAROUT(posDiff) << VAROUT(oriDiff);
+
+ Eigen::VectorXf cartesialVel(6);
+ cartesialVel << posDiff(0), posDiff(1), posDiff(2), oriDiff(0), oriDiff(1), oriDiff(2);
+ Eigen::VectorXf jnv = params.jointLimitAvoidanceKp * velocityController.calculateJointLimitAvoidance();
+ Eigen::VectorXf jv = velocityController.calculate(cartesialVel, jnv, VirtualRobot::IKSolver::All);
+
+
+ float stepLength = i < params.stepsInitial ? params.ikStepLengthInitial : params.ikStepLengthFineTune;
+ jv = jv * stepLength;
+
+ for (size_t n = 0; n < rns->getSize(); n++)
+ {
+ rns->getNode(n)->setJointValue(rns->getNode(n)->getJointValue() + jv(n));
+ }
+ }
+
+ Result result;
+ result.jointValues = rns->getJointValuesEigen();
+ result.posError = positionController.getPositionDiff(targetPose);
+ result.oriError = positionController.getOrientationError(targetPose);
+ result.reached = result.posError < params.maxPosError && result.oriError < params.maxOriError;
+
+ return result;
+}
diff --git a/source/RobotAPI/libraries/core/SimpleGridReachability.h b/source/RobotAPI/libraries/core/SimpleGridReachability.h
new file mode 100644
index 0000000000000000000000000000000000000000..4ac9bc70f16524bbf86cbba53efc5f034a5c4f96
--- /dev/null
+++ b/source/RobotAPI/libraries/core/SimpleGridReachability.h
@@ -0,0 +1,37 @@
+#pragma once
+#include "CartesianPositionController.h"
+
+namespace armarx
+{
+ class SimpleGridReachability
+ {
+ public:
+ struct Parameters
+ {
+ VirtualRobot::RobotNodePtr tcp;
+ VirtualRobot::RobotNodeSetPtr nodeSet;
+
+ // IK params
+ float ikStepLengthInitial = 0.2f;
+ float ikStepLengthFineTune = 0.5f;
+ size_t stepsInitial = 25;
+ size_t stepsFineTune = 5;
+ float maxPosError = 10.f;
+ float maxOriError = 0.05f;
+ float jointLimitAvoidanceKp = 2.0f;
+ };
+ struct Result
+ {
+ Eigen::VectorXf jointValues;
+ float posError;
+ float oriError;
+ bool reached;
+ };
+
+ static Result CalculateDiffIK(const Eigen::Matrix4f targetPose, const Parameters& params);
+
+
+ };
+
+}
+
diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h
index 10099dac87eca2e7bf35145871fdba58f82eaf3a..f4e02ebe1bdb449c9baf0f0e9677f3d2164064a1 100644
--- a/source/RobotAPI/libraries/core/Trajectory.h
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -411,15 +411,15 @@ namespace armarx
void ice_postUnmarshal() override;
// Serializable interface
- void serialize(const ObjectSerializerBasePtr& obj, const Ice::Current& = Ice::Current()) const override;
- void deserialize(const ObjectSerializerBasePtr&, const Ice::Current& = Ice::Current()) override;
+ void serialize(const ObjectSerializerBasePtr& obj, const Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const ObjectSerializerBasePtr&, const Ice::Current& = Ice::emptyCurrent) override;
// VariantDataClass interface
- VariantDataClassPtr clone(const Ice::Current& c = Ice::Current()) const override;
+ VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override;
TrajectoryPtr cloneMetaData() const;
- std::string output(const Ice::Current& c = Ice::Current()) const override;
- Ice::Int getType(const Ice::Current& c = Ice::Current()) const override;
- bool validate(const Ice::Current& c = Ice::Current()) override;
+ std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
+ Ice::Int getType(const Ice::Current& c = Ice::emptyCurrent) const override;
+ bool validate(const Ice::Current& c = Ice::emptyCurrent) override;
Ice::ObjectPtr ice_clone() const override;
diff --git a/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h b/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
index de3c5eea8dcf7789e9f78eedb732bf2b7fa9553a..b5f668bae403f8e9d3dd48839fd3163115f05ab0 100644
--- a/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
+++ b/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
@@ -54,7 +54,7 @@ namespace armarx
// DatafieldFilterBase interface
public:
- VariantBasePtr calculate(const Ice::Current& c = Ice::Current()) const override
+ VariantBasePtr calculate(const Ice::Current& c = Ice::emptyCurrent) const override
{
ScopedLock lock(historyMutex);
diff --git a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
index 2e4aa3efc6f3ae9d17c6012f3962f80b97411b66..9a2dbb98e6026f602741b6020b50e0d1fd1a9ad4 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
@@ -172,7 +172,10 @@ DatafieldRefBasePtr RobotStateObserver::getPoseDatafield(const std::string& node
void RobotStateObserver::updateNodeVelocities(const NameValueMap& jointVel, long timestampMicroSeconds)
{
-
+ if (jointVel.empty())
+ {
+ return;
+ }
if (getState() < eManagedIceObjectStarting)
{
return;
diff --git a/source/RobotAPI/libraries/core/test/CMakeLists.txt b/source/RobotAPI/libraries/core/test/CMakeLists.txt
index 37fd064ee6553f8324c2fd2916c89dbb801f29e7..3f70ca0b8d1b201a90bc46e0d5bdf11d3241baf4 100644
--- a/source/RobotAPI/libraries/core/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/test/CMakeLists.txt
@@ -9,3 +9,5 @@ armarx_add_test(CartesianVelocityControllerTest CartesianVelocityControllerTest.
armarx_add_test(CartesianVelocityRampTest CartesianVelocityRampTest.cpp "${LIBS}")
armarx_add_test(CartesianVelocityControllerWithRampTest CartesianVelocityControllerWithRampTest.cpp "${LIBS}")
+
+armarx_add_test(DebugDrawerTopicTest DebugDrawerTopicTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/core/test/DebugDrawerTopicTest.cpp b/source/RobotAPI/libraries/core/test/DebugDrawerTopicTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b6a2310df6ec38de4a5bb4f413636765212b5c0e
--- /dev/null
+++ b/source/RobotAPI/libraries/core/test/DebugDrawerTopicTest.cpp
@@ -0,0 +1,143 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::DebugDrawerTopicTest::Test
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+
+#include <ArmarXCore/core/test/IceTestHelper.h>
+
+#include <RobotAPI/libraries/core/visualization/DebugDrawerTopic.h>
+
+
+using namespace armarx;
+
+
+// PCL-like dummy types.
+
+struct PointXYZ
+{
+ float x, y, z;
+};
+
+struct PointXYZRGBA : public PointXYZ
+{
+ uint8_t r, g, b, a;
+};
+
+struct PointXYZRGBL : public PointXYZRGBA
+{
+ uint32_t label;
+};
+
+template <class PointT>
+struct PointCloud
+{
+private:
+ /// The point container type.
+ using VectorT = std::vector<PointT>;
+
+public:
+
+ PointCloud() {}
+ PointCloud(const VectorT& points) : points(points) {}
+
+ // Container methods.
+ std::size_t size() const { return points.size(); }
+
+ PointT& operator[](std::size_t i) { return points[i]; }
+ const PointT& operator[](std::size_t i) const { return points[i]; }
+
+ // Iterators.
+ typename VectorT::iterator begin() { return points.begin(); }
+ typename VectorT::const_iterator begin() const { return points.begin(); }
+ typename VectorT::iterator end() { return points.end(); }
+ typename VectorT::const_iterator end() const { return points.end(); }
+
+
+ /// The points.
+ VectorT points;
+};
+
+
+/* These test do not actually check any behaviour,
+ * but check whether this code compiles.
+ */
+
+template <class PointT>
+struct Fixture
+{
+ Fixture()
+ {
+ }
+
+ const DebugDrawerTopic::VisuID id {"name", "layer"};
+ const int pointSize = 10;
+
+ DebugDrawerTopic drawer;
+
+ PointCloud<PointT> pointCloudMutable;
+ const PointCloud<PointT>& pointCloud = pointCloudMutable;
+};
+
+
+BOOST_FIXTURE_TEST_CASE(test_drawPointCloud_PointXYZ, Fixture<PointXYZ>)
+{
+ pointCloudMutable.points = { {1, 2, 3}, {2, 3, 4}, {3, 4, 5} };
+
+ drawer.drawPointCloud(id, pointCloud);
+ drawer.drawPointCloud(id, pointCloud.points, DrawColor {0, 0.5, 1, 1});
+
+ drawer.drawPointCloud(id, pointCloud,
+ [](const PointXYZ&) { return DrawColor{0, 0.5, 1, 1}; }, pointSize);
+}
+
+
+BOOST_FIXTURE_TEST_CASE(test_drawPointCloud_PointXYZRGBA, Fixture<PointXYZRGBA>)
+{
+ drawer.drawPointCloud(id, pointCloud);
+ drawer.drawPointCloud(id, pointCloud.points, DrawColor {0, 0.5, 1, 1});
+
+ drawer.drawPointCloud(id, pointCloud,
+ [](const PointXYZRGBA&) { return DrawColor{0, 0.5, 1, 1}; }, pointSize);
+
+ drawer.drawPointCloudRGBA(id, pointCloud, pointSize);
+}
+
+
+BOOST_FIXTURE_TEST_CASE(test_drawPointCloud_PointXYZRGBL, Fixture<PointXYZRGBL>)
+{
+ drawer.drawPointCloud(id, pointCloud);
+ drawer.drawPointCloud(id, pointCloud.points, DrawColor {0, 0.5, 1, 1});
+
+ drawer.drawPointCloud(id, pointCloud,
+ [](const PointXYZRGBL&) { return DrawColor{0, 0.5, 1, 1}; }, pointSize);
+
+ drawer.drawPointCloudRGBA(id, pointCloud, pointSize);
+}
+
+
+
diff --git a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ef4c6d5a64a8a0dcc49ba960babc9c0c74848348
--- /dev/null
+++ b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
@@ -0,0 +1,933 @@
+#include "DebugDrawerTopic.h"
+
+#include <VirtualRobot/math/Helpers.h>
+#include <VirtualRobot/Visualization/TriMeshModel.h>
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
+#include "../Pose.h"
+
+#include "GlasbeyLUT.h"
+
+
+namespace armarx
+{
+
+ DebugDrawerTopic::VisuID::VisuID() : VisuID("", "")
+ {}
+
+ DebugDrawerTopic::VisuID::VisuID(const std::string& name, const std::string& layer) :
+ name(name), layer(layer)
+ {}
+
+ std::ostream& operator<<(std::ostream& os, const DebugDrawerTopic::VisuID& rhs)
+ {
+ os << "Visu '" << rhs.name << "' at layer '" << rhs.layer << "'";
+ return os;
+ }
+
+
+ const std::string DebugDrawerTopic::TOPIC_NAME = "DebugDrawerUpdates";
+ const std::string DebugDrawerTopic::DEFAULT_LAYER = "debug";
+ const DebugDrawerTopic::Defaults DebugDrawerTopic::DEFAULTS = {};
+
+ DebugDrawerTopic::DebugDrawerTopic(const std::string& layer) :
+ _layer(layer)
+ {}
+
+ DebugDrawerTopic::DebugDrawerTopic(
+ const DebugDrawerInterfacePrx& topic, const std::string& layer) :
+ topic(topic), _layer(layer)
+ {}
+
+ void DebugDrawerTopic::setTopic(const DebugDrawerInterfacePrx& topic)
+ {
+ this->topic = topic;
+ }
+
+ DebugDrawerInterfacePrx DebugDrawerTopic::getTopic() const
+ {
+ return topic;
+ }
+
+ void DebugDrawerTopic::offeringTopic(ManagedIceObject& component, const std::string& topicNameOverride) const
+ {
+ component.offeringTopic(topicNameOverride.empty() ? TOPIC_NAME : topicNameOverride);
+ }
+
+ void DebugDrawerTopic::getTopic(ManagedIceObject& component, const std::string& topicNameOverride)
+ {
+ setTopic(component.getTopic<DebugDrawerInterfacePrx>(topicNameOverride.empty() ? TOPIC_NAME : topicNameOverride));
+ }
+
+ const std::string& DebugDrawerTopic::getLayer() const
+ {
+ return _layer;
+ }
+
+ void DebugDrawerTopic::setLayer(const std::string& layer)
+ {
+ this->_layer = layer;
+ }
+
+ float DebugDrawerTopic::getLengthScale() const
+ {
+ return _lengthScale;
+ }
+
+ void DebugDrawerTopic::setLengthScale(float scale)
+ {
+ this->_lengthScale = scale;
+ }
+
+ void DebugDrawerTopic::setLengthScaleMetersToMillimeters()
+ {
+ setLengthScale(1000);
+ }
+
+ void DebugDrawerTopic::setLengthScaleMillimetersToMeters()
+ {
+ setLengthScale(0.001f);
+ }
+
+ float DebugDrawerTopic::getPoseScale() const
+ {
+ return _poseScale;
+ }
+
+ void DebugDrawerTopic::setPoseScale(float scale)
+ {
+ this->_poseScale = scale;
+ }
+
+ void DebugDrawerTopic::setPoseScaleMeters()
+ {
+ setPoseScale(0.001f);
+ }
+
+ void DebugDrawerTopic::setPoseScaleMillimeters()
+ {
+ setPoseScale(1);
+ }
+
+ void DebugDrawerTopic::shortSleep()
+ {
+ this->sleepFor(_shortSleepDuration);
+ }
+
+ void DebugDrawerTopic::clearAll(bool sleep)
+ {
+ if (enabled())
+ {
+ topic->clearAll();
+ if (sleep)
+ {
+ shortSleep();
+ }
+ }
+ }
+
+ void DebugDrawerTopic::clearLayer(bool sleep)
+ {
+ clearLayer(_layer, sleep);
+ }
+
+ void DebugDrawerTopic::clearLayer(const std::string& layer, bool sleep)
+ {
+ if (enabled())
+ {
+ topic->clearLayer(layer);
+ if (sleep)
+ {
+ shortSleep();
+ }
+ }
+ }
+
+ void DebugDrawerTopic::drawText(
+ const VisuID& id, const Eigen::Vector3f& position, const std::string& text,
+ int size, const DrawColor color,
+ bool ignoreLengthScale)
+ {
+ if (enabled())
+ {
+ const float scale = lengthScale(ignoreLengthScale);
+ topic->setTextVisu(layer(id), id.name, text, scaled(scale, position), color, size);
+ }
+ }
+
+ void DebugDrawerTopic::drawBox(
+ const VisuID& id, const Eigen::Vector3f& position, const Eigen::Quaternionf& orientation,
+ const Eigen::Vector3f& extents, const DrawColor& color,
+ bool ignoreLengthScale)
+ {
+ if (enabled())
+ {
+ const float scale = lengthScale(ignoreLengthScale);
+ topic->setBoxVisu(layer(id), id.name,
+ new Pose(scaled(scale, position), new Quaternion(orientation)),
+ scaled(scale, extents), color);
+ }
+ }
+
+ void DebugDrawerTopic::drawBox(const VisuID& id, const Eigen::Matrix4f& pose, const Eigen::Vector3f& extents,
+ const DrawColor& color, bool ignoreLengthScale)
+ {
+ drawBox(id, math::Helpers::Position(pose), Eigen::Quaternionf(math::Helpers::Orientation(pose)),
+ extents, color, ignoreLengthScale);
+ }
+
+ void DebugDrawerTopic::drawBox(const DebugDrawerTopic::VisuID& id, const VirtualRobot::BoundingBox& boundingBox,
+ const DrawColor& color, bool ignoreLengthScale)
+ {
+ drawBox(id, boundingBox, Eigen::Matrix4f::Identity(), color, ignoreLengthScale);
+ }
+
+ void DebugDrawerTopic::drawBox(
+ const DebugDrawerTopic::VisuID& id,
+ const VirtualRobot::BoundingBox& boundingBox, const Eigen::Matrix4f& pose,
+ const DrawColor& color, bool ignoreLengthScale)
+ {
+ const Eigen::Vector3f center = .5 * (boundingBox.getMin() + boundingBox.getMax());
+ drawBox(id, math::Helpers::TransformPosition(pose, center),
+ Eigen::Quaternionf(math::Helpers::Orientation(pose)),
+ boundingBox.getMax() - boundingBox.getMin(), color, ignoreLengthScale);
+ }
+
+ void DebugDrawerTopic::drawCylinder(const DebugDrawerTopic::VisuID& id,
+ const Eigen::Vector3f& center, const Eigen::Vector3f& direction, float radius, float length,
+ const DrawColor& color, bool ignoreLengthScale)
+ {
+ if (enabled())
+ {
+ const float scale = lengthScale(ignoreLengthScale);
+ topic->setCylinderVisu(layer(id), id.name, scaled(scale, center), new Vector3(direction),
+ scaled(scale, length), scaled(scale, radius), color);
+ }
+ }
+
+ void DebugDrawerTopic::drawCylinder(
+ const DebugDrawerTopic::VisuID& id,
+ const Eigen::Vector3f& center, const Eigen::Quaternionf& orientation, float radius, float length,
+ const DrawColor& color, bool ignoreLengthScale)
+ {
+ drawCylinder(id, center, orientation * Eigen::Vector3f::UnitY(), radius, length, color,
+ ignoreLengthScale);
+ }
+
+ void DebugDrawerTopic::drawCylinderFromTo(
+ const DebugDrawerTopic::VisuID& id,
+ const Eigen::Vector3f& from, const Eigen::Vector3f& to, float radius,
+ const DrawColor& color, bool ignoreLengthScale)
+ {
+ if (enabled())
+ {
+ const Eigen::Vector3f dir = (to - from); // no need for scaling at this point
+ const float length = dir.norm();
+ drawCylinder(id, .5 * (from + to), dir / length, radius, length, color, ignoreLengthScale);
+ }
+ }
+
+ void DebugDrawerTopic::drawSphere(
+ const DebugDrawerTopic::VisuID& id,
+ const Eigen::Vector3f& center, float radius,
+ const DrawColor& color, bool ignoreLengthScale)
+ {
+ if (enabled())
+ {
+ const float scale = lengthScale(ignoreLengthScale);
+ topic->setSphereVisu(layer(id), id.name, scaled(scale, center), color,
+ scaled(scale, radius));
+ }
+ }
+
+
+ void DebugDrawerTopic::drawArrow(
+ const VisuID& id,
+ const Eigen::Vector3f& position, const Eigen::Vector3f& direction, float length,
+ float width, const DrawColor& color, bool ignoreLengthScale)
+ {
+ if (enabled())
+ {
+ const float scale = lengthScale(ignoreLengthScale);
+ topic->setArrowVisu(layer(id), id.name, scaled(scale, position), new Vector3(direction),
+ color, scaled(scale, length), scaled(scale, width));
+ }
+ }
+
+ void DebugDrawerTopic::drawArrowFromTo(
+ const VisuID& id,
+ const Eigen::Vector3f& from, const Eigen::Vector3f& to,
+ float width, const DrawColor& color, bool ignoreLengthScale)
+ {
+ if (enabled())
+ {
+ const Eigen::Vector3f dir = (to - from); // no need for scaling at this point
+ const float length = dir.norm();
+ drawArrow(id, from, dir / length, length, width, color, ignoreLengthScale);
+ }
+ }
+
+ void DebugDrawerTopic::removeArrow(const DebugDrawerTopic::VisuID& id)
+ {
+ if (enabled())
+ {
+ topic->removeArrowVisu(layer(id), id.name);
+ }
+ }
+
+
+ void DebugDrawerTopic::drawPolygon(const VisuID& id,
+ const std::vector<Eigen::Vector3f>& points,
+ const DrawColor& colorFace, float lineWidth, const DrawColor& colorEdge,
+ bool ignoreLengthScale)
+ {
+ if (enabled())
+ {
+ const float scale = lengthScale(ignoreLengthScale);
+
+ PolygonPointList polyPoints;
+ polyPoints.reserve(points.size());
+
+ for (const auto& point : points)
+ {
+ polyPoints.push_back(scaled(scale, point));
+ }
+
+ topic->setPolygonVisu(layer(id), id.name, polyPoints,
+ colorFace, colorEdge, lineWidth);
+ }
+ }
+
+ void DebugDrawerTopic::drawLine(
+ const VisuID& id, const Eigen::Vector3f& from, const Eigen::Vector3f& to,
+ float width, const DrawColor& color, bool ignoreLengthScale)
+ {
+ if (enabled())
+ {
+ const float scale = lengthScale(ignoreLengthScale);
+
+ topic->setLineVisu(layer(id), id.name, scaled(scale, from), scaled(scale, to),
+ width, color);
+ }
+ }
+
+ void DebugDrawerTopic::drawLineSet(
+ const VisuID& id, const DebugDrawerLineSet& lineSet, bool ignoreLengthScale)
+ {
+ if (enabled())
+ {
+ const float scale = lengthScale(ignoreLengthScale);
+
+ if (1.f <= scale && scale <= 1.f)
+ {
+ // Can use lineSet directly.
+ topic->setLineSetVisu(layer(id), id.name, lineSet);
+ }
+ else
+ {
+ // Need to scale line set, hence, reconstruct it.
+ DebugDrawerLineSet scaledLineSet = lineSet;
+ for (auto& point : scaledLineSet.points)
+ {
+ scaleXYZ(scale, point);
+ }
+ topic->setLineSetVisu(layer(id), id.name, scaledLineSet);
+ }
+ }
+ }
+
+ void DebugDrawerTopic::drawLineSet(
+ const VisuID& id, const std::vector<Eigen::Vector3f>& points,
+ float width, const DrawColor& color, bool ignoreLengthScale)
+ {
+ if (enabled())
+ {
+ const float scale = lengthScale(ignoreLengthScale);
+
+ DebugDrawerLineSet lineSet;
+ for (const auto& point : points)
+ {
+ lineSet.points.push_back(scaledT<DebugDrawerPointCloudElement>(scale, point));
+ }
+
+ lineSet.lineWidth = width;
+ lineSet.colorNoIntensity = lineSet.colorFullIntensity = color;
+ lineSet.intensities.assign(points.size() / 2, 0.);
+
+ topic->setLineSetVisu(layer(id), id.name, lineSet);
+ }
+ }
+
+
+ void DebugDrawerTopic::drawLineSet(
+ const VisuID& id, const std::vector<Eigen::Vector3f>& points, float width,
+ const DrawColor& colorA, const DrawColor& colorB, const std::vector<float>& intensitiesB,
+ bool ignoreLengthScale)
+ {
+ if (enabled())
+ {
+ const float scale = lengthScale(ignoreLengthScale);
+
+ DebugDrawerLineSet lineSet;
+ for (const auto& point : points)
+ {
+ lineSet.points.push_back(scaledT<DebugDrawerPointCloudElement>(scale, point));
+ }
+
+ lineSet.lineWidth = width;
+ lineSet.colorNoIntensity = colorA;
+ lineSet.colorFullIntensity = colorB;
+ lineSet.intensities = intensitiesB;
+
+ topic->setLineSetVisu(layer(id), id.name, lineSet);
+ }
+ }
+
+
+ void DebugDrawerTopic::drawPose(
+ const VisuID& id, const Eigen::Matrix4f& pose, bool ignoreLengthScale)
+ {
+ drawPose(id, pose, _poseScale, ignoreLengthScale);
+ }
+
+ void DebugDrawerTopic::drawPose(
+ const VisuID& id,
+ const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
+ bool ignoreLengthScale)
+ {
+ drawPose(id, math::Helpers::Pose(pos, ori), _poseScale, ignoreLengthScale);
+ }
+
+ void DebugDrawerTopic::drawPose(
+ const VisuID& id, const Eigen::Matrix4f& pose, float scale,
+ bool ignoreLengthScale)
+ {
+ if (enabled())
+ {
+ const float lenghtScale = lengthScale(ignoreLengthScale);
+
+ if (scale >= 1 && scale <= 1) // squelch compiler warning that == is unsafe
+ {
+ topic->setPoseVisu(layer(id), id.name, scaled(lenghtScale, pose));
+ }
+ else
+ {
+ topic->setScaledPoseVisu(layer(id), id.name, scaled(lenghtScale, pose), scale);
+ }
+ }
+
+ }
+
+ void DebugDrawerTopic::drawPose(
+ const VisuID& id, const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
+ float scale, bool ignoreLengthScale)
+ {
+ drawPose(id, math::Helpers::Pose(pos, ori), scale, ignoreLengthScale);
+ }
+
+ void DebugDrawerTopic::removePose(const DebugDrawerTopic::VisuID& id)
+ {
+ if (enabled())
+ {
+ topic->removePoseVisu(layer(id), id.name);
+ }
+ }
+
+
+ void DebugDrawerTopic::drawRobot(const DebugDrawerTopic::VisuID& id,
+ const std::string& robotFile, const std::string& armarxProject,
+ DrawStyle drawStyle)
+ {
+ if (enabled())
+ {
+ topic->setRobotVisu(layer(id), id.name, robotFile, armarxProject, drawStyle);
+ }
+ }
+
+ void DebugDrawerTopic::updateRobotPose(
+ const DebugDrawerTopic::VisuID& id,
+ const Eigen::Matrix4f& pose, bool ignoreScale)
+ {
+ if (enabled())
+ {
+ topic->updateRobotPose(layer(id), id.name, scaled(lengthScale(ignoreScale), pose));
+ }
+ }
+
+ void DebugDrawerTopic::updateRobotPose(
+ const DebugDrawerTopic::VisuID& id,
+ const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori, bool ignoreScale)
+ {
+ updateRobotPose(id, math::Helpers::Pose(pos, ori), ignoreScale);
+ }
+
+ void DebugDrawerTopic::updateRobotConfig(
+ const DebugDrawerTopic::VisuID& id, const std::map<std::string, float>& config)
+ {
+ if (enabled())
+ {
+ topic->updateRobotConfig(layer(id), id.name, config);
+ }
+ }
+
+ void DebugDrawerTopic::updateRobotColor(
+ const DebugDrawerTopic::VisuID& id, const DrawColor& color)
+ {
+ if (enabled())
+ {
+ topic->updateRobotColor(layer(id), id.name, color);
+ }
+ }
+
+ void DebugDrawerTopic::updateRobotNodeColor(
+ const DebugDrawerTopic::VisuID& id,
+ const std::string& nodeName, const DrawColor& color)
+ {
+ if (enabled())
+ {
+ topic->updateRobotNodeColor(layer(id), id.name, nodeName, color);
+ }
+ }
+
+ void DebugDrawerTopic::removeRobot(const DebugDrawerTopic::VisuID& id)
+ {
+ if (enabled())
+ {
+ topic->removeRobotVisu(layer(id), id.name);
+ }
+ }
+
+
+ void DebugDrawerTopic::drawTriMesh(
+ const VisuID& id, const VirtualRobot::TriMeshModel& triMesh,
+ const DrawColor& color, bool ignoreLengthScale)
+ {
+ if (!enabled())
+ {
+ return;
+ }
+
+ const float scale = lengthScale(ignoreLengthScale);
+
+ DebugDrawerTriMesh dd;
+ dd.colors.push_back(color);
+
+ for (const auto& vertex : triMesh.vertices)
+ {
+ auto scaled = vertex * scale;
+ dd.vertices.push_back({ scaled.x(), scaled.y(), scaled.z() });
+ }
+
+ const std::size_t normalBase = dd.vertices.size();
+ for (const auto& normal : triMesh.normals)
+ {
+ dd.vertices.push_back({ normal.x(), normal.y(), normal.z() });
+ }
+
+ for (const auto& face : triMesh.faces)
+ {
+ DebugDrawerFace ddf;
+ ddf.vertex1.vertexID = static_cast<Ice::Int>(face.id1);
+ ddf.vertex2.vertexID = static_cast<Ice::Int>(face.id2);
+ ddf.vertex3.vertexID = static_cast<Ice::Int>(face.id3);
+
+ ddf.vertex1.colorID = ddf.vertex2.colorID = ddf.vertex3.colorID = 0;
+ ddf.vertex1.normalID = ddf.vertex2.normalID = ddf.vertex3.normalID = -1;
+
+ bool validNormalIDs = true;
+ for (const auto& id :
+ {
+ face.idNormal1, face.idNormal2, face.idNormal3
+ })
+ {
+ validNormalIDs &= id < triMesh.normals.size();
+ }
+
+ if (validNormalIDs)
+ {
+ ddf.vertex1.normalID = static_cast<Ice::Int>(normalBase + face.idNormal1);
+ ddf.vertex2.normalID = static_cast<Ice::Int>(normalBase + face.idNormal2);
+ ddf.vertex3.normalID = static_cast<Ice::Int>(normalBase + face.idNormal3);
+ }
+ else
+ {
+ const Eigen::Vector3f& normal = face.normal;
+ ddf.normal = { normal.x(), normal.y(), normal.z() };
+ }
+
+ dd.faces.push_back(ddf);
+ }
+
+ topic->setTriMeshVisu(layer(id), id.name, dd);
+ }
+
+ void DebugDrawerTopic::drawTriMeshAsPolygons(const VisuID& id,
+ const VirtualRobot::TriMeshModel& trimesh,
+ const DrawColor& colorFace, float lineWidth, const DrawColor& colorEdge,
+ bool ignoreLengthScale)
+ {
+ drawTriMeshAsPolygons(id, trimesh, Eigen::Matrix4f::Identity(),
+ colorFace, lineWidth, colorEdge, ignoreLengthScale);
+ }
+
+ void DebugDrawerTopic::drawTriMeshAsPolygons(const VisuID& id,
+ const VirtualRobot::TriMeshModel& trimesh, const Eigen::Matrix4f& pose,
+ const DrawColor& colorFace, float lineWidth, const DrawColor& colorEdge,
+ bool ignoreLengthScale)
+ {
+ if (!enabled())
+ {
+ return;
+ }
+
+ const float scale = lengthScale(ignoreLengthScale);
+ bool isIdentity = pose.isIdentity();
+
+ auto toVector3 = [&scale, &isIdentity, &pose](const Eigen::Vector3f & v)
+ {
+ return scaled(scale, isIdentity ? v : math::Helpers::TransformPosition(pose, v));
+ };
+
+ ARMARX_INFO << "Drawing trimesh as polygons";
+
+ int counter = 0;
+ for (std::size_t i = 0; i < trimesh.faces.size(); ++i)
+ {
+ const auto& face = trimesh.faces[i];
+ PolygonPointList points
+ {
+ toVector3(trimesh.vertices.at(face.id1)),
+ toVector3(trimesh.vertices.at(face.id2)),
+ toVector3(trimesh.vertices.at(face.id3))
+ };
+
+ topic->setPolygonVisu(layer(id), id.name + "_" + std::to_string(counter), points,
+ colorFace, colorEdge, lineWidth);
+ ++counter;
+ }
+ }
+
+ void DebugDrawerTopic::drawTriMeshAsPolygons(
+ const VisuID& id,
+ const VirtualRobot::TriMeshModel& trimesh,
+ const std::vector<DrawColor>& faceColorsInner, float lineWidth,
+ const DrawColor& colorEdge, bool ignoreLengthScale)
+ {
+ if (!enabled())
+ {
+ return;
+ }
+
+ //ARMARX_INFO << "Drawing trimesh as polygons colored by area";
+ const float scale = lengthScale(ignoreLengthScale);
+
+ for (std::size_t i = 0; i < trimesh.faces.size(); ++i)
+ {
+ const auto& face = trimesh.faces[i];
+ PolygonPointList points
+ {
+ scaled(scale, trimesh.vertices[face.id1]),
+ scaled(scale, trimesh.vertices[face.id2]),
+ scaled(scale, trimesh.vertices[face.id3])
+ };
+
+ topic->setPolygonVisu(layer(id), id.name + "_" + std::to_string(i), points,
+ faceColorsInner.at(i), colorEdge, lineWidth);
+ }
+ }
+
+ void DebugDrawerTopic::drawPointCloud(
+ const DebugDrawerTopic::VisuID& id,
+ const DebugDrawerPointCloud& pointCloud)
+ {
+ if (enabled())
+ {
+ topic->setPointCloudVisu(id.layer, id.name, pointCloud);
+ }
+ }
+
+ void DebugDrawerTopic::drawPointCloud(
+ const DebugDrawerTopic::VisuID& id,
+ const DebugDrawerColoredPointCloud& pointCloud)
+ {
+ if (enabled())
+ {
+ topic->setColoredPointCloudVisu(id.layer, id.name, pointCloud);
+ }
+ }
+
+ void DebugDrawerTopic::drawPointCloud(
+ const DebugDrawerTopic::VisuID& id,
+ const DebugDrawer24BitColoredPointCloud& pointCloud)
+ {
+ if (enabled())
+ {
+ topic->set24BitColoredPointCloudVisu(id.layer, id.name, pointCloud);
+ }
+ }
+
+ void DebugDrawerTopic::clearColoredPointCloud(const DebugDrawerTopic::VisuID& id)
+ {
+ // Draw an empty point cloud.
+ drawPointCloud(id, DebugDrawerColoredPointCloud{});
+ }
+
+ void DebugDrawerTopic::drawFloor(
+ const VisuID& id,
+ const Eigen::Vector3f& at, const Eigen::Vector3f& up, float size,
+ const DrawColor& color, bool ignoreLengthScale)
+ {
+ if (!enabled())
+ {
+ return;
+ }
+
+ Eigen::Vector3f seed = seed.UnitX();
+ if (std::abs(up.dot(seed)) <= 1e-6f)
+ {
+ seed = seed.UnitY();
+ }
+
+ /* ^ b
+ * | 3---0
+ * | | + |
+ * | 2---1
+ * +--------> a
+ */
+
+ const float halfSize = size / 2;
+
+ const Eigen::Vector3f a = halfSize * up.cross(seed).normalized();
+ const Eigen::Vector3f b = halfSize * up.cross(a).normalized();
+
+ std::vector<Eigen::Vector3f> points;
+ points.push_back(at + a + b);
+ points.push_back(at + a - b);
+ points.push_back(at - a - b);
+ points.push_back(at - a + b);
+
+ drawPolygon(id, points, color, ignoreLengthScale);
+ }
+
+
+ const std::string& DebugDrawerTopic::layer(const std::string& passedLayer) const
+ {
+ return passedLayer.empty() ? _layer : passedLayer;
+ }
+
+ const std::string& DebugDrawerTopic::layer(const VisuID& id) const
+ {
+ return layer(id.layer);
+ }
+
+ float DebugDrawerTopic::lengthScale(bool ignore) const
+ {
+ return ignore ? 1 : _lengthScale;
+ }
+
+ float DebugDrawerTopic::scaled(float scale, float value)
+ {
+ return scale * value;
+ }
+
+ Vector3BasePtr DebugDrawerTopic::scaled(float scale, const Eigen::Vector3f& vector)
+ {
+ if (scale >= 1 && scale <= 1)
+ {
+ return new Vector3(vector);
+ }
+ else
+ {
+ return new Vector3((vector * scale).eval());
+ }
+ }
+
+ PoseBasePtr DebugDrawerTopic::scaled(float scale, const Eigen::Matrix4f& pose)
+ {
+ if (scale >= 1 && scale <= 1)
+ {
+ return new Pose(pose);
+ }
+ else
+ {
+ Eigen::Matrix4f out = pose;
+ math::Helpers::Position(out) *= scale;
+ return new Pose(out);
+ }
+ }
+
+ bool DebugDrawerTopic::enabled() const
+ {
+ return topic;
+ }
+
+
+ DebugDrawerTopic::operator bool() const
+ {
+ return topic;
+ }
+
+ armarx::DebugDrawerTopic::operator DebugDrawerInterfacePrx& ()
+ {
+ return topic;
+ }
+
+ armarx::DebugDrawerTopic::operator const DebugDrawerInterfacePrx& () const
+ {
+ return topic;
+ }
+
+ DebugDrawerInterfacePrx& DebugDrawerTopic::operator->()
+ {
+ return topic;
+ }
+
+ const DebugDrawerInterfacePrx& DebugDrawerTopic::operator->() const
+ {
+ return topic;
+ }
+
+ Eigen::Vector3f DebugDrawerTopic::rgb2hsv(const Eigen::Vector3f& rgb)
+ {
+ // source: https://stackoverflow.com/a/6930407
+
+ Eigen::Vector3f hsv;
+ float min, max, delta;
+
+ min = rgb(0) < rgb(1) ? rgb(0) : rgb(1);
+ min = min < rgb(2) ? min : rgb(2);
+
+ max = rgb(0) > rgb(1) ? rgb(0) : rgb(1);
+ max = max > rgb(2) ? max : rgb(2);
+
+ hsv(2) = max; // v
+ delta = max - min;
+ if (delta < 1e-5f)
+ {
+ hsv(1) = 0;
+ hsv(0) = 0; // undefined, maybe nan?
+ return hsv;
+ }
+ if (max > 0.0f)
+ {
+ // NOTE: if Max is == 0, this divide would cause a crash
+ hsv(1) = (delta / max); // s
+ }
+ else
+ {
+ // if max is 0, then r = g = b = 0
+ // s = 0, h is undefined
+ hsv(1) = 0.0;
+ hsv(0) = std::nanf(""); // its now undefined
+ return hsv;
+ }
+ if (rgb(0) >= max) // > is bogus, just keeps compilor happy
+ {
+ hsv(0) = (rgb(1) - rgb(2)) / delta; // between yellow & magenta
+ }
+ else
+ {
+ if (rgb(1) >= max)
+ {
+ hsv(0) = 2.f + (rgb(2) - rgb(0)) / delta; // between cyan & yellow
+ }
+ else
+ {
+ hsv(0) = 4.f + (rgb(0) - rgb(1)) / delta; // between magenta & cyan
+ }
+ }
+
+ hsv(0) *= 60.0f; // degrees
+
+ if (hsv(0) < 0.f)
+ {
+ hsv(0) += 360.0f;
+ }
+
+ return hsv;
+ }
+
+ Eigen::Vector3f DebugDrawerTopic::hsv2rgb(const Eigen::Vector3f& hsv)
+ {
+ // source: https://stackoverflow.com/a/6930407
+
+ float hh, p, q, t, ff;
+ long i;
+ Eigen::Vector3f rgb;
+
+ if (hsv(1) <= 0.f) // < is bogus, just shuts up warnings
+ {
+ rgb(0) = hsv(2);
+ rgb(1) = hsv(2);
+ rgb(2) = hsv(2);
+ return rgb;
+ }
+ hh = hsv(0);
+ if (hh >= 360.f)
+ {
+ hh = 0.0;
+ }
+ hh /= 60.0f;
+ i = static_cast<long>(hh);
+ ff = hh - i;
+ p = hsv(2) * (1.f - hsv(1));
+ q = hsv(2) * (1.f - (hsv(1) * ff));
+ t = hsv(2) * (1.f - (hsv(1) * (1.f - ff)));
+
+ switch (i)
+ {
+ case 0:
+ rgb(0) = hsv(2);
+ rgb(1) = t;
+ rgb(2) = p;
+ break;
+ case 1:
+ rgb(0) = q;
+ rgb(1) = hsv(2);
+ rgb(2) = p;
+ break;
+ case 2:
+ rgb(0) = p;
+ rgb(1) = hsv(2);
+ rgb(2) = t;
+ break;
+
+ case 3:
+ rgb(0) = p;
+ rgb(1) = q;
+ rgb(2) = hsv(2);
+ break;
+ case 4:
+ rgb(0) = t;
+ rgb(1) = p;
+ rgb(2) = hsv(2);
+ break;
+ case 5:
+ default:
+ rgb(0) = hsv(2);
+ rgb(1) = p;
+ rgb(2) = q;
+ break;
+ }
+ return rgb;
+ }
+
+
+ DrawColor DebugDrawerTopic::getGlasbeyLUTColor(int id, float alpha)
+ {
+ return GlasbeyLUT::at(id, alpha);
+ }
+
+ DrawColor DebugDrawerTopic::getGlasbeyLUTColor(uint32_t id, float alpha)
+ {
+ return GlasbeyLUT::at(id, alpha);
+ }
+
+ DrawColor DebugDrawerTopic::getGlasbeyLUTColor(std::size_t id, float alpha)
+ {
+ return GlasbeyLUT::at(id, alpha);
+ }
+
+
+}
diff --git a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h
new file mode 100644
index 0000000000000000000000000000000000000000..b95de6d43258d5a34246a639f6735ea4ee1ee1b4
--- /dev/null
+++ b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h
@@ -0,0 +1,842 @@
+#pragma once
+
+#include <chrono>
+#include <functional>
+#include <thread>
+
+#include <Eigen/Geometry>
+
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+
+
+namespace VirtualRobot
+{
+ class TriMeshModel;
+ class BoundingBox;
+}
+
+
+namespace armarx
+{
+ // forward declaration
+ class ManagedIceObject;
+
+
+ /**
+ * @brief The `DebugDrawerTopic` wraps a `DebugDrawerInterfacePrx` and
+ * provides a more useful interface than the Ice interface.
+ *
+ * The methods by `DebugDrawerTopic` take "raw" types, such as Eigen or PCL
+ * types, and take care of conversion to Ice variants or data structures.
+ * In addition, this class provides useful overloads for different use cases.
+ *
+ * All methods check whether the internal topic proxy is set, and do
+ * nothing if it is not set. To disable visualization by this classs
+ * completely, just do not set the topic. To check whether visualization
+ * is enabled (i.e. a topic proxy is set), use `enabled()` or just convert
+ * `*this` to bool:
+ * @code
+ * DebugDrawerTopic debugDrawer;
+ * if (debugDrawer) // equivalent: if (debugDrawer.enabled())
+ * {
+ * // do stuff if visualization is enabled
+ * }
+ * @endcode
+ *
+ * The `DebugDrawerTopic` allows to set a layer on constructor or via
+ * `setLayer()`. This layer will be used if none is passed to a drawing
+ * method. If no layer is passed or set, `DebugDrawerTopic::DEFAULT_LAYER`
+ * is used.
+ *
+ *
+ * @par Initialisation by Offering and Getting Topic
+ *
+ * A `DebugDrawerTopic` needs an underlying `DebugDrawerInterfacePrx` topic proxy.
+ * This proxy can be passed on construction or set via `setTopic()`.
+ * In a component (or any other `ManagedIceObject`), `DebugDrawerTopic`
+ * provides convenience functions to register and fetch the topics.
+ *
+ * In `onInitComponent()` (or equivalent method), call:
+ * @code
+ * debugDrawer.offeringTopic(*this);
+ * @endcode
+ * In `onConnectComponent()` (or equivalent), call:
+ * @code
+ * debugDrawer.getTopic(*this);
+ * @endcode
+ * where `*this` is a `ManagedIceObject`.
+ *
+ * This will call `this->offeringTopic("...")` and `this->getTopic("...")`
+ * with the correct topic name (`DebugDrawerTopic::TOPIC_NAME`) and
+ * enable the `DebugDrawerTopic`.
+ *
+ *
+ * @par Scaling
+ *
+ * `DebugDrawerTopic` supports length scaling and pose scaling.
+ *
+ * If a length scale is set, all visualizations will be scaled up or down
+ * by this value. This scaling affects positions, sizes / extents, and
+ * distances / lengths. This is useful when drawing quantities of
+ * different sources using different scalings (such as meters vs
+ * millimeters).
+ * Length scale can be set via `setPoseScale()` or the short hands
+ * `setPoseScaleMetersToMillimeters()` and `setPoseScaleMillimetersToMeters()`.
+ *
+ * All applicable methods offer a final argument called
+ * `ignoreLengthScaling` (false by default, expect for robots, see below),
+ * which can be set to true to ignore the set length scale for this
+ * method call.
+ *
+ * @note Robots are always drawn in their native size (the
+ * `DebugDrawerInterface` offers no size scaling for robots).
+ * (That is, robots are drawn in millimeters most of the time.)
+ *
+ *
+ * In addition, this class allows to set a pose scale, which will be used
+ * for all drawn poses (if no other value is passed to drawPose()).
+ * This is useful when working with geometries scaled in meters, in which
+ * case a pose scale of 0.001 can be used.
+ *
+ *
+ * @par Argument Pattern
+ *
+ * All drawing methods take a `VisuID` as first argument, which specifies
+ * the name of the visualization and the layer to draw on.
+ * There are different ways to specify the first argument.
+ * For example, to specify a only the name for a pose, pass just the name:
+ *
+ * @code
+ * Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
+ * std::string name = "pose";
+ * debugDrawer.drawPose(name, pose);
+ * debugDrawer.drawPose({name}, pose); // equivalent to the line above
+ * @endcode
+ *
+ * This will draw a pose on the preset layer (i.e. the layer passed to the
+ * constructor or set via `setLayer()`, or "debug" by default).
+ * To specify both name and layer of a single visualization, pass both in
+ * an initializer list:
+ *
+ * @code
+ * Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
+ * std::string layer = "layer";
+ * std::string name = "pose";
+ * debugDrawer.drawPose({name, layer}, pose);
+ * @endcode
+ *
+ *
+ * After the VisuID, usually the essential geometric parameters follow,
+ * (e.g. position, size, length, point list, ...), depending on the type
+ * of visualization.
+ * Finally, decorative parameters like colors and width can be passed.
+ * Most of the time, they have sensible default values and can be omitted
+ * for quick-and-dirty drawing.
+ *
+ * (Added methods should adhere to this pattern.)
+ *
+ * @see `DebugDrawerTopic::VisuID`
+ */
+ class DebugDrawerTopic
+ {
+ public:
+
+ struct VisuID
+ {
+ /// Empty constructor.
+ VisuID();
+
+ /**
+ * @brief Construct a VisuID.
+ *
+ * This constructor can be called in the following ways
+ * (with `draw(const VisuID& id, ...)` being any drawing method):
+ *
+ * @code
+ * std::string name = "pose";
+ * std::string layer = "layer";
+ * draw(name, ...); // just the name, implicit call
+ * draw({name}, ...); // just the name, call with initializer list
+ * draw({name, layer}, ...); // name and layer, with initializer list
+ * @endcode
+ *
+ * (And of course by an explicit call if you want to be really verbose.)
+ * Not passing a layer will cause DebugDrawerTopic to use the
+ * preset layer.
+ *
+ * @param name the name
+ * @param layer the layer name
+ */
+ VisuID(const std::string& name, const std::string& layer = "");
+
+ /// Construct a VisuID from a non-std::string source (e.g. char[]).
+ template <typename Source>
+ VisuID(const Source& name) : VisuID(std::string(name))
+ {}
+
+ std::string name = ""; ///< The visu name (empty by default).
+ std::string layer = ""; ///< The layer name (empty by default).
+
+ friend std::ostream& operator<<(std::ostream& os, const VisuID& rhs);
+ };
+
+ struct Defaults
+ {
+ DrawColor colorText { 0, 0, 0, 1 };
+
+ DrawColor colorArrow { 1, .5, 0, 1 };
+ DrawColor colorBox { 1, 0, 0, 1 };
+ DrawColor colorCylinder { 0, 1, 0, 1 };
+ DrawColor colorLine { .5, 0, 0, 1 };
+ DrawColor colorSphere { 0, 0, 1, 1 };
+
+ DrawColor colorPolygonFace { 0, 1, 1, 1 };
+ DrawColor colorPolygonEdge { .75, .75, .75, 1 };
+
+ DrawColor colorFloor { .1f, .1f, .1f, 1 };
+
+ DrawColor colorPointCloud { .5, .5, .5, 1. };
+
+ // Default value of DebugDrawerColoredPointCloud etc.
+ float pointCloudPointSize = 3.0f;
+ };
+ static const Defaults DEFAULTS;
+
+
+ public:
+
+ // CONSTRUCTION & SETUP
+
+ /// Construct without topic, and optional layer.
+ DebugDrawerTopic(const std::string& layer = DEFAULT_LAYER);
+ /// Construct with given topic and optional layer.
+ DebugDrawerTopic(const DebugDrawerInterfacePrx& topic, const std::string& layer = DEFAULT_LAYER);
+
+
+ /// Set the topic.
+ void setTopic(const DebugDrawerInterfacePrx& topic);
+ /// Get the topic.
+ DebugDrawerInterfacePrx getTopic() const;
+
+ /**
+ * @brief Call offeringTopic([topicName]) on the given component.
+ * @param component The component (`*this` when called from a component).
+ * @param topicNameOverride Optional override for the topic name. If left empty (default),
+ * uses the standard topic name (see `TOPIC_NAME`).
+ */
+ void offeringTopic(ManagedIceObject& component, const std::string& topicNameOverride = "") const;
+ /**
+ * @brief Get the topic by calling getTopic([topicName]) on the given component.
+ * @param component The component (`*this` when called from a component).
+ * @param topicNameOverride Optional override for the topic name. If left empty (default),
+ * uses the standard topic name (see `TOPIC_NAME`).
+ */
+ void getTopic(ManagedIceObject& component, const std::string& topicNameOverride = "");
+
+ /// Get the default layer (used if no layer is passed to a method).
+ const std::string& getLayer() const;
+ /// Set the default layer (used if no layer is passed to a method).
+ void setLayer(const std::string& layer);
+
+ /// Get the scaling for positions, lengths and distances.
+ float getLengthScale() const;
+ /// Set the scale for positions, lengths and distances.
+ void setLengthScale(float scale);
+ /// Set the scale for positions, lengths and distances to 1000.
+ void setLengthScaleMetersToMillimeters();
+ /// Set the scale for positions, lengths and distances to 0.001.
+ void setLengthScaleMillimetersToMeters();
+
+ /// Get the scale for pose visualization.
+ float getPoseScale() const;
+ /// Set the scale for pose visualization.
+ /// This value will be used for all successive calls to drawPose().
+ void setPoseScale(float scale);
+ /// Set the pose scale to 0.001 (good when drawing in meters).
+ void setPoseScaleMeters();
+ /// Set the pose scale to 1 (good when drawing in millimeters).
+ void setPoseScaleMillimeters();
+
+
+
+ // SLEEP
+
+ /// Sleep for the `shortSleepDuration`. Useful after clearing.
+ void shortSleep();
+
+ /// If enabled, sleep for the given duration (e.g. a chrono duration).
+ template <typename DurationT>
+ void sleepFor(const DurationT& duration);
+
+ /// Set the duration for "short sleeps".
+ template <typename DurationT>
+ void setShortSleepDuration(const DurationT& duration);
+
+
+ // CLEAR
+
+ /// Clear all layers.
+ /// @param sleep If true, do a short sleep clearing.
+ void clearAll(bool sleep = false);
+
+ /// Clear the (set default) layer.
+ /// @param sleep If true, do a short sleep clearing.
+ void clearLayer(bool sleep = false);
+
+ /// Clear the given layer.
+ /// @param sleep If true, do a short sleep clearing.
+ void clearLayer(const std::string& layer, bool sleep = false);
+
+
+ // PRIMITIVES
+
+ /**
+ * @brief Draw text at the specified position.
+ * @param size the text size (in pixels, not affected by length scaling)
+ */
+ void drawText(const VisuID& id, const Eigen::Vector3f& position, const std::string& text,
+ int size = 10, const DrawColor color = DEFAULTS.colorText,
+ bool ignoreLengthScale = false);
+
+
+ /// Draw a box.
+ void drawBox(const VisuID& id, const Eigen::Vector3f& position, const Eigen::Quaternionf& orientation,
+ const Eigen::Vector3f& extents, const DrawColor& color = DEFAULTS.colorBox,
+ bool ignoreLengthScale = false);
+ /// Draw a box.
+ void drawBox(const VisuID& id, const Eigen::Matrix4f& pose, const Eigen::Vector3f& extents,
+ const DrawColor& color = DEFAULTS.colorBox,
+ bool ignoreLengthScale = false);
+
+ /// Draw an axis aligned bounding box.
+ void drawBox(const VisuID& id,
+ const VirtualRobot::BoundingBox& boundingBox,
+ const DrawColor& color = DEFAULTS.colorBox,
+ bool ignoreLengthScale = false);
+
+ /// Draw a locally axis aligned bounding box, transformed by the given pose.
+ void drawBox(const VisuID& id,
+ const VirtualRobot::BoundingBox& boundingBox, const Eigen::Matrix4f& pose,
+ const DrawColor& color = DEFAULTS.colorBox,
+ bool ignoreLengthScale = false);
+
+
+ /**
+ * @brief Draw a cylinder with center and direction.
+ * @param length the full length (not half-length)
+ */
+ void drawCylinder(
+ const VisuID& id,
+ const Eigen::Vector3f& center, const Eigen::Vector3f& direction, float radius, float length,
+ const DrawColor& color = DEFAULTS.colorCylinder,
+ bool ignoreLengthScale = false);
+
+ /**
+ * @brief Draw a cylinder with center and orientation.
+ * An identity orientation represents a cylinder with an axis aligned to the Y-axis.
+ * @param length the full length (not half-length)
+ */
+ void drawCylinder(
+ const VisuID& id,
+ const Eigen::Vector3f& center, const Eigen::Quaternionf& orientation, float radius, float length,
+ const DrawColor& color = DEFAULTS.colorCylinder,
+ bool ignoreLengthScale = false);
+
+ /// Draw a cylinder from start to end.
+ void drawCylinderFromTo(
+ const VisuID& id,
+ const Eigen::Vector3f& from, const Eigen::Vector3f& to, float radius,
+ const DrawColor& color = DEFAULTS.colorCylinder,
+ bool ignoreLengthScale = false);
+
+
+ /// Draw a sphere.
+ void drawSphere(
+ const VisuID& id,
+ const Eigen::Vector3f& center, float radius,
+ const DrawColor& color = DEFAULTS.colorSphere,
+ bool ignoreLengthScale = false);
+
+
+ /// Draw an arrow with position (start) and direction.
+ void drawArrow(
+ const VisuID& id,
+ const Eigen::Vector3f& position, const Eigen::Vector3f& direction, float length,
+ float width, const DrawColor& color = DEFAULTS.colorArrow,
+ bool ignoreLengthScale = false);
+
+ /// Draw an arrow with start and end.
+ void drawArrowFromTo(
+ const VisuID& id,
+ const Eigen::Vector3f& from, const Eigen::Vector3f& to,
+ float width, const DrawColor& color = DEFAULTS.colorArrow,
+ bool ignoreLengthScale = false);
+
+ /// Remove an arrow.
+ void removeArrow(const VisuID& id);
+
+
+ /// Draw a polygon.
+ void drawPolygon(
+ const VisuID& id,
+ const std::vector<Eigen::Vector3f>& points,
+ const DrawColor& colorFace,
+ float lineWidth = 0, const DrawColor& colorEdge = DEFAULTS.colorPolygonEdge,
+ bool ignoreLengthScale = false);
+
+
+ /// Draw a line from start to end.
+ void drawLine(
+ const VisuID& id,
+ const Eigen::Vector3f& from, const Eigen::Vector3f& to,
+ float width, const DrawColor& color = DEFAULTS.colorLine,
+ bool ignoreLengthScale = false);
+
+
+ /// Draw a line set.
+ void drawLineSet(
+ const VisuID& id,
+ const DebugDrawerLineSet& lineSet,
+ bool ignoreLengthScale = false);
+
+ /**
+ * @brief Draw a set of lines with constant color.
+ * @param points List of start and end points [ s1, e1, s2, e2, ..., sn, en ].
+ */
+ void drawLineSet(
+ const VisuID& id,
+ const std::vector<Eigen::Vector3f>& points,
+ float width, const DrawColor& color = DEFAULTS.colorLine,
+ bool ignoreLengthScale = false);
+
+ /**
+ * @brief Draw a set of lines with constant color.
+ * @param points List of start and end points [ s1, e1, s2, e2, ..., sn, en ].
+ * @param colors List of line colors [ c1, c2, ..., cn ].
+ */
+ void drawLineSet(
+ const VisuID& id,
+ const std::vector<Eigen::Vector3f>& points,
+ float width, const DrawColor& colorA, const DrawColor& colorB,
+ const std::vector<float>& intensitiesB,
+ bool ignoreLengthScale = false);
+
+
+ // POSE
+
+ /// Draw a pose (with the preset scale).
+ void drawPose(const VisuID& id, const Eigen::Matrix4f& pose,
+ bool ignoreLengthScale = false);
+ /// Draw a pose (with the preset scale).
+ void drawPose(const VisuID& id, const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
+ bool ignoreLengthScale = false);
+
+ /// Draw a pose with the given scale.
+ void drawPose(const VisuID& id, const Eigen::Matrix4f& pose, float scale,
+ bool ignoreLengthScale = false);
+ /// Draw a pose with the given scale.
+ void drawPose(const VisuID& id, const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
+ float scale,
+ bool ignoreLengthScale = false);
+
+ /// Remove a pose visualization.
+ void removePose(const VisuID& id);
+
+
+ // ROBOT
+
+ /**
+ * @brief Draw a robot.
+ * Afterwards, `updateRobot*()` methods can be used with the same VisuID.
+ *
+ * @param robotFile The robot file. Either an absolute path, or a path
+ * relative to the data directory of the ArmarX project specified by `armarxProject`.
+ * @param armarxProject The name of the ArmarX project keeping the robot model.
+ * @param drawStyle The draw style (full or collision model).
+ */
+ void drawRobot(
+ const VisuID& id,
+ const std::string& robotFile, const std::string& armarxProject,
+ armarx::DrawStyle drawStyle = armarx::DrawStyle::FullModel);
+
+ /// Update / set the robot pose.
+ void updateRobotPose(
+ const VisuID& id,
+ const Eigen::Matrix4f& pose,
+ bool ignoreScale = false);
+
+ /// Update / set the robot pose.
+ void updateRobotPose(
+ const VisuID& id,
+ const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
+ bool ignoreScale = false);
+
+ /// Update / set the robot configuration (joint angles).
+ void updateRobotConfig(
+ const VisuID& id,
+ const std::map<std::string, float>& config);
+
+ /// Update / set the robot color.
+ void updateRobotColor(const VisuID& id, const DrawColor& color);
+ /// Update / set the color of a robot node.
+ void updateRobotNodeColor(
+ const VisuID& id, const std::string& nodeName, const DrawColor& color);
+
+ /// Remove a robot visualization.
+ void removeRobot(const VisuID& id);
+
+
+ // TRI MESH
+
+ /// Draw a TriMeshModel as DebugDrawerTriMesh.
+ void drawTriMesh(
+ const VisuID& id,
+ const VirtualRobot::TriMeshModel& triMesh,
+ const DrawColor& color = {.5, .5, .5, 1},
+ bool ignoreLengthScale = false);
+
+ /// Draw a TriMeshModel as individual polygons.
+ void drawTriMeshAsPolygons(
+ const VisuID& id,
+ const VirtualRobot::TriMeshModel& trimesh,
+ const DrawColor& colorFace = DEFAULTS.colorPolygonFace,
+ float lineWidth = 0, const DrawColor& colorEdge = DEFAULTS.colorPolygonEdge,
+ bool ignoreLengthScale = false);
+
+ /// Draw a TriMeshModel as individual polygons, transformed by the given pose.
+ void drawTriMeshAsPolygons(
+ const VisuID& id,
+ const VirtualRobot::TriMeshModel& trimesh, const Eigen::Matrix4f& pose,
+ const DrawColor& colorFace = DEFAULTS.colorPolygonFace,
+ float lineWidth = 0, const DrawColor& colorEdge = DEFAULTS.colorPolygonEdge,
+ bool ignoreLengthScale = false);
+
+ /// Draw a TriMeshModel as individual polygons with individual face colors.
+ void drawTriMeshAsPolygons(
+ const VisuID& id,
+ const VirtualRobot::TriMeshModel& trimesh,
+ const std::vector<DrawColor>& faceColorsInner,
+ float lineWidth = 0, const DrawColor& colorEdge = DEFAULTS.colorPolygonEdge,
+ bool ignoreLengthScale = false);
+
+
+ // POINT CLOUD
+ /* (By templating these functions, we can make them usable for PCL
+ * point clouds without a dependency on PCL.)
+ */
+
+ /**
+ * @brief Draw a unicolored point cloud.
+ *
+ * `pointCloud` must be iterable and its elements must provide members `x, y, z`.
+ */
+ template <class PointCloudT>
+ void drawPointCloud(
+ const VisuID& id,
+ const PointCloudT& pointCloud,
+ const DrawColor& color = DEFAULTS.colorPointCloud,
+ float pointSize = DEFAULTS.pointCloudPointSize,
+ bool ignoreLengthScale = false);
+
+ /**
+ * @brief Draw a colored point cloud with RGBA information.
+ *
+ * `pointCloud` must be iterable and its elements must provide
+ * members `x, y, z, r, g, b, a`.
+ */
+ template <class PointCloudT>
+ void drawPointCloudRGBA(
+ const VisuID& id,
+ const PointCloudT& pointCloud,
+ float pointSize = DEFAULTS.pointCloudPointSize,
+ bool ignoreLengthScale = false);
+
+ /**
+ * @brief Draw a colored point cloud with colors according to labels.
+ *
+ * `pointCloud` must be iterable and its elements must provide
+ * members `x, y, z, label`.
+ */
+ template <class PointCloudT>
+ void drawPointCloudLabeled(
+ const VisuID& id,
+ const PointCloudT& pointCloud,
+ float pointSize = DEFAULTS.pointCloudPointSize,
+ bool ignoreLengthScale = false);
+
+ /**
+ * @brief Draw a colored point cloud with custom colors.
+ *
+ * `pointCloud` must be iterable and its elements must provide
+ * members `x, y, z`.
+ * The color of a point is specified by `colorFunc`, which must be
+ * a callable taking an element of `pointCloud` and returning its
+ * color as `armarx::DrawColor`.
+ */
+ template <class PointCloudT, class ColorFuncT>
+ void drawPointCloud(
+ const VisuID& id,
+ const PointCloudT& pointCloud,
+ const ColorFuncT& colorFunc,
+ float pointSize = DEFAULTS.pointCloudPointSize,
+ bool ignoreLengthScale = false);
+
+
+ // Debug Drawer Point Cloud Types
+
+ /// Draw a non-colored point cloud.
+ void drawPointCloud(const VisuID& id, const DebugDrawerPointCloud& pointCloud);
+
+ /// Draw a colored point cloud.
+ void drawPointCloud(const VisuID& id, const DebugDrawerColoredPointCloud& pointCloud);
+
+ /// Draw a 24 bit colored point cloud.
+ void drawPointCloud(const VisuID& id, const DebugDrawer24BitColoredPointCloud& pointCloud);
+
+
+ /// Forces the "deletion" of a point cloud by drawing an empty one.
+ void clearColoredPointCloud(const VisuID& id);
+
+
+ // CUSTOM
+
+ /**
+ * @brief Draw a quad representing the floor.
+ * @param at the quad'S center
+ * @param up the up direction (normal of the floor plane)
+ * @param size the quad's edge length
+ */
+ void drawFloor(
+ const VisuID& id = { "floor" },
+ const Eigen::Vector3f& at = Eigen::Vector3f::Zero(),
+ const Eigen::Vector3f& up = Eigen::Vector3f::UnitZ(),
+ float size = 5, const DrawColor& color = DEFAULTS.colorFloor,
+ bool ignoreLengthScale = false);
+
+
+ // STATUS
+
+ /// Indicate whether a topic is set, i.e. visualization is enabled.
+ bool enabled() const;
+ /// Indicate whether a topic is set, i.e. visualization is enabled.
+ operator bool() const;
+
+
+ // OPERATORS
+
+ /// Conversion operator to DebugDrawerInterfacePrx.
+ operator DebugDrawerInterfacePrx& ();
+ operator const DebugDrawerInterfacePrx& () const;
+
+ /// Pointer member access operator to access the raw debug drawer proxy.
+ DebugDrawerInterfacePrx& operator->();
+ const DebugDrawerInterfacePrx& operator->() const;
+
+
+ public: // STATIC
+
+ /**
+ * @brief Convert a RGB color to HSV.
+ * @param rgb RGB color as [r, g, b] with r, g, b in [0, 1].
+ * @return HSV color as [h, s, v] with h in [0 deg, 360 deg] and s, v in [0, 1].
+ */
+ static Eigen::Vector3f rgb2hsv(const Eigen::Vector3f& rgb);
+
+ /**
+ * @brief Convert a HSV color RGB.
+ * @param HSV color as [h, s, v] with h in [0 deg, 360 deg] and s, v in [0, 1].
+ * @return RGB color as [r, g, b] with r, g, b in [0, 1].
+ */
+ static Eigen::Vector3f hsv2rgb(const Eigen::Vector3f& hsv);
+
+
+ /**
+ * @brief Construct a DrawColor from the given color type.
+ *
+ * The used color type must have members named `r`, `g` and `b`.
+ * Applicable types include:
+ * - pcl::RGB (byteToFloat = true)
+ * - armarx::DrawColor (useful to get a color with a different alpha)
+ *
+ * @param alpha the alpha (default: 1)
+ * @param byteToFloat If true, scale from range [0, 255] to [0, 1]
+ */
+ template <class ColorT>
+ static DrawColor toDrawColor(const ColorT& color, float alpha = 1, bool byteToFloat = false);
+
+
+ /// Get a color from the Glasbey look-up-table.
+ static DrawColor getGlasbeyLUTColor(int id, float alpha = 1.f);
+ static DrawColor getGlasbeyLUTColor(uint32_t id, float alpha = 1.f);
+ static DrawColor getGlasbeyLUTColor(std::size_t id, float alpha = 1.f);
+
+
+ private:
+
+ /// Get the layer to draw on. (passedLayer if not empty, _layer otherwise).
+ const std::string& layer(const std::string& passedLayer) const;
+
+ /// Get the layer to draw on. (id.layer if not empty, _layer otherwise).
+ const std::string& layer(const VisuID& id) const;
+
+
+ /// Return _lengthScale if ignore is false (default), 1 otherwise.
+ float lengthScale(bool ignore = false) const;
+
+ /// Scale the value.
+ static float scaled(float scale, float value);
+ /// Scale the given vector and return it as Vector3 pointer.
+ static Vector3BasePtr scaled(float scale, const Eigen::Vector3f& vector);
+ /// Scale the translation of the given pose and return it as Pose pointer.
+ static PoseBasePtr scaled(float scale, const Eigen::Matrix4f& pose);
+
+ /// Scale the given vector and return it as `ScaledT`.
+ /// The constructor of `ScaledT` must take the x, y and z coordinates.
+ template <class ScaledT>
+ static ScaledT scaledT(float scale, const Eigen::Vector3f& vector);
+
+ /// Scale a value with .x, .y and .z attributes in-place.
+ template <class XYZT>
+ static void scaleXYZ(float scale, XYZT& xyz);
+
+
+ private:
+
+ /// The name of the debug drawer topic.
+ static const std::string TOPIC_NAME;
+ /// The default layer ("debug").
+ static const std::string DEFAULT_LAYER;
+
+
+ /// The debug drawer topic.
+ DebugDrawerInterfacePrx topic = nullptr;
+
+ /// The default layer (used if none is passed to the method).
+ std::string _layer = DEFAULT_LAYER;
+
+ /// Scaling for positions, lengths and distances.
+ float _lengthScale = 1;
+
+ /// Scaling for pose visualization (1 is good when drawing in millimeters).
+ float _poseScale = 1;
+
+ /// The duration for shortSleep().
+ std::chrono::milliseconds _shortSleepDuration { 100 };
+
+ };
+
+
+ template <typename DurationT>
+ void DebugDrawerTopic::sleepFor(const DurationT& duration)
+ {
+ if (enabled())
+ {
+ std::this_thread::sleep_for(duration);
+ }
+ }
+
+ template <typename DurationT>
+ void DebugDrawerTopic::setShortSleepDuration(const DurationT& duration)
+ {
+ this->_shortSleepDuration = std::chrono::duration_cast<std::chrono::milliseconds>(duration);
+ }
+
+ template <class ColorT>
+ DrawColor DebugDrawerTopic::toDrawColor(const ColorT& color, float alpha, bool byteToFloat)
+ {
+ const float scale = byteToFloat ? (1 / 255.f) : 1;
+ return { color.r * scale, color.g * scale, color.b * scale, alpha };
+ }
+
+
+ template <class PointCloudT>
+ void DebugDrawerTopic::drawPointCloud(
+ const VisuID& id,
+ const PointCloudT& pointCloud,
+ const DrawColor& color,
+ float pointSize,
+ bool ignoreLengthScale)
+ {
+ drawPointCloud(id, pointCloud, [&color](const auto&)
+ {
+ return color;
+ },
+ pointSize, ignoreLengthScale);
+ }
+
+ template<class PointCloudT>
+ void DebugDrawerTopic::drawPointCloudRGBA(
+ const VisuID& id,
+ const PointCloudT& pointCloud,
+ float pointSize,
+ bool ignoreLengthScale)
+ {
+ drawPointCloud(id, pointCloud, [](const auto & p)
+ {
+ return toDrawColor(p, p.a);
+ },
+ pointSize, ignoreLengthScale);
+ }
+
+ template <class PointCloudT>
+ void DebugDrawerTopic::drawPointCloudLabeled(
+ const VisuID& id,
+ const PointCloudT& pointCloud,
+ float pointSize,
+ bool ignoreLengthScale)
+ {
+ drawPointCloud(id, pointCloud, [](const auto & p)
+ {
+ return getGlasbeyLUTColor(p.label);
+ },
+ pointSize, ignoreLengthScale);
+ }
+
+ template <class PointCloudT, class ColorFuncT>
+ void DebugDrawerTopic::drawPointCloud(
+ const VisuID& id,
+ const PointCloudT& pointCloud,
+ const ColorFuncT& colorFn,
+ float pointSize,
+ bool ignoreLengthScale)
+ {
+ if (!enabled())
+ {
+ return;
+ }
+
+ const float lf = ignoreLengthScale ? 1.0 : _lengthScale;
+
+ DebugDrawerColoredPointCloud dd;
+ dd.points.reserve(pointCloud.size());
+
+ dd.pointSize = pointSize;
+
+ for (const auto& p : pointCloud)
+ {
+ dd.points.push_back(DebugDrawerColoredPointCloudElement
+ {
+ lf * p.x, lf * p.y, lf * p.z, colorFn(p)
+ });
+ }
+
+ drawPointCloud(id, dd);
+ }
+
+
+ template <class ScaledT>
+ ScaledT DebugDrawerTopic::scaledT(float scale, const Eigen::Vector3f& vector)
+ {
+ auto scaled = vector * scale;
+ return { scaled.x(), scaled.y(), scaled.z() };
+ }
+
+ template <class XYZT>
+ void DebugDrawerTopic::scaleXYZ(float scale, XYZT& xyz)
+ {
+ xyz.x *= scale;
+ xyz.y *= scale;
+ xyz.z *= scale;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/core/visualization/GlasbeyLUT.cpp b/source/RobotAPI/libraries/core/visualization/GlasbeyLUT.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..59d11b5ddb95fb399e361254d74cf8a3694d8135
--- /dev/null
+++ b/source/RobotAPI/libraries/core/visualization/GlasbeyLUT.cpp
@@ -0,0 +1,579 @@
+#include "GlasbeyLUT.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+// taken from:
+// https://github.com/PointCloudLibrary/pcl/blob/master/common/src/colors.cpp
+
+
+namespace armarx
+{
+
+ /// Glasbey lookup table
+ static const std::vector<unsigned char> GLASBEY_LUT =
+ {
+ 77, 175, 74,
+ 228, 26, 28,
+ 55, 126, 184,
+ 152, 78, 163,
+ 255, 127, 0,
+ 255, 255, 51,
+ 166, 86, 40,
+ 247, 129, 191,
+ 153, 153, 153,
+ 0, 0, 255,
+ 255, 0, 255,
+ 0, 255, 248,
+ 0, 255, 0,
+ 0, 101, 255,
+ 255, 255, 180,
+ 52, 68, 1,
+ 0, 0, 68,
+ 96, 0, 41,
+ 158, 147, 0,
+ 116, 0, 185,
+ 255, 0, 114,
+ 0, 149, 125,
+ 209, 186, 255,
+ 255, 183, 156,
+ 240, 0, 174,
+ 208, 255, 245,
+ 0, 255, 176,
+ 170, 255, 93,
+ 0, 207, 255,
+ 255, 190, 1,
+ 241, 117, 255,
+ 52, 74, 167,
+ 150, 166, 103,
+ 255, 114, 114,
+ 171, 100, 109,
+ 161, 0, 41,
+ 160, 135, 255,
+ 105, 86, 121,
+ 178, 21, 105,
+ 0, 3, 123,
+ 255, 221, 236,
+ 160, 238, 173,
+ 237, 161, 77,
+ 0, 141, 255,
+ 0, 97, 109,
+ 1, 238, 98,
+ 81, 0, 78,
+ 128, 103, 66,
+ 0, 108, 44,
+ 209, 224, 94,
+ 155, 0, 255,
+ 0, 45, 223,
+ 88, 28, 0,
+ 166, 2, 162,
+ 165, 205, 239,
+ 84, 121, 0,
+ 76, 109, 80,
+ 122, 180, 0,
+ 104, 204, 204,
+ 145, 95, 255,
+ 214, 208, 177,
+ 185, 130, 176,
+ 130, 120, 194,
+ 128, 96, 0,
+ 247, 161, 255,
+ 10, 65, 119,
+ 232, 102, 54,
+ 7, 191, 131,
+ 105, 54, 171,
+ 10, 177, 0,
+ 215, 191, 105,
+ 198, 66, 249,
+ 140, 255, 145,
+ 135, 60, 105,
+ 254, 170, 191,
+ 130, 173, 255,
+ 161, 35, 0,
+ 197, 255, 0,
+ 40, 153, 180,
+ 215, 83, 185,
+ 255, 77, 161,
+ 128, 175, 147,
+ 216, 91, 124,
+ 193, 144, 91,
+ 210, 196, 0,
+ 232, 39, 73,
+ 76, 52, 116,
+ 159, 206, 110,
+ 138, 147, 187,
+ 140, 5, 114,
+ 0, 56, 183,
+ 191, 105, 0,
+ 83, 58, 0,
+ 94, 224, 0,
+ 121, 99, 99,
+ 212, 123, 104,
+ 89, 160, 99,
+ 146, 58, 54,
+ 231, 46, 215,
+ 93, 245, 200,
+ 191, 147, 133,
+ 255, 211, 89,
+ 171, 77, 214,
+ 0, 121, 0,
+ 60, 14, 107,
+ 255, 82, 1,
+ 112, 115, 43,
+ 0, 172, 245,
+ 255, 184, 240,
+ 1, 210, 111,
+ 203, 151, 0,
+ 95, 114, 129,
+ 183, 215, 17,
+ 80, 110, 231,
+ 201, 25, 87,
+ 218, 250, 203,
+ 255, 148, 103,
+ 255, 217, 163,
+ 198, 172, 199,
+ 78, 139, 135,
+ 197, 255, 134,
+ 38, 0, 165,
+ 197, 208, 211,
+ 193, 117, 225,
+ 111, 0, 128,
+ 147, 255, 238,
+ 125, 62, 254,
+ 112, 213, 78,
+ 198, 76, 61,
+ 155, 48, 82,
+ 0, 199, 176,
+ 118, 6, 0,
+ 2, 106, 192,
+ 140, 167, 49,
+ 189, 81, 145,
+ 254, 162, 38,
+ 134, 138, 106,
+ 0, 68, 17,
+ 122, 73, 61,
+ 255, 251, 239,
+ 127, 94, 193,
+ 181, 140, 48,
+ 66, 235, 255,
+ 189, 140, 218,
+ 190, 0, 138,
+ 132, 177, 185,
+ 90, 54, 202,
+ 0, 35, 131,
+ 251, 139, 149,
+ 74, 0, 225,
+ 0, 106, 90,
+ 106, 199, 155,
+ 104, 169, 217,
+ 255, 239, 134,
+ 44, 94, 130,
+ 126, 0, 78,
+ 196, 214, 145,
+ 160, 238, 255,
+ 222, 215, 255,
+ 255, 87, 126,
+ 170, 161, 255,
+ 81, 120, 60,
+ 255, 242, 91,
+ 168, 130, 145,
+ 158, 153, 64,
+ 211, 123, 156,
+ 255, 0, 3,
+ 210, 118, 197,
+ 0, 41, 111,
+ 198, 178, 125,
+ 211, 255, 169,
+ 109, 215, 130,
+ 41, 90, 0,
+ 235, 193, 183,
+ 114, 58, 0,
+ 140, 96, 155,
+ 163, 223, 193,
+ 255, 142, 63,
+ 66, 155, 1,
+ 83, 96, 152,
+ 106, 133, 230,
+ 255, 85, 72,
+ 141, 216, 0,
+ 162, 102, 73,
+ 79, 0, 146,
+ 190, 0, 30,
+ 165, 0, 193,
+ 81, 84, 255,
+ 0, 148, 74,
+ 203, 0, 255,
+ 121, 54, 71,
+ 215, 255, 97,
+ 163, 178, 0,
+ 111, 154, 68,
+ 120, 93, 222,
+ 254, 187, 126,
+ 112, 0, 27,
+ 204, 59, 0,
+ 0, 165, 167,
+ 151, 255, 0,
+ 104, 41, 124,
+ 138, 89, 113,
+ 255, 94, 224,
+ 86, 91, 48,
+ 75, 255, 76,
+ 204, 190, 67,
+ 255, 224, 0,
+ 49, 126, 85,
+ 145, 196, 135,
+ 187, 64, 79,
+ 255, 130, 233,
+ 205, 127, 68,
+ 105, 195, 223,
+ 161, 213, 81,
+ 165, 183, 240,
+ 125, 255, 180,
+ 126, 255, 111,
+ 67, 255, 145,
+ 154, 138, 83,
+ 46, 145, 231,
+ 118, 121, 0,
+ 154, 2, 222,
+ 185, 119, 255,
+ 255, 0, 62,
+ 131, 28, 170,
+ 177, 70, 183,
+ 217, 0, 115,
+ 186, 196, 95,
+ 97, 46, 97,
+ 84, 134, 167,
+ 81, 54, 145,
+ 107, 117, 107,
+ 51, 15, 80,
+ 215, 139, 143,
+ 255, 247, 203,
+ 255, 86, 192,
+ 153, 91, 0,
+ 255, 1, 156,
+ 183, 79, 19,
+ 235, 255, 146,
+ 211, 1, 224,
+ 178, 167, 144,
+ 217, 97, 0,
+ 134, 91, 38,
+ 175, 151, 206,
+ 0, 182, 63,
+ 210, 40, 179,
+ 2, 213, 42,
+ 94, 84, 160,
+ 136, 48, 0,
+ 255, 110, 163,
+ 144, 121, 157,
+ 153, 61, 225,
+ 237, 87, 255,
+ 87, 24, 206,
+ 117, 143, 207,
+ };
+
+
+ DrawColor GlasbeyLUT::at(std::size_t id, float alpha)
+ {
+ const float scale = 1.f / 255.f; // Scale byte to float.
+
+ id = id % size();
+
+ DrawColor color;
+ color.r = scale * GLASBEY_LUT[id * 3 + 0];
+ color.g = scale * GLASBEY_LUT[id * 3 + 1];
+ color.b = scale * GLASBEY_LUT[id * 3 + 2];
+ color.a = alpha;
+ return color;
+ }
+
+
+ DrawColor24Bit GlasbeyLUT::atByte(std::size_t id)
+ {
+ id = id % size();
+
+ DrawColor24Bit color;
+ color.r = GLASBEY_LUT[id * 3 + 0];
+ color.g = GLASBEY_LUT[id * 3 + 1];
+ color.b = GLASBEY_LUT[id * 3 + 2];
+ return color;
+ }
+
+
+
+ std::size_t GlasbeyLUT::size()
+ {
+ ARMARX_CHECK_EQUAL(GLASBEY_LUT.size() % 3, 0);
+ return GLASBEY_LUT.size() / 3;
+ }
+
+ const std::vector<unsigned char>& GlasbeyLUT::data()
+ {
+ return GLASBEY_LUT;
+ }
+
+
+#if 0
+
+ /// Viridis lookup table
+ static const unsigned char VIRIDIS_LUT[] =
+ {
+ 68, 1, 84,
+ 68, 2, 85,
+ 69, 3, 87,
+ 69, 5, 88,
+ 69, 6, 90,
+ 70, 8, 91,
+ 70, 9, 93,
+ 70, 11, 94,
+ 70, 12, 96,
+ 71, 14, 97,
+ 71, 15, 98,
+ 71, 17, 100,
+ 71, 18, 101,
+ 71, 20, 102,
+ 72, 21, 104,
+ 72, 22, 105,
+ 72, 24, 106,
+ 72, 25, 108,
+ 72, 26, 109,
+ 72, 28, 110,
+ 72, 29, 111,
+ 72, 30, 112,
+ 72, 32, 113,
+ 72, 33, 115,
+ 72, 34, 116,
+ 72, 36, 117,
+ 72, 37, 118,
+ 72, 38, 119,
+ 72, 39, 120,
+ 71, 41, 121,
+ 71, 42, 121,
+ 71, 43, 122,
+ 71, 44, 123,
+ 71, 46, 124,
+ 70, 47, 125,
+ 70, 48, 126,
+ 70, 49, 126,
+ 70, 51, 127,
+ 69, 52, 128,
+ 69, 53, 129,
+ 69, 54, 129,
+ 68, 56, 130,
+ 68, 57, 131,
+ 68, 58, 131,
+ 67, 59, 132,
+ 67, 60, 132,
+ 67, 62, 133,
+ 66, 63, 133,
+ 66, 64, 134,
+ 65, 65, 134,
+ 65, 66, 135,
+ 65, 67, 135,
+ 64, 69, 136,
+ 64, 70, 136,
+ 63, 71, 136,
+ 63, 72, 137,
+ 62, 73, 137,
+ 62, 74, 137,
+ 61, 75, 138,
+ 61, 77, 138,
+ 60, 78, 138,
+ 60, 79, 138,
+ 59, 80, 139,
+ 59, 81, 139,
+ 58, 82, 139,
+ 58, 83, 139,
+ 57, 84, 140,
+ 57, 85, 140,
+ 56, 86, 140,
+ 56, 87, 140,
+ 55, 88, 140,
+ 55, 89, 140,
+ 54, 91, 141,
+ 54, 92, 141,
+ 53, 93, 141,
+ 53, 94, 141,
+ 52, 95, 141,
+ 52, 96, 141,
+ 51, 97, 141,
+ 51, 98, 141,
+ 51, 99, 141,
+ 50, 100, 142,
+ 50, 101, 142,
+ 49, 102, 142,
+ 49, 103, 142,
+ 48, 104, 142,
+ 48, 105, 142,
+ 47, 106, 142,
+ 47, 107, 142,
+ 47, 108, 142,
+ 46, 109, 142,
+ 46, 110, 142,
+ 45, 111, 142,
+ 45, 112, 142,
+ 45, 112, 142,
+ 44, 113, 142,
+ 44, 114, 142,
+ 43, 115, 142,
+ 43, 116, 142,
+ 43, 117, 142,
+ 42, 118, 142,
+ 42, 119, 142,
+ 41, 120, 142,
+ 41, 121, 142,
+ 41, 122, 142,
+ 40, 123, 142,
+ 40, 124, 142,
+ 40, 125, 142,
+ 39, 126, 142,
+ 39, 127, 142,
+ 38, 128, 142,
+ 38, 129, 142,
+ 38, 130, 142,
+ 37, 131, 142,
+ 37, 131, 142,
+ 37, 132, 142,
+ 36, 133, 142,
+ 36, 134, 142,
+ 35, 135, 142,
+ 35, 136, 142,
+ 35, 137, 142,
+ 34, 138, 141,
+ 34, 139, 141,
+ 34, 140, 141,
+ 33, 141, 141,
+ 33, 142, 141,
+ 33, 143, 141,
+ 32, 144, 141,
+ 32, 145, 140,
+ 32, 146, 140,
+ 32, 147, 140,
+ 31, 147, 140,
+ 31, 148, 140,
+ 31, 149, 139,
+ 31, 150, 139,
+ 31, 151, 139,
+ 30, 152, 139,
+ 30, 153, 138,
+ 30, 154, 138,
+ 30, 155, 138,
+ 30, 156, 137,
+ 30, 157, 137,
+ 30, 158, 137,
+ 30, 159, 136,
+ 30, 160, 136,
+ 31, 161, 136,
+ 31, 162, 135,
+ 31, 163, 135,
+ 31, 163, 134,
+ 32, 164, 134,
+ 32, 165, 134,
+ 33, 166, 133,
+ 33, 167, 133,
+ 34, 168, 132,
+ 35, 169, 131,
+ 35, 170, 131,
+ 36, 171, 130,
+ 37, 172, 130,
+ 38, 173, 129,
+ 39, 174, 129,
+ 40, 175, 128,
+ 41, 175, 127,
+ 42, 176, 127,
+ 43, 177, 126,
+ 44, 178, 125,
+ 46, 179, 124,
+ 47, 180, 124,
+ 48, 181, 123,
+ 50, 182, 122,
+ 51, 183, 121,
+ 53, 183, 121,
+ 54, 184, 120,
+ 56, 185, 119,
+ 57, 186, 118,
+ 59, 187, 117,
+ 61, 188, 116,
+ 62, 189, 115,
+ 64, 190, 114,
+ 66, 190, 113,
+ 68, 191, 112,
+ 70, 192, 111,
+ 72, 193, 110,
+ 73, 194, 109,
+ 75, 194, 108,
+ 77, 195, 107,
+ 79, 196, 106,
+ 81, 197, 105,
+ 83, 198, 104,
+ 85, 198, 102,
+ 88, 199, 101,
+ 90, 200, 100,
+ 92, 201, 99,
+ 94, 201, 98,
+ 96, 202, 96,
+ 98, 203, 95,
+ 101, 204, 94,
+ 103, 204, 92,
+ 105, 205, 91,
+ 108, 206, 90,
+ 110, 206, 88,
+ 112, 207, 87,
+ 115, 208, 85,
+ 117, 208, 84,
+ 119, 209, 82,
+ 122, 210, 81,
+ 124, 210, 79,
+ 127, 211, 78,
+ 129, 212, 76,
+ 132, 212, 75,
+ 134, 213, 73,
+ 137, 213, 72,
+ 139, 214, 70,
+ 142, 215, 68,
+ 144, 215, 67,
+ 147, 216, 65,
+ 149, 216, 63,
+ 152, 217, 62,
+ 155, 217, 60,
+ 157, 218, 58,
+ 160, 218, 57,
+ 163, 219, 55,
+ 165, 219, 53,
+ 168, 220, 51,
+ 171, 220, 50,
+ 173, 221, 48,
+ 176, 221, 46,
+ 179, 221, 45,
+ 181, 222, 43,
+ 184, 222, 41,
+ 187, 223, 39,
+ 189, 223, 38,
+ 192, 223, 36,
+ 195, 224, 35,
+ 197, 224, 33,
+ 200, 225, 32,
+ 203, 225, 30,
+ 205, 225, 29,
+ 208, 226, 28,
+ 211, 226, 27,
+ 213, 226, 26,
+ 216, 227, 25,
+ 219, 227, 24,
+ 221, 227, 24,
+ 224, 228, 24,
+ 226, 228, 24,
+ 229, 228, 24,
+ 232, 229, 25,
+ 234, 229, 25,
+ 237, 229, 26,
+ 239, 230, 27,
+ 242, 230, 28,
+ 244, 230, 30,
+ 247, 230, 31,
+ 249, 231, 33,
+ 251, 231, 35,
+ 254, 231, 36,
+ };
+
+#endif
+
+}
diff --git a/source/RobotAPI/libraries/core/visualization/GlasbeyLUT.h b/source/RobotAPI/libraries/core/visualization/GlasbeyLUT.h
new file mode 100644
index 0000000000000000000000000000000000000000..770a7fca64107c6c9446e0b634722ec0a0b696b2
--- /dev/null
+++ b/source/RobotAPI/libraries/core/visualization/GlasbeyLUT.h
@@ -0,0 +1,91 @@
+#pragma once
+
+#include <cmath>
+#include <type_traits>
+#include <vector>
+
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+
+
+namespace armarx
+{
+
+ struct DrawColor;
+ struct DrawColor24Bit;
+
+
+ /**
+ * "Color lookup table consisting of 256 colors structured in a maximally
+ * discontinuous manner. Generated using the method of Glasbey et al.
+ * (see https://github.com/taketwo/glasbey)" [1]
+ *
+ *
+ * [1](https://github.com/PointCloudLibrary/pcl/blob/master/common/include/pcl/common/colors.h)
+ */
+ class GlasbeyLUT
+ {
+ public:
+
+ /**
+ * @brief Get a color from the lookup table with given ID (with float values).
+ * The ID is automaticall wrapped if greater than `size()`.
+ * If `id` is negative, its absolute value is used.
+ */
+ template <typename UIntT, std::enable_if_t<std::is_unsigned<UIntT>::value, int> = 0>
+ static DrawColor at(UIntT id, float alpha = 1.f);
+ // If `id` is negative, its absolute value is used.
+ template <typename IntT, std::enable_if_t<std::is_signed<IntT>::value, int> = 0>
+ static DrawColor at(IntT id, float alpha = 1.f);
+ static DrawColor at(std::size_t id, float alpha = 1.f);
+
+ /**
+ * @brief Get a color from the lookup table with given ID (with integer values).
+ * The ID is automaticall wrapped if greater than `size()`.
+ * If `id` is negative, its absolute value is used.
+ */
+ template <typename UIntT, std::enable_if_t<std::is_unsigned<UIntT>::value, int> = 0>
+ static DrawColor24Bit atByte(UIntT id);
+ template <typename IntT, std::enable_if_t<std::is_signed<IntT>::value, int> = 0>
+ static DrawColor24Bit atByte(IntT id);
+ static DrawColor24Bit atByte(std::size_t id);
+
+ /// Get the number of colors in the lookup table.;
+ static std::size_t size();
+
+ /// Get the raw lookup table (flat).
+ static const std::vector<unsigned char>& data();
+
+
+ private:
+
+ /// Private constructor.
+ GlasbeyLUT() = default;
+
+ };
+
+
+ template <typename UIntT, std::enable_if_t<std::is_unsigned<UIntT>::value, int>>
+ DrawColor GlasbeyLUT::at(UIntT id, float alpha)
+ {
+ return at(static_cast<std::size_t>(id), alpha);
+ }
+
+ template <typename IntT, std::enable_if_t<std::is_signed<IntT>::value, int>>
+ DrawColor GlasbeyLUT::at(IntT id, float alpha)
+ {
+ return at(static_cast<std::size_t>(id >= 0 ? id : std::abs(id)), alpha);
+ }
+
+ template <typename UIntT, std::enable_if_t<std::is_unsigned<UIntT>::value, int>>
+ DrawColor24Bit GlasbeyLUT::atByte(UIntT id)
+ {
+ return atByte(static_cast<std::size_t>(id));
+ }
+
+ template <typename IntT, std::enable_if_t<std::is_signed<IntT>::value, int>>
+ DrawColor24Bit GlasbeyLUT::atByte(IntT id)
+ {
+ return atByte(static_cast<std::size_t>(id >= 0 ? id : std::abs(id)));
+ }
+
+}
diff --git a/source/RobotAPI/libraries/widgets/CMakeLists.txt b/source/RobotAPI/libraries/widgets/CMakeLists.txt
index bd78d50f3f8867cd5a0225362ca61a259cce44b7..8321b0aba4842080cc31585fee4dce13697c4a6c 100644
--- a/source/RobotAPI/libraries/widgets/CMakeLists.txt
+++ b/source/RobotAPI/libraries/widgets/CMakeLists.txt
@@ -10,8 +10,14 @@ armarx_build_if(jsoncpp_FOUND "jsoncpp not available")
set(LIB_NAME RobotAPIWidgets)
set(LIBS RobotAPICore)
-set(LIB_FILES DebugLayerControlWidget.cpp)
-set(LIB_HEADERS DebugLayerControlWidget.h)
+set(LIB_FILES
+ DebugLayerControlWidget.cpp
+ JSONTreeModel.cpp
+ )
+set(LIB_HEADERS
+ DebugLayerControlWidget.h
+ JSONTreeModel.h
+ )
set(GUI_UIS DebugLayerControlWidget.ui)
list(APPEND LIB_HEADERS ${GUI_UIS})
diff --git a/source/RobotAPI/libraries/widgets/JSONTreeModel.cpp b/source/RobotAPI/libraries/widgets/JSONTreeModel.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..74681c074bc599aa8148e3a8b5738da5d9fc37c0
--- /dev/null
+++ b/source/RobotAPI/libraries/widgets/JSONTreeModel.cpp
@@ -0,0 +1,168 @@
+#include "JSONTreeModel.h"
+
+namespace armarx
+{
+
+ void JSONTreeModel::setRoot(ValuePtr root)
+ {
+ this->root = root;
+ rows.clear();
+ rows[root] = 0;
+ parents.clear();
+ parents[root] = nullptr;
+ names.clear();
+ names[root] = "";
+ }
+
+ QModelIndex JSONTreeModel::index(int row, int column, const QModelIndex& parentIndex) const
+ {
+ ValuePtr parent = refFrom(parentIndex);
+ if (row >= (int)parent->size())
+ {
+ return QModelIndex();
+ }
+
+ ValuePtr child = nullptr;
+ if (parent->is_array())
+ {
+ child = &parent->at(row);
+ names[child] = "[" + std::to_string(row) + "]";
+ }
+ else if (parent->is_object())
+ {
+ auto it = parent->begin();
+ std::advance(it, row);
+ child = &it.value();
+ names[child] = it.key();
+ }
+
+ if (child)
+ {
+ parents[child] = parent;
+ rows[child] = row;
+
+ return createIndex(row, column, child);
+ }
+ else
+ {
+ return QModelIndex();
+ }
+
+ }
+
+ QModelIndex JSONTreeModel::parent(const QModelIndex& index) const
+ {
+ if (!index.isValid())
+ {
+ return QModelIndex();
+ }
+
+ ValuePtr child = refFrom(index);
+ ValuePtr parent = parents.at(child);
+ if (parent == nullptr)
+ {
+ return QModelIndex();
+ }
+ int row = rows.at(parent);
+
+ return createIndex(row, 0, parent);
+ }
+
+ int JSONTreeModel::rowCount(const QModelIndex& parentIndex) const
+ {
+ if (parentIndex.column() > 1)
+ {
+ return 0;
+ }
+
+ ValuePtr parent = refFrom(parentIndex);
+ if (parent->is_array() || parent->is_object())
+ {
+ return parent->size();
+ }
+ else
+ {
+ return 0;
+ }
+ }
+
+ int JSONTreeModel::columnCount(const QModelIndex& parent) const
+ {
+ return 2;
+ }
+
+ QVariant JSONTreeModel::data(const QModelIndex& index, int role) const
+ {
+ if (!index.isValid())
+ {
+ return QVariant();
+ }
+
+ if (role != Qt::DisplayRole)
+ {
+ return QVariant();
+ }
+
+ nlohmann::json* ref = refFrom(index);
+
+ if (index.column() == 0)
+ {
+ std::string const& name = names.at(ref);
+ return QVariant(name.c_str());
+ }
+
+ switch (ref->type())
+ {
+ case nlohmann::json::value_t::null:
+ return QVariant("null");
+ case nlohmann::json::value_t::object:
+ return QVariant("object");
+ case nlohmann::json::value_t::array:
+ return QVariant("array");
+ case nlohmann::json::value_t::string:
+ return QVariant(ref->get<std::string>().c_str());
+ case nlohmann::json::value_t::boolean:
+ return QVariant(ref->get<bool>());
+ case nlohmann::json::value_t::number_integer:
+ return QVariant(ref->get<int>());
+ case nlohmann::json::value_t::number_unsigned:
+ return QVariant(ref->get<unsigned int>());
+ case nlohmann::json::value_t::number_float:
+ return QVariant(ref->get<float>());
+
+ default:
+ return QVariant("n/a");
+ }
+ }
+
+ QVariant JSONTreeModel::headerData(int section, Qt::Orientation orientation, int role) const
+ {
+ if (role != Qt::DisplayRole)
+ {
+ return QVariant();
+ }
+
+ switch (section)
+ {
+ case 0:
+ return QVariant("Name");
+ case 1:
+ return QVariant("Value");
+ default:
+ return QVariant();
+ }
+ }
+
+ auto JSONTreeModel::refFrom(const QModelIndex& index) const -> ValuePtr
+ {
+ if (index.isValid())
+ {
+ return static_cast<ValuePtr>(index.internalPointer());
+ }
+ else
+ {
+ return root;
+ }
+ }
+
+}
diff --git a/source/RobotAPI/libraries/widgets/JSONTreeModel.h b/source/RobotAPI/libraries/widgets/JSONTreeModel.h
new file mode 100644
index 0000000000000000000000000000000000000000..83de308bd4a6cea7bccf9a1c0b21b414a633df31
--- /dev/null
+++ b/source/RobotAPI/libraries/widgets/JSONTreeModel.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include <VirtualRobot/Util/json/json.hpp>
+#include <QAbstractItemModel>
+
+namespace armarx
+{
+
+ class JSONTreeModel : public QAbstractItemModel
+ {
+ public:
+ using ValuePtr = nlohmann::json*;
+
+ void setRoot(ValuePtr root);
+
+ // QAbstractItemModel interface
+ QModelIndex index(int row, int column, const QModelIndex& parent) const override;
+ QModelIndex parent(const QModelIndex& index) const override;
+ int rowCount(const QModelIndex& parent) const override;
+ int columnCount(const QModelIndex& parent) const override;
+ QVariant data(const QModelIndex& index, int role) const override;
+ QVariant headerData(int section, Qt::Orientation orientation, int role) const override;
+
+ private:
+ ValuePtr refFrom(QModelIndex const& index) const;
+
+ private:
+ mutable std::map<ValuePtr, int> rows;
+ mutable std::map<ValuePtr, ValuePtr> parents;
+ mutable std::map<ValuePtr, std::string> names;
+ ValuePtr root;
+ };
+
+}
diff --git a/source/RobotAPI/statecharts/operations/RobotControl.h b/source/RobotAPI/statecharts/operations/RobotControl.h
index 78f9a394c4eb907a257bee597766db35d3623ee9..7aca952c718ff55f39188ad2ef246cbf36e85ba7 100644
--- a/source/RobotAPI/statecharts/operations/RobotControl.h
+++ b/source/RobotAPI/statecharts/operations/RobotControl.h
@@ -76,7 +76,7 @@ namespace armarx
void onExitRemoteStateOfferer() override;
void startRobotStatechart();
- void hardReset(const Ice::Current& = ::Ice::Current()) override;
+ void hardReset(const Ice::Current& = Ice::emptyCurrent) override;
PropertyDefinitionsPtr createPropertyDefinitions() override;