def generate_marker_obstacle(self, obj): sx, sy, sz = obj.size r = Rectangle(center=transform.point(obj.x + sx / 2, obj.y), dimensions=(sx, sy), orient=obj.theta) r.obstacle = obj return r
def parts_to_node(self,node): parts = [] for part in self.robot_parts: center = transform.point(part.center[0,0]+node.x, part.center[1,0]+node.y) tmat = transform.translate(center[0,0],center[1,0]) \ .dot(transform.aboutZ(part.orient)) this_part = part.instantiate(tmat) parts.append(this_part) return parts
def __init__(self,robot): base_frame = Joint('base', collision_model=Rectangle(transform.point(), dimensions=(95,60))) # cor is center of rotation cor_frame = Joint('cor', parent=base_frame, r=-19.) # Use link instead of joint for world_frame world_frame = Joint('world', parent=base_frame, type='world', getter=self.get_world) front_axle_frame = Joint('front_axle', parent=base_frame, alpha=pi/2) back_axle_frame = Joint('back_axle', parent=base_frame, r=-46., alpha=pi/2) # This frame is on the midline. Could add separate left and right shoulders. # Positive angle is up, so z must point to the right. # x is forward, y points up. shoulder_frame = Joint('shoulder', parent=base_frame, type='revolute', getter=self.get_shoulder, d=21., r=-39., alpha=pi/2) lift_attach_frame = \ Joint('lift_attach', parent=shoulder_frame, type='revolute', getter=self.get_lift_attach, r=66., collision_model=Circle(transform.point(), radius=10)) # Positive head angle is up, so z must point to the right. # With x pointing forward, y must point up. head_frame = Joint('head', parent=base_frame, type='revolute', getter=self.get_head, d=35., r=-10., alpha=pi/2) # Dummy joint located below head joint at level of the camera frame, # and x axis points down, z points forward, y points left camera_dummy = Joint('camera_dummy', parent=head_frame, theta=-pi/2, r=-7.5, alpha=-pi/2) # x axis points right, y points down, z points forward camera_frame = Joint('camera', parent=camera_dummy, d=15., theta=-pi/2) joints = [base_frame, world_frame, cor_frame, front_axle_frame, back_axle_frame, shoulder_frame, lift_attach_frame, head_frame, camera_dummy, camera_frame] super().__init__(joints,robot)
def generate_foreign_obstacle(self, obj): r = Rectangle(center=transform.point(obj.x, obj.y), dimensions=(obj.size[0:2]), orient=obj.theta) r.obstacle = obj return r
def generate_chip_obstacle(self, obj): r = Circle(center=transform.point(obj.x, obj.y), radius=obj.radius) r.obstacle = obj return r
def __init__(self): self.center = transform.point() self.rotmat = transform.identity()
def __init__(self, center=transform.point(), radius=25 / 2): super().__init__() self.center = center self.radius = radius self.orient = 0.
def generate_cube_obstacle(self,obj): r = Rectangle(center=transform.point(obj.x, obj.y), dimensions=obj.size[0:2], orient=obj.theta) return r
def __init__(self, robot): base_frame = Joint( 'base', description='Base frame: the root of the kinematic tree') # cor is center of rotation cor_frame = Joint('cor', parent=base_frame, description='Center of rotation', r=-19., collision_model=Rectangle(transform.point(), dimensions=(95, 60))) # Use link instead of joint for world_frame world_frame = Joint( 'world', parent=base_frame, type='world', getter=self.get_world, description='World origin in base frame coordinates', qmin=None, qmax=None) front_axle_frame = Joint('front_axle', parent=base_frame, description='Center of the front axle', alpha=pi / 2) back_axle_frame = Joint('back_axle', parent=base_frame, r=-46., alpha=pi / 2) # This frame is on the midline. Could add separate left and right shoulders. # Positive angle is up, so z must point to the right. # x is forward, y points up. shoulder_frame = Joint( 'shoulder', parent=base_frame, type='revolute', getter=self.get_shoulder, description='Rotation axis of the lift; z points to the right', qmin=cozmo.robot.MIN_LIFT_ANGLE.radians, qmax=cozmo.robot.MAX_LIFT_ANGLE.radians, d=21., r=-39., alpha=pi / 2) lift_attach_frame = \ Joint('lift_attach', parent=shoulder_frame, type='revolute', description='Tip of the lift, where cubes attach; distal end of four-bar linkage', getter=self.get_lift_attach, r=66., qmax = - cozmo.robot.MIN_LIFT_ANGLE.radians, qmin = - cozmo.robot.MAX_LIFT_ANGLE.radians, #collision_model=Circle(transform.point(), radius=10)) ) # Positive head angle is up, so z must point to the right. # With x pointing forward, y must point up. head_frame = Joint( 'head', parent=base_frame, type='revolute', getter=self.get_head, description='Axis of head rotation; z points to the right', qmin=cozmo.robot.MIN_HEAD_ANGLE.radians, qmax=cozmo.robot.MAX_HEAD_ANGLE.radians, d=35., r=-10., alpha=pi / 2) # Dummy joint located below head joint at level of the camera frame, # and x axis points down, z points forward, y points left camera_dummy = Joint( 'camera_dummy', parent=head_frame, description= 'Dummy joint below the head, at the level of the camera frame', theta=-pi / 2, r=7.5, alpha=-pi / 2) # x axis points right, y points down, z points forward camera_frame = Joint( 'camera', parent=camera_dummy, description='Camera reference frame; y is down and z is outward', d=15., theta=-pi / 2) joints = [ base_frame, world_frame, cor_frame, front_axle_frame, back_axle_frame, shoulder_frame, lift_attach_frame, head_frame, camera_dummy, camera_frame ] super().__init__(joints, robot)