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)