Added contact sensor

git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@639 042f3d55-54a8-47e9-b7fb-15903f145c44

SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp

@@ -395,8 +395,8 @@ void BulletEngine::updateRobots(btScalar timeStep)
 {
 	for (size_t i=0; i < robots.size();i++)
 	{
-		robots[i]->actuateJoints(double(timeStep));
-		robots[i]->updateSensors();
+		robots[i]->actuateJoints(static_cast<double>(timeStep));
+		robots[i]->updateSensors(static_cast<double>(timeStep));
 	}
 }
 

SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp

@@ -11,12 +11,14 @@
 #include <VirtualRobot/RobotNodeSet.h>
 #include <VirtualRobot/Nodes/RobotNodeRevolute.h>
 #include <VirtualRobot/Nodes/ForceTorqueSensor.h>
+#include <VirtualRobot/Nodes/ContactSensor.h>
 
 // either hinge or generic6DOF constraints can be used
 //#define USE_BULLET_GENERIC_6DOF_CONSTRAINT
 
-
 #include <boost/pointer_cast.hpp>
+#include <boost/unordered_set.hpp>
+#include <boost/unordered_map.hpp>
 
 //#define DEBUG_FIXED_OBJECTS
 //#define DEBUG_SHOW_LINKS
@@ -36,11 +38,10 @@ BulletRobot::BulletRobot(VirtualRobot::RobotPtr rob, bool enableJointMotors)
     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());
+            VirtualRobot::RobotNodePtr node = ftSensor->getRobotNode();
             THROW_VR_EXCEPTION_IF(!node, "parent of sensor could not be casted to RobotNode")
 
             const LinkInfo& link = getLink(node);
@@ -967,23 +968,75 @@ void BulletRobot::actuateJoints(double dt)
     setPoseNonActuatedRobotNodes();
 }
 
-void BulletRobot::updateSensors()
+void BulletRobot::updateSensors(double dt)
 {
-    std::vector<SensorPtr>::iterator it = sensors.begin();
-    for(; it != sensors.end(); it++)
+    boost::unordered_set<std::string> contactObjectNames;
+    // this seems stupid and it is, but that is abstract interfaces for you.
+    for(std::vector<SensorPtr>::iterator it = sensors.begin(); it != sensors.end(); it++)
+    {
+        ContactSensorPtr contactSensor = boost::dynamic_pointer_cast<ContactSensor>(*it);
+        if (contactSensor)
+        {
+            contactObjectNames.insert(contactSensor->getRobotNode()->getName());
+        }
+    }
+
+    DynamicsWorldPtr world = DynamicsWorld::GetWorld();
+    std::vector<SimDynamics::DynamicsEngine::DynamicsContactInfo> contacts = world->getEngine()->getContacts();
+    boost::unordered_map<std::string, VirtualRobot::ContactSensor::ContactFrame> frameMap;
+    for (std::vector<SimDynamics::DynamicsEngine::DynamicsContactInfo>::iterator it = contacts.begin();
+         it != contacts.end(); it++)
+    {
+        const SimDynamics::DynamicsEngine::DynamicsContactInfo& contact = *it;
+
+        float sign;
+        std::string key;
+        if (contactObjectNames.find(contact.objectA->getName()) != contactObjectNames.end())
+        {
+            sign = 1.0f;
+            key = contact.objectA->getName();
+        }
+        else if (contactObjectNames.find(contact.objectB->getName()) != contactObjectNames.end())
+        {
+            sign = -1.0f;
+            key = contact.objectB->getName();
+        }
+        else
+        {
+            continue;
+        }
+
+        VirtualRobot::ContactSensor::ContactFrame& frame = frameMap[key];
+        double zForce = sign * contact.normalGlobalB.z() * contact.appliedImpulse;
+        VirtualRobot::ContactSensor::ContactForce cf;
+        cf.contactPoint = contact.posGlobalB;
+        cf.zForce = zForce;
+        frame.forces.push_back(cf);
+    }
+
+    // Update forces and troques
+    for(std::vector<SensorPtr>::iterator it = sensors.begin(); 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;
+        }
+        else
+        {
+            ContactSensorPtr contactSensor = boost::dynamic_pointer_cast<ContactSensor>(*it);
+            if (contactSensor)
+            {
+                VirtualRobot::RobotNodePtr node = contactSensor->getRobotNode();
+                THROW_VR_EXCEPTION_IF(!node, "parent of sensor could not be casted to RobotNode")
+                const VirtualRobot::ContactSensor::ContactFrame& frame = frameMap[node->getName()];
+                contactSensor->updateSensors(frame, dt);
+            }
         }
     }
 }

SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h

@@ -106,8 +106,7 @@ public:
 		\param dt Timestep
 	*/
 	virtual void actuateJoints(double dt);
-
-    virtual void updateSensors();
+    virtual void updateSensors(double dt);
 
 	virtual double getJointAngle(VirtualRobot::RobotNodePtr rn);
     virtual double getJointSpeed(VirtualRobot::RobotNodePtr rn);

SimDynamics/DynamicsEngine/DynamicsRobot.h

@@ -80,7 +80,7 @@ public:
 		\param dt Timestep
 	*/
     virtual void actuateJoints(double dt);
-    virtual void updateSensors(){}
+    virtual void updateSensors(double dt){}
 
 	// experimental...
 	virtual void ensureKinematicConstraints();

VirtualRobot/CMakeLists.txt

@@ -44,9 +44,11 @@ Nodes/RobotNodeActuator.cpp
 Nodes/Sensor.cpp
 Nodes/PositionSensor.cpp
 Nodes/ForceTorqueSensor.cpp
+Nodes/ContactSensor.cpp
 Nodes/CameraSensor.cpp
 Nodes/PositionSensorFactory.cpp
 Nodes/ForceTorqueSensorFactory.cpp
+Nodes/ContactSensorFactory.cpp
 Nodes/CameraSensorFactory.cpp
 Visualization/Visualization.cpp
 Visualization/VisualizationNode.cpp

VirtualRobot/Nodes/ContactSensor.cpp

+
+#include "ContactSensor.h"
+#include "ContactSensorFactory.h"
+#include "../XML/BaseIO.h"
+
+using namespace boost;
+
+namespace VirtualRobot {
+
+ContactSensor::ContactSensor(RobotNodeWeakPtr robotNode,
+							 const std::string &name)
+: Sensor(robotNode, name)
+, timestamp(0.0)
+{
+}
+
+
+ContactSensor::~ContactSensor()
+{
+}
+
+void ContactSensor::print( bool printChildren, bool printDecoration ) const
+{
+	if (printDecoration)
+		cout << "******** ContactSensor ********" << endl;
+	Sensor::print(printChildren,false);
+}
+
+
+SensorPtr ContactSensor::_clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr visualizationModel, float scaling)
+{
+	SensorPtr result(new ContactSensor(newRobotNode,name));
+	return result;
+}
+
+std::string ContactSensor::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='" << ContactSensorFactory::getName() << "'/>" << endl;
+    std::string pre2 = pre + t;
+
+    return ss.str();
+}
+
+void ContactSensor::updateSensors(const ContactSensor::ContactFrame& frame, double dt)
+{
+    this->frame = frame;
+    timestamp += dt;
+}
+
+} // namespace VirtualRobot

VirtualRobot/Nodes/ContactSensor.h

+/**
+* 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     Patrick Niklaus
+* @copyright  2014 Patrick Niklaus
+*             GNU Lesser General Public License
+*
+*/
+#ifndef _VirtualRobot_ContactSensor_h_
+#define _VirtualRobot_ContactSensor_h_
+
+#include "Sensor.h"
+
+namespace VirtualRobot
+{
+
+class ContactSensor;
+typedef boost::shared_ptr<ContactSensor> ContactSensorPtr;
+
+/*!
+ * 	This ia a contact sensor that reports all contact points with other objects.
+*/
+class VIRTUAL_ROBOT_IMPORT_EXPORT ContactSensor : public Sensor
+{
+public:
+	friend class Robot;
+	friend class RobotIO;
+
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+	struct ContactForce
+	{
+		Eigen::Vector3f contactPoint;
+		double zForce;
+
+		EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	};
+	struct ContactFrame
+	{
+		std::vector<ContactForce> forces;
+	};
+
+	/*!
+	  Constructor with settings.
+	  */
+	ContactSensor(RobotNodeWeakPtr robotNode, const std::string &name);
+
+	/*!
+	*/
+	virtual ~ContactSensor();
+
+	/*!
+	 * Set the new contact frame for this node.
+	 */
+	void updateSensors(const ContactSensor::ContactFrame& frame, double dt);
+
+	/*!
+	 * Returns contact frame
+	 */
+	const ContactSensor::ContactFrame& getContacts() { return frame; }
+
+	/*!
+	 * Returns true of this node as contact with another object.
+	 */
+	bool hasContact()
+    {
+        return (frame.forces.size() > 0);
+    }
+
+	/*!
+	  Print status information.
+	  */
+	virtual void print(bool printChildren = false, bool printDecoration = true) const;
+
+
+	virtual std::string toXML(const std::string &modelPath, int tabs);
+
+protected:
+
+	ContactSensor(){}
+
+	/*!
+	Derived classes must implement their clone method here.
+	*/
+	virtual SensorPtr _clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr visualizationModel, float scaling);
+
+	ContactFrame frame;
+    double timestamp;
+};
+
+} // namespace VirtualRobot
+
+#endif // _VirtualRobot_ForceTorqueSensor_h_

