# Source code for pyrltr.worlds.NumericalSimulator

"""Numerical simulation environments for single and two DoF robotic arm."""

import ArmSimulation
import numpy as np
import math
from BaseWorld import BaseWorld
import ForwardKinematics

[docs]class SingleDoF(ArmSimulation.SingleDoF):
"""
Arm with 1 DoF.

Attributes:
maxStepNumber: maximum number of steps to complete one rotation
maxStepLengths: the angular length of one step
agentPosition: the agent's position in joint space
goalPosition: the goal's position in joint space.
"""

maxStepNumber = 8
maxStepLength = 2 * math.pi / maxStepNumber

def _perform(self, position, action):
"""
Performs the action.

Parameters:
position -- the current position of the arm in angular space.
action -- the action or angular change to perform.
returns -- the new position after performing the action
"""
assert action.ndim == 1
assert action.shape == (1,)
scaledAction = action * self.maxStepLength
newPosition = position + scaledAction
return newPosition[0]

[docs]    def targetReached(self, distance):
"""
Returns True if the distance is smaller than the goal zone's radius.
"""

[docs]class TwoDoF(BaseWorld):
"""
Arm with 2 DoF.

Attributes:
maxStepNumber: maximum number of steps to complete a circle
maxStepLength: angular step length
agentPosition: the position of the agent in joint space
goalPosition: the position of the goal in the base coordinate system.
"""

maxStepNumber = 8
maxStepLength = 2 * math.pi / maxStepNumber

def __init__(self):
"""
Initialises the kinematic engine.
"""

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

BaseWorld.__init__(self, maxEpisodeLength=10)

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.
"""
return self.kinematics.validateAngles(position)

[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[0:2]
inBaseCoordinates = self.kinematics.inBaseCoordinateSystem(position, np.zeros((3,)))[:-1]
assert goalWorkspacePosition.shape == inBaseCoordinates.shape, "%s == %s" %(str(goalWorkspacePosition.shape), str(inBaseCoordinates.shape))
return np.linalg.norm(goalWorkspacePosition - inBaseCoordinates)

[docs]    def getStateQuality(self, state):
"""
Returns the quality of a state as the inverse of the distance. The
closer the arm gets to its goal the better the state.
Parameters:
state -- the state to evaluate
returns -- the quality of said state
"""
armAngles = state[0:2]
return -self.calculateDistanceToTarget(armAngles, self.goalPosition)

[docs]    def getSensors(self):
"""
Returns the current position in joint space.
"""
goalWorkspacePosition = self.goalPosition[0:2]
sensorOutput = np.array(np.concatenate((self.agentPosition,
goalWorkspacePosition),
axis=0))
return sensorOutput

[docs]    def targetReached(self, distance):

def _getInitialAgentPosition(self):

lower = self.kinematics.ranges[0]
higher = self.kinematics.ranges[1]
return self.randomState.uniform(lower, higher, len(lower))

def _getInitialGoalPosition(self):

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.

Parameters:
position -- the current position of the arm in joint space.
action -- the action or angular change to perform.
returns -- the new position after performing the action
"""
assert action.ndim == 1, "%s == 1" % action.ndim
assert action.shape == (2,)
assert position.ndim == 1
assert position.shape == (2,)

newPosition = position + (action * self.maxStepLength)
assert newPosition.shape == (2,)
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:]
assert self.agentPosition.shape == goalPositionActionSpace.shape, "%s == %s" % (str(self.agentPosition), str(goalPositionActionSpace))
return np.linalg.norm(self.agentPosition - goalPositionActionSpace)

[docs]class ThreeDoF(TwoDoF):
"""
Arm with 3 DoF.

Attributes:
maxStepNumber: maximum number of steps to complete a circle
maxStepLength: angular step length
agentPosition: the position of the agent in joint space
goalPosition: the position of the goal in the base coordinate
system.
"""

maxStepNumber = 8
maxStepLength = 2 * math.pi / maxStepNumber

def __init__(self):
"""
Constructor, initialises the forward kinematic engine.
"""

ranges = (("z", (-math.pi / 2, math.pi / 2)), ("z", (-math.pi, 0)), ("z", (-math.pi / 2, math.pi / 2)))
linkLengths = np.array(((0, 0, 0), (0, 0.366, 0), (0, 0.366, 0), (0, 0.2, 0)))

BaseWorld.__init__(self, maxEpisodeLength=10)

def _perform(self, position, action):
"""
Performs the action.

Parameters:
position -- the current position of the arm in joint space.
action -- the action or angular change to perform.
returns -- the new position after performing the action
"""
assert action.ndim == 1, "%s == 1" % action.ndim
assert action.shape == (3,)
assert position.ndim == 1
assert position.shape == (3,)

newPosition = position + (action * self.maxStepLength)
assert newPosition.shape == (3,)
return newPosition

[docs]    def getStateQuality(self, state):
"""
Returns the quality of a state as the inverse of the distance. The
closer the arm gets to its goal the better the state.
Parameters:
state -- the state to evaluate
returns -- the quality of said state
"""
armAngles = state[0:3]
return -self.calculateDistanceToTarget(armAngles, self.goalPosition)

[docs]class Nimbro(TwoDoF):
"""
Implementation with a standard Nimbro arm. Uses 3 DoF in 3 dimensions.
Attributes:
maxStepNumber: maximum number of steps to complete a circle
maxStepLength: angular step length
agentPosition: the position of the agent in joint space
goalPosition: the position of the goal in the base coordinate system.
"""

def __init__(self):
"""
Constructor, initialises the forward kinematic engine.
"""

ranges = (("x", (-math.pi / 2, math.pi / 2)), ("z", (-math.pi / 2, math.pi / 2)), ("x", (-math.pi / 2, math.pi / 2)))
linkLengths = np.array(((0, 0, 0), (0, 0, 0), (0, 0.366, 0), (0, 0.366, 0)))

BaseWorld.__init__(self, maxEpisodeLength=10)

def _perform(self, position, action):
"""
Performs the action.

Parameters:
position -- the current position of the arm in joint space.
action -- the action or angular change to perform.
returns -- the new position after performing the action
"""
assert action.ndim == 1, "%s == 1" % action.ndim
assert action.shape == (3,)
assert position.ndim == 1
assert position.shape == (3,)

newPosition = position + (action * self.maxStepLength)
assert newPosition.shape == (3,)
return newPosition

[docs]    def getSensors(self):
"""
Returns the current position in joint space.
"""
goalWorkspacePosition = self.goalPosition[0:3]
sensorOutput = np.array(np.concatenate((self.agentPosition,
goalWorkspacePosition),
axis=0))
return sensorOutput

[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[0:3]
inBaseCoordinates = self.kinematics.inBaseCoordinateSystem(position, np.zeros((3,)))
assert position.shape == inBaseCoordinates.shape, "%s == %s" %(str(position.shape), str(inBaseCoordinates.shape))
return np.linalg.norm(goalWorkspacePosition - inBaseCoordinates)

[docs]    def getStateQuality(self, state):
armAngles = state[0:3]
return -self.calculateDistanceToTarget(armAngles, self.goalPosition)

[docs]    def getDistanceInActionSpace(self):
"""
Returns the distance in action space between the current arm's position
and the goal.
"""
return np.linalg.norm(self.agentPosition - self.goalPosition[3:-1])