def sample_grasps(self, planner, max_steps=50000): if planner.lower() == "simulated_annealing": grasps = GraspitCommander.planGrasps(max_steps=max_steps) elif planner.lower() == "uniform": grasps = grid_sample_client.GridSampleClient.computePreGrasps(5, 2) else: print( "Planner " + planner + " not found.\nDefaulting to the simulated annealing planner") grasps = GraspitCommander.planGrasps(max_steps=50000) return grasps.grasps
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))
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
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" service_proxy('demo_cube', '/home/masatoshichang/cube.ply', object_pose)
# add object / gripper to GraspIt! and plan grasps from geometry_msgs.msg import Pose from graspit_commander import GraspitCommander gc = GraspitCommander() gc.clearWorld() gc.importRobot('fetch_gripper') pose = Pose() pose.orientation.w = 1.0 gc.importGraspableBody( "/home/vlad/Downloads/ycb_meshes/spam_12oz/meshes/spam_12oz.ply", pose) graspit_grasps = gc.planGrasps(graspable_body_id=0) # add Spam to MoveIt import geometry_msgs.msg import world_manager from geometry_msgs.msg import Pose, Point, Quaternion pose = geometry_msgs.msg.PoseStamped() pose.header.frame_id = '/base_link' pose.pose = Pose(Point(0.042, 0.384, 0.826), Quaternion(0, 0, 0, 1)) world_manager.world_manager_client.add_mesh( 'spam', '/home/vlad/Downloads/spam_12oz.dae', pose) ### Using MoveIt to move arm -- this works by itsels, without using CURPP. The arm moves to this location. import rospy from moveit_msgs.msg import MoveItErrorCodes from moveit_python import MoveGroupInterface, PlanningSceneInterface from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
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)