diff --git a/etc/cmake/ArmarXPackageVersion.cmake b/etc/cmake/ArmarXPackageVersion.cmake
index 3c8ad2971be26bad437f27eb6b387cdd36bb6d18..46287a285b0c05f5307a33ead03dd540d0e6b84a 100644
--- a/etc/cmake/ArmarXPackageVersion.cmake
+++ b/etc/cmake/ArmarXPackageVersion.cmake
@@ -1,7 +1,7 @@
# armarx version file
set(ARMARX_PACKAGE_LIBRARY_VERSION_MAJOR "0")
set(ARMARX_PACKAGE_LIBRARY_VERSION_MINOR "3")
-set(ARMARX_PACKAGE_LIBRARY_VERSION_PATCH "1")
+set(ARMARX_PACKAGE_LIBRARY_VERSION_PATCH "3")
set(ARMARX_PACKAGE_LIBRARY_VERSION "${ARMARX_PACKAGE_LIBRARY_VERSION_MAJOR}.${ARMARX_PACKAGE_LIBRARY_VERSION_MINOR}.${ARMARX_PACKAGE_LIBRARY_VERSION_PATCH}")
diff --git a/etc/doxygen/pages/5.Components.dox b/etc/doxygen/pages/5.Components.dox
index d99251036c390793fd454fba30b767c26f801cf5..46eac4e8ee2725910d23c143c8c081c70ec983cc 100644
--- a/etc/doxygen/pages/5.Components.dox
+++ b/etc/doxygen/pages/5.Components.dox
@@ -1,8 +1,12 @@
/**
\addtogroup Components 5. Components
+Components are the main building blocks of ArmarX and offer services for other applications via Ice.
+Each component usually implements one Ice interface by which it offers its services.
+They can be configured with \ref armarx-componentproperties via config files.
\defgroup RobotAPI-Components RobotAPI Components
\ingroup RobotAPI Components
+Robot related components like RobotStateComponent
...
*/
diff --git a/etc/doxygen/pages/SensorActorUnits.dox b/etc/doxygen/pages/SensorActorUnits.dox
index f0c6731cdc79b47601db8d948eeebe0b1c15c92a..bfff40b5809a5c897ee72551f9784b9a615a7cd3 100644
--- a/etc/doxygen/pages/SensorActorUnits.dox
+++ b/etc/doxygen/pages/SensorActorUnits.dox
@@ -1,6 +1,6 @@
/**
\defgroup RobotAPI-SensorActorUnits RobotAPI Sensor-Actor Units
- \ingroup RobotAPI
+ \ingroup RobotAPI RobotAPI-Components
* @brief Sensor-Actor Units are Armarx \ref armarx::Component "Components" that communicate usually directly with their
* associated hardware and feed the topic for their connected \ref armarx::Observer. They always implement
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index 5973e08afb3e22be1965db36dfe82b425767c387..f0df3989462c072aa1e13f2ddbcffedfe0454043 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -362,6 +362,47 @@ void DebugDrawerComponent::drawPointCloud(const PointCloudData &d)
layer.mainNode->addChild(sep);
}
+void DebugDrawerComponent::drawPolygon(const DebugDrawerComponent::PolygonData &d)
+{
+ ScopedRecursiveLockPtr l = getScopedLock();
+
+ auto& layer=requestLayer(d.layerName);
+
+ removePolygon(d.layerName,d.name);
+
+ if (!d.active)
+ return;
+
+ SoSeparator *sep = VirtualRobot::CoinVisualizationFactory::CreatePolygonVisualization(d.points,d.colorInner,d.colorBorder,d.lineWidth);
+ layer.addedPolygonVisualizations[d.name] = sep;
+ layer.mainNode->addChild(sep);
+}
+
+void DebugDrawerComponent::drawArrow(const DebugDrawerComponent::ArrowData &d)
+{
+ ScopedRecursiveLockPtr l = getScopedLock();
+
+ auto& layer=requestLayer(d.layerName);
+
+ removeArrow(d.layerName,d.name);
+
+ if (!d.active)
+ return;
+
+ SoSeparator *sep = new SoSeparator;
+
+ SoTransform *tr = new SoTransform;
+ tr->translation.setValue(d.position.x(), d.position.y(), d.position.z());
+ sep->addChild(tr);
+
+ SoSeparator *sepArrow = VirtualRobot::CoinVisualizationFactory::CreateArrow(d.direction,d.length,d.width,d.color);
+ sep->addChild(sepArrow);
+
+ layer.addedArrowVisualizations[d.name] = sep;
+ layer.mainNode->addChild(sep);
+
+}
+
void DebugDrawerComponent::removeLine(const std::string& layerName, const std::string &name)
{
ScopedRecursiveLockPtr l = getScopedLock();
@@ -374,6 +415,7 @@ void DebugDrawerComponent::removeLine(const std::string& layerName, const std::s
{
return;
}
+
layer.mainNode->removeChild(layer.addedLineVisualizations[name]);
layer.addedLineVisualizations.erase(name);
}
@@ -446,6 +488,40 @@ void DebugDrawerComponent::removePointCloud(const std::string &layerName, const
layer.addedPointCloudVisualizations.erase(name);
}
+void DebugDrawerComponent::removePolygon(const std::string &layerName, const std::string &name)
+{
+ ScopedRecursiveLockPtr l = getScopedLock();
+ if(!hasLayer(layerName))
+ {
+ return;
+ }
+ auto& layer=layers.at(layerName);
+ if(layer.addedPolygonVisualizations.find(name) == layer.addedPolygonVisualizations.end())
+ {
+ return;
+ }
+
+ layer.mainNode->removeChild(layer.addedPolygonVisualizations[name]);
+ layer.addedPolygonVisualizations.erase(name);
+}
+
+void DebugDrawerComponent::removeArrow(const std::string &layerName, const std::string &name)
+{
+ ScopedRecursiveLockPtr l = getScopedLock();
+ if(!hasLayer(layerName))
+ {
+ return;
+ }
+ auto& layer=layers.at(layerName);
+ if(layer.addedArrowVisualizations.find(name) == layer.addedArrowVisualizations.end())
+ {
+ return;
+ }
+
+ layer.mainNode->removeChild(layer.addedArrowVisualizations[name]);
+ layer.addedArrowVisualizations.erase(name);
+}
+
void DebugDrawerComponent::removeCoordSystem(const std::string& layerName, const std::string &name)
{
ScopedRecursiveLockPtr l = getScopedLock();
@@ -759,6 +835,94 @@ void DebugDrawerComponent::removePointCloudDebugLayerVisu(const std::string &poi
removePointCloudVisu(DEBUG_LAYER_NAME, pointCloudName);
}
+void DebugDrawerComponent::setPolygonVisu(const std::string &layerName, const std::string &polygonName, const std::vector<Vector3BasePtr> &polygonPoints, const DrawColor &colorInner, const DrawColor &colorBorder, float lineWidth, const Ice::Current &)
+{
+ {
+ ScopedRecursiveLockPtr l = getScopedDataLock();
+ std::string entryName = "__" + layerName + "__" + polygonName + "__";
+ PolygonData &d = accumulatedUpdateData.polygons[entryName];
+
+ std::vector< Eigen::Vector3f > points;
+ for (size_t i=0;i<polygonPoints.size();i++)
+ {
+ Eigen::Vector3f p = Vector3Ptr::dynamicCast(polygonPoints.at(i))->toEigen();;
+ points.push_back(p);
+ }
+
+ d.points = points;
+ d.colorInner = VirtualRobot::VisualizationFactory::Color(colorInner.r, colorInner.g, colorInner.b, 1 - colorInner.a);;
+ d.colorBorder = VirtualRobot::VisualizationFactory::Color(colorBorder.r, colorBorder.g, colorBorder.b, 1 - colorBorder.a);;
+
+ d.lineWidth = lineWidth;
+ d.layerName = layerName;
+ d.name = polygonName;
+ d.active = true;
+ }
+}
+
+void DebugDrawerComponent::setPolygonDebugLayerVisu(const std::string &polygonName, const std::vector<Vector3BasePtr> &polygonPoints, const DrawColor &colorInner, const DrawColor &colorBorder, float lineWidth, const Ice::Current &)
+{
+ setPolygonVisu(DEBUG_LAYER_NAME, polygonName, polygonPoints, colorInner, colorBorder, lineWidth);
+}
+
+void DebugDrawerComponent::removePolygonVisu(const std::string &layerName, const std::string &polygonName, const Ice::Current &)
+{
+ {
+ ScopedRecursiveLockPtr l = getScopedDataLock();
+ std::string entryName = "__" + layerName + "__" + polygonName + "__";
+ PolygonData &d = accumulatedUpdateData.polygons[entryName];
+ d.layerName = layerName;
+ d.name = polygonName;
+ d.active = false;
+ }
+}
+
+void DebugDrawerComponent::removePolygonDebugLayerVisu(const std::string &polygonName, const Ice::Current &)
+{
+ removePolygonVisu(DEBUG_LAYER_NAME, polygonName);
+}
+
+void DebugDrawerComponent::setArrowVisu(const std::string &layerName, const std::string &arrowName, const Vector3BasePtr &position, const Vector3BasePtr &direction, const DrawColor &color, float length, float width, const Ice::Current &)
+{
+ {
+ ScopedRecursiveLockPtr l = getScopedDataLock();
+ std::string entryName = "__" + layerName + "__" + arrowName + "__";
+ ArrowData &d = accumulatedUpdateData.arrows[entryName];
+
+ d.position = Vector3Ptr::dynamicCast(position)->toEigen();
+ d.direction = Vector3Ptr::dynamicCast(direction)->toEigen();
+ d.color = VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);;
+ d.length = length;
+ d.width = width;
+
+ d.layerName = layerName;
+ d.name = arrowName;
+ d.active = true;
+ }
+}
+
+void DebugDrawerComponent::setArrowDebugLayerVisu(const std::string &arrowName, const Vector3BasePtr &position, const Vector3BasePtr &direction, const DrawColor &color, float length, float width, const Ice::Current &)
+{
+ setArrowVisu(DEBUG_LAYER_NAME, arrowName, position, direction, color, length, width);
+}
+
+void DebugDrawerComponent::removeArrowVisu(const std::string &layerName, const std::string &arrowName, const Ice::Current &)
+{
+ {
+ ScopedRecursiveLockPtr l = getScopedDataLock();
+ std::string entryName = "__" + layerName + "__" + arrowName + "__";
+ ArrowData &d = accumulatedUpdateData.arrows[entryName];
+ d.layerName = layerName;
+ d.name = arrowName;
+ d.active = false;
+ }
+}
+
+void DebugDrawerComponent::removeArrowDebugLayerVisu(const std::string &arrowName, const Ice::Current &)
+{
+ removeArrowVisu(DEBUG_LAYER_NAME, arrowName);
+}
+
//todo: in some rare cases the mutex3D lock does not work and results in a broken coin timer setup. No updates of the scene will be drawn in this case
// -> check for a qt-thread solution
void DebugDrawerComponent::clearLayer(const std::string &layerName, const Ice::Current &)
@@ -780,6 +944,8 @@ void DebugDrawerComponent::clearLayer(const std::string &layerName, const Ice::C
layer.addedTextVisualizations.clear();
layer.addedSphereVisualizations.clear();
layer.addedPointCloudVisualizations.clear();
+ layer.addedPolygonVisualizations.clear();
+ layer.addedArrowVisualizations.clear();
layer.mainNode->removeAllChildren();
}
@@ -877,6 +1043,18 @@ void DebugDrawerComponent::updateVisualization()
}
accumulatedUpdateData.pointcloud.clear();
+ for (auto i = accumulatedUpdateData.polygons.begin(); i != accumulatedUpdateData.polygons.end(); i++)
+ {
+ drawPolygon(i->second);
+ }
+ accumulatedUpdateData.polygons.clear();
+
+ for (auto i = accumulatedUpdateData.arrows.begin(); i != accumulatedUpdateData.arrows.end(); i++)
+ {
+ drawArrow(i->second);
+ }
+ accumulatedUpdateData.arrows.clear();
+
}
bool DebugDrawerComponent::hasLayer(const std::string& layerName, const ::Ice::Current&)
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
index 7b734202acf9959a71c2a353a226c2e950849571..f8cbc853bf84c82208852b135b26e3d5a8e808d4 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -65,7 +65,7 @@ public:
/*!
\class DebugDrawerComponent
- \ingroup Components
+ \ingroup RobotAPI-Components
* \brief The DebugDrawerComponent class implements a component that listens to layered / debug drawer commands and creates internal representations of the visualization
* which can be conveniently used by gui components to visualize the content.
*
@@ -171,6 +171,16 @@ public:
virtual void removePointCloudVisu(const std::string &layerName, const std::string &pointCloudName, const ::Ice::Current& = ::Ice::Current());
virtual void removePointCloudDebugLayerVisu(const std::string& pointCloudName, const ::Ice::Current& = ::Ice::Current());
+ virtual 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());
+ virtual 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());
+ virtual void removePolygonVisu(const std::string &layerName, const std::string &polygonName, const ::Ice::Current& = ::Ice::Current());
+ virtual void removePolygonDebugLayerVisu(const std::string& polygonName, const ::Ice::Current& = ::Ice::Current());
+
+ virtual 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());
+ virtual 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());
+ virtual void removeArrowVisu(const std::string &layerName, const std::string &arrowName, const ::Ice::Current& = ::Ice::Current());
+ virtual void removeArrowDebugLayerVisu(const std::string& arrowName, const ::Ice::Current& = ::Ice::Current());
+
virtual void clearLayer(const std::string& layerName, const ::Ice::Current& = ::Ice::Current());
virtual void clearDebugLayer(const ::Ice::Current& = ::Ice::Current());
@@ -255,6 +265,21 @@ protected:
{
DebugDrawerPointCloud pointCloud;
};
+ struct PolygonData : public DrawData
+ {
+ std::vector< Eigen::Vector3f > points;
+ float lineWidth;
+ VirtualRobot::VisualizationFactory::Color colorInner;
+ VirtualRobot::VisualizationFactory::Color colorBorder;
+ };
+ struct ArrowData : public DrawData
+ {
+ Eigen::Vector3f position;
+ Eigen::Vector3f direction;
+ float length;
+ float width;
+ VirtualRobot::VisualizationFactory::Color color;
+ };
struct UpdateData
{
@@ -264,6 +289,8 @@ protected:
std::map<std::string, TextData> text;
std::map<std::string, SphereData> sphere;
std::map<std::string, PointCloudData> pointcloud;
+ std::map<std::string, PolygonData> polygons;
+ std::map<std::string, ArrowData> arrows;
};
UpdateData accumulatedUpdateData;
@@ -277,6 +304,8 @@ protected:
void drawText(const TextData &d);
void drawSphere(const SphereData &d);
void drawPointCloud(const PointCloudData &d);
+ void drawPolygon(const PolygonData &d);
+ void drawArrow(const ArrowData &d);
void removeCoordSystem(const std::string& layerName, const std::string &name);
void removeLine(const std::string& layerName, const std::string &name);
@@ -284,6 +313,8 @@ protected:
void removeText(const std::string& layerName, const std::string &name);
void removeSphere(const std::string& layerName, const std::string &name);
void removePointCloud(const std::string& layerName, const std::string &name);
+ void removePolygon(const std::string& layerName, const std::string &name);
+ void removeArrow(const std::string& layerName, const std::string &name);
void setLayerVisibility(const std::string& layerName, bool visible);
@@ -299,6 +330,8 @@ protected:
std::map<std::string, SoSeparator*> addedTextVisualizations;
std::map<std::string, SoSeparator*> addedSphereVisualizations;
std::map<std::string, SoSeparator*> addedPointCloudVisualizations;
+ std::map<std::string, SoSeparator*> addedPolygonVisualizations;
+ std::map<std::string, SoSeparator*> addedArrowVisualizations;
bool visible;
};
diff --git a/source/RobotAPI/components/MMMPlayer/MMMPlayer.h b/source/RobotAPI/components/MMMPlayer/MMMPlayer.h
index 9d72efcc6b4f6f4f2817912105817cd411cd5bae..f333e476935159108401a9f7706d8fb833051350 100644
--- a/source/RobotAPI/components/MMMPlayer/MMMPlayer.h
+++ b/source/RobotAPI/components/MMMPlayer/MMMPlayer.h
@@ -78,7 +78,7 @@ namespace armarx
/**
* @class MMMPlayer
- * @ingroup Components
+ * @ingroup RobotAPI-Components
* @brief A brief description
*
* Detailed Description
diff --git a/source/RobotAPI/components/WeissHapticSensorListener/WeissHapticSensorListener.h b/source/RobotAPI/components/WeissHapticSensorListener/WeissHapticSensorListener.h
index a6570fc94663003ea7dbeaf0f9341890a9d7be48..2a1501e57324edf37063f283b87dff43242c176f 100644
--- a/source/RobotAPI/components/WeissHapticSensorListener/WeissHapticSensorListener.h
+++ b/source/RobotAPI/components/WeissHapticSensorListener/WeissHapticSensorListener.h
@@ -52,7 +52,7 @@ namespace armarx
/**
* @class WeissHapticSensorListener
- * @ingroup Components
+ * @ingroup RobotAPI-Components
* @brief A brief description
*
* Detailed Description
diff --git a/source/RobotAPI/components/robotstate/RobotStateComponent.h b/source/RobotAPI/components/robotstate/RobotStateComponent.h
index 8bd87085c81106b0ad350afb710968073d510f56..80d9ec37bceea376ae17a87338246df463eef2ea 100644
--- a/source/RobotAPI/components/robotstate/RobotStateComponent.h
+++ b/source/RobotAPI/components/robotstate/RobotStateComponent.h
@@ -57,40 +57,33 @@ namespace armarx
/**
* @class RobotStateComponent
- * @ingroup Components
+ * @ingroup RobotAPI-Components
* The RobotStateComponent creates an internal robot representation from
- * a VirtualRobot XML file.
+ * a VirtualRobot (see [Simox](http://simox.sourceforge.net/)) XML file.
* Upon request, an Ice proxy to a shared instance of this internal robot
* can be acquired through a call to RobotStateComponent::getSynchronizedRobot().
* Additionally it is possible to retrieve a proxy for robot snapshot with
* RobotStateComponent::getRobotSnapshot().
* While the synchronized robot will constantly update its internal state
- * the robot snapshot is a clone of the original robot an won't update its
- * configuration over time.
- * See \ref remoterobot for more details and the usage of this component.
+ * the robot snapshot is a clone of the original robot and won't update its
+ * configuration over time. This is useful, if several components need
+ * to calculate on the same values.
+ * See \ref armarx::RemoteRobot "RemoteRobot" for more details and the usage of this component.
*/
class ARMARXCOMPONENT_IMPORT_EXPORT RobotStateComponent :
virtual public Component,
virtual public RobotStateComponentInterface
{
public:
- // inherited from KinematicUnitInterface
- void reportControlModeChanged(const NameControlModeMap& jointModes, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
- void reportJointAngles(const NameValueMap& jointAngles, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
- void reportJointVelocities(const NameValueMap& jointVelocities, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
- void reportJointTorques(const NameValueMap& jointTorques, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
- void reportJointAccelerations(const NameValueMap& jointAccelerations, bool aValueChanged, const Ice::Current& c);
- void reportJointCurrents(const NameValueMap& jointCurrents, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
- void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
- void reportJointStatuses(const NameStatusMap& jointStatuses, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+
/**
- * \return SharedRobotInterface proxy to the internal synchronized robot (gets created upon first call)
+ * \return SharedRobotInterface proxy to the internal synchronized robot.
*/
virtual SharedRobotInterfacePrx getSynchronizedRobot(const Ice::Current&) const;
/**
- * \return clone of the internal synchronized robot
+ * \return Clone of the internal synchronized robot.
*/
virtual SharedRobotInterfacePrx getRobotSnapshot(const std::string & time, const Ice::Current&);
@@ -99,6 +92,10 @@ namespace armarx
*/
virtual std::string getRobotFilename(const Ice::Current&) const;
+ /**
+ *
+ * \return The name of this robot instance.
+ */
virtual std::string getRobotName(const Ice::Current &) const;
/**
@@ -127,6 +124,16 @@ namespace armarx
* Calls unref() on RobotStateComponent::_synchronizedPrx.
*/
virtual ~RobotStateComponent();
+
+ // inherited from KinematicUnitInterface
+ void reportControlModeChanged(const NameControlModeMap& jointModes, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointAngles(const NameValueMap& jointAngles, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointVelocities(const NameValueMap& jointVelocities, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointTorques(const NameValueMap& jointTorques, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointAccelerations(const NameValueMap& jointAccelerations, bool aValueChanged, const Ice::Current& c);
+ void reportJointCurrents(const NameValueMap& jointCurrents, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointStatuses(const NameStatusMap& jointStatuses, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
private:
SharedRobotInterfacePrx _synchronizedPrx;
SharedRobotServantPtr _sharedRobotServant;
diff --git a/source/RobotAPI/components/units/HeadIKUnit.cpp b/source/RobotAPI/components/units/HeadIKUnit.cpp
index 21bad518407020e02b87e428149a545f97b9732b..766d076c5d4c4f7ebbed4f8c466325fa855ee099 100644
--- a/source/RobotAPI/components/units/HeadIKUnit.cpp
+++ b/source/RobotAPI/components/units/HeadIKUnit.cpp
@@ -59,7 +59,7 @@ namespace armarx
void HeadIKUnit::onDisconnectComponent()
{
release();
- ScopedLock lock(accessMutex);
+ //ScopedLock lock(accessMutex);
if (execTask) execTask->stop();
@@ -146,10 +146,20 @@ namespace armarx
{
ScopedLock lock(accessMutex);
- ARMARX_IMPORTANT << "Releasing HeadIKUnit";
+ ARMARX_INFO << "Releasing HeadIKUnit";
requested = false;
- kinematicUnitPrx->stop();
- ARMARX_IMPORTANT << "Released HeadIKUnit";
+
+ // why do we stop the kin unit?
+ /*try
+ {
+ if (kinematicUnitPrx)
+ kinematicUnitPrx->stop();
+ } catch (...)
+ {
+ ARMARX_IMPORTANT << "Released HeadIKUnit failed";
+ }*/
+
+ ARMARX_INFO << "Released HeadIKUnit";
}
@@ -160,10 +170,16 @@ namespace armarx
bool doCalculation = false;
{
- ScopedLock lock(accessMutex);
-
- doCalculation = requested && newTargetSet;
- newTargetSet = false;
+ ScopedTryLock lock(accessMutex);
+ if (lock.owns_lock())
+ {
+ doCalculation = requested && newTargetSet;
+ newTargetSet = false;
+ }
+ else
+ {
+ return;
+ }
}
@@ -186,22 +202,33 @@ namespace armarx
ikSolver.enableJointLimitAvoidance(true);
auto globalPos = targetPosition->toGlobal(localRobot);
- ARMARX_VERBOSE << deactivateSpam(1) << "Calculating IK for target position " << globalPos->output();
+ ARMARX_VERBOSE << "Calculating IK for target position " << globalPos->output();
if (!ikSolver.solve(globalPos->toEigen()))
{
ARMARX_WARNING << "IKSolver found no solution!";
}
else
{
+ ARMARX_DEBUG << "Solution found";
NameValueMap targetJointAngles;
+ NameControlModeMap controlModes;
for (int i = 0; i < (signed int)kinematicChain->getSize(); i++)
{
if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") != 0)
{
targetJointAngles[kinematicChain->getNode(i)->getName()] = kinematicChain->getNode(i)->getJointValue();
+ controlModes[kinematicChain->getNode(i)->getName()] = ePositionControl;
}
+ ARMARX_DEBUG << kinematicChain->getNode(i)->getName() << ": " << kinematicChain->getNode(i)->getJointValue();
+ }
+ try
+ {
+ kinematicUnitPrx->switchControlMode(controlModes);
+ kinematicUnitPrx->setJointAngles(targetJointAngles);
+ } catch (...)
+ {
+ ARMARX_IMPORTANT << "setJointAngles failed";
}
- kinematicUnitPrx->setJointAngles(targetJointAngles);
}
}
}
diff --git a/source/RobotAPI/components/units/TCPControlUnit.h b/source/RobotAPI/components/units/TCPControlUnit.h
index 60f455365f07c237ebc1eba9835b72029eef2167..a63f4b17b07ef313594c6433c3e8a2bc76753fbc 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.h
+++ b/source/RobotAPI/components/units/TCPControlUnit.h
@@ -67,7 +67,7 @@ namespace armarx {
* updates the joint velocities. To set another cycle time use setCycleTime().
* To set the velocity for a node use setTCPVelocity. Calling setTCPVelocity again with another nodeset
* will add this nodeset to the list of currently controlled TCPs.
- * @node After usage release() must be called to stop the recalcuation and setting of joint velocities.
+ * @node After usage release() **must** be called to stop the recalcuation and setting of joint velocities.
*/
class TCPControlUnit :
@@ -127,14 +127,15 @@ namespace armarx {
// UnitResourceManagementInterface interface
/**
- * @brief Triggers the calculation loop for using cartesian velocity. Call once before setting a tcp velocity, with SetTCPVelocity.
+ * @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());
/**
* @brief Releases and stops the recalculation and updating of joint velocities.
- * Call always when finished with cartesian control.
+ * Call always when finished with cartesian control. The target velocity values of
+ * all node set will be deleted in this function.
* @param c Ice Context, leave blank.
*/
void release(const Ice::Current &c = Ice::Current());
diff --git a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
index a278fd88a48297b7f525f367e8bb536b0f0ecebe..bc0454a50e84bd80453658fda212bdad2fca1f97 100644
--- a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
+++ b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
@@ -64,6 +64,8 @@ module armarx
};
sequence<DebugDrawerPointCloudElement> DebugDrawerPointCloud;
+ sequence< Vector3Base > PolygonPointList;
+
/*!
* \brief A layered drawing interface.
* All drawing operations are identified with a layer name in order to distinguish different drawing entitties.
@@ -85,6 +87,8 @@ module armarx
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);
void setPointCloudVisu(string layerName, string pointCloudName, DebugDrawerPointCloud pointCloud);
+ void setPolygonVisu(string layerName, string polygonName, PolygonPointList polygonPoints, DrawColor colorInner, DrawColor colorBorder, float lineWidth);
+ void setArrowVisu(string layerName, string arrowName, Vector3Base position, Vector3Base direction, DrawColor color, float length, float width);
/*!
* \brief setPoseVisu draws on the "debug" layer
@@ -98,6 +102,8 @@ module armarx
void setTextDebugLayerVisu(string textName, string text, Vector3Base globalPosition, DrawColor color, int size);
void setSphereDebugLayerVisu(string sphereName, Vector3Base globalPosition, DrawColor color, float radius);
void setPointCloudDebugLayerVisu(string pointCloudName, DebugDrawerPointCloud pointCloud);
+ void setPolygonDebugLayerVisu(string polygonName, PolygonPointList polygonPoints, DrawColor colorInner, DrawColor colorBorder, float lineWidth);
+ void setArrowDebugLayerVisu(string arrowName, Vector3Base position, Vector3Base direction, DrawColor color, float length, float width);
/*!
* \brief Remove visualization of coordinate system.
@@ -110,6 +116,8 @@ module armarx
void removeTextVisu(string layerName, string textName);
void removeSphereVisu(string layerName, string sphereName);
void removePointCloudVisu(string layerName, string pointCloudName);
+ void removePolygonVisu(string layerName, string polygonName);
+ void removeArrowVisu(string layerName, string arrowName);
/*!
* \brief Removes pose from the "debug" layer.
@@ -120,6 +128,8 @@ module armarx
void removeTextDebugLayerVisu(string textName);
void removeSphereDebugLayerVisu(string sphereName);
void removePointCloudDebugLayerVisu(string pointCloudName);
+ void removePolygonDebugLayerVisu(string polygonName);
+ void removeArrowDebugLayerVisu(string arrowName);
/*!
* \brief clear removes all visualizations for the given layer
diff --git a/source/RobotAPI/libraries/core/FramedPose.cpp b/source/RobotAPI/libraries/core/FramedPose.cpp
index 6f31a74198570c0200f7802d90b114444c0ad5a4..c9c160c7105da6ecaf19d411986ea1f93254e653 100644
--- a/source/RobotAPI/libraries/core/FramedPose.cpp
+++ b/source/RobotAPI/libraries/core/FramedPose.cpp
@@ -595,16 +595,21 @@ namespace armarx
{
VirtualRobot::LinkedCoordinate c(virtualRobot);
std::string frame;
- if(position)
+ if (position)
{
frame = position->getFrame();
- if(!frame.empty() && frame != orientation->getFrame())
- throw armarx::UserException("Error: frames mismatch");
+ if (orientation)
+ {
+ if(!frame.empty() && frame != orientation->getFrame())
+ {
+ throw armarx::UserException("Error: frames mismatch");
+ }
+ }
}
else
{
- if(!orientation)
+ if (!orientation)
armarx::UserException("createLinkedCoordinate: orientation and position are both NULL");
else
frame = orientation->getFrame();
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
index e5573185978d87565ff606285651844e22bed391..7c30f1f0104fb621e0761241c8841ecfa5588f09 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
@@ -163,6 +163,10 @@ namespace armarx
virtual VirtualRobot::RobotNodeSetPtr getRobotNodeSet(const std::string &nodeSetName);
virtual void getRobotNodeSets(std::vector<VirtualRobot::RobotNodeSetPtr> &storeNodeSet);
+ /**
+ *
+ * @return Global pose of the robot
+ */
virtual Eigen::Matrix4f getGlobalPose();
@@ -220,7 +224,11 @@ namespace armarx
virtual void deregisterRobotNodeSet(VirtualRobot::RobotNodeSetPtr nodeSet);
/// Not implemented yet
virtual void registerEndEffector(VirtualRobot::EndEffectorPtr endEffector);
- /// Not implemented yet
+ /**
+ * @brief Sets the global pose of the robot.
+ * @param globalPose new global pose
+ * @param applyValues No effect. Will be always applied.
+ */
virtual void setGlobalPose(const Eigen::Matrix4f &globalPose, bool applyValues = true);
VirtualRobot::RobotNodePtr createLocalNode(SharedRobotNodeInterfacePrx remoteNode, std::vector<VirtualRobot::RobotNodePtr> &allNodes, std::map<VirtualRobot::RobotNodePtr, std::vector<std::string> > &childrenMap, VirtualRobot::RobotPtr robo);