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()
def vizAsCylinder(self,
radius,
length,
axis='z',
color='blue',
offset=0,
format='pydy'):
""" """
if format == 'pydy':
# pydy cylinder is along y and centered at the middle of the cylinder
from pydy.viz.shapes import Cylinder
from pydy.viz.visualization_frame import VisualizationFrame
if axis == 'y':
e = self.frame
a = self.frame.y
elif axis == 'z':
e = self.frame.orientnew('CF_' + self.name, 'Axis',
(np.pi / 2, self.frame.x))
a = self.frame.z
elif axis == 'x':
e = self.frame.orientnew('CF_' + self.name, 'Axis',
(np.pi / 2, self.frame.z))
a = self.frame.x
shape = Cylinder(radius=radius, length=length, color=color)
center = Point('CC_' + self.name)
center.set_pos(self.origin, (length / 2 + offset) * a)
return VisualizationFrame(e, center, shape)
else:
raise NotImplementedError()
def frame_viz(Origin, frame, l=1, r=0.08):
""" """
from pydy.viz.shapes import Cylinder
from pydy.viz.visualization_frame import VisualizationFrame
from sympy.physics.mechanics import Point
#
X_frame = frame.orientnew('ffx', 'Axis', (-np.pi/2, frame.z) ) # Make y be x
Z_frame = frame.orientnew('ffz', 'Axis', (+np.pi/2, frame.x) ) # Make y be z
X_shape = Cylinder(radius=r, length=l, color='red') # Cylinder are along y
Y_shape = Cylinder(radius=r, length=l, color='green')
Z_shape = Cylinder(radius=r, length=l, color='blue')
X_center=Point('X'); X_center.set_pos(Origin, l/2 * X_frame.y)
Y_center=Point('Y'); Y_center.set_pos(Origin, l/2 * frame.y)
Z_center=Point('Z'); Z_center.set_pos(Origin, l/2 * Z_frame.y)
X_viz_frame = VisualizationFrame(X_frame, X_center, X_shape)
Y_viz_frame = VisualizationFrame( frame, Y_center, Y_shape)
Z_viz_frame = VisualizationFrame(Z_frame, Z_center, Z_shape)
return X_viz_frame, Y_viz_frame, Z_viz_frame
def vizFrame(self, radius=0.1, length=1.0, format='pydy'):
if format == 'pydy':
from pydy.viz.shapes import Cylinder
from pydy.viz.visualization_frame import VisualizationFrame
from sympy.physics.mechanics import Point
X_frame = self.frame.orientnew(
'ffx', 'Axis', (-np.pi / 2, self.frame.z)) # Make y be x
Z_frame = self.frame.orientnew(
'ffz', 'Axis', (+np.pi / 2, self.frame.x)) # Make y be z
X_shape = Cylinder(radius=radius, length=length,
color='red') # Cylinder are along y
Y_shape = Cylinder(radius=radius, length=length, color='green')
Z_shape = Cylinder(radius=radius, length=length, color='blue')
X_center = Point('X')
X_center.set_pos(self.origin, length / 2 * X_frame.y)
Y_center = Point('Y')
Y_center.set_pos(self.origin, length / 2 * self.frame.y)
Z_center = Point('Z')
Z_center.set_pos(self.origin, length / 2 * Z_frame.y)
X_viz_frame = VisualizationFrame(X_frame, X_center, X_shape)
Y_viz_frame = VisualizationFrame(self.frame, Y_center, Y_shape)
Z_viz_frame = VisualizationFrame(Z_frame, Z_center, Z_shape)
return X_viz_frame, Y_viz_frame, Z_viz_frame
def vizAsRotor(self,
radius=0.1,
length=1,
nB=3,
axis='x',
color='white',
format='pydy'):
# --- Bodies visualization
if format == 'pydy':
from pydy.viz.shapes import Cylinder
from pydy.viz.visualization_frame import VisualizationFrame
blade_shape = Cylinder(radius=radius, length=length, color=color)
viz = []
if axis == 'x':
for iB in np.arange(nB):
frame = self.frame.orientnew(
'b', 'Axis', (-np.pi / 2 + (iB - 1) * 2 * np.pi / nB,
self.frame.x)) # Y pointing along blade
center = Point('RB')
center.set_pos(self.origin, length / 2 * frame.y)
viz.append(VisualizationFrame(frame, center, blade_shape))
return viz
else:
raise NotImplementedError()
# Plot angles
fig2 = plt.figure()
ax2 = fig2.add_subplot(111)
ax2.plot(t, rad2deg(y[:, 2]), label='theta 1')
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
from highfidelitymodel import *
from pydy.viz.shapes import Cylinder, Sphere
import pydy.viz
from pydy.viz.visualization_frame import VisualizationFrame
from pydy.viz.scene import Scene
body_shape = Sphere(color='black', radius=0.1)
motor_shape = Sphere(color='black', radius=0.1)
arm_shape = Cylinder(radius=0.08, length=body_arm, color='blue')
blade_shape = Cylinder(radius=0.08, length=blade_length, color='red')
blade_tip_shape = Sphere(color='black', radius=.08)
body_viz_frame = VisualizationFrame(N, body_masscenter, body_shape)
motor_viz_frame = []
arm_center = []
arm_viz_frame = []
blade_tip_viz_frame = []
blade_center = []
blade_viz_frame = []
arm_angles = [-pi / 4, 3 * pi / 4, 1 * pi / 4, 5 * pi / 4]
for i in range(4):
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',
# ===============
# Each link in the pendulum is visualized with a cylinder, and a sphere at its
# far end.
link = Cylinder(name='link', radius=0.5, length=l, color='red')
sphere = Sphere(name='sphere', radius=1.0)
# By default, Cylinders are drawn so that their center is at the origin of the
# VisualizationFrame, and their axis is the y axis of the VisualizationFrame.
# We want the end of the Cylinder to be at the origin of the
# VisualizationFrame, and we want the Cylinder's axis to be aligned with the x
# axis of the VisualizationFrame. For these reasons, we must use the
# 'orientnew' and 'locatenew' methods to create new frames/points.
linkP_frame = A.orientnew('frameP', 'Axis', [0.5 * pi, N.z])
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
#!/usr/bin/env python
from pydy.viz.shapes import Cylinder, Sphere
from pydy.viz.visualization_frame import VisualizationFrame
from pydy.viz.scene import Scene
from .simulation import *
ankle_shape = Sphere(color='black', radius=0.1)
knee_shape = Sphere(color='black', radius=0.1)
hip_shape = Sphere(color='black', radius=0.1)
head_shape = Sphere(color='black', radius=0.125)
ankle_viz_frame = VisualizationFrame(inertial_frame, ankle, ankle_shape)
knee_viz_frame = VisualizationFrame(inertial_frame, knee, knee_shape)
hip_viz_frame = VisualizationFrame(inertial_frame, hip, hip_shape)
head = Point('N') # N for Noggin
head.set_pos(hip, 2 * torso_com_length * torso_frame.y)
head_viz_frame = VisualizationFrame(inertial_frame, head, head_shape)
constants_dict = dict(zip(constants, numerical_constants))
lower_leg_center = Point('l_c')
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)
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)
# ax = fig.add_subplot()
# plt.plot(t, rad2deg(y[:, :3])) #generalized positions-first 3 states
# plt.draw()
# plt.pause(30)
#visualization ----------------------------------------------------------------
#print(pydy.shapes.__all__)
#draw spheres at each joint
j0_shape = Sphere(color='black', radius=0.025)
j1_shape = Sphere(color='black', radius=0.025)
j2_shape = Sphere(color='peachpuff', radius=0.025)
EE_shape = Sphere(color='red', radius=0.025)
#create VisualizationFrame - attaches a shape to a reference frame and a point
j0_viz_frame = VisualizationFrame(inertial_frame, joint0, j0_shape)
j1_viz_frame = VisualizationFrame(inertial_frame, joint1, j1_shape)
j2_viz_frame = VisualizationFrame(inertial_frame, joint2, j2_shape)
EE_viz_frame = VisualizationFrame(inertial_frame, EE, EE_shape)
#make cylindrical links to connect joints
j0_center = Point('j0_c')
j1_center = Point('j1_c')
j2_center = Point('j2_c')
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,
for i, (body, particle,
mass_center) in enumerate(zip(bodies, particles, mass_centers)):
# body_shape = Cylinder(name='cylinder{}'.format(i),
# radius=0.05,
# length=lengths[i],
# color='red')
#
# viz_frames.append(VisualizationFrame('link_frame{}'.format(i), body,
# body_shape))
particle_shape = Sphere(name='sphere{}'.format(i),
radius=0.06,
color=colors[i])
viz_frames.append(
VisualizationFrame('particle_frame{}'.format(i), body.frame, particle,
particle_shape))
mass_center_shape = Sphere(name='sphere{}'.format(i),
radius=0.02,
color='black')
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))
def vizCOG(self, radius=1.0, color='red', format='pydy'):
if format == 'pydy':
from pydy.viz.shapes import Sphere
from pydy.viz.visualization_frame import VisualizationFrame
return VisualizationFrame(self.frame, self.masscenter,
Sphere(color=color, radius=radius))
def vizOrigin(self, radius=1.0, color='black', format='pydy'):
if format == 'pydy':
from pydy.viz.shapes import Sphere
from pydy.viz.visualization_frame import VisualizationFrame
return VisualizationFrame(self.frame, self.origin,
Sphere(color=color, radius=radius))
def vis_f(poi, sh):
return VisualizationFrame(Ro, poi, sh)
from simulate import link_length, link_radius, particle_radius
# A cylinder will be attached to each link and a sphere to each bob for the
# visualization.
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.')