void chain3DOF2( model &M, int nlinks) { cout << "Creating a chain3DOF2 model" << endl; // make a chain model double m = 0.0; // mass of links double L = 1.0; // length of each chain link int N = 3*nlinks; // define N, parent, pitch, Xtree, I M.setN(N); // deifne size M.setParent( linspace<irowvec>( 0, N-1, N) ); // define vector (3D pos) argument for xtree vec com; vec inertia; RBbox* box1 = new RBbox( m, L, L/5, L/5); RBnull* null1 = new RBnull(); M.setXtree(0, xlt( zeros<vec>(3) ) ); vec r(3); r(0) = L; for (int i = 0; i < N-2; i+=3) { if ( i != 0 ) { M.setXtree(0+i, xlt( r ) ); } M.setXtree(1+i, xlt( zeros<vec>(3) ) ); M.setXtree(2+i, xlt( zeros<vec>(3) ) ); // m = 0.0; com = zeros<vec>(3); inertia = zeros<vec>(3); M.setI(0+i, mcI( m, com, diagmat(inertia)) ); M.setI(1+i, mcI( m, com, diagmat(inertia)) ); com(0) = L/2.0; m = 1.0; inertia = m*pow(L,2)/12.0 * ones<vec>(3); inertia(0) = 0.01; M.setI(2+i, mcI( m, com, diagmat(inertia)) ); // M.setRB( 0+i, null1); M.setRB( 1+i, null1); M.setRB( 2+i, box1); M.setPitch(i+0, 0); M.setPitch(i+1, 1); M.setPitch(i+2, 2); } // define pitch }
void chain3DOF( model &M) { cout << "Creating a chain3DOF model" << endl; // make a chain model double m = 0.0; // mass of links double L = 1.0; // length of each chain link int N = 3; // define N, parent, pitch, Xtree, I M.setN(N); // deifne size M.setParent( linspace<irowvec>( 0, N-1, N) ); // define vector (3D pos) argument for xtree M.setXtree(0, xlt( zeros<vec>(3) ) ); M.setXtree(1, rotx( -math::pi()/2 ) ); M.setXtree(2, roty( math::pi()/2 ) ); // cdefine center of mass 3D pos vector vec com = zeros<vec>(3); vec inertia = zeros<vec>(3); M.setI(0, mcI( m, com, diagmat(inertia)) ); M.setI(1, mcI( m, com, diagmat(inertia)) ); com(0) = L/2.0; m = 1.0; // define inertia inertia = m*pow(L,2)/12.0 * ones<vec>(3); inertia(0) = 0.01; // no thickness in this direction M.setI(2, mcI( m, com, diagmat(inertia)) ); // RIGID BODY RBbox* box1 = new RBbox( m, L, L/5, L/5); RBnull* null1 = new RBnull(); M.setRB( 0, null1); M.setRB( 1, null1); M.setRB( 2, box1); // define pitch M.setPitch( 2*ones<rowvec>(N) ); /* M.setXtree(0, xlt(zeros<vec>(3))); M.setXtree(1, rotx( math::pi()/2) ); M.setXtree(2, roty( math::pi()/2) ); */ }
void chain( model &M, int N) { cout << "Creating a chain model" << endl; // make a chain model double m = 1.0; // mass of links double L = 1.0; // length of each chain link // define N, parent, pitch, Xtree, I M.setN(N); // deifne size if (N == 1) { M.setParent( zeros<irowvec>(1) ); } else { M.setParent( linspace<irowvec>( 0, N-1, N) ); } // define vector (3D pos) argument for xtree vec r = zeros<vec>(3); M.setXtree( 0, xlt(r) ); // cdefine center of mass 3D pos vector vec com = zeros<vec>(3); com(0) = L/2.0; // define inertia vec inertia = m*pow(L,2)/12.0 * ones<vec>(3); inertia(0) = 0.01; // no thickness in this direction M.setI(0, mcI( m, com, diagmat(inertia)) ); // RIGID BODY RBbox* box1 = new RBbox( m, L, L/5, L/5); /* char* torus = const_cast<char*>("torus.obj"); RBobj* box1 = new RBobj( torus ); box1->normalize(); */ M.setRB( 0, box1); r(0) = L; // for the rest of the links // fill in the fields for (int i = 1; i < N; i++) { M.setXtree( i, xlt(r) ); M.setI( i, mcI( m, com, diagmat(inertia)) ); M.setRB( i, box1); } // define pitch M.setPitch( 2*ones<rowvec>(N) ); }
void chainrot( model &M, int nlinks) { cout << "Creating a chainrot model" << endl; // make a chain model int N = nlinks + 1; double m = 1.0; // mass of links double L = 1.0; // length of each chain link // define N, parent, pitch, Xtree, I M.setN(N); // deifne size M.setParent( linspace<irowvec>( 0, N-1, N) ); // define vector (3D pos) argument for xtree vec r = zeros<vec>(3); // RIGID BODY RBnull* null1 = new RBnull(); RBbox* box1 = new RBbox( m, L, L/6, L/6); // cdefine center of mass 3D pos vector vec com = zeros<vec>(3); com(0) = L/2.0; // define inertia vec inertia = m*pow(L,2)/12.0 * ones<vec>(3); inertia(0) = 0.01; // no thickness in this direction M.setXtree( 0, xlt(r) ); M.setI(0, mcI( 0, zeros<vec>(3), diagmat(zeros<vec>(3) ) ) ); M.setRB( 0, null1); M.setXtree( 1, xlt(r) ); M.setI(1, mcI( m, com, diagmat(inertia)) ); M.setRB( 1, box1); r(0) = L; // for the rest of the links // fill in the fields for (int i = 2; i < N; i++) { M.setXtree( i, xlt(r) ); M.setI( i, mcI( m, com, diagmat(inertia)) ); M.setRB( i, box1); } // define pitch rowvec pitch = 2*ones<rowvec>(N); pitch(0) = 1; M.setPitch( pitch ); }