Input file for Force-Controlled Maneuver of Flexible Robot Arm

/* 
 *  =====================================================================
 *  Time-history Analysis of Force-driven Robot Arm with 2D geometrically
 *  exact rod finite element.
 * 
 *  This example is taken from Loc Vu Quoc's Ph.D. dissertaion.
 * 
 *  Written By: Mark Austin                                September 2000
 *  =====================================================================
 */ 

/* Setup problem specific parameters */

   NDimension         = 2;
   NDofPerNode        = 3;
   MaxNodesPerElement = 2;

   StartMesh();

/* Generate line of nodes */

   node = 1;
   AddNode(node, [  0 m,  0 m ] );
   node = 2;
   AddNode(node, [  2 m,  0 m ] );
   node = 3;
   AddNode(node, [  4 m,  0 m ] );
   node = 4;
   AddNode(node, [  6 m,  0 m ] );
   node = 5;
   AddNode(node, [  8 m,  0 m ] );
   node = 6;
   AddNode(node, [ 10 m,  0 m ] );

/* Attach beam elements to nodes */

   elmtno = 1;
   AddElmt( elmtno, [ 1, 2 ], "rodproperties");
   elmtno = 2;
   AddElmt( elmtno, [ 2, 3 ], "rodproperties");
   elmtno = 3;
   AddElmt( elmtno, [ 3, 4 ], "rodproperties");
   elmtno = 4;
   AddElmt( elmtno, [ 4, 5 ], "rodproperties");
   elmtno = 5;
   AddElmt( elmtno, [ 5, 6 ], "rodproperties");

/* Define section and material properties */

   ElementAttr("rodproperties") { type     = "GEXACT_2D";
                                  section  = "rodsection";
                                  material = "rodmaterial";
                                }

   SectionAttr("rodsection") { Izz       = 1 m^4;
                               area      = 0.1 m^2;
                             }

   MaterialAttr("rodmaterial") { density = 10000 kg/m^3;
                                 poisson = 0.25;
                                 E       = 5000 kPa;
                               }

/* Apply full-fixity to columns at foundation level */

   nodeno = 1;
   FixNode( nodeno, [ 1, 1, 0 ]);

/* Add external load to node 4 */

   NodeLoad( 1, [ 0.0 N, 0.0 N, 100000 N*m ]);

/* Compile and Print Finite Element Mesh */

   EndMesh();
   PrintMesh();

/* Compute mass, stiffness, and external load matrices */

   mass  = Mass( [1] );
   mass_inv = Inverse( mass );

   stiff = Stiff();
   eload = ExternalLoad();

   P_ext = eload;
   P_old = eload - eload;
   Fs_i  = P_ext - P_ext;

/* Setup initial displacement, velocity and acceleration */

   displ     = Solve( stiff, P_old );
   velocity  = displ/(1 sec);
   accel     = mass_inv*( P_ext - stiff*displ);
   PrintMatrix( accel );
   displ_old    = displ;
   displ_new    = displ;
   velocity_new = velocity;
   accel_new    = accel;

/* Setup parameters for time-history analysis */

   nsteps  = 60;
   dt      = 0.1 sec;
   no_node = 6;

/* =============================================================== */
/* Allocate memory for displacment response : store every 0.5 secs */
/* =============================================================== */

   ntime = nsteps*dt/(0.5 sec);

   xResponse = Matrix( [ ntime+1, no_node ]);
   yResponse = Matrix( [ ntime+1, no_node ]);
   for(i = 1; i <= no_node; i = i + 1) {
       xResponse = ColumnUnits( xResponse, [m], [i] );
       yResponse = ColumnUnits( yResponse, [m], [i] );
   }

/* Save initial displacement in response vector */

   for(j = 1; j <= no_node; j = j + 1) {
       coord = GetCoord( [j] );
       xResponse[1][j] = coord [1][1];
       yResponse[1][j] = coord [1][2];
   }

/* Main loop for Newmark Analysis */

   for(i = 1; i <= nsteps; i = i + 1) {

       time = i*dt;
       print "\n";
       print "*** Start at step : ", i ," : time = ", time, "\n";

       /* Compute vector of external loads */

       if( time <= 5 sec ) then {
           P_ext = eload;
       } else {
           eload[1][1] = 0.0 N*m;
           P_ext = eload;
       }

       /* Compute effective loading increment : dPeff */

       dPeff = P_ext-P_old + ((4/dt)*mass)*velocity + 2*mass*accel;

       /* While loop to check converge : Keff*U = dPeff */

       tol = 0.0000001;
       dp  = displ - displ;
       err = 1 + tol;

       ii = 1;
       while( err > tol ) {

          /* Compute effective stiffness from tangent stiffness */

          Keff = stiff + (4/dt/dt)*mass;

          /* Solve for d_displacement, d_velocity */

          dp_i = Solve( Keff, dPeff);
          dp = dp + dp_i;
          dv = (2/dt)*dp - 2*velocity;

          /* Compute displacement, velocity */

          displ_new    = displ + dp;
          velocity_new = velocity + dv;

         /* Compute incremental displacement/internal load using old stiffness */

          dFs = stiff*dp_i;
          Fs_i = Fs_i + dFs;
          if( ii==1 ) {
             x = L2Norm(dFs);
             if( x==0 ) {x=1;}
          }

          /* Save displacement to database      */

          ElmtStateDet( displ_new );
          UpdateResponse();

          /* Compute new tangent stiffness      */

          stiff = Stiff();

          /* Compute new internal load and damping force, */

          Fs = InternalLoad( displ_new );

          /* Calculate the unbalance force and error percentage */

          P_err = Fs_i - Fs;
          y = L2Norm(P_err);
          err = y/x;

          /* Assign new effective incremental load */

          dPeff = P_err;
          Fs_i  = Fs;

          ii = ii+1;
          if( ii > 20 ) {
              flag = 1;  err = tol;
          }
       }

       print "*** ---------------------------------\n";
       print "*** Nonlinear Iterations = ",ii-1,  "\n";
       print "***                  err = ", err,  "\n";
       print "*** ---------------------------------\n";

       /* Compute new acceleration */

       accel_new = mass_inv*( P_ext - Fs );

       /* Update histories for this time step */

       ElmtStateDet( displ_new );
       UpdateResponse();

       P_old    = P_ext;
       Ps_int   = Fs;
       accel    = accel_new;
       velocity = velocity_new;
       displ    = displ_new;

       print "\n";
       print "Coordinates of Displaced Rod\n";
       print "============================\n\n";

       for (ii = 1; ii <= no_node; ii = ii + 1 ) {
            coord     = GetCoord([ii]);
            nodal_dof = GetDof([ii]);

            if( nodal_dof[1][1] > 0 ) {
                coord[1][1] = coord[1][1] + displ[ nodal_dof[1][1] ][1];
            }
            if( nodal_dof[1][2] > 0 ) {
                coord[1][2] = coord[1][2] + displ[ nodal_dof[1][2] ][1];
            }

            print "Node : ", ii, " x = " ,coord [1][1], " y = ", coord[1][2], "\n";

            if(i%5 == 0) {
               xResponse[ i/5 + 1 ][ ii ] = coord [1][1];
               yResponse[ i/5 + 1 ][ ii ] = coord [1][2];
            }
      }
   }

   /* Summary of time-history response */

   PrintMatrix(xResponse);
   PrintMatrix(yResponse);

   quit;