def create_bodies(): width = 3 height = 2 body1 = vp.box(pos=vp.vector(-5, 0, 0), axis=vp.vector(0, 0, 1), width=width, height=height, arrow=vp.arrow(pos=vp.vector(0, 0, 0), shaftwidth=0.5, color=vp.color.red, visible=False), radius=1/2 * math.sqrt(width**2 + height**2), mass=10, # kg moment_inertia=100, # [kg*m^2] area=10, # [m^2] vel=vp.vector(0, 0, 0), # [m/s] ang_vel=vp.vector(0, 0, 0), # [rad/s^2] theta=vp.radians(20)) # [rad] width = 2 height = 3 body2 = vp.box(pos=vp.vector(3, 0.5, 0), axis=vp.vector(0, 0, 1), width=width, height=height, arrow=vp.arrow(pos=vp.vector(0, 0, 0), shaftwidth=0.5, color=vp.color.blue, visible=False), radius=1/2 * math.sqrt(width**2 + height**2), mass=10, # [kg] moment_inertia=100, # [kg*m^2] area=10, # [m^2] vel=vp.vector(0, 0, 0), # [m/s] ang_vel=vp.vector(0, 0, 0), # [rad/s^2] theta=vp.radians(0)) # [rad] bodies = [body1, body2] for body in bodies: body.vertices = vertices(body) body.rotate(angle=body.theta) return bodies
def init_small_arm(self): # cal axis 0, 1 axis_0 = self.upper_arm_axis.norm() axis_2 = self.upper_arm_subaxis.norm() axis_1 = axis_0.cross(axis_2).norm() # draw small_arm self.small_arm = cylinder(pos=self.joint_pos, axis=axis_0 * self.small_arm_len, radius=self.small_arm_rad, color=color.cyan, opacity=0.5) self.small_arm.rotate(axis=axis_1, angle=radians(self.small_arm_flex)) # draw xyz self.small_arm_x = arrow(pos=self.joint_pos, axis=axis_0 * self.note_len, color=color.red, radius=self.sketch_rad) self.small_arm_x.rotate(axis=axis_1, angle=radians(self.small_arm_flex)) self.small_arm_y = arrow(pos=self.joint_pos, axis=axis_1 * self.note_len, color=color.green, radius=self.sketch_rad) self.small_arm_z = arrow(pos=self.joint_pos, axis=axis_2 * self.note_len, color=color.blue, radius=self.sketch_rad) self.small_arm_z.rotate(axis=axis_1, angle=radians(self.small_arm_flex))
def wrap_to_pi(angle_type, angle): """ Wrap the given angle (deg or rad) to [-pi pi] :param angle_type: String of whether the angle is deg or rad :type angle_type: `str` :param angle: The angle to wrap :type angle: `float` :raises ValueError: Throws the error if the given string is not "deg" or "rad" :return: The wrapped angle :rtype: `float` """ if angle_type == "deg": angle = angle % 360 if angle > 180: angle -= 360 elif angle_type == "rad": angle = angle % radians(360) if angle > radians(180): angle -= radians(360) else: raise ValueError('angle_type must be "deg" or "rad"') return angle
def motion_no_drag(data): """ Create animation for projectile motion with no dragging force """ ball_nd = sphere(pos=vector(0, data['init_height'], 0), radius=1, color=color.cyan, make_trail=True) # Follow the movement of the ball scene.camera.follow(ball_nd) # Set initial velocity & position # Animate & store data["x1"] = [] data["y1"] = [] y0 = data["init_height"] x0 = 0 x, y = x0, y0 ball_nd.pos.x = x ball_nd.pos.y = y dt = data['deltat'] t = 0 vx0 = data['init_velocity'] * cos(radians(data['theta'])) vy0 = data['init_velocity'] * sin(radians(data['theta'])) while y >= 0: rate(1000) data["x1"].append(x) data["y1"].append(y) ball_nd.pos = vector(x, y, 0) t = t + dt x = x0 + vx0 * t y = y0 + vy0 * t + (data['gravity'] / 2) * (t**2)
def __init__(self, ): self.cav = vp.canvas() self.cav.camera.rotate(angle=vp.radians(180), axis=self.cav.up) self.cav.camera.rotate(angle=vp.radians(180), axis=self.cav.forward) joints_radius = 10 joints_color = vpvec([1, 1, 1]) joints_opacity = 0.8 line_color = vp.color.cyan line_radius = 5 self.joints = [ vp.sphere( canvas=self.cav, pos=vpvec([0, 0, 0]), radius=joints_radius * 0.9, color=joints_color, opacity=joints_opacity, ) for i in range(21) ] self.jointline = { c: vp.curve( canvas=self.cav, pos=[vpvec([0, 0, 0]), vpvec([0, 0, 0])], color=line_color, radius=line_radius, ) for c in connections }
def moving_taiqi(arm=arm): # Taiqi print('Taiqi starts ...') for _ in range(30): axis_ = arm.upper_arm_axis.cross(arm.upper_arm_subaxis).norm() arm.upper_arm_axis = arm.upper_arm_axis.rotate(axis=axis_, angle=radians(2)) arm.upper_arm_subaxis = arm.upper_arm_subaxis.rotate(axis=axis_, angle=radians(2)) rate(30) arm.update_parts() print('Taiqi done')
def make_robot(): chassis_width = 155 # left side to right chassis_thickness = 3 # plastic thickness chassis_length = 200 # front to back wheel_thickness = 26 wheel_diameter = 70 axle_x = 30 # wheel axle from axle_z = -20 rear_castor_position = vp.vector(-80, -6, -30) rear_castor_radius = 14 rear_caster_thickness = 12 base = vp.box(length=chassis_length, height=chassis_thickness, width=chassis_width) # rotate to match body - so Z is height and Y is width base.rotate(angle=vp.radians(90), axis=vp.vector(1, 0, 0)) wheel_dist = chassis_width / 2 wheel_l = vp.cylinder(radius=wheel_diameter / 2, length=wheel_thickness, pos=vp.vector(axle_x, -wheel_dist, axle_z), axis=vp.vector(0, -1, 0)) wheel_r = vp.cylinder(radius=wheel_diameter / 2, length=wheel_thickness, pos=vp.vector(axle_x, wheel_dist, axle_z), axis=vp.vector(0, 1, 0)) rear_castor = vp.cylinder(radius=rear_castor_radius, length=rear_caster_thickness, pos=rear_castor_position, axis=vp.vector(0, 1, 0)) return vp.compound([base, wheel_l, wheel_r, rear_castor])
def rotate(self, layer, n=45): # print(layer) layerIndex = self.faceIndexDict[layer] move = self.moves[layerIndex] for cycle in move: # print("cycle:",cycle) objectPositions = [vObject.pos for vObject in cycle] print("positions:", objectPositions) center = vpy.vector(0, 0, 0) for pos in objectPositions: center += pos center = center / len(objectPositions) # center = sum(objectPositions)/len(objectPositions) print("center of mass", center) for object1, object2 in zip(cycle, cycle[1:] + [cycle[0]]): print(object1, object2) object1pos = object1.pos - center object2pos = object2.pos - center coefficientMatrix = [[object1pos.x, object1pos.y], [object2pos.x, object2pos.y]] solutionVector = [-object1pos.z, -object2pos.z] # z = 1 x, y = np.linalg.solve(coefficientMatrix, solutionVector) print(f"axis: ({x},{y},1)") # y = ( (object1pos.z - object1pos.x) / (object2pos.y * object2pos.z) ) / ((object1pos.y - object1pos.x) / object2pos.y * object2pos.y ) # x = (object1pos.z - object1pos.y * y) / object1pos.x # z = -1 axis = vpy.vector(x, y, 1) for _ in range(n): object1.rotate(angle=vpy.radians(90 / n), axis=axis, origin=center) time.sleep(0.02)
def moving_waizhan(arm=arm): # Waizhan print('Waizhan starts ...') for _ in range(30): arm.upper_arm_axis = arm.upper_arm_axis.rotate( axis=arm.upper_arm_subaxis, angle=radians(-2)) rate(30) arm.update_parts() print('Waizhan done.')
def motion_quzhou(angle=radians(2)): print('Quzhou ...') for _ in range(55): rate(30) axis = joint_axis[0].axis.cross(joint_axis[1].axis) origin = joint_anchor.pos [e.rotate(origin=origin, angle=angle, axis=axis) for e in [small_arm, hand_anchor] + joint_axis] print('Done.')
def update(self, dt): accel_pitch, accel_roll = self.imu.read_accelerometer_pitch_and_roll() gyro = self.imu.read_gyroscope() # By filtering 95% gyro (which changes quickly, but drifts) with the 5% accel, which is absolute, but is slow when filtered # We get the best of both sensors. self.pitch = self.filter(self.pitch + gyro.y * dt, accel_pitch) self.roll = self.filter(self.roll + gyro.x * dt, accel_roll) # read the magnetometer mag = self.imu.read_magnetometer() # Compensate for pitch and tilt mag = mag.rotate(radians(self.pitch), vector(0, 1, 0)) mag = mag.rotate(radians(self.roll), vector(1, 0, 0)) mag_yaw = -degrees(atan2(mag.y, mag.x)) self.yaw = self.filter(self.yaw + gyro.z * dt, mag_yaw) print(mag_yaw, self.yaw)
def motion_waizhan(angle=radians(2/1.5)): print('Waizhan ...') for _ in range(30): rate(30) axis = - joint_axis[1].axis origin = shoulder_anchor.pos [e.rotate(origin=origin, angle=angle, axis=axis) for e in [upper_arm, joint_anchor, small_arm, hand_anchor] + shoulder_axis + joint_axis] print('Done.')
def rotate(self, layer, n=45): toplayer, axis = self.getLayer(layer) time.sleep(0.1) # print(axis) # vpy.arrow(canvas = self.canvas, axis=axis, pos=vpy.vector(5,0,0)) # vpy.arrow(canvas = self.canvas, axis=axis, pos=vpy.vector(0,5,0)) # vpy.arrow(canvas = self.canvas, axis=axis, pos=vpy.vector(0,0,5)) for i in range(n): for box in toplayer: box.rotate(angle=vpy.radians(90 / n), axis=axis, origin=R.zero) time.sleep(0.02)
def rotation_C0(ta, C0=C0): if C0.axis == ta: return True cv = C0.axis.norm().cross(ta.norm()) if cv.dot(cv) < 0.1: rate(100) C0.axis = ta.norm() * 10 return False rate(100) C0.rotate(angle=radians(1), axis=cv) return False
def motion_drag(data): """ Create animation for projectile motion with dragging force """ ball_wd = sphere(pos=vector(0, data['init_height'], 0), radius=1, color=color.orange, make_trail=True) scene.camera.follow(ball_wd) data["x2"] = [] data['vx2'] = [] data["y2"] = [] data['vy2'] = [] x = ball_wd.pos.x y = ball_wd.pos.y beta = data['beta'] g = data['gravity'] dt = data['deltat'] data['vx2'].append(data['init_velocity'] * cos(radians(data['theta']))) data['vy2'].append(data['init_velocity'] * sin(radians(data['theta']))) rate(1000) while y >= 0: speed = vector(x, y, 0).mag data['vx2'].append(data['vx2'][-1] * (1.0 - beta * speed * dt)) data['vy2'].append(data['vy2'][-1] + (g - beta * speed * data['vy2'][-1]) * dt) data["x2"].append(data['vx2'][-1] * dt + x) data["y2"].append(data['vy2'][-1] * dt + y) # ball_wd.pos = vector(data["x2"][-1], data["y2"][-1], 0) x = data["x2"][-1] y = data["y2"][-1] for x, y in zip(data["x2"], data["y2"]): rate(1000) ball_wd.pos.x = x ball_wd.pos.y = y
def update_small_arm(self): # cal axis 0, 1, 2 axis_0 = self.upper_arm_axis.norm() axis_2 = self.upper_arm_subaxis.norm() axis_1 = axis_0.cross(axis_2).norm() # update small_arm self.small_arm_x.pos = self.joint_pos self.small_arm_x.axis = axis_0 * self.note_len self.small_arm_x.rotate(axis=axis_1, angle=radians(self.small_arm_flex)) self.small_arm_y.pos = self.joint_pos self.small_arm_y.axis = axis_1 * self.note_len self.small_arm_z.pos = self.joint_pos self.small_arm_z.axis = axis_2 * self.note_len self.small_arm_z.rotate(axis=axis_1, angle=radians(self.small_arm_flex)) self.small_arm.pos = self.joint.pos self.small_arm.axis = axis_0 * self.small_arm_len self.small_arm.rotate(axis=axis_1, angle=radians(self.small_arm_flex))
def motion_taishou(angle=radians(2)): print('Taishouing ...') for _ in range(35): rate(30) if _ % 5 == 0: axis = joint_axis[0].axis.cross(joint_axis[1].axis) origin = joint_anchor.pos [e.rotate(origin=origin, angle=angle, axis=axis) for e in [small_arm, hand_anchor] + joint_axis] axis = shoulder_axis[0].axis.cross(shoulder_axis[1].axis) origin = shoulder_anchor.pos [e.rotate(origin=origin, angle=angle, axis=axis) for e in [upper_arm, joint_anchor, small_arm, hand_anchor] + shoulder_axis + joint_axis] print('Done.')
def motion_backing_small_arm(angle=radians(2)): # return False, if we are fine if all([abs(joint_axis[0].axis.diff_angle(small_arm_main_axis)) < angle, abs(joint_axis[1].axis.diff_angle(small_arm_sub_axis)) < angle]): return False origin = joint_anchor.pos # back main_axis if abs(joint_axis[0].axis.diff_angle(small_arm_main_axis)) > angle: axis = joint_axis[0].axis.cross(small_arm_main_axis) [e.rotate(origin=origin, axis=axis, angle=angle) for e in [small_arm, hand_anchor] + joint_axis] return True # back sub_axis if abs(joint_axis[1].axis.diff_angle(small_arm_sub_axis)) > angle: axis = joint_axis[1].axis.cross(small_arm_sub_axis) [e.rotate(origin=origin, axis=axis, angle=angle) for e in [small_arm, hand_anchor] + joint_axis] return True
def motion_shenchu(angle=radians(2)): print('Shenchuing ...') for _ in range(40): rate(30) axis = joint_axis[0].axis.cross(joint_axis[1].axis) origin = joint_anchor.pos [e.rotate(origin=origin, angle=angle, axis=axis) for e in [small_arm, hand_anchor] + joint_axis] time.sleep(0.5) for __ in range(2): for _ in range(30): rate(30) axis = shoulder_axis[0].axis.cross(shoulder_axis[1].axis) origin = shoulder_anchor.pos [e.rotate(origin=origin, angle=angle, axis=axis) for e in [upper_arm, joint_anchor, small_arm, hand_anchor] + shoulder_axis + joint_axis] axis = joint_axis[0].axis.cross(joint_axis[1].axis) origin = joint_anchor.pos [e.rotate(origin=origin, angle=-angle, axis=axis) for e in [small_arm, hand_anchor] + joint_axis] time.sleep(0.2) for _ in range(30): rate(30) axis = shoulder_axis[0].axis.cross(shoulder_axis[1].axis) origin = shoulder_anchor.pos [e.rotate(origin=origin, angle=-angle, axis=axis) for e in [upper_arm, joint_anchor, small_arm, hand_anchor] + shoulder_axis + joint_axis] axis = joint_axis[0].axis.cross(joint_axis[1].axis) origin = joint_anchor.pos [e.rotate(origin=origin, angle=angle, axis=axis) for e in [small_arm, hand_anchor] + joint_axis] print('Done.')
def create_bodies(): ramp = vp.box(pos=vp.vector(0, -0.25, 0), length=5, width=2.5, height=0.5, alpha=vp.radians(15)) # Minus, because function (vp.rotate) rotate counter-clockwise ramp.rotate(angle=-ramp.alpha, axis=vp.vector(0, 0, 1)) roller = vp.cylinder(pos=vp.vector( -1 * math.cos(ramp.alpha) * 0.5 * ramp.length, math.sin(ramp.alpha) * 0.5 * ramp.length + 0.5, -1), axis=vp.vector(0, 0, 2), radius=0.5, texture=texture(), vel=vp.vector(0, 0, 0), ang_vel=0, mass=1, friction_coeff=0.15) slope_dir = vp.norm(vp.rotate(vp.vector(1, 0, 0), angle=-ramp.alpha)) arrow = vp.arrow(pos=vp.vector(0, 2, 1), axis=slope_dir) return roller, ramp, arrow
def back_parts(self): # cal axis 0, 1, 2 axis_0 = self.upper_arm_axis.norm() axis_2 = self.upper_arm_subaxis.norm() # return False, if we are fine if all([(abs(axis_0.diff_angle(-self.up)) < radians(3)), (abs(axis_2.diff_angle(self.front)) < radians(3)), (self.small_arm_flex == 30)]): return False # back axis_0 if abs(axis_0.diff_angle(-self.up)) < radians(3): axis_0 = -self.up else: axis_0 = axis_0.rotate(axis=axis_0.cross(-self.up), angle=radians(2)) # back axis_2 if abs(axis_2.diff_angle(self.front)) < radians(3): axis_2 = self.front else: axis_2 = axis_2.rotate(axis=axis_2.cross(self.front), angle=radians(2)) # setting backed parameters self.upper_arm_axis = axis_0.norm() self.upper_arm_subaxis = axis_2.norm() if abs(self.small_arm_flex) - 30 < 3: self.small_arm_flex = 30 else: if self.small_arm_flex > 30: self.small_arm_flex -= 2 else: self.small_arm_flex += 2 return True
def rollRight(self, angle=22.5): self.upDirection = self.upDirection.rotate(radians(-angle), self.heading) self.leftDirection = self.leftDirection.rotate(radians(-angle), self.heading)
# coding: utf-8 from vpython import compound, color, vector, box, radians, rate box1 = box(pos=vector(1, 0, 0), length=1, width=2, height=3, color=color.red) box2 = box(pos=vector(5, 0, 0), length=1, width=2, height=3, color=color.blue) bb = compound([box1, box2]) print(bb) print(box2) while True: rate(30) bb.rotate(axis=vector(1, 0, 0), angle=radians(2)) # invalid for box1 and box2
from vpython import arrow, box, color, compound, radians, rate, vector O = vector(0, 0, 0) up = vector(1, 0, 0) right = vector(0, 1, 0) front = vector(0, 0, 1) arrow(pos=O, axis=up, color=color.red) arrow(pos=O, axis=right, color=color.green) arrow(pos=O, axis=front, color=color.blue) box1 = box(pos=O + vector(0, 10, 0), width=1, height=2, length=3, color=color.cyan, opacity=0.5) box2 = box(pos=O + vector(0, -10, 0), width=1, height=2, length=3, color=color.cyan, opacity=0.5) cb = compound([box1, box2]) while True: rate(30) cb.rotate(axis=up, angle=radians(2))
bbox.shininess = 0 # body init body = draw_part('body') skincolor = vector(1, 0.7, 0.2) # right_leg init draw_part('upper_right_leg', skincolor) upper_right_leg = draw_part('upper_right_leg', skincolor) small_right_leg = draw_part('small_right_leg', skincolor) # right_leg sit motion main_axis = (p0_right_leg-p1_right_leg).norm() sub_axis = main_axis.cross(front).cross(main_axis).norm() small_right_leg.rotate(origin=p1_right_leg, axis=sub_axis.cross(main_axis), angle=radians(90)) s = sphere(pos=p1_right_leg+vector(1, 0, 0), radius=1, color=skincolor) main_axis = (p1_right_leg-p2_right_leg).norm() sub_axis = main_axis.cross(front).cross(main_axis).norm() [e.rotate(origin=p2_right_leg, axis=main_axis.cross(sub_axis), angle=radians(90)) for e in [upper_right_leg, small_right_leg, s]] # left_leg init draw_part('upper_left_leg', skincolor) upper_left_leg = draw_part('upper_left_leg', skincolor) small_left_leg = draw_part('small_left_leg', skincolor) # left_leg sit motion main_axis = (p0_left_leg-p1_left_leg).norm() sub_axis = main_axis.cross(front).cross(main_axis).norm() small_left_leg.rotate(origin=p1_left_leg, axis=sub_axis.cross(main_axis), angle=radians(90))
def get_mappings_score(self, target_aabb, source_aabb): """ Returns analogy scores for all mappings of two AABB objects. Args: target_aabb(AABB): The target AABB that we want to map to source AABB. source_aabb(AABB): The source AABB that from source scene. Returns: Analogy scores for all mappings (sequence permutations) of two AABB objects based on collided sides simmilarity, ratio of AABBs, matching surfaces. """ # collision match weights exact_collision_weight = 0.99 #0.9 partial_collision_weight = 0.5 #0.4 no_collision_match_weight = .0 #-0.1 # surface match weights top_match_weight = .02 #0.1 bottom_match_weight = .02 #0.1 front_match_weight = .01 #0.01 back_match_weight = .01 #0.01 right_match_weight = .01 #0.01 left_match_weight = .01 #0.01 no_surf_match_weight = .0 #-0.01 # surface ratio difference penalties xy_ratio_weight = 0.1 zy_ratio_weight = 0.1 xz_ratio_weight = 0.1 mappings_score = [] for perm_sequence in self.all_permutations.keys(): score = 1.0 # default score for surface in self.all_permutations[perm_sequence].keys(): # scores for exact collision/partial/no-collision match if target_aabb.collided_sides[ surface] == source_aabb.collided_sides[ self.all_permutations[perm_sequence][surface]]: score += 1.0 * exact_collision_weight # match partially collided sides and fully collided sides elif target_aabb.collided_sides[ surface] != 0 and source_aabb.collided_sides[ self.all_permutations[perm_sequence] [surface]] != 0: score += 1.0 * partial_collision_weight else: # there is no match score += 1.0 * no_collision_match_weight # score for surface match if (surface == 'top' and self.all_permutations[perm_sequence][surface] == 'top'): score += 1.0 * top_match_weight elif (surface == 'bottom' and self.all_permutations[perm_sequence][surface] == 'bottom'): score += 1.0 * bottom_match_weight elif (surface == 'front' and self.all_permutations[perm_sequence][surface] == 'front'): score += 1.0 * front_match_weight elif (surface == 'back' and self.all_permutations[perm_sequence][surface] == 'back'): score += 1.0 * back_match_weight elif (surface == 'right' and self.all_permutations[perm_sequence][surface] == 'right'): score += 1.0 * right_match_weight elif (surface == 'left' and self.all_permutations[perm_sequence][surface] == 'left'): score += 1.0 * left_match_weight else: score += 1.0 * no_surf_match_weight # create vpython vec for xyz half size of the target object vpython_vec_xyz = vpython.vec(target_aabb.half_size[0], target_aabb.half_size[1], target_aabb.half_size[2]) # rotate it based on rotation sequence for rotation in reversed(perm_sequence): if rotation == 'x': vpython_vec_xyz = vpython.rotate( vpython_vec_xyz, angle=vpython.radians(-90), axis=vpython.vec(1, 0, 0)) elif rotation == 'y': vpython_vec_xyz = vpython.rotate( vpython_vec_xyz, angle=vpython.radians(-90), axis=vpython.vec(0, 1, 0)) # compare xy, zy, xz ratios of target and source aabbs after rotation # and penalise for difference xy_ratio_diff = abs( (abs(vpython_vec_xyz.x) / abs(vpython_vec_xyz.y)) - (source_aabb.half_size[0] / source_aabb.half_size[1])) xy_ratio_diff *= xy_ratio_weight zy_ratio_diff = abs( (abs(vpython_vec_xyz.z) / abs(vpython_vec_xyz.y)) - (source_aabb.half_size[2] / source_aabb.half_size[1])) zy_ratio_diff *= zy_ratio_weight xz_ratio_diff = abs( (abs(vpython_vec_xyz.x) / abs(vpython_vec_xyz.z)) - (source_aabb.half_size[0] / source_aabb.half_size[2])) xz_ratio_diff *= xz_ratio_weight sum_ratio_diff = xy_ratio_diff + zy_ratio_diff + xz_ratio_diff # penalise for ratio difference score -= sum_ratio_diff mappings_score.append((score, perm_sequence)) return mappings_score
def __create_object(self): """ Create the physical graphical object :returns: The graphical entity :rtype: class:`vpython.baseobj` """ if self.shape == '': # 2D coords of the circle boundary shape_path = shapes.circle(radius=self.__marker_size / 2) elif self.shape == '+': # 2D coords of the cross boundary shape_path = shapes.cross(width=self.__marker_size, thickness=self.__marker_size / 5) elif self.shape == 'o': # 2D coords of the circle boundary shape_path = shapes.circle(radius=self.__marker_size / 2) elif self.shape == '*': # 2D coords of the star boundary shape_path = shapes.star(radius=self.__marker_size / 2, n=6) elif self.shape == '.': # 2D coords of the square boundary shape_path = shapes.rectangle(width=self.__marker_size, height=self.__marker_size) elif self.shape == 'x': # 2D coords of the cross boundary shape_path = shapes.cross(width=self.__marker_size, thickness=self.__marker_size / 5, rotate=radians(45)) elif self.shape == 's': # 2D coords of the square boundary shape_path = shapes.rectangle(width=self.__marker_size, height=self.__marker_size, thickness=self.__marker_size) elif self.shape == 'd': # 2D coords of the diamond boundary shape_path = shapes.rectangle(width=self.__marker_size, height=self.__marker_size, thickness=self.__marker_size, rotate=radians(45)) elif self.shape == '^': # 2D coords of the triangle boundary shape_path = shapes.triangle(length=self.__marker_size) elif self.shape == 'v': # 2D coords of the triangle boundary shape_path = shapes.triangle(length=self.__marker_size, rotate=radians(180)) elif self.shape == '<': # 2D coords of the triangle boundary shape_path = shapes.triangle(length=self.__marker_size, rotate=radians(90)) elif self.shape == '>': # 2D coords of the triangle boundary shape_path = shapes.triangle(length=self.__marker_size, rotate=radians(-90)) elif self.shape == 'p': # 2D coords of the pentagon boundary shape_path = shapes.pentagon(length=self.__marker_size) elif self.shape == 'h': # 2D coords of the hexagon boundary shape_path = shapes.hexagon(length=self.__marker_size) # CURRENTLY UNUSED # elif self.__shape == 'o': # # 2D coords of the octagon boundary # shape_path = shapes.octagon(length=1) # elif self.__shape == 'r': # # 2D coords of the ring boundary (with thickness = 10%) # shape_path = shapes.circle(radius=0.5, thickness=0.1) else: raise ValueError("Invalid shape given") # Create the shape x = self.se2.t[0] y = self.se2.t[1] obj = extrusion(scene=self.scene, path=[vector(x, y, 0.001), vector(x, y, -0.001)], shape=shape_path, color=self.colourVec, shininess=0) self.size = obj.size if self.shape == '': obj.visible = False return obj
def _create_hab(self, entity: Entity, texture: str) -> \ vpython.compound: def vertex(x: float, y: float, z: float) -> vpython.vertex: return vpython.vertex(pos=vpython.vector(x, y, z)) # See the docstring of ThreeDeeObj._create_obj for why the dimensions # that define the shape of the habitat will not actually directly # translate to world-space. body = vpython.cylinder(pos=vpython.vec(0, 0, 0), axis=vpython.vec(-5, 0, 0), radius=10) head = vpython.cone(pos=vpython.vec(0, 0, 0), axis=vpython.vec(2, 0, 0), radius=10) wing = vpython.triangle(v0=vertex(0, 0, 0), v1=vertex(-5, 30, 0), v2=vertex(-5, -30, 0)) wing2 = vpython.triangle(v0=vertex(0, 0, 0), v1=vertex(-5, 0, 30), v2=vertex(-5, 0, -30)) hab = vpython.compound([body, head, wing, wing2], make_trail=True, texture=texture) hab.axis = calc.angle_to_vpy(entity.heading) hab.radius = entity.r / 2 hab.shininess = 0.1 hab.length = entity.r * 2 boosters: List[vpython.cylinder] = [] body_radius = entity.r / 8 for quadrant in range(0, 4): # Generate four SRB bodies. normal = vpython.rotate(vpython.vector(0, 1, 1).hat, angle=quadrant * vpython.radians(90), axis=vpython.vector(1, 0, 0)) boosters.append( vpython.cylinder(radius=self.BOOSTER_RADIUS, pos=(self.BOOSTER_RADIUS + body_radius) * normal)) boosters.append( vpython.cone( radius=self.BOOSTER_RADIUS, length=0.2, pos=((self.BOOSTER_RADIUS + body_radius) * normal + vpython.vec(1, 0, 0)))) # Append an invisible point to shift the boosters down the fuselage. # For an explanation of why that matters, read the # ThreeDeeObj._create_obj docstring (and if that doesn't make sense, # get in touch with Patrick M please hello hi I'm always free!) boosters.append(vpython.sphere(opacity=0, pos=vpython.vec(1.2, 0, 0))) booster_texture = texture.replace(f'{entity.name}.jpg', 'SRB.jpg') hab.boosters = vpython.compound(boosters, texture=booster_texture) hab.boosters.length = entity.r * 2 hab.boosters.axis = hab.axis parachute: List[vpython.standardAttributes] = [] string_length = entity.r * 0.5 parachute_texture = texture.replace(f'{entity.name}.jpg', 'Parachute.jpg') # Build the parachute fabric. parachute.append( vpython.extrusion( path=vpython.paths.circle(radius=0.5, up=vpython.vec(-1, 0, 0)), shape=vpython.shapes.arc(angle1=vpython.radians(5), angle2=vpython.radians(95), radius=1), pos=vpython.vec(string_length + self.PARACHUTE_RADIUS / 2, 0, 0))) parachute[0].height = self.PARACHUTE_RADIUS * 2 parachute[0].width = self.PARACHUTE_RADIUS * 2 parachute[0].length = self.PARACHUTE_RADIUS for quadrant in range(0, 4): # Generate parachute attachment lines. string = vpython.cylinder(axis=vpython.vec(string_length, self.PARACHUTE_RADIUS, 0), radius=0.2) string.rotate(angle=(quadrant * vpython.radians(90) - vpython.radians(45)), axis=vpython.vector(1, 0, 0)) parachute.append(string) parachute.append( vpython.sphere(opacity=0, pos=vpython.vec( -(string_length + self.PARACHUTE_RADIUS), 0, 0))) hab.parachute = vpython.compound(parachute, texture=parachute_texture) hab.parachute.visible = False return hab
import virtual_robot imu = RobotImu(gyro_offsets=imu_settings.gyro_offsets) pitch = 0 roll = 0 yaw = 0 model = virtual_robot.make_robot() virtual_robot.robot_view() latest = time.time() while True: vp.rate(1000) current = time.time() dt = current - latest latest = current gyro = imu.read_gyroscope() roll += gyro.x * dt pitch += gyro.y * dt yaw += gyro.z * dt # reset the model model.up = vp.vector(0, 1, 0) model.axis = vp.vector(1, 0, 0) # Reposition it model.rotate(angle=vp.radians(roll), axis=vp.vector(1, 0, 0)) model.rotate(angle=vp.radians(pitch), axis=vp.vector(0, 1, 0)) model.rotate(angle=vp.radians(yaw), axis=vp.vector(0, 0, 1))
model = virtual_robot.make_robot() virtual_robot.robot_view() compass = vp.canvas(width=400, height=400) vp.cylinder(radius=1, axis=vp.vector(0, 0, 1), pos=vp.vector(0, 0, -1)) needle = vp.arrow(axis=vp.vector(1, 0, 0), color=vp.color.red) vp.graph(xmin=0, xmax=60, scroll=True) graph_roll = vp.gcurve(color=vp.color.red) graph_pitch = vp.gcurve(color=vp.color.green) graph_yaw = vp.gcurve(color=vp.color.blue) timer = DeltaTimer() while True: vp.rate(100) dt, elapsed = timer.update() fusion.update(dt) # reset the model model.up = vp.vector(0, 1, 0) model.axis = vp.vector(1, 0, 0) # Reposition it model.rotate(angle=vp.radians(fusion.roll), axis=vp.vector(1, 0, 0)) model.rotate(angle=vp.radians(fusion.pitch), axis=vp.vector(0, 1, 0)) model.rotate(angle=vp.radians(fusion.yaw), axis=vp.vector(0, 0, 1)) needle.axis = vp.vector(vp.sin(vp.radians(fusion.yaw)), vp.cos(vp.radians(fusion.yaw)), 0) graph_roll.plot(elapsed, fusion.roll) graph_pitch.plot(elapsed, fusion.pitch) graph_yaw.plot(elapsed, fusion.yaw)