def execute_linear_move(self, goal):
rospy.loginfo("[epc_move_actionlib] Execute linear move.")
result = EPCLinearMoveResult()
start_pose_stamped = self.tf_listener.transformPose(
"torso_lift_link", goal.start_pose)
start_pose = util.pose_msg_to_mat(start_pose_stamped)
end_pose_stamped = self.tf_listener.transformPose(
"torso_lift_link", goal.end_pose)
end_pose = util.pose_msg_to_mat(end_pose_stamped)
if goal.reset_controllers:
self.epc_move.reset_controllers()
def preempt_callback(err_pos, err_ang):
if self.linear_move_act.is_preempt_requested():
return "preempted"
return None
result.result = self.epc_move.linear_move(
start_pose, end_pose, goal.tool_frame, goal.velocity,
goal.err_pos_max, goal.err_ang_max, goal.setup_steps,
preempt_callback)
rospy.loginfo(
"[epc_move_actionlib] Linear move completed with result '%s'." %
result.result)
if result.result == "succeeded":
self.linear_move_act.set_succeeded(result)
elif result.result == "preempted":
self.linear_move_act.set_preempted(result)
else:
self.linear_move_act.set_aborted(result)
def make_approach_pose(ud):
frame_B_head = util.pose_msg_to_mat(ud.head_click_pose)
base_B_frame = self.get_transform("base_link", ud.head_click_pose.header.frame_id)
if base_B_frame is None:
return 'tf_failure'
base_B_head = base_B_frame * frame_B_head
norm_z = np.array([base_B_head[0,2], base_B_head[1,2], 0])
norm_z /= np.linalg.norm(norm_z)
base_B_head[:3,:3] = np.mat([[-norm_z[0], norm_z[1], 0],
[-norm_z[1],-norm_z[0], 0],
[ 0, 0, 1]])
# offset in the x-direction by the given parameter
offset_B_base = self.get_transform(self.base_offset_frame, "base_link")
if offset_B_base is None:
return 'tf_failure'
nav_pose = base_B_head * offset_B_base
print nav_pose
nav_pose[:4,3] = nav_pose * np.mat([[-self.nav_approach_dist, 0, 0, 1]]).T
print nav_pose
now = rospy.Time.now()
nav_pose_ps = util.pose_mat_to_stamped_msg('base_link', nav_pose, now)
self.tf_listener.waitForTransform("/map", "base_link",
now, timeout=rospy.Duration(20))
nav_pose_map_ps = self.tf_listener.transformPose("/map", nav_pose_ps)
#self.nav_pub.publish(util.pose_mat_to_stamped_msg('base_link', base_B_head, rospy.Time.now()))
print base_B_head, base_B_head.T * base_B_head
self.nav_pub.publish(nav_pose_map_ps)
ud.nav_pose_ps = nav_pose_map_ps
return 'succeeded'
def execute_setup(self, goal):
rospy.loginfo(
"[linear_move_relative] Execute relative arm movement setup.")
result = LinearMoveRelativeSetupResult()
self.lin_move.reset_controllers()
# find setup poses
torso_B_frame = self.lin_move.get_transform(
"torso_lift_link", goal.goal_pose.header.frame_id)
frame_B_goal = util.pose_msg_to_mat(goal.goal_pose)
appr_B_tool = self.lin_move.get_transform(self.tool_approach_frame,
self.tool_frame)
torso_B_tool_appr = torso_B_frame * frame_B_goal * appr_B_tool
appr_B_wrist = self.lin_move.get_transform(
self.tool_approach_frame, self.arm + "_wrist_roll_link")
torso_B_wrist = torso_B_frame * frame_B_goal * appr_B_wrist
# wait for arm to stop moving
self.wait_arm_moving(stop_wait=True)
# start collision detection
self.start_detection(joint_detect=True,
finger_detect=True,
force_detect=False,
joint_behavior="overhead_grasp",
joint_sig_level=0.99)
# move to best estimate for IK for the wrist
angles = self.lin_move.biased_IK(torso_B_wrist,
self.INIT_ANGS,
self.JOINTS_BIAS,
num_iters=6)
if angles is None:
self.move_arm_setup_act.set_aborted(result)
return
self.lin_move.command_joint_angles(angles, 3.0)
# wait for the arm to start moving, then stop moving
self.wait_arm_moving(stop_wait=False)
rospy.sleep(1.0)
self.wait_arm_moving(stop_wait=True)
# use the epc control to accruately move to the setup position
move_success = self.lin_move.move_to_pose(torso_B_tool_appr)
# wait for the arm to start moving, then stop moving
self.wait_arm_moving(stop_wait=False)
rospy.sleep(1.0)
self.wait_arm_moving(stop_wait=True)
self.stop_detection()
# return result message
result.collided = self.coll_state.collided
self.move_arm_setup_act.set_succeeded(result)
self.coll_state.collided = False
rospy.loginfo(
"[linear_move_relative] Relative arm movement setup complete.")
return True
def execute_direct_move(self, goal):
rospy.loginfo("[epc_move_actionlib] Execute direct move.")
result = EPCDirectMoveResult()
transformed_pose_stamped = self.tf_listener.transformPose("torso_lift_link", goal.target_pose)
transformed_pose = util.pose_msg_to_mat(transformed_pose_stamped)
if goal.reset_controllers:
self.epc_move.reset_controllers()
def preempt_callback(err_pos, err_ang):
if self.direct_move_act.is_preempt_requested():
return "preempted"
return None
result.result = self.epc_move.direct_move(
transformed_pose,
goal.tool_frame,
goal.err_pos_goal,
goal.err_ang_goal,
goal.err_pos_max,
goal.err_ang_max,
goal.steady_steps,
preempt_callback,
)
rospy.loginfo("[epc_move_actionlib] Direct move completed with result '%s'." % result.result)
if result.result == "succeeded":
self.direct_move_act.set_succeeded(result)
elif result.result == "preempted":
self.direct_move_act.set_preempted(result)
else:
self.direct_move_act.set_aborted(result)
def execute_move(self, goal):
rospy.loginfo("[linear_move_relative] Execute relative arm movement.")
result = LinearMoveRelativeResult()
torso_B_frame = self.lin_move.get_transform("torso_lift_link",
goal.goal_pose.header.frame_id)
frame_B_goal = util.pose_msg_to_mat(goal.goal_pose)
appr_B_tool = self.lin_move.get_transform(self.tool_approach_frame, self.tool_frame)
torso_B_tool_appr = torso_B_frame * frame_B_goal * appr_B_tool
torso_B_tool_goal = torso_B_frame * frame_B_goal
self.wait_arm_moving(stop_wait=True)
move_success = self.lin_move.linear_trajectory(torso_B_tool_appr, torso_B_tool_goal,
velocity=self.MOVE_VELOCITY)
self.move_arm_act.set_succeeded(result)
return
if not move_success:
rospy.loginfo("[linear_move_relative] Relative arm movement failed.")
self.move_arm_act.set_aborted(move_success)
return
self.wait_arm_moving(stop_wait=False)
self.start_detection("overhead_grasp", 1.0)
rospy.sleep(2)
self.wait_arm_moving(stop_wait=True)
self.stop_detection()
rospy.loginfo("[linear_move_relative] Relative arm movement complete.")
result.collided = self.coll_state.collided
self.move_arm_act.set_succeeded(result)
self.coll_state.collided = False
def execute_direct_move(self, goal):
rospy.loginfo("[epc_move_actionlib] Execute direct move.")
result = EPCDirectMoveResult()
transformed_pose_stamped = self.tf_listener.transformPose(
"torso_lift_link", goal.target_pose)
transformed_pose = util.pose_msg_to_mat(transformed_pose_stamped)
if goal.reset_controllers:
self.epc_move.reset_controllers()
def preempt_callback(err_pos, err_ang):
if self.direct_move_act.is_preempt_requested():
return "preempted"
return None
result.result = self.epc_move.direct_move(
transformed_pose, goal.tool_frame, goal.err_pos_goal,
goal.err_ang_goal, goal.err_pos_max, goal.err_ang_max,
goal.steady_steps, preempt_callback)
rospy.loginfo(
"[epc_move_actionlib] Direct move completed with result '%s'." %
result.result)
if result.result == "succeeded":
self.direct_move_act.set_succeeded(result)
elif result.result == "preempted":
self.direct_move_act.set_preempted(result)
else:
self.direct_move_act.set_aborted(result)
def process_touch_pose(ud):
#########################################################################
# tranformation logic for manipulation
# put touch pose in torso frame
frame_B_touch = util.pose_msg_to_mat(ud.touch_click_pose)
torso_B_frame = self.get_transform("torso_lift_link",
ud.touch_click_pose.header.frame_id)
if torso_B_frame is None:
return 'tf_failure'
torso_B_touch_bad = torso_B_frame * frame_B_touch
# rotate pixel23d the right way
t_pos, t_quat = util.pose_mat_to_pq(torso_B_touch_bad)
# rotate so x axis pointing out
quat_flip_rot = tf_trans.quaternion_from_euler(0.0, np.pi/2.0, 0.0)
quat_flipped = tf_trans.quaternion_multiply(t_quat, quat_flip_rot)
# rotate around x axis so the y axis is flat
mat_flipped = tf_trans.quaternion_matrix(quat_flipped)
rot_angle = np.arctan(-mat_flipped[2,1] / mat_flipped[2,2])
quat_ortho_rot = tf_trans.quaternion_from_euler(rot_angle + np.pi, 0.0, 0.0)
quat_flipped_ortho = tf_trans.quaternion_multiply(quat_flipped, quat_ortho_rot)
torso_B_touch = util.pose_pq_to_mat(t_pos, quat_flipped_ortho)
# offset the touch location by the approach tranform
appr_B_tool = self.get_transform(self.tool_approach_frame, self.tool_frame)
if appr_B_tool is None:
return 'tf_failure'
torso_B_touch_appr = torso_B_touch * appr_B_tool
# project the approach back into the wrist
appr_B_wrist = self.get_transform(self.tool_frame,
self.arm + "_wrist_roll_link")
if appr_B_wrist is None:
return 'tf_failure'
torso_B_wrist = torso_B_touch_appr * appr_B_wrist
#########################################################################
# create PoseStamped
appr_wrist_ps = util.pose_mat_to_stamped_msg('torso_lift_link',
torso_B_wrist)
appr_tool_ps = util.pose_mat_to_stamped_msg('torso_lift_link',
torso_B_touch_appr)
touch_tool_ps = util.pose_mat_to_stamped_msg('torso_lift_link',
torso_B_touch)
# visualization
self.wrist_pub.publish(appr_wrist_ps)
self.appr_pub.publish(appr_tool_ps)
self.touch_pub.publish(touch_tool_ps)
# set return values
ud.appr_wrist_mat = torso_B_wrist
ud.appr_tool_ps = appr_tool_ps
ud.touch_tool_ps = touch_tool_ps
return 'succeeded'
def execute_setup(self, goal):
rospy.loginfo("[linear_move_relative] Execute relative arm movement setup.")
result = LinearMoveRelativeSetupResult()
self.lin_move.reset_controllers()
# find setup poses
torso_B_frame = self.lin_move.get_transform("torso_lift_link",
goal.goal_pose.header.frame_id)
frame_B_goal = util.pose_msg_to_mat(goal.goal_pose)
appr_B_tool = self.lin_move.get_transform(self.tool_approach_frame, self.tool_frame)
torso_B_tool_appr = torso_B_frame * frame_B_goal * appr_B_tool
appr_B_wrist = self.lin_move.get_transform(self.tool_approach_frame,
self.arm + "_wrist_roll_link")
torso_B_wrist = torso_B_frame * frame_B_goal * appr_B_wrist
# wait for arm to stop moving
self.wait_arm_moving(stop_wait=True)
# start collision detection
self.start_detection(joint_detect=True, finger_detect=True, force_detect=False,
joint_behavior="overhead_grasp", joint_sig_level=0.99)
# move to best estimate for IK for the wrist
angles = self.lin_move.biased_IK(torso_B_wrist, self.INIT_ANGS,
self.JOINTS_BIAS, num_iters=6)
if angles is None:
self.move_arm_setup_act.set_aborted(result)
return
self.lin_move.command_joint_angles(angles, 3.0)
# wait for the arm to start moving, then stop moving
self.wait_arm_moving(stop_wait=False)
rospy.sleep(1.0)
self.wait_arm_moving(stop_wait=True)
# use the epc control to accruately move to the setup position
move_success = self.lin_move.move_to_pose(torso_B_tool_appr)
# wait for the arm to start moving, then stop moving
self.wait_arm_moving(stop_wait=False)
rospy.sleep(1.0)
self.wait_arm_moving(stop_wait=True)
self.stop_detection()
# return result message
result.collided = self.coll_state.collided
self.move_arm_setup_act.set_succeeded(result)
self.coll_state.collided = False
rospy.loginfo("[linear_move_relative] Relative arm movement setup complete.")
return True
def execute_linear_move(self, goal):
rospy.loginfo("[epc_move_actionlib] Execute linear move.")
result = EPCLinearMoveResult()
start_pose_stamped = self.tf_listener.transformPose("torso_lift_link", goal.start_pose)
start_pose = util.pose_msg_to_mat(start_pose_stamped)
end_pose_stamped = self.tf_listener.transformPose("torso_lift_link", goal.end_pose)
end_pose = util.pose_msg_to_mat(end_pose_stamped)
if goal.reset_controllers:
self.epc_move.reset_controllers()
def preempt_callback(err_pos, err_ang):
if self.linear_move_act.is_preempt_requested():
return "preempted"
return None
result.result = self.epc_move.linear_move(
start_pose,
end_pose,
goal.tool_frame,
goal.velocity,
goal.err_pos_max,
goal.err_ang_max,
goal.setup_steps,
preempt_callback,
)
rospy.loginfo("[epc_move_actionlib] Linear move completed with result '%s'." % result.result)
if result.result == "succeeded":
self.linear_move_act.set_succeeded(result)
elif result.result == "preempted":
self.linear_move_act.set_preempted(result)
else:
self.linear_move_act.set_aborted(result)
def execute_move(self, goal):
rospy.loginfo("[linear_move_relative] Execute relative arm movement.")
result = LinearMoveRelativeResult()
torso_B_frame = self.lin_move.get_transform(
"torso_lift_link", goal.goal_pose.header.frame_id)
frame_B_goal = util.pose_msg_to_mat(goal.goal_pose)
appr_B_tool = self.lin_move.get_transform(self.tool_approach_frame,
self.tool_frame)
torso_B_tool_appr = torso_B_frame * frame_B_goal * appr_B_tool
torso_B_tool_goal = torso_B_frame * frame_B_goal
self.wait_arm_moving(stop_wait=True)
move_success = self.lin_move.linear_trajectory(
torso_B_tool_appr, torso_B_tool_goal, velocity=self.MOVE_VELOCITY)
self.move_arm_act.set_succeeded(result)
return
if not move_success:
rospy.loginfo(
"[linear_move_relative] Relative arm movement failed.")
self.move_arm_act.set_aborted(move_success)
return
self.wait_arm_moving(stop_wait=False)
self.start_detection("overhead_grasp", 1.0)
rospy.sleep(2)
self.wait_arm_moving(stop_wait=True)
self.stop_detection()
rospy.loginfo("[linear_move_relative] Relative arm movement complete.")
result.collided = self.coll_state.collided
self.move_arm_act.set_succeeded(result)
self.coll_state.collided = False
