Ejemplos de Skeleton::getPositions en C++ (Cpp)

Ejemplo n.º 1

Archivo: WorldConstructor.cpp Proyecto: dtbinh/bioGP

void WorldConstructor::Debug()
{
	return;
	// Create a humanoid controller
	//msHumanoid = new bioloidgp::robot::HumanoidController(robot, msWorld->getConstraintSolver());
	DartLoader urdfLoader;
	Skeleton* robot
		= urdfLoader.parseSkeleton(
		DATA_DIR"/urdf/BioloidGP/BioloidGP.URDF");
	robot->enableSelfCollision();
	msWorld->withdrawSkeleton(msHumanoid->robot());
	msWorld->addSkeleton(robot);

	msHumanoid->reset();

	Skeleton* tmp = msHumanoid->robot();
	msHumanoid->robot() = robot;
	msHumanoid->setJointDamping(0.0);
	msHumanoid->reset();

	Eigen::VectorXd qOrig = tmp->getPositions();
	Eigen::VectorXd q = msHumanoid->robot()->getPositions();
	Eigen::VectorXd err = qOrig - q;

	Eigen::VectorXd qDotOrig = tmp->getVelocities();
	Eigen::VectorXd qDot = msHumanoid->robot()->getVelocities();
	Eigen::VectorXd errDot = qDotOrig - qDot;


}