Kinematics functions of the Aleph 0 Rover. The logic is a little bit mixed up. This class should only compute the kinematics, but it also is used to publish messages to ROS.

kinematics.py

from math import sin, cos, atan2, sqrt, acos, pi

import rospy
import numpy
import tf
from sensor_msgs.msg import JointState


class UnreachablePositionError(Exception):
    pass


class Kinematics:
    """
    This class represents the kinematics of Aleph 0 Rover. It is not meant to
    be used in any other manipulators.

    Parts of the manipulator:
    A - rotatable base, a link from which always points up,
    B - first part of the arm
    C - second part of the arm
    D - efector

    The logic is a little bit mixed up. This class should only compute the
    kinematics, but it also is used to publish messages to ROS.
    """
    lenA = 0.138
    lenB = 0.4
    lenC = 0.4
    lenD = 0.27   # replace it when changing efector

    # real angles for fwd kinematics
    angleA = 0
    angleBA = 0
    angleCB = 0
    angleDC = 0
    angleD = 0

    # angles set by inv kinematics
    setAngleA = None
    setAngleBA = None
    setAngleCB = None
    setAngleDC = None
    setAngleD = None

    MAX_REFRESH_RATE = None

    def __init__(self, publisher=True):
        self.publisher = publisher
        if publisher:
            self.pub = rospy.Publisher('manipulator_joint_states',
                                       JointState, queue_size=1)
            self.last_publish = rospy.get_rostime()
            self.MAX_REFRESH_RATE = rospy.Rate(20)  # 10 Hz

    @property
    def position(self):
        z = 1*self.lenA + \
            sin(self.angleBA)*self.lenB + \
            sin(self.angleBA+self.angleCB)*self.lenC + \
            sin(self.angleBA+self.angleCB+self.angleDC)*self.lenD

        r = 0*self.lenA + \
            cos(self.angleBA)*self.lenB + \
            cos(self.angleBA+self.angleCB)*self.lenC + \
            cos(self.angleBA+self.angleCB+self.angleDC)*self.lenD

        A = -(self.angleA - 1.57)
        x = sin(A)*r
        y = cos(A)*r

        return (x, y, z)

    @property
    def orientation(self):
        yaw = self.angleA
        pitch = self.angleBA + self.angleCB + self.angleDC
        roll = self.angleD
        return tf.transformations.quaternion_from_euler(roll, -pitch, yaw)

    def CB_joint_pos(self, a, b):
        B = self.lenB
        C = self.lenC
        A = a**2 + b**2 + B**2 - C**2
        # dY = 16 * (-a**2 * A**2 + 4*a**2*(-1 + B**2*b**2 + a**2*B**2))
        dY = 16 * ((A**2 * b**2) - (b**2+a**2)*(A**2-4*a**2*B**2))
        if(dY < 0):
            return []
        sqrt_dy = sqrt(dY)

        a2 = 2*(4*b**2 + 4*a**2)
        y1 = (4*A*b + sqrt_dy) / a2
        y2 = (4*A*b - sqrt_dy) / a2

        x1 = (A - 2*b*y1) / (2*a)
        x2 = (A - 2*b*y2) / (2*a)

        if(sqrt_dy == 0):
            return [(x1, y1)]
        return [(x1, y1), (x2, y2)]

    def choose_angles(self, angles_list):
        """
        Given a list of touples (angleBA, angleCB), chooses the most appropiate
        one. (Currently the one with lower angleBA)
        """
        try:
            return max(angles_list, key=lambda a: a[0])
        except ValueError:
            raise UnreachablePositionError

    def angles_from_CB_joint_pos(self, CB_pos, D_pos):
        """
        Returns angleBA and angle CB given the position of CB joint
        """
        angleBA = atan2(CB_pos[1], CB_pos[0])
        angleCB = - angleBA + atan2(D_pos[1]-CB_pos[1], D_pos[0]-CB_pos[0])
        return(angleBA, angleCB)

    @staticmethod
    def angle_between_quaternions(q1, q2, angleA):
        inv = tf.transformations.quaternion_inverse(q1)
        res = tf.transformations.quaternion_multiply(q2, inv)

        s = sqrt(1-res[3]**2)
        x = res[0] / s
        y = res[1] / s
        # z = res[2] / s

        angle = -(angleA + pi/2)

        nx = x*cos(angle) - y*sin(angle)
        # ny = x*sin(angle) + y*cos(angle)
        if(nx < 0):
            return -acos(res[3]) * 2
        else:
            return acos(res[3]) * 2

    def inverse_from_pose(self, pose):
        p = pose.position
        o = pose.orientation

        # transform orientation to Euler angles
        orientation = numpy.array([o.x, o.y, o.z, o.w])

        # yaw should be an orientation quaternion casted
        # on xy plane, but the following is good enough

        angleA = self.inverse_angleA(p.x, p.y)
        yaw = tf.transformations.quaternion_from_euler(0, 0, angleA)
        angle = self.angle_between_quaternions(orientation, yaw, angleA)

        return self.inverse(p.x, p.y, p.z, angle)

    def inverse_angleA(self, x, y):
        try:
            angleA = atan2(y, x)
        except ZeroDivisionError:
            angleA = 0
        return angleA

    def inverse(self, x, y, z, angle):
        z -= self.lenA
        angleA = self.inverse_angleA(x, y)

        # TODO:
        # The following value might need to be adjusted after changing the
        # model
        # angleA -= 1.5707963267948966

        r = sqrt(x**2+y**2)

        rD = r-cos(angle)*self.lenD
        zD = z-sin(angle)*self.lenD
        point_list = self.CB_joint_pos(rD, zD)
        angles = [self.angles_from_CB_joint_pos(p, (rD, zD))
                  for p in point_list]
        angleBA, angleCB = self.choose_angles(angles)

        angleDC = angle - angleBA - angleCB
        angleD = 0
        return (angleA, angleBA, angleCB, angleDC, angleD)

    def get_angles(self):
        return (self.angleA, self.angleBA, self.angleCB,
                self.angleDC, self.angleD)

    def set_angles(self, angleA, angleBA, angleCB, angleDC, angleD):
        self.setAngleA = angleA
        self.setAngleBA = angleBA
        self.setAngleCB = angleCB
        self.setAngleDC = angleDC
        self.setAngleD = angleD

    @staticmethod
    def get_continous_angle(old_angle, new_angle):
        dangle = new_angle - old_angle
        while dangle > pi:
            dangle -= 2*pi

        while dangle < -pi:
            dangle += 2*pi

        return old_angle + dangle

    def publish(self):
        if not self.publisher:
            raise Exception("This object was not created as a publisher")

        if rospy.get_rostime() - self.last_publish <= \
                self.MAX_REFRESH_RATE.sleep_dur:
            return

        if self.setAngleBA is None:
            return

        command = JointState()
        command.header.stamp = rospy.get_rostime()
        command.name = ['base_to_frame', 'B_to_base', 'C_to_B', 'efector_to_C']
        command.position = [self.setAngleA, self.setAngleBA, self.setAngleCB,
                            self.setAngleDC]
        command.velocity = []
        self.pub.publish(command)
        self.last_publish = rospy.get_rostime()