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))
#!/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)
# 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
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_graspable_object(self, object_name, pose=None):
if pose == None:
GraspitCommander.importGraspableBody(object_name)
else:
GraspitCommander.importGraspableBody(object_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