def Graspit(): GraspitCommander.clearWorld() Rotation = tf.transformations.quaternion_from_euler(0, math.pi / 2, 0) object_pose = geometry_msgs.msg.Pose() object_pose.position.x = 3 object_pose.position.y = 3 object_pose.position.z = 0.09 object_pose.orientation.x = Rotation[0] object_pose.orientation.y = Rotation[1] object_pose.orientation.z = Rotation[2] object_pose.orientation.w = Rotation[3] '''robot_transformation = np.array([[math.cos(67.5),0,math.sin(67.5),0],[0,1,0,0],[-math.sin(67.5),0,math.cos(67.5),0],[0,0,0,1]]) robot_rotation = tf.transformations.quaternion_from_matrix(robot_transformation) robot_pose = geometry_msgs.msg.Pose() robot_pose.position.x = -3 robot_pose.position.y = -3 robot_pose.position.z = 0.01 robot_pose.orientation.x = 0 robot_pose.orientation.y = 0 robot_pose.orientation.z = 0 robot_pose.orientation.w = 0''' GraspitCommander.importObstacle("floor") GraspitCommander.importGraspableBody("longBox", pose=object_pose) GraspitCommander.importRobot("fetch_gripper") plan = GraspitCommander.planGrasps(max_steps=50000) return plan
def graspit(): GraspitCommander.clearWorld() rotation = tf.transformations.quaternion_from_euler( 1.5707963, 0, -2.094129) poses = geometry_msgs.msg.Pose() poses.position.x = 3 poses.position.y = 3 poses.position.z = 0.001 poses.orientation.x = rotation[0] poses.orientation.y = rotation[1] poses.orientation.z = rotation[2] poses.orientation.w = rotation[3] GraspitCommander.importObstacle("floor") GraspitCommander.importGraspableBody("cracker", pose=poses) GraspitCommander.importRobot("fetch_gripper") plan = GraspitCommander.planGrasps(max_steps=50000) return plan
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))
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) object_pose.pose.orientation.w = 1 object_pose.header.frame_id = "map"
#!/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)
def collectData( handType, num ): #Workhorse of this file, handType is the chosen hand, num is the type of shape directory = './' + str(handType) + '_' + str(int( time.time())) + '/' #Directory for saving metrics and labels os.mkdir(directory, 0755) shapes = ['cube', 'ellipse', 'cylinder', 'cone'] shapes = [shapes[num % 4]] eng = matlab.engine.start_matlab() eng.cd(r'/home/irongiant/NearContactStudy/ShapeGenerator/', nargout=0) env = openravepy.Environment() collisionChecker = openravepy.RaveCreateCollisionChecker(env, 'ode') env.SetCollisionChecker(collisionChecker) #env.SetViewer('qtcoin') #Set viewer (optional) robot, init_transform = loadRobot( env, handType ) #load robot into Openrave and get transform of the hand in OpenRAVE iter = 0 #viewer = env.GetViewer() gc = GraspitCommander() GraspitCommander.GRASPIT_NODE_NAME = "/graspit" + str(num + 1) + "/" hand_points = getManuallyLabelledPoints(handType) centerRet, surface_norms = getHandPoints(handType) if handType == 'Barrett': #The 'trianglesTransformed' variable is a hack used to handle the trianglesTransformed = 0 # inconsistencies between the meshes of the hands. I'll explain more in a writeup elif handType == 'pr2_gripper': trianglesTransformed = 2 else: trianglesTransformed = 1 points_in_hand_plane = getGridOnHand( robot, hand_points.keys(), centerRet, surface_norms ) #Generates the points and normals of the grids on the hand points_in_hand_plane = morphPoints( robot, points_in_hand_plane, trianglesTransformed ) #Morphs the points from the grid plane onto the mesh of the hand if handType == 'pr2_gripper': numJoints = 0 else: numJoints = len(robot.GetActiveDOFValues()) T_rotation = numpy.eye(4) T_rotation[0:3, 0:3] = rotation_matrix_from_rpy(0, 0, math.pi / 2) for shape in shapes: #Only iterates over 1 shape, see line 264 parameterShape = generateParameters(shape) for parameters in parameterShape: item, filename = loadObject(env, eng, shape, parameters) STLtoPLY(filename) gc.clearWorld() gc.importRobot(handType) gc.importGraspableBody('/home/irongiant/Grasp-Metrics/Shapes/' + filename + '.ply') robot_pose = getPoseMsg(getRobotPose(numpy.eye(4), handType)) gc.setRobotPose(robot_pose) gc.setGraspableBodyPose( 0, getPoseMsg(numpy.matmul(T_rotation, numpy.eye(4)))) item.SetVisible(1) #Can make item visible or invisible for viewing print filename generateSDF( filename, 0.0025, 70) #Signed distance field needs grid resolution parameters transforms = sample_obj_transforms(env, gc, item, handType) graspit_transform = getRobotPose( transforms, handType) #Get the tarnsform for loading the hand into graspit for transform in transforms: item.SetTransform(transform) #transform[0:3,0:3] = numpy.matmul(transform[0:3,0:3], rotation_matrix_from_rpy(math.pi/2, math.pi, 0)) gc.setGraspableBodyPose( 0, getPoseMsg(numpy.matmul(T_rotation, transform))) print transform print getPoseMsg(transform) generateHandFeatures( env, gc, directory + filename + str(iter), robot, item, filename, transform, points_in_hand_plane, trianglesTransformed, math.pi / 6, 5) #Saves signed distance metric and wedge metric #env.Save(directory + filename + '_' + str(iter) + '.dae') configuration = [0] * (6 + numJoints) noise_std = [0.005, 0.005, 0.005, 0.1, 0.1, 0.1] + [ 0.1 ] * numJoints #Covariance matrix parameters for gaussian noise injection noise_distr = generateGaussianNoise( handType, configuration, noise_std, 100) #Samples noise from multivariate gaussian # volume = epsilon = [] epsilon = [] contacts = [] for noise in noise_distr: noise_induced_transform = numpy.eye(4) noise_induced_transform[0:3, 3] = numpy.array(noise[0:3]) noise_induced_transform[0:3, 0:3] = rotation_matrix_from_rpy( noise[3:6][0], noise[3:6][1], noise[3:6][2]) #if handType != "pr2_gripper": # robot.SetTransform(numpy.matmul(init_transform, noise_induced_transform)) #else: robot.SetTransform( numpy.matmul(noise_induced_transform, init_transform) ) #Inject noise into transform of hand for i in range(6, 6 + numJoints): if noise[i] < 0: noise[i] = 0 if handType != "pr2_gripper": robot.SetDOFValues( noise[6:6 + numJoints], numpy.arange(numJoints) ) #Inject noise into joint angles of hand collExcept = 0 try: robot_pose = getPoseMsg( getRobotPose(noise_induced_transform, handType)) gc.setRobotPose( robot_pose ) #Sets teh same transform set in OpenRAVE into GraspIt! result = gc.computeQuality() #Computes grasp quality except (RobotCollisionException, InvalidRobotPoseException): collExcept = 1 if not env.CheckCollision(robot) and collExcept == 0: if handType == "Barrett": gc.forceRobotDof(noise[6:6 + numJoints]) if handType == 'ShadowHand': gc.approachToContact() gc.autoGrasp() g_contacts = [] try: r = gc.getRobot(0) contact = r.robot.contacts for c in contact: point = c.ps.pose.position pose = c.ps.pose.orientation g_contacts.append([ point.x, point.y, point.z, pose.x, pose.y, pose.z, pose.w ]) result = gc.computeQuality() # volume += [result.volume] epsilon += [ result.epsilon ] #List of grasp qualities as a result of noise contacts.append( g_contacts ) # list of contacts made between hand and object during grasp as a result of noise except InvalidRobotPoseException: # volume += [0] epsilon += [0] if handType != 'ShadowHand': dofs = [0] * numJoints else: dofs = [0] * 17 if handType != 'pr2_gripper': gc.forceRobotDof(dofs) gc.autoOpen() robot.SetTransform(init_transform) robot_pose = getPoseMsg(getRobotPose(numpy.eye(4), handType)) gc.setRobotPose(robot_pose) if handType != "pr2_gripper": robot.SetDOFValues([0] * len(robot.GetActiveDOFValues()), numpy.arange( 0, len(robot.GetActiveDOFValues()))) numpy.savetxt(directory + filename + str(iter) + '_epsi_labels' + '.out', epsilon, delimiter=',', fmt='%s') #Saves the grasp qualities numpy.savetxt(directory + filename + str(iter) + '_cont_labels' + '.out', contacts, delimiter=',', fmt='%s') #Saves the contacts made iter += 1 env.Remove(env.GetBodies()[1])
math.sin(67.5), 0], [0, 1, 0, 0], [-math.sin(67.5), 0, math.cos(67.5), 0], [0, 0, 0, 1]]) robot_rotation = tf.transformations.quaternion_from_matrix( robot_transformation) robot_pose = geometry_msgs.msg.Pose() robot_pose.position.x = -3 robot_pose.position.y = -3 robot_pose.position.z = 0.01 robot_pose.orientation.x = 0 robot_pose.orientation.y = 0 robot_pose.orientation.z = 0 robot_pose.orientation.w = 0 GraspitCommander.importObstacle("floor") GraspitCommander.importGraspableBody("longBox", pose=object_pose) GraspitCommander.importRobot("fetch_gripper", pose=robot_pose) GraspitCommander.planGrasps(max_steps=50000)
def import_robot(self, robot_name, pose=None): if pose == None: GraspitCommander.importRobot(robot_name) else: GraspitCommander.importRobot(robot_name, pose)
cd graspit_ros_ws source devel/setup.bash roslaunch graspit_interface graspit_interface.roslaunch import numpy from graspit_commander import GraspitCommander from geometry_msgs.msg import Pose gc = GraspitCommander() gc.importRobot('ShadowHand') gc.autoGrasp(0) gc.importGraspableBody('/home/eadom/Grasp-Metrics/Shapes/cube_h5_w5_e5.ply') r = gc.getRobot(0) dofs = list(r.robot.dofs) q = [0,0,0,1] T = numpy.eye(4) T[2][3] = 0.023 objPose = Pose() objPose.position.x = 0 objPose.position.y = 0 objPose.position.z = 0.05 objPose.orientation.x = q[0] objPose.orientation.y = q[1] objPose.orientation.z = q[2] objPose.orientation.w = q[3] T = numpy.eye(4) robotPose = Pose() robotPose.position.x = 0