コード例 #1
0
def circleArcAroundPivot(pivot, start, end):
    pivotToStart = vector.translate(vector.minus(pivot))(start)
    pivotToEnd = vector.translate(vector.minus(pivot))(end)
    normalToPlane = vector.crossproduct(pivotToStart,pivotToEnd)
    firstRadialDirection = vector.crossproduct(normalToPlane, pivotToStart)
    
    planeCoefficients = pivotToEnd
    def dotByPlaneCoefficients(vec):
        output = 0
        for i in range(0,3):
            output += planeCoefficients[i] * vec[i]
        return output
    
    t = (dotByPlaneCoefficients(end) - dotByPlaneCoefficients(start))/(dotByPlaneCoefficients(firstRadialDirection))
    center = vector.translate(start)(vector.scale(firstRadialDirection,t))
    return circleArc(center, start, end)
コード例 #2
0
ファイル: al5x.py プロジェクト: zinnirahkasim/robotarm
 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")
コード例 #3
0
ファイル: al5x.py プロジェクト: knightwupz/robotarm
 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")
コード例 #4
0
ファイル: al5x.py プロジェクト: zinnirahkasim/robotarm
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