def _test_jacobian(self, J, theta_vec):
""" Checks a jacobian for correctness. This function compares delta_x1 = J(theta) * delta_theta
and delta_x2 = FK(theta) - FK(theta + delta_theta) for a very small vector delta_theta.
If delta_x1 and delta_x2 are not close, an exception is thrown.
Params:
J: 6x15-matrix: The manipulator jacobian at theta_vec
theta_vec: 1x15-vector: The joint angles for which the Jacobian is valid
"""
theta_vec = np.atleast_2d(theta_vec)
delta_theta_small = np.zeros((self.n_DOFS, 1))
std = 1e-4
delta_theta_small[:, 0] = np.random.normal(loc=0,
scale=std,
size=self.n_DOFS)
old_jc = theta_vec
new_jc = theta_vec + delta_theta_small.T
true_T = self.FK(old_jc)[0]
new_T = self.FK(new_jc)[0]
true_quat = quaternion_from_matrix(true_T[:3, :3])
new_quat = quaternion_from_matrix(new_T[:3, :3])
pos_error = np.sqrt(np.sum(np.square(new_T[:3, 3] - true_T[:3, 3])))
ori_error = 2 * np.arccos(np.abs(np.inner(new_quat, true_quat)))
cart_error_j = np.dot(J, delta_theta_small)
pos_error_j = np.sqrt(np.sum(np.square(cart_error_j[:3])))
ori_error_j = np.sqrt(np.sum(np.square(cart_error_j[3:])))
threshold = 1e-7
assert np.abs(pos_error -
pos_error_j) < threshold, "Jacobian test failed"
assert np.abs(ori_error -
ori_error_j) < threshold, "Jacobian test failed"
def __init__(self, robot, pred_cartesian, pred_joints, true_cartesian,
true_joints):
""" Constructor:
Params:
robot: object of the robot (e.g. class Compi) which contains the relevant robot parameters
pred_cartesian: nx4x4- or nx7-matrix: predicted end effector poses in cartesian space,
either homogenous transformation matrices or positions+quaternions.
In the latter case, the first three columns have to represent the position.
n: number of samples
pred_joints: nxk-matrix: predictions in joint space (rad)
rows: samples
columns: joints
true_cartesian: nx4x4- or nx7-matrix: true end effector poses in cartesian space,
either homogenous transformation matrices or positions+quaternions.
In the latter case, the first three columns have to represent the position.
n: number of samples
true_joints: nxk-matrix: true joint angles (rad)
rows: samples
columns: joints
"""
self.robot = robot
self.pred_joints = pred_joints
self.pred_cartesian = pred_cartesian
self.true_joints = true_joints
self.true_cartesian = true_cartesian
if len(pred_cartesian.shape) == 3:
#transformation matrix was given
#convert it to position + quaternion
pos_quat = np.zeros((pred_cartesian.shape[0], 7))
pos_quat[:, :3] = pred_cartesian[:, :3, 3]
pos_quat[:, 3:] = quaternion_from_matrix(pred_cartesian[:, :3, :3])
self.pred_cartesian = pos_quat
if len(true_cartesian.shape) == 3:
#transformation matrix was given
#convert it to position + quaternion
pos_quat = np.zeros((true_cartesian.shape[0], 7))
pos_quat[:, :3] = true_cartesian[:, :3, 3]
pos_quat[:, 3:] = quaternion_from_matrix(true_cartesian[:, :3, :3])
self.true_cartesian = pos_quat
def min_cart_error(self):
""" Calculates the comparative 3D cartesian error resulting from the given standard deviation in joint space.
Returns:
mean_min_cartesian_errors: 2x1-vector: comparative mean of the euclidean position error
(same unit as robot link lengths) and the orientation error (rad)
median_min_cartesian_errors: 2x1-vector: comparative median of the euclidean position error
(same unit as link_lengths) and the orientation error (rad)
"""
if np.any(np.isnan(self.true_joints)):
return [np.nan, np.nan], [np.nan, np.nan]
cartesian_errors = np.zeros((self.true_joints.shape[0], 6))
eucl_cartesian_errors = np.zeros((self.true_joints.shape[0], 2))
#draw delta_thetas from a gaussian distribution
np.random.seed(0)
mean_error_sum = 0
median_error_sum = 0
num_draws = 1
if self.robot.has_jacobian:
#construct jacobians for given manipulator and joint configurations
J = self.robot.jacobian(self.true_joints, test=False)
for k in range(num_draws):
delta_theta = np.zeros(
(self.true_joints.shape[1], self.true_joints.shape[0]))
for i in range(self.true_joints.shape[1]):
delta_theta[i, :] = np.random.normal(
loc=0,
scale=self.robot.theta_stds[i],
size=(self.true_joints.shape[0]))
if self.robot.has_jacobian:
#delta_x = J * delta_theta
for i in range(self.true_joints.shape[0]):
cartesian_errors[i, :] = np.dot(J[i], delta_theta[:, i])
eucl_cartesian_errors[:, 0] = np.sqrt(
np.sum(np.square(cartesian_errors[:, :3]), axis=1))
#jacobian multiplication leads to an axis*angle representation
#hence, calculate the minimum rotation angle via the magnitude
eucl_cartesian_errors[:, 1] = np.sqrt(
np.sum(np.square(cartesian_errors[:, 3:]), axis=1))
else:
#delta_x = FK(theta+delta_theta) - FK(theta)
SE3_deltatheta = self.robot.FK(self.true_joints +
delta_theta.T)
pos_quat_deltatheta = np.zeros((SE3_deltatheta.shape[0], 7))
pos_quat_deltatheta[:, :3] = SE3_deltatheta[:, :3, 3]
pos_quat_deltatheta[:, 3:] = quaternion_from_matrix(
SE3_deltatheta[:, :3, :3])
eucl_cartesian_errors[:, 0] = self._get_pos_differences(
pos_quat_deltatheta[:, :3], self.true_cartesian[:, :3])
eucl_cartesian_errors[:, 1] = self._get_ori_differences(
pos_quat_deltatheta[:, 3:], self.true_cartesian[:, 3:])
mean_min_cartesian_errors_tmp = np.mean(eucl_cartesian_errors,
axis=0)
median_min_cartesian_errors_tmp = np.median(eucl_cartesian_errors,
axis=0)
mean_error_sum += mean_min_cartesian_errors_tmp
median_error_sum += median_min_cartesian_errors_tmp
mean_min_cartesian_errors = mean_error_sum / num_draws
median_min_cartesian_errors = median_error_sum / num_draws
return mean_min_cartesian_errors, median_min_cartesian_errors
dets = np.linalg.det(J)
args_dets_sorted = np.argsort(np.abs(dets))
val_data_after = val_data_before[
args_dets_sorted[int(args_dets_sorted.shape[0] / 2):]]
joint_configurations = np.vstack(
(joint_configurations[:split], val_data_after))
if not train_sampling == "cartesian":
#transform the samples via FK to cartesian space
SE3 = robot.FK(joint_configurations)
#convert rotation matrices to quaternions
#one sample shall be a pair {7-dim vector(x, y, z, 4-dim-quaternion), n_DOFS-dim vector(joint angles)}
pos_quat = np.zeros((joint_configurations.shape[0], 7))
pos_quat[:, :3] = SE3[:, :3, 3]
pos_quat[:, 3:] = quaternion_from_matrix(SE3[:, :3, :3])
#if specified, train on one half of the workspace, test on the other
if use_hold_out_area:
train_bools = pos_quat[:, 0] > 0
assert np.sum(
train_bools
) > split, "Not enough samples to extract the desired number of training samples. Increase the number of samples."
#extract training data
pos_quat_train = pos_quat[train_bools]
pos_quat_train = pos_quat_train[:split]
SE3_train = SE3[train_bools]
SE3_train = SE3_train[:split]
joint_configurations_train = joint_configurations[train_bools]
joint_configurations_train = joint_configurations_train[:split]