def print_trajectory_stats(ep_traj, traj, t_vals):
diffs_pos = [
np.linalg.norm(traj[i + 1][0] - traj[i][0])
for i in range(len(traj) - 1)
]
diffs_rot = [
2 * np.arccos(
np.fabs(
np.dot(
PoseConverter.to_pos_quat(traj[i + 1])[1],
PoseConverter.to_pos_quat(traj[i])[1])))
for i in range(len(traj) - 1)
]
ptiles = [10, 25, 50, 75, 90, 100]
print "Diffs pos:", ", ".join([
"%d:%f" % (ptile, stats.scoreatpercentile(diffs_pos, ptile))
for ptile in ptiles
])
print "Diffs pos:", ", ".join([
"%d:%f" % (ptile, stats.scoreatpercentile(diffs_rot, ptile))
for ptile in ptiles
])
if max(diffs_pos) > 0.01:
print traj
print ep_traj
print t_vals
print "ind:", np.argmax(diffs_pos)
print ep_traj.T[np.argmax(diffs_pos)]
print traj[np.argmax(diffs_pos)]
def ellipsoidal_to_pose(self, lat, lon, height):
pos = self.ellipsoidal_to_cart(lat, lon, height)
df_du = self.partial_u(lat, lon, height)
nx, ny, nz = df_du.T.A[0] / np.linalg.norm(df_du)
j = np.sqrt(1. / (1. + ny * ny / (nz * nz)))
k = -ny * j / nz
norm_rot = np.mat([[-nx, ny * k - nz * j, 0], [-ny, -nx * k, j],
[-nz, nx * j, k]])
_, norm_quat = PoseConverter.to_pos_quat(np.mat([0, 0, 0]).T, norm_rot)
rot_angle = np.arctan(-norm_rot[2, 1] / norm_rot[2, 2])
#print norm_rot
quat_ortho_rot = tf_trans.quaternion_from_euler(
rot_angle + np.pi, 0.0, 0.0)
norm_quat_ortho = tf_trans.quaternion_multiply(norm_quat,
quat_ortho_rot)
norm_rot_ortho = np.mat(
tf_trans.quaternion_matrix(norm_quat_ortho)[:3, :3])
if norm_rot_ortho[2, 2] > 0:
flip_axis_ang = 0
else:
flip_axis_ang = np.pi
quat_flip = tf_trans.quaternion_from_euler(flip_axis_ang, 0.0, 0.0)
norm_quat_ortho_flipped = tf_trans.quaternion_multiply(
norm_quat_ortho, quat_flip)
return PoseConverter.to_pos_quat(pos, norm_quat_ortho_flipped)
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 main():
rospy.init_node("tf_link_flipper")
child_frame = rospy.get_param("~child_frame")
parent_frame = rospy.get_param("~parent_frame")
link_frame = rospy.get_param("~link_frame")
rate = rospy.get_param("~rate", 100)
link_trans = rospy.get_param("~link_transform")
l_B_c = PoseConverter.to_homo_mat(link_trans['pos'], link_trans['quat'])
tf_broad = tf.TransformBroadcaster()
tf_listener = tf.TransformListener()
rospy.sleep(1)
r = rospy.Rate(rate)
while not rospy.is_shutdown():
time = rospy.Time.now()
tf_listener.waitForTransform(child_frame, parent_frame, rospy.Time(0),
rospy.Duration(1))
pos, quat = tf_listener.lookupTransform(child_frame, parent_frame,
rospy.Time(0))
c_B_p = PoseConverter.to_homo_mat(pos, quat)
l_B_p = l_B_c * c_B_p
tf_pos, tf_quat = PoseConverter.to_pos_quat(l_B_p)
tf_broad.sendTransform(tf_pos, tf_quat, time, parent_frame, link_frame)
r.sleep()
def publish_transform():
optitrak_params = rosparam.get_param("optitrak_calibration")
remap_mat = PoseConverter.to_homo_mat(optitrak_params["position"],
optitrak_params["orientation"])**-1
tf_list = tf.TransformListener()
tf_broad = tf.TransformBroadcaster()
small_dur = rospy.Duration(0.001)
robot_mat = PoseConverter.to_homo_mat([0., 0., 0.], [0., 0., 0., 1.])
opti_mat = PoseConverter.to_homo_mat([0., 0., 0.], [0., 0., 0., 1.])
while not rospy.is_shutdown():
try:
now = rospy.Time(0.)
(opti_pos,
opti_rot) = tf_list.lookupTransform("/optitrak", "/pr2_antenna",
now)
opti_mat = PoseConverter.to_homo_mat(opti_pos, opti_rot)
now = rospy.Time(0.)
(robot_pos,
robot_rot) = tf_list.lookupTransform("/wide_stereo_link",
"/base_footprint", now)
robot_mat = PoseConverter.to_homo_mat(robot_pos, robot_rot)
odom_mat = opti_mat * remap_mat * robot_mat
odom_pos, odom_rot = PoseConverter.to_pos_quat(odom_mat)
tf_broad.sendTransform(odom_pos, odom_rot, rospy.Time.now(),
"/base_footprint", "/optitrak")
rospy.sleep(0.001)
except Exception as e:
print e
def publish_transform():
optitrak_params = rosparam.get_param("optitrak_calibration")
remap_mat = PoseConverter.to_homo_mat(optitrak_params["position"], optitrak_params["orientation"]) ** -1
tf_list = tf.TransformListener()
tf_broad = tf.TransformBroadcaster()
small_dur = rospy.Duration(0.001)
robot_mat = PoseConverter.to_homo_mat([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0])
opti_mat = PoseConverter.to_homo_mat([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0])
while not rospy.is_shutdown():
try:
now = rospy.Time(0.0)
(opti_pos, opti_rot) = tf_list.lookupTransform("/optitrak", "/pr2_antenna", now)
opti_mat = PoseConverter.to_homo_mat(opti_pos, opti_rot)
now = rospy.Time(0.0)
(robot_pos, robot_rot) = tf_list.lookupTransform("/wide_stereo_link", "/base_footprint", now)
robot_mat = PoseConverter.to_homo_mat(robot_pos, robot_rot)
odom_mat = opti_mat * remap_mat * robot_mat
odom_pos, odom_rot = PoseConverter.to_pos_quat(odom_mat)
tf_broad.sendTransform(odom_pos, odom_rot, rospy.Time.now(), "/base_footprint", "/optitrak")
rospy.sleep(0.001)
except Exception as e:
print e
def reset_ell_ep(self):
ee_pose = PoseConverter.to_pose_stamped_msg("/torso_lift_link", self.arm.get_end_effector_pose())
cur_time = rospy.Time.now()
ee_pose.header.stamp = cur_time
self.tf_list.waitForTransform("/torso_lift_link", "/ellipse_frame", cur_time, rospy.Duration(3))
ell_pose = self.tf_list.transformPose("/ellipse_frame", ee_pose)
pos, quat = PoseConverter.to_pos_quat(ell_pose)
self.ell_ep = list(self.ell_space.pos_to_ellipsoidal(*pos))
def print_trajectory_stats(ep_traj, traj, t_vals):
diffs_pos = [np.linalg.norm(traj[i+1][0] - traj[i][0]) for i in range(len(traj)-1)]
diffs_rot = [2 * np.arccos(np.fabs(np.dot(PoseConverter.to_pos_quat(traj[i+1])[1],
PoseConverter.to_pos_quat(traj[i])[1])))
for i in range(len(traj)-1)]
ptiles = [10, 25, 50, 75, 90, 100]
print "Diffs pos:", ", ".join(["%d:%f" % (ptile, stats.scoreatpercentile(diffs_pos, ptile))
for ptile in ptiles])
print "Diffs pos:", ", ".join(["%d:%f" % (ptile, stats.scoreatpercentile(diffs_rot, ptile))
for ptile in ptiles])
if max(diffs_pos) > 0.01:
print traj
print ep_traj
print t_vals
print "ind:", np.argmax(diffs_pos)
print ep_traj.T[np.argmax(diffs_pos)]
print traj[np.argmax(diffs_pos)]
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 reset_ell_ep(self):
ee_pose = PoseConverter.to_pose_stamped_msg(
"/torso_lift_link", self.arm.get_end_effector_pose())
cur_time = rospy.Time.now()
ee_pose.header.stamp = cur_time
self.tf_list.waitForTransform("/torso_lift_link", "/ellipse_frame",
cur_time, rospy.Duration(3))
ell_pose = self.tf_list.transformPose("/ellipse_frame", ee_pose)
pos, quat = PoseConverter.to_pos_quat(ell_pose)
self.ell_ep = list(self.ell_space.pos_to_ellipsoidal(*pos))
def ellipsoidal_to_pose(self, lat, lon, height):
pos = self.ellipsoidal_to_cart(lat, lon, height)
df_du = self.partial_u(lat, lon, height)
nx, ny, nz = df_du.T.A[0] / np.linalg.norm(df_du)
j = np.sqrt(1./(1.+ny*ny/(nz*nz)))
k = -ny*j/nz
norm_rot = np.mat([[-nx, ny*k - nz*j, 0],
[-ny, -nx*k, j],
[-nz, nx*j, k]])
_, norm_quat = PoseConverter.to_pos_quat(np.mat([0, 0, 0]).T, norm_rot)
rot_angle = np.arctan(-norm_rot[2,1] / norm_rot[2,2])
#print norm_rot
quat_ortho_rot = tf_trans.quaternion_from_euler(rot_angle + np.pi, 0.0, 0.0)
norm_quat_ortho = tf_trans.quaternion_multiply(norm_quat, quat_ortho_rot)
norm_rot_ortho = np.mat(tf_trans.quaternion_matrix(norm_quat_ortho)[:3,:3])
if norm_rot_ortho[2, 2] > 0:
flip_axis_ang = 0
else:
flip_axis_ang = np.pi
quat_flip = tf_trans.quaternion_from_euler(flip_axis_ang, 0.0, 0.0)
norm_quat_ortho_flipped = tf_trans.quaternion_multiply(norm_quat_ortho, quat_flip)
return PoseConverter.to_pos_quat(pos, norm_quat_ortho_flipped)
def main():
rospy.init_node("tf_link_flipper")
child_frame = rospy.get_param("~child_frame")
parent_frame = rospy.get_param("~parent_frame")
link_frame = rospy.get_param("~link_frame")
rate = rospy.get_param("~rate", 100)
link_trans = rospy.get_param("~link_transform")
l_B_c = PoseConverter.to_homo_mat(link_trans['pos'], link_trans['quat'])
tf_broad = tf.TransformBroadcaster()
tf_listener = tf.TransformListener()
rospy.sleep(1)
r = rospy.Rate(rate)
while not rospy.is_shutdown():
time = rospy.Time.now()
tf_listener.waitForTransform(child_frame, parent_frame, rospy.Time(0), rospy.Duration(1))
pos, quat = tf_listener.lookupTransform(child_frame, parent_frame, rospy.Time(0))
c_B_p = PoseConverter.to_homo_mat(pos, quat)
l_B_p = l_B_c * c_B_p
tf_pos, tf_quat = PoseConverter.to_pos_quat(l_B_p)
tf_broad.sendTransform(tf_pos, tf_quat, time, parent_frame, link_frame)
r.sleep()
def main():
gray = (100,100,100)
corner_len = 5
chessboard = ChessboardInfo()
chessboard.n_cols = 6
chessboard.n_rows = 7
chessboard.dim = 0.02273
cboard_frame = "kinect_cb_corner"
#kinect_tracker_frame = "kinect"
#TODO
use_pygame = False
kinect_tracker_frame = "pr2_antenna"
rospy.init_node("kinect_calib_test")
img_list = ImageListener("/kinect_head/rgb/image_color")
pix3d_srv = rospy.ServiceProxy("/pixel_2_3d", Pixel23d, True)
tf_list = tf.TransformListener()
if use_pygame:
pygame.init()
clock = pygame.time.Clock()
screen = pygame.display.set_mode((640, 480))
calib = Calibrator([chessboard])
done = False
corner_list = np.ones((2, corner_len)) * -1000.0
corner_i = 0
saved_corners_2d, saved_corners_3d, cb_locs = [], [], []
while not rospy.is_shutdown():
try:
cb_pos, cb_quat = tf_list.lookupTransform(kinect_tracker_frame,
cboard_frame,
rospy.Time())
except:
rospy.sleep(0.001)
continue
cv_img = img_list.get_cv_img()
if cv_img is not None:
has_corners, corners, chess = calib.get_corners(cv_img)
for corner2d in saved_corners_2d:
cv.Circle(cv_img, corner2d, 4, [0, 255, 255])
if has_corners:
corner_i += 1
corner = corners[0]
if use_pygame:
for event in pygame.event.get():
if event.type == pygame.KEYDOWN:
print event.dict['key'], pygame.K_d
if event.dict['key'] == pygame.K_d:
done = True
if event.dict['key'] == pygame.K_q:
return
if done:
break
corner_list[:, corner_i % corner_len] = corner
if np.linalg.norm(np.var(corner_list, 1)) < 1.0:
corner_avg = np.mean(corner_list, 1)
corner_avg_tuple = tuple(corner_avg.round().astype(int).tolist())
cv.Circle(cv_img, corner_avg_tuple, 4, [0, 255, 0])
pix3d_resp = pix3d_srv(*corner_avg_tuple)
if pix3d_resp.error_flag == pix3d_resp.SUCCESS:
corner_3d_tuple = (pix3d_resp.pixel3d.pose.position.x,
pix3d_resp.pixel3d.pose.position.y,
pix3d_resp.pixel3d.pose.position.z)
if len(saved_corners_3d) == 0:
cb_locs.append(cb_pos)
saved_corners_2d.append(corner_avg_tuple)
saved_corners_3d.append(corner_3d_tuple)
else:
diff_arr = np.array(np.mat(saved_corners_3d) - np.mat(corner_3d_tuple))
if np.min(np.sqrt(np.sum(diff_arr ** 2, 1))) >= 0.03:
cb_locs.append(cb_pos)
saved_corners_2d.append(corner_avg_tuple)
saved_corners_3d.append(corner_3d_tuple)
print "Added sample", len(saved_corners_2d) - 1
else:
cv.Circle(cv_img, corner, 4, [255, 0, 0])
else:
corner_list = np.ones((2, corner_len)) * -1000.0
if use_pygame:
if cv_img is None:
screen.fill(gray)
else:
screen.blit(img_list.get_pg_img(cv_img), (0, 0))
pygame.display.flip()
rospy.sleep(0.001)
A = np.mat(saved_corners_3d).T
B = np.mat(cb_locs).T
print A, B
t, R = umeyama_method(A, B)
print A, B, R, t
print "-" * 60
print "Transformation Parameters:"
pos, quat = PoseConverter.to_pos_quat(t, R)
print '%f %f %f %f %f %f %f' % tuple(pos + quat)
t_r, R_r = ransac(A, B, 0.02, percent_set_train=0.5, percent_set_fit=0.6)
print t_r, R_r
pos, quat = PoseConverter.to_pos_quat(t_r, R_r)
print '%f %f %f %f %f %f %f' % tuple(pos + quat)
def update_pose(self, pose):
self.tf_pose = PoseConverter.to_pos_quat(pose)
self.pub_tf(None)
def update_pose(self, pose):
self.tf_pose = PoseConverter.to_pos_quat(pose)
self.pub_tf(None)