def __init__(self,
beams,
servo_controller=None,
parked_state=None,
servo_map=None,
avg_speed=15.0,
dt=0.007):
self.beams = dict(
zip(['arm', 'forearm', 'gripper'], map(Vector, beams)))
if servo_controller is not None:
self.sc = servo_controller
else:
self.sc = NullServo()
self.current_state = dict()
# set up parked_state
p = sum(self.beams.values())
ga = 0.0
g = 0.0
wr = 0.0
self.parked_state = dict(pos=p, grip_angle=ga, grip=g, wrist_rotate=wr)
if parked_state is not None:
self.parked_state.update(parked_state)
if servo_map is not None:
self.servo_map = servo_map
else:
self.servo_map = SERVO_MAP
self.avg_speed = avg_speed
self.dt = dt
self.immediate_move(self.parked_state)
