void TestNewtonEuler() { RobotDynamics3D robot; for(int n=2;n<10;n++) { robot.Initialize(n); MakePlanarChain(robot,n,One); Assert(robot.q.n == n); Assert(robot.dq.n == n); robot.q.setZero(); robot.dq.setZero(); for(int i=0;i<n;i++) { robot.q(i) = Rand(-1.0,1.0); //robot.q(i) = Pi/Pow(2.0,i+1); } //robot.q(0) = 0; //robot.q(1) = Pi/4.0; for(int i=0;i<n;i++) { //robot.dq(i) = Rand(-1.0,1.0); robot.dq(i) = Rand(-10.0,10.0); //robot.dq(i) = 1.0/(i+1.0); } //robot.dq(0) = 1; //robot.dq(1) = 1.0/3.0; robot.UpdateFrames(); robot.UpdateDynamics(); NewtonEulerSolver ne(robot); //ne.SetGravityWrenches(Vector3(0,0,-9.8)); ne.SelfTest(); } }