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. http://continuum.uni.wroc.pl/ Copyright Michał Barciś

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