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()
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',
"""
from numpy import pi
from pydy.viz.shapes import Cylinder, Sphere
from pydy.viz.scene import Scene
from pydy.viz.visualization_frame import VisualizationFrame
from simulate import *
# Create geometry
# ===============
# 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)
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)
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,
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)
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,
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
EE = Point('E')
EE.set_pos(joint2, j2_com_length * j2_frame.y)
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 / 2 * j2_frame.y)
constants_dict = dict(zip(constants, numerical_constants))
l0_shape = Cylinder(radius=0.01,
length=constants_dict[j0_length],
color='blue')
l0_viz_frame = VisualizationFrame('Link 0', j0_frame, j0_center, l0_shape)
l1_shape = Cylinder(radius=0.01,
length=constants_dict[j1_length],
color='blue')
l1_viz_frame = VisualizationFrame('Link 1', j1_frame, j1_center, l1_shape)
l2_shape = Cylinder(radius=0.01,
length=constants_dict[j2_com_length],
color='blue')
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
"""
from numpy import pi
from pydy.viz.shapes import Cylinder, Sphere
from pydy.viz.scene import Scene
from pydy.viz.visualization_frame import VisualizationFrame
from simulate import *
# Create geometry
# ===============
# 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=constants[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)
from pydy.viz.visualization_frame import VisualizationFrame
# local
from derive import I, O, links, particles, kane
from simulate import param_syms, state_trajectories, param_vals
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)