From 7b963a537ce75679bfbd657928b4dccf51dd9ebb Mon Sep 17 00:00:00 2001
From: mirkowaechter <mirkowaechter@042f3d55-54a8-47e9-b7fb-15903f145c44>
Date: Mon, 2 Dec 2013 16:30:46 +0000
Subject: [PATCH] Added ForceTorqueSensor to Dynamic Simulation and
VirtualRobot
git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@447 042f3d55-54a8-47e9-b7fb-15903f145c44
---
.../BulletEngine/BulletCoinQtViewer.cpp | 3 +-
.../BulletEngine/BulletOpenGLViewer.cpp | 1 +
.../BulletEngine/BulletRobot.cpp | 99 +++++++++++++++-
.../DynamicsEngine/BulletEngine/BulletRobot.h | 40 ++++++-
SimDynamics/DynamicsEngine/DynamicsRobot.cpp | 2 +
SimDynamics/DynamicsEngine/DynamicsRobot.h | 7 +-
VirtualRobot/CMakeLists.txt | 4 +
VirtualRobot/Nodes/ForceTorqueSensor.cpp | 63 ++++++++++
VirtualRobot/Nodes/ForceTorqueSensor.h | 90 ++++++++++++++
.../Nodes/ForceTorqueSensorFactory.cpp | 112 ++++++++++++++++++
VirtualRobot/Nodes/ForceTorqueSensorFactory.h | 62 ++++++++++
.../data/robots/ArmarIII/ArmarIII-LeftArm.xml | 3 +-
.../robots/ArmarIII/ArmarIII-RightArm.xml | 2 +-
13 files changed, 477 insertions(+), 11 deletions(-)
create mode 100644 VirtualRobot/Nodes/ForceTorqueSensor.cpp
create mode 100644 VirtualRobot/Nodes/ForceTorqueSensor.h
create mode 100644 VirtualRobot/Nodes/ForceTorqueSensorFactory.cpp
create mode 100644 VirtualRobot/Nodes/ForceTorqueSensorFactory.h
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.cpp
index 40e7d36e0..ce4ffa992 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.cpp
@@ -21,7 +21,7 @@ BulletCoinQtViewer::BulletCoinQtViewer(DynamicsWorldPtr world)
bulletMaxSubSteps = 50;
enablePhysicsUpdates = true;
- const float TIMER_MS = 30.0f;
+ const float TIMER_MS = 30.0f;
SIMDYNAMICS_ASSERT(world);
@@ -176,6 +176,7 @@ void BulletCoinQtViewer::updateMotors(float dt)
for (size_t i=0;i<robots.size();i++)
{
robots[i]->actuateJoints(dt);
+ robots[i]->updateSensors();
}
}
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.cpp
index 7df45327e..4576b8896 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.cpp
@@ -56,6 +56,7 @@ void BulletOpenGLViewer::updateMotors(float dt)
for (size_t i=0;i<robots.size();i++)
{
robots[i]->actuateJoints(dt);
+ robots[i]->updateSensors();
}
}
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
index ac32d0e3e..a13694a7d 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
@@ -9,6 +9,7 @@
#include <VirtualRobot/Nodes/RobotNodePrismatic.h>
#include <VirtualRobot/Nodes/RobotNodeFixed.h>
#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
+#include <VirtualRobot/Nodes/ForceTorqueSensor.h>
// either hinge or generic6DOF constraints can be used
//#define USE_BULLET_GENERIC_6DOF_CONSTRAINT
@@ -29,6 +30,23 @@ BulletRobot::BulletRobot(VirtualRobot::RobotPtr rob, bool enableJointMotors)
bulletMaxMotorImulse = 50.0f;
bulletMotorVelFactor = 10.0f;
buildBulletModels(enableJointMotors);
+
+ // activate force torque sensors
+ std::vector<SensorPtr>::iterator it = sensors.begin();
+ for(; it != sensors.end(); it++)
+ {
+// SensorPtr sensor = *it;
+ ForceTorqueSensorPtr ftSensor = boost::dynamic_pointer_cast<ForceTorqueSensor>(*it);
+ if(ftSensor)
+ {
+ VirtualRobot::RobotNodePtr node = ftSensor->getRobotNode();//boost::dynamic_pointer_cast<VirtualRobot::RobotNode>(ftSensor->getParent());
+ THROW_VR_EXCEPTION_IF(!node, "parent of sensor could not be casted to RobotNode")
+
+ const LinkInfo& link = getLink(node);
+ enableForceTorqueFeedback(link);
+ std::cout << "Found force torque sensor: " << node->getName() << std::endl;
+ }
+ }
}
BulletRobot::~BulletRobot()
@@ -980,6 +998,27 @@ void BulletRobot::actuateJoints(float dt)
setPoseNonActuatedRobotNodes();
}
+void BulletRobot::updateSensors()
+{
+ std::vector<SensorPtr>::iterator it = sensors.begin();
+ for(; it != sensors.end(); it++)
+ {
+// SensorPtr sensor = *it;
+ ForceTorqueSensorPtr ftSensor = boost::dynamic_pointer_cast<ForceTorqueSensor>(*it);
+ if(ftSensor)
+ {
+ VirtualRobot::RobotNodePtr node = ftSensor->getRobotNode();
+ //boost::dynamic_pointer_cast<VirtualRobot::RobotNode>(ftSensor->getParent());
+ THROW_VR_EXCEPTION_IF(!node, "parent of sensor could not be casted to RobotNode")
+
+ const LinkInfo& link = getLink(node);
+ Eigen::VectorXf forceTorques = getJointForceTorqueGlobal(link);
+ ftSensor->updateSensors(forceTorques);
+ std::cout << "Updating force torque sensor: " << node->getName() << ": " << forceTorques << std::endl;
+ }
+ }
+}
+
BulletRobot::LinkInfo BulletRobot::getLink( VirtualRobot::RobotNodePtr node )
{
for (size_t i=0;i<links.size();i++)
@@ -1223,6 +1262,29 @@ float BulletRobot::getNodeTarget( VirtualRobot::RobotNodePtr node )
}
+Eigen::Vector3f BulletRobot::getJointTorques(RobotNodePtr rn)
+{
+ VR_ASSERT(rn);
+ Eigen::Vector3f result;
+ result.setZero();
+ if (!hasLink(rn))
+ {
+ VR_ERROR << "No link with node " << rn->getName() << endl;
+ return result;
+ }
+ LinkInfo link = getLink(rn);
+
+
+
+
+ if(rn->isRotationalJoint())
+ {
+ result = getJointForceTorqueGlobal(link).tail(3);
+ }
+
+ return result;
+}
+
Eigen::Matrix4f BulletRobot::getComGlobal( VirtualRobot::RobotNodePtr rn )
{
BulletObjectPtr bo = boost::dynamic_pointer_cast<BulletObject>(getDynamicsRobotNode(rn));
@@ -1280,11 +1342,11 @@ void BulletRobot::setPoseNonActuatedRobotNodes()
}
}
-void BulletRobot::enableForceTorqueFeedback( const LinkInfo& link )
+void BulletRobot::enableForceTorqueFeedback(const LinkInfo& link , bool enable)
{
- if (!link.joint->needsFeedback())
+ if (!link.joint->needsFeedback() && enable)
{
- link.joint->enableFeedback(true);
+ link.joint->enableFeedback(true);
btJointFeedback* feedback = new btJointFeedback;
feedback->m_appliedForceBodyA = btVector3(0, 0, 0);
feedback->m_appliedForceBodyB = btVector3(0, 0, 0);
@@ -1292,6 +1354,10 @@ void BulletRobot::enableForceTorqueFeedback( const LinkInfo& link )
feedback->m_appliedTorqueBodyB = btVector3(0, 0, 0);
link.joint->setJointFeedback(feedback);
}
+ else if(link.joint->needsFeedback() && !enable)
+ {
+ link.joint->enableFeedback(false);
+ }
}
Eigen::VectorXf BulletRobot::getForceTorqueFeedbackA( const LinkInfo& link )
@@ -1322,7 +1388,32 @@ Eigen::VectorXf BulletRobot::getForceTorqueFeedbackB( const LinkInfo& link )
if (!feedback)
return r;
r << feedback->m_appliedForceBodyB[0],feedback->m_appliedForceBodyB[1],feedback->m_appliedForceBodyB[2],feedback->m_appliedTorqueBodyB[0],feedback->m_appliedTorqueBodyB[1],feedback->m_appliedTorqueBodyB[2];
- return r;
+ return r;
+}
+
+Eigen::VectorXf BulletRobot::getJointForceTorqueGlobal(const BulletRobot::LinkInfo &link)
+{
+ Eigen::VectorXf ftA = getForceTorqueFeedbackA(link);
+ Eigen::VectorXf ftB = getForceTorqueFeedbackB(link);
+
+ Eigen::Vector3f jointGlobal = link.nodeJoint->getGlobalPose().block(0,3,3,1);
+ Eigen::Vector3f comBGlobal = link.nodeB->getCoMGlobal();
+
+ // force that is applied on objectA by objectB -> so the force that object B applies on the joint
+ Eigen::Vector3f forceOnBGlobal = ftB.head(3);
+
+ Eigen::Vector3f torqueBGlobal = ftB.tail(3);
+
+ // the lever from Object B CoM to Joint
+ Eigen::Vector3f leverOnJoint = (comBGlobal-jointGlobal) * 0.001;
+ // Calculate the torque in Joint by taking the torque that presses on the CoM of BodyB and the Torque of BodyB on the joint
+ // forceOnBGlobal is inverted in next line because it is the force of A on B to hold it in position
+ // torqueBGlobal is inverted in next line because it is the torque on B from A to compensate torque of other objects (which is the torque we would like) to hold it in place and therefore needs to be inverted as well
+ Eigen::Vector3f torqueJointGlobal = (leverOnJoint).cross(-forceOnBGlobal) + (-1)*torqueBGlobal;
+ Eigen::VectorXf result(6);
+ result.head(3) = ftA.head(3); // force in joint is same as force on CoM of A
+ result.tail(3) = torqueJointGlobal;
+ return result;
}
} // namespace VirtualRobot
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
index 5445900c2..a3e9893db 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
@@ -59,10 +59,32 @@ public:
};
- void enableForceTorqueFeedback(const LinkInfo& link);
+ /**
+ * @brief The force torque output needs to be activated before use
+ * @param link Link for which the force torque should be enabled/disabled
+ * @param enable
+ */
+ void enableForceTorqueFeedback(const LinkInfo& link, bool enable = true);
+
+ /**
+ * @brief getForceTorqueFeedbackA retrieves the force torque in the first body of the link
+ * @param link Link for which the force torque should be retrieved
+ * @return 6 Dim. vector. First 3 values are the forces, last 3 are torques.
+ * Values are in N and N*m. Position of the values is the center of mass of first body of the link
+ * in the global coordinate system.
+ */
Eigen::VectorXf getForceTorqueFeedbackA(const LinkInfo& link);
- Eigen::VectorXf getForceTorqueFeedbackB(const LinkInfo& link);
-
+ Eigen::VectorXf getForceTorqueFeedbackB(const LinkInfo& link);
+ /**
+ * @brief getJointForceTorqueGlobal retrieves the force torque in the joint between the bodies
+ * @param link Link for which the force torque should be retrieved
+ * @return 6 Dim. vector. First 3 values are the forces, last 3 are torques.
+ * Values are in N and N*m. Position of the values is in the middle of the joint
+ * in the global coordinate system.
+ */
+ Eigen::VectorXf getJointForceTorqueGlobal(const LinkInfo& link);
+
+
// We do not allow to re-adjust the robot.
// The position of the robot is queried once on construction.
// Then the physics simulation takes over.
@@ -85,11 +107,23 @@ public:
*/
virtual void actuateJoints(float dt);
+ virtual void updateSensors();
+
virtual float getJointAngle(VirtualRobot::RobotNodePtr rn);
virtual float getJointSpeed(VirtualRobot::RobotNodePtr rn);
virtual float getJointTargetSpeed(VirtualRobot::RobotNodePtr rn);
virtual float getNodeTarget(VirtualRobot::RobotNodePtr node);
+ /*!
+ * \brief getJointTorques retrieves the torques in the given joint.
+ * \param rn
+ * \return Returns the torques in the given joint. If rn is not a
+ * rotational joint.
+ * Values are in N*m. Position of the values is in the middle of the joint
+ * in the global coordinate system.
+ */
+ Eigen::Vector3f getJointTorques(VirtualRobot::RobotNodePtr rn);
+
/*!
Returns the CoM pose, which is reported by bullet
*/
diff --git a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
index 437b5a0f9..eba2353e8 100644
--- a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
+++ b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
@@ -9,6 +9,8 @@ DynamicsRobot::DynamicsRobot(VirtualRobot::RobotPtr rob)
{
THROW_VR_EXCEPTION_IF(!rob,"NULL object");
robot = rob;
+ sensors = rob->getSensors();
+
}
DynamicsRobot::~DynamicsRobot()
diff --git a/SimDynamics/DynamicsEngine/DynamicsRobot.h b/SimDynamics/DynamicsEngine/DynamicsRobot.h
index b5760285e..dce08aa5b 100644
--- a/SimDynamics/DynamicsEngine/DynamicsRobot.h
+++ b/SimDynamics/DynamicsEngine/DynamicsRobot.h
@@ -26,6 +26,8 @@
#include "../SimDynamics.h"
#include "DynamicsObject.h"
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/Nodes/Sensor.h>
+
namespace SimDynamics
{
@@ -80,7 +82,8 @@ public:
Usually this method is called by the framework in every tick to perform joint actuation.
\param dt Timestep
*/
- virtual void actuateJoints(float dt);
+ virtual void actuateJoints(float dt);
+ virtual void updateSensors(){}
// experimental...
virtual void ensureKinematicConstraints();
@@ -120,6 +123,8 @@ protected:
VirtualRobot::RobotPtr robot;
+ std::vector<VirtualRobot::SensorPtr> sensors;
+
std::vector<VirtualRobot::RobotNodePtr> robotNodes;
std::map<VirtualRobot::RobotNodePtr, DynamicsObjectPtr> dynamicRobotNodes;
};
diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt
index 58cd392c2..3a1998f8c 100644
--- a/VirtualRobot/CMakeLists.txt
+++ b/VirtualRobot/CMakeLists.txt
@@ -31,8 +31,10 @@ Nodes/RobotNodeFixedFactory.cpp
Nodes/RobotNodeActuator.cpp
Nodes/Sensor.cpp
Nodes/PositionSensor.cpp
+Nodes/ForceTorqueSensor.cpp
Nodes/CameraSensor.cpp
Nodes/PositionSensorFactory.cpp
+Nodes/ForceTorqueSensorFactory.cpp
Nodes/CameraSensorFactory.cpp
Visualization/Visualization.cpp
Visualization/VisualizationNode.cpp
@@ -100,9 +102,11 @@ Nodes/RobotNodeActuator.h
Nodes/ConditionedLock.h
Nodes/Sensor.h
Nodes/PositionSensor.h
+Nodes/ForceTorqueSensor.h
Nodes/CameraSensor.h
Nodes/SensorFactory.h
Nodes/PositionSensorFactory.h
+Nodes/ForceTorqueSensorFactory.h
Nodes/CameraSensorFactory.h
Transformation/DHParameter.h
Visualization/VisualizationFactory.h
diff --git a/VirtualRobot/Nodes/ForceTorqueSensor.cpp b/VirtualRobot/Nodes/ForceTorqueSensor.cpp
new file mode 100644
index 000000000..405dc90ee
--- /dev/null
+++ b/VirtualRobot/Nodes/ForceTorqueSensor.cpp
@@ -0,0 +1,63 @@
+
+#include "ForceTorqueSensor.h"
+#include "ForceTorqueSensorFactory.h"
+#include "../XML/BaseIO.h"
+
+using namespace boost;
+
+namespace VirtualRobot {
+
+ForceTorqueSensor::ForceTorqueSensor(RobotNodeWeakPtr robotNode,
+ const std::string &name) : Sensor(robotNode,name),
+ forceTorqueValues(6)
+{
+ forceTorqueValues.setZero();
+}
+
+
+ForceTorqueSensor::~ForceTorqueSensor()
+{
+}
+
+const Eigen::VectorXf &ForceTorqueSensor::getForceTorque()
+{
+ return forceTorqueValues;
+}
+
+
+
+void ForceTorqueSensor::print( bool printChildren, bool printDecoration ) const
+{
+ if (printDecoration)
+ cout << "******** ForceTorqueSensor ********" << endl;
+ Sensor::print(printChildren,false);
+}
+
+
+SensorPtr ForceTorqueSensor::_clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr visualizationModel, float scaling)
+{
+ SensorPtr result(new ForceTorqueSensor(newRobotNode,name));
+ return result;
+}
+
+
+std::string ForceTorqueSensor::toXML(const std::string &modelPath, int tabs)
+{
+ std::stringstream ss;
+ std::string t = "\t";
+ std::string pre = "";
+ for (int i=0;i<tabs;i++)
+ pre += t;
+ ss << pre << "<Sensor type='" << ForceTorqueSensorFactory::getName() << "'/>" << endl;
+ std::string pre2 = pre + t;
+
+
+ return ss.str();
+}
+
+void ForceTorqueSensor::updateSensors(const Eigen::VectorXf &newForceTorque)
+{
+ forceTorqueValues = newForceTorque;
+}
+
+} // namespace VirtualRobot
diff --git a/VirtualRobot/Nodes/ForceTorqueSensor.h b/VirtualRobot/Nodes/ForceTorqueSensor.h
new file mode 100644
index 000000000..50c0e094e
--- /dev/null
+++ b/VirtualRobot/Nodes/ForceTorqueSensor.h
@@ -0,0 +1,90 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox 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 Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package VirtualRobot
+* @author Nikolaus Vahrenkamp
+* @copyright 2013 Nikolaus Vahrenkamp
+* GNU Lesser General Public License
+*
+*/
+#ifndef _VirtualRobot_ForceTorqueSensor_h_
+#define _VirtualRobot_ForceTorqueSensor_h_
+
+#include "Sensor.h"
+
+
+namespace VirtualRobot
+{
+
+class ForceTorqueSensor;
+typedef boost::shared_ptr<ForceTorqueSensor> ForceTorqueSensorPtr;
+
+
+/*!
+ This is a force torque sensor in a joint
+ The force torque values are in world coordinates
+*/
+class VIRTUAL_ROBOT_IMPORT_EXPORT ForceTorqueSensor : public Sensor
+{
+public:
+ friend class Robot;
+ friend class RobotIO;
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ /*!
+ Constructor with settings.
+ */
+ ForceTorqueSensor( RobotNodeWeakPtr robotNode,
+ const std::string &name
+ );
+
+ /*!
+ */
+ virtual ~ForceTorqueSensor();
+
+ void updateSensors(const Eigen::VectorXf &newForceTorque);
+
+
+ const Eigen::VectorXf &getForceTorque();
+ const Eigen::Vector3f &getForce();
+ const Eigen::Vector3f &getTorque();
+
+ /*!
+ Print status information.
+ */
+ virtual void print(bool printChildren = false, bool printDecoration = true) const;
+
+
+ virtual std::string toXML(const std::string &modelPath, int tabs);
+
+protected:
+
+ ForceTorqueSensor(){}
+
+
+ /*!
+ Derived classes must implement their clone method here.
+ */
+ virtual SensorPtr _clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr visualizationModel, float scaling);
+ Eigen::VectorXf forceTorqueValues;
+};
+
+typedef boost::shared_ptr<ForceTorqueSensor> ForceTorqueSensorPtr;
+
+} // namespace VirtualRobot
+
+#endif // _VirtualRobot_ForceTorqueSensor_h_
diff --git a/VirtualRobot/Nodes/ForceTorqueSensorFactory.cpp b/VirtualRobot/Nodes/ForceTorqueSensorFactory.cpp
new file mode 100644
index 000000000..cd24947e6
--- /dev/null
+++ b/VirtualRobot/Nodes/ForceTorqueSensorFactory.cpp
@@ -0,0 +1,112 @@
+
+
+#include "ForceTorqueSensorFactory.h"
+#include "Sensor.h"
+#include "../XML/BaseIO.h"
+#include "../XML/rapidxml.hpp"
+#include "ForceTorqueSensor.h"
+
+
+namespace VirtualRobot {
+
+ForceTorqueSensorFactory::ForceTorqueSensorFactory()
+{
+}
+
+
+ForceTorqueSensorFactory::~ForceTorqueSensorFactory()
+{
+}
+
+
+/**
+ * This method creates a VirtualRobot::ForceTorqueSensor.
+ *
+ * \return instance of VirtualRobot::ForceTorqueSensor.
+ */
+SensorPtr ForceTorqueSensorFactory::createSensor(RobotNodePtr node, const std::string& name, VisualizationNodePtr visualization,
+ const Eigen::Matrix4f &rnTrafo) const
+{
+ SensorPtr Sensor(new ForceTorqueSensor(node, name));
+
+ return Sensor;
+}
+
+SensorPtr ForceTorqueSensorFactory::createSensor(RobotNodePtr node, rapidxml::xml_node<char>* sensorXMLNode, RobotIO::RobotDescription loadMode, const std::string basePath) const
+{
+ THROW_VR_EXCEPTION_IF(!sensorXMLNode,"NULL data");
+ THROW_VR_EXCEPTION_IF(!node,"NULL data");
+
+
+ // get name
+ std::string sensorName = BaseIO::processNameAttribute(sensorXMLNode,true);
+ if (sensorName.empty())
+ {
+ std::stringstream ss;
+ ss << node->getName() << "_ForceTorqueSensor";
+ sensorName = ss.str();
+ }
+
+
+ // visu data
+ bool visuProcessed = false;
+ bool enableVisu = true;
+ bool useAsColModel = false;
+
+ VisualizationNodePtr visualizationNode;
+
+ Eigen::Matrix4f transformMatrix = Eigen::Matrix4f::Identity();
+
+ rapidxml::xml_node<>* nodeXML = sensorXMLNode->first_node();
+ while (nodeXML)
+ {
+ std::string nodeName = BaseIO::getLowerCase(nodeXML->name());
+// if (nodeName == "visualization")
+// {
+// if (loadMode==RobotIO::eFull)
+// {
+// THROW_VR_EXCEPTION_IF(visuProcessed, "Two visualization tags defined in Sensor '" << sensorName << "'." << endl);
+// visualizationNode = BaseIO::processVisualizationTag(nodeXML, sensorName, basePath, useAsColModel);
+// visuProcessed = true;
+// }// else silently ignore tag
+// } else if (nodeName == "transform")
+// {
+// BaseIO::processTransformNode(sensorXMLNode,sensorName,transformMatrix);
+// } else
+ {
+ THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in Sensor <" << sensorName << ">." << endl);
+ }
+
+ nodeXML = nodeXML->next_sibling();
+ }
+
+
+
+ SensorPtr Sensor(new ForceTorqueSensor(node, sensorName));
+
+ return Sensor;
+}
+
+
+/**
+ * register this class in the super class factory
+ */
+SensorFactory::SubClassRegistry ForceTorqueSensorFactory::registry(ForceTorqueSensorFactory::getName(), &ForceTorqueSensorFactory::createInstance);
+
+
+/**
+ * \return "position"
+ */
+std::string ForceTorqueSensorFactory::getName() {return "forcetorque";}
+
+
+/**
+ * \return new instance of ForceTorqueSensorFactory.
+ */
+boost::shared_ptr<SensorFactory> ForceTorqueSensorFactory::createInstance(void*)
+{
+ boost::shared_ptr<ForceTorqueSensorFactory> psFactory(new ForceTorqueSensorFactory());
+ return psFactory;
+}
+
+} // namespace VirtualRobot
diff --git a/VirtualRobot/Nodes/ForceTorqueSensorFactory.h b/VirtualRobot/Nodes/ForceTorqueSensorFactory.h
new file mode 100644
index 000000000..3ecddca07
--- /dev/null
+++ b/VirtualRobot/Nodes/ForceTorqueSensorFactory.h
@@ -0,0 +1,62 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox 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 Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package VirtualRobot
+* @author Nikolaus Vahrenkamp
+* @copyright 2013 Nikolaus Vahrenkamp
+* GNU Lesser General Public License
+*
+*/
+#ifndef _VirtualRobot_ForceTorqueSensorFactory_h_
+#define _VirtualRobot_ForceTorqueSensorFactory_h_
+
+#include "../VirtualRobotImportExport.h"
+#include "../SceneObject.h"
+#include "SensorFactory.h"
+
+#include <boost/shared_ptr.hpp>
+
+
+namespace VirtualRobot
+{
+class Sensor;
+
+class VIRTUAL_ROBOT_IMPORT_EXPORT ForceTorqueSensorFactory : public SensorFactory
+{
+public:
+ ForceTorqueSensorFactory();
+ virtual ~ForceTorqueSensorFactory();
+
+ //! Standard init method
+ virtual SensorPtr createSensor(RobotNodePtr node, const std::string& name, VisualizationNodePtr visualization = VisualizationNodePtr(),
+ const Eigen::Matrix4f &rnTrafo = Eigen::Matrix4f::Identity() ) const;
+
+ /*!
+ Create sensor from XML tag.
+ */
+ virtual SensorPtr createSensor(RobotNodePtr node, rapidxml::xml_node<char>* sensorXMLNode, RobotIO::RobotDescription loadMode = RobotIO::eFull, const std::string basePath = std::string() ) const;
+
+// AbstractFactoryMethod
+public:
+ static std::string getName();
+ static boost::shared_ptr<SensorFactory> createInstance(void*);
+private:
+ static SubClassRegistry registry;
+};
+
+} // namespace VirtualRobot
+
+#endif // _VirtualRobot_ForceTorqueSensorFactory_h_
diff --git a/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftArm.xml b/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftArm.xml
index 3718c0b6f..5f9bf2a6e 100644
--- a/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftArm.xml
+++ b/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftArm.xml
@@ -173,8 +173,9 @@
<CoM location="VisualizationBBoxCenter"/>
<Mass value="2.59945309" units="kg" />
<IgnoreCollision name="Underarm L"/>
+
</Physics>
-
+ <Sensor type="forcetorque"/>
<Child name="EndArmL"/>
</RobotNode>
diff --git a/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightArm.xml b/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightArm.xml
index 20e4f37f8..78e88f4c9 100644
--- a/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightArm.xml
+++ b/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightArm.xml
@@ -170,7 +170,7 @@
<Mass value="2.59945309" units="kg" />
<IgnoreCollision name="Underarm R"/>
</Physics>
-
+ <Sensor type="forcetorque"/>
<Child name="EndArmR"/>
</RobotNode>
--
GitLab