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)