def retreat_move(self, height, velocity, forced=False):
if forced:
self.is_forced_retreat = True
cart_pos, cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal((cart_pos, cart_quat))
#print "Retreat EP:", ell_pos
latitude = ell_pos[0]
if self.trim_retreat:
latitude = min(latitude, TRIM_RETREAT_LATITUDE)
#rospy.loginfo("[face_adls_manager] Retreating from current location.")
retreat_pos = [latitude, ell_pos[1], height]
retreat_pos = self.ellipsoid.enforce_bounds(retreat_pos)
retreat_quat = [0,0,0,1]
if forced:
cart_path = self.ellipsoid.ellipsoidal_to_pose((retreat_pos, retreat_quat))
cart_msg = [PoseConv.to_pose_msg(cart_path)]
else:
ell_path = self.ellipsoid.create_ellipsoidal_path(ell_pos,
ell_quat,
retreat_pos,
retreat_quat,
velocity=0.01,
min_jerk=True)
cart_path = [self.ellipsoid.ellipsoidal_to_pose(ell_pose) for ell_pose in ell_path]
cart_msg = [PoseConv.to_pose_msg(pose) for pose in cart_path]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_msg)
self.pose_traj_pub.publish(pose_array)
self.is_forced_retreat = False
def create_base_marker(pose, id, color):
marker = Marker()
marker.header.frame_id = "base_link"
marker.header.stamp = rospy.Time.now()
marker.ns = "ar_servo"
marker.id = id
marker.pose = PoseConv.to_pose_msg(pose)
marker.color = ColorRGBA(*(color + (1.0,)))
marker.scale.x = 0.7; marker.scale.y = 0.7; marker.scale.z = 0.2
return marker
def stop_moving(self):
"""Send empty PoseArray to skin controller to stop movement"""
for x in range(2):
self.pose_traj_pub.publish(PoseArray()) #Send empty msg to skin controller
cart_traj_msg = [PoseConv.to_pose_msg(self.current_ee_pose(self.ee_frame))]
head = Header()
head.frame_id = '/torso_lift_link'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
rospy.loginfo( "Sent stop moving commands!")
def create_arrow_marker(pose, m_id, color=ColorRGBA(1., 0., 0., 1.)):
m = Marker()
m.header.frame_id = "/base_link"
m.header.stamp = rospy.Time.now()
m.ns = "ell_pose_viz"
m.id = m_id
m.type = Marker.ARROW
m.action = Marker.ADD
m.scale = Vector3(0.19, 0.09, 0.02)
m.color = color
m.pose = PoseConv.to_pose_msg(pose)
return m
def create_arrow_marker(pose, m_id, color=ColorRGBA(1., 0., 0., 1.)):
m = Marker()
m.header.frame_id = "/ellipse_frame"
m.header.stamp = rospy.Time.now()
m.ns = "ell_pose"
m.id = m_id
m.type = Marker.ARROW
m.action = Marker.ADD
m.scale = Vector3(0.19, 0.09, 0.02)
m.color = color
m.pose = PoseConv.to_pose_msg(pose)
return m
def retreat_move(self, height, velocity, forced=False):
if forced:
self.is_forced_retreat = True
cart_pos, cart_quat = self.current_ee_pose(self.ee_frame,
'/ellipse_frame')
ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal(
(cart_pos, cart_quat))
#print "Retreat EP:", ell_pos
latitude = ell_pos[0]
if self.trim_retreat:
latitude = min(latitude, TRIM_RETREAT_LATITUDE)
#rospy.loginfo("[face_adls_manager] Retreating from current location.")
retreat_pos = [latitude, ell_pos[1], height]
retreat_pos = self.ellipsoid.enforce_bounds(retreat_pos)
retreat_quat = [0, 0, 0, 1]
if forced:
cart_path = self.ellipsoid.ellipsoidal_to_pose(
(retreat_pos, retreat_quat))
cart_msg = [PoseConv.to_pose_msg(cart_path)]
else:
ell_path = self.ellipsoid.create_ellipsoidal_path(ell_pos,
ell_quat,
retreat_pos,
retreat_quat,
velocity=0.01,
min_jerk=True)
cart_path = [
self.ellipsoid.ellipsoidal_to_pose(ell_pose)
for ell_pose in ell_path
]
cart_msg = [PoseConv.to_pose_msg(pose) for pose in cart_path]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_msg)
self.pose_traj_pub.publish(pose_array)
self.is_forced_retreat = False
def create_ear_marker(self, pose, m_id, color=ColorRGBA(0., 1., 1., 1.)):
m = Marker()
#m.header.frame_id = "/base_link"
m.header.frame_id = "/ellipse_frame"
m.header.stamp = rospy.Time.now()
m.ns = "head_markers"
m.id = m_id
m.type = Marker.CYLINDER
m.action = Marker.ADD
m.scale = ear_scale
m.color = color
m.pose = PoseConv.to_pose_msg(pose)
return m
def create_ear_marker(self, pose, m_id, color=ColorRGBA(0., 1., 1., 1.)):
m = Marker()
#m.header.frame_id = "/base_link"
m.header.frame_id = "/ellipse_frame"
m.header.stamp = rospy.Time.now()
m.ns = "head_markers"
m.id = m_id
m.type = Marker.CYLINDER
m.action = Marker.ADD
m.scale = ear_scale
m.color = color
m.pose = PoseConv.to_pose_msg(pose)
return m
def stop_moving(self):
"""Send empty PoseArray to skin controller to stop movement"""
for x in range(2):
self.pose_traj_pub.publish(
PoseArray()) #Send empty msg to skin controller
cart_traj_msg = [
PoseConv.to_pose_msg(self.current_ee_pose(self.ee_frame))
]
head = Header()
head.frame_id = '/torso_lift_link'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
rospy.loginfo("Sent stop moving commands!")
def joint_states_cb(self, msg):
state_msg = EndpointState()
# Forward Position Kinematics
q, qd, eff = self.kinematics.extract_joint_state(msg)
T06 = self.kinematics.forward(q)
state_msg.pose = PoseConv.to_pose_msg(T06)
# Forward Velocity Kinematics
end_frame = PyKDL.FrameVel()
q_vel = PyKDL.JntArrayVel(joint_list_to_kdl(q), joint_list_to_kdl(qd))
self.fk_vel_solver.JntToCart(q_vel, end_frame)
state_msg.twist = TwistKDLToMsg(end_frame.GetTwist())
state_msg.header.frame_id = self.frame_id
state_msg.header.stamp = rospy.Time.now()
try:
self.state_pub.publish(state_msg)
except:
pass
def local_input_cb(self, msg):
self.force_monitor.update_activity()
if self.is_forced_retreat:
return
self.stop_moving()
button_press = msg.data
curr_cart_pos, curr_cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal((curr_cart_pos, curr_cart_quat))
if button_press in ell_trans_params:
self.publish_feedback(Messages.LOCAL_START % button_names_dict[button_press])
change_trans_ep = ell_trans_params[button_press]
goal_ell_pos = [curr_ell_pos[0]+change_trans_ep[0],
curr_ell_pos[1]+change_trans_ep[1],
curr_ell_pos[2]+change_trans_ep[2]]
ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
goal_ell_pos, curr_ell_quat,
velocity=ELL_LOCAL_VEL, min_jerk=True)
elif button_press in ell_rot_params:
self.publish_feedback(Messages.LOCAL_START % button_names_dict[button_press])
change_rot_ep = ell_rot_params[button_press]
rot_quat = trans.quaternion_from_euler(*change_rot_ep)
goal_ell_quat = trans.quaternion_multiply(curr_ell_quat, rot_quat)
ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
curr_ell_pos, goal_ell_quat,
velocity=ELL_ROT_VEL, min_jerk=True)
elif button_press == "reset_rotation":
self.publish_feedback(Messages.ROT_RESET_START)
ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
curr_ell_pos,(0.,0.,0.,1.),
velocity=ELL_ROT_VEL, min_jerk=True)
else:
rospy.logerr("[face_adls_manager] Unknown local ellipsoidal command")
cart_traj = [self.ellipsoid.ellipsoidal_to_pose(pose) for pose in ell_path]
cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in cart_traj]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
self.force_monitor.update_activity()
def global_input_cb(self, msg):
self.force_monitor.update_activity()
if self.is_forced_retreat:
return
rospy.loginfo("[face_adls_manager] Performing global move.")
if type(msg) == String:
self.publish_feedback(Messages.GLOBAL_START %msg.data)
goal_ell_pose = self.global_poses[msg.data]
elif type(msg) == PoseStamped:
try:
self.tfl.waitForTransform(msg.header.frame_id, '/ellipse_frame', msg.header.stamp, rospy.Duration(8.0))
goal_cart_pose = self.tfl.transformPose('/ellipse_frame', msg)
goal_cart_pos, goal_cart_quat = PoseConv.to_pos_quat(goal_cart_pose)
flip_quat = trans.quaternion_from_euler(-np.pi/2, np.pi, 0)
goal_cart_quat_flipped = trans.quaternion_multiply(goal_cart_quat, flip_quat)
goal_cart_pose = PoseConv.to_pose_stamped_msg('/ellipse_frame', (goal_cart_pos, goal_cart_quat_flipped))
self.publish_feedback('Moving around ellipse to clicked position).')
goal_ell_pose = self.ellipsoid.pose_to_ellipsoidal(goal_cart_pose)
except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
rospy.logwarn("[face_adls_manager] TF Failure getting clicked position in ellipse_frame:\r\n %s" %e)
return
curr_cart_pos, curr_cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal((curr_cart_pos, curr_cart_quat))
retreat_ell_pos = [curr_ell_pos[0], curr_ell_pos[1], RETREAT_HEIGHT]
retreat_ell_quat = trans.quaternion_multiply(self.gripper_rot, [1.,0.,0.,0.])
approach_ell_pos = [goal_ell_pose[0][0], goal_ell_pose[0][1], RETREAT_HEIGHT]
approach_ell_quat = trans.quaternion_multiply(self.gripper_rot, goal_ell_pose[1])
final_ell_pos = [goal_ell_pose[0][0], goal_ell_pose[0][1], goal_ell_pose[0][2] - HEIGHT_CLOSER_ADJUST]
final_ell_quat = trans.quaternion_multiply(self.gripper_rot, goal_ell_pose[1])
cart_ret_pose = self.ellipsoid.ellipsoidal_to_pose((retreat_ell_pos, retreat_ell_quat))
cart_ret_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_ret_pose)
cart_app_pose = self.ellipsoid.ellipsoidal_to_pose((approach_ell_pos, approach_ell_quat))
cart_app_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_app_pose)
cart_goal_pose = self.ellipsoid.ellipsoidal_to_pose((final_ell_pos, final_ell_quat))
cart_goal_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_goal_pose)
retreat_ell_traj = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
retreat_ell_pos, retreat_ell_quat,
velocity=0.01, min_jerk=True)
transition_ell_traj = self.ellipsoid.create_ellipsoidal_path(retreat_ell_pos, retreat_ell_quat,
approach_ell_pos, approach_ell_quat,
velocity=0.01, min_jerk=True)
approach_ell_traj = self.ellipsoid.create_ellipsoidal_path(approach_ell_pos, approach_ell_quat,
final_ell_pos, final_ell_quat,
velocity=0.01, min_jerk=True)
full_ell_traj = retreat_ell_traj + transition_ell_traj + approach_ell_traj
full_cart_traj = [self.ellipsoid.ellipsoidal_to_pose(pose) for pose in full_ell_traj]
cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in full_cart_traj]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
ps = PoseStamped()
ps.header = head
ps.pose = cart_traj_msg[0]
self.force_monitor.update_activity()
def local_input_cb(self, msg):
self.force_monitor.update_activity()
if self.is_forced_retreat:
return
self.stop_moving()
button_press = msg.data
curr_cart_pos, curr_cart_quat = self.current_ee_pose(
self.ee_frame, '/ellipse_frame')
curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal(
(curr_cart_pos, curr_cart_quat))
if button_press in ell_trans_params:
self.publish_feedback(Messages.LOCAL_START %
button_names_dict[button_press])
change_trans_ep = ell_trans_params[button_press]
goal_ell_pos = [
curr_ell_pos[0] + change_trans_ep[0],
curr_ell_pos[1] + change_trans_ep[1],
curr_ell_pos[2] + change_trans_ep[2]
]
ell_path = self.ellipsoid.create_ellipsoidal_path(
curr_ell_pos,
curr_ell_quat,
goal_ell_pos,
curr_ell_quat,
velocity=ELL_LOCAL_VEL,
min_jerk=True)
elif button_press in ell_rot_params:
self.publish_feedback(Messages.LOCAL_START %
button_names_dict[button_press])
change_rot_ep = ell_rot_params[button_press]
rot_quat = trans.quaternion_from_euler(*change_rot_ep)
goal_ell_quat = trans.quaternion_multiply(curr_ell_quat, rot_quat)
ell_path = self.ellipsoid.create_ellipsoidal_path(
curr_ell_pos,
curr_ell_quat,
curr_ell_pos,
goal_ell_quat,
velocity=ELL_ROT_VEL,
min_jerk=True)
elif button_press == "reset_rotation":
self.publish_feedback(Messages.ROT_RESET_START)
ell_path = self.ellipsoid.create_ellipsoidal_path(
curr_ell_pos,
curr_ell_quat,
curr_ell_pos, (0., 0., 0., 1.),
velocity=ELL_ROT_VEL,
min_jerk=True)
else:
rospy.logerr(
"[face_adls_manager] Unknown local ellipsoidal command")
cart_traj = [
self.ellipsoid.ellipsoidal_to_pose(pose) for pose in ell_path
]
cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in cart_traj]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
self.force_monitor.update_activity()
def global_input_cb(self, msg):
self.force_monitor.update_activity()
if self.is_forced_retreat:
return
rospy.loginfo("[face_adls_manager] Performing global move.")
if type(msg) == String:
self.publish_feedback(Messages.GLOBAL_START % msg.data)
goal_ell_pose = self.global_poses[msg.data]
elif type(msg) == PoseStamped:
try:
self.tfl.waitForTransform(msg.header.frame_id,
'/ellipse_frame', msg.header.stamp,
rospy.Duration(8.0))
goal_cart_pose = self.tfl.transformPose('/ellipse_frame', msg)
goal_cart_pos, goal_cart_quat = PoseConv.to_pos_quat(
goal_cart_pose)
flip_quat = trans.quaternion_from_euler(-np.pi / 2, np.pi, 0)
goal_cart_quat_flipped = trans.quaternion_multiply(
goal_cart_quat, flip_quat)
goal_cart_pose = PoseConv.to_pose_stamped_msg(
'/ellipse_frame', (goal_cart_pos, goal_cart_quat_flipped))
self.publish_feedback(
'Moving around ellipse to clicked position).')
goal_ell_pose = self.ellipsoid.pose_to_ellipsoidal(
goal_cart_pose)
except (LookupException, ConnectivityException,
ExtrapolationException, Exception) as e:
rospy.logwarn(
"[face_adls_manager] TF Failure getting clicked position in ellipse_frame:\r\n %s"
% e)
return
curr_cart_pos, curr_cart_quat = self.current_ee_pose(
self.ee_frame, '/ellipse_frame')
curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal(
(curr_cart_pos, curr_cart_quat))
retreat_ell_pos = [curr_ell_pos[0], curr_ell_pos[1], RETREAT_HEIGHT]
retreat_ell_quat = trans.quaternion_multiply(self.gripper_rot,
[1., 0., 0., 0.])
approach_ell_pos = [
goal_ell_pose[0][0], goal_ell_pose[0][1], RETREAT_HEIGHT
]
approach_ell_quat = trans.quaternion_multiply(self.gripper_rot,
goal_ell_pose[1])
final_ell_pos = [
goal_ell_pose[0][0], goal_ell_pose[0][1],
goal_ell_pose[0][2] - HEIGHT_CLOSER_ADJUST
]
final_ell_quat = trans.quaternion_multiply(self.gripper_rot,
goal_ell_pose[1])
cart_ret_pose = self.ellipsoid.ellipsoidal_to_pose(
(retreat_ell_pos, retreat_ell_quat))
cart_ret_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',
cart_ret_pose)
cart_app_pose = self.ellipsoid.ellipsoidal_to_pose(
(approach_ell_pos, approach_ell_quat))
cart_app_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',
cart_app_pose)
cart_goal_pose = self.ellipsoid.ellipsoidal_to_pose(
(final_ell_pos, final_ell_quat))
cart_goal_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',
cart_goal_pose)
retreat_ell_traj = self.ellipsoid.create_ellipsoidal_path(
curr_ell_pos,
curr_ell_quat,
retreat_ell_pos,
retreat_ell_quat,
velocity=0.01,
min_jerk=True)
transition_ell_traj = self.ellipsoid.create_ellipsoidal_path(
retreat_ell_pos,
retreat_ell_quat,
approach_ell_pos,
approach_ell_quat,
velocity=0.01,
min_jerk=True)
approach_ell_traj = self.ellipsoid.create_ellipsoidal_path(
approach_ell_pos,
approach_ell_quat,
final_ell_pos,
final_ell_quat,
velocity=0.01,
min_jerk=True)
full_ell_traj = retreat_ell_traj + transition_ell_traj + approach_ell_traj
full_cart_traj = [
self.ellipsoid.ellipsoidal_to_pose(pose) for pose in full_ell_traj
]
cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in full_cart_traj]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
ps = PoseStamped()
ps.header = head
ps.pose = cart_traj_msg[0]
self.force_monitor.update_activity()
#!/usr/bin/python
import numpy as np
import roslib
roslib.load_manifest("hrl_geom")
import rospy
from hrl_geom.pose_converter import PoseConv
if __name__ == "__main__":
rospy.init_node("test_poseconv")
homo_mat = PoseConv.to_homo_mat([0., 1., 2.], [0., 0., np.pi/2])
pose_msg = PoseConv.to_pose_msg(homo_mat)
pos, quat = PoseConv.to_pos_quat(pose_msg)
pose_stamped_msg = PoseConv.to_pose_stamped_msg("/base_link", pos, quat)
pose_stamped_msg2 = PoseConv.to_pose_stamped_msg("/base_link", [pos, quat])
tf_stamped = PoseConv.to_tf_stamped_msg("/new_link", pose_stamped_msg)
p, R = PoseConv.to_pos_rot("/new_link", tf_stamped)
for name in ["homo_mat", "pose_msg", "pos", "quat", "pose_stamped_msg",
"pose_stamped_msg2", "tf_stamped", "p", "R"]:
print "%s: \n" % name, locals()[name]
