Add axsis torque to ForceTorqueSensor

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

VirtualRobot/Nodes/ForceTorqueSensor.cpp

@@ -24,6 +24,17 @@ const Eigen::VectorXf &ForceTorqueSensor::getForceTorque()
     return forceTorqueValues;
 }
 
+double ForceTorqueSensor::getAxisTorque()
+{
+	Eigen::Vector3f torqueVector = forceTorqueValues.tail(3);
+
+	// project onto joint axis
+	RobotNodePtr rn(robotNode);
+	double torque = (torqueVector.adjoint() * rn->getGlobalPose().block(0, 2, 3, 1))(0, 0);
+
+	return torque;
+}
+
 
 
 void ForceTorqueSensor::print( bool printChildren, bool printDecoration ) const

VirtualRobot/Nodes/ForceTorqueSensor.h

@@ -63,6 +63,11 @@ public:
     const Eigen::Vector3f &getForce();
     const Eigen::Vector3f &getTorque();
 
+	/**
+	 * Projects torque on joint axis and returns amplitude.
+	 */
+	double getAxisTorque();
+
 	/*!
 		Print status information.
 	*/

VirtualRobot/VirtualRobot.h

@@ -177,6 +177,7 @@ namespace VirtualRobot
 	class BasicGraspQualityMeasure;
 	class WorkspaceGrid;
     class WorkspaceDataArray;
+	class ForceTorqueSensor;
 
     typedef boost::shared_ptr<RobotNode> RobotNodePtr;
     typedef boost::shared_ptr<RobotNodeSet> RobotNodeSetPtr;
@@ -213,6 +214,7 @@ namespace VirtualRobot
 	typedef boost::shared_ptr<SphereApproximator> SphereApproximatorPtr;
 	typedef boost::shared_ptr<BasicGraspQualityMeasure> BasicGraspQualityMeasurePtr;
 	typedef boost::shared_ptr<WorkspaceGrid> WorkspaceGridPtr;
+	typedef boost::shared_ptr<ForceTorqueSensor> ForceTorqueSensorPtr;
 
 
 #define VR_INFO std::cout <<__FILE__ << ":" << __LINE__ << ": "