diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index e75b9614da16d4ca9deeb42b556107ca7c9001a4..9176c4481965c7e5f9ab6e3944b0c3e1927dcd08 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -248,7 +248,7 @@ namespace armarx
removeSelectionCallbacks();
selectionNode->deselectAll();
- for (auto& e : selectedElements)
+ for (auto & e : selectedElements)
{
SoNode* n = SoSelection::getByName(SELECTION_NAME(e.layerName, e.elementName));
if (n)
@@ -288,7 +288,7 @@ namespace armarx
name = name.substr(index + strlen(SELECTION_NAME_SPLITTER));
// Check if selected element is 'selectable'
- for (auto& l : layers)
+ for (auto & l : layers)
{
std::string layer = l.first;
if (layers[layer].addedBoxVisualizations.find(name) != layers[layer].addedBoxVisualizations.end()
@@ -443,17 +443,29 @@ namespace armarx
void DebugDrawerComponent::exportScene(const std::string& filename, const Ice::Current&)
{
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+
ARMARX_INFO << "Exporting scene to '" << filename << "'";
+ SoSeparator* sep = new SoSeparator;
+ SoUnits* u = new SoUnits;
+ u->units = SoUnits::MILLIMETERS;
+ sep->addChild(u);
+ sep->addChild(layerMainNode);
+
SoWriteAction writeAction;
writeAction.getOutput()->openFile(filename.c_str());
writeAction.getOutput()->setBinary(false);
- writeAction.apply(layerMainNode);
+ writeAction.apply(sep);
writeAction.getOutput()->closeFile();
+
+ sep->unref();
}
void DebugDrawerComponent::exportLayer(const std::string& filename, const std::string& layerName, const Ice::Current&)
{
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+
ARMARX_INFO << "Exporting layer '" << layerName << "' to '" << filename << "'";
if (!hasLayer(layerName))
@@ -462,11 +474,19 @@ namespace armarx
return;
}
+ SoSeparator* sep = new SoSeparator;
+ SoUnits* u = new SoUnits;
+ u->units = SoUnits::MILLIMETERS;
+ sep->addChild(u);
+ sep->addChild(layers[layerName].mainNode);
+
SoWriteAction writeAction;
writeAction.getOutput()->openFile(filename.c_str());
writeAction.getOutput()->setBinary(false);
- writeAction.apply(layers[layerName].mainNode);
+ writeAction.apply(sep);
writeAction.getOutput()->closeFile();
+
+ sep->unref();
}
void DebugDrawerComponent::drawCoordSystem(const CoordData& d)
@@ -1063,11 +1083,21 @@ namespace armarx
// load robot
std::string filename = d.robotFile;
ArmarXDataPath::getAbsolutePath(filename, filename);
+
+ VirtualRobot::SceneObject::VisualizationType visuType = VirtualRobot::SceneObject::Full;
+ VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull;
+
+ if (d.drawStyle == DrawStyle::CollisionModel)
+ {
+ visuType = VirtualRobot::SceneObject::Collision;
+ loadMode = VirtualRobot::RobotIO::eCollisionModel;
+ }
+
ARMARX_INFO << "Loading robot from " << filename;
try
{
- result = RobotIO::loadRobot(filename);
+ result = RobotIO::loadRobot(filename, loadMode);
}
catch (...)
{
@@ -1087,15 +1117,6 @@ namespace armarx
m->setOverride(false);
sep->addChild(m);
-
-
- VirtualRobot::SceneObject::VisualizationType visuType = VirtualRobot::SceneObject::Full;
-
- if (d.drawStyle == DrawStyle::CollisionModel)
- {
- visuType = VirtualRobot::SceneObject::Collision;
- }
-
boost::shared_ptr<CoinVisualization> robVisu = result->getVisualization<CoinVisualization>(visuType);
if (robVisu)
@@ -1927,7 +1948,7 @@ namespace armarx
d.layerName = layerName;
d.name = robotName;
- for (auto& it : configuration)
+ for (auto & it : configuration)
{
d.configuration[it.first] = it.second;
}
@@ -1974,7 +1995,7 @@ namespace armarx
void DebugDrawerComponent::clearAll(const Ice::Current&)
{
- for (auto& i : layers)
+ for (auto & i : layers)
{
clearLayer(i.first);
}
@@ -2017,72 +2038,72 @@ namespace armarx
Layer& layer = layers.at(layerName);
- for (const auto& i : layer.addedCoordVisualizations)
+ for (const auto & i : layer.addedCoordVisualizations)
{
removePoseVisu(layerName, i.first);
}
- for (const auto& i : layer.addedLineVisualizations)
+ for (const auto & i : layer.addedLineVisualizations)
{
removeLineVisu(layerName, i.first);
}
- for (const auto& i : layer.addedLineSetVisualizations)
+ for (const auto & i : layer.addedLineSetVisualizations)
{
removeLineSetVisu(layerName, i.first);
}
- for (const auto& i : layer.addedBoxVisualizations)
+ for (const auto & i : layer.addedBoxVisualizations)
{
removeBoxVisu(layerName, i.first);
}
- for (const auto& i : layer.addedTextVisualizations)
+ for (const auto & i : layer.addedTextVisualizations)
{
removeTextVisu(layerName, i.first);
}
- for (const auto& i : layer.addedSphereVisualizations)
+ for (const auto & i : layer.addedSphereVisualizations)
{
removeSphereVisu(layerName, i.first);
}
- for (const auto& i : layer.addedCylinderVisualizations)
+ for (const auto & i : layer.addedCylinderVisualizations)
{
removeCylinderVisu(layerName, i.first);
}
- for (const auto& i : layer.addedPointCloudVisualizations)
+ for (const auto & i : layer.addedPointCloudVisualizations)
{
removePointCloudVisu(layerName, i.first);
}
- for (const auto& i : layer.addedPolygonVisualizations)
+ for (const auto & i : layer.addedPolygonVisualizations)
{
removePolygonVisu(layerName, i.first);
}
- for (const auto& i : layer.addedTriMeshVisualizations)
+ for (const auto & i : layer.addedTriMeshVisualizations)
{
removeTriMeshVisu(layerName, i.first);
}
- for (const auto& i : layer.added24BitColoredPointCloudVisualizations)
+ for (const auto & i : layer.added24BitColoredPointCloudVisualizations)
{
remove24BitColoredPointCloudVisu(layerName, i.first);
}
- for (const auto& i : layer.addedArrowVisualizations)
+ for (const auto & i : layer.addedArrowVisualizations)
{
removeArrowVisu(layerName, i.first);
}
- for (const auto& i : layer.addedRobotVisualizations)
+ for (const auto & i : layer.addedRobotVisualizations)
{
removeRobotVisu(layerName, i.first);
}
- for (const auto& i : layer.addedCustomVisualizations)
+ for (const auto & i : layer.addedCustomVisualizations)
{
removeCustomVisu(layerName, i.first);
}
@@ -2155,74 +2176,74 @@ namespace armarx
// check for clear&remove
//(updates only contain elements to add for each layer after the last clear for this layer was executed)
//this prevents a sequence add,clear,add to result in an empty layer
- for (const auto& layer : accumulatedUpdateData.clearLayers)
+ for (const auto & layer : accumulatedUpdateData.clearLayers)
{
clearLayerQt(layer);
}
accumulatedUpdateData.clearLayers.clear();
- for (const auto& layer : accumulatedUpdateData.removeLayers)
+ for (const auto & layer : accumulatedUpdateData.removeLayers)
{
removeLayerQt(layer);
}
accumulatedUpdateData.removeLayers.clear();
//add elements
- for (auto& e : accumulatedUpdateData.coord)
+ for (auto & e : accumulatedUpdateData.coord)
{
drawCoordSystem(e.second);
}
accumulatedUpdateData.coord.clear();
- for (auto& e : accumulatedUpdateData.box)
+ for (auto & e : accumulatedUpdateData.box)
{
drawBox(e.second);
}
accumulatedUpdateData.box.clear();
- for (auto& e : accumulatedUpdateData.line)
+ for (auto & e : accumulatedUpdateData.line)
{
drawLine(e.second);
}
accumulatedUpdateData.line.clear();
- for (auto& e : accumulatedUpdateData.lineSet)
+ for (auto & e : accumulatedUpdateData.lineSet)
{
drawLineSet(e.second);
}
accumulatedUpdateData.lineSet.clear();
- for (auto& e : accumulatedUpdateData.sphere)
+ for (auto & e : accumulatedUpdateData.sphere)
{
drawSphere(e.second);
}
accumulatedUpdateData.sphere.clear();
- for (auto& e : accumulatedUpdateData.cylinder)
+ for (auto & e : accumulatedUpdateData.cylinder)
{
drawCylinder(e.second);
}
accumulatedUpdateData.cylinder.clear();
- for (auto& e : accumulatedUpdateData.text)
+ for (auto & e : accumulatedUpdateData.text)
{
drawText(e.second);
}
accumulatedUpdateData.text.clear();
- for (auto& e : accumulatedUpdateData.pointcloud)
+ for (auto & e : accumulatedUpdateData.pointcloud)
{
drawPointCloud(e.second);
}
accumulatedUpdateData.pointcloud.clear();
- for (auto& e : accumulatedUpdateData.polygons)
+ for (auto & e : accumulatedUpdateData.polygons)
{
drawPolygon(e.second);
}
accumulatedUpdateData.polygons.clear();
- for (auto& e : accumulatedUpdateData.arrows)
+ for (auto & e : accumulatedUpdateData.arrows)
{
drawArrow(e.second);
}
@@ -2234,7 +2255,7 @@ namespace armarx
}
accumulatedUpdateData.triMeshes.clear();
- for (auto& e : accumulatedUpdateData.robots)
+ for (auto & e : accumulatedUpdateData.robots)
{
ARMARX_DEBUG << "update visu / drawRobot for robot " << e.first;
ensureExistingRobotNodes(e.second);
@@ -2243,13 +2264,13 @@ namespace armarx
}
accumulatedUpdateData.robots.clear();
- for (auto& e : accumulatedUpdateData.coloredpointcloud)
+ for (auto & e : accumulatedUpdateData.coloredpointcloud)
{
drawColoredPointCloud(e.second);
}
accumulatedUpdateData.coloredpointcloud.clear();
- for (auto& e : accumulatedUpdateData.colored24Bitpointcloud)
+ for (auto & e : accumulatedUpdateData.colored24Bitpointcloud)
{
draw24BitColoredPointCloud(e.second);
}
@@ -2354,7 +2375,7 @@ namespace armarx
ScopedRecursiveLockPtr l = getScopedVisuLock();
Ice::StringSeq seq {};
- for (const auto& layer : layers)
+ for (const auto & layer : layers)
{
seq.push_back(layer.first);
}
@@ -2367,7 +2388,7 @@ namespace armarx
::armarx::LayerInformationSequence seq {};
ScopedRecursiveLockPtr l = getScopedVisuLock();
- for (const auto& layer : layers)
+ for (const auto & layer : layers)
{
int count = layer.second.addedCoordVisualizations.size() +
layer.second.addedLineVisualizations.size() +
diff --git a/source/RobotAPI/components/RobotState/SharedRobotServants.cpp b/source/RobotAPI/components/RobotState/SharedRobotServants.cpp
index 3f78deea41d9139c9248fc10024d6b73ed38c5d2..94d35b724d947e9c1cfd7e2351e1c5fdd18aa943 100644
--- a/source/RobotAPI/components/RobotState/SharedRobotServants.cpp
+++ b/source/RobotAPI/components/RobotState/SharedRobotServants.cpp
@@ -277,6 +277,32 @@ namespace armarx
return _node->getJointLimitLow();
}
+ Vector3BasePtr SharedRobotNodeServant::getCoM(const Current& current) const
+ {
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ return new Vector3(_node->getCoMLocal());
+ }
+
+ FloatSeq SharedRobotNodeServant::getInertia(const Current& current) const
+ {
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ FloatSeq result;
+
+ for (int i = 0; i < 3; i++)
+ for (int j = 0; j < 3; j++)
+ {
+ result.push_back(_node->getInertiaMatrix()(i, j));
+ }
+
+ return result;
+ }
+
+ float SharedRobotNodeServant::getMass(const Current& current) const
+ {
+ ReadLockPtr lock = _node->getRobot()->getReadLock();
+ return _node->getMass();
+ }
+
///////////////////////////////
// SharedRobotServant
diff --git a/source/RobotAPI/components/RobotState/SharedRobotServants.h b/source/RobotAPI/components/RobotState/SharedRobotServants.h
index ffb43145a0476a69e7969fd716311413f95f0365..bf48eef8e2da125b9d4495bc10fe0ce343308b04 100644
--- a/source/RobotAPI/components/RobotState/SharedRobotServants.h
+++ b/source/RobotAPI/components/RobotState/SharedRobotServants.h
@@ -72,7 +72,7 @@ namespace armarx
*/
class SharedRobotNodeServant :
virtual public SharedRobotNodeInterface,
- public SharedObjectBase
+ public SharedObjectBase
{
public:
SharedRobotNodeServant(VirtualRobot::RobotNodePtr node /*,const Ice::Current & current = Ice::Current()*/);
@@ -98,6 +98,10 @@ namespace armarx
virtual float getJointLimitHigh(const Ice::Current& current = Ice::Current()) const;
virtual float getJointLimitLow(const Ice::Current& current = Ice::Current()) const;
+ virtual Vector3BasePtr getCoM(const Ice::Current& current = Ice::Current()) const;
+ virtual Ice::FloatSeq getInertia(const Ice::Current& current = Ice::Current()) const;
+ virtual float getMass(const Ice::Current& current = Ice::Current()) const;
+
protected:
diff --git a/source/RobotAPI/interface/core/RobotState.ice b/source/RobotAPI/interface/core/RobotState.ice
index 8db9ea88f97194426603e449fe9f149bf3e82f33..056ef0c53f99599b49bf7b8ed7d7f6d846cbf504 100644
--- a/source/RobotAPI/interface/core/RobotState.ice
+++ b/source/RobotAPI/interface/core/RobotState.ice
@@ -28,6 +28,8 @@
#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
#include <RobotAPI/interface/core/FramedPoseBase.ice>
#include <ArmarXCore/interface/observers/Timestamp.ice>
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <Ice/BuiltinSequences.ice>
module armarx
{
@@ -115,6 +117,15 @@ module armarx
["cpp:const"] idempotent
float getJointLimitLow();
+ ["cpp:const"] idempotent
+ Vector3Base getCoM();
+
+ ["cpp:const"] idempotent
+ Ice::FloatSeq getInertia();
+
+ ["cpp:const"] idempotent
+ float getMass();
+
};
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
index a75392014800b19193af241b99232e4ef240b2d9..3cb73dba296fc23c108ab0d74f929323af6b8b13 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
@@ -234,6 +234,16 @@ namespace armarx
JointType jt = remoteNode->getType();
+ SceneObject::Physics physics;
+ physics.localCoM = Vector3Ptr::dynamicCast(remoteNode->getCoM())->toEigen();
+ std::vector<float> inertia = remoteNode->getInertia();
+ for (int i = 0; i < 3; i++)
+ for (int j = 0; j < 3; j++)
+ {
+ physics.inertiaMatrix(i, j) = inertia.at(i * 3 + j);
+ }
+ physics.massKg = remoteNode->getMass();
+
switch (jt)
{
case ePrismatic:
@@ -248,13 +258,13 @@ namespace armarx
axis = result4f.block(0, 0, 3, 1);
result = prismaticNodeFactory->createRobotNode(robo, name, VirtualRobot::VisualizationNodePtr(), VirtualRobot::CollisionModelPtr(),
- jvLo, jvHi, jointOffset, idMatrix, idVec3, axis);
+ jvLo, jvHi, jointOffset, idMatrix, idVec3, axis, physics);
}
break;
case eFixed:
result = fixedNodeFactory->createRobotNode(robo, name, VirtualRobot::VisualizationNodePtr(), VirtualRobot::CollisionModelPtr(), 0,
- 0, 0, localTransform, idVec3, idVec3);
+ 0, 0, localTransform, idVec3, idVec3, physics);
break;
case eRevolute:
@@ -269,7 +279,7 @@ namespace armarx
axis = result4f.block(0, 0, 3, 1);
result = revoluteNodeFactory->createRobotNode(robo, name, VirtualRobot::VisualizationNodePtr(), VirtualRobot::CollisionModelPtr(),
- jvLo, jvHi, jointOffset, localTransform, axis, idVec3);
+ jvLo, jvHi, jointOffset, localTransform, axis, idVec3, physics);
}
break;