def simulate(state,tspan,nbodies,bodiesGEBF,bodiesRigid,joints,BC1,BC2): ''' This function extracts the generalized coordinates from the solution of the equations of motion after calling the DCA to solve the equations of motion ''' # only pin joints connecting rigid bodies q = state[:nRIGID*ndofsRigid + nGEBF*ndofsGEBF] u = state[nRIGID*ndofsRigid + nGEBF*ndofsGEBF:] # slice out rigid generalized coordinates and speeds from GEBF qRigid = q[:nRIGID*ndofsRigid] uRigid = u[:nRIGID*ndofsRigid] qGEBF = q[nRIGID*ndofsRigid:] uGEBF = u[nRIGID*ndofsRigid:] # Get the Kinematics 2D n-link Pendulum kinematicsRigid2D(bodiesRigid,qRigid,uRigid) kinematicsGEBF2D(bodiesGEBF,qGEBF,uGEBF) # slice out generalized cooridnates # and make a sublist for each body # these u's are not generalized speeds these are the displacements of the nodes u = qGEBF.tolist() u = [u[i*ndofsGEBF:(i*ndofsGEBF)+ndofsGEBF] for i in range((int(len(u)/ndofsGEBF)))] # 'pop' off the rotational coordinates # those are dealt with through the 'kinematic' sweep for ue in u: ue.pop(3) ue.pop(0) # compute the inverse inertial properties of the body # with the updated generalized for body,ue in zip(bodiesGEBF,u): body.intProps('gebf',ue) for body in bodiesRigid: body.intProps('rigid') # join the lists of bodies for a total list # order matters bodies = bodiesRigid + bodiesGEBF # Call the Recursive DCA Algorithm # This returns a list of the form: # [A11,A12,A21,A22,...,An1,An2] # where Axy corresponds to the acceleration # of the yth handle of the xth body accel = DCA.solve(nbodies,0,bodies,joints,BC1,BC2) # compute the generalized accelerations for rigid bodies state_dot = get_gen_accel_Rigid(nbodies, joints, state, accel) return state_dot
def simulate(state,tspan,nbodies,bodiesGEBF,bodiesRigid,joints,BC1,BC2): ''' This function extracts the generalized coordinates from the solution of the equations of motion after calling the DCA to solve the equations of motion ''' # only pin joints connecting rigid bodies q = state[:nRIGID*ndofsRigid + nGEBF*ndofsGEBF] u = state[nRIGID*ndofsRigid + nGEBF*ndofsGEBF:] # slice out rigid generalized coordinates and speeds from GEBF qRigid = q[:nRIGID*ndofsRigid] uRigid = u[:nRIGID*ndofsRigid] qGEBF = q[nRIGID*ndofsRigid:] uGEBF = u[nRIGID*ndofsRigid:] # Update the Kinematics 2D n-link Pendulum # Now that these functions "woe # This should be a class function for each body type not an absolute function # e.g., [body.updateKin for body in bodies] MBF.kinematics_Rigid2D(bodiesRigid,qRigid,uRigid) # MBF.kinematics_GEBF2D(bodiesGEBF,qGEBF,uGEBF) # # slice out generalized cooridnates # # and make a sublist for each body # # these u's are not generalized speeds these are the displacements of the nodes # u = qGEBF.tolist() # u = [u[i*ndofsGEBF:(i*ndofsGEBF)+ndofsGEBF] for i in range((int(len(u)/ndofsGEBF)))] # # 'pop' off the rotational coordinates # # those are dealt with through the 'kinematic' sweep # for ue in u: # ue.pop(3) # ue.pop(0) # compute the inverse inertial properties of the body # with the updated generalized speeds for body,q,u in zip(bodiesGEBF, np.array_split(qGEBF,nGEBF), np.array_split(uGEBF,nGEBF)): body.intProps('gebf', q, u) # Mass-matricies constant (don't need to pass generalized coords) for body in bodiesRigid: body.intProps('rigid') # join the lists of bodies for a total list # order matters bodies = bodiesRigid + bodiesGEBF # Call the Recursive DCA Algorithm # This returns a list of the form: # [A11,A12,A21,A22,...,An1,An2] # where Axy corresponds to the acceleration # of the yth handle of the xth body accel = DCA.solve(nbodies,0,bodies,joints,BC1,BC2) accelRigid = accel[:2*len(bodiesRigid)] accelGEBF = accel[2*len(bodiesRigid):] # compute the generalized accelerations for kinematic joints udot_Rigid = MBF.get_gen_accel_Rigid(len(bodiesRigid), joints, accelRigid) udot_GEBF = MBF.get_gen_accel_GEBF(len(bodiesGEBF), accelGEBF) udot = np.hstack((udot_Rigid,udot_GEBF)) state_dot = np.hstack((state[nRIGID*ndofsRigid + nGEBF*ndofsGEBF:],udot)) return state_dot