stretch = q - q0 system.add_spring_force1(k, (stretch) * N.z, wNA) system.addforce(-b * v2, v2) system.addforcegravity(-g * N.y) pos = sympy.cos(system.t * 2 * pi / 2) eq = pos * N.x - p1 eq_d = eq.time_derivative() eq_dd = eq_d.time_derivative() eq_dd_scalar = [] eq_dd_scalar.append(eq_dd.dot(N.x)) system.add_constraint(AccelerationConstraint(eq_dd_scalar)) f, ma = system.getdynamics() func1, lambda1 = system.state_space_post_invert( f, ma, constants=system.constant_values, return_lambda=True) states = pynamics.integration.integrate_odeint(func1, ini, t, rtol=tol, atol=tol, args=({ 'constants': {}, 'alpha': 1e2, 'beta': 1e1 }, )) lambda1_n = numpy.array([lambda1(tt, ss) for tt, ss in zip(t, states)]) plt.figure()
def Cal_system(initial_states, drag_direction, tinitial, tstep, tfinal, fit_vel, f1, f2): g_k, g_b_damping, g_b_damping1 = [0.30867935, 1.42946955, 1.08464536] system = System() pynamics.set_system(__name__, system) global_q = True lO = Constant(7 / 1000, 'lO', system) lA = Constant(33 / 1000, 'lA', system) lB = Constant(33 / 1000, 'lB', system) lC = Constant(33 / 1000, 'lC', system) mO = Constant(10 / 1000, 'mA', system) mA = Constant(2.89 / 1000, 'mA', system) mB = Constant(2.89 / 1000, 'mB', system) mC = Constant(2.89 / 1000, 'mC', system) k = Constant(g_k, 'k', system) k1 = Constant(0.4, 'k1', system) friction_perp = Constant(f1, 'f_perp', system) friction_par = Constant(f2, 'f_par', system) b_damping = Constant(g_b_damping, 'b_damping', system) b_damping1 = Constant(g_b_damping1, 'b_damping1', system) preload0 = Constant(0 * pi / 180, 'preload0', system) preload1 = Constant(0 * pi / 180, 'preload1', system) preload2 = Constant(0 * pi / 180, 'preload2', system) preload3 = Constant(0 * pi / 180, 'preload3', system) Ixx_O = Constant(1, 'Ixx_O', system) Iyy_O = Constant(1, 'Iyy_O', system) Izz_O = Constant(1, 'Izz_O', system) Ixx_A = Constant(1, 'Ixx_A', system) Iyy_A = Constant(1, 'Iyy_A', system) Izz_A = Constant(1, 'Izz_A', system) Ixx_B = Constant(1, 'Ixx_B', system) Iyy_B = Constant(1, 'Iyy_B', system) Izz_B = Constant(1, 'Izz_B', system) Ixx_C = Constant(1, 'Ixx_C', system) Iyy_C = Constant(1, 'Iyy_C', system) Izz_C = Constant(1, 'Izz_C', system) y, y_d, y_dd = Differentiable('y', system) qO, qO_d, qO_dd = Differentiable('qO', system) qA, qA_d, qA_dd = Differentiable('qA', system) qB, qB_d, qB_dd = Differentiable('qB', system) qC, qC_d, qC_dd = Differentiable('qC', system) fit_states = initial_states initialvalues = {} initialvalues[y] = fit_states[0] initialvalues[y_d] = fit_states[5] initialvalues[qO] = 0 initialvalues[qO_d] = 0 initialvalues[qA] = fit_states[2] initialvalues[qA_d] = fit_states[7] initialvalues[qB] = fit_states[3] initialvalues[qB_d] = fit_states[8] initialvalues[qC] = fit_states[4] initialvalues[qC_d] = fit_states[9] statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] N = Frame('N') O = Frame('O') A = Frame('A') B = Frame('B') C = Frame('C') system.set_newtonian(N) if not global_q: O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system) A.rotate_fixed_axis_directed(O, [0, 0, 1], qA, system) B.rotate_fixed_axis_directed(A, [0, 0, 1], qB, system) C.rotate_fixed_axis_directed(B, [0, 0, 1], qC, system) else: O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system) A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system) B.rotate_fixed_axis_directed(N, [0, 0, 1], qB, system) C.rotate_fixed_axis_directed(N, [0, 0, 1], qC, system) pNO = 0 * N.x + y * N.y pOA = lO * N.x + y * N.y pAB = pOA + lA * A.x pBC = pAB + lB * B.x pCtip = pBC + lC * C.x pOcm = pNO + lO / 2 * N.x pAcm = pOA + lA / 2 * A.x pBcm = pAB + lB / 2 * B.x pCcm = pBC + lC / 2 * C.x wNO = N.getw_(O) wOA = N.getw_(A) wAB = A.getw_(B) wBC = B.getw_(C) IO = Dyadic.build(O, Ixx_O, Iyy_O, Izz_O) IA = Dyadic.build(A, Ixx_A, Iyy_A, Izz_A) IB = Dyadic.build(B, Ixx_B, Iyy_B, Izz_B) IC = Dyadic.build(C, Ixx_C, Iyy_C, Izz_C) BodyO = Body('BodyO', O, pOcm, mO, IO, system) BodyA = Body('BodyA', A, pAcm, mA, IA, system) BodyB = Body('BodyB', B, pBcm, mB, IB, system) BodyC = Body('BodyC', C, pCcm, mC, IC, system) # vOcm = pOcm.time_derivative() vAcm = pAcm.time_derivative() vBcm = pBcm.time_derivative() vCcm = pCcm.time_derivative() # system.add_spring_force1(k1+10000*(qA+abs(qA)),(qA-qO-preload1)*N.z,wOA) # system.add_spring_force1(k+10000*(qB+abs(qB)),(qB-qA-preload2)*N.z,wAB) # system.add_spring_force1(k+10000*(qC+abs(qC)),(qC-qB-preload3)*N.z,wBC) system.add_spring_force1(k1, (qA - qO - preload1) * N.z, wOA) system.add_spring_force1(k, (qB - qA - preload2) * N.z, wAB) system.add_spring_force1(k, (qC - qB - preload3) * N.z, wBC) #new Method use nJoint nvAcm = 1 / vAcm.length() * vAcm nvBcm = 1 / vBcm.length() * vBcm nvCcm = 1 / vCcm.length() * vCcm vSoil = drag_direction * 1 * N.y nSoil = 1 / vSoil.length() * vSoil if fit_vel == 0: vSoil = 1 * 1 * N.y nSoil = 1 / vSoil.length() * vSoil faperp = friction_perp * nSoil.dot(A.y) * A.y fapar = friction_par * nSoil.dot(A.x) * A.x system.addforce(-(faperp + fapar), vAcm) fbperp = friction_perp * nSoil.dot(B.y) * B.y fbpar = friction_par * nSoil.dot(B.x) * B.x system.addforce(-(fbperp + fbpar), vBcm) fcperp = friction_perp * nSoil.dot(C.y) * C.y fcpar = friction_par * nSoil.dot(C.x) * C.x system.addforce(-(fcperp + fcpar), vCcm) else: faperp = friction_perp * nvAcm.dot(A.y) * A.y fapar = friction_par * nvAcm.dot(A.x) * A.x system.addforce(-(faperp + fapar), vAcm) fbperp = friction_perp * nvBcm.dot(B.y) * B.y fbpar = friction_par * nvBcm.dot(B.x) * B.x system.addforce(-(fbperp + fbpar), vBcm) fcperp = friction_perp * nvCcm.dot(C.y) * C.y fcpar = friction_par * nvCcm.dot(C.x) * C.x system.addforce(-(fcperp + fcpar), vCcm) system.addforce(-b_damping1 * wOA, wOA) system.addforce(-b_damping * wAB, wAB) system.addforce(-b_damping * wBC, wBC) eq = [] eq_d = [(system.derivative(item)) for item in eq] eq_d.append(y_d - fit_vel) eq_dd = [(system.derivative(item)) for item in eq_d] f, ma = system.getdynamics() func1 = system.state_space_post_invert(f, ma, eq_dd) points = [pNO, pOA, pAB, pBC, pCtip] constants = system.constant_values states = pynamics.integration.integrate_odeint(func1, ini, t, args=({ 'constants': constants }, )) points_output = PointsOutput(points, system, constant_values=constants) y = points_output.calc(states) final = numpy.asarray(states[-1, :]) time1 = time.time() points_output.animate(fps=30, movie_name=str(time1) + 'video_1.mp4', lw=2, marker='o', color=(1, 0, 0, 1), linestyle='-') return final, states, y, system
def init_system(v, drag_direction, time_step): import pynamics from pynamics.frame import Frame from pynamics.variable_types import Differentiable, Constant from pynamics.system import System from pynamics.body import Body from pynamics.dyadic import Dyadic from pynamics.output import Output, PointsOutput from pynamics.particle import Particle import pynamics.integration import logging import sympy import numpy import matplotlib.pyplot as plt from math import pi from scipy import optimize from sympy import sin import pynamics.tanh as tanh from fit_qs import exp_fit import fit_qs # time_step = tstep x = numpy.zeros((7, 1)) friction_perp = x[0] friction_par = x[1] given_b = x[2] given_k = x[3] given_k1 = x[4] given_b1 = x[4] system = System() pynamics.set_system(__name__, system) global_q = True lO = Constant(7 / 1000, 'lO', system) lA = Constant(33 / 1000, 'lA', system) lB = Constant(33 / 1000, 'lB', system) lC = Constant(33 / 1000, 'lC', system) mO = Constant(10 / 1000, 'mA', system) mA = Constant(2.89 / 1000, 'mA', system) mB = Constant(2.89 / 1000, 'mB', system) mC = Constant(2.89 / 1000, 'mC', system) k = Constant(0.209, 'k', system) k1 = Constant(0.209, 'k1', system) friction_perp = Constant(1.2, 'f_perp', system) friction_par = Constant(-0.2, 'f_par', system) b_damping = Constant(given_b, 'b_damping', system) # time_step = 1/00 if v == 0: [ t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2, qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3, qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3 ] = fit_qs.fit_0_amount(time_step) elif v == 10: [ t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2, qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3, qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3 ] = fit_qs.fit_10_amount(time_step) elif v == 20: [ t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2, qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3, qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3 ] = fit_qs.fit_20_amount(time_step) elif v == 30: [ t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2, qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3, qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3 ] = fit_qs.fit_30_amount(time_step) elif v == 40: [ t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2, qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3, qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3 ] = fit_qs.fit_40_amount(time_step) elif v == 50: [ t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2, qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3, qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3 ] = fit_qs.fit_50_amount(time_step) distance = 200 / 1000 nums = int(tfinal / tstep) array_num = numpy.arange(0, nums) array_num1 = numpy.repeat(array_num, nums, axis=0) array_num1.shape = (nums, nums) error_k = array_num1 / 8000 + numpy.ones((nums, nums)) fit_t = t fit_qA = exp_fit(fit_t, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2, qAa3, qAb3, qAc3) fit_qB = exp_fit(fit_t, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3, qBc3) fit_qC = exp_fit(fit_t, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3) fit_qAd1 = numpy.diff(fit_qA) / numpy.diff(fit_t) fit_qAd = numpy.append(fit_qAd1[0], fit_qAd1) fit_qBd1 = numpy.diff(fit_qB) / numpy.diff(fit_t) fit_qBd = numpy.append(fit_qBd1[0], fit_qBd1) fit_qCd1 = numpy.diff(fit_qC) / numpy.diff(fit_t) fit_qCd = numpy.append(fit_qCd1[0], fit_qCd1) fit_states1 = numpy.stack( (fit_qA, fit_qB, fit_qC, fit_qAd, fit_qBd, fit_qCd), axis=1) fit_states1[:, 0:3] = fit_states1[:, 0:3] - fit_states1[0, 0:3] fit_states = -drag_direction * numpy.deg2rad(fit_states1) # plt.plot(t,fit_states) if drag_direction == -1: zero_shape = fit_states.shape fit_states = numpy.zeros(zero_shape) fit_vel = drag_direction * distance / (tfinal) if qAa1 == 0: fit_vel = 0 fit_v = numpy.ones(t.shape) * fit_vel if qAa1 == 0: fit_d = numpy.ones(t.shape) * fit_vel else: fit_d = drag_direction * numpy.r_[tinitial:distance:tstep * abs(fit_vel)] preload0 = Constant(0 * pi / 180, 'preload0', system) preload1 = Constant(0 * pi / 180, 'preload1', system) preload2 = Constant(0 * pi / 180, 'preload2', system) preload3 = Constant(0 * pi / 180, 'preload3', system) Ixx_O = Constant(1, 'Ixx_O', system) Iyy_O = Constant(1, 'Iyy_O', system) Izz_O = Constant(1, 'Izz_O', system) Ixx_A = Constant(1, 'Ixx_A', system) Iyy_A = Constant(1, 'Iyy_A', system) Izz_A = Constant(1, 'Izz_A', system) Ixx_B = Constant(1, 'Ixx_B', system) Iyy_B = Constant(1, 'Iyy_B', system) Izz_B = Constant(1, 'Izz_B', system) Ixx_C = Constant(1, 'Ixx_C', system) Iyy_C = Constant(1, 'Iyy_C', system) Izz_C = Constant(1, 'Izz_C', system) y, y_d, y_dd = Differentiable('y', system) qO, qO_d, qO_dd = Differentiable('qO', system) qA, qA_d, qA_dd = Differentiable('qA', system) qB, qB_d, qB_dd = Differentiable('qB', system) qC, qC_d, qC_dd = Differentiable('qC', system) initialvalues = {} initialvalues[y] = 0 + 1e-14 initialvalues[y_d] = fit_vel + 1e-14 initialvalues[qO] = 0 + 1e-14 initialvalues[qO_d] = 0 + 1e-14 initialvalues[qA] = fit_states[0, 0] + 1e-14 initialvalues[qA_d] = fit_states[0, 3] + 1e-14 initialvalues[qB] = fit_states[0, 1] + 1e-14 initialvalues[qB_d] = fit_states[0, 4] + 1e-14 initialvalues[qC] = fit_states[0, 2] + 1e-14 initialvalues[qC_d] = fit_states[0, 5] + 1e-14 statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] N = Frame('N') O = Frame('O') A = Frame('A') B = Frame('B') C = Frame('C') drag_direction = drag_direction velocity = 200 / tfinal / 1000 vSoil = drag_direction * velocity * N.y nSoil = 1 / vSoil.length() * vSoil system.set_newtonian(N) if not global_q: O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system) A.rotate_fixed_axis_directed(O, [0, 0, 1], qA, system) B.rotate_fixed_axis_directed(A, [0, 0, 1], qB, system) C.rotate_fixed_axis_directed(B, [0, 0, 1], qC, system) else: O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system) A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system) B.rotate_fixed_axis_directed(N, [0, 0, 1], qB, system) C.rotate_fixed_axis_directed(N, [0, 0, 1], qC, system) pNO = 0 * N.x + y * N.y pOA = lO * N.x + y * N.y pAB = pOA + lA * A.x pBC = pAB + lB * B.x pCtip = pBC + lC * C.x pOcm = pNO + lO / 2 * N.x pAcm = pOA + lA / 2 * A.x pBcm = pAB + lB / 2 * B.x pCcm = pBC + lC / 2 * C.x wNO = N.getw_(O) wOA = N.getw_(A) wAB = A.getw_(B) wBC = B.getw_(C) IO = Dyadic.build(O, Ixx_O, Iyy_O, Izz_O) IA = Dyadic.build(A, Ixx_A, Iyy_A, Izz_A) IB = Dyadic.build(B, Ixx_B, Iyy_B, Izz_B) IC = Dyadic.build(C, Ixx_C, Iyy_C, Izz_C) BodyO = Body('BodyO', O, pOcm, mO, IO, system) BodyA = Body('BodyA', A, pAcm, mA, IA, system) BodyB = Body('BodyB', B, pBcm, mB, IB, system) BodyC = Body('BodyC', C, pCcm, mC, IC, system) # BodyC = Particle(pCcm,mC,'ParticleC',system) vOcm = pOcm.time_derivative() vAcm = pAcm.time_derivative() vBcm = pBcm.time_derivative() vCcm = pCcm.time_derivative() system.add_spring_force1(k1 + 10000 * (qA + abs(qA)), (qA - qO - preload1) * N.z, wOA) system.add_spring_force1(k + 10000 * (qB + abs(qB)), (qB - qA - preload2) * N.z, wAB) system.add_spring_force1(k + 10000 * (qC + abs(qC)), (qC - qB - preload3) * N.z, wBC) #new Method use nJoint nvAcm = 1 / vAcm.length() * vAcm nvBcm = 1 / vBcm.length() * vBcm nvCcm = 1 / vCcm.length() * vCcm faperp = friction_perp * nvAcm.dot(A.y) * A.y fapar = friction_par * nvAcm.dot(A.x) * A.x system.addforce(-(faperp + fapar), vAcm) fbperp = friction_perp * nvBcm.dot(B.y) * B.y fbpar = friction_par * nvBcm.dot(B.x) * B.x system.addforce(-(fbperp + fbpar), vBcm) fcperp = friction_perp * nvCcm.dot(C.y) * C.y fcpar = friction_par * nvCcm.dot(C.x) * C.x system.addforce(-(fcperp + fcpar), vCcm) system.addforce(-b_damping * wOA, wOA) system.addforce(-b_damping * wAB, wAB) system.addforce(-b_damping * wBC, wBC) eq = [] eq_d = [(system.derivative(item)) for item in eq] eq_d.append(y_d - fit_vel) eq_dd = [(system.derivative(item)) for item in eq_d] f, ma = system.getdynamics() func1 = system.state_space_post_invert(f, ma, eq_dd) points = [pNO, pOA, pAB, pBC, pCtip] constants = system.constant_values return system, f, ma, func1, points, t, ini, constants, b_damping, k, k1, tstep, fit_states
def Cal_robot(direction, given_l, given_arm_l, omega1, t1, t2, ini_states, name1, video_on, x1, x2): system1 = System() time_a = time.time() pynamics.set_system(__name__, system1) given_k, given_b = x1 f1, f2, f3 = x2 global_q = True damping_r = 0 tinitial = 0 tfinal = (t1 - t2) / omega1 tstep = 1 / 50 t = numpy.r_[tinitial:tfinal:tstep] tol_1 = 1e-6 tol_2 = 1e-6 lO = Constant(27.5 / 1000, 'lO', system1) # given_arm_l = 65 lR = Constant(given_arm_l / 1000, 'lR', system1) # lR = Constant(40.5/1000,'lR',system11) lA = Constant(given_l / 1000, 'lA', system1) lB = Constant(given_l / 1000, 'lB', system1) lC = Constant(given_l / 1000, 'lC', system1) mO = Constant(1e0 * 154.5 / 1000, 'mO', system1) # mR = Constant(9.282/1000 ,'mR',system1) mR = Constant((1.158 + 0.1445 * given_arm_l) / 1000, 'mR', system1) mA = Constant(given_l * 2.75 * 0.14450000000000002 / 1000, 'mA', system1) mB = Constant(given_l * 2.75 * 0.14450000000000002 / 1000, 'mB', system1) mC = Constant(given_l * 2.75 * 0.14450000000000002 / 1000, 'mC', system1) k = Constant(given_k, 'k', system1) friction_perp = Constant(f1, 'f_perp', system1) friction_par = Constant(-1, 'f_par', system1) friction_arm_perp = Constant(2 + given_arm_l * f3, 'fr_perp', system1) friction_arm_par = Constant(-0.3, 'fr_par', system1) b_damping = Constant(given_b, 'b_damping', system1) preload0 = Constant(0 * pi / 180, 'preload0', system1) preload1 = Constant(0 * pi / 180, 'preload1', system1) preload2 = Constant(0 * pi / 180, 'preload2', system1) preload3 = Constant(0 * pi / 180, 'preload3', system1) Ixx_O = Constant(1, 'Ixx_O', system1) Iyy_O = Constant(1, 'Iyy_O', system1) Izz_O = Constant(1, 'Izz_O', system1) Ixx_R = Constant(1, 'Ixx_R', system1) Iyy_R = Constant(1, 'Iyy_R', system1) Izz_R = Constant(1, 'Izz_R', system1) Ixx_A = Constant(1, 'Ixx_A', system1) Iyy_A = Constant(1, 'Iyy_A', system1) Izz_A = Constant(1, 'Izz_A', system1) Ixx_B = Constant(1, 'Ixx_B', system1) Iyy_B = Constant(1, 'Iyy_B', system1) Izz_B = Constant(1, 'Izz_B', system1) Ixx_C = Constant(1, 'Ixx_C', system1) Iyy_C = Constant(1, 'Iyy_C', system1) Izz_C = Constant(1, 'Izz_C', system1) y, y_d, y_dd = Differentiable('y', system1) qO, qO_d, qO_dd = Differentiable('qO', system1) qR, qR_d, qR_dd = Differentiable('qR', system1) qA, qA_d, qA_dd = Differentiable('qA', system1) qB, qB_d, qB_dd = Differentiable('qB', system1) qC, qC_d, qC_dd = Differentiable('qC', system1) initialvalues = {} initialvalues[y] = ini_states[0] + tol_1 initialvalues[qO] = ini_states[1] + tol_1 initialvalues[qR] = ini_states[2] + tol_1 initialvalues[qA] = ini_states[3] + tol_1 initialvalues[qB] = ini_states[4] + tol_1 initialvalues[qC] = ini_states[5] + tol_1 initialvalues[y_d] = ini_states[6] + tol_1 initialvalues[qO_d] = ini_states[7] + tol_1 initialvalues[qR_d] = ini_states[8] + tol_1 initialvalues[qA_d] = ini_states[9] + tol_1 initialvalues[qB_d] = ini_states[10] + tol_1 initialvalues[qC_d] = ini_states[11] + tol_1 statevariables = system1.get_state_variables() ini = [initialvalues[item] for item in statevariables] N = Frame('N') O = Frame('O') R = Frame('R') A = Frame('A') B = Frame('B') C = Frame('C') system1.set_newtonian(N) if not global_q: O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system1) R.rotate_fixed_axis_directed(O, [0, 0, 1], qR, system1) A.rotate_fixed_axis_directed(O, [0, 0, 1], qA, system1) B.rotate_fixed_axis_directed(A, [0, 0, 1], qB, system1) C.rotate_fixed_axis_directed(B, [0, 0, 1], qC, system1) else: O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system1) R.rotate_fixed_axis_directed(N, [0, 0, 1], qR, system1) A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system1) B.rotate_fixed_axis_directed(N, [0, 0, 1], qB, system1) C.rotate_fixed_axis_directed(N, [0, 0, 1], qC, system1) pNO = 0 * N.x + y * N.y pOR = pNO + lO * N.x # pOR= pNO +lO*N.x pRA = pOR + lR * R.x pAB = pRA + lA * A.x pBC = pAB + lB * B.x pCtip = pBC + lC * C.x # pOcm=pNO +lO/2*N.x pOcm = pNO pRcm = pOR + lR / 2 * R.x pAcm = pRA + lA / 2 * A.x pBcm = pAB + lB / 2 * B.x pCcm = pBC + lC / 2 * C.x wNO = N.getw_(O) wOR = N.getw_(R) wRA = R.getw_(A) wAB = A.getw_(B) wBC = B.getw_(C) IO = Dyadic.build(O, Ixx_O, Iyy_O, Izz_O) IR = Dyadic.build(R, Ixx_R, Iyy_R, Izz_R) IA = Dyadic.build(A, Ixx_A, Iyy_A, Izz_A) IB = Dyadic.build(B, Ixx_B, Iyy_B, Izz_B) IC = Dyadic.build(C, Ixx_C, Iyy_C, Izz_C) BodyO = Body('BodyO', O, pOcm, mO, IO, system1) BodyR = Body('BodyR', R, pRcm, mR, IR, system1) BodyA = Body('BodyA', A, pAcm, mA, IA, system1) BodyB = Body('BodyB', B, pBcm, mB, IB, system1) BodyC = Body('BodyC', C, pCcm, mC, IC, system1) j_tol = 0 * pi / 180 inv_k = 1e2 alw = 1 system1.add_spring_force1( k + inv_k * (qA - qR + alw * abs(qA - qR - j_tol)), (qA - qR - preload1) * N.z, wRA) system1.add_spring_force1( k + inv_k * (qB - qA + alw * abs(qB - qA - j_tol)), (qB - qA - preload2) * N.z, wAB) system1.add_spring_force1( k + inv_k * (qC - qB + alw * abs(qC - qB - j_tol)), (qC - qB - preload3) * N.z, wBC) # system1.add_spring_force1(k,(qA-qR-preload1)*N.z,wRA) # system1.add_spring_force1(k,(qB-qA-preload2)*N.z,wAB) # system1.add_spring_force1(k,(qC-qB-preload3)*N.z,wBC) vOcm = y_d * N.y # vOcm = pOcm.time_derivative() vRcm = pRcm.time_derivative() vAcm = pAcm.time_derivative() vBcm = pBcm.time_derivative() vCcm = pCcm.time_derivative() nvRcm = 1 / (vRcm.length() + tol_1) * vRcm nvAcm = 1 / (vAcm.length() + tol_1) * vAcm nvBcm = 1 / (vBcm.length() + tol_1) * vBcm nvCcm = 1 / (vCcm.length() + tol_1) * vCcm vSoil = -direction * 1 * N.y nSoil = 1 / vSoil.length() * vSoil # reff = abs( abs(y_d+0.01)-abs(y_d-0.01))*1/0.02*9.75 # foperp = reff*nSoil foperp = f2 * nSoil system1.addforce(foperp, vOcm) # system1.addforce(9.75*1*nSoil,vOcm) # system1.addforce(9.75*1*nSoil*y_d,vOcm) # system1.addforce(-100*N.x, y_d*N.x) frperp = friction_arm_perp * nvRcm.dot(R.y) * R.y frpar = friction_arm_par * nvRcm.dot(R.x) * R.x system1.addforce(-(frperp + frpar), vRcm) faperp = friction_perp * nvAcm.dot(A.y) * A.y fapar = friction_par * nvAcm.dot(A.x) * A.x system1.addforce(-(faperp + fapar), vAcm) fbperp = friction_perp * nvBcm.dot(B.y) * B.y fbpar = friction_par * nvBcm.dot(B.x) * B.x system1.addforce(-(fbperp + fbpar), vBcm) fcperp = friction_perp * nvCcm.dot(C.y) * C.y fcpar = friction_par * nvCcm.dot(C.x) * C.x system1.addforce(-(fcperp + fcpar), vCcm) system1.addforce(-b_damping * 1 * wRA, wRA) system1.addforce(-b_damping * 1 * wAB, wAB) system1.addforce(-b_damping * 1 * wBC, wBC) eq = [] eq_d = [(system1.derivative(item)) for item in eq] eq_d.append(qR_d - omega1) # eq_dd= [(system1.derivative(item)) for item in eq_d] eq_dd = [system1.derivative(eq_d[0])] f, ma = system1.getdynamics() func1 = system1.state_space_post_invert(f, ma, eq_dd) points = [pNO, pOR, pRA, pAB, pBC, pCtip] constants = system1.constant_values states = pynamics.integration.integrate_odeint(func1, ini, t, args=({ 'constants': constants }, )) final = numpy.asarray(states[-1, :]) logger1 = logging.getLogger('pynamics.system1') logger2 = logging.getLogger('pynamics.integration') logger3 = logging.getLogger('pynamics.output') logger1.disabled = True logger2.disabled = True logger3.disabled = True points_output = PointsOutput(points, system1, constant_values=constants) y1 = points_output.calc(states) if video_on == 1: plt.figure() plt.plot(*(y1[::int(len(y1) / 20)].T) * 1000) plt.axis('equal') plt.axis('equal') plt.title("Plate Configuration vs Distance") plt.xlabel("Configuration") plt.ylabel("Distance (mm)") plt.figure() plt.plot(t, numpy.rad2deg(states[:, 2])) plt.plot(t, numpy.rad2deg(states[:, 8])) plt.legend(["qR", "qR_d"]) plt.hlines(numpy.rad2deg(t1), tinitial, tfinal) plt.hlines(numpy.rad2deg(t2), tinitial, tfinal) plt.title("Robot Arm angle and velocitues (qR and qR_d) over Time") plt.xlabel("Time (s)") plt.ylabel("Angles,Velocities (deg, deg/s)") plt.figure() q_states = numpy.c_[(states[:, 2], states[:, 3], states[:, 4], states[:, 5])] plt.plot(t, numpy.rad2deg(q_states)) plt.title("Joint Angule over Time") plt.xlabel("Time (s)") plt.ylabel("Joint Angles (deg)") plt.legend(["Arm", "Joint 1", "Joint 2", "Joint 3"]) plt.figure() qd_states = numpy.c_[(states[:, 8], states[:, 9], states[:, 10], states[:, 11])] plt.plot(t, numpy.rad2deg(qd_states)) plt.legend(["qR_d", "qA_d", "qB_d", "qC_d"]) plt.title("Joint Angular Velocities over Time") plt.xlabel("Time (s)") plt.ylabel("Joint Angular Velocities (deg/s)") plt.legend(["Arm", "Joint 1", "Joint 2", "Joint 3"]) plt.figure() plt.plot(t, states[:, 0], '--') plt.plot(t, states[:, 6]) y_d1 = states[:, 6] # force1 = abs( abs(y_d1+0.1)-abs(y_d1-0.1))*40 # plt.plot(t,force1) plt.title("Robot Distance and Velocity over time") plt.xlabel("Time (s)") plt.ylabel("Distance (mm)") plt.legend(["Distance", "Velocity of the robot"]) points_output.animate(fps=1 / tstep, movie_name=name1, lw=2, marker='o', color=(1, 0, 0, 1), linestyle='-') else: pass return final, states, y1