Source code for pyrltr.worlds.InverseKinematics

import numpy as np

from BaseWorld import BaseWorld
import ForwardKinematics
import math


[docs]class TwoDoF(BaseWorld): """ A two DoF arm of which the learning is supposed to learn the inverse kinematics. """ def __init__(self): """ Initialises the kinematic engine. """ self.kinematics = self.getKinematics() self.goalZoneRadius = 0.12 BaseWorld.__init__(self, maxEpisodeLength=10)
[docs] def getKinematics(self): """ Returns the kinematic object for this environment. """ ranges = (("z", (-math.pi / 2, math.pi / 2)), ("z", (-math.pi, 0))) linkLengths = np.array(((0, 0, 0), (0, 0.366, 0), (0, 0.366, 0))) return ForwardKinematics.ForwardKinematics(ranges, linkLengths)
def _validatePosition(self, position): """ Makes sure the position does not leave the boundaries of its set-up. For instance an angle cannot be larger than its legal range. """ validatedAngles = self.kinematics.validateAngles(position[2:]) validatedPosition = np.concatenate((position[:2], validatedAngles)) return validatedPosition
[docs] def calculateDistanceToTarget(self, position, goalPosition): """ Calculates the distance to the target as Euclidean distance in joint space. Parameters: position -- point A to calculate the distance goalPosition -- point B to calculate the distance returns -- the Euclidean distance between the two points """ goalWorkspacePosition = goalPosition[:2] inBaseCoordinates = position[:2] return np.linalg.norm(goalWorkspacePosition - inBaseCoordinates)
[docs] def getSensors(self): """ Returns the current position which is the agent's position in joint space. """ return self.goalPosition[0:2]
[docs] def targetReached(self, distance): return False
def _getInitialAgentPosition(self): """ Returns an initial agent position. It contains both, the position in work and joint space. """ return self._getInitialGoalPosition() def _getInitialGoalPosition(self): """ Returns an initial goal position. It contains both, the position in work and joint space. """ randomPosition = self.kinematics.getRandomPosition() # remove third dimension from base coordinate system position = np.concatenate((randomPosition[0:2], randomPosition[3:])) return position def _perform(self, position, action): """ Performs the action, i.e. returns the action directly, because it should be the inverse of the sensor position. Parameters: position -- the current position of the arm in joint space. action -- the inverse kinematics value of the current state value returns -- the new position after performing the action """ assert action.shape == (2,), "%s == (2,)" % action.shape assert position.shape == (4,), "%s == (4,)" % position.shape validatedAngles = self.kinematics.validateAngles(action) baseCoords = self.kinematics.inBaseCoordinateSystem(validatedAngles, np.zeros((3,))) newPosition = np.concatenate((baseCoords[:-1], action)) assert newPosition.shape == (4,), "%s == (4,)" % position.shape return newPosition
[docs] def getDistanceInActionSpace(self): """ Returns the distance in action space between the current arm's position and the goal. """ goalPositionActionSpace = self.goalPosition[2:] agentPosition = self.agentPosition[2:] assert agentPosition.shape == goalPositionActionSpace.shape,\ "%s == %s" % (str(agentPosition), str(goalPositionActionSpace)) return np.linalg.norm(agentPosition - goalPositionActionSpace)
[docs] def getReward(self): """ Returns the reward as -distance. """ return -self.distance
[docs]class TwoDoFReduntant(TwoDoF): """ This class models a redundant two DoF robotic arm. """
[docs] def getKinematics(self): """ Returns the kinematic object for this environment. """ ranges = (("z", (-math.pi / 2, math.pi / 2)), ("z", (-math.pi / 2, math.pi / 2))) linkLengths = np.array(((0, 0, 0), (0, 0.366, 0), (0, 0.366, 0))) return ForwardKinematics.ForwardKinematics(ranges, linkLengths)