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

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