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)
