Exemplos de RobotDynamics3D::Initialize em C++ (Cpp)

Exemplo n.º 1

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