Skip to content

Instantly share code, notes, and snippets.

@domitry

domitry/hoge.rb

Created Dec 13, 2014
Embed
What would you like to do?
def integrand(t, q, f_args)
g = 9.80665
theta1, theta2 = q[0..1]
d_theta1_dt, d_theta2_dt = q[2..3]
m_mat = N[[(m1+m2)*l1*l1, m2*l1*l2*Math.cos(theta1-theta2)],
[m2*l1*l2*Math.cos(theta1-theta2), m2*l2**2]]
vec = N[[-m2*l1*l2*(d_theta2_dt**2)*Math.sin(theta1-theta2)-(m1+m2)*g*l1*Math.sin(theta1)],
[m2*l1*l2*(d_theta1_dt**2)*Math.sin(theta1-theta2)-m2*g*l2*Math.sin(theta2)]]
ddq_dtdt = m_mat.inverse.dot(vec)
q[2..3].concat(ddq_dtdt.transpose.to_a)
end
def run_n(n)
@r ||= Ode::Solver.new(method(:integrand)).init(@t, @q)
ret = []
n.times do |i|
@t += @dt
@r.integrate(@t)
ret.push(@r.y[0..1])
end
ret
end
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment