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 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 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_pregrasps(self, req):
x = time.time()
rospy.loginfo("/get_pregrasps service has been called...")
obj_id = req.object_id
obj_pose = req.object_pose
num_configurations = req.num_configurations
req = GetGraspConfigurationsRequest()
req.object_id = obj_id
grasp_configuration = (self.client(req)).grasp_configuration
rotacion = grasping_functions.rotation_matrix(obj_pose)
resp = GetPreGraspResponse()
resp.side = []
resp.mside = []
resp.top = []
resp.front = []
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 = Pose()
pre.position.x = t[0]
pre.position.y = t[1]
pre.position.z = t[2]
pre.orientation.x = q[0]
pre.orientation.y = q[1]
pre.orientation.z = q[2]
pre.orientation.w = q[3]
g = Pose()
g.position.x = tg[0]
g.position.y = tg[1]
g.position.z = tg[2]
g.orientation.x = qg[0]
g.orientation.y = qg[1]
g.orientation.z = qg[2]
g.orientation.w = qg[3]
aux = GraspConfiguration()
aux.object_id = obj_id
aux.hand_type = "SDH"
aux.sdh_joint_values = grasp_configuration[i].sdh_joint_values
aux.target_link = "/sdh_palm_link"
aux.pre_grasp.pose = pre
aux.grasp.pose = g
aux.category = grasping_functions.get_grasp_category(
pre.position, g.position)
if aux.category == "TOP":
if len(resp.top) < num_configurations:
resp.top.append(aux)
elif aux.category == "FRONT":
if len(resp.front) < num_configurations:
resp.front.append(aux)
elif aux.category == "SIDE":
if len(resp.side) < num_configurations:
resp.side.append(aux)
elif aux.category == "-SIDE":
if len(resp.mside) < num_configurations:
resp.mside.append(aux)
else:
continue
if len(resp.top) == num_configurations and len(
resp.side) == num_configurations and len(
resp.mside) == num_configurations and len(
resp.front) == num_configurations:
break
rospy.loginfo("/get_pregrasps call has finished.")
print "Time employed: " + str(time.time() - x)
print "---------------------------------------"
return resp
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 get_pregrasps(self, req):
x = time.time();
rospy.loginfo("/get_pregrasps service has been called...");
obj_id = req.object_id;
obj_pose = req.object_pose;
num_configurations = req.num_configurations;
req = GetGraspConfigurationsRequest();
req.object_id = obj_id;
grasp_configuration = (self.client(req)).grasp_configuration;
rotacion = grasping_functions.rotation_matrix(obj_pose);
resp = GetPreGraspResponse();
resp.side = []
resp.mside = []
resp.top = []
resp.front = []
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 = Pose();
pre.position.x = t[0];
pre.position.y = t[1];
pre.position.z = t[2];
pre.orientation.x = q[0];
pre.orientation.y = q[1];
pre.orientation.z = q[2];
pre.orientation.w = q[3];
g = Pose();
g.position.x = tg[0];
g.position.y = tg[1];
g.position.z = tg[2];
g.orientation.x = qg[0];
g.orientation.y = qg[1];
g.orientation.z = qg[2];
g.orientation.w = qg[3];
aux = GraspConfiguration();
aux.object_id = obj_id;
aux.hand_type = "SDH";
aux.sdh_joint_values = grasp_configuration[i].sdh_joint_values;
aux.target_link = "/sdh_palm_link";
aux.pre_grasp.pose = pre;
aux.grasp.pose = g;
aux.category = grasping_functions.get_grasp_category(pre.position, g.position);
if aux.category == "TOP":
if len(resp.top) < num_configurations:
resp.top.append(aux);
elif aux.category == "FRONT":
if len(resp.front) < num_configurations:
resp.front.append(aux);
elif aux.category == "SIDE":
if len(resp.side) < num_configurations:
resp.side.append(aux);
elif aux.category == "-SIDE":
if len(resp.mside) < num_configurations:
resp.mside.append(aux);
else:
continue
if len(resp.top)==num_configurations and len(resp.side)==num_configurations and len(resp.mside)==num_configurations and len(resp.front)==num_configurations:
break;
rospy.loginfo("/get_pregrasps call has finished.");
print "Time employed: " + str(time.time() - x);
print "---------------------------------------";
return resp;
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)