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 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 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 _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 _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 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 inverse_biased(self, pose, q_init, q_bias, q_bias_weights, rot_weight=1.0, 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 _make_generic(args):
try:
if type(args[0]) == str:
frame_id = args[0]
header, homo_mat, rot_quat, rot_euler = PoseConv._make_generic(
args[1:])
if homo_mat is None:
return None, None, None, None
if header is None:
header = [0, rospy.Time.now(), '']
header[2] = frame_id
return header, homo_mat, rot_quat, rot_euler
if len(args) == 2:
return PoseConv._make_generic(((args[0], args[1]), ))
elif len(args) == 1:
pose_type = PoseConv.get_type(*args)
if pose_type is None:
return None, None, None, None
if pose_type == 'pose_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_pose_msg(
args[0])
return None, homo_mat, rot_quat, rot_euler
elif pose_type == 'pose_stamped_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_pose_msg(
args[0].pose)
seq = args[0].header.seq
stamp = args[0].header.stamp
frame_id = args[0].header.frame_id
return [seq, stamp,
frame_id], homo_mat, rot_quat, rot_euler
elif pose_type == 'tf_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_tf_msg(
args[0])
return None, homo_mat, rot_quat, rot_euler
elif pose_type == 'tf_stamped_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_tf_msg(
args[0].transform)
seq = args[0].header.seq
stamp = args[0].header.stamp
frame_id = args[0].header.frame_id
return [seq, stamp,
frame_id], homo_mat, rot_quat, rot_euler
elif pose_type == 'point_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_point_msg(
args[0])
return None, homo_mat, rot_quat, rot_euler
elif pose_type == 'point_stamped_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_point_msg(
args[0].point)
seq = args[0].header.seq
stamp = args[0].header.stamp
frame_id = args[0].header.frame_id
return [seq, stamp,
frame_id], homo_mat, rot_quat, rot_euler
elif pose_type == 'twist_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_twist_msg(
args[0])
return None, homo_mat, rot_quat, rot_euler
elif pose_type == 'twist_stamped_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_twist_msg(
args[0].twist)
seq = args[0].header.seq
stamp = args[0].header.stamp
frame_id = args[0].header.frame_id
return [seq, stamp,
frame_id], homo_mat, rot_quat, rot_euler
elif pose_type == 'homo_mat':
return (None, np.mat(args[0]),
trans.quaternion_from_matrix(args[0]).tolist(),
trans.euler_from_matrix(args[0]))
elif pose_type in [
'pos_rot', 'pos_euler', 'pos_quat', 'pos_axis_angle'
]:
pos_arg = np.mat(args[0][0])
if pos_arg.shape == (1, 3):
# matrix is row, convert to column
pos = pos_arg.T
elif pos_arg.shape == (3, 1):
pos = pos_arg
if pose_type == 'pos_axis_angle':
homo_mat = np.mat(np.eye(4))
homo_mat[:3, :3] = axis_angle_to_rot_mat(
args[0][1][0], args[0][1][1])
quat = trans.quaternion_from_matrix(homo_mat)
rot_euler = trans.euler_from_matrix(homo_mat)
elif pose_type == 'pos_rot':
# rotation matrix
homo_mat = np.mat(np.eye(4))
homo_mat[:3, :3] = np.mat(args[0][1])
quat = trans.quaternion_from_matrix(homo_mat)
rot_euler = trans.euler_from_matrix(homo_mat)
else:
rot_arg = np.mat(args[0][1])
if rot_arg.shape[1] == 1:
rot_arg = rot_arg.T
rot_list = rot_arg.tolist()[0]
if pose_type == 'pos_euler':
# Euler angles rotation
homo_mat = np.mat(trans.euler_matrix(*rot_list))
quat = trans.quaternion_from_euler(*rot_list)
rot_euler = rot_list
elif pose_type == 'pos_quat':
# quaternion rotation
homo_mat = np.mat(
trans.quaternion_matrix(rot_list))
quat = rot_list
rot_euler = trans.euler_from_quaternion(quat)
homo_mat[:3, 3] = pos
return None, homo_mat, np.array(quat), rot_euler
except:
pass
return None, None, None, None
def homo_mat_to_2d(mat):
rot = trans.euler_from_matrix(mat)[2]
return mat[0,3], mat[1,3], rot
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 _make_generic(args):
try:
if type(args[0]) == str:
frame_id = args[0]
header, homo_mat, rot_quat, rot_euler = PoseConv._make_generic(args[1:])
if homo_mat is None:
return None, None, None, None
if header is None:
header = [0, rospy.Time.now(), '']
header[2] = frame_id
return header, homo_mat, rot_quat, rot_euler
if len(args) == 2:
return PoseConv._make_generic(((args[0], args[1]),))
elif len(args) == 1:
pose_type = PoseConv.get_type(*args)
if pose_type is None:
return None, None, None, None
if pose_type == 'pose_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_pose_msg(args[0])
return None, homo_mat, rot_quat, rot_euler
elif pose_type == 'pose_stamped_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_pose_msg(args[0].pose)
seq = args[0].header.seq
stamp = args[0].header.stamp
frame_id = args[0].header.frame_id
return [seq, stamp, frame_id], homo_mat, rot_quat, rot_euler
elif pose_type == 'tf_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_tf_msg(args[0])
return None, homo_mat, rot_quat, rot_euler
elif pose_type == 'tf_stamped_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_tf_msg(args[0].transform)
seq = args[0].header.seq
stamp = args[0].header.stamp
frame_id = args[0].header.frame_id
return [seq, stamp, frame_id], homo_mat, rot_quat, rot_euler
elif pose_type == 'point_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_point_msg(args[0])
return None, homo_mat, rot_quat, rot_euler
elif pose_type == 'point_stamped_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_point_msg(args[0].point)
seq = args[0].header.seq
stamp = args[0].header.stamp
frame_id = args[0].header.frame_id
return [seq, stamp, frame_id], homo_mat, rot_quat, rot_euler
elif pose_type == 'twist_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_twist_msg(args[0])
return None, homo_mat, rot_quat, rot_euler
elif pose_type == 'twist_stamped_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_twist_msg(args[0].twist)
seq = args[0].header.seq
stamp = args[0].header.stamp
frame_id = args[0].header.frame_id
return [seq, stamp, frame_id], homo_mat, rot_quat, rot_euler
elif pose_type == 'homo_mat':
return (None, np.mat(args[0]), trans.quaternion_from_matrix(args[0]).tolist(),
trans.euler_from_matrix(args[0]))
elif pose_type in ['pos_rot', 'pos_euler', 'pos_quat']:
pos_arg = np.mat(args[0][0])
rot_arg = np.mat(args[0][1])
if pos_arg.shape == (1, 3):
# matrix is row, convert to column
pos = pos_arg.T
elif pos_arg.shape == (3, 1):
pos = pos_arg
if pose_type == 'pos_rot':
# rotation matrix
homo_mat = np.mat(np.eye(4))
homo_mat[:3,:3] = rot_arg
quat = trans.quaternion_from_matrix(homo_mat)
rot_euler = trans.euler_from_matrix(homo_mat)
else:
if rot_arg.shape[1] == 1:
rot_arg = rot_arg.T
rot_list = rot_arg.tolist()[0]
if pose_type == 'pos_euler':
# Euler angles rotation
homo_mat = np.mat(trans.euler_matrix(*rot_list))
quat = trans.quaternion_from_euler(*rot_list)
rot_euler = rot_list
elif pose_type == 'pos_quat':
# quaternion rotation
homo_mat = np.mat(trans.quaternion_matrix(rot_list))
quat = rot_list
rot_euler = trans.euler_from_quaternion(quat)
homo_mat[:3, 3] = pos
return None, homo_mat, np.array(quat), rot_euler
except:
pass
return None, None, None, None
