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
#!/usr/bin/env python
from geometry_msgs.msg import Pose
from graspit_commander import GraspitCommander
gc = GraspitCommander()
ROS_SERVICE_TIMEOUT = 1
# Refresh world
gc.clearWorld()
gc.importRobot('fetch_gripper')
gc.autoOpen()
cupPose = Pose()
cupPose.position.x = 0.05
cupPose.position.y = 0
cupPose.position.z = 0
cupPose.orientation.x = 1.6
cupPose.orientation.y = -0.3
cupPose.orientation.z = 0.1
cupPose.orientation.w = 1.0
gc.importGraspableBody('mug', cupPose)
gc.findInitialContact()
gc.autoGrasp()
pose = gc.getRobot(0)
print(pose)