def _normal_to_ellipse_prolate(self, lat, lon, height):
pos = self.ellipsoidal_to_cart(lat, lon, height)
df_du = self.partial_height(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 = PoseConv.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 = trans.quaternion_from_euler(rot_angle + np.pi, 0.0,
0.0)
norm_quat_ortho = trans.quaternion_multiply(norm_quat, quat_ortho_rot)
norm_rot_ortho = np.mat(
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 = trans.quaternion_from_euler(flip_axis_ang, 0.0, 0.0)
norm_quat_ortho_flipped = trans.quaternion_multiply(
norm_quat_ortho, quat_flip)
pose = PoseConv.to_pos_quat(pos, norm_quat_ortho_flipped)
#print ("ellipsoidal_to_pose: latlonheight: %f, %f, %f" %
# (lat, lon, height) +
# str(PoseConv.to_homo_mat(pose)))
return pose
def _normal_to_ellipse_oblate(self, lat, lon, height):
pos = self.ellipsoidal_to_cart(lat, lon, height)
df_du = self.partial_height(-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 = PoseConv.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 = trans.quaternion_from_euler(rot_angle, 0.0, 0.0)
norm_quat_ortho = trans.quaternion_multiply(norm_quat, quat_ortho_rot)
quat_ortho_rot2 = trans.quaternion_from_euler(0.0, np.pi / 2, 0.0)
norm_quat_ortho = trans.quaternion_multiply(norm_quat_ortho,
quat_ortho_rot2)
if lon >= np.pi:
quat_flip = trans.quaternion_from_euler(0.0, 0.0, np.pi)
norm_quat_ortho = trans.quaternion_multiply(
norm_quat_ortho, quat_flip)
pose = PoseConv.to_pos_quat(pos, norm_quat_ortho)
#print ("ellipsoidal_to_pose: latlonheight: %f, %f, %f" %
# (lat, lon, height) +
# str(PoseConv.to_homo_mat(pose)))
return pose
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 cart_error(self, ep_actual, ep_desired):
pos_act, rot_act = PoseConv.to_pos_rot(ep_actual)
pos_des, rot_des = PoseConv.to_pos_rot(ep_desired)
err = np.mat(np.zeros((6, 1)))
err[:3,0] = pos_act - pos_des
err[3:6,0] = np.mat(trans.euler_from_matrix(rot_des.T * rot_act)).T
return err
def command_move_cb(self, msg):
if self.arm is None:
rospy.logwarn(
"[cartesian_manager] Cartesian controller not enabled.")
return
self.cart_ctrl.stop_moving(wait=True)
if msg.header.frame_id == "":
msg.header.frame_id = "torso_lift_link"
if self.kin is None or msg.header.frame_id not in self.kin.get_link_names(
):
self.kin = create_joint_kin("torso_lift_link", msg.header.frame_id)
torso_pos_ep, torso_rot_ep = PoseConv.to_pos_rot(self.arm.get_ep())
torso_B_ref = self.kin.forward(base_link="torso_lift_link",
end_link=msg.header.frame_id)
_, torso_rot_ref = PoseConv.to_pos_rot(torso_B_ref)
torso_rot_ref *= self.frame_rot
ref_pos_off, ref_rot_off = PoseConv.to_pos_rot(msg)
change_pos = torso_rot_ep.T * torso_rot_ref * ref_pos_off
change_pos_xyz = change_pos.T.A[0]
ep_rot_ref = torso_rot_ep.T * torso_rot_ref
change_rot = ep_rot_ref * ref_rot_off * ep_rot_ref.T
_, change_rot_rpy = PoseConv.to_pos_euler(
np.mat([0] * 3).T, change_rot)
rospy.loginfo("[cartesian_manager] Command relative movement." +
str((change_pos_xyz, change_rot_rpy)))
self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot_rpy),
((0, 0, 0), 0),
velocity=self.velocity,
blocking=True)
def command_absolute_cb(self, msg):
if self.arm is None:
rospy.logwarn(
"[cartesian_manager] Cartesian controller not enabled.")
return
self.cart_ctrl.stop_moving(wait=True)
if msg.header.frame_id == "":
msg.header.frame_id = "torso_lift_link"
if self.kin is None or msg.header.frame_id not in self.kin.get_link_names(
):
self.kin = create_joint_kin("torso_lift_link", msg.header.frame_id)
torso_pos_ep, torso_rot_ep = self.arm.get_ep()
torso_B_ref = self.kin.forward(base_link="torso_lift_link",
end_link=msg.header.frame_id)
ref_B_goal = PoseConv.to_homo_mat(msg)
torso_B_goal = torso_B_ref * ref_B_goal
change_pos, change_rot = PoseConv.to_pos_rot(torso_B_goal)
change_pos_xyz = change_pos.T.A[0]
rospy.loginfo("[cartesian_manager] Command absolute movement." +
str((change_pos_xyz, change_rot)))
self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot),
((1, 1, 1), 1),
velocity=self.velocity,
blocking=True)
def robot_ellipsoidal_pose(self, lat, lon, height, orient_quat, kinect_frame_mat=None):
if kinect_frame_mat is None:
kinect_frame_mat = self.get_ell_frame()
pos, quat = self.ell_space.ellipsoidal_to_pose(lat, lon, height)
quat_rotated = trans.quaternion_multiply(quat, orient_quat)
ell_pose_mat = PoseConv.to_homo_mat(pos, quat_rotated)
return PoseConv.to_pos_rot(kinect_frame_mat * ell_pose_mat)
def _normal_to_ellipse_prolate(self, lat, lon, height):
pos = self.ellipsoidal_to_cart(lat, lon, height)
df_du = self.partial_height(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 = PoseConv.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 = trans.quaternion_from_euler(rot_angle + np.pi, 0.0, 0.0)
norm_quat_ortho = trans.quaternion_multiply(norm_quat, quat_ortho_rot)
norm_rot_ortho = np.mat(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 = trans.quaternion_from_euler(flip_axis_ang, 0.0, 0.0)
norm_quat_ortho_flipped = trans.quaternion_multiply(norm_quat_ortho, quat_flip)
pose = PoseConv.to_pos_quat(pos, norm_quat_ortho_flipped)
#print ("ellipsoidal_to_pose: latlonheight: %f, %f, %f" %
# (lat, lon, height) +
# str(PoseConv.to_homo_mat(pose)))
return pose
def _normal_to_ellipse_oblate(self, lat, lon, height):
pos = self.ellipsoidal_to_cart(lat, lon, height)
df_du = self.partial_height(-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 = PoseConv.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 = trans.quaternion_from_euler(rot_angle, 0.0, 0.0)
norm_quat_ortho = trans.quaternion_multiply(norm_quat, quat_ortho_rot)
quat_ortho_rot2 = trans.quaternion_from_euler(0.0, np.pi/2, 0.0)
norm_quat_ortho = trans.quaternion_multiply(norm_quat_ortho, quat_ortho_rot2)
if lon >= np.pi:
quat_flip = trans.quaternion_from_euler(0.0, 0.0, np.pi)
norm_quat_ortho = trans.quaternion_multiply(norm_quat_ortho, quat_flip)
pose = PoseConv.to_pos_quat(pos, norm_quat_ortho)
#print ("ellipsoidal_to_pose: latlonheight: %f, %f, %f" %
# (lat, lon, height) +
# str(PoseConv.to_homo_mat(pose)))
return pose
def create_ellipsoidal_path(self,
start_ell_pos,
start_ell_quat,
end_ell_pos,
end_ell_quat,
velocity=0.001,
min_jerk=True):
print "Start rot (%s):\r\n%s" % (type(start_ell_quat), start_ell_quat)
print "End rot (%s):\r\n%s" % (type(end_ell_quat), end_ell_quat)
_, start_ell_rot = PoseConv.to_pos_rot((start_ell_pos, start_ell_quat))
_, end_ell_rot = PoseConv.to_pos_rot((end_ell_pos, end_ell_quat))
rpy = trans.euler_from_matrix(
start_ell_rot.T *
end_ell_rot) # get roll, pitch, yaw of angle diff
end_ell_pos[1] = np.mod(end_ell_pos[1],
2 * np.pi) # wrap longitude value
ell_init = np.mat(start_ell_pos).T
ell_final = np.mat(end_ell_pos).T
# find the closest longitude angle to interpolate to
if np.fabs(2 * np.pi + ell_final[1, 0] - ell_init[1, 0]) < np.pi:
ell_final[1, 0] += 2 * np.pi
elif np.fabs(-2 * np.pi + ell_final[1, 0] - ell_init[1, 0]) < np.pi:
ell_final[1, 0] -= 2 * np.pi
if np.any(np.isnan(ell_init)) or np.any(np.isnan(ell_final)):
rospy.logerr("[ellipsoid_space] Nan values in ellipsoid. " +
"ell_init: %f, %f, %f; " %
(ell_init[0, 0], ell_init[1, 0], ell_init[2, 0]) +
"ell_final: %f, %f, %f; " %
(ell_final[0, 0], ell_final[1, 0], ell_final[2, 0]))
return None
num_samps = np.max([
2,
int(np.linalg.norm(ell_final - ell_init) / velocity),
int(np.linalg.norm(rpy) / velocity)
])
if min_jerk:
t_vals = min_jerk_traj(num_samps)
else:
t_vals = np.linspace(0, 1, num_samps)
# smoothly interpolate from init to final
ell_lat_traj = np.interp(t_vals, (0, 1),
(start_ell_pos[0], end_ell_pos[0]))
ell_lon_traj = np.interp(t_vals, (0, 1),
(start_ell_pos[1], end_ell_pos[1]))
ell_height_traj = np.interp(t_vals, (0, 1),
(start_ell_pos[2], end_ell_pos[2]))
ell_pos_traj = np.vstack((ell_lat_traj, ell_lon_traj, ell_height_traj))
ell_quat_traj = [
trans.quaternion_slerp(start_ell_quat, end_ell_quat, t)
for t in t_vals
]
return [(ell_pos_traj[:, i], ell_quat_traj[i])
for i in xrange(num_samps)]
def cal_joint_angle_kdl(self, x_w, y_w, z_w):
pos_euler = [[x_w, y_w, z_w], [0.0, 0.0, 0.0]]
twist_msg = PoseConv.to_twist_msg(pos_euler)
homo_mat = PoseConv.to_homo_mat(twist_msg)
print(homo_mat)
q = self.kdl_kin.random_joint_angles()
q_r = self.kdl_kin.inverse(homo_mat, q)
return q_r
def load_params(self, params):
kinect_B_head = PoseConv.to_homo_mat(params.e_frame)
base_B_kinect = self.kin_head.forward(base_link="base_link",
end_link=self.kinect_frame)
base_B_head = base_B_kinect * kinect_B_head
self.head_center = PoseConv.to_pose_stamped_msg("/base_link",base_B_head)
self.ell_space = EllipsoidSpace()
self.ell_space.load_ell_params(params.E, params.is_oblate, params.height)
rospy.loginfo("Loaded ellispoidal parameters.")
def init_reg_cb(self, req):
# TODO REMOVE THIS FACE SIDE MESS
self.publish_feedback("Performing Head Registration. Please Wait.")
self.face_side = rospy.get_param("/face_side", 'r')
bag_str = self.reg_dir + '/' + '_'.join(
[self.subject, self.face_side, "head_transform"]) + ".bag"
rospy.loginfo("[%s] Loading %s" % (rospy.get_name(), bag_str))
try:
bag = rosbag.Bag(bag_str, 'r')
for topic, msg, ts in bag.read_messages():
head_tf = msg
assert (head_tf
is not None), "Error reading head transform bagfile"
bag.close()
except Exception as e:
self.publish_feedback(
"Registration failed: Error loading saved registration.")
rospy.logerr(
"[%s] Cannot load registration parameters from %s:\r\n%s" %
(rospy.get_name(), bag_str, e))
return (False, PoseStamped())
if self.face_side == 'r':
head_registration = self.head_registration_r
else:
head_registration = self.head_registration_l
try:
rospy.loginfo(
"[%s] Requesting head registration for %s at pixel (%d, %d)." %
(rospy.get_name(), self.subject, req.u, req.v))
self.head_pc_reg = head_registration(req.u, req.v).reg_pose
if ((self.head_pc_reg.pose.position == Point(0.0, 0.0, 0.0))
and (self.head_pc_reg.pose.orientation == Quaternion(
0.0, 0.0, 0.0, 1.0))):
raise rospy.ServiceException("Unable to find a good match.")
self.head_pc_reg = None
except rospy.ServiceException as se:
self.publish_feedback("Registration failed: %s" % se)
return (False, PoseStamped())
pc_reg_mat = PoseConv.to_homo_mat(self.head_pc_reg)
head_tf_mat = PoseConv.to_homo_mat(
Transform(head_tf.transform.translation,
head_tf.transform.rotation))
self.head_pose = PoseConv.to_pose_stamped_msg(pc_reg_mat**-1 *
head_tf_mat)
self.head_pose.header.frame_id = self.head_pc_reg.header.frame_id
self.head_pose.header.stamp = self.head_pc_reg.header.stamp
side = "right" if (self.face_side == 'r') else "left"
self.publish_feedback(
"Registered head using %s cheek model, please check and confirm." %
side)
# rospy.loginfo('[%s] Head frame registered at:\r\n %s' %(rospy.get_name(), self.head_pose))
self.test_pose.publish(self.head_pose)
return (True, self.head_pose)
def pose_to_ellipsoidal(self, pose):
pose_pos, pose_rot = PoseConv.to_pos_rot(pose)
lat, lon, height = self.pos_to_ellipsoidal(pose_pos[0, 0],
pose_pos[1, 0], pose_pos[2,
0])
_, ell_rot = PoseConv.to_pos_rot(
self.normal_to_ellipse(lat, lon, height))
_, quat_rot = PoseConv.to_pos_quat(
np.mat([0] * 3).T, ell_rot.T * pose_rot)
return [lat, lon, height], quat_rot
def load_params(self, params):
kinect_B_head = PoseConv.to_homo_mat(params.e_frame)
base_B_kinect = self.kin_head.forward(base_link="base_link",
end_link=self.kinect_frame)
base_B_head = base_B_kinect * kinect_B_head
self.head_center = PoseConv.to_pose_stamped_msg(
"/base_link", base_B_head)
self.ell_space = EllipsoidSpace()
self.ell_space.load_ell_params(params.E, params.is_oblate,
params.height)
rospy.loginfo("Loaded ellispoidal parameters.")
def create_tool_arrow():
arrows = MarkerArray()
color = ColorRGBA(0., 0., 1., 1.)
for i, param in enumerate(params):
ell_pos, ell_rot = params[param]
_, ell_rot_mat = PoseConv.to_pos_rot([0]*3, ell_rot)
cart_pose = PoseConv.to_homo_mat(ell_space.ellipsoidal_to_pose(*ell_pos))
cart_pose[:3,:3] = cart_pose[:3,:3] * ell_rot_mat
arrow = create_arrow_marker(cart_pose, i, color)
arrow.header.stamp = rospy.Time.now()
arrows.markers.append(arrow)
return arrows
def robot_ellipsoidal_pose(self,
lat,
lon,
height,
orient_quat,
kinect_frame_mat=None):
if kinect_frame_mat is None:
kinect_frame_mat = self.get_ell_frame()
pos, quat = self.ell_space.ellipsoidal_to_pose(lat, lon, height)
quat_rotated = trans.quaternion_multiply(quat, orient_quat)
ell_pose_mat = PoseConv.to_homo_mat(pos, quat_rotated)
return PoseConv.to_pos_rot(kinect_frame_mat * ell_pose_mat)
def register_ellipse(self, mode, side):
reg_e_params = EllipsoidParams()
try:
now = rospy.Time.now()
self.tfl.waitForTransform("/base_link", "/head_frame", now,
rospy.Duration(10.))
pos, quat = self.tfl.lookupTransform("/head_frame", "/base_frame",
now)
self.head_pose = PoseStamped()
self.head_pose.header.stamp = now
self.head_pose.header.frame_id = "/base_link"
self.head_pose.pose.position = Point(*pos)
self.head_pose.pose.orientation = Quaternion(*quat)
except:
rospy.logwarn("[%s] Head registration not loaded yet." %
rospy.get_name())
return (False, reg_e_params)
reg_prefix = rospy.get_param("~registration_prefix", "")
registration_files = rospy.get_param("~registration_files", None)
if mode not in registration_files:
rospy.logerr("[%s] Mode not in registration_files parameters" %
(rospy.get_name()))
return (False, reg_e_params)
try:
bag_str = reg_prefix + registration_files[mode][side]
rospy.loginfo("[%s] Loading %s" % (rospy.get_name(), bag_str))
bag = rosbag.Bag(bag_str, 'r')
e_params = None
for topic, msg, ts in bag.read_messages():
e_params = msg
assert e_params is not None
bag.close()
except:
rospy.logerr("[%s] Cannot load registration parameters from %s" %
(rospy.get_name(), bag_str))
return (False, reg_e_params)
head_reg_mat = PoseConv.to_homo_mat(self.head_pose)
ell_reg = PoseConv.to_homo_mat(
Transform(e_params.e_frame.transform.translation,
e_params.e_frame.transform.rotation))
reg_e_params.e_frame = PoseConv.to_tf_stamped_msg(head_reg_mat**-1 *
ell_reg)
reg_e_params.e_frame.header.frame_id = self.head_reg_tf.header.frame_id
reg_e_params.e_frame.child_frame_id = '/ellipse_frame'
reg_e_params.height = e_params.height
reg_e_params.E = e_params.E
self.ell_params_pub.publish(reg_e_params)
self.ell_frame = reg_e_params.e_frame
return (True, reg_e_params)
def main():
rospy.init_node("visualize_poses")
pose_file = file(sys.argv[1], 'r')
params = yaml.load(pose_file)
ell_reg_bag = rosbag.Bag(sys.argv[2], 'r')
for topic, ell_reg, ts in ell_reg_bag.read_messages():
pass
ell_space = EllipsoidSpace(E=ell_reg.E)
pub_head_pose = rospy.Publisher("/head_center_test", PoseStamped)
pub_arrows = rospy.Publisher("visualization_markers_array", MarkerArray)
def create_tool_arrow():
arrows = MarkerArray()
color = ColorRGBA(0., 0., 1., 1.)
for i, param in enumerate(params):
ell_pos, ell_rot = params[param]
_, ell_rot_mat = PoseConv.to_pos_rot([0]*3, ell_rot)
cart_pose = PoseConv.to_homo_mat(ell_space.ellipsoidal_to_pose(*ell_pos))
cart_pose[:3,:3] = cart_pose[:3,:3] * ell_rot_mat
arrow = create_arrow_marker(cart_pose, i, color)
arrow.header.stamp = rospy.Time.now()
arrows.markers.append(arrow)
return arrows
r = rospy.Rate(10)
while not rospy.is_shutdown():
pub_head_pose.publish(PoseConv.to_pose_stamped_msg("/base_link", [0]*3, [0]*3))
arrows = create_tool_arrow()
pub_arrows.publish(arrows)
r.sleep()
def inverse(self, pose, q_guess=None, min_joints=None, max_joints=None):
pos, rot = PoseConv.to_pos_rot(pose)
pos_kdl = kdl.Vector(pos[0,0], pos[1,0], pos[2,0])
rot_kdl = kdl.Rotation(rot[0,0], rot[0,1], rot[0,2],
rot[1,0], rot[1,1], rot[1,2],
rot[2,0], rot[2,1], rot[2,2])
frame_kdl = kdl.Frame(rot_kdl, pos_kdl)
if min_joints is None:
min_joints = self.joint_safety_lower
if max_joints is None:
max_joints = self.joint_safety_upper
mins_kdl = joint_list_to_kdl(min_joints)
maxs_kdl = joint_list_to_kdl(max_joints)
ik_p_kdl = kdl.ChainIkSolverPos_NR_JL(self.chain, mins_kdl, maxs_kdl,
self._fk_kdl, self._ik_v_kdl)
if q_guess == None:
# use the midpoint of the joint limits as the guess
lower_lim = np.where(np.isfinite(min_joints), min_joints, 0.)
upper_lim = np.where(np.isfinite(max_joints), max_joints, 0.)
q_guess = (lower_lim + upper_lim) / 2.0
q_guess = np.where(np.isnan(q_guess), [0.]*len(q_guess), q_guess)
q_kdl = kdl.JntArray(self.num_joints)
q_guess_kdl = joint_list_to_kdl(q_guess)
if ik_p_kdl.CartToJnt(q_guess_kdl, frame_kdl, q_kdl) >= 0:
return np.array(joint_kdl_to_list(q_kdl))
else:
return None
def inverse_biased(self,
pose,
q_init,
q_bias,
q_bias_weights,
rot_weight=1.,
bias_vel=0.01,
num_iter=100):
# This code is potentially volitile
q_out = np.mat(self.inverse_search(pose)).T
for i in range(num_iter):
pos_fk, rot_fk = PoseConv.to_pos_rot(self.forward(q_out))
delta_twist = np.mat(np.zeros((6, 1)))
pos_delta = pos - pos_fk
delta_twist[:3, 0] = pos_delta
rot_delta = np.mat(np.eye(4))
rot_delta[:3, :3] = rot * rot_fk.T
rot_delta_angles = np.mat(trans.euler_from_matrix(rot_delta)).T
delta_twist[3:6, 0] = rot_delta_angles
J = self.jacobian(q_out)
J[3:6, :] *= np.sqrt(rot_weight)
delta_twist[3:6, 0] *= np.sqrt(rot_weight)
J_tinv = np.linalg.inv(J.T * J + np.diag(q_bias_weights) *
np.eye(len(q_init))) * J.T
q_bias_diff = q_bias - q_out
q_bias_diff_normed = q_bias_diff * bias_vel / np.linalg.norm(
q_bias_diff)
delta_q = q_bias_diff_normed + J_tinv * (delta_twist -
J * q_bias_diff_normed)
q_out += delta_q
q_out = np.mat(self.clip_joints_safe(q_out.T.A[0])).T
return q_out
def inverse(self, pose, q_guess=None, min_joints=None, max_joints=None):
pos, rot = PoseConv.to_pos_rot(pose)
pos_kdl = kdl.Vector(pos[0, 0], pos[1, 0], pos[2, 0])
rot_kdl = kdl.Rotation(rot[0, 0], rot[0, 1], rot[0, 2], rot[1, 0],
rot[1, 1], rot[1, 2], rot[2, 0], rot[2, 1],
rot[2, 2])
frame_kdl = kdl.Frame(rot_kdl, pos_kdl)
if min_joints is None:
min_joints = self.joint_safety_lower
if max_joints is None:
max_joints = self.joint_safety_upper
mins_kdl = joint_list_to_kdl(min_joints)
maxs_kdl = joint_list_to_kdl(max_joints)
ik_p_kdl = kdl.ChainIkSolverPos_NR_JL(self.chain, mins_kdl, maxs_kdl,
self._fk_kdl, self._ik_v_kdl)
if q_guess == None:
# use the midpoint of the joint limits as the guess
lower_lim = np.where(np.isfinite(min_joints), min_joints, 0.)
upper_lim = np.where(np.isfinite(max_joints), max_joints, 0.)
q_guess = (lower_lim + upper_lim) / 2.0
q_guess = np.where(np.isnan(q_guess), [0.] * len(q_guess), q_guess)
q_kdl = kdl.JntArray(self.num_joints)
q_guess_kdl = joint_list_to_kdl(q_guess)
if ik_p_kdl.CartToJnt(q_guess_kdl, frame_kdl, q_kdl) >= 0:
return np.array(joint_kdl_to_list(q_kdl))
else:
return None
def init_reg_cb(self, req):
# TODO REMOVE THIS FACE SIDE MESS
self.publish_feedback("Performing Head Registration. Please Wait.")
self.face_side = rospy.get_param("~face_side1", 'r')
bag_str = self.reg_dir + '/' + '_'.join([self.subject, self.face_side, "head_transform"]) + ".bag"
rospy.loginfo("[%s] Loading %s" %(rospy.get_name(), bag_str))
try:
bag = rosbag.Bag(bag_str, 'r')
for topic, msg, ts in bag.read_messages():
head_tf = msg
assert (head_tf is not None), "Error reading head transform bagfile"
bag.close()
except Exception as e:
self.publish_feedback("Registration failed: Error loading saved registration.")
rospy.logerr("[%s] Cannot load registration parameters from %s:\r\n%s" %
(rospy.get_name(), bag_str, e))
return (False, PoseStamped())
if self.face_side == 'r':
head_registration = self.head_registration_r
else:
head_registration = self.head_registration_l
try:
rospy.loginfo("[%s] Requesting head registration for %s at pixel (%d, %d)." %(rospy.get_name(),
self.subject,
req.u, req.v))
self.head_pc_reg = head_registration(req.u, req.v).reg_pose
if ((self.head_pc_reg.pose.position == Point(0.0, 0.0, 0.0)) and
(self.head_pc_reg.pose.orientation == Quaternion(0.0, 0.0, 0.0, 1.0))):
raise rospy.ServiceException("Unable to find a good match.")
self.head_pc_reg = None
except rospy.ServiceException as se:
self.publish_feedback("Registration failed: %s" %se)
return (False, PoseStamped())
pc_reg_mat = PoseConv.to_homo_mat(self.head_pc_reg)
head_tf_mat = PoseConv.to_homo_mat(Transform(head_tf.transform.translation,
head_tf.transform.rotation))
self.head_pose = PoseConv.to_pose_stamped_msg(pc_reg_mat**-1 * head_tf_mat)
self.head_pose.header.frame_id = self.head_pc_reg.header.frame_id
self.head_pose.header.stamp = self.head_pc_reg.header.stamp
side = "right" if (self.face_side == 'r') else "left"
self.publish_feedback("Registered head using %s cheek model, please check and confirm." %side)
# rospy.loginfo('[%s] Head frame registered at:\r\n %s' %(rospy.get_name(), self.head_pose))
self.test_pose.publish(self.head_pose)
return (True, self.head_pose)
def get_head_pose(self, name, gripper_rot=0.):
lat, lon, height = head_poses[name][0]
roll, pitch, yaw = head_poses[name][1]
pos, rot = PoseConv.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 FK(self, q, link_number=None):
if link_number is not None:
end_link = self.get_link_names(fixed=False)[link_number]
else:
end_link = None
homo_mat = self.forward(q, end_link)
pos, rot = PoseConv.to_pos_rot(homo_mat)
return pos, rot
def FK(self, q, link_number=None):
if link_number is not None:
end_link = self.get_link_names(fixed=False)[link_number]
else:
end_link = None
homo_mat = self.forward(q, end_link)
pos, rot = PoseConv.to_pos_rot(homo_mat)
return pos, rot
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 = PoseConv.to_pos_rot(
self.ell_space.ellipsoidal_to_pose(lat, lon, height))
rot = rot * trans.euler_matrix(yaw, pitch, roll + gripper_rot,
'szyx')[:3, :3]
return pos, rot
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)
frame = "/ellipse_frame"
pose_stamped = PoseConv.to_pose_stamped_msg(frame, pose)
#self.tmp_pub.publish(pose_stamped)
return pose_stamped
def point_towards_midpoint(self, clean_joints, psm1_pos, psm2_pos, key_hole,ecm_pose):
mid_point = (psm1_pos + psm2_pos)/2
# mid_point = ecm_pose[0:3,3] - numpy.array([0,0,.01]).reshape(3,1)
self.add_marker(PoseConv.to_homo_mat([mid_point, [0,0,0]]), '/marker_subscriber',color=[1,0,0], scale=[0.047/5,0.047/5,0.047/5])
self.add_marker(PoseConv.to_homo_mat([key_hole,[0,0,0]]), '/keyhole_subscriber',[0,0,1])
self.add_marker(ecm_pose, '/current_ecm_pose', [1,0,0], Marker.ARROW, scale=[.1,.005,.005])
temp = clean_joints['ecm'].position
b,_ = self.ecm_kin.FK([temp[0],temp[1],.14,temp[3]])
# find the equation of the line that goes through the key_hole and the
# mid_point
ab_vector = (mid_point-key_hole)
ecm_current_direction = b-key_hole
self.add_marker(ecm_pose, '/midpoint_to_keyhole', [0,1,1], type=Marker.LINE_LIST, scale = [0.005, 0.005, 0.005], points=[b, key_hole])
self.add_marker(PoseConv.to_homo_mat([ab_vector,[0,0,0]]), '/ab_vector',[0,1,0], type=Marker.ARROW)
r = self.find_rotation_matrix_between_two_vectors(ecm_current_direction, ab_vector)
m = math.sqrt(ab_vector[0]**2 + ab_vector[1]**2 + ab_vector[2]**2) # ab_vector's length
# insertion joint length
l = math.sqrt( (ecm_pose[0,3]-key_hole[0])**2 + (ecm_pose[1,3]-key_hole[1])**2 + (ecm_pose[2,3]-key_hole[2])**2)
# Equation of the line that passes through the midpoint of the tools and the key hole
x = lambda t: key_hole[0] + ab_vector[0] * t
y = lambda t: key_hole[1] + ab_vector[1] * t
z = lambda t: key_hole[2] + ab_vector[2] * t
t = l/m
new_ecm_position = numpy.array([x(t), y(t), z(t)]).reshape(3,1)
ecm_pose[0:3,0:3] = r* ecm_pose[0:3,0:3]
ecm_pose[0:3,3] = new_ecm_position
self.add_marker(ecm_pose, '/target_ecm_pose', [0,0,1], Marker.ARROW, scale=[.1,.005,.005])
output_msg = clean_joints['ecm']
try:
p = self.ecm_kin.inverse(ecm_pose)
except Exception as e:
rospy.logerr('error')
if p != None:
p[3] = 0
output_msg.position = p
return output_msg
def register_ellipse(self, mode, side):
reg_e_params = EllipsoidParams()
try:
now = rospy.Time.now()
self.tfl.waitForTransform("/base_link", "/head_frame", now, rospy.Duration(10.))
pos, quat = self.tfl.lookupTransform("/head_frame", "/base_frame", now)
self.head_pose = PoseStamped()
self.head_pose.header.stamp = now
self.head_pose.header.frame_id = "/base_link"
self.head_pose.pose.position = Point(*pos)
self.head_pose.pose.orientation = Quaternion(*quat)
except:
rospy.logwarn("[%s] Head registration not loaded yet." %rospy.get_name())
return (False, reg_e_params)
reg_prefix = rospy.get_param("~registration_prefix", "")
registration_files = rospy.get_param("~registration_files", None)
if mode not in registration_files:
rospy.logerr("[%s] Mode not in registration_files parameters" % (rospy.get_name()))
return (False, reg_e_params)
try:
bag_str = reg_prefix + registration_files[mode][side]
rospy.loginfo("[%s] Loading %s" %(rospy.get_name(), bag_str))
bag = rosbag.Bag(bag_str, 'r')
e_params = None
for topic, msg, ts in bag.read_messages():
e_params = msg
assert e_params is not None
bag.close()
except:
rospy.logerr("[%s] Cannot load registration parameters from %s" %(rospy.get_name(), bag_str))
return (False, reg_e_params)
head_reg_mat = PoseConv.to_homo_mat(self.head_pose)
ell_reg = PoseConv.to_homo_mat(Transform(e_params.e_frame.transform.translation,
e_params.e_frame.transform.rotation))
reg_e_params.e_frame = PoseConv.to_tf_stamped_msg(head_reg_mat**-1 * ell_reg)
reg_e_params.e_frame.header.frame_id = self.head_reg_tf.header.frame_id
reg_e_params.e_frame.child_frame_id = '/ellipse_frame'
reg_e_params.height = e_params.height
reg_e_params.E = e_params.E
self.ell_params_pub.publish(reg_e_params)
self.ell_frame = reg_e_params.e_frame
return (True, reg_e_params)
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 _ctrl_state_cb(self, ctrl_state):
self._save_ep(PoseConv.to_homo_mat(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"] = PoseConv.to_pos_rot(ctrl_state.x_desi)
self.ctrl_state_dict["xd_desi"] = extract_twist(ctrl_state.xd_desi)
self.ctrl_state_dict["x_act"] = PoseConv.to_pos_rot(ctrl_state.x)
self.ctrl_state_dict["xd_act"] = extract_twist(ctrl_state.xd)
self.ctrl_state_dict["x_desi_filt"] = PoseConv.to_pos_rot(
ctrl_state.x_desi_filtered)
self.ctrl_state_dict["x_err"] = extract_twist(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 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 execute_cart_move(self, change_ep, abs_sel, velocity=0.001,
num_samps=None, blocking=True):
if not self.cmd_lock.acquire(False):
return False
cur_pos, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep())
change_pos_ep, change_rot_ep = change_ep
abs_cart_ep_sel, is_abs_rot = abs_sel
pos_f = np.where(abs_cart_ep_sel, change_pos_ep,
np.array(cur_pos + cur_rot * np.mat(change_pos_ep).T).T[0])
if is_abs_rot:
rot_mat_f = change_rot_ep
else:
rpy = change_rot_ep
_, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep())
rot_mat = np.mat(trans.euler_matrix(*rpy))[:3,:3]
rot_mat_f = cur_rot * rot_mat
traj = self._create_cart_trajectory(pos_f, rot_mat_f, velocity, num_samps)
retval = self._run_traj(traj, blocking=blocking)
self.cmd_lock.release()
return retval
def enable_controller_cb(req):
if req.enable:
_, frame_rot = PoseConv.to_pos_rot([0]*3,
[req.frame_rot.x, req.frame_rot.y, req.frame_rot.z])
if req.velocity == 0:
req.velocity = 0.03
success = self.enable_controller(req.end_link, req.ctrl_params, req.ctrl_name,
frame_rot, velocity=req.velocity)
else:
success = self.disable_controller()
return EnableCartControllerResponse(success)
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 _run_traj(self, traj, blocking=True):
self.start_pub.publish(
PoseConv.to_pose_stamped_msg("/torso_lift_link", traj[0]))
self.end_pub.publish(
PoseConv.to_pose_stamped_msg("/torso_lift_link", traj[-1]))
# make sure traj beginning is close to current end effector position
init_pos_tolerance = rospy.get_param("~init_pos_tolerance", 0.05)
init_rot_tolerance = rospy.get_param("~init_rot_tolerance", np.pi/12)
ee_pos, ee_rot = PoseConv.to_pos_rot(self.arm.get_end_effector_pose())
_, rot_diff = PoseConv.to_pos_euler((ee_pos, ee_rot * traj[0][1].T))
pos_diff = np.linalg.norm(ee_pos - traj[0][0])
if pos_diff > init_pos_tolerance:
rospy.logwarn("[controller_base] End effector too far from current position. " +
"Pos diff: %.3f, Tolerance: %.3f" % (pos_diff, init_pos_tolerance))
return False
if np.linalg.norm(rot_diff) > init_rot_tolerance:
rospy.logwarn("[controller_base] End effector too far from current rotation. " +
"Rot diff: %.3f, Tolerance: %.3f" % (np.linalg.norm(rot_diff),
init_rot_tolerance))
return False
return self.execute_cart_traj(self.arm, traj, self.time_step, blocking=blocking)
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 command_absolute_cb(self, msg):
if self.arm is None:
rospy.logwarn("[cartesian_manager] Cartesian controller not enabled.")
return
self.cart_ctrl.stop_moving(wait=True)
if msg.header.frame_id == "":
msg.header.frame_id = "torso_lift_link"
if self.kin is None or msg.header.frame_id not in self.kin.get_link_names():
self.kin = create_joint_kin("torso_lift_link", msg.header.frame_id)
torso_pos_ep, torso_rot_ep = self.arm.get_ep()
torso_B_ref = self.kin.forward(base_link="torso_lift_link",
end_link=msg.header.frame_id)
ref_B_goal = PoseConv.to_homo_mat(msg)
torso_B_goal = torso_B_ref * ref_B_goal
change_pos, change_rot = PoseConv.to_pos_rot(torso_B_goal)
change_pos_xyz = change_pos.T.A[0]
rospy.loginfo("[cartesian_manager] Command absolute movement." +
str((change_pos_xyz, change_rot)))
self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot), ((1, 1, 1), 1),
velocity=self.velocity, blocking=True)
def create_ellipsoidal_path(self, start_ell_pos, start_ell_quat,
end_ell_pos, end_ell_quat,
velocity=0.001, min_jerk=True):
print "Start rot (%s):\r\n%s" %(type(start_ell_quat),start_ell_quat)
print "End rot (%s):\r\n%s" %(type(end_ell_quat),end_ell_quat)
_, start_ell_rot = PoseConv.to_pos_rot((start_ell_pos,start_ell_quat))
_, end_ell_rot = PoseConv.to_pos_rot((end_ell_pos,end_ell_quat))
rpy = trans.euler_from_matrix(start_ell_rot.T * end_ell_rot) # get roll, pitch, yaw of angle diff
end_ell_pos[1] = np.mod(end_ell_pos[1], 2 * np.pi) # wrap longitude value
ell_init = np.mat(start_ell_pos).T
ell_final = np.mat(end_ell_pos).T
# find the closest longitude angle to interpolate to
if np.fabs(2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi:
ell_final[1,0] += 2 * np.pi
elif np.fabs(-2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi:
ell_final[1,0] -= 2 * np.pi
if np.any(np.isnan(ell_init)) or np.any(np.isnan(ell_final)):
rospy.logerr("[ellipsoid_space] Nan values in ellipsoid. " +
"ell_init: %f, %f, %f; " % (ell_init[0,0], ell_init[1,0], ell_init[2,0]) +
"ell_final: %f, %f, %f; " % (ell_final[0,0], ell_final[1,0], ell_final[2,0]))
return None
num_samps = np.max([2, int(np.linalg.norm(ell_final - ell_init) / velocity),
int(np.linalg.norm(rpy) / velocity)])
if min_jerk:
t_vals = min_jerk_traj(num_samps)
else:
t_vals = np.linspace(0,1,num_samps)
# smoothly interpolate from init to final
ell_lat_traj = np.interp(t_vals, (0,1),(start_ell_pos[0], end_ell_pos[0]))
ell_lon_traj = np.interp(t_vals, (0,1),(start_ell_pos[1], end_ell_pos[1]))
ell_height_traj = np.interp(t_vals, (0,1),(start_ell_pos[2], end_ell_pos[2]))
ell_pos_traj = np.vstack((ell_lat_traj, ell_lon_traj, ell_height_traj))
ell_quat_traj = [trans.quaternion_slerp(start_ell_quat, end_ell_quat, t) for t in t_vals]
return [(ell_pos_traj[:,i], ell_quat_traj[i]) for i in xrange(num_samps)]
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 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 _create_cart_trajectory(self, pos_f, rot_mat_f, velocity=0.001, num_samps=None):
cur_pos, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep())
rpy = trans.euler_from_matrix(cur_rot.T * rot_mat_f) # get roll, pitch, yaw of angle diff
if num_samps is None:
num_samps = np.max([2, int(np.linalg.norm(pos_f - cur_pos) / velocity),
int(np.linalg.norm(rpy) / velocity)])
traj = self.arm.interpolate_ep([cur_pos, cur_rot],
[np.mat(pos_f).T, rot_mat_f],
min_jerk_traj(num_samps))
return traj
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 ar_sub(self, msg):
if self.kin_arm == None:
self.kin_arm = create_joint_kin(base_link="base_link",
end_link=msg.header.frame_id)
base_B_camera = self.kin_arm.forward()
camera_B_tag = PoseConv.to_homo_mat(msg.markers[0].pose.pose) #changed to use Alvar Markers
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("Tag behind robot: Strange AR toolkit bug!")
return
self.cur_ar_pose = cur_ar_pose
self.ar_pose_updated = True
def main():
rospy.init_node("joint_kinematics")
import sys
def usage():
print("Tests for kdl_parser:\n")
print("kdl_parser <urdf file>")
print("\tLoad the URDF from file.")
print("kdl_parser")
print("\tLoad the URDF from the parameter server.")
sys.exit(1)
if len(sys.argv) > 2:
usage()
if len(sys.argv) == 2 and (sys.argv[1] == "-h" or sys.argv[1] == "--help"):
usage()
if (len(sys.argv) == 1):
robot = URDF.from_parameter_server()
else:
f = file(sys.argv[1], 'r')
robot = Robot.from_xml_string(f.read())
f.close()
if True:
import random
base_link = robot.get_root()
end_link = robot.link_map.keys()[random.randint(
0,
len(robot.link_map) - 1)]
print("Root link: %s; Random end link: %s" % (base_link, end_link))
js_kin = JointKinematics(robot, base_link, end_link)
print("Joint angles:", js_kin.get_joint_angles())
print("Joint angles (wrapped):", js_kin.get_joint_angles(True))
print("Joint velocities:", js_kin.get_joint_velocities())
print("Joint efforts:", js_kin.get_joint_efforts())
print("Jacobian:", js_kin.jacobian())
kdl_pose = js_kin.forward()
print("FK:", kdl_pose)
print("End effector force:", js_kin.end_effector_force())
if True:
import tf
from hrl_geom.pose_converter import PoseConv
tf_list = tf.TransformListener()
rospy.sleep(1)
t = tf_list.getLatestCommonTime(base_link, end_link)
tf_pose = PoseConv.to_homo_mat(
tf_list.lookupTransform(base_link, end_link, t))
print("Error with TF:", np.linalg.norm(kdl_pose - tf_pose))
def command_move_cb(self, msg):
if self.arm is None:
rospy.logwarn("[cartesian_manager] Cartesian controller not enabled.")
return
self.cart_ctrl.stop_moving(wait=True)
if msg.header.frame_id == "":
msg.header.frame_id = "torso_lift_link"
if self.kin is None or msg.header.frame_id not in self.kin.get_link_names():
self.kin = create_joint_kin("torso_lift_link", msg.header.frame_id)
torso_pos_ep, torso_rot_ep = PoseConv.to_pos_rot(self.arm.get_ep())
torso_B_ref = self.kin.forward(base_link="torso_lift_link",
end_link=msg.header.frame_id)
_, torso_rot_ref = PoseConv.to_pos_rot(torso_B_ref)
torso_rot_ref *= self.frame_rot
ref_pos_off, ref_rot_off = PoseConv.to_pos_rot(msg)
change_pos = torso_rot_ep.T * torso_rot_ref * ref_pos_off
change_pos_xyz = change_pos.T.A[0]
ep_rot_ref = torso_rot_ep.T * torso_rot_ref
change_rot = ep_rot_ref * ref_rot_off * ep_rot_ref.T
_, change_rot_rpy = PoseConv.to_pos_euler(np.mat([0]*3).T, change_rot)
rospy.loginfo("[cartesian_manager] Command relative movement." +
str((change_pos_xyz, change_rot_rpy)))
self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot_rpy), ((0, 0, 0), 0),
velocity=self.velocity, blocking=True)
def enable_controller_cb(req):
if req.enable:
_, frame_rot = PoseConv.to_pos_rot(
[0] * 3,
[req.frame_rot.x, req.frame_rot.y, req.frame_rot.z])
if req.velocity == 0:
req.velocity = 0.03
success = self.enable_controller(req.end_link,
req.ctrl_params,
req.ctrl_name,
frame_rot,
velocity=req.velocity)
else:
success = self.disable_controller()
return EnableCartControllerResponse(success)
def _create_ell_trajectory(self, ell_f, rot_mat_f, orient_quat=[0., 0., 0., 1.], velocity=0.001):
_, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep())
rpy = trans.euler_from_matrix(cur_rot.T * rot_mat_f) # get roll, pitch, yaw of angle diff
ell_f[1] = np.mod(ell_f[1], 2 * np.pi) # wrap longitude value
# get the current ellipsoidal location of the end effector
ell_init = np.mat(self.get_ell_ep()).T
ell_final = np.mat(ell_f).T
# find the closest longitude angle to interpolate to
if np.fabs(2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi:
ell_final[1,0] += 2 * np.pi
elif np.fabs(-2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi:
ell_final[1,0] -= 2 * np.pi
if np.any(np.isnan(ell_init)) or np.any(np.isnan(ell_final)):
rospy.logerr("[ellipsoid_controller] Nan values in ellipsoid EPs. " +
"ell_init: %f, %f, %f; " % (ell_init[0,0], ell_init[1,0], ell_init[2,0]) +
"ell_final: %f, %f, %f; " % (ell_final[0,0], ell_final[1,0], ell_final[2,0]))
return None
num_samps = np.max([2, int(np.linalg.norm(ell_final - ell_init) / velocity),
int(np.linalg.norm(rpy) / velocity)])
t_vals = min_jerk_traj(num_samps) # makes movement smooth
# smoothly interpolate from init to final
ell_traj = np.array(ell_init) + np.array(np.tile(ell_final - ell_init,
(1, num_samps))) * np.array(t_vals)
ell_frame_mat = self.ell_server.get_ell_frame()
ell_pose_traj = [self.ell_server.robot_ellipsoidal_pose(
ell_traj[0,i], ell_traj[1,i], ell_traj[2,i], orient_quat, ell_frame_mat)
for i in range(ell_traj.shape[1])]
# modify rotation of trajectory
_, ell_init_rot = self.ell_server.robot_ellipsoidal_pose(
ell_init[0,0], ell_init[1,0], ell_init[2,0], orient_quat, ell_frame_mat)
rot_adjust_traj = self.arm.interpolate_ep([np.mat([0]*3).T, cur_rot],
[np.mat([0]*3).T, rot_mat_f],
min_jerk_traj(num_samps))
ell_pose_traj = [(ell_pose_traj[i][0],
ell_pose_traj[i][1] * ell_init_rot.T * rot_adjust_traj[i][1])
for i in range(num_samps)]
return ell_pose_traj
def __init__(self):
# Parse the yaml file to obtain the trajectory
time_step = float(read_parameter('~time_step', 0.05))
points = read_parameter('~points', list())
frame_id = read_parameter('~frame_id', 'base_link')
# Set-up publishers/subscribers
cmd_pub = rospy.Publisher('/grips/ik_command', PoseStamped)
# Give it time to create the publisher
rospy.sleep(2.0)
for point in points:
pose = PoseConv.to_pose_stamped_msg(frame_id, point[4:7], point[:4])
cmd_pub.publish(pose)
try:
rospy.sleep(time_step)
except:
pass
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 add_marker(self, pose, name, color=[1,0,1], type=Marker.SPHERE, scale = [.02,.02,.02], points=None):
vis_pub = rospy.Publisher(name, Marker, queue_size=10)
marker = Marker()
marker.header.frame_id = "world"
marker.header.stamp = rospy.Time()
marker.ns = "my_namespace"
marker.id = 0
marker.type = type
marker.action = Marker.ADD
if type == Marker.LINE_LIST:
for point in points:
p = Point()
p.x = point[0]
p.y = point[1]
p.z = point[2]
marker.points.append(p)
else:
r = self.find_rotation_matrix_between_two_vectors([1,0,0], [0,0,1])
rot = pose[0:3,0:3] * r
pose2 = numpy.matrix(numpy.identity(4))
pose2[0:3,0:3] = rot
pose2[0:3,3] = pose[0:3,3]
quat_pose = PoseConv.to_pos_quat(pose2)
marker.pose.position.x = quat_pose[0][0]
marker.pose.position.y = quat_pose[0][1]
marker.pose.position.z = quat_pose[0][2]
marker.pose.orientation.x = quat_pose[1][0]
marker.pose.orientation.y = quat_pose[1][1]
marker.pose.orientation.z = quat_pose[1][2]
marker.pose.orientation.w = quat_pose[1][3]
marker.scale.x = scale[0]
marker.scale.y = scale[1]
marker.scale.z = scale[2]
marker.color.a = .5
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
vis_pub.publish(marker)
def main():
rospy.init_node("joint_kinematics")
import sys
def usage():
print("Tests for kdl_parser:\n")
print("kdl_parser <urdf file>")
print("\tLoad the URDF from file.")
print("kdl_parser")
print("\tLoad the URDF from the parameter server.")
sys.exit(1)
if len(sys.argv) > 2:
usage()
if len(sys.argv) == 2 and (sys.argv[1] == "-h" or sys.argv[1] == "--help"):
usage()
if (len(sys.argv) == 1):
robot = URDF.load_from_parameter_server(verbose=False)
else:
robot = URDF.load_xml_file(sys.argv[1], verbose=False)
if True:
import random
base_link = robot.get_root()
end_link = robot.links.keys()[random.randint(0, len(robot.links)-1)]
print "Root link: %s; Random end link: %s" % (base_link, end_link)
js_kin = JointKinematics(robot, base_link, end_link)
print "Joint angles:", js_kin.get_joint_angles()
print "Joint angles (wrapped):", js_kin.get_joint_angles(True)
print "Joint velocities:", js_kin.get_joint_velocities()
print "Joint efforts:", js_kin.get_joint_efforts()
print "Jacobian:", js_kin.jacobian()
kdl_pose = js_kin.forward()
print "FK:", kdl_pose
print "End effector force:", js_kin.end_effector_force()
if True:
import tf
from hrl_geom.pose_converter import PoseConv
tf_list = tf.TransformListener()
rospy.sleep(1)
t = tf_list.getLatestCommonTime(base_link, end_link)
tf_pose = PoseConv.to_homo_mat(tf_list.lookupTransform(base_link, end_link, t))
print "Error with TF:", np.linalg.norm(kdl_pose - tf_pose)
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()
