Added forceTorwue feedback to Bullet implementation.

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

SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp

@@ -1280,4 +1280,49 @@ void BulletRobot::setPoseNonActuatedRobotNodes()
     }
 }
 
+void BulletRobot::enableForceTorqueFeedback( const LinkInfo& link )
+{
+	if (!link.joint->needsFeedback())
+	{
+		link.joint->enableFeedback(true);
+		btJointFeedback* feedback = new btJointFeedback;
+		feedback->m_appliedForceBodyA = btVector3(0, 0, 0);
+		feedback->m_appliedForceBodyB = btVector3(0, 0, 0);
+		feedback->m_appliedTorqueBodyA = btVector3(0, 0, 0);
+		feedback->m_appliedTorqueBodyB = btVector3(0, 0, 0);
+		link.joint->setJointFeedback(feedback);
+	}
+}
+
+Eigen::VectorXf BulletRobot::getForceTorqueFeedbackA( const LinkInfo& link )
+{
+	Eigen::VectorXf r(6);
+	r.setZero();
+	if (!link.joint || !link.joint->needsFeedback())
+	{
+		return r;
+	}
+
+	btJointFeedback* feedback = link.joint->getJointFeedback();
+	if (!feedback)
+		return r;
+	r << feedback->m_appliedForceBodyA[0],feedback->m_appliedForceBodyA[1],feedback->m_appliedForceBodyA[2],feedback->m_appliedTorqueBodyA[0],feedback->m_appliedTorqueBodyA[1],feedback->m_appliedTorqueBodyA[2];
+	return r;
+}
+Eigen::VectorXf BulletRobot::getForceTorqueFeedbackB( const LinkInfo& link )
+{
+	Eigen::VectorXf r(6);
+	r.setZero();
+	if (!link.joint || !link.joint->needsFeedback())
+	{
+		return r;
+	}
+
+	btJointFeedback* feedback = link.joint->getJointFeedback();
+	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;
+}
+
 } // namespace VirtualRobot

SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h

@@ -58,6 +58,10 @@ public:
 		float jointValueOffset; // offset simox -> bullet joint values
 	};
 
+
+	void enableForceTorqueFeedback(const LinkInfo& link);
+	Eigen::VectorXf getForceTorqueFeedbackA(const LinkInfo& link);
+	Eigen::VectorXf getForceTorqueFeedbackB(const LinkInfo& link);
 	
 	// We do not allow to re-adjust the robot. 
 	// The position of the robot is queried once on construction.