def generate_marker_obstacle(obj, obstacle_inflation=0):
sx,sy,sz = obj.size
r = Rectangle(center=geometry.point(obj.x+sx/2, obj.y),
dimensions=(sx+2*obstacle_inflation,sy+2*obstacle_inflation),
orient=obj.theta)
r.obstacle_id = obj.id
return r
def generate_cube_obstacle(obj, obstacle_inflation=0):
r = Rectangle(center=geometry.point(obj.x, obj.y),
dimensions=[
obj.size[0] + 2 * obstacle_inflation,
obj.size[1] + 2 * obstacle_inflation
],
orient=obj.theta)
r.obstacle_id = obj.id
return r
def generate_foreign_obstacle(obj):
r = Rectangle(center=geometry.point(obj.x, obj.y),
dimensions=(obj.size[0:2]),
orient=obj.theta)
r.obstacle_id = obj.id
return r
def generate_chip_obstacle(obj, obstacle_inflation=0):
r = Circle(center=geometry.point(obj.x, obj.y),
radius=obj.radius + obstacle_inflation)
r.obstacle_id = obj.id
return r
def __init__(self, center=geometry.point()):
if center is None: raise ValueError()
self.center = center
self.rotmat = geometry.identity()
self.obstacle_id = None # only store the string, so shape is pickle-able
def __init__(self, center=geometry.point(), radius=25 / 2):
super().__init__(center)
self.radius = radius
self.orient = 0.
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(geometry.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(geometry.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)
