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]