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 generate_trajectory(ud):
b_B_s = PoseConverter.to_homo_mat(ud.start_click)
b_B_e = PoseConverter.to_homo_mat(ud.end_click)
s_B_e = (b_B_s ** -1) * b_B_e
b_normal = b_B_s[:3, 2] / np.linalg.norm(b_B_s[:3, 2])
s_vel = np.mat([s_B_e[0, 3], s_B_e[1, 3], 0]).T
s_vel = s_vel / np.linalg.norm(s_vel)
b_vel = b_B_s[:3, :3].T * s_vel
b_ortho = np.mat(np.cross(b_normal.T, b_vel.T)).T
b_ortho = b_ortho / np.linalg.norm(b_ortho)
b_R_traj = np.vstack([b_vel.T, b_ortho.T, b_normal.T])
b_p_start = b_B_s[:3, 3]
b_p_end = b_B_e[:3, 3]
b_p_end = 3 #TODO TODO
self.start_frame_pub.publish(PoseConverter.to_pose_stamped_msg(ud.start_click.header.frame_id,
(b_p_start, b_R_traj)))
self.end_frame_pub.publish(PoseConverter.to_pose_stamped_msg(ud.start_click.header.frame_id,
(b_p_end, b_R_traj)))
ud.start_traj_frame = (b_p_start, b_R_traj)
ud.end_traj_frame = (b_p_end, b_R_traj)
return 'succeeded'
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 generate_trajectory(ud):
b_B_s = PoseConverter.to_homo_mat(ud.start_click)
b_B_e = PoseConverter.to_homo_mat(ud.end_click)
s_B_e = (b_B_s**-1) * b_B_e
b_normal = b_B_s[:3, 2] / np.linalg.norm(b_B_s[:3, 2])
s_vel = np.mat([s_B_e[0, 3], s_B_e[1, 3], 0]).T
s_vel = s_vel / np.linalg.norm(s_vel)
b_vel = b_B_s[:3, :3].T * s_vel
b_ortho = np.mat(np.cross(b_normal.T, b_vel.T)).T
b_ortho = b_ortho / np.linalg.norm(b_ortho)
b_R_traj = np.vstack([b_vel.T, b_ortho.T, b_normal.T])
b_p_start = b_B_s[:3, 3]
b_p_end = b_B_e[:3, 3]
b_p_end = 3 #TODO TODO
self.start_frame_pub.publish(
PoseConverter.to_pose_stamped_msg(
ud.start_click.header.frame_id, (b_p_start, b_R_traj)))
self.end_frame_pub.publish(
PoseConverter.to_pose_stamped_msg(
ud.start_click.header.frame_id, (b_p_end, b_R_traj)))
ud.start_traj_frame = (b_p_start, b_R_traj)
ud.end_traj_frame = (b_p_end, b_R_traj)
return 'succeeded'
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 pub_head_registration(ud):
cheek_pose_base_link = self.tf_list.transformPose(
"/base_link", ud.cheek_pose)
# find the center of the ellipse given a cheek click
cheek_transformation = np.mat(
tf_trans.euler_matrix(2.6 * np.pi / 6, 0, 0, 'szyx'))
cheek_transformation[0:3, 3] = np.mat([-0.08, -0.04, 0]).T
cheek_pose = PoseConverter.to_homo_mat(cheek_pose_base_link)
#b_B_c[0:3,0:3] = np.eye(3)
norm_xy = cheek_pose[0:2, 2] / np.linalg.norm(cheek_pose[0:2, 2])
head_rot = np.arctan2(norm_xy[1], norm_xy[0])
cheek_pose[0:3,
0:3] = tf_trans.euler_matrix(0, 0, head_rot,
'sxyz')[0:3, 0:3]
self.cheek_pub.publish(
PoseConverter.to_pose_stamped_msg("/base_link", cheek_pose))
ell_center = cheek_pose * cheek_transformation
self.ell_center_pub.publish(
PoseConverter.to_pose_stamped_msg("/base_link", ell_center))
# create an ellipsoid msg and command it
ep = EllipsoidParams()
ep.e_frame.transform = PoseConverter.to_tf_msg(ell_center)
ep.height = 0.924
ep.E = 0.086
self.ep_pub.publish(ep)
return 'succeeded'
def capture_data():
tf_list = tf.TransformListener()
opti_mat, robot_mat = [], []
print "Hit enter to collect a data sample"
i = 0
while not rospy.is_shutdown():
rospy.sleep(0.3)
# if raw_input(".") == 'q':
# break
now = rospy.Time(0.0)
(opti_pos, opti_rot) = tf_list.lookupTransform("/pr2_antenna", "/optitrak", now)
(robot_pos, robot_rot) = tf_list.lookupTransform("/wide_stereo_link", "/base_footprint", now)
opti_mat.append(opti_pos)
opti_rot_foot = np.mat(
[
[-0.17496687, 0.03719102, -0.98387165],
[-0.98098458, -0.09183928, 0.17098186],
[-0.08399907, 0.99507908, 0.05255265],
]
).T
opti_pos_foot = np.mat([[1.276], [-0.81755], [-0.06905]])
foot_B_opti = PoseConverter.to_homo_mat(opti_pos_foot, opti_rot_foot) ** -1
opti_B_ant = PoseConverter.to_homo_mat(opti_pos, opti_rot) ** -1
wide_B_foot = PoseConverter.to_homo_mat(robot_pos, robot_rot)
print wide_B_foot * foot_B_opti * opti_B_ant
offset_robot_pos = (wide_B_foot * foot_B_opti)[:3, 3]
print offset_robot_pos, opti_pos
robot_mat.append(offset_robot_pos.T.A[0].tolist())
i += 1
if i % 10 == 0:
print i, "samples collected"
return opti_mat, robot_mat
def capture_data():
tf_list = tf.TransformListener()
opti_mat, robot_mat = [], []
print "Hit enter to collect a data sample"
i = 0
while not rospy.is_shutdown():
rospy.sleep(0.3)
#if raw_input(".") == 'q':
# break
now = rospy.Time(0.)
(opti_pos, opti_rot) = tf_list.lookupTransform("/pr2_antenna",
"/optitrak", now)
(robot_pos,
robot_rot) = tf_list.lookupTransform("/wide_stereo_link",
"/base_footprint", now)
opti_mat.append(opti_pos)
opti_rot_foot = np.mat([[-0.17496687, 0.03719102, -0.98387165],
[-0.98098458, -0.09183928, 0.17098186],
[-0.08399907, 0.99507908, 0.05255265]]).T
opti_pos_foot = np.mat([[1.276], [-0.81755], [-0.06905]])
foot_B_opti = PoseConverter.to_homo_mat(opti_pos_foot,
opti_rot_foot)**-1
opti_B_ant = PoseConverter.to_homo_mat(opti_pos, opti_rot)**-1
wide_B_foot = PoseConverter.to_homo_mat(robot_pos, robot_rot)
print wide_B_foot * foot_B_opti * opti_B_ant
offset_robot_pos = (wide_B_foot * foot_B_opti)[:3, 3]
print offset_robot_pos, opti_pos
robot_mat.append(offset_robot_pos.T.A[0].tolist())
i += 1
if i % 10 == 0:
print i, "samples collected"
return opti_mat, robot_mat
def execute_trajectory(self, ell_f, gripper_rot, duration=5.):
ell_f[1] = np.mod(ell_f[1], 2 * np.pi)
num_samps = int(duration / self.time_step)
t_vals = min_jerk_traj(num_samps)
self.arm.reset_ep()
self.reset_ell_ep()
ell_init = np.mat(self.ell_ep).T
ell_final = np.mat(ell_f).T
if np.fabs(2 * np.pi + ell_final[1, 0] - ell_init[1, 0]) < np.pi:
ell_final[1, 0] += 2 * np.pi
print "wrapping; ell_f:", ell_f
elif np.fabs(-2 * np.pi + ell_final[1, 0] - ell_init[1, 0]) < np.pi:
ell_final[1, 0] -= 2 * np.pi
print "wrapping; ell_f:", ell_f
print "init", ell_init, "final", ell_final
ell_traj = np.array(ell_init) + np.array(
np.tile(ell_final - ell_init, (1, num_samps))) * np.array(t_vals)
# find the current ellipsoid frame
cur_time = rospy.Time.now()
self.tf_list.waitForTransform("/torso_lift_link", "/ellipse_frame",
cur_time, rospy.Duration(3))
ell_frame_mat = PoseConverter.to_homo_mat(
self.tf_list.lookupTransform("/torso_lift_link", "/ellipse_frame",
cur_time))
print ell_frame_mat
print ell_traj.shape
ell_pose_traj = [
self.robot_ellipsoidal_pose(ell_traj[0, i], ell_traj[1, i],
ell_traj[2, i], gripper_rot,
ell_frame_mat)
for i in range(ell_traj.shape[1])
]
# replace the rotation matricies with a simple cartesian slerp
cart_interp_traj = self.arm.interpolate_ep(self.arm.get_ep(),
ell_pose_traj[-1], t_vals)
fixed_traj = [(ell_pose_traj[i][0], cart_interp_traj[i][1])
for i in range(num_samps)]
self.start_pub.publish(
PoseConverter.to_pose_stamped_msg("/torso_lift_link",
cart_interp_traj[0]))
self.end_pub.publish(
PoseConverter.to_pose_stamped_msg("/torso_lift_link",
cart_interp_traj[-1]))
#ep_traj_control = EPTrajectoryControl(self.arm, cart_interp_traj)
print_trajectory_stats(ell_traj, fixed_traj, t_vals)
ep_traj_control = EPTrajectoryControl(self.arm, fixed_traj)
self.action_preempted = False
self.ell_traj_behavior.stop_epc = False
monitor_timer = rospy.Timer(rospy.Duration(0.1), self._check_preempt)
self.ell_traj_behavior.epc_motion(ep_traj_control, self.time_step)
monitor_timer.shutdown()
if self.action_preempted:
self.reset_ell_ep()
else:
self.ell_ep = ell_f
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 robot_ellipsoidal_pose(self, lat, lon, height, gripper_rot, ell_frame_mat):
"""
Get pose in robot's frame of ellipsoidal coordinates
"""
pos, quat = self.ell_space.ellipsoidal_to_pose(lat, lon, height)
quat_gripper_rot = tf_trans.quaternion_from_euler(gripper_rot, 0, 0)
quat_rotated = tf_trans.quaternion_multiply(quat, quat_gripper_rot)
ell_pose_mat = PoseConverter.to_homo_mat(pos, quat_rotated)
return PoseConverter.to_pos_rot(ell_frame_mat * ell_pose_mat)
def ar_sub(self, msg):
cur_ar_ps = PoseConverter.to_pose_stamped_msg(msg.header.frame_id, msg.pose.pose)
cur_ar_ps.header.stamp = msg.header.stamp
try:
cur_ar_in_base = self.tf_list.transformPose("/base_link", cur_ar_ps)
except tf.Exception as e:
return
self.cur_ar_pose = PoseConverter.to_homo_mat(cur_ar_in_base)
self.ar_pose_updated = True
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 run_experiment(self):
self.backend.disable_interface("Setting up arm.")
pos, rot = PoseConverter.to_pos_rot(self.backend.controller.get_ell_frame())
pos += rot * np.mat([0.3, -0.2, -0.05]).T
_, tool_rot = PoseConverter.to_pos_rot([0, 0, 0], [np.pi, 0, 3./4. * np.pi])
rot *= tool_rot
ep_ac = EPArmController(self.cart_arm)
ep_ac.execute_interpolated_ep((pos, rot), 6.)
self.backend.set_arm(self.cart_arm)
self.backend.enable_interface("Controller ready.")
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 robot_ellipsoidal_pose(self, lat, lon, height, gripper_rot,
ell_frame_mat):
"""
Get pose in robot's frame of ellipsoidal coordinates
"""
pos, quat = self.ell_space.ellipsoidal_to_pose(lat, lon, height)
quat_gripper_rot = tf_trans.quaternion_from_euler(gripper_rot, 0, 0)
quat_rotated = tf_trans.quaternion_multiply(quat, quat_gripper_rot)
ell_pose_mat = PoseConverter.to_homo_mat(pos, quat_rotated)
return PoseConverter.to_pos_rot(ell_frame_mat * ell_pose_mat)
def ar_sub(self, msg):
cur_ar_ps = PoseConverter.to_pose_stamped_msg(msg.header.frame_id,
msg.pose.pose)
cur_ar_ps.header.stamp = msg.header.stamp
try:
cur_ar_in_base = self.tf_list.transformPose(
"/base_link", cur_ar_ps)
except tf.Exception as e:
return
self.cur_ar_pose = PoseConverter.to_homo_mat(cur_ar_in_base)
self.ar_pose_updated = True
def run_experiment(self):
self.backend.disable_interface("Setting up arm.")
pos, rot = PoseConverter.to_pos_rot(
self.backend.controller.get_ell_frame())
pos += rot * np.mat([0.3, -0.2, -0.05]).T
_, tool_rot = PoseConverter.to_pos_rot([0, 0, 0],
[np.pi, 0, 3. / 4. * np.pi])
rot *= tool_rot
ep_ac = EPArmController(self.cart_arm)
ep_ac.execute_interpolated_ep((pos, rot), 6.)
self.backend.set_arm(self.cart_arm)
self.backend.enable_interface("Controller ready.")
def execute_trajectory(self, ell_f, gripper_rot, duration=5.):
ell_f[1] = np.mod(ell_f[1], 2 * np.pi)
num_samps = int(duration / self.time_step)
t_vals = min_jerk_traj(num_samps)
self.arm.reset_ep()
self.reset_ell_ep()
ell_init = np.mat(self.ell_ep).T
ell_final = np.mat(ell_f).T
if np.fabs(2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi:
ell_final[1,0] += 2 * np.pi
print "wrapping; ell_f:", ell_f
elif np.fabs(-2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi:
ell_final[1,0] -= 2 * np.pi
print "wrapping; ell_f:", ell_f
print "init", ell_init, "final", ell_final
ell_traj = np.array(ell_init) + np.array(np.tile(ell_final - ell_init, (1, num_samps))) * np.array(t_vals)
# find the current ellipsoid frame
cur_time = rospy.Time.now()
self.tf_list.waitForTransform("/torso_lift_link", "/ellipse_frame", cur_time, rospy.Duration(3))
ell_frame_mat = PoseConverter.to_homo_mat(
self.tf_list.lookupTransform("/torso_lift_link",
"/ellipse_frame", cur_time))
print ell_frame_mat
print ell_traj.shape
ell_pose_traj = [self.robot_ellipsoidal_pose(ell_traj[0,i], ell_traj[1,i], ell_traj[2,i],
gripper_rot, ell_frame_mat)
for i in range(ell_traj.shape[1])]
# replace the rotation matricies with a simple cartesian slerp
cart_interp_traj = self.arm.interpolate_ep(self.arm.get_ep(), ell_pose_traj[-1],
t_vals)
fixed_traj = [(ell_pose_traj[i][0], cart_interp_traj[i][1]) for i in range(num_samps)]
self.start_pub.publish(PoseConverter.to_pose_stamped_msg("/torso_lift_link", cart_interp_traj[0]))
self.end_pub.publish(PoseConverter.to_pose_stamped_msg("/torso_lift_link", cart_interp_traj[-1]))
#ep_traj_control = EPTrajectoryControl(self.arm, cart_interp_traj)
print_trajectory_stats(ell_traj, fixed_traj, t_vals)
ep_traj_control = EPTrajectoryControl(self.arm, fixed_traj)
self.action_preempted = False
self.ell_traj_behavior.stop_epc = False
monitor_timer = rospy.Timer(rospy.Duration(0.1), self._check_preempt)
self.ell_traj_behavior.epc_motion(ep_traj_control, self.time_step)
monitor_timer.shutdown()
if self.action_preempted:
self.reset_ell_ep()
else:
self.ell_ep = ell_f
def pose_saver(tf_frame, bag_file, pickle_file):
arm = create_pr2_arm('l', PR2ArmJTransposeTask)
tf_list = tf.TransformListener()
named_poses = NamedPoses()
named_poses.pose_arr.header.frame_id = tf_frame
pose_dict = {}
while True:
if rospy.is_shutdown():
return
pose_name = raw_input("Type the name of the pose and hit enter to save it (Type 'q' to quit) ")
if pose_name == 'q':
break
arm_pose = PoseConverter.to_pose_stamped_msg("/torso_lift_link", arm.get_end_effector_pose())
tf_pose = tf_list.transformPose(tf_frame, arm_pose)
named_poses.pose_arr.poses.append(tf_pose)
named_poses.names.append(pose_name)
pose_dict[pose_name] = tf_pose
if bag_file != "":
bag = rosbag.Bag(bag_file, 'w')
bag.write("/named_poses", named_poses)
bag.close()
if pickle_file != "":
f = file(pickle_file, 'w')
pickle.dump(pose_dict, f)
f.close()
def pose_saver(tf_frame, bag_file, pickle_file):
arm = create_pr2_arm('l', PR2ArmJTransposeTask)
tf_list = tf.TransformListener()
named_poses = NamedPoses()
named_poses.pose_arr.header.frame_id = tf_frame
pose_dict = {}
while True:
if rospy.is_shutdown():
return
pose_name = raw_input(
"Type the name of the pose and hit enter to save it (Type 'q' to quit) "
)
if pose_name == 'q':
break
arm_pose = PoseConverter.to_pose_stamped_msg(
"/torso_lift_link", arm.get_end_effector_pose())
tf_pose = tf_list.transformPose(tf_frame, arm_pose)
named_poses.pose_arr.poses.append(tf_pose)
named_poses.names.append(pose_name)
pose_dict[pose_name] = tf_pose
if bag_file != "":
bag = rosbag.Bag(bag_file, 'w')
bag.write("/named_poses", named_poses)
bag.close()
if pickle_file != "":
f = file(pickle_file, 'w')
pickle.dump(pose_dict, f)
f.close()
def update_ellipse_pose(self):
cur_time = rospy.Time.now()
self.tf_list.waitForTransform("/torso_lift_link", "/ellipse_frame",
cur_time, rospy.Duration(3))
cur_tf = self.tf_list.lookupTransform("/torso_lift_link",
"/ellipse_frame", cur_time,
rospy.Duration(3))
self.ell_tf = PoseConverter.to_homo_mat(cur_tf)
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 get_head_pose(self, ell_coords_rot, gripper_rot=0.):
lat, lon, height = ell_coords_rot[0]
roll, pitch, yaw = ell_coords_rot[1]
pos, rot = PoseConverter.to_pos_rot(
self.ell_space.ellipsoidal_to_pose(lat, lon, height))
rot = rot * tf_trans.euler_matrix(yaw, pitch, roll + gripper_rot,
'szyx')[:3, :3]
return pos, rot
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 _ctrl_state_cb(self, ctrl_state):
with self.lock:
self.ep = PoseConverter.to_pos_rot(ctrl_state.x_desi_filtered)
with self.ctrl_state_lock:
self.ctrl_state_dict["frame"] = ctrl_state.header.frame_id
self.ctrl_state_dict["x_desi"] = PoseConverter.to_pos_rot(
ctrl_state.x_desi)
self.ctrl_state_dict["xd_desi"] = PoseConverter.to_pos_rot(
ctrl_state.xd_desi)
self.ctrl_state_dict["x_act"] = PoseConverter.to_pos_rot(
ctrl_state.x)
self.ctrl_state_dict["xd_act"] = PoseConverter.to_pos_rot(
ctrl_state.xd)
self.ctrl_state_dict["x_desi_filt"] = PoseConverter.to_pos_rot(
ctrl_state.x_desi_filtered)
self.ctrl_state_dict["x_err"] = PoseConverter.to_pos_rot(
ctrl_state.x_err)
self.ctrl_state_dict["tau_pose"] = np.array(ctrl_state.tau_pose)
self.ctrl_state_dict["tau_posture"] = np.array(
ctrl_state.tau_posture)
self.ctrl_state_dict["tau"] = np.array(ctrl_state.tau)
self.ctrl_state_dict["F"] = np.array([
ctrl_state.F.force.x, ctrl_state.F.force.y,
ctrl_state.F.force.z, ctrl_state.F.torque.x,
ctrl_state.F.torque.y, ctrl_state.F.torque.z
])
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 point_head(self):
print "Pointing head"
head_goal = PointHeadGoal()
head_goal.target = PoseConverter.to_point_stamped_msg('/torso_lift_link',
np.mat([1., 0.4, 0.]).T,
np.mat(np.eye(3)))
head_goal.target.header.stamp = rospy.Time()
head_goal.min_duration = rospy.Duration(3.)
head_goal.max_velocity = 0.2
self.head_point_sac.send_goal_and_wait(head_goal)
def get_head_pose_srv(self, req):
if req.name not in head_poses:
pose = (np.mat([-9999, -9999, -9999]).T, np.mat(np.zeros((3, 3))))
else:
pose = self.get_head_pose(req.name, req.gripper_rot)
if USE_ELLIPSE_FRAME:
frame = "/ellipse_frame"
else:
frame = "/base_link"
pose_stamped = PoseConverter.to_pose_stamped_msg(frame, pose)
#self.tmp_pub.publish(pose_stamped)
return pose_stamped
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 get_head_pose_srv(self, req):
if req.name not in head_poses:
pose = (np.mat([-9999, -9999, -9999]).T, np.mat(np.zeros((3, 3))))
else:
pose = self.get_head_pose(req.name, req.gripper_rot)
if USE_ELLIPSE_FRAME:
frame = "/ellipse_frame"
else:
frame = "/base_link"
pose_stamped = PoseConverter.to_pose_stamped_msg(frame, pose)
# self.tmp_pub.publish(pose_stamped)
return pose_stamped
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 pub_head_registration(ud):
cheek_pose_base_link = self.tf_list.transformPose("/base_link", ud.cheek_pose)
# find the center of the ellipse given a cheek click
cheek_transformation = np.mat(tf_trans.euler_matrix(2.6 * np.pi/6, 0, 0, 'szyx'))
cheek_transformation[0:3, 3] = np.mat([-0.08, -0.04, 0]).T
cheek_pose = PoseConverter.to_homo_mat(cheek_pose_base_link)
#b_B_c[0:3,0:3] = np.eye(3)
norm_xy = cheek_pose[0:2, 2] / np.linalg.norm(cheek_pose[0:2, 2])
head_rot = np.arctan2(norm_xy[1], norm_xy[0])
cheek_pose[0:3,0:3] = tf_trans.euler_matrix(0, 0, head_rot, 'sxyz')[0:3,0:3]
self.cheek_pub.publish(PoseConverter.to_pose_stamped_msg("/base_link", cheek_pose))
ell_center = cheek_pose * cheek_transformation
self.ell_center_pub.publish(PoseConverter.to_pose_stamped_msg("/base_link", ell_center))
# create an ellipsoid msg and command it
ep = EllipsoidParams()
ep.e_frame.transform = PoseConverter.to_tf_msg(ell_center)
ep.height = 0.924
ep.E = 0.086
self.ep_pub.publish(ep)
return 'succeeded'
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 ar_sub(self, msg):
if self.kin_arm == None:
self.kin_arm = kin_from_param(base_link="base_link",
end_link=msg.header.frame_id)
base_B_camera = self.kin_arm.forward_filled()
camera_B_tag = PoseConverter.to_homo_mat(msg.pose.pose)
cur_ar_pose = base_B_camera * camera_B_tag
# check to see if the tag is in front of the robot
if cur_ar_pose[0, 3] < 0.:
#rospy.logwarn("Strange AR toolkit bug!")
return
self.cur_ar_pose = cur_ar_pose
self.ar_pose_updated = True
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 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("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 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 publish_vector(self, m_id):
new_m = copy.deepcopy(self.m)
new_m.id = m_id
u, v, p = 1.0, np.random.uniform(0, np.pi), np.random.uniform(0, 2 * np.pi)
pos = self.sspace.spheroidal_to_cart((u, v, p))
new_m.points.append(Point(*pos))
df_du = self.sspace.partial_u((u, v, p))
df_du *= 0.1 / np.linalg.norm(df_du)
new_m.points.append(Point(*(pos+df_du)))
self.ma.markers.append(new_m)
self.vis_pub.publish(self.ma)
rot_gripper = np.pi/4.
pos, rot = self.sspace.spheroidal_to_pose((u,v,p), rot_gripper)
ps_msg = PoseConverter.to_pose_stamped_msg("torso_lift_link", pos, rot)
self.pose_pub.publish(ps_msg)
def publish_vector(self, m_id):
new_m = copy.deepcopy(self.m)
new_m.id = m_id
u, v, p = 1.0, np.random.uniform(0, np.pi), np.random.uniform(
0, 2 * np.pi)
pos = self.sspace.spheroidal_to_cart((u, v, p))
new_m.points.append(Point(*pos))
df_du = self.sspace.partial_u((u, v, p))
df_du *= 0.1 / np.linalg.norm(df_du)
new_m.points.append(Point(*(pos + df_du)))
self.ma.markers.append(new_m)
self.vis_pub.publish(self.ma)
rot_gripper = np.pi / 4.
pos, rot = self.sspace.spheroidal_to_pose((u, v, p), rot_gripper)
ps_msg = PoseConverter.to_pose_stamped_msg("torso_lift_link", pos, rot)
self.pose_pub.publish(ps_msg)
def _ctrl_state_cb(self, ctrl_state):
with self.lock:
self.ep = PoseConverter.to_pos_rot(ctrl_state.x_desi_filtered)
with self.ctrl_state_lock:
self.ctrl_state_dict["frame"] = ctrl_state.header.frame_id
self.ctrl_state_dict["x_desi"] = PoseConverter.to_pos_rot(ctrl_state.x_desi)
self.ctrl_state_dict["xd_desi"] = PoseConverter.to_pos_rot(ctrl_state.xd_desi)
self.ctrl_state_dict["x_act"] = PoseConverter.to_pos_rot(ctrl_state.x)
self.ctrl_state_dict["xd_act"] = PoseConverter.to_pos_rot(ctrl_state.xd)
self.ctrl_state_dict["x_desi_filt"] = PoseConverter.to_pos_rot(
ctrl_state.x_desi_filtered)
self.ctrl_state_dict["x_err"] = PoseConverter.to_pos_rot(ctrl_state.x_err)
self.ctrl_state_dict["tau_pose"] = np.array(ctrl_state.tau_pose)
self.ctrl_state_dict["tau_posture"] = np.array(ctrl_state.tau_posture)
self.ctrl_state_dict["tau"] = np.array(ctrl_state.tau)
self.ctrl_state_dict["F"] = np.array([ctrl_state.F.force.x,
ctrl_state.F.force.y,
ctrl_state.F.force.z,
ctrl_state.F.torque.x,
ctrl_state.F.torque.y,
ctrl_state.F.torque.z])
def set_ep(self, cep, duration, delay=0.0):
cep_pose_stmp = PoseConverter.to_pose_stamped_msg('torso_lift_link', cep)
cep_pose_stmp.header.stamp = rospy.Time.now()
self.command_pose_pub.publish(cep_pose_stmp)
def update_ellipse_pose(self):
cur_time = rospy.Time.now()
self.tf_list.waitForTransform("/torso_lift_link", "/ellipse_frame", cur_time, rospy.Duration(3))
cur_tf = self.tf_list.lookupTransform("/torso_lift_link", "/ellipse_frame", cur_time, rospy.Duration(3))
self.ell_tf = PoseConverter.to_homo_mat(cur_tf)
def get_head_pose(self, name, gripper_rot=0.0):
lat, lon, height = head_poses[name][0]
roll, pitch, yaw = head_poses[name][1]
pos, rot = PoseConverter.to_pos_rot(self.ell_space.ellipsoidal_to_pose(lat, lon, height))
rot = rot * tf_trans.euler_matrix(yaw, pitch, roll + gripper_rot, "rzyx")[:3, :3]
return pos, rot
def update_pose(self, pose):
self.tf_pose = PoseConverter.to_pos_quat(pose)
self.pub_tf(None)
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 load_ell_params(self, e_params):
self.center, self.rot = PoseConverter.to_pos_rot(e_params.e_frame)
self.E = e_params.E
self.a = self.A * self.E
self.height = e_params.height
def set_ep(self, cep, duration, delay=0.0):
cep_pose_stmp = PoseConverter.to_pose_stamped_msg(
'torso_lift_link', cep)
cep_pose_stmp.header.stamp = rospy.Time.now()
self.command_pose_pub.publish(cep_pose_stmp)
def get_head_pose(self, ell_coords_rot, gripper_rot=0.):
lat, lon, height = ell_coords_rot[0]
roll, pitch, yaw = ell_coords_rot[1]
pos, rot = PoseConverter.to_pos_rot(self.ell_space.ellipsoidal_to_pose(lat, lon, height))
rot = rot * tf_trans.euler_matrix(yaw, pitch, roll + gripper_rot, 'szyx')[:3, :3]
return pos, rot
def main():
rospy.init_node("switch_controller")
from optparse import OptionParser
p = OptionParser()
p.add_option('-s',
'--stiff',
dest="stiff",
default=False,
action="store_true",
help="Enable stiff controller.")
p.add_option('-f',
'--force',
dest="force",
default=False,
action="store_true",
help="Enable force controller.")
p.add_option('-m',
'--force_mag',
dest="force_mag",
default=2,
help="Specify force magnitude.")
p.add_option('-x',
'--max_force',
dest="max_force",
default=-1,
help="Specify max force magnitude.")
p.add_option('-i',
'--impedance',
dest="impedance",
default=False,
action="store_true",
help="Enable impedance controller.")
p.add_option('-c',
'--compliance',
dest="compliance",
default=-1,
help="Enable impedance controller.")
p.add_option('-t',
'--tip_frame',
dest="tip_frame",
default="/l_gripper_tool_frame",
help="Set tip to this frame.")
p.add_option('-z',
'--zero_sensor',
dest="zero_sensor",
default=False,
action="store_true",
help="Just zero the sensor.")
p.add_option('-r',
'--reset_pose',
dest="reset_pose",
default=False,
action="store_true",
help="Use the saved position in the data file.")
(opts, args) = p.parse_args()
arm = create_pr2_arm('l', PR2ArmHybridForce)
rospy.sleep(0.1)
# reset arm
arm.zero_sensor()
if opts.zero_sensor:
return
arm.set_force(6 * [0])
#
if opts.reset_pose:
pkg_dir = roslib.rospack.rospackexec(["find", "hrl_phri_2011"])
bag = rosbag.Bag(pkg_dir + "/data/saved_teleop_pose.bag", 'r')
for topic, msg, stamp in bag.read_messages("/teleop_pose"):
pose = PoseConverter.to_pos_rot(
[msg.position.x, msg.position.y, msg.position.z], [
msg.orientation.x, msg.orientation.y, msg.orientation.z,
msg.orientation.w
])
for topic, msg, stamp in bag.read_messages("/teleop_posture"):
posture = msg.data
bag.close()
arm.set_posture(posture)
i_poses = arm.interpolate_ep(arm.get_end_effector_pose(), pose, 100)
for cur_pose in i_poses:
arm.set_ep(cur_pose, 1)
rospy.sleep(0.1)
return
# set common parameters
arm.set_force_max([float(opts.max_force), -1, -1, -1, -1, -1])
arm.set_tip_frame(opts.tip_frame)
if opts.stiff:
compliance = float(opts.compliance)
if compliance < 0:
compliance = 1300
#arm.set_force_gains(p_trans=[1, 1, 1], p_rot=0.1, i_trans=[0.002, 0.001, 0.001], i_max_trans=[10, 5, 5], i_rot=0, i_max_rot=0)
arm.set_force_gains(p_trans=[1, 0, 0],
p_rot=0.1,
i_trans=[0.002, 0, 0],
i_max_trans=[10, 0, 0],
i_rot=0,
i_max_rot=0)
arm.set_motion_gains(p_trans=[compliance, 1300, 1300],
d_trans=[16, 10, 10],
p_rot=120,
d_rot=0)
arm.set_force_directions([])
arm.set_force(6 * [0])
elif opts.impedance:
compliance = float(opts.compliance)
if compliance < 0:
compliance = 80
arm.set_force_gains(p_trans=[3, 1, 1],
p_rot=0.1,
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=[compliance, 1300, 1300],
d_trans=[16, 10, 10],
p_rot=120,
d_rot=0)
arm.set_force_directions([])
arm.set_force(6 * [0])
elif opts.force:
arm.set_force_gains(p_trans=[3, 1, 1],
p_rot=0.1,
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=[float(opts.compliance), 1300, 1300],
d_trans=[16, 10, 10],
p_rot=120,
d_rot=0)
arm.set_force_directions(['x'])
arm.set_force([float(opts.force_mag), 0, 0, 0, 0, 0])
else:
p.print_help()
return
arm.update_gains()
def main():
rospy.init_node("switch_controller")
from optparse import OptionParser
p = OptionParser()
p.add_option('-s', '--stiff', dest="stiff", default=False,
action="store_true", help="Enable stiff controller.")
p.add_option('-f', '--force', dest="force", default=False,
action="store_true", help="Enable force controller.")
p.add_option('-m', '--force_mag', dest="force_mag", default=2,
help="Specify force magnitude.")
p.add_option('-x', '--max_force', dest="max_force", default=-1,
help="Specify max force magnitude.")
p.add_option('-i', '--impedance', dest="impedance", default=False,
action="store_true", help="Enable impedance controller.")
p.add_option('-c', '--compliance', dest="compliance", default=-1,
help="Enable impedance controller.")
p.add_option('-t', '--tip_frame', dest="tip_frame", default="/l_gripper_tool_frame",
help="Set tip to this frame.")
p.add_option('-z', '--zero_sensor', dest="zero_sensor", default=False,
action="store_true", help="Just zero the sensor.")
p.add_option('-r', '--reset_pose', dest="reset_pose", default=False,
action="store_true", help="Use the saved position in the data file.")
(opts, args) = p.parse_args()
arm = create_pr2_arm('l', PR2ArmHybridForce)
rospy.sleep(0.1)
# reset arm
arm.zero_sensor()
if opts.zero_sensor:
return
arm.set_force(6 * [0])
#
if opts.reset_pose:
pkg_dir = roslib.rospack.rospackexec(["find", "hrl_phri_2011"])
bag = rosbag.Bag(pkg_dir + "/data/saved_teleop_pose.bag", 'r')
for topic, msg, stamp in bag.read_messages("/teleop_pose"):
pose = PoseConverter.to_pos_rot([msg.position.x, msg.position.y, msg.position.z],
[msg.orientation.x, msg.orientation.y, msg.orientation.z,
msg.orientation.w])
for topic, msg, stamp in bag.read_messages("/teleop_posture"):
posture = msg.data
bag.close()
arm.set_posture(posture)
i_poses = arm.interpolate_ep(arm.get_end_effector_pose(), pose, 100)
for cur_pose in i_poses:
arm.set_ep(cur_pose, 1)
rospy.sleep(0.1)
return
# set common parameters
arm.set_force_max([float(opts.max_force), -1, -1, -1, -1, -1])
arm.set_tip_frame(opts.tip_frame)
if opts.stiff:
compliance = float(opts.compliance)
if compliance < 0:
compliance = 1300
#arm.set_force_gains(p_trans=[1, 1, 1], p_rot=0.1, i_trans=[0.002, 0.001, 0.001], i_max_trans=[10, 5, 5], i_rot=0, i_max_rot=0)
arm.set_force_gains(p_trans=[1, 0, 0], p_rot=0.1, i_trans=[0.002, 0, 0], i_max_trans=[10, 0, 0], i_rot=0, i_max_rot=0)
arm.set_motion_gains(p_trans=[compliance, 1300, 1300], d_trans=[16, 10, 10], p_rot=120, d_rot=0)
arm.set_force_directions([])
arm.set_force(6 * [0])
elif opts.impedance:
compliance = float(opts.compliance)
if compliance < 0:
compliance = 80
arm.set_force_gains(p_trans=[3, 1, 1], p_rot=0.1, 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=[compliance, 1300, 1300], d_trans=[16, 10, 10], p_rot=120, d_rot=0)
arm.set_force_directions([])
arm.set_force(6 * [0])
elif opts.force:
arm.set_force_gains(p_trans=[3, 1, 1], p_rot=0.1, 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=[float(opts.compliance), 1300, 1300], d_trans=[16, 10, 10], p_rot=120, d_rot=0)
arm.set_force_directions(['x'])
arm.set_force([float(opts.force_mag), 0, 0, 0, 0, 0])
else:
p.print_help()
return
arm.update_gains()
def load_ell_params(self, e_params):
self.center, self.rot = PoseConverter.to_pos_rot(e_params.e_frame)
self.E = e_params.E
self.a = self.A * self.E
self.height = e_params.height
def update_pose(self, pose):
self.tf_pose = PoseConverter.to_pos_quat(pose)
self.pub_tf(None)
