Add CoM logging to RobotLogger

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

SimDynamics/DynamicsEngine/BulletEngine/BulletRobotLogger.cpp

@@ -27,12 +27,15 @@ void BulletRobotLogger::writeToFile(const std::string& path)
 	output << "Timestamp" << ",";
 	for (int dof = 0; dof < num_dof; dof++)
 	{
-		std::string name = (*nodes)[dof]->getName();
+		std::string name = (*jointNodes)[dof]->getName();
 		output << "TargetAngle" << name << ",";
 		output << "ActualAngle" << name << ",";
 		output << "TargetVelocity" << name << ",";
 		output << "ActualVelocity" << name << ",";
 	}
+	output << "CoM X" << ",";
+	output << "CoM Y" << ",";
+	output << "CoM Z" << ",";
 	output << std::endl;
 
 	for (int frame = 0; frame < num_frames; frame++)
@@ -46,6 +49,9 @@ void BulletRobotLogger::writeToFile(const std::string& path)
 			output << targetVelocityLog[frame](dof) << ",";
 			output << actualVelocityLog[frame](dof) << ",";
 		}
+		output << actualCoMLog[frame].x() << ",";
+		output << actualCoMLog[frame].y() << ",";
+		output << actualCoMLog[frame].z() << ",";
 		output << std::endl;
 	}
 }
@@ -68,15 +74,15 @@ void BulletRobotLogger::log(btScalar dt)
 		return;
 	}
 
-	int dof = nodes->getSize();
+	int dof = jointNodes->getSize();
 	Eigen::VectorXf actualAngle(dof);
 	Eigen::VectorXf targetAngle(dof);
 	Eigen::VectorXf actualVelocity(dof);
 	Eigen::VectorXf targetVelocity(dof);
 
-	for (unsigned int i = 0; i < nodes->getSize(); i++)
+	for (unsigned int i = 0; i < jointNodes->getSize(); i++)
 	{
-		const VirtualRobot::RobotNodePtr& node = (*nodes)[i];
+		const VirtualRobot::RobotNodePtr& node = (*jointNodes)[i];
 		actualAngle(i)    = robot->getJointAngle(node);
 		// bullet changes the sign???
 		actualVelocity(i) = -robot->getJointSpeed(node);
@@ -84,6 +90,7 @@ void BulletRobotLogger::log(btScalar dt)
 		targetVelocity(i) = robot->getJointTargetSpeed(node);
 	}
 
+	actualCoMLog.push_back(bodyNodes->getCoM());
 	actualVelocityLog.push_back(actualVelocity);
 	actualAngleLog.push_back(actualAngle);
 	targetAngleLog.push_back(targetAngle);

SimDynamics/DynamicsEngine/BulletEngine/BulletRobotLogger.h

@@ -15,10 +15,14 @@ namespace SimDynamics
 class SIMDYNAMICS_IMPORT_EXPORT BulletRobotLogger
 {
 public:
-	BulletRobotLogger(BulletEnginePtr engine, const BulletRobotPtr robot, const VirtualRobot::RobotNodeSetPtr& nodes)
+	BulletRobotLogger(BulletEnginePtr engine,
+					  const BulletRobotPtr robot,
+					  const VirtualRobot::RobotNodeSetPtr& jointNodes,
+					  const VirtualRobot::RobotNodeSetPtr& bodyNodes)
 	: robot(robot)
 	, running(false)
-	, nodes(nodes)
+	, jointNodes(jointNodes)
+	, bodyNodes(bodyNodes)
 	, max_samples(1024 * 1024)
 	, timestamp(0.0f)
 	{
@@ -31,11 +35,13 @@ public:
 
 private:
 	const BulletRobotPtr robot;
-	VirtualRobot::RobotNodeSetPtr nodes;
+	VirtualRobot::RobotNodeSetPtr jointNodes;
+	VirtualRobot::RobotNodeSetPtr bodyNodes;
 	std::vector<Eigen::VectorXf> targetAngleLog;
 	std::vector<Eigen::VectorXf> targetVelocityLog;
 	std::vector<Eigen::VectorXf> actualAngleLog;
 	std::vector<Eigen::VectorXf> actualVelocityLog;
+	std::vector<Eigen::Vector3f> actualCoMLog;
 	std::vector<float> timestamps;
 	float timestamp;
 	bool running;