class GraspActionServer:
def __init__(self):
self.server = actionlib.SimpleActionServer('object_pick', pickAction,
self.execute, False)
self.result = pickResult()
self.moveit = MoveIt()
self.object_size = {
"evergreen": [0.215, 0.05, 0.095],
"tomatosoup": [0.1, 0.03, 0.13],
"basetech": [0.16, 0.045, 0.17],
"eraserbox": [0.155, 0.05, 0.055],
"usbhub": [0.13, 0.07, 0.075]
}
self.server.start()
def execute(self, goal):
print('+++++++++++++++++++++++++++++++++++++++++++++++++')
rospy.loginfo('Picking request received')
self.moveit.close_fingers()
x, y, z = goal.request.position
z_max = goal.request.z_max
rospy.loginfo("Received z_max from behav: {}".format(z_max))
rospy.loginfo("Received z from behav: {}".format(z))
success = self.moveit.clear_object(
x, y, z, z_max, goal.request.yaw,
self.object_size[goal.request.object_label])
self.result = pickResult() # Reset object array
if success:
self.server.set_succeeded(self.result)
else:
self.moveit.rtb()
self.server.set_aborted(self.result)
def __init__(self):
self.server = actionlib.SimpleActionServer('object_pick', pickAction,
self.execute, False)
self.result = pickResult()
self.moveit = MoveIt()
self.object_size = {
"evergreen": [0.215, 0.05, 0.095],
"tomatosoup": [0.1, 0.03, 0.13],
"basetech": [0.16, 0.045, 0.17],
"eraserbox": [0.155, 0.05, 0.055],
"usbhub": [0.13, 0.07, 0.075]
}
self.server.start()
move_head = rospy.ServiceProxy("move_head", MoveHead)
clear_octomap = rospy.ServiceProxy("clear_octomap", Empty)
# Set the head into
move_head_req = MoveHeadRequest()
move_head_req.pitch = 0.7
move_head_req.yaw = 0.0
move_head(move_head_req)
model = load_model(os.environ["HOME"] + "/DATA/model.h5")
model_classification = load_model(os.environ["HOME"] + "/DATA/model_classification.h5")
clear_octomap() # Reseting the Octomap because we moved before
alice_object = AliceObject() # Class for interfacing with alice_object node
moveit = MoveIt() # Class for interfacing with MoveIt (Needs to be implemented!)
# For deleting the model from Gazebo (Pretending we dropped in a bin on Alice)
delete_model = rospy.ServiceProxy("gazebo/delete_model", DeleteModel)
# you can change the cropping in GetObjectInformation function
all_object_data = alice_object.GetObjectInformation() # Gather RGB and Depth cropping data and x,y,z information
moveit.close_fingers() # Close the fingers before planning!
zero_image = np.zeros((28,28))
one_image = np.ones((28,28))
for objects in all_object_data:
x = 0
y = 0
import rospy
from moveit import MoveIt
import argparse
import numpy as np
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('pose', type=float, nargs=6)
args = parser.parse_args()
pose = args.pose
pose[3:] = np.radians(pose[3:])
rospy.init_node('move_kinova_to')
moveit = MoveIt()
# moveit.move_to(0.37, -0.24, 0.1, math.pi , 0., 0.)
moveit.move_to(*pose)
#!/usr/bin/python
from moveit import MoveIt
import rospy
from moveit_grasp.msg import pickAction, pickResult
import actionlib
rospy.init_node('test')
result = pickResult()
moveit = MoveIt()
moveit.open_fingers()
moveit.rtb()
# def execute(self, goal):
# print('+++++++++++++++++++++++++++++++++++++++++++++++++')
# rospy.loginfo('Picking request received')
# self.moveit.close_fingers()
# x, y, z = goal.request.position
# z_max = goal.request.z_max
# rospy.loginfo("Received z_max from behav: {}".format(z_max))
# rospy.loginfo("Received z from behav: {}".format(z))
# success = self.moveit.clear_object(x, y, z, z_max, goal.request.yaw, self.object_size[goal.request.object_label])
# self.moveit.rtb()
# self.result = pickResult() # Reset object array
# if success:
# self.server.set_succeeded(self.result)
# else:
# self.server.set_aborted(self.result)
import rospy
from moveit import MoveIt
from position_action_client import move_to_position
from tf.transformations import quaternion_from_euler, euler_from_quaternion
import math
import argparse
import numpy as np
if __name__ == '__main__':
rospy.init_node('kinova_tests')
moveit = MoveIt()
if moveit.grasp(0.37, -0.24, 0.1, math.pi , 0., 0.):
print 'Success!'
else:
print 'Failure :('
moveit.open_fingers()