def demo(): rospy.init_node('hw3_demo', anonymous=True) gc = GraspitCommander() gc.clearWorld() # Load in the fetch gripper and fully extend out the gripper rospy.loginfo("import the fetch gripper...") gripper = gc.importRobot('fetch_gripper') rospy.loginfo("extend out the fetch gripper...") count_seconds(2) gc.autoOpen() # Load the a mug rospy.loginfo("import mug") mug = gc.importGraspableBody('mug') # Place the robot and the object rospy.loginfo("manual grasp pose") count_seconds(3) gripper_pose = Pose(position=Point(-0.0704542484271, -0.201466631816, 0.0016985854928), orientation=Quaternion(0.0108604258991, 0.0360529356943, 0.675958273869, 0.735977342698)) gc.setRobotPose(gripper_pose, id=0) test_grasp(gc, 2) # Start plalnning (succeed if contact is reached) rospy.loginfo("manual grasp pose") count_seconds(3) gripper_pose = Pose(position=Point(0.06, -0.20, 0), orientation=Quaternion(0, 0, 1.1, 1)) gc.setRobotPose(gripper_pose, id=0) attempts = 1 while (not gc.dynamicAutoGraspComplete() and attempts < 5): rospy.loginfo("graspit planing attempt " + str(attempts)) attempts += 1 gc.planGrasps() test_grasp(gc, 2) if (gc.dynamicAutoGraspComplete()): rospy.loginfo("graspit done") else: rospy.loginfo("graspit failed") # Show Grasp Pose gripper_pose = gc.getRobot().robot.pose rospy.loginfo("current gripper pose:\n" + str(gripper_pose))
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)
array([[1. , 0. , 0. , 0. ], [0. , 1. , 0. , 0. ], [0. , 0. , 1. , 0.117], [0. , 0. , 0. , 1. ]]) Graspit Transform: openrave apply 180 roll to hand import time gc.loadWorld("plannerMug") gc.approachToContact() gc.setDynamics(True) gc.autoGrasp() while not gc.dynamicAutoGraspComplete(): time.sleep(0.01) gc.setDynamics(False) result = gc.computeQuality() gc.setDynamics(False) gc.toggleDynamicsController(False) while not gc.dynamicAutoGraspComplete(): r = gc.getRobot(0) print r.robot.dofs gc.setRobotDOFForces([0, 100000000, 100000000, 100000000]) gc.stepDynamics(1)