Return axis torque as vector

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

VirtualRobot/Nodes/ForceTorqueSensor.cpp

@@ -34,15 +34,16 @@ const Eigen::VectorXf &ForceTorqueSensor::getForceTorque()
     return forceTorqueValues;
 }
 
-double ForceTorqueSensor::getAxisTorque()
+Eigen::Vector3f 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);
+    Eigen::Vector3f zAxis = rn->getGlobalPose().block(0, 2, 3, 1);
+	Eigen::Vector3f axisTorque = (torqueVector.dot(zAxis)) * zAxis;
 
-	return torque;
+	return axisTorque;
 }
 
 

VirtualRobot/Nodes/ForceTorqueSensor.h

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