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; }