def __init__(self): self.state = State() self.state.timestep = ts_simulation self.rigid_body = RigidBody() self.gravity = Gravity(self.state) self.wind = Wind(self.state) self.aero = Aerodynamics(self.state) self.thrust = Thrust(self.state) self.intg = get_integrator(self.state.timestep, self.eom)
def __init__(self, ampal): self.ampal = ampal ######### Inhereted classes ############### self.rigid_body = RigidBody(self.ampal) self.vdw_analysis = VdW_Radii(self.ampal) ######################################### self.n_threads = 4 self.Atoms_XYZ = [ numpy.array([atom.x, atom.y, atom.z]) for atom in self.ampal.get_atoms() ] self.cartesian_base = self.rigid_body.get_assembly_reference() self.e_x = self.cartesian_base[0] self.e_y = self.cartesian_base[1] self.e_z = self.cartesian_base[2] self.COM = self.rigid_body.get_assembly_com()
class Drone(): def __init__(self): self.state = State() self.state.timestep = ts_simulation self.rigid_body = RigidBody() self.gravity = Gravity(self.state) self.wind = Wind(self.state) self.aero = Aerodynamics(self.state) self.thrust = Thrust(self.state) self.intg = get_integrator(self.state.timestep, self.eom) def eom(self, t, x, delta): # Update rigid body state self.state.rigid_body = x # Split up drone inputs delta delta_aero = delta[:3] # elevator, aileron, rudder setting delta_thrust = delta[3] # thrust setting # Update wind self.wind.update() # Update aero force and moment self.aero.update(delta_aero) # Update thrust force and moment self.thrust.update(delta_thrust) # Add aero, thrust force and moment force_moment = self.aero.force_moment \ + self.thrust.force_moment # Add gravity force force_moment[:3] += self.gravity.force return self.rigid_body.eom(t, x, force_moment) def update(self, delta): x = self.intg.step(self.state.time, self.state.rigid_body, delta) self.state.time += self.state.timestep self.state.rigid_body = x
def __init__(self, image, screen_pos, world_pos): pygame.sprite.Sprite.__init__(self) self.world_pos = world_pos self.screen_pos = screen_pos self.delta_pos = pygame.math.Vector2() self.src_image = pygame.image.load(image) # Correct for car being upside-down self.src_image = pygame.transform.rotate(self.src_image, 180) self.rect = self.src_image.get_rect() self.rect.center = screen_pos self.aabb = self.rect self.direction = 3 * math.pi / 2 self.bounding_box = BoundingBox(self.aabb, self.direction) self.forward = self.reverse = self.left = self.right = False self.force_line_pairs = [] self.vel_line_pairs = [] # Our rigid body simulator half_size = pygame.math.Vector2(self.rect.width * 0.25, self.rect.height * 0.25) self.rigid_body = RigidBody(2 * half_size, self.MASS) wheel_pos = [] wheel_pos.append(half_size) wheel_pos.append(pygame.math.Vector2(half_size.x * -1, half_size.y)) wheel_pos.append(half_size * -1) wheel_pos.append(pygame.math.Vector2(half_size.x, half_size.y * -1)) self.wheels = [] self.wheels.append(Wheel(wheel_pos[0], 1)) self.wheels.append(Wheel(wheel_pos[1], 1)) self.wheels.append(Wheel(wheel_pos[2], 1)) self.wheels.append(Wheel(wheel_pos[3], 1)) self.rigid_body.set_position(self.world_pos, self.direction)
class Drone(): def __init__(self): self.state = State() self.state.timestep = ts_simulation self.rigid_body = RigidBody() self.gravity = Gravity(self.state) self.wind = Wind(self.state) self.aero = Aerodynamics(self.state) self.thrust = Thrust(self.state) self.intg = get_integrator(self.state.timestep, self.eom) def eom(self, t, x, delta): # Update rigid body state self.state.rigid_body = x # Split up drone inputs delta delta_aero = delta[ :3] # elevator, aileron, rudder setting delta_thrust = delta[3] # thrust setting # Update wind self.wind.update() # Update aero force and moment self.aero.update(delta_aero) # Update thrust force and moment self.thrust.update(delta_thrust) # Add aero, thrust force and moment force_moment = self.aero.force_moment \ + self.thrust.force_moment # Add gravity force force_moment[ :3] += self.gravity.force return self.rigid_body.eom(t, x, force_moment) def update(self, delta): x = self.intg.step(self.state.time, self.state.rigid_body, delta) self.state.time += self.state.timestep self.state.rigid_body = x # update the class structure for the true state: # [pn, pe, h, Va, alpha, beta, phi, theta, chi, p, q, r, Vg, wn, we, psi, gyro_bx, gyro_by, gyro_bz] pdot = quat2rot(x[6:10]) @ x[3:6] self.state.chi = np.arctan2(pdot.item(1), pdot.item(0))
class RadialProfile(): """Hello""" def __init__(self, ampal): self.ampal = ampal ######### Inhereted classes ############### self.rigid_body = RigidBody(self.ampal) self.vdw_analysis = VdW_Radii(self.ampal) ######################################### self.n_threads = 4 self.Atoms_XYZ = [ numpy.array([atom.x, atom.y, atom.z]) for atom in self.ampal.get_atoms() ] self.cartesian_base = self.rigid_body.get_assembly_reference() self.e_x = self.cartesian_base[0] self.e_y = self.cartesian_base[1] self.e_z = self.cartesian_base[2] self.COM = self.rigid_body.get_assembly_com() ############################################ # Types of profiles ############################################ def primitive(self): prims = self.rigid_body.get_primitives() xyz = prims[0] return self.get_profile(xyz) def punctual(self): xyz = self.Atoms_XYZ return self.get_profile(xyz) def vdw(self, vdw_type): if vdw_type == 'simple': vdw_data = self.vdw_analysis.simple.get_radii() elif vdw_type == 'amber': vdw_data = self.vdw_analysis.amber.get_radii() else: print("Not valid VdW Radius type") ################################################ vdw_circle_union = self.get_union_vdw_circles(vdw_data) profile = numpy.array(vdw_circle_union.exterior.coords).T return profile def vdw_per_residue(self, vdw_type): if vdw_type == 'simple': vdw_data = self.vdw_analysis.simple.get_radii() elif vdw_type == 'amber': vdw_data = self.vdw_analysis.amber.get_radii() else: print("Not valid VdW Radius type") ################################################ vdw_unions_per_residue = self.get_union_vdw_circles_per_residue( vdw_data) profiles_per_residue = [] sequence = self.ampal[0].sequence for i in range(len(sequence)): vdw_union = vdw_unions_per_residue[i] profile = numpy.array(vdw_union.exterior.coords).T profiles_per_residue.append([sequence[i], i + 1, profile]) return profiles_per_residue def get_profile(self, xyz): N = len(xyz) r = xyz - self.COM # Coordinates relative to COM # Projection onto Assembly frame of reference A_x = numpy.dot(r, self.e_x).reshape((N, 1)) A_y = numpy.dot(r, self.e_y).reshape((N, 1)) e_x = self.e_x.reshape((1, 3)) e_y = self.e_y.reshape((1, 3)) ################################################ r_xy = numpy.matmul(A_x, e_x) + numpy.matmul(A_y, e_y) d_xy = numpy.linalg.norm(r_xy, axis=1) ################################################ r_z = numpy.dot(r, self.e_z) ################################################ profile = [r_z, d_xy] return profile def get_vdw_circles(self, vdw_data): Z, R = self.punctual() VdW_Radii = [float(x[-1]) for x in vdw_data] data = list(zip(Z, R, VdW_Radii)) circles = [Point(r_z, d_xy).buffer(vdwr) for r_z, d_xy, vdwr in data] return circles def get_union_vdw_circles(self, vdw_data): circles = self.get_vdw_circles(vdw_data) return cascaded_union(circles) def get_union_vdw_circles_per_residue(self, vdw_data): circles = self.get_vdw_circles(vdw_data) ################################################ chain = self.ampal[0] tags = [x.unique_id for x in list(chain.get_atoms())] ################################################ # Get sorted list of residue numbers resnum = sorted(list(map(int, set(map(lambda x: x[1], tags))))) ################################################ # Get atom indices corresponding to each residue indices_per_residue = [[x[-1] - 1 for x in tags if x[1] == str(n)] for n in resnum] ################################################ union_per_residue = [] for indices in indices_per_residue: union_residue = cascaded_union(itemgetter(*indices)(circles)) union_per_residue.append(union_residue) ################################################ return union_per_residue
import math, sys, pygame, pdb, euclid from rigid_body import RigidBody from rigid_body import pygame_to_euclid_vector half_size = pygame.math.Vector2(10, 15) rb = RigidBody(half_size, 10) rb_pos = pygame.math.Vector2(0, 0) rb_rot = math.pi / 2 rb.set_position(rb_pos, rb_rot) rb.vel = pygame.math.Vector2(5, 5) rb.ang_vel = 5.0 rel = pygame.math.Vector2(0, 5) world = rb.relative_to_world(rel) ground_vel = rb.point_vel(world) wheel_vel = rb.world_to_relative(ground_vel) print("rel ="+str(rel)) print("world ="+str(world)) print("grnd_vel="+str(ground_vel)) print("whl_vel ="+str(wheel_vel)) print("---------------------")
class Car(pygame.sprite.Sprite): THROTTLE_TORQUE = 10.0 BRAKE_TORQUE = 15.0 MASS = 4 # Should the car stay centered on the screen? STAY_CENTERED = False def __init__(self, image, screen_pos, world_pos): pygame.sprite.Sprite.__init__(self) self.world_pos = world_pos self.screen_pos = screen_pos self.delta_pos = pygame.math.Vector2() self.src_image = pygame.image.load(image) # Correct for car being upside-down self.src_image = pygame.transform.rotate(self.src_image, 180) self.rect = self.src_image.get_rect() self.rect.center = screen_pos self.aabb = self.rect self.direction = 3 * math.pi / 2 self.bounding_box = BoundingBox(self.aabb, self.direction) self.forward = self.reverse = self.left = self.right = False self.force_line_pairs = [] self.vel_line_pairs = [] # Our rigid body simulator half_size = pygame.math.Vector2(self.rect.width * 0.25, self.rect.height * 0.25) self.rigid_body = RigidBody(2 * half_size, self.MASS) wheel_pos = [] wheel_pos.append(half_size) wheel_pos.append(pygame.math.Vector2(half_size.x * -1, half_size.y)) wheel_pos.append(half_size * -1) wheel_pos.append(pygame.math.Vector2(half_size.x, half_size.y * -1)) self.wheels = [] self.wheels.append(Wheel(wheel_pos[0], 1)) self.wheels.append(Wheel(wheel_pos[1], 1)) self.wheels.append(Wheel(wheel_pos[2], 1)) self.wheels.append(Wheel(wheel_pos[3], 1)) self.rigid_body.set_position(self.world_pos, self.direction) # angle in degrees counter-clockwise def get_angle(self): return self.rigid_body.angle# * 180.0 / math.pi def get_ang_vel(self): return self.rigid_body.ang_vel def get_vel(self): return self.rigid_body.vel.length() # angle in degrees counter-clockwise def get_steering(self): return self.steering * 180.0 / math.pi def get_throttle(self): return self.throttle def get_brake(self): return self.brake def __str__(self): output = "throttle=" + str(self.get_throttle()) + "\n" output += "brake =" + str(self.get_brake()) + "\n" output += "steering=" + "{:3.1f}".format(self.get_steering()) + "\n" output += "velocity=" + "{:3.1f}".format(self.get_vel()) + "\n" output += "angle =" + "{:3.1f}".format(self.get_angle()) + "\n" output += "ang vel =" + "{:3.1f}".format(self.get_ang_vel()) + "\n" for wheel in self.wheels: output += "wheel=" + str(wheel) + "\n" return output # Returns an array of points which can be used to draw lines representing the car's wheels def get_wheel_lines(self): line_length = 20.0 wheel_vec_pairs = [] # Rotate front wheels with steering angle rot_matrix = euclid.Matrix3.new_rotate(self.steering) for wheel in self.wheels[:2]: pos = wheel.pos vec = euclid.Vector2(0, -line_length / 2) wheel_vec_pair = [] wheel_vec_pair.append(pygame.math.Vector2(rot_matrix * vec) + pos) vec.y = line_length / 2 wheel_vec_pair.append(pygame.math.Vector2(rot_matrix * vec) + pos) wheel_vec_pairs.append(wheel_vec_pair) for wheel in self.wheels[2:]: pos = wheel.pos wheel_vec_pair = [] wheel_vec_pair.append(pygame.math.Vector2(pos.x, pos.y - line_length / 2)) wheel_vec_pair.append(pygame.math.Vector2(pos.x, pos.y + line_length / 2)) wheel_vec_pairs.append(wheel_vec_pair) wheel_point_pairs = [] for pair in wheel_vec_pairs: point_pair = [] for vec in pair: vec_rot = self.rigid_body.relative_to_world(vec) point_pair.append( (vec_rot.x, vec_rot.y) ) wheel_point_pairs.append(point_pair) #print("("+str(vec.x)+","+str(vec.y)+" => ("+str(vec_rot.x)+","+str(vec_rot.y)+")") return wheel_point_pairs def get_force_lines(self): force_lines = self.force_line_pairs print("num pairs="+str(len(self.force_line_pairs))) self.force_line_pairs = [] return force_lines def get_vel_lines(self): vel_lines = self.vel_line_pairs self.vel_line_pairs = [] return vel_lines def set_steering(self, theta): self.wheels[0].set_steering_angle(theta) self.wheels[1].set_steering_angle(theta) def set_throttle(self, throttle): self.wheels[1].add_torque(self.THROTTLE_TORQUE * throttle) self.wheels[0].add_torque(self.THROTTLE_TORQUE * throttle) self.wheels[2].add_torque(self.THROTTLE_TORQUE * throttle) self.wheels[3].add_torque(self.THROTTLE_TORQUE * throttle) def set_brake(self, brake): for wheel in self.wheels: torque = self.BRAKE_TORQUE * -wheel.vel * brake wheel.add_torque(torque) def updateKey(self, key, pressed): if key == pygame.K_UP: self.forward = pressed elif key == pygame.K_DOWN: self.reverse = pressed elif key == pygame.K_LEFT: self.right = pressed elif key == pygame.K_RIGHT: self.left = pressed def processKeys(self): if self.forward: self.throttle = 100 else: self.throttle = 0 if self.reverse: self.brake = 1 else: self.brake = 0 self.set_brake(self.brake) self.set_throttle(self.throttle) if self.left: self.steering = math.pi / 4 elif self.right: self.steering = -math.pi / 4 else: self.steering = 0 self.set_steering(self.steering) def update(self, deltat): self.processKeys() num = 0 for wheel in self.wheels: rel_ground_vel = self.rigid_body.rel_point_vel(wheel.pos) vel_pair = [] vel_pair.append(self.rigid_body.relative_to_world(wheel.pos)) vel_pair.append(self.rigid_body.relative_to_world(wheel.pos) + self.rigid_body.rot_relative_to_world(rel_ground_vel)) self.vel_line_pairs.append(vel_pair) rel_response_force = wheel.calculate_force(rel_ground_vel, deltat) response_force_offset = self.rigid_body.rot_relative_to_world(rel_response_force) + self.rigid_body.relative_to_world(wheel.pos) force_pair = [] force_pair.append(self.rigid_body.relative_to_world(wheel.pos)) force_pair.append( (response_force_offset.x, response_force_offset.y) ) self.force_line_pairs.append(force_pair) self.rigid_body.add_rel_force(rel_response_force, wheel.pos) self.rigid_body.update(deltat) self.direction = self.rigid_body.angle self.delta_pos += self.rigid_body.pos - self.world_pos self.world_pos = self.rigid_body.pos self.image = pygame.transform.rotate(self.src_image, -1 * self.direction * 180 / math.pi) # degrees counter-clockwise self.rect = self.image.get_rect() self.rect.center = self.screen_pos if self.STAY_CENTERED else self.rigid_body.pos self.aabb.center = self.rigid_body.pos self.bounding_box = BoundingBox(self.aabb, self.direction) def collide(self): self.speed = 0 self.position = self.old_position self.direction = self.old_direction self.image = pygame.transform.rotate(self.src_image, self.direction) self.rect = self.image.get_rect() self.rect.center = self.position self.aabb.center = self.position self.bounding_box = BoundingBox(self.aabb, -1 * self.direction)
def __init__(self, radius=1, length=1, density=1): self.radius = radius self.length = length self.density = density self.mass, self.jsa = jsa_cylinder(radius, length, density) RigidBody.__init__(self, self.mass, self.jsa, self.length)
class Car(pygame.sprite.Sprite): THROTTLE_TORQUE = 10.0 BRAKE_TORQUE = 15.0 MASS = 4 # Should the car stay centered on the screen? STAY_CENTERED = False def __init__(self, image, screen_pos, world_pos): pygame.sprite.Sprite.__init__(self) self.world_pos = world_pos self.screen_pos = screen_pos self.delta_pos = pygame.math.Vector2() self.src_image = pygame.image.load(image) # Correct for car being upside-down self.src_image = pygame.transform.rotate(self.src_image, 180) self.rect = self.src_image.get_rect() self.rect.center = screen_pos self.aabb = self.rect self.direction = 3 * math.pi / 2 self.bounding_box = BoundingBox(self.aabb, self.direction) self.forward = self.reverse = self.left = self.right = False self.force_line_pairs = [] self.vel_line_pairs = [] # Our rigid body simulator half_size = pygame.math.Vector2(self.rect.width * 0.25, self.rect.height * 0.25) self.rigid_body = RigidBody(2 * half_size, self.MASS) wheel_pos = [] wheel_pos.append(half_size) wheel_pos.append(pygame.math.Vector2(half_size.x * -1, half_size.y)) wheel_pos.append(half_size * -1) wheel_pos.append(pygame.math.Vector2(half_size.x, half_size.y * -1)) self.wheels = [] self.wheels.append(Wheel(wheel_pos[0], 1)) self.wheels.append(Wheel(wheel_pos[1], 1)) self.wheels.append(Wheel(wheel_pos[2], 1)) self.wheels.append(Wheel(wheel_pos[3], 1)) self.rigid_body.set_position(self.world_pos, self.direction) # angle in degrees counter-clockwise def get_angle(self): return self.rigid_body.angle # * 180.0 / math.pi def get_ang_vel(self): return self.rigid_body.ang_vel def get_vel(self): return self.rigid_body.vel.length() # angle in degrees counter-clockwise def get_steering(self): return self.steering * 180.0 / math.pi def get_throttle(self): return self.throttle def get_brake(self): return self.brake def __str__(self): output = "throttle=" + str(self.get_throttle()) + "\n" output += "brake =" + str(self.get_brake()) + "\n" output += "steering=" + "{:3.1f}".format(self.get_steering()) + "\n" output += "velocity=" + "{:3.1f}".format(self.get_vel()) + "\n" output += "angle =" + "{:3.1f}".format(self.get_angle()) + "\n" output += "ang vel =" + "{:3.1f}".format(self.get_ang_vel()) + "\n" for wheel in self.wheels: output += "wheel=" + str(wheel) + "\n" return output # Returns an array of points which can be used to draw lines representing the car's wheels def get_wheel_lines(self): line_length = 20.0 wheel_vec_pairs = [] # Rotate front wheels with steering angle rot_matrix = euclid.Matrix3.new_rotate(self.steering) for wheel in self.wheels[:2]: pos = wheel.pos vec = euclid.Vector2(0, -line_length / 2) wheel_vec_pair = [] wheel_vec_pair.append(pygame.math.Vector2(rot_matrix * vec) + pos) vec.y = line_length / 2 wheel_vec_pair.append(pygame.math.Vector2(rot_matrix * vec) + pos) wheel_vec_pairs.append(wheel_vec_pair) for wheel in self.wheels[2:]: pos = wheel.pos wheel_vec_pair = [] wheel_vec_pair.append( pygame.math.Vector2(pos.x, pos.y - line_length / 2)) wheel_vec_pair.append( pygame.math.Vector2(pos.x, pos.y + line_length / 2)) wheel_vec_pairs.append(wheel_vec_pair) wheel_point_pairs = [] for pair in wheel_vec_pairs: point_pair = [] for vec in pair: vec_rot = self.rigid_body.relative_to_world(vec) point_pair.append((vec_rot.x, vec_rot.y)) wheel_point_pairs.append(point_pair) #print("("+str(vec.x)+","+str(vec.y)+" => ("+str(vec_rot.x)+","+str(vec_rot.y)+")") return wheel_point_pairs def get_force_lines(self): force_lines = self.force_line_pairs print("num pairs=" + str(len(self.force_line_pairs))) self.force_line_pairs = [] return force_lines def get_vel_lines(self): vel_lines = self.vel_line_pairs self.vel_line_pairs = [] return vel_lines def set_steering(self, theta): self.wheels[0].set_steering_angle(theta) self.wheels[1].set_steering_angle(theta) def set_throttle(self, throttle): self.wheels[1].add_torque(self.THROTTLE_TORQUE * throttle) self.wheels[0].add_torque(self.THROTTLE_TORQUE * throttle) self.wheels[2].add_torque(self.THROTTLE_TORQUE * throttle) self.wheels[3].add_torque(self.THROTTLE_TORQUE * throttle) def set_brake(self, brake): for wheel in self.wheels: torque = self.BRAKE_TORQUE * -wheel.vel * brake wheel.add_torque(torque) def updateKey(self, key, pressed): if key == pygame.K_UP: self.forward = pressed elif key == pygame.K_DOWN: self.reverse = pressed elif key == pygame.K_LEFT: self.right = pressed elif key == pygame.K_RIGHT: self.left = pressed def processKeys(self): if self.forward: self.throttle = 100 else: self.throttle = 0 if self.reverse: self.brake = 1 else: self.brake = 0 self.set_brake(self.brake) self.set_throttle(self.throttle) if self.left: self.steering = math.pi / 4 elif self.right: self.steering = -math.pi / 4 else: self.steering = 0 self.set_steering(self.steering) def update(self, deltat): self.processKeys() num = 0 for wheel in self.wheels: rel_ground_vel = self.rigid_body.rel_point_vel(wheel.pos) vel_pair = [] vel_pair.append(self.rigid_body.relative_to_world(wheel.pos)) vel_pair.append( self.rigid_body.relative_to_world(wheel.pos) + self.rigid_body.rot_relative_to_world(rel_ground_vel)) self.vel_line_pairs.append(vel_pair) rel_response_force = wheel.calculate_force(rel_ground_vel, deltat) response_force_offset = self.rigid_body.rot_relative_to_world( rel_response_force) + self.rigid_body.relative_to_world( wheel.pos) force_pair = [] force_pair.append(self.rigid_body.relative_to_world(wheel.pos)) force_pair.append( (response_force_offset.x, response_force_offset.y)) self.force_line_pairs.append(force_pair) self.rigid_body.add_rel_force(rel_response_force, wheel.pos) self.rigid_body.update(deltat) self.direction = self.rigid_body.angle self.delta_pos += self.rigid_body.pos - self.world_pos self.world_pos = self.rigid_body.pos self.image = pygame.transform.rotate( self.src_image, -1 * self.direction * 180 / math.pi) # degrees counter-clockwise self.rect = self.image.get_rect() self.rect.center = self.screen_pos if self.STAY_CENTERED else self.rigid_body.pos self.aabb.center = self.rigid_body.pos self.bounding_box = BoundingBox(self.aabb, self.direction) def collide(self): self.speed = 0 self.position = self.old_position self.direction = self.old_direction self.image = pygame.transform.rotate(self.src_image, self.direction) self.rect = self.image.get_rect() self.rect.center = self.position self.aabb.center = self.position self.bounding_box = BoundingBox(self.aabb, -1 * self.direction)