Source code for pyrltr.worlds.ForwardKinematics

# Copyright (c) 2015, Chris Stahlhut
# All rights reserved.
#
# 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). linkLengths -- the lengths of the links, i.e. the translations 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} def __init__(self, joints, linkLengths): """ Constructor. Parameters: joints -- the list of joints as tuples of the rotation axis as string and the maximum values of the joints rotations linkLength -- the lengths of the links, starting with an offset 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 self.linkLengths = linkLengths
[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) translation = np.dot(Rotation, -self.linkLengths[i]) 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) translation = np.array([self.linkLengths[i]]).T 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)