def clear_world(self):
try:
GraspitCommander.clearWorld()
except:
print "Could not call graspit commander clearWold. Will try again"
try:
GraspitCommander.clearWorld()
except:
print "Could not clear world so I will just continue"
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 run(self):
grasp_quality = GraspitCommander.computeQuality()
pub = rospy.Publisher('graspQuality', String, queue_size=3)
rate = rospy.Rate(1)
while not rospy.is_shutdown():
msg_str = str(grasp_quality.epsilon) + " " + str(
grasp_quality.volume)
pub.publish(msg_str)
rate.sleep()
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 act(self):
robot = GraspitCommander.getRobot(0)
self.dofs = robot.robot.dofs
if self.key < 15:
for i in range(15):
if actions[i] == self.action:
self.dofs[i] += ACTION_STEP
else:
for i in range(15, len(actions)):
if actions[i] == self.action:
self.dofs[i] -= ACTION_STEP
robot.moveDOFToContacts(self.dofs, 2, True)
from grasp_execution import *
print('reach')
#import reachability_analyzer
import grasp_execution
# from grasp_execution.grasp_execution_node import *
"""
"""
import IPython
IPython.embed()
"""
#from reachability_analyzer.grasp_reachability_analyzer import GraspReachabilityAnalyzer
#from grasp_execution.grasp_execution_node import *
from graspit_commander import GraspitCommander
GraspitCommander.loadWorld("test_grasp")
print('START GRASP' * 100)
grasp_output = GraspitCommander.planGrasps(max_steps=50000)
print('DONE GRASP' * 100)
print(grasp_output)
"""
import imp
try:
spam_info = imp.find_module('grasp_execution')
spam = imp.load_module('grasp_execution', *spam_info)
print(dir(spam))
print(spam.__package__)
print(spam.__name__)
print(spam.__path__)
# 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
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)
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 evaluate_pregrasp(self, pre_grasp):
GraspitCommander.toggleAllCollisions(False)
GraspitCommander.forceRobotDof([pre_grasp.dofs[0], 0, 0, 0])
GraspitCommander.setRobotPose(pre_grasp.pose)
GraspitCommander.toggleAllCollisions(True)
GraspitCommander.findInitialContact()
GraspitCommander.approachToContact(250)
GraspitCommander.autoGrasp()
result = GraspitCommander.getRobot(0)
quality = self.compute_quality()
return result, quality
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])
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:
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
def compute_quality(self):
return GraspitCommander.computeQuality()
#!/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)
#! /usr/bin/env python
import math
from graspit_commander import GraspitCommander
import rospy
import tf
import geometry_msgs.msg
import numpy as np
if __name__ == '__main__':
GraspitCommander.clearWorld()
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]])
Rotation = tf.transformations.quaternion_from_matrix(Transformation)
object_pose = geometry_msgs.msg.Pose()
object_pose.position.x = 3
object_pose.position.y = 3
object_pose.position.z = 0.1
object_pose.orientation.x = Rotation[0]
object_pose.orientation.y = Rotation[1]
def import_graspable_object(self, object_name, pose=None):
if pose == None:
GraspitCommander.importGraspableBody(object_name)
else:
GraspitCommander.importGraspableBody(object_name, pose)
def import_robot(self, robot_name, pose=None):
if pose == None:
GraspitCommander.importRobot(robot_name)
else:
GraspitCommander.importRobot(robot_name, pose)
def setup_scene_with_barrett_hand(self, add_collision_plane=False):
self.clear_world()
if add_collision_plane:
GraspitCommander.importObstacle("floor")
self.import_robot("BarrettBH8_280")
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)
def __init__(self, host, port, eval, real, id):
self.id = id
self.save_task_id = 0
self.eval = eval
self.real = real
self.h = np.arange(int(d.camera_h))[:, np.newaxis]
self.w = np.arange(int(d.camera_w))[np.newaxis, :]
self.obj_index = -1
self.in_grasp = False
self.all_pybullet_envs = {
i: None
for i in os.listdir(d.pybullet_env_dir)
}
self.all_pybullet_envs_prefix = {
os.path.splitext(f)[0]: 0
for f in self.all_pybullet_envs
}
for f in self.all_pybullet_envs:
self.all_pybullet_envs_prefix[os.path.splitext(f)[0]] += 1
self.multi_seen_envs = list(
set([
f for f in self.all_pybullet_envs_prefix if 'seen' in f
and not f.startswith('1-') and self.files_exist(f)
]))
self.multi_novel_envs = list(
set([
f for f in self.all_pybullet_envs_prefix if 'novel' in f
and not f.startswith('1-') and self.files_exist(f)
]))
self.single_seen_envs = list(
set([
f for f in self.all_pybullet_envs_prefix
if 'seen' in f and f.startswith('1-') and self.files_exist(f)
]))
self.single_novel_envs = list(
set([
f for f in self.all_pybullet_envs_prefix
if 'novel' in f and f.startswith('1-') and self.files_exist(f)
]))
self.envs_dict = {
'multi': {
'seen': self.multi_seen_envs,
'novel': self.multi_novel_envs
},
'single': {
'seen': self.single_seen_envs,
'novel': self.single_novel_envs
}
}
if self.real:
if g.hand == 'barrett':
from staubli_barrett_commander.staubli_barrett_commander import StabuliBarrettCommander
self.commander = StabuliBarrettCommander(self.id)
else:
from ur5_seed_commander.ur5_seed_commander import Ur5SeedCommander
self.commander = Ur5SeedCommander(self.id)
else:
if self.is_graspit():
self.commander = GraspitCommander(host, port)
else:
if g.hand == 'barrett':
self.commander = PybulletBarrettCommander(eval)
else:
self.commander = PybulletSeedCommander(eval)
self.host = host
self.port = port