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
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 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")