# Source code for pyrltr.worlds.ForwardKinematics

# Copyright (c) 2015, Chris Stahlhut
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are
# met:
#
#     (1) Redistributions of source code must retain the above copyright
#     notice, this list of conditions and the following disclaimer.
#
#     (2) Redistributions in binary form must reproduce the above copyright
#     notice, this list of conditions and the following disclaimer in
#     the documentation and/or other materials provided with the
#     distribution.
#
#     (3) The name of the author may not be used to
#     endorse or promote products derived from this software without
#     specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE AUTHOR AS IS'' AND ANY EXPRESS OR
# IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
# INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
# HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
# STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
# IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

import numpy as np
import math

[docs]class ForwardKinematics:
"""Implements the forward kinematic of a three dimensional robot arm.

Attributes:
joints -- The legal joints in the form (axis, ranges), ranges = (lowerValues, higherValues).
between the coordinate systems.
"""

[docs]    def identity(sinTheta, cosTheta, transpose):
"""No rotation at all."""
rotation = np.identity(3)
return rotation

[docs]    def xRotation(sinTheta, cosTheta, transpose):
""""Rotation matrix around the x-axis:
sinTheta -- sine of the rotation angle theta.
cosTheta -- cos of the rotation angle theta.
transpose -- False if the rotation axis shall not be transposed.
"""
rotation = np.array([[1, 0, 0],
[0, cosTheta, -sinTheta],
[0, sinTheta, cosTheta]])
return rotation if not transpose else rotation.T

[docs]    def yRotation(sinTheta, cosTheta, transpose):
""""Rotation matrix around the y-axis:
sinTheta -- sine of the rotation angle theta.
cosTheta -- cos of the rotation angle theta.
transpose -- False if the rotation axis shall not be transposed.
"""
rotation = np.array([[cosTheta, 0, sinTheta],
[0, 1, 0],
[-sinTheta, 0, cosTheta]])
return rotation if not transpose else rotation.T

[docs]    def zRotation(sinTheta, cosTheta, transpose):
""""Rotation matrix around the z-axis:
sinTheta -- sine of the rotation angle theta.
cosTheta -- cos of the rotation angle theta.
transpose -- False if the rotation axis shall not be transposed.
"""
rotation = np.array([[cosTheta, -sinTheta, 0],
[sinTheta, cosTheta, 0],
[0, 0, 1]])
return rotation if not transpose else rotation.T

rotationMatrices = {"x":xRotation, "y":yRotation, "z":zRotation, "i":identity}

"""
Constructor.

Parameters:
joints -- the list of joints as tuples of the rotation axis as
string and the maximum values of the joints rotations
from the base to the first link. If the base is the
same as the position of the first joint, than the
first length is (0, 0, 0)
"""
self.randomState = np.random.RandomState()
self.axis = np.array(map(lambda x: x[0], joints) + ["i"])
self.numberOfJoints = len(joints)
self.ranges = np.array(map(lambda x: x[1], joints)).T

[docs]    def inEffCoordinateSystem(self, angles, point):
"""Returns the point in end-effector coordinates using the given
angles.

Parameters:
angles -- the angles to use for the transformations, cannot be
outside the specified ranges
point -- to transform given in world coordinates
returns -- the point in end-effector coordinates in 3d
"""

assert point.ndim == 1, "%s == 1" % point.ndim
assert point.shape == (3,), "%s == (3,)" % str(point.shape)
assert len(angles) == self.numberOfJoints, "%s == %s" % (len(angles), self.numberOfJoints)

pointHomogeneous = np.concatenate((point, [1]))

correctedAngles = self.validateAngles(angles)

# append 0 rotation for last translation
correctedAngles = np.concatenate((correctedAngles, [0]), axis=0)

pointInCoordinateSystem = np.copy(pointHomogeneous)

for i in xrange(len(correctedAngles)):

cosTheta = math.cos(correctedAngles[i])
sinTheta = math.sin(correctedAngles[i])

# we go backwards through the chain, therefore the translation
Rotation = self.rotationMatrices[self.axis[i]](sinTheta, cosTheta, transpose=True)

HomogeneousTransformation = np.concatenate((Rotation,
np.array([translation]).T),
axis=1)
HomogeneousTransformation = np.concatenate((
HomogeneousTransformation, [[0, 0, 0, 1]]), axis=0)

pointInCoordinateSystem = np.dot(HomogeneousTransformation,
pointInCoordinateSystem)

# remove the 4th dimension from the homogeneous matrix
return pointInCoordinateSystem[:-1]

[docs]    def inBaseCoordinateSystem(self, angles, point):
"""Returns the point given in base coordinates using the given angles.

Parameters:
angles -- the angles to use for the transformations, cannot be
outside the specified ranges
point -- to transform given in world coordinates
returns -- the point in base coordinates in 3d
"""

assert point.ndim == 1, "%s == 1" % point.ndim
assert point.shape == (3,), "%s == (3,)" % str(point.shape)
assert len(angles) == self.numberOfJoints, "%s == %s" % (len(angles), self.numberOfJoints)

pointHomogeneous = np.concatenate((point, [1]))

correctedAngles = self.validateAngles(angles)

# append 0 rotation for last translation
correctedAngles = np.concatenate((correctedAngles, [0]), axis=0)

pointInCoordinateSystem = np.copy(pointHomogeneous)

for i in xrange(len(correctedAngles) - 1, -1, -1):

cosTheta = math.cos(correctedAngles[i])
sinTheta = math.sin(correctedAngles[i])

Rotation = self.rotationMatrices[self.axis[i]](sinTheta, cosTheta, transpose=False)

HomogeneousTransformation = np.concatenate((Rotation, translation), axis=1)
HomogeneousTransformation = np.concatenate((HomogeneousTransformation, [[0, 0, 0, 1]]), axis=0)

pointInCoordinateSystem = np.dot(HomogeneousTransformation, pointInCoordinateSystem)

# remove the 4th dimension from the homogeneous matrix
return pointInCoordinateSystem[:-1]

[docs]    def validateAngles(self, angles):
"""
Limits the angles to the valid ones specified in the list of links for
the constructor.
"""

correctedAngles = np.where(angles >= self.ranges[0], angles, self.ranges[0])
correctedAngles = np.where(correctedAngles <= self.ranges[1], correctedAngles, self.ranges[1])
return correctedAngles

[docs]    def getRandomPosition(self):
"""
Returns a random position in base (3d) and joint coordinates. The
positions are drawn uniformly from the joint space.

return -- a radomly drawn position in the form
(base coordinate, joint coordinates)
"""

jointCoordinates = self.randomState.uniform(self.ranges[0],
self.ranges[1])
baseCoordinates = self.inBaseCoordinateSystem(jointCoordinates,
np.array([0, 0, 0]))

return np.concatenate((baseCoordinates, jointCoordinates), axis=0)