def GetHandPoints(self, cloud, normals=None, cloudTree=None):
'''Determines which points are in the hand descriptor.'''
# Step 1: Find points within a ball containing the hand.
if cloudTree is None:
X = cloud
N = normals
else:
searchRadius = norm(array([self.imD, self.imW, self.imH]) / 2.0)
indices = cloudTree.query_ball_point(self.center, searchRadius)
X = cloud[indices, :]
if normals is not None:
N = normals[indices, :]
# Step 2: Transform points into grasp frame.
# Step 3: Filter out points that are not in the hand.
hTb = inv(self.T)
workspace = [(-self.imH / 2, self.imH / 2),
(-self.imW / 2, self.imW / 2),
(-self.imD / 2, self.imD / 2)]
if normals is None:
X = point_cloud.Transform(hTb, X)
X = point_cloud.FilterWorkspace(workspace, X)
else:
X, N = point_cloud.Transform(hTb, X, N)
X, N = point_cloud.FilterWorkspace(hTb, X, N)
if normals is None:
self.handPoints = X
return X
self.handNormals = N
return X, N
def IsRobotInCollision(self, descriptor):
'''Checks collision between the robot and the world.
- Input descriptor: HandDescriptor object for the current hand pose.
- Returns: True if in collision and False otherwise.
- Returns objCloudsInHandFrame: List of point clouds, one for each object, in the descriptor
reference frame. Or, None if a collision is detected. (This is to avoid performing transforms
of all object clouds twice.)
'''
# ODE misses several box-cylinder collisions. So we have to implement this ourselves.
# check collision with table
bX = point_cloud.Transform(descriptor.T, self.externalHandPoints)
if (bX[:, 2] < self.GetTableHeight()).any():
return True, None
# some preparation
hTb = inv(descriptor.T)
self.robot.SetTransform(descriptor.T) # for visualization only
objects = self.objects + self.supportObjects
objCloudsInHandFrame = []
# check if any object points intersect hand collision geometry
for i, obj in enumerate(objects):
bTo = obj.GetTransform()
hX = point_cloud.Transform(dot(hTb, bTo), obj.cloud)
X = point_cloud.FilterWorkspace(self.handFingerRegionL, hX)
if X.size > 0: return True, None
X = point_cloud.FilterWorkspace(self.handFingerRegionR, hX)
if X.size > 0: return True, None
X = point_cloud.FilterWorkspace(self.handTopRegion, hX)
if X.size > 0: return True, None
if i < len(self.objects): objCloudsInHandFrame.append(hX)
return False, objCloudsInHandFrame
def IsAntipodalGrasp(self, descriptor, targetObject, maxAngleToFinger):
'''Returns True if a grasp is near antipodal, based on the parameters.
- Input descriptor: HandDescriptor object with pose of grasp.
- Input targetObject: OpenRAVE object handle with cloud and normals entries.
- Input maxAngleToFinger: Maximum angle between surfance normal and finger in degrees. Used
10 degrees for blocks, 15 degrees for mugs, and 15 degrees for bottles.
- Returns: True if antipodal grasp, False otherwise.
'''
# parameters
contactWidth = 0.01
maxAntipodalDist = 0.01
maxAngleToFinger = cos(maxAngleToFinger * (pi / 180))
# put cloud into hand reference frame
bTo = targetObject.GetTransform()
bTh = descriptor.T
hTo = dot(inv(bTh), bTo)
X, N = point_cloud.Transform(hTo, targetObject.cloud,
targetObject.normals)
X, N = point_cloud.FilterWorkspace(
[(-descriptor.height / 2, descriptor.height / 2),
(-descriptor.width / 2, descriptor.width / 2),
(-descriptor.depth / 2, descriptor.depth / 2)], X, N)
if X.shape[0] == 0:
#print("No points in hand.")
return False
# find contact points
leftPoint = min(X[:, 1])
rightPoint = max(X[:, 1])
lX, lN = point_cloud.FilterWorkspace(
[(-1, 1), (leftPoint, leftPoint + contactWidth), (-1, 1)], X, N)
rX, rN = point_cloud.FilterWorkspace(
[(-1, 1), (rightPoint - contactWidth, rightPoint), (-1, 1)], X, N)
# find contact points normal to finger
lX = lX[-lN[:, 1] >= maxAngleToFinger, :]
rX = rX[rN[:, 1] >= maxAngleToFinger, :]
if lX.shape[0] == 0 or rX.shape[0] == 0:
#print("No contact points normal to finger.")
return False
# are the closest two contact points nearly antipodal?
leftTree = cKDTree(lX[:, (0, 2)])
d, idxs = leftTree.query(rX[:, (0, 2)])
#if min(d) >= maxAntipodalDist:
# print("Contacts not antipodal.")
return min(d) < maxAntipodalDist
def IsGrasp(self, descriptor):
'''Checks if, when the hand is placed at the descriptor's pose and closed, a grasp takes place.
- Input descriptor: HandDescriptor object of the target hand pose.
- Returns graspedObject: The handle of the grasped object if a cylinder can be grasped from the
target hand pose; otherwise None.
'''
# check collision
if self.IsRobotInCollision(descriptor):
return None
# check intersection of exactly 1 object
graspedObject = None
for i, obj in enumerate(self.objects):
bTo = obj.GetTransform()
hTo = dot(inv(descriptor.T), bTo)
X = point_cloud.Transform(hTo, obj.cloud)
X = point_cloud.FilterWorkspace(self.handClosingRegion, X)
intersect = X.size > 0
if intersect:
if graspedObject is None:
graspedObject = obj
else:
# intersection of multiple objects
return None
if graspedObject is None:
# intersection of no objects
return None
# check antipodal condition
if self.IsAntipodalGrasp(descriptor, graspedObject,
self.graspFrictionCone):
return graspedObject
return None
def GetHandPoints(self, cloud):
'''Determines which points are in the descriptor volume.
- Input cloud: Point cloud in the base/world frame.
- Returns X: Point cloud in the hand frame, including only points in the descriptor volume.
'''
workspace = [(-self.imW / 2, self.imW / 2),
(-self.imW / 2, self.imW / 2),
(-self.imD / 2, self.imD / 2)]
X = point_cloud.Transform(inv(self.T), cloud)
return point_cloud.FilterWorkspace(workspace, X)
def GetCloud(self, workspace=None):
'''Agent gets point cloud from its sensor from the current position.'''
self.StartSensor()
self.env.StepSimulation(0.001)
data = self.sensor.GetSensorData(openravepy.Sensor.Type.Laser)
cloud = data.ranges + data.positions[0]
self.StopSensor()
if workspace is not None:
cloud = point_cloud.FilterWorkspace(cloud, workspace)
return cloud
def main():
'''Entrypoint to the program.'''
# PARAMETERS =====================================================================================
# system
gpuId = 0
nAgents = 6
# objects
nObjects = 10
objectFolder = os.getcwd() + "/../../Data/RaveObjects/RectangularBlocks"
graspColor = [0, 0, 1]
placeColor = [1, 0, 0]
# reaching
nActionSamples = [500, 1000, 361, 181, 361, 303]
stateImageWHD = [0.09, 0.09, 0.22]
# view
viewCenter = array([0, 0, 0])
viewKeepout = 0.70
viewWorkspace = [(-1.0, 1.0), (-1.0, 1.0), (-1.0, 1.0)]
viewWorkspaceNoTable = [(-1.0, 1.0), (-1.0, 1.0), (0.002, 1.0)]
# learning
nTrainingRounds = 200
nScenes = 100
nEpisodes = 20
minEpsilon = 0.05
unbiasOnRound = nTrainingRounds - 5
maxExperiences = 50000
trainingBatchSize = maxExperiences
minTrainBatchSize = 1024
# visualization/saving
saveFileName = "results.mat"
recordLoss = True
loadNetwork = False
loadDatabase = False
showViewer = False
showSteps = False
plotImages = False
# visualize policy
visualizePolicy = True
if visualizePolicy:
nEpisodes = 1
unbiasOnRound = 0
loadNetwork = True
loadDatabase = False
showViewer = True
showSteps = True
plotImages = True
# INITIALIZATION =================================================================================
rlEnv = RlEnvironmentPlacing(showViewer)
experiences = []
rlAgentsGrasping = []
for i in xrange(nAgents):
rlAgent = eval("RlAgentLevel" + str(i) + \
"(rlEnv, gpuId, False, nActionSamples[i], \"weights-grasp-blocks/\")")
rlAgentsGrasping.append(rlAgent)
experiences.append([])
rlAgentsGrasping[0].SetSamplingWorkspace(-viewWorkspaceNoTable[2][1],
-viewWorkspaceNoTable[2][0])
rlAgentsPlacing = []
for i in xrange(nAgents):
rlAgent = eval(
"RlAgentLevel" + str(i) +
"(rlEnv, gpuId, True, nActionSamples[i], \"weights-place-blocks/\")"
)
rlAgentsPlacing.append(rlAgent)
experiences.append([])
rlAgentsPlacing[0].SetSamplingWorkspace(-viewWorkspaceNoTable[2][1],
-viewWorkspaceNoTable[2][0])
rlAgents = rlAgentsGrasping + rlAgentsPlacing
if loadNetwork:
for rlAgent in rlAgents:
rlAgent.LoadNetworkWeights()
rlAgent.caffeFirstTrain = False
if loadDatabase:
for i, rlAgent in enumerate(rlAgents):
experiences[i] = rlAgent.LoadExperienceDatabase()
# RUN TEST =======================================================================================
avgReturn = []
avgPlaceReward = []
avgGraspReward = []
epsilonGraspRound = []
epsilonPlaceRound = []
graspDatabaseSize = []
placeDatabaseSize = []
roundTime = []
trainLoss = []
testLoss = []
for i in xrange(len(rlAgents)):
trainLoss.append([])
testLoss.append([])
for trainingRound in xrange(nTrainingRounds):
# initialization
iterationStartTime = time.time()
Return = []
placeReward = []
graspReward = []
# cool exploration and check if it's time to unbias data
if trainingRound >= unbiasOnRound:
epsilonGrasp = 0.0
epsilonPlace = 0.0
else:
epsilonGrasp = max(
minEpsilon,
1.0 - float(len(experiences[0])) / float(maxExperiences))
epsilonPlace = max(
minEpsilon,
1.0 - float(len(experiences[-1]) / float(maxExperiences)))
epsilonGraspRound.append(epsilonGrasp)
epsilonPlaceRound.append(epsilonPlace)
for scene in xrange(nScenes):
# place random object in random orientation on table
rlAgents[0].MoveHandToHoldingPose()
objHandles = rlEnv.PlaceObjects(randint(2, nObjects + 1),
objectFolder)
if showSteps: raw_input("Placed objects.")
# get a point cloud
cloudScene, cloudTreeScene = rlAgentsPlacing[0].GetDualCloud(
viewCenter, viewKeepout, viewWorkspace)
rlAgentsPlacing[0].PlotCloud(cloudScene)
if showSteps: raw_input("Acquired point cloud.")
cloudSceneNoTable = point_cloud.FilterWorkspace(
viewWorkspaceNoTable, cloudScene)
cloudTreeSceneNoTable = cKDTree(
cloudSceneNoTable) if cloudSceneNoTable.shape[0] > 0 else None
for episode in xrange(nEpisodes):
states = []
actions = []
graspDesc = None
placeDesc = None
# grasp an object
for rlAgent in rlAgentsGrasping:
s, a, graspDesc, v = rlAgent.SenseAndAct(
None, graspDesc, cloudTreeScene, epsilonGrasp)
rlAgentsGrasping[0].PlotDescriptors([graspDesc],
graspColor)
if plotImages: rlAgent.PlotImages(s, a)
states.append(s)
actions.append(a)
# evaluate grasp
rGrasp = rlEnv.TransitionTopGraspHalfConditions(
graspDesc, rlAgentsGrasping[-1], objHandles)
if showSteps:
raw_input("Grasp received reward {}.".format(rGrasp))
graspReward.append(rGrasp)
# if grasp was a success, try a place
if rGrasp > 0.00:
rlEnv.SetObjectPoses(objHandles)
rlEnv.GraspObjectAndCenter(rlAgentsPlacing[0], graspDesc,
objHandles)
if showSteps: raw_input("Removed object.")
# update hand contents
graspDesc.imW = stateImageWHD[0]
graspDesc.imH = stateImageWHD[1]
graspDesc.imD = stateImageWHD[2]
graspDesc.GenerateDepthImage(cloudSceneNoTable,
cloudTreeSceneNoTable)
# get a point cloud
cloud, cloudTree = rlAgentsPlacing[0].GetDualCloud(
viewCenter, viewKeepout, viewWorkspace)
rlAgentsPlacing[0].PlotCloud(cloud)
if showSteps: raw_input("Acquired point cloud.")
# act at each level in the hierarchy
for rlAgent in rlAgentsPlacing:
s, a, placeDesc, v = rlAgent.SenseAndAct(
graspDesc, placeDesc, cloudTree, epsilonPlace)
rlAgentsPlacing[0].PlotDescriptors([placeDesc],
placeColor)
if plotImages: rlAgent.PlotImages(s, a)
states.append(s)
actions.append(a)
if showSteps: raw_input("Decided place pose.")
# check place and finish
rPlace = rlEnv.TransitionPlaceOnObject(
placeDesc, rlAgentsPlacing[-1], objHandles, showSteps)
if showSteps:
raw_input("Place received reward {}.".format(rPlace))
placeReward.append(rPlace)
rlEnv.ResetObjectPoses(objHandles)
else: # grasp was a failure
rPlace = 0.0
# save experiences
for i in xrange(nAgents):
experiences[i].append(
(states[i], actions[i], rGrasp + rPlace))
if len(states) > nAgents:
for i in xrange(nAgents, 2 * nAgents):
experiences[i].append((states[i], actions[i], rPlace))
# cleanup this episode
r = rGrasp + rPlace
print("Episode {}.{}.{} had return {}".format(
trainingRound, scene, episode, r))
Return.append(r)
# cleanup this scene
rlEnv.RemoveObjectSet(objHandles)
# Train each agent.
for i, rlAgent in enumerate(rlAgents):
if len(experiences[i]) < minTrainBatchSize: continue
experiences[i] = rlAgent.PruneDatabase(experiences[i],
maxExperiences)
Dl = rlAgent.DownsampleData(experiences[i], trainingBatchSize)
loss = rlAgent.Train(Dl, recordLoss=recordLoss)
trainLoss[i].append(loss[0])
testLoss[i].append(loss[1])
# Save results
avgReturn.append(mean(Return))
if len(placeReward) == 0: placeReward.append(0)
avgPlaceReward.append(mean(placeReward))
avgGraspReward.append(mean(graspReward))
graspDatabaseSize.append(len(experiences[0]))
placeDatabaseSize.append(len(experiences[-1]))
roundTime.append(time.time() - iterationStartTime)
saveData = {
"gpuId": gpuId,
"nAgents": nAgents,
"nObjects": nObjects,
"objectFolder": objectFolder,
"viewCenter": viewCenter,
"viewKeepout": viewKeepout,
"viewWorkspace": viewWorkspace,
"viewWorkspaceNoTable": viewWorkspaceNoTable,
"nActionSamples": nActionSamples,
"stateImageWHD": stateImageWHD,
"nTrainingRounds": nTrainingRounds,
"nScenes": nScenes,
"nEpisodes": nEpisodes,
"epsilonGraspRound": epsilonGraspRound,
"epsilonPlaceRound": epsilonPlaceRound,
"minEpsilon": minEpsilon,
"unbiasOnRound": unbiasOnRound,
"maxExperiences": maxExperiences,
"trainingBatchSize": trainingBatchSize,
"minTrainBatchSize": minTrainBatchSize,
"avgReturn": avgReturn,
"avgPlaceReward": avgPlaceReward,
"avgGraspReward": avgGraspReward,
"graspDatabaseSize": graspDatabaseSize,
"placeDatabaseSize": placeDatabaseSize,
"roundTime": roundTime,
"trainLoss0": trainLoss[0],
"testLoss0": testLoss[0],
"trainLoss1": trainLoss[1],
"testLoss1": testLoss[1],
"trainLoss2": trainLoss[2],
"testLoss2": testLoss[2],
"trainLoss3": trainLoss[3],
"testLoss3": testLoss[3],
"trainLoss4": trainLoss[4],
"testLoss4": testLoss[4],
"trainLoss5": trainLoss[5],
"testLoss5": testLoss[5],
"trainLoss6": trainLoss[6],
"testLoss6": testLoss[6],
"trainLoss7": trainLoss[7],
"testLoss7": testLoss[7],
"trainLoss8": trainLoss[8],
"testLoss8": testLoss[8],
"trainLoss9": trainLoss[9],
"testLoss9": testLoss[9],
"trainLoss10": trainLoss[10],
"testLoss10": testLoss[10],
"trainLoss11": trainLoss[11],
"testLoss11": testLoss[11]
}
savemat(saveFileName, saveData)
# Backup experience database
if (trainingRound == nTrainingRounds - 1) or (trainingRound % 10 == 9):
for i, rlAgent in enumerate(rlAgents):
rlAgent.SaveExperienceDatabase(experiences[i])
def IsPegGrasp(self, descriptor):
'''Checks if, when the hand is placed at the descriptor's pose and closed, a grasp takes place.
A grasp must be (1) collision-free (2) contain exactly 1 peg's geometry, (3) contain the
cylinder's axis, and (4) not contact the side and cap of the cylinder.
- Input descriptor: HandDescriptor object of the target hand pose.
- Returns graspedObject: The handle of the grasped object if a cylinder can be grasped from the
target hand pose; otherwise None.
- Returns isCapGrasp: True if this is a good grasp and each finger contacts the bottom/top of
the peg.
'''
# check collision
collision, objCloudsInHandFrame = self.IsRobotInCollision(descriptor)
if collision: return None, False
# check intersection of exactly 1 object
graspedObject = None
pointsInHand = None
for i, obj in enumerate(self.objects):
X = point_cloud.FilterWorkspace(self.handClosingRegion,
objCloudsInHandFrame[i])
intersect = X.size > 0
if intersect:
if graspedObject is None:
graspedObject = obj
pointsInHand = X
else:
# intersection of multiple objects
return None, False
if graspedObject is None:
# intersection of no objects
return None, False
# A cylinder can only be upright or on the side. We handle these two cases separately.
bTo = graspedObject.GetTransform()
if self.IsPegUpright(graspedObject):
# Top-center of cylinder in the hand is necessary and sufficient.
bp = copy(bTo[0:3, 3])
bp[2] += graspedObject.height / 2.0
hp = point_cloud.Transform(inv(descriptor.T), array([bp]))
hP = point_cloud.FilterWorkspace(self.handClosingRegion, hp)
if hP.size == 0:
return None, False
return graspedObject, False
# Cylinder is on its side.
# check if finger tips are below cylinder axis
cylinderZ = bTo[2, 3]
fingerZ = descriptor.center[2] - descriptor.depth / 2.0
if fingerZ > cylinderZ:
return None, False
# make sure cylinder caps are not in hand
contactIdxs = array(
[argmax(pointsInHand[:, 1]),
argmin(pointsInHand[:, 1])])
contacts = pointsInHand[contactIdxs, :]
oX = point_cloud.Transform(dot(inv(bTo), descriptor.T), pointsInHand)
capIdxs = sum(
power(oX[:, 0:2], 2),
1) < (graspedObject.radius - self.pointToRealRadiusError)**2
capIdxs = capIdxs.flatten()
nContactsOnCap = sum(capIdxs[contactIdxs])
if nContactsOnCap == 1 or sum(power(contacts[0, 0:2] - contacts[1, 0:2], 2)) < \
(min(2 * graspedObject.radius, graspedObject.height) - 2 * self.pointToRealRadiusError)**2:
# 1 finger contacts cap, other finger contacts side
return None, False
# side grasp is good
return graspedObject, nContactsOnCap == 2