def execute(self, userdata):
rospy.loginfo('Executing state GRASP')
#Open SDH at the pre-grasp position -----------------------------------------------
sss.move("sdh", "cylopen")
#Get the current arm joint states.
sub = rospy.Subscriber("/arm_controller/state", JointTrajectoryControllerState, self.get_joint_state)
while sub.get_num_connections() == 0:
time.sleep(0.3)
continue
#Move to grasp position with SDH open ---------------------------------------------
#pregrasp
pre_grasp_stamped = PoseStamped();
pre_grasp_stamped.header.frame_id = "/base_link";
pre_grasp_stamped.pose = userdata.grasp_configuration.pre_grasp;
#grasp
grasp_stamped = PoseStamped();
grasp_stamped.header.frame_id = "/base_link";
grasp_stamped.pose = userdata.grasp_configuration.grasp;
#offset
"""
offset_x = 0#(userdata.grasp_configuration.grasp.position.x - userdata.grasp_configuration.pre_grasp.position.x)/3
offset_y = 0#(userdata.grasp_configuration.grasp.position.y - userdata.grasp_configuration.pre_grasp.position.y)/3
offset_z = 0#(userdata.grasp_configuration.grasp.position.z - userdata.grasp_configuration.pre_grasp.position.z)/3
pre_grasp_stamped.pose.position.x += offset_x
pre_grasp_stamped.pose.position.y += offset_y
pre_grasp_stamped.pose.position.z -= offset_z
grasp_stamped.pose.position.x += offset_x
grasp_stamped.pose.position.y += offset_y
grasp_stamped.pose.position.z -= offset_z
"""
sol = False
for i in range(0,10):
(pre_grasp_conf, error_code) = grasping_functions.callIKSolver(current_joint_configuration, pre_grasp_stamped)
if(error_code.val == error_code.SUCCESS):
for j in range(0,10):
(grasp_conf, error_code) = grasping_functions.callIKSolver(pre_grasp_conf, grasp_stamped)
sol = True
break
if sol:
break;
if not sol:
return 'failed';
else:
arm_handle = sss.move("arm", [pre_grasp_conf], False)
arm_handle.wait();
rospy.sleep(2);
arm_handle = sss.move("arm", [grasp_conf], False)
arm_handle.wait();
rospy.sleep(2);
#Close SDH based on the grasp configuration to grasp.
arm_handle = sss.move("sdh", [list(userdata.grasp_configuration.sdh_joint_values)], False)
arm_handle.wait();
rospy.sleep(2);
#TODO: Confirm the grasp based on force feedback
successful_grasp = grasping_functions.sdh_tactil_sensor_result();
if successful_grasp:
return 'succeeded'
else:
#TODO: Regrasp (close MORE the fingers)
regrasp = list(userdata.grasp_configuration.sdh_joint_values)
regrasp[2] -= 0.1
regrasp[4] -= 0.1
regrasp[6] -= 0.1
arm_handle = sss.move("sdh", [regrasp], False)
arm_handle.wait();
successful_regrasp = grasping_functions.sdh_tactil_sensor_result();
if successful_regrasp:
return 'succeeded'
else:
return 'failed'
def execute(self):
# prepare for grasping
#self.sss.move("base","kitchen")
self.sss.move("arm", "look_at_table")
self.sss.move("sdh", "cylopen")
rospy.sleep(2)
#current_joint_configuration
sub = rospy.Subscriber("/arm_controller/state",
JointTrajectoryControllerState,
self.get_joint_state)
while sub.get_num_connections() == 0:
time.sleep(0.3)
continue
#Detection
self.srv_name_object_detection = '/object_detection/detect_object'
detector_service = rospy.ServiceProxy(self.srv_name_object_detection,
DetectObjects)
req = DetectObjectsRequest()
req.object_name.data = "anti_grippal"
res = detector_service(req)
for i in range(0, len(res.object_list.detections)):
print str(i) + ": " + res.object_list.detections[i].label
index = -1
while (index < 0):
index = int(raw_input("Select object to grasp: "))
obj = res.object_list.detections[index].pose
obj.header.stamp = self.listener.getLatestCommonTime(
"/base_link", obj.header.frame_id)
obj = self.listener.transformPose("/base_link", obj)
print "Object pose in base_link:", obj
object_id = 28 #self.getObjectID(req.object_name.data);
print "Calling get_grasps_from_position service..."
get_grasps_from_position = rospy.ServiceProxy(
'get_grasps_from_position', GetGraspsFromPosition)
req = srs_grasping.srv.GetGraspsFromPositionRequest(
object_id, obj.pose)
grasp_configuration = (
get_grasps_from_position(req)).grasp_configuration
print "get_grasps_from_position service has finished."
for i in range(0, len(grasp_configuration)):
pre = PoseStamped()
pre.header.stamp = rospy.Time.now()
pre.header.frame_id = "/base_link"
pre.pose.position.x = grasp_configuration[i].pre_grasp.position.x
pre.pose.position.y = grasp_configuration[i].pre_grasp.position.y
pre.pose.position.z = grasp_configuration[i].pre_grasp.position.z
pre.pose.orientation.x = grasp_configuration[
i].pre_grasp.orientation.x
pre.pose.orientation.y = grasp_configuration[
i].pre_grasp.orientation.y
pre.pose.orientation.z = grasp_configuration[
i].pre_grasp.orientation.z
pre.pose.orientation.w = grasp_configuration[
i].pre_grasp.orientation.w
g = PoseStamped()
g.header.stamp = rospy.Time.now()
g.header.frame_id = "/base_link"
g.pose.position.x = grasp_configuration[i].grasp.position.x
g.pose.position.y = grasp_configuration[i].grasp.position.y
g.pose.position.z = grasp_configuration[i].grasp.position.z
g.pose.orientation.x = grasp_configuration[i].grasp.orientation.x
g.pose.orientation.y = grasp_configuration[i].grasp.orientation.y
g.pose.orientation.z = grasp_configuration[i].grasp.orientation.z
g.pose.orientation.w = grasp_configuration[i].grasp.orientation.w
offset_x = 0 #(g.pose.position.x - pre.pose.position.x)/3
offset_y = 0 #(g.pose.position.y - pre.pose.position.y)/3
offset_z = 0 #(g.pose.position.z - pre.pose.position.z)/3
pre.pose.position.x += offset_x
pre.pose.position.y += offset_y
pre.pose.position.z -= offset_z
g.pose.position.x += offset_x
g.pose.position.y += offset_y
g.pose.position.z -= offset_z
sol = False
for w in range(0, 10):
(pre_grasp_conf, error_code) = grasping_functions.callIKSolver(
current_joint_configuration, pre)
if (error_code.val == error_code.SUCCESS):
for k in range(0, 10):
(grasp_conf,
error_code) = grasping_functions.callIKSolver(
pre_grasp_conf, g)
if (error_code.val == error_code.SUCCESS):
print str(i) + ": IK solution found"
sol = True
break
if sol:
break
if sol:
print "PREGRASP: #####################################"
print grasp_configuration[i]
print "###############################################"
res = raw_input("Execute this grasp? (y/n): ")
if res != "y":
continue
else:
#grasping_functions.grasp_view(object_id, grasp_configuration[i], obj.pose)
# execute grasp
handle_say = self.sss.say(
["I am grasping the object now."], False)
handle_arm = self.sss.move("arm", [pre_grasp_conf], False)
self.sss.move("sdh", "cylopen")
handle_say.wait()
handle_arm.wait()
raw_input("Grasp...")
handle_arm2 = self.sss.move("arm", [grasp_conf], False)
handle_arm2.wait()
raw_input("Catch the object")
handle_sdh = self.sss.move(
"sdh", [list(grasp_configuration[i].sdh_joint_values)],
False)
handle_sdh.wait()
rospy.sleep(4)
"""
# place obj on tray
handle01 = self.sss.move("arm","grasp-to-tray",False)
self.sss.move("tray","up")
handle01.wait()
self.sss.move("arm","tray")
self.sss.move("sdh","cylopen")
handle01 = self.sss.move("arm","tray-to-folded",False)
self.sss.sleep(4)
self.sss.move("sdh","cylclosed",False)
handle01.wait()
# deliver cup to order position
#self.sss.move("base","order")
self.sss.say("Here's your drink.")
self.sss.move("torso","nod")
res = grasping_functions.sdh_tactil_sensor_result()
if not res:
val = list(grasp_configuration[i].sdh_joint_values)
val[2] -= 0.1
val[4] -= 0.1
val[6] -= 0.1
handle_sdh = self.sss.move("sdh", [val], False)
handle_sdh.wait()
rospy.sleep(4);
return 0;
"""
return 0
return -1
def execute(self):
# prepare for grasping
#self.sss.move("base","kitchen")
self.sss.move("arm","look_at_table")
self.sss.move("sdh","cylopen")
rospy.sleep(2);
#current_joint_configuration
sub = rospy.Subscriber("/arm_controller/state", JointTrajectoryControllerState, self.get_joint_state)
while sub.get_num_connections() == 0:
time.sleep(0.3)
continue
#Detection
self.srv_name_object_detection = '/object_detection/detect_object'
detector_service = rospy.ServiceProxy(self.srv_name_object_detection, DetectObjects)
req = DetectObjectsRequest()
req.object_name.data = "anti_grippal"
res = detector_service(req)
for i in range(0,len(res.object_list.detections)):
print str(i)+": "+res.object_list.detections[i].label
index = -1;
while (index < 0):
index = int(raw_input("Select object to grasp: "))
obj = res.object_list.detections[index].pose
obj.header.stamp = self.listener.getLatestCommonTime("/base_link", obj.header.frame_id)
obj = self.listener.transformPose("/base_link", obj)
print "Object pose in base_link:",obj
object_id = 28#self.getObjectID(req.object_name.data);
print "Calling get_grasps_from_position service..."
get_grasps_from_position = rospy.ServiceProxy('get_grasps_from_position', GetGraspsFromPosition)
req = srs_grasping.srv.GetGraspsFromPositionRequest(object_id, obj.pose)
grasp_configuration = (get_grasps_from_position(req)).grasp_configuration
print "get_grasps_from_position service has finished."
for i in range(0,len(grasp_configuration)):
pre = PoseStamped()
pre.header.stamp = rospy.Time.now()
pre.header.frame_id = "/base_link"
pre.pose.position.x = grasp_configuration[i].pre_grasp.position.x
pre.pose.position.y = grasp_configuration[i].pre_grasp.position.y
pre.pose.position.z = grasp_configuration[i].pre_grasp.position.z
pre.pose.orientation.x = grasp_configuration[i].pre_grasp.orientation.x
pre.pose.orientation.y = grasp_configuration[i].pre_grasp.orientation.y
pre.pose.orientation.z = grasp_configuration[i].pre_grasp.orientation.z
pre.pose.orientation.w = grasp_configuration[i].pre_grasp.orientation.w
g = PoseStamped()
g.header.stamp = rospy.Time.now()
g.header.frame_id = "/base_link"
g.pose.position.x = grasp_configuration[i].grasp.position.x
g.pose.position.y = grasp_configuration[i].grasp.position.y
g.pose.position.z = grasp_configuration[i].grasp.position.z
g.pose.orientation.x = grasp_configuration[i].grasp.orientation.x
g.pose.orientation.y = grasp_configuration[i].grasp.orientation.y
g.pose.orientation.z = grasp_configuration[i].grasp.orientation.z
g.pose.orientation.w = grasp_configuration[i].grasp.orientation.w
offset_x = 0#(g.pose.position.x - pre.pose.position.x)/3
offset_y = 0#(g.pose.position.y - pre.pose.position.y)/3
offset_z = 0#(g.pose.position.z - pre.pose.position.z)/3
pre.pose.position.x += offset_x
pre.pose.position.y += offset_y
pre.pose.position.z -= offset_z
g.pose.position.x += offset_x
g.pose.position.y += offset_y
g.pose.position.z -= offset_z
sol = False
for w in range(0,10):
(pre_grasp_conf, error_code) = grasping_functions.callIKSolver(current_joint_configuration, pre)
if(error_code.val == error_code.SUCCESS):
for k in range(0,10):
(grasp_conf, error_code) = grasping_functions.callIKSolver(pre_grasp_conf, g)
if(error_code.val == error_code.SUCCESS):
print str(i)+": IK solution found"
sol = True
break
if sol:
break
if sol:
print "PREGRASP: #####################################"
print grasp_configuration[i]
print "###############################################"
res = raw_input("Execute this grasp? (y/n): ")
if res != "y":
continue
else:
#grasping_functions.grasp_view(object_id, grasp_configuration[i], obj.pose)
# execute grasp
handle_say = self.sss.say(["I am grasping the object now."], False)
handle_arm = self.sss.move("arm", [pre_grasp_conf], False)
self.sss.move("sdh", "cylopen")
handle_say.wait()
handle_arm.wait()
raw_input("Grasp...")
handle_arm2 = self.sss.move("arm", [grasp_conf], False)
handle_arm2.wait()
raw_input("Catch the object")
handle_sdh = self.sss.move("sdh", [list(grasp_configuration[i].sdh_joint_values)], False)
handle_sdh.wait()
rospy.sleep(4);
"""
# place obj on tray
handle01 = self.sss.move("arm","grasp-to-tray",False)
self.sss.move("tray","up")
handle01.wait()
self.sss.move("arm","tray")
self.sss.move("sdh","cylopen")
handle01 = self.sss.move("arm","tray-to-folded",False)
self.sss.sleep(4)
self.sss.move("sdh","cylclosed",False)
handle01.wait()
# deliver cup to order position
#self.sss.move("base","order")
self.sss.say("Here's your drink.")
self.sss.move("torso","nod")
res = grasping_functions.sdh_tactil_sensor_result()
if not res:
val = list(grasp_configuration[i].sdh_joint_values)
val[2] -= 0.1
val[4] -= 0.1
val[6] -= 0.1
handle_sdh = self.sss.move("sdh", [val], False)
handle_sdh.wait()
rospy.sleep(4);
return 0;
"""
return 0;
return -1
def get_grasps_from_position(self, req):
x = time.time()
rospy.loginfo("/get_grasps_from_position service has been called...")
obj_id = req.object_id
obj_pose = req.object_pose
req = GetGraspConfigurationsRequest()
req.object_id = obj_id
grasp_configuration = (self.client(req)).grasp_configuration
#current_joint_configuration
while self.arm_state.get_num_connections() == 0:
time.sleep(0.3)
continue
rotacion = grasping_functions.rotation_matrix(obj_pose)
resp = GetGraspsFromPositionResponse()
resp.feasible_grasp_available = False
resp.grasp_configuration = []
for i in range(0, len(grasp_configuration)):
pre_trans = rotacion * grasping_functions.matrix_from_pose(
grasp_configuration[i].pre_grasp.pose)
grasp_trans = rotacion * grasping_functions.matrix_from_pose(
grasp_configuration[i].grasp.pose)
t = translation_from_matrix(pre_trans)
q = quaternion_from_matrix(pre_trans)
tg = translation_from_matrix(grasp_trans)
qg = quaternion_from_matrix(grasp_trans)
pre = PoseStamped()
pre.header.stamp = rospy.Time.now()
pre.header.frame_id = "/base_link"
pre.pose.position.x = t[0]
pre.pose.position.y = t[1]
pre.pose.position.z = t[2]
pre.pose.orientation.x = q[0]
pre.pose.orientation.y = q[1]
pre.pose.orientation.z = q[2]
pre.pose.orientation.w = q[3]
g = PoseStamped()
g.header.stamp = rospy.Time.now()
g.header.frame_id = "/base_link"
g.pose.position.x = tg[0]
g.pose.position.y = tg[1]
g.pose.position.z = tg[2]
g.pose.orientation.x = qg[0]
g.pose.orientation.y = qg[1]
g.pose.orientation.z = qg[2]
g.pose.orientation.w = qg[3]
sol = False
for w in range(0, self.ik_loop_reply):
(pre_grasp_conf, error_code) = grasping_functions.callIKSolver(
current_joint_configuration, pre)
if (error_code.val == error_code.SUCCESS):
for k in range(0, self.ik_loop_reply):
(grasp_conf,
error_code) = grasping_functions.callIKSolver(
pre_grasp_conf, g)
if (error_code.val == error_code.SUCCESS):
new_valid_grasp = GraspSubConfiguration()
new_valid_grasp.sdh_joint_values = grasp_configuration[
i].sdh_joint_values
new_valid_grasp.grasp = g.pose
new_valid_grasp.pre_grasp = pre.pose
new_valid_grasp.category = grasping_functions.get_grasp_category(
pre.pose.position, g.pose.position)
sol = grasping_functions.valid_grasp(
new_valid_grasp)
if sol:
resp.feasible_grasp_available = True
resp.grasp_configuration.append(
new_valid_grasp)
break
if sol:
break
#for
#for
#order grasps
if len(resp.grasp_configuration) > 0:
(resp.grasp_configuration).sort()
rospy.loginfo(
str(len(resp.grasp_configuration)) +
" valid grasps for this pose.")
rospy.loginfo("/get_grasps_from_position call has finished.")
print "Time employed: " + str(time.time() - x)
print "---------------------------------------"
return resp
def execute(self, userdata):
rospy.loginfo('Executing state GRASP')
#Open SDH at the pre-grasp position -----------------------------------------------
#sss.move("sdh", "cylopen")
pre_p = userdata.grasp_configuration[userdata.grasp_configuration_id].pre_grasp.position
g_p = userdata.grasp_configuration[userdata.grasp_configuration_id].grasp.position
category = grasping_functions.get_grasp_category(pre_p , g_p);
if category == "TOP" or category == "DOWN":
userdata.grasp_categorisation = 'top'
arm_handle=sss.move("sdh", "spheropen")
else:
userdata.grasp_categorisation = 'side'
arm_handle=sss.move("sdh", "cylopen")
arm_handle.wait()
rospy.sleep(2)
#Get the current arm joint states.
while self.sub.get_num_connections() == 0:
time.sleep(0.3)
continue
#Move to grasp position with SDH open ---------------------------------------------
#pregrasp
pre_grasp_stamped = PoseStamped();
pre_grasp_stamped.header.frame_id = "/base_link";
pre_grasp_stamped.pose = userdata.grasp_configuration[userdata.grasp_configuration_id].pre_grasp;
#grasp
grasp_stamped = PoseStamped();
grasp_stamped.header.frame_id = "/base_link";
grasp_stamped.pose = userdata.grasp_configuration[userdata.grasp_configuration_id].grasp;
#postgrasp
post_grasp_stamped = copy.deepcopy(grasp_stamped);
post_grasp_stamped.pose.position.z += 0.1;
#offset
"""
offset_x = 0#(userdata.grasp_configuration.grasp.position.x - userdata.grasp_configuration.pre_grasp.position.x)/3
offset_y = 0#(userdata.grasp_configuration.grasp.position.y - userdata.grasp_configuration.pre_grasp.position.y)/3
offset_z = 0#(userdata.grasp_configuration.grasp.position.z - userdata.grasp_configuration.pre_grasp.position.z)/3
pre_grasp_stamped.pose.position.x += offset_x
pre_grasp_stamped.pose.position.y += offset_y
pre_grasp_stamped.pose.position.z -= offset_z
grasp_stamped.pose.position.x += offset_x
grasp_stamped.pose.position.y += offset_y
grasp_stamped.pose.position.z -= offset_z
"""
#global current_joint_configuration
sol = False
for w in range(0,10):
(pre_grasp_conf, error_code) = grasping_functions.callIKSolver(self.current_joint_configuration, pre_grasp_stamped)
if(error_code.val == error_code.SUCCESS):
for k in range(0,10):
(grasp_conf, error_code) = grasping_functions.callIKSolver(pre_grasp_conf, grasp_stamped)
if(error_code.val == error_code.SUCCESS):
(post_grasp_conf, error_code) = grasping_functions.callIKSolver(pre_grasp_conf, post_grasp_stamped)
if(error_code.val == error_code.SUCCESS):
sol = True
break
if sol:
break
if not sol:
return 'not_completed';
else:
#sss.move("torso","home")
arm_handle = sss.move("arm", [pre_grasp_conf, grasp_conf], False)
arm_handle.wait();
#Close SDH based on the grasp configuration to grasp.
arm_handle = sss.move("sdh", [list(userdata.grasp_configuration[userdata.grasp_configuration_id].sdh_joint_values)], False)
arm_handle.wait();
rospy.sleep(2);
#TODO: Confirm the grasp based on force feedback
successful_grasp = False#grasping_functions.sdh_tactil_sensor_result();
if not successful_grasp:
#TODO: Regrasp (close MORE the fingers)
regrasp = list(userdata.grasp_configuration[userdata.grasp_configuration_id].sdh_joint_values)
print "Current config, trying regrasp", regrasp
#regrasp[2] -= 0.1
#regrasp[4] -= 0.1
#regrasp[6] -= 0.1
regrasp[1] += 0.07
regrasp[3] += 0.07
regrasp[5] += 0.07
print "to", regrasp
arm_handle = sss.move("sdh", [regrasp], False)
arm_handle.wait();
successful_grasp = True#grasping_functions.sdh_tactil_sensor_result();
if not successful_grasp:
return 'not_completed'
sss.move("arm",[post_grasp_conf,"look_at_table","hold"])
return 'succeeded'
def get_grasps_from_position(self, req):
x = time.time();
rospy.loginfo("/get_grasps_from_position service has been called...");
obj_id = req.object_id;
obj_pose = req.object_pose;
req = GetGraspConfigurationsRequest();
req.object_id = obj_id;
grasp_configuration = (self.client(req)).grasp_configuration;
#current_joint_configuration
rospy.loginfo("Waiting /arm_controller/state...");
sub = rospy.Subscriber("/arm_controller/state", JointTrajectoryControllerState, self.get_joint_state);
while sub.get_num_connections() == 0:
time.sleep(0.3);
continue;
rospy.loginfo("/arm_controller/state has finished.");
rotacion = grasping_functions.rotation_matrix(obj_pose);
resp = GetGraspsFromPositionResponse();
resp.feasible_grasp_available = False;
resp.grasp_configuration = [];
for i in range(0,len(grasp_configuration)):
pre_trans = rotacion * grasping_functions.matrix_from_pose(grasp_configuration[i].pre_grasp.pose);
grasp_trans = rotacion * grasping_functions.matrix_from_pose(grasp_configuration[i].grasp.pose);
t = translation_from_matrix(pre_trans);
q = quaternion_from_matrix(pre_trans);
tg = translation_from_matrix(grasp_trans);
qg = quaternion_from_matrix(grasp_trans);
pre = PoseStamped();
pre.header.stamp = rospy.Time.now();
pre.header.frame_id = "/base_link";
pre.pose.position.x = t[0];
pre.pose.position.y = t[1];
pre.pose.position.z = t[2];
pre.pose.orientation.x = q[0];
pre.pose.orientation.y = q[1];
pre.pose.orientation.z = q[2];
pre.pose.orientation.w = q[3];
g = PoseStamped();
g.header.stamp = rospy.Time.now();
g.header.frame_id = "/base_link";
g.pose.position.x = tg[0];
g.pose.position.y = tg[1];
g.pose.position.z = tg[2];
g.pose.orientation.x = qg[0];
g.pose.orientation.y = qg[1];
g.pose.orientation.z = qg[2];
g.pose.orientation.w = qg[3];
offset_x = 0#(g.pose.position.x - pre.pose.position.x)/3
offset_y = 0#(g.pose.position.y - pre.pose.position.y)/3
offset_z = 0#(g.pose.position.z - pre.pose.position.z)/3
pre.pose.position.x += offset_x
pre.pose.position.y += offset_y
pre.pose.position.z -= offset_z
g.pose.position.x += offset_x
g.pose.position.y += offset_y
g.pose.position.z -= offset_z
sol = False;
for w in range(0,self.ik_loop_reply):
(pre_grasp_conf, error_code) = grasping_functions.callIKSolver(current_joint_configuration, pre);
if(error_code.val == error_code.SUCCESS):
for k in range(0,self.ik_loop_reply):
(grasp_conf, error_code) = grasping_functions.callIKSolver(pre_grasp_conf, g);
if(error_code.val == error_code.SUCCESS):
new_valid_grasp = GraspSubConfiguration();
new_valid_grasp.sdh_joint_values = grasp_configuration[i].sdh_joint_values;
new_valid_grasp.grasp = g.pose;
new_valid_grasp.pre_grasp = pre.pose;
new_valid_grasp.category = grasping_functions.get_grasp_category(pre.pose.position, g.pose.position);
sol = grasping_functions.valid_grasp(new_valid_grasp);
if sol:
resp.feasible_grasp_available = True;
resp.grasp_configuration.append(new_valid_grasp);
break;
if sol:
break;
#for
#for
#order grasps
if len(resp.grasp_configuration) > 0:
(resp.grasp_configuration).sort();
rospy.loginfo(str(len(resp.grasp_configuration))+" valid grasps for this pose.");
rospy.loginfo("/get_grasps_from_position call has finished.");
print "Time employed: " + str(time.time() - x);
print "---------------------------------------";
return resp;
def get_grasps_from_position(self, req):
x = time.time()
rospy.loginfo("/get_grasps_from_position service has been called...")
obj_id = req.object_id
obj_pose = req.object_pose
req = GetGraspConfigurationsRequest()
req.object_id = obj_id
grasp_configuration = (self.client(req)).grasp_configuration
# current_joint_configuration
while self.arm_state.get_num_connections() == 0:
time.sleep(0.3)
continue
rotacion = grasping_functions.rotation_matrix(obj_pose)
resp = GetGraspsFromPositionResponse()
resp.feasible_grasp_available = False
resp.grasp_configuration = []
for i in range(0, len(grasp_configuration)):
pre_trans = rotacion * grasping_functions.matrix_from_pose(grasp_configuration[i].pre_grasp.pose)
grasp_trans = rotacion * grasping_functions.matrix_from_pose(grasp_configuration[i].grasp.pose)
t = translation_from_matrix(pre_trans)
q = quaternion_from_matrix(pre_trans)
tg = translation_from_matrix(grasp_trans)
qg = quaternion_from_matrix(grasp_trans)
pre = PoseStamped()
pre.header.stamp = rospy.Time.now()
pre.header.frame_id = "/base_link"
pre.pose.position.x = t[0]
pre.pose.position.y = t[1]
pre.pose.position.z = t[2]
pre.pose.orientation.x = q[0]
pre.pose.orientation.y = q[1]
pre.pose.orientation.z = q[2]
pre.pose.orientation.w = q[3]
g = PoseStamped()
g.header.stamp = rospy.Time.now()
g.header.frame_id = "/base_link"
g.pose.position.x = tg[0]
g.pose.position.y = tg[1]
g.pose.position.z = tg[2]
g.pose.orientation.x = qg[0]
g.pose.orientation.y = qg[1]
g.pose.orientation.z = qg[2]
g.pose.orientation.w = qg[3]
sol = False
for w in range(0, self.ik_loop_reply):
(pre_grasp_conf, error_code) = grasping_functions.callIKSolver(current_joint_configuration, pre)
if error_code.val == error_code.SUCCESS:
for k in range(0, self.ik_loop_reply):
(grasp_conf, error_code) = grasping_functions.callIKSolver(pre_grasp_conf, g)
if error_code.val == error_code.SUCCESS:
new_valid_grasp = GraspSubConfiguration()
new_valid_grasp.sdh_joint_values = grasp_configuration[i].sdh_joint_values
new_valid_grasp.grasp = g.pose
new_valid_grasp.pre_grasp = pre.pose
new_valid_grasp.category = grasping_functions.get_grasp_category(
pre.pose.position, g.pose.position
)
sol = grasping_functions.valid_grasp(new_valid_grasp)
if sol:
resp.feasible_grasp_available = True
resp.grasp_configuration.append(new_valid_grasp)
break
if sol:
break
# for
# for
# order grasps
if len(resp.grasp_configuration) > 0:
(resp.grasp_configuration).sort()
rospy.loginfo(str(len(resp.grasp_configuration)) + " valid grasps for this pose.")
rospy.loginfo("/get_grasps_from_position call has finished.")
print "Time employed: " + str(time.time() - x)
print "---------------------------------------"
return resp
offset_x = 0#(userdata.grasp_configuration.grasp.position.x - userdata.grasp_configuration.pre_grasp.position.x)/3
offset_y = 0#(userdata.grasp_configuration.grasp.position.y - userdata.grasp_configuration.pre_grasp.position.y)/3
offset_z = 0#(userdata.grasp_configuration.grasp.position.z - userdata.grasp_configuration.pre_grasp.position.z)/3
pre_grasp_stamped.pose.position.x += offset_x
pre_grasp_stamped.pose.position.y += offset_y
pre_grasp_stamped.pose.position.z -= offset_z
grasp_stamped.pose.position.x += offset_x
grasp_stamped.pose.position.y += offset_y
grasp_stamped.pose.position.z -= offset_z
"""
sol = False
for i in range(0,10):
(pre_grasp_conf, error_code) = grasping_functions.callIKSolver(current_joint_configuration, pre_grasp_stamped)
if(error_code.val == error_code.SUCCESS):
for j in range(0,10):
(grasp_conf, error_code) = grasping_functions.callIKSolver(pre_grasp_conf, grasp_stamped)
sol = True
break
if sol:
break;
if not sol:
return 'failed';
else:
arm_handle = sss.move("arm", [pre_grasp_conf], False)
arm_handle.wait();
rospy.sleep(2);
def execute_cb(self, server_goal):
x = time.time()
rospy.loginfo("/get_grasps_from_position_server has been called...")
obj_id = server_goal.object_id
obj = server_goal.object_pose
goal = GraspCGoal()
goal.object_id = obj_id
self.client.send_goal(goal)
self.client.wait_for_result(rospy.Duration.from_sec(5.0))
grasp_configuration = (self.client.get_result()).grasp_configuration
# current_joint_configuration
rospy.loginfo("Waiting /arm_controller/state...")
sub = rospy.Subscriber("/arm_controller/state", JointTrajectoryControllerState, self.get_joint_state)
while sub.get_num_connections() == 0:
time.sleep(0.3)
continue
rospy.loginfo("/arm_controller/state has finished.")
rotacion = grasping_functions.rotation_matrix(obj)
server_result = GraspFActionResult().result
server_result.feasible_grasp_available = False
server_result.grasp_configuration = []
for i in range(0, len(grasp_configuration)):
pre_trans = rotacion * grasping_functions.matrix_from_pose(grasp_configuration[i].pre_grasp.pose)
grasp_trans = rotacion * grasping_functions.matrix_from_pose(grasp_configuration[i].grasp.pose)
t = translation_from_matrix(pre_trans)
q = quaternion_from_matrix(pre_trans)
tg = translation_from_matrix(grasp_trans)
qg = quaternion_from_matrix(grasp_trans)
pre = PoseStamped()
pre.header.stamp = rospy.Time.now()
pre.header.frame_id = "/base_link"
pre.pose.position.x = t[0]
pre.pose.position.y = t[1]
pre.pose.position.z = t[2]
pre.pose.orientation.x = q[0]
pre.pose.orientation.y = q[1]
pre.pose.orientation.z = q[2]
pre.pose.orientation.w = q[3]
g = PoseStamped()
g.header.stamp = rospy.Time.now()
g.header.frame_id = "/base_link"
g.pose.position.x = tg[0]
g.pose.position.y = tg[1]
g.pose.position.z = tg[2]
g.pose.orientation.x = qg[0]
g.pose.orientation.y = qg[1]
g.pose.orientation.z = qg[2]
g.pose.orientation.w = qg[3]
offset_x = 0 # (g.pose.position.x - pre.pose.position.x)/3
offset_y = 0 # (g.pose.position.y - pre.pose.position.y)/3
offset_z = 0 # (g.pose.position.z - pre.pose.position.z)/3
pre.pose.position.x += offset_x
pre.pose.position.y += offset_y
pre.pose.position.z -= offset_z
g.pose.position.x += offset_x
g.pose.position.y += offset_y
g.pose.position.z -= offset_z
sol = False
for w in range(0, self.ik_loop_reply):
(pre_grasp_conf, error_code) = grasping_functions.callIKSolver(current_joint_configuration, pre)
if error_code.val == error_code.SUCCESS:
for k in range(0, self.ik_loop_reply):
(grasp_conf, error_code) = grasping_functions.callIKSolver(pre_grasp_conf, g)
if error_code.val == error_code.SUCCESS:
print i
new_valid_grasp = GraspSubConfiguration()
new_valid_grasp.sdh_joint_values = grasp_configuration[i].sdh_joint_values
new_valid_grasp.grasp = g.pose
new_valid_grasp.pre_grasp = pre.pose
new_valid_grasp.category = grasping_functions.get_grasp_category(
pre.pose.position, g.pose.position
)
server_result.feasible_grasp_available = True
server_result.grasp_configuration.append(new_valid_grasp)
sol = True
break
if sol:
break
# for
# while
# order grasps
if len(server_result.grasp_configuration) > 0:
(server_result.grasp_configuration).sort()
rospy.loginfo(str(len(server_result.grasp_configuration)) + " valid grasps for this pose.")
rospy.loginfo("/get_grasps_from_position call has finished.")
print "Time employed: " + str(time.time() - x)
print "---------------------------------------"
self.get_grasps_from_position_server.set_succeeded(server_result)
def execute(self, userdata):
#TODO: THIS PART SHOULD BE IN THE PREVIOUS STATE #####################################################
grasp_configuration_id = len(userdata.grasp_configuration) - 1
#TODO: userdata.grasp_configuration_id
for i in range(0, len(userdata.grasp_configuration)):
print userdata.grasp_configuration[i].category
if userdata.grasp_configuration[i].category == "TOP":
grasp_configuration_id = i
#TODO: userdata.grasp_configuration_id
break
#######################################################################################################
category = userdata.grasp_configuration[
grasp_configuration_id].category #TODO: userdata.grasp_configuration_id
if category == "TOP":
userdata.grasp_categorisation = 'top'
sdh_handle = sss.move("sdh", "spheropen")
else:
userdata.grasp_categorisation = 'side'
sdh_handle = sss.move("sdh", "cylopen")
sdh_handle.wait()
rospy.sleep(2)
#Get the current arm joint states.
while self.arm_state.get_num_connections() == 0:
time.sleep(0.3)
continue
#Move to grasp position with SDH open ---------------------------------------------
#pregrasp
pre_grasp_stamped = PoseStamped()
pre_grasp_stamped.header.frame_id = "/base_link"
pre_grasp_stamped.pose = userdata.grasp_configuration[
grasp_configuration_id].pre_grasp
#TODO: userdata.grasp_configuration_id
#grasp
grasp_stamped = PoseStamped()
grasp_stamped.header.frame_id = "/base_link"
grasp_stamped.pose = userdata.grasp_configuration[
grasp_configuration_id].grasp
#TODO: userdata.grasp_configuration_id
#postgrasp
post_grasp_stamped = copy.deepcopy(grasp_stamped)
post_grasp_stamped.pose.position.z += 0.1
sol = False
for w in range(0, 10):
(pre_grasp_conf, error_code) = grasping_functions.callIKSolver(
self.current_arm_state, pre_grasp_stamped)
if (error_code.val == error_code.SUCCESS):
for k in range(0, 10):
(grasp_conf, error_code) = grasping_functions.callIKSolver(
pre_grasp_conf, grasp_stamped)
if (error_code.val == error_code.SUCCESS):
(post_grasp_conf,
error_code) = grasping_functions.callIKSolver(
pre_grasp_conf, post_grasp_stamped)
if (error_code.val == error_code.SUCCESS):
sol = True
break
if sol:
break
if not sol:
sss.say(["I can not catch the object!"], False)
return 'not_completed'
else:
sss.say(["I am grasping the object now!"], False)
#Close SDH based on the grasp configuration to grasp.
arm_handle = sss.move("arm", [pre_grasp_conf, grasp_conf])
arm_handle.wait()
rospy.sleep(4)
#Close SDH based on the grasp configuration to grasp.
ssh_handle = sss.move("sdh", [
list(userdata.grasp_configuration[grasp_configuration_id].
sdh_joint_values)
]) #TODO: userdata.grasp_configuration_id
ssh_handle.wait()
rospy.sleep(2)
#Confirm the grasp based on force feedback
successful_grasp = grasping_functions.sdh_tactil_sensor_result()
#False
if not successful_grasp:
#Regrasp (close MORE the fingers)
regrasp = list(
userdata.grasp_configuration[grasp_configuration_id].
sdh_joint_values) #TODO: userdata.grasp_configuration_id
print "Current config, trying regrasp", regrasp
regrasp[1] += 0.07
regrasp[3] += 0.07
regrasp[5] += 0.07
print "to", regrasp
sdh_handle = sss.move("sdh", [regrasp])
sdh_handle.wait()
rospy.sleep(2)
successful_grasp = grasping_functions.sdh_tactil_sensor_result(
)
#True
if not successful_grasp:
sss.say(["I can not catch the object.!"], False)
return 'not_completed'
arm_handle = sss.move("arm",
[post_grasp_conf, "look_at_table", "hold"])
arm_handle.wait()
rospy.sleep(2)
sss.say(["I have grasped the object with success!"], False)
return 'succeeded'
def execute(self, userdata):
#TODO: THIS PART SHOULD BE IN THE PREVIOUS STATE #####################################################
grasp_configuration_id = len(userdata.grasp_configuration)-1; #TODO: userdata.grasp_configuration_id
for i in range(0, len(userdata.grasp_configuration)):
print userdata.grasp_configuration[i].category
if userdata.grasp_configuration[i].category == "TOP":
grasp_configuration_id = i; #TODO: userdata.grasp_configuration_id
break;
#######################################################################################################
category = userdata.grasp_configuration[grasp_configuration_id].category #TODO: userdata.grasp_configuration_id
if category == "TOP":
userdata.grasp_categorisation = 'top'
sdh_handle=sss.move("sdh", "spheropen")
else:
userdata.grasp_categorisation = 'side'
sdh_handle=sss.move("sdh", "cylopen")
sdh_handle.wait()
rospy.sleep(2)
#Get the current arm joint states.
while self.arm_state.get_num_connections() == 0:
time.sleep(0.3)
continue
#Move to grasp position with SDH open ---------------------------------------------
#pregrasp
pre_grasp_stamped = PoseStamped();
pre_grasp_stamped.header.frame_id = "/base_link";
pre_grasp_stamped.pose = userdata.grasp_configuration[grasp_configuration_id].pre_grasp; #TODO: userdata.grasp_configuration_id
#grasp
grasp_stamped = PoseStamped();
grasp_stamped.header.frame_id = "/base_link";
grasp_stamped.pose = userdata.grasp_configuration[grasp_configuration_id].grasp; #TODO: userdata.grasp_configuration_id
#postgrasp
post_grasp_stamped = copy.deepcopy(grasp_stamped);
post_grasp_stamped.pose.position.z += 0.1;
sol = False
for w in range(0,10):
(pre_grasp_conf, error_code) = grasping_functions.callIKSolver(self.current_arm_state, pre_grasp_stamped)
if(error_code.val == error_code.SUCCESS):
for k in range(0,10):
(grasp_conf, error_code) = grasping_functions.callIKSolver(pre_grasp_conf, grasp_stamped)
if(error_code.val == error_code.SUCCESS):
(post_grasp_conf, error_code) = grasping_functions.callIKSolver(pre_grasp_conf, post_grasp_stamped)
if(error_code.val == error_code.SUCCESS):
sol = True
break
if sol:
break
if not sol:
sss.say(["I can not catch the object!"], False)
return 'not_completed';
else:
sss.say(["I am grasping the object now!"], False)
#Close SDH based on the grasp configuration to grasp.
arm_handle = sss.move("arm", [pre_grasp_conf, grasp_conf])
arm_handle.wait()
rospy.sleep(4)
#Close SDH based on the grasp configuration to grasp.
ssh_handle = sss.move("sdh", [list(userdata.grasp_configuration[grasp_configuration_id].sdh_joint_values)]) #TODO: userdata.grasp_configuration_id
ssh_handle.wait()
rospy.sleep(2);
#Confirm the grasp based on force feedback
successful_grasp = grasping_functions.sdh_tactil_sensor_result();#False
if not successful_grasp:
#Regrasp (close MORE the fingers)
regrasp = list(userdata.grasp_configuration[grasp_configuration_id].sdh_joint_values) #TODO: userdata.grasp_configuration_id
print "Current config, trying regrasp", regrasp
regrasp[1] += 0.07
regrasp[3] += 0.07
regrasp[5] += 0.07
print "to", regrasp
sdh_handle = sss.move("sdh", [regrasp])
sdh_handle.wait()
rospy.sleep(2)
successful_grasp = grasping_functions.sdh_tactil_sensor_result();#True
if not successful_grasp:
sss.say(["I can not catch the object.!"], False)
return 'not_completed'
arm_handle = sss.move("arm",[post_grasp_conf,"look_at_table","hold"])
arm_handle.wait()
rospy.sleep(2)
sss.say(["I have grasped the object with success!"], False)
return 'succeeded'
def execute_cb(self, server_goal):
x = time.time()
rospy.loginfo("/get_grasps_from_position_server has been called...")
obj_id = server_goal.object_id
obj = server_goal.object_pose
goal = GraspCGoal()
goal.object_id = obj_id
self.client.send_goal(goal)
self.client.wait_for_result(rospy.Duration.from_sec(5.0))
grasp_configuration = (self.client.get_result()).grasp_configuration
#current_joint_configuration
rospy.loginfo("Waiting /arm_controller/state...")
sub = rospy.Subscriber("/arm_controller/state",
JointTrajectoryControllerState,
self.get_joint_state)
while sub.get_num_connections() == 0:
time.sleep(0.3)
continue
rospy.loginfo("/arm_controller/state has finished.")
rotacion = grasping_functions.rotation_matrix(obj)
server_result = GraspFActionResult().result
server_result.feasible_grasp_available = False
server_result.grasp_configuration = []
for i in range(0, len(grasp_configuration)):
pre_trans = rotacion * grasping_functions.matrix_from_pose(
grasp_configuration[i].pre_grasp.pose)
grasp_trans = rotacion * grasping_functions.matrix_from_pose(
grasp_configuration[i].grasp.pose)
t = translation_from_matrix(pre_trans)
q = quaternion_from_matrix(pre_trans)
tg = translation_from_matrix(grasp_trans)
qg = quaternion_from_matrix(grasp_trans)
pre = PoseStamped()
pre.header.stamp = rospy.Time.now()
pre.header.frame_id = "/base_link"
pre.pose.position.x = t[0]
pre.pose.position.y = t[1]
pre.pose.position.z = t[2]
pre.pose.orientation.x = q[0]
pre.pose.orientation.y = q[1]
pre.pose.orientation.z = q[2]
pre.pose.orientation.w = q[3]
g = PoseStamped()
g.header.stamp = rospy.Time.now()
g.header.frame_id = "/base_link"
g.pose.position.x = tg[0]
g.pose.position.y = tg[1]
g.pose.position.z = tg[2]
g.pose.orientation.x = qg[0]
g.pose.orientation.y = qg[1]
g.pose.orientation.z = qg[2]
g.pose.orientation.w = qg[3]
offset_x = 0 #(g.pose.position.x - pre.pose.position.x)/3
offset_y = 0 #(g.pose.position.y - pre.pose.position.y)/3
offset_z = 0 #(g.pose.position.z - pre.pose.position.z)/3
pre.pose.position.x += offset_x
pre.pose.position.y += offset_y
pre.pose.position.z -= offset_z
g.pose.position.x += offset_x
g.pose.position.y += offset_y
g.pose.position.z -= offset_z
sol = False
for w in range(0, self.ik_loop_reply):
(pre_grasp_conf, error_code) = grasping_functions.callIKSolver(
current_joint_configuration, pre)
if (error_code.val == error_code.SUCCESS):
for k in range(0, self.ik_loop_reply):
(grasp_conf,
error_code) = grasping_functions.callIKSolver(
pre_grasp_conf, g)
if (error_code.val == error_code.SUCCESS):
new_valid_grasp = GraspSubConfiguration()
new_valid_grasp.sdh_joint_values = grasp_configuration[
i].sdh_joint_values
new_valid_grasp.grasp = g.pose
new_valid_grasp.pre_grasp = pre.pose
new_valid_grasp.category = grasping_functions.get_grasp_category(
pre.pose.position, g.pose.position)
sol = grasping_functions.valid_grasp(
new_valid_grasp)
if sol:
server_result.feasible_grasp_available = True
server_result.grasp_configuration.append(
new_valid_grasp)
break
if sol:
break
#for
#while
#order grasps
if len(server_result.grasp_configuration) > 0:
(server_result.grasp_configuration).sort()
rospy.loginfo(
str(len(server_result.grasp_configuration)) +
" valid grasps for this pose.")
rospy.loginfo("/get_grasps_from_position call has finished.")
print "Time employed: " + str(time.time() - x)
print "---------------------------------------"
self.get_grasps_from_position_server.set_succeeded(server_result)
