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.')