def evaluateSimAnnGrasps(pre_grasps):
import graspit_commander
gc = graspit_commander.GraspitCommander()
grasps = []
for i, pre_grasp in enumerate(pre_grasps):
gc.toggleAllCollisions(False)
gc.setRobotPose(pre_grasp.pose)
gc.forceRobotDof(pre_grasp.dofs)
gc.toggleAllCollisions(True)
gc.autoGrasp()
robot_state = gc.getRobot(0)
try:
quality = gc.computeQuality()
volume_quality = quality.volume
epsilon_quality = quality.epsilon
except:
volume_quality = -1
epsilon_quality = -1
grasp = copy.deepcopy(pre_grasp)
grasp.pose = robot_state.robot.pose
grasp.volume_quality = volume_quality
grasp.epsilon_quality = epsilon_quality
grasp.dofs = robot_state.robot.dofs
grasps.append(grasp)
return grasps
def evaluatePreGrasps(cls, pre_grasps):
import graspit_commander
gc = graspit_commander.GraspitCommander()
grasps = []
for i, pre_grasp in enumerate(pre_grasps):
gc.toggleAllCollisions(False)
gc.forceRobotDof(pre_grasp.dofs)
gc.setRobotPose(pre_grasp.pose)
gc.toggleAllCollisions(True)
gc.findInitialContact()
gc.autoGrasp()
robot_state = gc.getRobot(0)
try:
quality = gc.computeQuality()
volume_quality = quality.volume
epsilon_quality = quality.epsilon
except:
volume_quality = -1
epsilon_quality = -1
result = gc.getRobot(0)
grasp = copy.deepcopy(pre_grasp)
grasp.pose = robot_state.robot.pose
grasp.volume_quality = volume_quality
grasp.epsilon_quality = epsilon_quality
grasp.dofs = robot_state.robot.dofs
grasps.append(grasp)
grasps = sorted(grasps, key=lambda x: x.volume_quality, reverse=True)
return grasps
def get_grasps_from_graspit_sim_ann(
mesh_filepath,
target_object_pose,
search_energy,
max_steps,
robot="fetch_gripper",
obstacle_info=[]):
rospy.loginfo("get_grasps_from_graspit_sim_ann")
gc = graspit_commander.GraspitCommander()
gc.clearWorld()
gc.importRobot(robot)
gc.importGraspableBody(mesh_filepath, target_object_pose)
if obstacle_info:
for each_object in obstacle_info:
object_filepath = each_object['file_path']
# obstacle_mesh_filepath = os.path.join(args.mesh_root, object_name)
object_pose = each_object['pose']
object_pose_in_graspit = set_object_pose(object_pose)
gc.importObstacle(object_filepath, object_pose_in_graspit)
grasp_results = gc.planGrasps(search_energy=search_energy, max_steps=max_steps)
return grasp_results
def get_grasp_from_graspit(model_name,
mesh_path,
robot="fetch_gripper",
obstacle="table"):
gc = graspit_commander.GraspitCommander()
gc.clearWorld()
# gc.importObstacle(obstacle)
# table_pose = geometry_msgs.msg.Pose()
# table_pose.orientation.w = 1
# table_pose.position.x = 0.53
# table_pose.position.y = -0.687
# table_pose.position.z = 0.505
# gc.setBodyPose(0,table_pose)
gc.importRobot(robot)
gc.importGraspableBody(mesh_path)
# response = gc.planGrasps()
gl = GridSampleClient()
result = gl.computePreGrasps(20, 1)
pre_grasps = result.grasps
unchecked_for_reachability_grasps = gl.evaluatePreGrasps(
pre_grasps, pre_grasp_dofs=(4, ))
return unchecked_for_reachability_grasps
def display_grasp_in_graspit(grasp, robot, scene_object):
gc = graspit_commander.GraspitCommander()
gc.clearWorld()
gc.importRobot(robot)
gc.importGraspableBody(scene_object.mesh_filepath,
scene_object.pose_stamped.pose)
gc.setRobotPose(grasp.pose)
def visualize_grasps_in_graspit_with_mesh(grasp,
mesh_filepath,
robot="fetch_gripper"):
gc = graspit_commander.GraspitCommander()
gc.clearWorld()
gc.importRobot(robot)
gc.importGraspableBody(mesh_filepath)
gc.setRobotPose(grasp.pose)
rospy.loginfo("Showing grasp in Graspit!")
def evaluate_grasp_complete(grasps,
mesh_filepath,
search_energy="HYBRID_REACHABLE_GRASP_ENERGY",
robot="fetch_gripper",
obstacle="table"):
gc = graspit_commander.GraspitCommander()
gc.clearWorld()
gc.importRobot(robot)
gc.importGraspableBody(mesh_filepath)
grasp_results = evaluate_grasp_list(grasps, search_energy)
return grasp_results
def evaluate_grasp_list(grasps, search_energy="HYBRID_REACHABLE_GRASP_ENERGY"):
# this assumes the hand and object is already loaded
gc = graspit_commander.GraspitCommander()
energies = []
for g in grasps:
gc.autoOpen()
gc.setRobotPose(g.pose)
gc.forceRobotDof(g.dofs)
grasp_energy = gc.computeEnergy(search_energy)
energies.append(grasp_energy.energy)
# energies, grasps = zip(*sorted(zip(energies, grasps)))
grasp_results = GraspEnergies(grasps=grasps, energies=energies)
return grasp_results
def get_grasps_from_graspit(task=GraspReachabilityTask()):
"""
This takes in the task and returns list of planned grasps
:param task: type(GraspReachabilityTask)
:return:
"""
feedbacks = []
def feedback_cb(fb):
feedbacks.append(fb)
cmd_str = "roslaunch {} reachability_energy_plugin.launch".format(
task.robot_reachability_package)
start_graspit(cmd_str)
gc = graspit_commander.GraspitCommander()
gc.clearWorld()
gc.importRobot(task.hand_name)
gc.importGraspableBody(task.target_object.mesh_filepath,
task.target_object.pose_stamped.pose)
for obstacle in task.obstacles:
gc.importObstacle(obstacle.mesh_filepath, obstacle.pose_stamped.pose)
gc.planGrasps(search_energy=task.search_energy_type,
max_steps=task.max_steps,
feedback_cb=feedback_cb,
feedback_num_steps=task.planning_step_interval)
kill_graspit()
# collate results into tasks
for fb in feedbacks:
grasp_reachability_results = GraspReachabilityResult()
grasp_reachability_results.grasps = fb.grasps
grasp_reachability_results.energies = fb.energies
grasp_reachability_results.num_planning_steps = fb.current_step
grasp_reachability_results.search_energy = fb.search_energy
task.results.append(grasp_reachability_results)
return task
def calculate_grasp(self):
grasp_comm = graspit_commander.GraspitCommander()
grasp_comm.clearWorld()
grasp_comm.toggleAllCollisions(True)
grasp_comm.importObstacle("floor")
grasp_comm.importRobot('fetch_gripper')
pro = geometry_msgs.msg.Pose()
(pro.orientation.x, pro.orientation.y, pro.orientation.z, pro.orientation.w) = tf.transformations.quaternion_from_euler(0,math.pi/2, 0)
[pro.position.x, pro.position.y, pro.position.z] = [2.5, 4, 0.090]
grasp_comm.importGraspableBody("longBox", pose = pro)
grasps = grasp_comm.planGrasps()
robot = grasp_comm.getRobots().ids[0]
final_pose = grasp_comm.getRobot(robot).robot.pose
return final_pose
def get_grasp_from_graspit_sim_ann(
mesh_filepath,
search_energy="HYBRID_REACHABLE_GRASP_ENERGY",
max_steps=70000,
robot="fetch_gripper",
obstacle="table"):
gc = graspit_commander.GraspitCommander()
gc.clearWorld()
gc.importRobot(robot)
gc.importGraspableBody(mesh_filepath)
result = gc.planGrasps(search_energy=search_energy, max_steps=max_steps)
# close hand and evaluate grasps
pre_grasps = result.grasps
grasps = evaluateSimAnnGrasps(pre_grasps)
result.grasps = grasps
return result
def get_grasp_from_graspit_ellipse(
mesh_filepath,
search_energy="HYBRID_REACHABLE_GRASP_ENERGY",
robot="fetch_gripper",
pre_grasp_dofs=(4, ),
obstacle="table"):
gc = graspit_commander.GraspitCommander()
gc.clearWorld()
gc.importRobot(robot)
gc.importGraspableBody(mesh_filepath)
gl = grid_sample_client.GridSampleClient()
result = gl.computePreGrasps(20, 0)
pre_grasps = result.grasps
grasps = gl.evaluatePreGrasps(pre_grasps, pre_grasp_dofs=pre_grasp_dofs)
# evaluating grasps
result = evaluate_grasp_list(grasps, search_energy=search_energy)
return result
def main():
rospy.init_node("graspit_to_moveit_demo")
args = get_args()
gc = graspit_commander.GraspitCommander()
mgc_arm = moveit_commander.MoveGroupCommander('arm')
mgc_gripper = moveit_commander.MoveGroupCommander("gripper")
# skills.open_gripper()
# skills.home_arm(mgc_arm)
add_object_to_planning_scene(args.object_name, args.mesh_filepath,
args.object_ps, True)
# get grasps from graspit
grasp_results = skills.get_grasps_from_graspit_sim_ann(
mesh_filepath=args.mesh_filepath,
target_object_pose=args.object_ps.pose,
search_energy=args.search_energy,
max_steps=args.max_steps,
robot="MicoGripper",
obstacle_info=[])
# check grasps for reachability
grasps_for_moveit = fix_grasps_for_moveit(grasp_results,
args.object_ps.pose)
# import IPython
# IPython.embed()
# execute grasps
for i, moveit_grasp in enumerate(grasps_for_moveit):
success, pick_plan = skills.get_pick_plan(mgc_arm, moveit_grasp,
args.object_name)
if success:
result_info["index_first_reachable"] = i
success = skills.execute_pickup_plan(mgc_arm, mgc_gripper,
pick_plan, args.object_name)
break
#!/usr/bin/env python
import graspit_commander
import geometry_msgs
from tf.transformations import quaternion_from_euler
import math
import rospy
import time
grasp_comm = graspit_commander.GraspitCommander()
grasp_comm.clearWorld()
grasp_comm.toggleAllCollisions(True)
grasp_comm.importObstacle("floor")
grasp_comm.importRobot('fetch_gripper')
pro = geometry_msgs.msg.Pose()
(pro.orientation.x, pro.orientation.y, pro.orientation.z,
pro.orientation.w) = quaternion_from_euler(0, math.pi / 2, 0)
[pro.position.x, pro.position.y, pro.position.z] = [2.5, 4, 0.090]
grasp_comm.importGraspableBody("longBox", pose=pro)
grasps = grasp_comm.planGrasps()
robot = grasp_comm.getRobots().ids[0]
final_pose = grasp_comm.getRobot(robot).robot.pose
def visualize_grasps_in_graspit(grasp):
gc = graspit_commander.GraspitCommander()
gc.setRobotPose(grasp.pose)
rospy.loginfo("Showing grasp in Graspit!")