def evaluate_pregrasp(self, pre_grasp):
GraspitCommander.toggleAllCollisions(False)
GraspitCommander.forceRobotDof([pre_grasp.dofs[0], 0, 0, 0])
GraspitCommander.setRobotPose(pre_grasp.pose)
GraspitCommander.toggleAllCollisions(True)
GraspitCommander.findInitialContact()
GraspitCommander.approachToContact(250)
GraspitCommander.autoGrasp()
result = GraspitCommander.getRobot(0)
quality = self.compute_quality()
return result, quality
from graspit_commander import GraspitCommander
import time
if __name__ == "__main__":
GraspitCommander.clearWorld()
GraspitCommander.loadWorld("plannerMug")
GraspitCommander.approachToContact()
GraspitCommander.setDynamics(True)
GraspitCommander.setDynamics(False)
GraspitCommander.toggleDynamicsController(False)
while not GraspitCommander.dynamicAutoGraspComplete():
r = GraspitCommander.getRobot(0)
print r.robot.dofs
GraspitCommander.setRobotDOFForces([0, 100000000, 100000000, 100000000])
GraspitCommander.stepDynamics(1)
r = GraspitCommander.getRobot(0)
dofs = list(r.robot.dofs)
for i in range(len(dofs)):
dofs[i] -= 0.01
GraspitCommander.toggleAllCollisions(False)
GraspitCommander.autoOpen(0)
GraspitCommander.approachToContact(-10)
GraspitCommander.toggleAllCollisions(True)
def collectData(
handType, num
): #Workhorse of this file, handType is the chosen hand, num is the type of shape
directory = './' + str(handType) + '_' + str(int(
time.time())) + '/' #Directory for saving metrics and labels
os.mkdir(directory, 0755)
shapes = ['cube', 'ellipse', 'cylinder', 'cone']
shapes = [shapes[num % 4]]
eng = matlab.engine.start_matlab()
eng.cd(r'/home/irongiant/NearContactStudy/ShapeGenerator/', nargout=0)
env = openravepy.Environment()
collisionChecker = openravepy.RaveCreateCollisionChecker(env, 'ode')
env.SetCollisionChecker(collisionChecker)
#env.SetViewer('qtcoin') #Set viewer (optional)
robot, init_transform = loadRobot(
env, handType
) #load robot into Openrave and get transform of the hand in OpenRAVE
iter = 0
#viewer = env.GetViewer()
gc = GraspitCommander()
GraspitCommander.GRASPIT_NODE_NAME = "/graspit" + str(num + 1) + "/"
hand_points = getManuallyLabelledPoints(handType)
centerRet, surface_norms = getHandPoints(handType)
if handType == 'Barrett': #The 'trianglesTransformed' variable is a hack used to handle the
trianglesTransformed = 0 # inconsistencies between the meshes of the hands. I'll explain more in a writeup
elif handType == 'pr2_gripper':
trianglesTransformed = 2
else:
trianglesTransformed = 1
points_in_hand_plane = getGridOnHand(
robot, hand_points.keys(), centerRet, surface_norms
) #Generates the points and normals of the grids on the hand
points_in_hand_plane = morphPoints(
robot, points_in_hand_plane, trianglesTransformed
) #Morphs the points from the grid plane onto the mesh of the hand
if handType == 'pr2_gripper':
numJoints = 0
else:
numJoints = len(robot.GetActiveDOFValues())
T_rotation = numpy.eye(4)
T_rotation[0:3, 0:3] = rotation_matrix_from_rpy(0, 0, math.pi / 2)
for shape in shapes: #Only iterates over 1 shape, see line 264
parameterShape = generateParameters(shape)
for parameters in parameterShape:
item, filename = loadObject(env, eng, shape, parameters)
STLtoPLY(filename)
gc.clearWorld()
gc.importRobot(handType)
gc.importGraspableBody('/home/irongiant/Grasp-Metrics/Shapes/' +
filename + '.ply')
robot_pose = getPoseMsg(getRobotPose(numpy.eye(4), handType))
gc.setRobotPose(robot_pose)
gc.setGraspableBodyPose(
0, getPoseMsg(numpy.matmul(T_rotation, numpy.eye(4))))
item.SetVisible(1) #Can make item visible or invisible for viewing
print filename
generateSDF(
filename, 0.0025,
70) #Signed distance field needs grid resolution parameters
transforms = sample_obj_transforms(env, gc, item, handType)
graspit_transform = getRobotPose(
transforms,
handType) #Get the tarnsform for loading the hand into graspit
for transform in transforms:
item.SetTransform(transform)
#transform[0:3,0:3] = numpy.matmul(transform[0:3,0:3], rotation_matrix_from_rpy(math.pi/2, math.pi, 0))
gc.setGraspableBodyPose(
0, getPoseMsg(numpy.matmul(T_rotation, transform)))
print transform
print getPoseMsg(transform)
generateHandFeatures(
env, gc, directory + filename + str(iter), robot, item,
filename, transform, points_in_hand_plane,
trianglesTransformed, math.pi / 6,
5) #Saves signed distance metric and wedge metric
#env.Save(directory + filename + '_' + str(iter) + '.dae')
configuration = [0] * (6 + numJoints)
noise_std = [0.005, 0.005, 0.005, 0.1, 0.1, 0.1] + [
0.1
] * numJoints #Covariance matrix parameters for gaussian noise injection
noise_distr = generateGaussianNoise(
handType, configuration, noise_std,
100) #Samples noise from multivariate gaussian
# volume = epsilon = []
epsilon = []
contacts = []
for noise in noise_distr:
noise_induced_transform = numpy.eye(4)
noise_induced_transform[0:3, 3] = numpy.array(noise[0:3])
noise_induced_transform[0:3,
0:3] = rotation_matrix_from_rpy(
noise[3:6][0], noise[3:6][1],
noise[3:6][2])
#if handType != "pr2_gripper":
# robot.SetTransform(numpy.matmul(init_transform, noise_induced_transform))
#else:
robot.SetTransform(
numpy.matmul(noise_induced_transform, init_transform)
) #Inject noise into transform of hand
for i in range(6, 6 + numJoints):
if noise[i] < 0:
noise[i] = 0
if handType != "pr2_gripper":
robot.SetDOFValues(
noise[6:6 + numJoints], numpy.arange(numJoints)
) #Inject noise into joint angles of hand
collExcept = 0
try:
robot_pose = getPoseMsg(
getRobotPose(noise_induced_transform, handType))
gc.setRobotPose(
robot_pose
) #Sets teh same transform set in OpenRAVE into GraspIt!
result = gc.computeQuality() #Computes grasp quality
except (RobotCollisionException,
InvalidRobotPoseException):
collExcept = 1
if not env.CheckCollision(robot) and collExcept == 0:
if handType == "Barrett":
gc.forceRobotDof(noise[6:6 + numJoints])
if handType == 'ShadowHand':
gc.approachToContact()
gc.autoGrasp()
g_contacts = []
try:
r = gc.getRobot(0)
contact = r.robot.contacts
for c in contact:
point = c.ps.pose.position
pose = c.ps.pose.orientation
g_contacts.append([
point.x, point.y, point.z, pose.x, pose.y,
pose.z, pose.w
])
result = gc.computeQuality()
# volume += [result.volume]
epsilon += [
result.epsilon
] #List of grasp qualities as a result of noise
contacts.append(
g_contacts
) # list of contacts made between hand and object during grasp as a result of noise
except InvalidRobotPoseException:
# volume += [0]
epsilon += [0]
if handType != 'ShadowHand':
dofs = [0] * numJoints
else:
dofs = [0] * 17
if handType != 'pr2_gripper':
gc.forceRobotDof(dofs)
gc.autoOpen()
robot.SetTransform(init_transform)
robot_pose = getPoseMsg(getRobotPose(numpy.eye(4), handType))
gc.setRobotPose(robot_pose)
if handType != "pr2_gripper":
robot.SetDOFValues([0] * len(robot.GetActiveDOFValues()),
numpy.arange(
0, len(robot.GetActiveDOFValues())))
numpy.savetxt(directory + filename + str(iter) +
'_epsi_labels' + '.out',
epsilon,
delimiter=',',
fmt='%s') #Saves the grasp qualities
numpy.savetxt(directory + filename + str(iter) +
'_cont_labels' + '.out',
contacts,
delimiter=',',
fmt='%s') #Saves the contacts made
iter += 1
env.Remove(env.GetBodies()[1])
robotPose.position.x = 0
robotPose.position.y = 0
robotPose.position.z = 0
robotPose.orientation.x = q[0]
robotPose.orientation.y = q[1]
robotPose.orientation.z = q[2]
robotPose.orientation.w = q[3]
gc.setGraspableBodyPose(0, objPose)
gc.setRobotPose(robotPose)
dofs = [0, 3, 3, 3]
gc.moveDOFToContacts(dofs, [0.01,0.01,0.01,0.01], True)
gc.forceRobotDof(dofs)
graspQuality = gc.computeQuality()
gc.approachToContact()
gc.moveDOFToContacts(dofs, [0.01,0.01,0.01,0.01], True)
gc.setRobotDOFForces([0, 100000000, 100000000, 100000000])
gc.autoGrasp(0)
graspQuality = gc.computeQuality()
gc.autoOpen()
Openrave Transform:
>>> T
array([[1. , 0. , 0. , 0. ],
[0. , 1. , 0. , 0. ],
[0. , 0. , 1. , 0.117],
[0. , 0. , 0. , 1. ]])
Graspit Transform: