void RobotDynamics3D::Subset(const RobotDynamics3D& robot,const std::vector<int>& subset) { RobotDynamics3D::Subset(robot,subset); for(size_t i=0;i<subset.size();i++) { dq(i) = robot.dq(subset[i]); torqueMax(i) = robot.torqueMax(subset[i]); velMin(i) = robot.velMin(subset[i]); velMax(i) = robot.velMax(subset[i]); powerMax(i) = robot.powerMax(subset[i]); } }
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(); } }