VirtualRobot/Nodes/ContactSensorFactory.cpp

+
+
+#include "ContactSensorFactory.h"
+#include "Sensor.h"
+#include "../XML/BaseIO.h"
+#include "../XML/rapidxml.hpp"
+#include "ContactSensor.h"
+
+
+namespace VirtualRobot {
+
+ContactSensorFactory::ContactSensorFactory()
+{
+}
+
+
+ContactSensorFactory::~ContactSensorFactory()
+{
+}
+
+
+/**
+ * This method creates a VirtualRobot::ContactSensor.
+ *
+ * \return instance of VirtualRobot::ContactSensor.
+ */
+SensorPtr ContactSensorFactory::createSensor(RobotNodePtr node, const std::string& name, VisualizationNodePtr visualization,
+											  const Eigen::Matrix4f &rnTrafo) const
+{
+    SensorPtr Sensor(new ContactSensor(node, name));
+
+	return Sensor;
+}
+
+SensorPtr ContactSensorFactory::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() << "_ContactSensor";
+		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());
+		{
+			THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in Sensor <" << sensorName << ">." << endl);
+		}
+
+		nodeXML = nodeXML->next_sibling();
+	}
+
+    SensorPtr Sensor(new ContactSensor(node, sensorName));
+
+	return Sensor;
+}
+
+
+/**
+ * register this class in the super class factory
+ */
+SensorFactory::SubClassRegistry ContactSensorFactory::registry(ContactSensorFactory::getName(), &ContactSensorFactory::createInstance);
+
+
+/**
+ * \return "contact"
+ */
+std::string ContactSensorFactory::getName() {return "contact";}
+
+
+/**
+ * \return new instance of ContactSensorFactory.
+ */
+boost::shared_ptr<SensorFactory> ContactSensorFactory::createInstance(void*)
+{
+    boost::shared_ptr<ContactSensorFactory> psFactory(new ContactSensorFactory());
+    return psFactory;
+}
+
+} // namespace VirtualRobot

VirtualRobot/Nodes/ContactSensorFactory.h

+/**
+* 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_ContactSensorFactory_h_
+#define _VirtualRobot_ContactSensorFactory_h_
+
+#include "../VirtualRobotImportExport.h"
+#include "../SceneObject.h"
+#include "SensorFactory.h"
+
+#include <boost/shared_ptr.hpp>
+
+
+namespace VirtualRobot
+{
+class Sensor;
+
+class VIRTUAL_ROBOT_IMPORT_EXPORT ContactSensorFactory  : public SensorFactory
+{
+public:
+	ContactSensorFactory();
+	virtual ~ContactSensorFactory();
+
+	//! 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_ContactSensorFactory_h_

VirtualRobot/VirtualRobot.h

@@ -178,6 +178,7 @@ namespace VirtualRobot
 	class WorkspaceGrid;
     class WorkspaceDataArray;
 	class ForceTorqueSensor;
+	class ContactSensor;
 
     typedef boost::shared_ptr<RobotNode> RobotNodePtr;
     typedef boost::shared_ptr<RobotNodeSet> RobotNodeSetPtr;
@@ -215,6 +216,7 @@ namespace VirtualRobot
 	typedef boost::shared_ptr<BasicGraspQualityMeasure> BasicGraspQualityMeasurePtr;
 	typedef boost::shared_ptr<WorkspaceGrid> WorkspaceGridPtr;
 	typedef boost::shared_ptr<ForceTorqueSensor> ForceTorqueSensorPtr;
+	typedef boost::shared_ptr<ContactSensor> ContactSensorPtr;
 
 
 #define VR_INFO std::cout <<__FILE__ << ":" << __LINE__ << ": "