<%= "" %> <% # Pendulum axes test # Pendulums with different link frame orientations require "matrix" # Inertial parameters # Aluminum 2700 kg/m^3 density = 2700.0 # Largest pendulum box dimension dd = 0.1 # Initial angle of revolute joints theta = 0.1* Math::PI pendula = { "longZ_Xaxis" => {:pose => [0, 0, -dd/2, 0, 0, 0], :size => [dd/10, dd/10, dd], :axis => [1, 0, 0]}, "longZ_Yaxis" => {:pose => [0, 0, -dd/2, 0, 0, 0], :size => [dd/10, dd/10, dd], :axis => [0, 1, 0]}, "longX_Xaxis" => {:pose => [dd/2, 0, 0, 0, Math::PI/2, 0], :size => [dd, dd/10, dd/10], :axis => [1, 0, 0]}, "longX_Yaxis" => {:pose => [dd/2, 0, 0, 0, Math::PI/2, 0], :size => [dd, dd/10, dd/10], :axis => [0, 1, 0]}, "longY_Xaxis" => {:pose => [0, -dd/2, 0, Math::PI/2, 0, 0], :size => [dd/10, dd, dd/10], :axis => [1, 0, 0]}, "longY_Yaxis" => {:pose => [0, -dd/2, 0, Math::PI/2, 0, 0], :size => [dd/10, dd, dd/10], :axis => [0, 1, 0]}, } %> model://sun <% # Revolute joints i = 0 pendula.keys.each do |p| i = i + 1 name = 'pendulum_' + p size = Vector.elements(pendula[p][:size]) dx = size[0] dy = size[1] dz = size[2] mass = density * dx * dy * dz ixx = 1.0 / 12 * mass * (dy*dy + dz*dz) iyy = 1.0 / 12 * mass * (dz*dz + dx*dx) izz = 1.0 / 12 * mass * (dx*dx + dy*dy) pose = Vector.elements(pendula[p][:pose]) axis = Vector.elements(pendula[p][:axis]) %> <%= "" %> <%= dd*2.1*i %> 0 <%= dd*1.8 %> <%= (theta*axis).to_a.join(" ") %> <%= pose.to_a.join(" ") %> <%= mass %> <%= ixx %> <%= iyy %> <%= izz %> 0.0 0.0 0.0 <%= size.to_a.join(" ") %> <%= dx %> <%= dy %> <%= dz %> <%= (-1 * pose).to_a.join(" ") %> world link <%= axis.to_a.join(" ") %> <% end %>