Exemplo n.º 1
0
    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)
Exemplo n.º 2
0
    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)
Exemplo n.º 3
0
class Al5x(object):
    """Represents an AL5x robot arm from Lynxmotion

    The state of the arm is represented with by a dict with these keys:

        pos: the gripper position in 3-dimensions
        gripper_angle: the grippers angle from horizon in degrees
        grip: the grippers distance between fingers
        wrist_rotate: the angle from center in degrees

    Initiation and usage:

        arm  = Al5x(AL5D, servo_controller=None,
                    parked_state=dict(pos=(0,8,3)), dt=0.010)
        a.move(dict(pos=(-4,6,6), grip_angle=15.0, grip=0.0))
        a.move(dict(pos=(4,4,10), grip_angle=0.0, grip=0.5))
        a.park()

    """
    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)

    def get_state(self):
        """Return current state"""
        return dict(self.current_state)

    def __zip_state(self, new_state):
        """Returns combined state of current_state and new_state"""
        state = dict(self.current_state)
        state.update(new_state)
        return state

    def immediate_move(self, new_state, time=0):
        """Move arm to new_state without interpolation

        This bypasses the straight-line and accelleration calculations and
        simply finds the servo positions for the new state and steps servos to
        it. This will cause a non-linear gripper movement.

        """
        state = self.__zip_state(new_state)
        servos = dict()
        servos.update(self.calc_pos(state['pos'], state['grip_angle']))
        servos.update(self.calc_grip(state['grip']))
        self.sc.servos(servos, time)
        self.current_state.update(state)

    def park(self):
        """Moves arm to parked state"""
        self.move(self.parked_state)

    def calc_grip(self, val):
        """Calculate grip servo value, returns dict(servo)"""
        return dict({self.servo_map['grip']: val})

    def set_grip(self, val):
        self.sc.servos(self.calc_grip(val))
        self.current_state.update(dict(grip=val))

    def calc_pos(self, pos, grip_angle=0.0):
        """Calculate servo values for arm position, returns dict(servos)"""
        position = Vector(pos)
        grip_angle = float(grip_angle)
        # unit vector translation of position on xy plane
        xy_unit = Vector(position.x, position.y, 0).unit
        # get a grip... vector
        gripper = (xy_unit * self.beams['gripper'].mag)
        # ... and rotate to angle specified
        gripper = rotate(gripper, crossproduct(gripper, Z),
                         radians(grip_angle))
        # Subtract to get Sub Arm (sum of vectors 0 and 1)
        composite = position - gripper
        # Calculate sub-vectors
        # Get angle betweens
        try:
            arm2compangle = trisss([
                self.beams['arm'].mag,
                self.beams['forearm'].mag,
                composite.mag,
            ])[1]
        except ValueError, m:
            raise ValueError("Position is beyond range of motion")
        # get arm vector
        arm = composite.unit * self.beams['arm'].mag
        # ... and rotate to calculated angle
        arm = rotate(arm, crossproduct(arm, Z), arm2compangle)
        # the easy part...
        forearm = composite - arm
        # set servo values
        servo_values = dict()
        servo_values[self.servo_map['base']] = rad2float(angle(X, xy_unit))
        servo_values[self.servo_map['shoulder']] = rad2float(
            angle(xy_unit, arm))
        servo_values[self.servo_map['elbow']] = rad2float(angle(arm, forearm))
        servo_values[self.servo_map['wrist']] = rad2float(
            pi / 2 -
            angle(forearm, gripper) * sign(forearm.unit.z - gripper.unit.z))
        return servo_values
Exemplo n.º 4
0
class Al5x(object):
    """Represents an AL5x robot arm from Lynxmotion

    The state of the arm is represented with by a dict with these keys:

        pos: the gripper position in 3-dimensions
        gripper_angle: the grippers angle from horizon in degrees
        grip: the grippers distance between fingers
        wrist_rotate: the angle from center in degrees

    Initiation and usage:

        arm  = Al5x(AL5D, servo_controller=None,
                    parked_state=dict(pos=(0,8,3)), dt=0.010)
        a.move(dict(pos=(-4,6,6), grip_angle=15.0, grip=0.0))
        a.move(dict(pos=(4,4,10), grip_angle=0.0, grip=0.5))
        a.park()

    """
    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)

    def get_state(self):
        """Return current state"""
        return dict(self.current_state)

    def __zip_state(self, new_state):
        """Returns combined state of current_state and new_state"""
        state = dict(self.current_state)
        state.update(new_state)
        return state

    def immediate_move(self, new_state, time=0):
        """Move arm to new_state without interpolation

        This bypasses the straight-line and accelleration calculations and
        simply finds the servo positions for the new state and steps servos to
        it. This will cause a non-linear gripper movement.

        """
        state = self.__zip_state(new_state)
        servos = dict()
        servos.update(self.calc_pos(state['pos'], state['grip_angle']))
        servos.update(self.calc_grip(state['grip']))
        self.sc.servos(servos, time)
        self.current_state.update(state)

    def park(self):
        """Moves arm to parked state"""
        self.move(self.parked_state)

    def calc_grip(self, val):
        """Calculate grip servo value, returns dict(servo)"""
        return dict({self.servo_map['grip']: val})

    def set_grip(self, val):
        self.sc.servos(self.calc_grip(val))
        self.current_state.update(dict(grip=val))

    def calc_pos(self, pos, grip_angle=0.0):
        """Calculate servo values for arm position, returns dict(servos)"""
        position = Vector(pos)
        grip_angle = float(grip_angle)
        # unit vector translation of position on xy plane
        xy_unit = Vector(position.x, position.y, 0).unit
        # get a grip... vector
        gripper = (xy_unit * self.beams['gripper'].mag)
        # ... and rotate to angle specified
        gripper = rotate(gripper, crossproduct(gripper, Z),
                                       radians(grip_angle))
        # Subtract to get Sub Arm (sum of vectors 0 and 1)
        composite = position - gripper
        # Calculate sub-vectors
        # Get angle betweens
        try:
            arm2compangle = trisss([
                    self.beams['arm'].mag,
                    self.beams['forearm'].mag,
                    composite.mag,
                    ])[1]
        except ValueError, m:
            raise ValueError("Position is beyond range of motion")
        # get arm vector
        arm = composite.unit * self.beams['arm'].mag
        # ... and rotate to calculated angle
        arm = rotate(arm, crossproduct(arm, Z), arm2compangle)
        # the easy part...
        forearm = composite - arm
        # set servo values
        servo_values = dict()
        servo_values[self.servo_map['base']] = rad2float(angle(X, xy_unit))
        servo_values[self.servo_map['shoulder']] = rad2float(
                angle(xy_unit, arm))
        servo_values[self.servo_map['elbow']] = rad2float(angle(arm, forearm))
        servo_values[self.servo_map['wrist']] = rad2float(pi / 2 - angle(
                forearm, gripper) * sign(forearm.unit.z - gripper.unit.z))
        return servo_values