def runManipulator(self, configFileName, shouldSavePlots, baseSaveFName):
# the workspace doesn't change for this simulation
currWorkspace = activeSpaces.get(robotSpaceType='WORKSPACE',
configFileName=configFileName,
shouldSavePlots=shouldSavePlots,
baseSaveFName=baseSaveFName)
# this simulation is for a manipulator robot, so get that class from
# the activeRobots factory interface
currRobot = activeRobots.get(robotType='MANIPULATOR',
configFileName=configFileName,
workspace=currWorkspace,
shouldSavePlots=shouldSavePlots,
baseSaveFName=baseSaveFName)
currPlanner = availablePlanners.get(plannerType='WAVEFRONT',
cSpace=currRobot.cSpace,
workspace=currWorkspace,
robot=currRobot,
configFileName=configFileName,
shouldSavePlots=shouldSavePlots,
baseSaveFName=baseSaveFName)
currRobot.runAndPlot(planner=currPlanner, plotTitle='')
def __init__(self, robotType, configData, workspace, shouldSavePlots,
baseSaveFName):
# calling the superclass contructor to inherit its properties
Robot.__init__(self,
robotType=robotType,
configData=configData,
workspace=workspace,
shouldSavePlots=shouldSavePlots,
baseSaveFName=baseSaveFName)
# need to maintain a separate history for c states and workspace states
# however, for a point robot, they are the same
self.cStateHistory = []
self.startCState = self.startState
self.goalCState = self.goalState
self.currentState = self.startState
self.updateRobotState(self.startState)
linearDiscretizationDensity = configData['linearDiscretizationDensity']
makeSquareCSpace = configData['makeSquareCSpace']
# have the factory make the PointRobot's C-space
self.cSpace = activeSpaces.get(robotSpaceType='POINTROBOTCSPACE',
robot=self,
N=linearDiscretizationDensity,
makeSquare=makeSquareCSpace,
shouldSavePlots=shouldSavePlots,
baseSaveFName=baseSaveFName)
def __init__(self, robotType, configData, workspace, shouldSavePlots,
baseSaveFName):
# calling the superclass contructor to inherit its properties
Robot.__init__(self,
robotType=robotType,
configData=configData,
workspace=workspace,
shouldSavePlots=shouldSavePlots,
baseSaveFName=baseSaveFName)
self.currentState = self.startState
(relativeShapeVerts, robotOriginIdx) = self.setRobotShape(configData)
self.workspaceVertCoords = []
# this is how the robot's vertices are the robot's verices in its
# local coordinate system
self.relativeShapeVerts = relativeShapeVerts
# need to store which of the robot's vertices is its origin
self.robotOriginIdx = robotOriginIdx
# have the factory get make the PolygonalRobot's C-space
self.cSpace = activeSpaces.get(robotSpaceType='POLYGONALROBOTCSPACE',
robot=self,
shouldSavePlots=shouldSavePlots,
baseSaveFName=baseSaveFName)
def __init__(self, robotType, configData, workspace, shouldSavePlots,
baseSaveFName):
# calling the superclass contructor to inherit its properties
Robot.__init__(self,
robotType=robotType,
configData=configData,
workspace=workspace,
shouldSavePlots=shouldSavePlots,
baseSaveFName=baseSaveFName)
self.configData = configData
self.shouldSavePlots = shouldSavePlots
self.baseSaveFName = baseSaveFName
# configure manipulator's links
linkLengths = configData['linkLengths']
jointOffset = configData['jointOffset']
links = []
for linkLength in linkLengths:
link = Link(origin=[0, 0],
linkLength=linkLength,
jointOffset=jointOffset)
links.append(link)
self.links = links
self.numLinks = len(links)
# need to save history of link updates for plotting motion of robot
self.linkHistory = []
# tolerancing for inverse kinematics
self.IK_TOL = 1e-6
# need to compute its start / goal cspace coordinates given the
# starting workspace state
self.cStateHistory = []
(self.startCState, self.currentState,
self.links) = self.setEffectorToDesired(self.startState)
(self.goalCState, _, _) = self.setEffectorToDesired(self.goalState)
# now "state" is the workspace end effector position after inverse
# kinematics
self.startState = self.currentState
# now, call the state update routine to make sure everything is set
# correctly after initialization
self.updateRobotState(self.startCState)
linearDiscretizationDensity = configData['linearDiscretizationDensity']
# have the factory make the ManipulatorRobot's C-space
self.cSpace = activeSpaces.get(robotSpaceType='MANIPULATORROBOTCSPACE',
robot=self,
N=linearDiscretizationDensity,
shouldSavePlots=shouldSavePlots,
baseSaveFName=baseSaveFName)
def runCspaceViz(self, configFileName, shouldSavePlots, baseSaveFName):
# the workspace doesn't change for this simulation
currWorkspace = activeSpaces.get(robotSpaceType='WORKSPACE',
configFileName=configFileName,
shouldSavePlots=shouldSavePlots,
baseSaveFName=baseSaveFName)
# this simulation is for a polygonal robot, so get that class from the
# activeRobots factory interface
currRobot = activeRobots.get(robotType='POLYGONALROBOT',
configFileName=configFileName,
workspace=currWorkspace,
shouldSavePlots=shouldSavePlots,
baseSaveFName=baseSaveFName)
currRobot.runAndPlot(planner=None, plotTitle='')
def runGradientPlanner(self, configFileName, shouldSavePlots,
baseSaveFName):
# the workspace doesn't change for this simulation
currWorkspace = activeSpaces.get(robotSpaceType='WORKSPACE',
configFileName=configFileName,
shouldSavePlots=shouldSavePlots,
baseSaveFName=baseSaveFName)
currRobot = activeRobots.get(robotType='POINTROBOT',
configFileName=configFileName,
workspace=currWorkspace,
shouldSavePlots=shouldSavePlots,
baseSaveFName=baseSaveFName)
currPlanner = availablePlanners.get(plannerType='GRADIENT',
cSpace=currRobot.cSpace,
workspace=currWorkspace,
robot=currRobot,
configFileName=configFileName,
shouldSavePlots=shouldSavePlots,
baseSaveFName=baseSaveFName)
currRobot.runAndPlot(planner=currPlanner, plotTitle='')