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 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]