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',
#!/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)
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)
x0,
t,
args=(numerical_specified, numerical_constants))
#plot trajectory of each joint
# fig = plt.figure(1)
# 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)
viz_frames = []
colors = ['yellow', 'green', 'red', 'red', 'red', 'green', 'blue']
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')
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))
# 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.')
scene.generate_visualization_json(kane._q + kane._u, param_syms,
state_trajectories, param_vals)
print('Done.')