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 goalZoneRadius = math.sqrt(0.0012) 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. """ return distance < self.goalZoneRadius
[docs]class TwoDoF(BaseWorld): """ Arm with 2 DoF. Attributes: maxStepNumber: maximum number of steps to complete a circle maxStepLength: angular step length goalZoneRadius: the radius of the goal zone 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 goalZoneRadius = 0.12 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))) self.kinematics = ForwardKinematics.ForwardKinematics(ranges, linkLengths) 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): return distance <= self.goalZoneRadius
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 goalZoneRadius: the radius of the goal zone 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 goalZoneRadius = 0.12 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))) self.kinematics = ForwardKinematics.ForwardKinematics(ranges, linkLengths) 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 goalZoneRadius: the radius of the goal zone 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))) self.kinematics = ForwardKinematics.ForwardKinematics(ranges, linkLengths) 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])