def visualize():
# Create shapes
cube_1_shape = Cube(color='darkblue', length = 0.05)
cube_2_shape = Cube(color='darkred', length = 0.05)
# create frames to attach to shapes
cube_1_frame = VisualizationFrame(I1, p1, cube_1_shape)
cube_2_frame = VisualizationFrame(I2, p2, cube_2_shape)
# create scene with a frame base and origin
scene = Scene(N,N_o)
# append frames that should be visualized
scene.visualization_frames = [cube_1_frame,
cube_2_frame]
# provide constants
constants_dict = dict(zip(constants, numerical_constants))
# give symbolic states and their values
scene.states_symbols = coordinates + speeds
scene.constants = constants_dict
scene.states_trajectories = y
#visualize
scene.frames_per_second=800 # default is 30, which is very slow
scene.display()
lower_leg_center.set_pos(ankle, lower_leg_length / 2 * lower_leg_frame.y)
upper_leg_center.set_pos(knee, upper_leg_length / 2 * upper_leg_frame.y)
torso_center.set_pos(hip, torso_com_length * torso_frame.y)
lower_leg_shape = Cylinder(radius=0.08,
length=constants_dict[lower_leg_length],
color='blue')
lower_leg_viz_frame = VisualizationFrame('Lower Leg', lower_leg_frame,
lower_leg_center, lower_leg_shape)
upper_leg_shape = Cylinder(radius=0.08,
length=constants_dict[upper_leg_length],
color='green')
upper_leg_viz_frame = VisualizationFrame('Upper Leg', upper_leg_frame,
upper_leg_center, upper_leg_shape)
torso_shape = Cylinder(radius=0.08,
length=2 * constants_dict[torso_com_length],
color='red')
torso_viz_frame = VisualizationFrame('Torso', torso_frame, torso_center,
torso_shape)
scene = Scene(inertial_frame, ankle,
ankle_viz_frame, knee_viz_frame, hip_viz_frame, head_viz_frame,
lower_leg_viz_frame, upper_leg_viz_frame, torso_viz_frame)
scene.constants = constants_dict
scene.states_symbols = coordinates + speeds
scene.states_trajectories = y
for i, (link, particle) in enumerate(zip(links, particles)):
link_shape = Cylinder(name='cylinder{}'.format(i),
radius=link_radius,
length=link_length,
color='red')
viz_frames.append(VisualizationFrame('link_frame{}'.format(i), link,
link_shape))
particle_shape = Sphere(name='sphere{}'.format(i),
radius=particle_radius,
color='blue')
viz_frames.append(VisualizationFrame('particle_frame{}'.format(i),
link.frame,
particle,
particle_shape))
# Now the visualization frames can be passed in to create a scene.
scene = Scene(I, O, *viz_frames)
# And the motion of the shapes is generated by providing the scene with the
# state trajectories.
print('Generating transform time histories.')
scene.generate_visualization_json(kane._q + kane._u, param_syms,
state_trajectories, param_vals)
print('Done.')
scene.display()
def __init__(self, parameters=None, samples=None):
## coordinates
# theta: pitch
# psi: yaw
# phi: roll
# delta: steer
# these correspond to x_aux[:] + x[0:3] in the auxiliary state and
# state vectors
x, y, theta, psi, phi, delta = symbols('x y θ ψ φ δ')
# rr: rear radius
# rf: front radius
rr, rf = symbols('rr rf')
# d1: orthogonal distance from rear wheel center to steer axis
# d3: orthogonal distance from front wheel center to steer axis
# d2: steer axis separation
d1, d2, d3 = symbols('d1:4')
## reference frames
# N: inertial frame
# D: rear wheel frame
# C: rear assembly frame - yaw, roll, pitch from inertial frame
# E: front assembly frame
# F: front wheel frame
N = ReferenceFrame('N')
C = N.orientnew('C', 'body', [psi, phi, theta], 'zxy')
E = C.orientnew('E', 'axis', [delta, C.z]) # steer
# Since it will not matter if circles (wheels) rotate, we can attach
# them to the respective assembly frames
# However, we need to rotate the rear/front assembly frames for a
# cylinder shape to be attached correctly. We can simply rotate pi/2
# about the z-axis for C and about the x-axis for E.
Cy = C.orientnew('Cy', 'axis', [pi/2, C.z])
Ey = C.orientnew('Ey', 'axis', [pi/2, E.x])
## points
# define unit vectors from rear/front wheel centers to ground in the
# wheel plane which will be used to describe points
Dz = ((C.y ^ N.z) ^ C.y).normalize()
Fz = ((E.y ^ N.z) ^ E.y).normalize()
# origin of inertial frame
O = Point('O')
# rear wheel/ground contact point
dn = O.locatenew('dn', x*N.x + y*N.y)
# rear wheel center point
do = dn.locatenew('do', -rr*Dz)
# orthogonal projection of rear wheel center on steer axis
ce = do.locatenew('ce', d1*C.x)
# front wheel center point
fo = ce.locatenew('fo', d2*C.z + d3*E.x)
# front wheel/ground contact point
fn = fo.locatenew('fn', rf*Fz)
## define additional points for visualization
# rear assembly center
cc = do.locatenew('cc', d1/2*C.x)
# front assembly center
ec = ce.locatenew('ec', d2/2*C.z)
## shapes
Ds = Cylinder(name='rear wheel', radius=rr, length=0.01)
Cs = Cylinder(name='rear assembly', radius=0.02, length=d1,
color='lightseagreen')
Es = Cylinder(name='front assembly', radius=0.02, length=d2,
color='lightseagreen')
Fs = Cylinder(name='front wheel', radius=rf, length=0.01)
## visualization frames
Dv = VisualizationFrame('Rear Wheel', C, do, Ds)
Cv = VisualizationFrame('Rear Assembly', Cy, cc, Cs)
Ev = VisualizationFrame('Front Assembly', Ey, ec, Es)
Fv = VisualizationFrame('Front Wheel', E, fo, Fs)
# V: visualization frame
V = N.orientnew('V', 'axis', [pi, N.x])
self.coordinate = Coordinates(x, y, theta, psi, phi, delta)
self.parameter = Parameters(rr, rf, d1, d2, d3)
self.frame = Frames(N, V, C, E)
self.shape = Visual(Ds, Cs, Es, Fs)
self.point = Visual(do, cc, ec, fo)
self.vframe = Visual(Dv, Cv, Ev, Fv)
self.scene = Scene(V, dn) # scene moves with rear contact point
self.scene.visualization_frames = list(self.vframe.__dict__.values())
self.scene.states_symbols = list(self.coordinate.__dict__.values())
if parameters is not None:
self.set_parameters(parameters)
if samples is not None:
self.set_trajectory(samples)
j0_center.set_pos(joint0, j0_length / 2 * j0_frame.y)
j1_center.set_pos(joint1, j1_length / 2 * j1_frame.y)
j2_center.set_pos(joint2, j2_com_length * j2_frame.y)
constants_dict = dict(zip(constants, numerical_constants))
l0_shape = Cylinder(radius=0.025,
length=constants_dict[j0_length],
color='peachpuff')
l0_viz_frame = VisualizationFrame('Link 0', j0_frame, j0_center, l0_shape)
l1_shape = Cylinder(radius=0.025,
length=constants_dict[j1_length],
color='peachpuff')
l1_viz_frame = VisualizationFrame('Link 1', j1_frame, j1_center, l1_shape)
l2_shape = Cylinder(radius=0.025,
length=constants_dict[j2_com_length] * 2,
color='peachpuff')
l2_viz_frame = VisualizationFrame('Link 2', j2_frame, j2_center, l2_shape)
scene = Scene(inertial_frame, joint0)
#make list of frames we want in scene
scene.visualization_frames = [
j0_viz_frame, j1_viz_frame, j2_viz_frame, EE_viz_frame, l0_viz_frame,
l1_viz_frame, l2_viz_frame
]
scene.states_symbols = coordinates + speeds
scene.constants = constants_dict
scene.states_trajectories = y
scene.display()
viz_frames.append(
VisualizationFrame('mass_center_frame{}'.format(i), body.frame,
mass_center, mass_center_shape))
target_shape = Sphere(name='sphere{}'.format(i), radius=0.02, color='green')
target_right_leg = Point('target_right_leg')
target_right_leg.set_pos(origin, (x_des[2] * inertial_frame.x) +
(x_des[3] * inertial_frame.y))
viz_frames.append(
VisualizationFrame('target_frame_right', inertial_frame, target_right_leg,
target_shape))
## Now the visualization frames can be passed in to create a scene.
scene = Scene(inertial_frame, origin, *viz_frames)
# Provide the data to compute the trajectories of the visualization frames.
scene.constants = dict(zip(constants, numerical_constants))
scene.states_symbols = coordinates + speeds
scene.states_trajectories = y
scene.display()
#%% export to c header
t = symbols('t')
a0 = ankle_left.pos_from(origin).express(inertial_frame).simplify().to_matrix(
inertial_frame)
k0 = knee_left.pos_from(origin).express(inertial_frame).simplify().to_matrix(
inertial_frame)
hl = hip_left.pos_from(origin).express(inertial_frame).simplify().to_matrix(
inertial_frame)
lower_leg_center.set_pos(ankle, lower_leg_length / 2 * lower_leg_frame.y)
upper_leg_center.set_pos(knee, upper_leg_length / 2 * upper_leg_frame.y)
torso_center.set_pos(hip, torso_com_length * torso_frame.y)
lower_leg_shape = Cylinder(radius=0.08,
length=constants_dict[lower_leg_length],
color='blue')
lower_leg_viz_frame = VisualizationFrame('Lower Leg', lower_leg_frame,
lower_leg_center, lower_leg_shape)
upper_leg_shape = Cylinder(radius=0.08,
length=constants_dict[upper_leg_length],
color='green')
upper_leg_viz_frame = VisualizationFrame('Upper Leg', upper_leg_frame,
upper_leg_center, upper_leg_shape)
torso_shape = Cylinder(radius=0.08,
length=2 * constants_dict[torso_com_length],
color='red')
torso_viz_frame = VisualizationFrame('Torso', torso_frame, torso_center,
torso_shape)
scene = Scene(inertial_frame, ankle,
ankle_viz_frame, knee_viz_frame, hip_viz_frame, head_viz_frame,
lower_leg_viz_frame, upper_leg_viz_frame, torso_viz_frame)
scene.generate_visualization_json(coordinates + speeds, constants, y,
numerical_constants)
motor_viz_frame.append(
VisualizationFrame(N, motor_masscenter[i], motor_shape))
arm_center.append(Point('a_c[%u]' % (i, )))
arm_center[i].set_pos(body_masscenter, body_arm / 2. * motorloc[i])
arm_viz_frame.append(
VisualizationFrame(
'arm[%u]' % (i, ),
body_frame.orientnew('arm%u' % (i, ), 'Axis',
[arm_angles[i], body_frame.z]), arm_center[i],
arm_shape))
for i in range(4):
for j in range(2):
blade_tip_viz_frame.append(
VisualizationFrame(N, blade_tip[2 * i + j], blade_tip_shape))
blade_center.append(Point('p_c[%u]' % (2 * i + j, )))
blade_center[2 * i + j].set_pos(
motor_masscenter[i], blade_length / 2. * blade_frame[2 * i + j].y)
blade_viz_frame.append(
VisualizationFrame('blade[%u]' % (2 * i + j, ),
blade_frame[2 * i + j], blade_center[2 * i + j],
blade_shape))
scene = Scene(N, O)
scene.visualization_frames = [
body_viz_frame
] + arm_viz_frame + motor_viz_frame + blade_viz_frame
print scene.visualization_frames
scene.states_symbols = q_sym + u_sym
scene.states_trajectories = y
scene.constants = {}
scene.display()
upper_leg_center = Point('u_c')
torso_center = Point('t_c')
lower_leg_center.set_pos(ankle, lower_leg_length / 2 * lower_leg_frame.y)
upper_leg_center.set_pos(knee, upper_leg_length / 2 * upper_leg_frame.y)
torso_center.set_pos(hip, torso_com_length * torso_frame.y)
constants_dict = dict(zip(constants, numerical_constants))
lower_leg_shape = Cylinder(radius=0.08, length=constants_dict[lower_leg_length], color='blue')
lower_leg_viz_frame = VisualizationFrame('Lower Leg', lower_leg_frame, lower_leg_center, lower_leg_shape)
upper_leg_shape = Cylinder(radius=0.08, length=constants_dict[upper_leg_length], color='green')
upper_leg_viz_frame = VisualizationFrame('Upper Leg', upper_leg_frame, upper_leg_center, upper_leg_shape)
torso_shape = Cylinder(radius=0.08, length=2 * constants_dict[torso_com_length], color='red')
torso_viz_frame = VisualizationFrame('Torso', torso_frame, torso_center, torso_shape)
scene = Scene(inertial_frame, ankle)
#make list of frames we want in the scene
scene.visualization_frames = [ankle_viz_frame,
knee_viz_frame,
hip_viz_frame,
head_viz_frame,
lower_leg_viz_frame,
upper_leg_viz_frame,
torso_viz_frame]
scene.states_symbols = coordinates + speeds
scene.constants = constants_dict
scene.states_trajectories = y
scene.display()
#scene.display_ipython()
return dx
x0 = hstack(( 0, ones(len(q) - 1) , 1e-3 * ones(len(u)) )) # Initial conditions, q and u
t = [0, .01] # Time vector
state = x0
def step_model(state):
y = odeint(right_hand_side, state, t, args=(parameter_vals,)) # Actual integration
return y[1]
class Scene():
pass
scene = Scene()
scene.cube = Cube()
def render_scene(scene):
for shape in scene['shapes']:
shape.render()
def animate():
screen = pygame.display.set_mode((600,400), 0, 32)
def render_state(state):
logger.debug(state[0])
glTranslatef(state[0],0.0, 1)
scene.cube.render()
linkP_origin = O.locatenew('originP', 0.5 * l * A.x)
linkP_viz_frame = VisualizationFrame('linkP', linkP_frame, linkP_origin, link)
linkR_frame = B.orientnew('frameR', 'Axis', [0.5 * pi, N.z])
linkR_origin = P.locatenew('originP', 0.5 * l * B.x)
linkR_viz_frame = VisualizationFrame('linkR', linkR_frame, linkR_origin, link)
sphereP_viz_frame = VisualizationFrame('sphereP', N, P, sphere)
sphereR_viz_frame = VisualizationFrame('sphereR', N, R, sphere)
# Construct the scene
# ===================
# We want gravity to be directed downwards in the visualization. Gravity is in
# the -x direction. By default, the visualization uses the xz plane as the
# ground plane. Thus, gravity is contained in the ground plane. However, we
# want gravity to point in the -y direction in the visualization. To achieve
# this, we create a world frame that is rotated +90 degrees about the N frame's
# z direction.
world_frame = N.orientnew('world', 'Axis', [0.5 * pi, N.z])
scene = Scene(world_frame, O, linkP_viz_frame, linkR_viz_frame,
sphereP_viz_frame, sphereR_viz_frame)
# Create the visualization
# ========================
scene.generate_visualization_json(coordinates + speeds, constants.keys(), x,
constants.values())
scene.display()
viz_frames = []
for i, (link, particle) in enumerate(zip(links, particles)):
link_shape = Cylinder(name='cylinder{}'.format(i),
radius=link_radius,
length=link_length,
color='red')
viz_frames.append(
VisualizationFrame('link_frame{}'.format(i), link, link_shape))
particle_shape = Sphere(name='sphere{}'.format(i),
radius=particle_radius,
color='blue')
viz_frames.append(
VisualizationFrame('particle_frame{}'.format(i), link.frame, particle,
particle_shape))
# Now the visualization frames can be passed in to create a scene.
scene = Scene(I, O, *viz_frames)
# And the motion of the shapes is generated by providing the scene with the
# state trajectories.
print('Generating transform time histories.')
scene.generate_visualization_json(kane._q + kane._u, param_syms,
state_trajectories, param_vals)
print('Done.')
scene.display()
target_shape = Sphere(name='sphere{}'.format(i),
radius=0.02,
color='green')
target_right_leg = Point('target_right_leg')
target_right_leg.set_pos(origin, (x_des[2] * inertial_frame.x)+(x_des[3] * inertial_frame.y))
viz_frames.append(VisualizationFrame('target_frame_right',
inertial_frame,
target_right_leg,
target_shape))
## Now the visualization frames can be passed in to create a scene.
scene = Scene(inertial_frame, origin, *viz_frames)
# Provide the data to compute the trajectories of the visualization frames.
scene.constants = dict(zip(constants, numerical_constants))
scene.states_symbols = coordinates+speeds
scene.states_trajectories = y
scene.display()
#%% export to c header
t = symbols('t')
a0 = ankle_left.pos_from(origin).express(inertial_frame).simplify().to_matrix(inertial_frame)
k0 = knee_left.pos_from(origin).express(inertial_frame).simplify().to_matrix(inertial_frame)
hl = hip_left.pos_from(origin).express(inertial_frame).simplify().to_matrix(inertial_frame)
hc = hip_center.pos_from(origin).express(inertial_frame).simplify().to_matrix(inertial_frame)
hr = hip_right.pos_from(origin).express(inertial_frame).simplify().to_matrix(inertial_frame)
k1 = knee_right.pos_from(origin).express(inertial_frame).simplify().to_matrix(inertial_frame)
r_hip_shape = Sphere(color='black', radius = 0.1)
r_ankle_shape = Sphere(color='black', radius = 0.1)
l_ankle_viz_frame = VisualizationFrame(inertial_frame, l_ankle, l_ankle_shape)
l_hip_viz_frame = VisualizationFrame(inertial_frame, l_hip, l_hip_shape)
r_hip_viz_frame = VisualizationFrame(inertial_frame, r_hip, r_hip_shape)
r_ankle_viz_frame = VisualizationFrame(inertial_frame, r_leg_mass_center, r_ankle_shape)
constants_dict = dict(zip(constants, numerical_constants))
l_leg_shape = Cylinder(radius = 0.08, length = constants_dict[l_leg_length], color = 'blue')
l_leg_viz_frame = VisualizationFrame('Left Leg', l_leg_frame, l_leg_mass_center, l_leg_shape)
body_shape = Cylinder(radius = 0.08, length = constants_dict[hip_width], color = 'blue')
body_viz_frame = VisualizationFrame('Body', body_frame, body_mass_center, body_shape)
r_leg_shape = Cylinder(radius = 0.08, length = constants_dict[l_leg_length], color = 'red')
r_leg_viz_frame = VisualizationFrame('Right Leg', r_leg_frame, r_leg_mass_center, r_leg_shape)
scene = Scene(inertial_frame, l_ankle)
scene.visualization_frames = [l_ankle_viz_frame,l_hip_viz_frame, r_hip_viz_frame, r_ankle_viz_frame, l_leg_viz_frame, body_viz_frame, r_leg_viz_frame]
scene.generate_visualization_json(coordinates + speeds, constants, y, numerical_constants)
scene.display()
viz_frames = []
for i, (link, particle) in enumerate(zip(links, particles)):
link_shape = Cylinder(name='cylinder{}'.format(i),
radius=link_radius,
length=link_length,
color='red')
viz_frames.append(VisualizationFrame('link_frame{}'.format(i), link,
link_shape))
particle_shape = Sphere(name='sphere{}'.format(i),
radius=particle_radius,
color='blue')
viz_frames.append(VisualizationFrame('particle_frame{}'.format(i),
link.frame,
particle,
particle_shape))
# Now the visualization frames can be passed in to create a scene.
scene = Scene(I, O, *viz_frames)
# Provide the data to compute the trajectories of the visualization frames.
scene.constants = dict(zip(param_syms, param_vals))
scene.states_symbols = q + u
scene.states_trajectories = state_trajectories
scene.display()
linkR_frame = B.orientnew('frameR', 'Axis', [0.5 * pi, N.z])
linkR_origin = P.locatenew('originP', 0.5 * l * B.x)
linkR_viz_frame = VisualizationFrame('linkR', linkR_frame, linkR_origin, link)
sphereP_viz_frame = VisualizationFrame('sphereP', N, P, sphere)
sphereR_viz_frame = VisualizationFrame('sphereR', N, R, sphere)
# Construct the scene
# ===================
# We want gravity to be directed downwards in the visualization. Gravity is in
# the -x direction. By default, the visualization uses the xz plane as the
# ground plane. Thus, gravity is contained in the ground plane. However, we
# want gravity to point in the -y direction in the visualization. To achieve
# this, we create a world frame that is rotated +90 degrees about the N frame's
# z direction.
world_frame = N.orientnew('world', 'Axis', [0.5 * pi, N.z])
scene = Scene(world_frame, O, linkP_viz_frame, linkR_viz_frame,
sphereP_viz_frame, sphereR_viz_frame)
# Create the visualization
# ========================
scene.generate_visualization_json_system(sys)
if __name__ == "__main__":
try: #If called from inside notebook,
scene.display_ipython()
except: #If called from interpreter
scene.display()
linkR_frame = B.orientnew('frameR', 'Axis', [0.5 * pi, N.z])
linkR_origin = P.locatenew('originP', 0.5 * l * B.x)
linkR_viz_frame = VisualizationFrame('linkR', linkR_frame, linkR_origin, link)
sphereP_viz_frame = VisualizationFrame('sphereP', N, P, sphere)
sphereR_viz_frame = VisualizationFrame('sphereR', N, R, sphere)
# Construct the scene
# ===================
# We want gravity to be directed downwards in the visualization. Gravity is in
# the -x direction. By default, the visualization uses the xz plane as the
# ground plane. Thus, gravity is contained in the ground plane. However, we
# want gravity to point in the -y direction in the visualization. To achieve
# this, we create a world frame that is rotated +90 degrees about the N frame's
# z direction.
world_frame = N.orientnew('world', 'Axis', [0.5 * pi, N.z])
scene = Scene(world_frame, O,
linkP_viz_frame, linkR_viz_frame, sphereP_viz_frame, sphereR_viz_frame)
# Create the visualization
# ========================
scene.generate_visualization_json_system(sys)
if __name__ == "__main__":
scene.display()
ax2.plot(t, rad2deg(y[:, 3]), label='theta 2')
ax2.set_xlabel('Time [s]')
ax2.set_ylabel('Angle [deg]')
ax2.legend()
### Visualization
# Create shapes
cube_i_shape = Cube(color='darkblue', length=0.05)
cube_j_shape = Cube(color='darkred', length=0.05)
# create frames to attach to shapes
cube_i_frame = VisualizationFrame(I, p_i, cube_i_shape)
cube_j_frame = VisualizationFrame(J, p_j, cube_j_shape)
# create scene with a frame base and origin
scene = Scene(N, N_o)
# append frames that should be visualized
scene.visualization_frames = [cube_i_frame, cube_j_frame]
# privide constants
constants_dict = dict(zip(constants, numerical_constants))
# give symbolic states and their values
scene.states_symbols = coordinates + speeds
scene.constants = constants_dict
scene.states_trajectories = y
#visualize
scene.frames_per_second = 1800 # default is 30, which is very slow
#scene.display()
# TODO Create points and forces at correct locations and catching sides,rather than having coil at CoM
lower_leg_center.set_pos(ankle, lower_leg_length / 2 * lower_leg_frame.y)
upper_leg_center.set_pos(knee, upper_leg_length / 2 * upper_leg_frame.y)
torso_center.set_pos(hip, torso_com_length * torso_frame.y)
lower_leg_shape = Cylinder(radius=0.08,
length=constants_dict[lower_leg_length],
color='blue')
lower_leg_viz_frame = VisualizationFrame('Lower Leg', lower_leg_frame,
lower_leg_center, lower_leg_shape)
upper_leg_shape = Cylinder(radius=0.08,
length=constants_dict[upper_leg_length],
color='green')
upper_leg_viz_frame = VisualizationFrame('Upper Leg', upper_leg_frame,
upper_leg_center, upper_leg_shape)
torso_shape = Cylinder(radius=0.08,
length=2 * constants_dict[torso_com_length],
color='red')
torso_viz_frame = VisualizationFrame('Torso', torso_frame, torso_center,
torso_shape)
scene = Scene(inertial_frame, ankle, ankle_viz_frame, knee_viz_frame,
hip_viz_frame, head_viz_frame, lower_leg_viz_frame,
upper_leg_viz_frame, torso_viz_frame)
scene.generate_visualization_json(coordinates + speeds, constants, y,
numerical_constants)
