import grasp_execution import grasp_execution_node from graspit_commander import GraspitCommander import graspit_interface.msg from graspit_msgs.srv import * import geometry_msgs import actionlib import rospy from geometry_msgs.msg import PoseStamped, Point GraspitCommander.loadWorld("test_grasp") # GraspitCommander.importGraspableBody("/home/masatoshichang/models/doughnut4.ply") GraspitCommander.importRobot('fetch_gripper') print('doughnut') grasp_output = GraspitCommander.planGrasps(max_steps=50000) print(grasp_output) unchecked_for_reachability_grasps = grasp_output.grasps service_proxy = rospy.ServiceProxy("/world_manager/add_object", graspit_msgs.srv.AddObject) object_pose = PoseStamped() object_pose.pose.position = Point(2.75, 0, 0.58)
import os import time import traceback import numpy as np from graspit_commander import GraspitCommander from utils import gen_example GraspitCommander.clearWorld() # GraspitCommander.loadWorld("barrettTactileGlassDyn") GraspitCommander.loadWorld("plannerMugTactile") save_dir = os.path.expanduser('~/Desktop/critic_image_dataset') batch_size = 10000 while True: states = [] values = [] images = [] count = 0 while count < batch_size: try: state, value, image = gen_example(GraspitCommander) states.append(state) values.append(value) images.append(image) count += 1 print(count, batch_size) except:
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)
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: 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)