def main(): rospy.init_node("teleop_positioner") from optparse import OptionParser p = OptionParser() p.add_option('-r', '--rate', dest="rate", default=10, help="Set rate.") (opts, args) = p.parse_args() arm = create_pr2_arm('l', PR2ArmHybridForce) rospy.sleep(0.1) arm.zero_sensor() cur_pose = arm.get_end_effector_pose() arm.set_ep(cur_pose, 1) arm.set_force_directions([]) arm.set_force_gains(p_trans=[3, 1, 1], p_rot=0.5, i_trans=[0.002, 0.001, 0.001], i_max_trans=[10, 5, 5], i_rot=0, i_max_rot=0) arm.set_motion_gains(p_trans=400, d_trans=[16, 10, 10], p_rot=[10, 10, 10], d_rot=0) arm.set_tip_frame("/l_gripper_tool_frame") arm.update_gains() arm.set_force(6 * [0]) r = rospy.Rate(float(opts.rate)) while not rospy.is_shutdown(): ep_pose = arm.get_ep() cur_pose = arm.get_end_effector_pose() err_ep = arm.ep_error(cur_pose, ep_pose) if np.linalg.norm(err_ep[0:3]) > 0.012 or np.linalg.norm( err_ep[3:]) > np.pi / 8.: arm.set_ep(cur_pose, 1) r.sleep() cur_pose = arm.get_end_effector_pose() arm.set_ep(cur_pose, 1) q = arm.get_joint_angles() q_posture = q.tolist()[0:3] + 4 * [9999] arm.set_posture(q_posture) arm.set_motion_gains(p_trans=400, d_trans=[16, 10, 10], p_rot=[20, 50, 50], d_rot=0) arm.update_gains() print PoseConverter.to_pos_quat(cur_pose) pkg_dir = roslib.rospack.rospackexec(["find", "hrl_phri_2011"]) bag = rosbag.Bag(pkg_dir + "/data/saved_teleop_pose.bag", 'w') bag.write("/teleop_pose", PoseConverter.to_pose_msg(cur_pose)) q_posture_msg = Float64MultiArray() q_posture_msg.data = q_posture bag.write("/teleop_posture", q_posture_msg) bag.close()
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 = PoseConverter.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 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 = PoseConverter.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 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 = PoseConverter.to_pose_msg(pose) return m
def create_arrow_marker(pose, m_id, color=ColorRGBA(1.0, 0.0, 0.0, 1.0)): m = Marker() if USE_ELLIPSE_FRAME: m.header.frame_id = "/ellipse_frame" else: m.header.frame_id = "/base_link" 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 = PoseConverter.to_pose_msg(pose) return m
def create_arrow_marker(pose, m_id, color=ColorRGBA(1., 0., 0., 1.)): m = Marker() if USE_ELLIPSE_FRAME: m.header.frame_id = "/ellipse_frame" else: m.header.frame_id = "/base_link" 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 = PoseConverter.to_pose_msg(pose) return m
def main(): rospy.init_node("teleop_positioner") from optparse import OptionParser p = OptionParser() p.add_option('-r', '--rate', dest="rate", default=10, help="Set rate.") (opts, args) = p.parse_args() arm = create_pr2_arm('l', PR2ArmHybridForce) rospy.sleep(0.1) arm.zero_sensor() cur_pose = arm.get_end_effector_pose() arm.set_ep(cur_pose, 1) arm.set_force_directions([]) arm.set_force_gains(p_trans=[3, 1, 1], p_rot=0.5, i_trans=[0.002, 0.001, 0.001], i_max_trans=[10, 5, 5], i_rot=0, i_max_rot=0) arm.set_motion_gains(p_trans=400, d_trans=[16, 10, 10], p_rot=[10, 10, 10], d_rot=0) arm.set_tip_frame("/l_gripper_tool_frame") arm.update_gains() arm.set_force(6 * [0]) r = rospy.Rate(float(opts.rate)) while not rospy.is_shutdown(): ep_pose = arm.get_ep() cur_pose = arm.get_end_effector_pose() err_ep = arm.ep_error(cur_pose, ep_pose) if np.linalg.norm(err_ep[0:3]) > 0.012 or np.linalg.norm(err_ep[3:]) > np.pi / 8.: arm.set_ep(cur_pose, 1) r.sleep() cur_pose = arm.get_end_effector_pose() arm.set_ep(cur_pose, 1) q = arm.get_joint_angles() q_posture = q.tolist()[0:3] + 4 * [9999] arm.set_posture(q_posture) arm.set_motion_gains(p_trans=400, d_trans=[16, 10, 10], p_rot=[20, 50, 50], d_rot=0) arm.update_gains() print PoseConverter.to_pos_quat(cur_pose) pkg_dir = roslib.rospack.rospackexec(["find", "hrl_phri_2011"]) bag = rosbag.Bag(pkg_dir + "/data/saved_teleop_pose.bag", 'w') bag.write("/teleop_pose", PoseConverter.to_pose_msg(cur_pose)) q_posture_msg = Float64MultiArray() q_posture_msg.data = q_posture bag.write("/teleop_posture", q_posture_msg) bag.close()