diff --git a/CMakeLists.txt b/CMakeLists.txt
index a0f73bd4fe3434516a9528fe308fe8c2c25ce29f..f6aa2024fb4bb00397189d1dc3d17239ff5d4a2f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -14,7 +14,7 @@ set(ARMARX_ENABLE_AUTO_CODE_FORMATTING TRUE)
armarx_project("RobotAPI")
depends_on_armarx_package(ArmarXGui "OPTIONAL")
-set(ArmarX_Simox_VERSION 2.3.16)
+set(ArmarX_Simox_VERSION 2.3.25)
find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
if (Simox_FOUND)
diff --git a/data/RobotAPI/robots/Armar3/ArmarIII-Head.xml b/data/RobotAPI/robots/Armar3/ArmarIII-Head.xml
index e958a6c6534e9c4139e744537440c34feaa41717..b21010a3900f02a7d8bf5944d60e3f847e6d325b 100644
--- a/data/RobotAPI/robots/Armar3/ArmarIII-Head.xml
+++ b/data/RobotAPI/robots/Armar3/ArmarIII-Head.xml
@@ -246,7 +246,7 @@
<Translation x="105" y="70" z="0" units='mm'/>
<rollpitchyaw roll="0" pitch="90" yaw="0" unitsAngle="degree"/>
<rollpitchyaw roll="0" pitch="0" yaw="180" unitsAngle="degree"/>
- <rollpitchyaw roll="20.7" pitch="0" yaw="0" unitsAngle="degree"/>
+ <rollpitchyaw roll="16.0" pitch="0" yaw="0" unitsAngle="degree"/>
<rollpitchyaw roll="0" pitch="0" yaw="180" unitsAngle="degree"/>
</Transform>
diff --git a/data/RobotAPI/robots/Armar3/ArmarIII-LeftArm.xml b/data/RobotAPI/robots/Armar3/ArmarIII-LeftArm.xml
index bb90ca542332fe578bc5e395796c3f86ae8adc54..23373322946aadb671a13acfdc026d4f99aa3d8e 100644
--- a/data/RobotAPI/robots/Armar3/ArmarIII-LeftArm.xml
+++ b/data/RobotAPI/robots/Armar3/ArmarIII-LeftArm.xml
@@ -99,7 +99,7 @@
<Joint type="revolute">
<!--DH a="0" d="-7.5" theta="90" alpha="-90" units="degree"/-->
<axis x="0" y="0" z="1"/>
- <Limits unit="degree" lo="-90" hi="27"/>
+ <Limits unit="degree" lo="-90" hi="19"/>
<MaxVelocity unit="radian" value="1"/>
<MaxAcceleration value="10"/>
<MaxTorque value="3000"/>
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index d3df4657dba7c1276ddde5da3e4fb2d93bb23005..b7696a628ecbba97f223fae013581de9a5be5b5e 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -43,6 +43,7 @@
#include <Inventor/nodes/SoTranslation.h>
#include <Inventor/nodes/SoMaterialBinding.h>
#include <Inventor/nodes/SoDrawStyle.h>
+#include <Inventor/nodes/SoLineSet.h>
#include <Inventor/SbVec3f.h>
#include <Inventor/fields/SoMFVec3f.h>
#include <Inventor/fields/SoMFColor.h>
@@ -440,6 +441,85 @@ namespace armarx
ARMARX_DEBUG << "drawLine2" << flush;
}
+ void DebugDrawerComponent::drawLineSet(const DebugDrawerComponent::LineSetData& d)
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+ ARMARX_DEBUG << "drawLineSet";
+
+ auto& layer = requestLayer(d.layerName);
+
+ removeLineSet(d.layerName, d.name);
+
+ if (!d.active)
+ {
+ return;
+ }
+
+ if (d.lineSet.points.size() % 2 != 0)
+ {
+ ARMARX_WARNING << "A line set requires an even amount of points";
+ return;
+ }
+
+ if (d.lineSet.points.size() != 2 * d.lineSet.intensities.size())
+ {
+ ARMARX_WARNING << "Amounts of points and intensities have to match for a line set";
+ return;
+ }
+
+ // Initialize color map for affordance visualization
+ std::vector<VirtualRobot::VisualizationFactory::Color> colors;
+ colors.push_back(VirtualRobot::VisualizationFactory::Color(d.lineSet.colorNoIntensity.r, d.lineSet.colorNoIntensity.g, d.lineSet.colorNoIntensity.b));
+ colors.push_back(VirtualRobot::VisualizationFactory::Color(d.lineSet.colorFullIntensity.r, d.lineSet.colorFullIntensity.g, d.lineSet.colorFullIntensity.b));
+ VirtualRobot::ColorMap visualizationColorMap = VirtualRobot::ColorMap::customColorMap(colors);
+
+ // Initialize visualization nodes
+ SoSeparator* sep = new SoSeparator;
+
+ SoMaterialBinding* binding = new SoMaterialBinding;
+ binding->value = SoMaterialBinding::PER_PART;
+ sep->addChild(binding);
+
+ SoDrawStyle* lineStyle = new SoDrawStyle;
+ lineStyle->lineWidth.setValue(d.lineSet.lineWidth);
+ sep->addChild(lineStyle);
+
+ SoCoordinate3* coordinateNode = new SoCoordinate3;
+ sep->addChild(coordinateNode);
+
+ SoMaterial* materialNode = new SoMaterial;
+ sep->addChild(materialNode);
+
+ SoLineSet* lineSetNode = new SoLineSet;
+ lineSetNode->startIndex.setValue(0);
+ sep->addChild(lineSetNode);
+
+
+ // Allocate index and material arrays
+ SbVec3f* coordinateValues = new SbVec3f[d.lineSet.points.size()];
+ int32_t* lineSetValues = new int32_t[d.lineSet.intensities.size()];
+ SbColor* colorValues = new SbColor[d.lineSet.intensities.size()];
+
+ for (unsigned int i = 0; i < d.lineSet.intensities.size(); i++)
+ {
+ lineSetValues[i] = 2;
+
+ coordinateValues[2 * i].setValue(d.lineSet.points[2 * i].x, d.lineSet.points[2 * i].y, d.lineSet.points[2 * i].z);
+ coordinateValues[2 * i + 1].setValue(d.lineSet.points[2 * i + 1].x, d.lineSet.points[2 * i + 1].y, d.lineSet.points[2 * i + 1].z);
+
+ VirtualRobot::VisualizationFactory::Color c = visualizationColorMap.getColor(d.lineSet.intensities[i]);
+ colorValues[i].setValue(c.r, c.g, c.b);
+ }
+
+ coordinateNode->point.setValuesPointer(d.lineSet.points.size(), coordinateValues);
+ lineSetNode->numVertices.setValuesPointer(d.lineSet.intensities.size(), lineSetValues);
+ materialNode->ambientColor.setValuesPointer(d.lineSet.intensities.size(), colorValues);
+ materialNode->diffuseColor.setValuesPointer(d.lineSet.intensities.size(), colorValues);
+
+ layer.addedLineSetVisualizations[d.name] = sep;
+ layer.mainNode->addChild(sep);
+ }
+
void DebugDrawerComponent::drawBox(const BoxData& d)
{
ScopedRecursiveLockPtr l = getScopedVisuLock();
@@ -908,6 +988,26 @@ namespace armarx
layer.addedLineVisualizations.erase(name);
}
+ void DebugDrawerComponent::removeLineSet(const std::string& layerName, const std::string& name)
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+
+ if (!hasLayer(layerName))
+ {
+ return;
+ }
+
+ auto& layer = layers.at(layerName);
+
+ if (layer.addedLineSetVisualizations.find(name) == layer.addedLineSetVisualizations.end())
+ {
+ return;
+ }
+
+ layer.mainNode->removeChild(layer.addedLineSetVisualizations[name]);
+ layer.addedLineSetVisualizations.erase(name);
+ }
+
void DebugDrawerComponent::removeBox(const std::string& layerName, const std::string& name)
{
ScopedRecursiveLockPtr l = getScopedVisuLock();
@@ -1259,6 +1359,42 @@ namespace armarx
removeLineVisu(DEBUG_LAYER_NAME, lineName);
}
+ void DebugDrawerComponent::setLineSetVisu(const std::string& layerName, const std::string& lineSetName, const DebugDrawerLineSet& lineSet, const Ice::Current&)
+ {
+ ARMARX_DEBUG << VAROUT(layerName) << VAROUT(lineSetName);
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + lineSetName + "__";
+ LineSetData& d = accumulatedUpdateData.lineSet[entryName];
+ d.lineSet = lineSet;
+ d.layerName = layerName;
+ d.name = lineSetName;
+ d.active = true;
+ }
+ }
+
+ void DebugDrawerComponent::setLineSetDebugLayerVisu(const std::string& lineSetName, const DebugDrawerLineSet& lineSet, const Ice::Current&)
+ {
+ setLineSetVisu(DEBUG_LAYER_NAME, lineSetName, lineSet);
+ }
+
+ void DebugDrawerComponent::removeLineSetVisu(const std::string& layerName, const std::string& lineSetName, const Ice::Current&)
+ {
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + lineSetName + "__";
+ LineSetData& d = accumulatedUpdateData.lineSet[entryName];
+ d.layerName = layerName;
+ d.name = lineSetName;
+ d.active = false;
+ }
+ }
+
+ void DebugDrawerComponent::removeLineSetDebugLayerVisu(const std::string& lineSetName, const Ice::Current&)
+ {
+ removeLineSetVisu(DEBUG_LAYER_NAME, lineSetName);
+ }
+
void DebugDrawerComponent::setBoxVisu(const std::string& layerName, const std::string& boxName, const PoseBasePtr& globalPose, const Vector3BasePtr& dimensions, const DrawColor& color, const Ice::Current&)
{
Eigen::Matrix4f gp = PosePtr::dynamicCast(globalPose)->toEigen();
@@ -1680,7 +1816,7 @@ namespace armarx
removeAccumulatedData(layerName);
- auto& layer = layers.at(layerName);
+ Layer& layer = layers.at(layerName);
for (const auto& i : layer.addedCoordVisualizations)
{
@@ -1692,6 +1828,11 @@ namespace armarx
removeLineVisu(layerName, i.first);
}
+ for (const auto& i : layer.addedLineSetVisualizations)
+ {
+ removeLineSetVisu(layerName, i.first);
+ }
+
for (const auto& i : layer.addedBoxVisualizations)
{
removeBoxVisu(layerName, i.first);
@@ -1722,6 +1863,11 @@ namespace armarx
removePolygonVisu(layerName, i.first);
}
+ for (const auto& i : layer.added24BitColoredPointCloudVisualizations)
+ {
+ remove24BitColoredPointCloudVisu(layerName, i.first);
+ }
+
for (const auto& i : layer.addedArrowVisualizations)
{
removeArrowVisu(layerName, i.first);
@@ -1823,6 +1969,13 @@ namespace armarx
accumulatedUpdateData.line.clear();
+ for (auto i = accumulatedUpdateData.lineSet.begin(); i != accumulatedUpdateData.lineSet.end(); i++)
+ {
+ drawLineSet(i->second);
+ }
+
+ accumulatedUpdateData.lineSet.clear();
+
for (auto i = accumulatedUpdateData.sphere.begin(); i != accumulatedUpdateData.sphere.end(); i++)
{
drawSphere(i->second);
@@ -2003,6 +2156,22 @@ namespace armarx
}
}
+ {
+ auto i1 = accumulatedUpdateData.lineSet.begin();
+
+ while (i1 != accumulatedUpdateData.lineSet.end())
+ {
+ if (boost::starts_with(i1->first, entryName))
+ {
+ i1 = accumulatedUpdateData.lineSet.erase(i1);
+ }
+ else
+ {
+ i1++;
+ }
+ }
+ }
+
{
auto i1 = accumulatedUpdateData.sphere.begin();
@@ -2158,6 +2327,7 @@ namespace armarx
{
int count = layer.second.addedCoordVisualizations.size() +
layer.second.addedLineVisualizations.size() +
+ layer.second.addedLineSetVisualizations.size() +
layer.second.addedBoxVisualizations.size() +
layer.second.addedTextVisualizations.size() +
layer.second.addedSphereVisualizations.size() +
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
index c231930010733505f544495a5f8e02fd5b372855..63aa758748bf9de5222e0e1e8b0b0850ccb1c6c6 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -157,6 +157,11 @@ namespace armarx
virtual void removeLineVisu(const std::string& layerName, const std::string& lineName, const ::Ice::Current& = ::Ice::Current());
virtual void removeLineDebugLayerVisu(const std::string& lineName, const ::Ice::Current& = ::Ice::Current());
+ virtual void setLineSetVisu(const std::string& layerName, const std::string& lineSetName, const DebugDrawerLineSet& lineSet, const ::Ice::Current& = ::Ice::Current());
+ virtual void setLineSetDebugLayerVisu(const std::string& lineSetName, const DebugDrawerLineSet& lineSet, const ::Ice::Current& = ::Ice::Current());
+ virtual void removeLineSetVisu(const std::string& layerName, const std::string& lineSetName, const ::Ice::Current& = ::Ice::Current());
+ virtual void removeLineSetDebugLayerVisu(const std::string& lineSetName, const ::Ice::Current& = ::Ice::Current());
+
virtual 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());
virtual void setBoxDebugLayerVisu(const std::string& boxName, const ::armarx::PoseBasePtr& globalPose, const ::armarx::Vector3BasePtr& dimensions, const DrawColor& color, const ::Ice::Current& = ::Ice::Current());
virtual void removeBoxVisu(const std::string& layerName, const std::string& boxName, const ::Ice::Current& = ::Ice::Current());
@@ -301,6 +306,10 @@ namespace armarx
float scale;
VirtualRobot::VisualizationFactory::Color color;
};
+ struct LineSetData : public DrawData
+ {
+ DebugDrawerLineSet lineSet;
+ };
struct BoxData : public DrawData
{
Eigen::Matrix4f globalPose;
@@ -384,6 +393,7 @@ namespace armarx
{
std::map<std::string, CoordData> coord;
std::map<std::string, LineData> line;
+ std::map<std::string, LineSetData> lineSet;
std::map<std::string, BoxData> box;
std::map<std::string, TextData> text;
std::map<std::string, SphereData> sphere;
@@ -414,6 +424,7 @@ namespace armarx
void drawCoordSystem(const CoordData& d);
void drawLine(const LineData& d);
+ void drawLineSet(const LineSetData& d);
void drawBox(const BoxData& d);
void drawText(const TextData& d);
void drawSphere(const SphereData& d);
@@ -427,6 +438,7 @@ namespace armarx
void removeCoordSystem(const std::string& layerName, const std::string& name);
void removeLine(const std::string& layerName, const std::string& name);
+ void removeLineSet(const std::string& layerName, const std::string& name);
void removeBox(const std::string& layerName, const std::string& name);
void removeText(const std::string& layerName, const std::string& name);
void removeSphere(const std::string& layerName, const std::string& name);
@@ -455,6 +467,7 @@ namespace armarx
SoSeparator* mainNode;
std::map<std::string, SoSeparator*> addedCoordVisualizations;
std::map<std::string, SoSeparator*> addedLineVisualizations;
+ std::map<std::string, SoSeparator*> addedLineSetVisualizations;
std::map<std::string, SoSeparator*> addedBoxVisualizations;
std::map<std::string, SoSeparator*> addedTextVisualizations;
std::map<std::string, SoSeparator*> addedSphereVisualizations;
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.cpp b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
index fb26ce56f3166d15cba8a3565b53c3aa22f8ea82..756b00b155b3bcd8c0e551d14a9d00eef6879c83 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
@@ -77,7 +77,7 @@ void ForceTorqueObserver::onInitObserver()
offerConditionCheck("magnitudesmaller", new ConditionCheckMagnitudeSmaller());
usingProxy("RobotStateComponent");
- usingTopic("DebugDrawerUpdates");
+ offeringTopic("DebugDrawerUpdates");
}
void ForceTorqueObserver::onConnectObserver()
diff --git a/source/RobotAPI/components/units/HeadIKUnit.cpp b/source/RobotAPI/components/units/HeadIKUnit.cpp
index 46dcd0ab50fc6bca4e71b3015614707f949fde63..c7ccaca529f6c7569d56d6e5880078c99b6fbaaf 100644
--- a/source/RobotAPI/components/units/HeadIKUnit.cpp
+++ b/source/RobotAPI/components/units/HeadIKUnit.cpp
@@ -121,7 +121,7 @@ namespace armarx
this->targetPosition->frame = targetPosition->frame;
FramedPositionPtr globalTarget = FramedPositionPtr::dynamicCast(targetPosition)->toGlobal(robotStateComponentPrx->getSynchronizedRobot());
- if (drawer)
+ if (drawer && getProperty<bool>("VisualizeIKTarget").getValue())
{
drawer->setSphereDebugLayerVisu("HeadViewTarget",
globalTarget,
@@ -295,7 +295,7 @@ namespace armarx
ARMARX_DEBUG << "Solution found";
- if (drawer && localRobot->hasRobotNode("Cameras"))
+ if (drawer && localRobot->hasRobotNode("Cameras") && getProperty<bool>("VisualizeIKTarget").getValue())
{
Vector3Ptr startPos = new Vector3(localRobot->getRobotNode("Cameras")->getGlobalPose());
drawer->setSphereDebugLayerVisu("HeadViewTargetSolution",
diff --git a/source/RobotAPI/components/units/HeadIKUnit.h b/source/RobotAPI/components/units/HeadIKUnit.h
index 6deff5a7add3ea52ae1bc09721a4e421c316df2d..f76b57c7db2cfe8f1980eda8a124b8b741b6fb90 100644
--- a/source/RobotAPI/components/units/HeadIKUnit.h
+++ b/source/RobotAPI/components/units/HeadIKUnit.h
@@ -50,6 +50,7 @@ namespace armarx
defineOptionalProperty<std::string>("RobotStateTopicName", "RobotState", "Name of the RobotComponent State topic.");
defineOptionalProperty<std::string>("HeadIKUnitTopicName", "HeadIKUnitTopic", "Name of the HeadIKUnit Topic");
defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the RobotStateComponent that should be used");
+ defineOptionalProperty<bool>("VisualizeIKTarget", true, "Visualize the current IK target using the debug drawer");
defineOptionalProperty<int>("CycleTime", 30, "Cycle time of the tcp control in ms");
}
};
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
index 00c597a2be1467b87c331c9059726c50c5a0bb81..2e284ba71c4dab425e4f0522d721fc4fadfe5854 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
@@ -53,7 +53,7 @@ void PlatformUnitWidget::onInitComponent()
{
usingProxy(platformUnitProxyName);
usingTopic(platformName + "State");
- ARMARX_WARNING << "Listening on Topic: " << platformName + "State";
+ ARMARX_INFO << "Listening on Topic: " << platformName + "State";
}
void PlatformUnitWidget::onConnectComponent()
diff --git a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
index 198ff86013a71e0cf4c6131b53aaac18a90e8e3a..8ffc400d4901806595a88f52333bea04cedc4486 100644
--- a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
+++ b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
@@ -109,6 +109,19 @@ module armarx
float pointSize = three;
};
+ sequence<DebugDrawerPointCloudElement> DebugDrawerLineSetPointList;
+ sequence<float> DebugDrawerLineSetIntensityList;
+
+ struct DebugDrawerLineSet
+ {
+ float lineWidth;
+ DrawColor colorNoIntensity;
+ DrawColor colorFullIntensity;
+
+ DebugDrawerLineSetPointList points;
+ DebugDrawerLineSetIntensityList intensities;
+ };
+
sequence< Vector3Base > PolygonPointList;
enum DrawStyle { FullModel, CollisionModel };
@@ -134,6 +147,7 @@ module armarx
void setPoseVisu(string layerName, string poseName, PoseBase globalPose);
void setScaledPoseVisu(string layerName, string poseName, PoseBase globalPose, float scale);
void setLineVisu(string layerName, string lineName, Vector3Base globalPosition1, Vector3Base globalPosition2, float lineWidth, DrawColor color);
+ void setLineSetVisu(string layerName, string lineSetName, DebugDrawerLineSet lineSet);
void setBoxVisu(string layerName, string boxName, PoseBase globalPose, Vector3Base dimensions, DrawColor color);
void setTextVisu(string layerName, string textName, string text, Vector3Base globalPosition, DrawColor color, int size);
void setSphereVisu(string layerName, string sphereName, Vector3Base globalPosition, DrawColor color, float radius);
@@ -174,6 +188,7 @@ module armarx
void setPoseDebugLayerVisu(string poseName, PoseBase globalPose);
void setScaledPoseDebugLayerVisu(string poseName, PoseBase globalPose, float scale);
void setLineDebugLayerVisu(string lineName, Vector3Base globalPosition1, Vector3Base globalPosition2, float lineWidth, DrawColor color);
+ void setLineSetDebugLayerVisu(string lineSetName, DebugDrawerLineSet lineSet);
void setBoxDebugLayerVisu(string boxName, PoseBase globalPose, Vector3Base dimensions, DrawColor color);
void setTextDebugLayerVisu(string textName, string text, Vector3Base globalPosition, DrawColor color, int size);
void setSphereDebugLayerVisu(string sphereName, Vector3Base globalPosition, DrawColor color, float radius);
@@ -189,6 +204,7 @@ module armarx
*/
void removePoseVisu(string layerName, string poseName);
void removeLineVisu(string layerName, string lineName);
+ void removeLineSetVisu(string layerName, string lineSetName);
void removeBoxVisu(string layerName, string boxName);
void removeTextVisu(string layerName, string textName);
void removeSphereVisu(string layerName, string sphereName);
@@ -205,6 +221,7 @@ module armarx
*/
void removePoseDebugLayerVisu(string poseName);
void removeLineDebugLayerVisu(string lineName);
+ void removeLineSetDebugLayerVisu(string lineSetName);
void removeBoxDebugLayerVisu(string boxName);
void removeTextDebugLayerVisu(string textName);
void removeSphereDebugLayerVisu(string sphereName);