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_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()
