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)