def move_object(rwdobj: DetailsForReward): """move object to a position""" distance_pos = euclidean(rwdobj.new_obj_pos, rwdobj.goal_pos_orientation.obj_pos) # norm positional distance distance_pos = distance_pos / euclidean( rwdobj.init_obj_pos, rwdobj.goal_pos_orientation.obj_pos ) # (x-min) / ( max-min)-> normalize form 0..1, min=0 distance_or = Quaternion.distance( Quaternion(rwdobj.new_obj_or), Quaternion(rwdobj.goal_pos_orientation.obj_or)) # norm orientational distance distance_or = distance_or / Quaternion.distance( Quaternion(rwdobj.init_obj_or), Quaternion(rwdobj.goal_pos_orientation.obj_or)) grip_strength = np.round(np.sum(np.abs(rwdobj.tactile_state[-1])), 2) obj_fell = grip_strength < 0.01 * rwdobj.action_space reached = (distance_pos < 0.1) and (distance_or < 0.1) if reached: print("reached") # print(distance_or, distance_pos, reached) if obj_fell: return rwdobj.extrinsic_reward, True # reward finish else: assert distance_pos >= 0, "positional distance is negative." assert distance_or >= 0, "Quaternion distance is negative." return 1 / np.exp(distance_pos + distance_or), reached
def move_connection_to_object(rwdobj: DetailsForReward): """move object to a position""" distance_pos = euclidean(rwdobj.new_obj_pos, rwdobj.goal_pos_orientation.obj_pos) # norm positional distance distance_pos = distance_pos / euclidean( rwdobj.init_obj_pos, rwdobj.goal_pos_orientation.obj_pos ) # (x-min) / ( max-min)-> normalize form 0..1, min=0 distance_or = Quaternion.distance( Quaternion(rwdobj.new_obj_or), Quaternion(rwdobj.goal_pos_orientation.obj_or)) # norm orientational distance distance_or = distance_or / Quaternion.distance( Quaternion(rwdobj.init_obj_or), Quaternion(rwdobj.goal_pos_orientation.obj_or)) num_fingers_connected, obj_fell = for_weighted_connect(rwdobj) reached = (distance_pos < 0.01) and (distance_or < 0.01) if reached: print("reached") if obj_fell: return -1, True # reward finish else: assert distance_pos >= 0, "positional distance is negative." assert distance_or >= 0, "Quaternion distance is negative." weighted_distance = num_fingers_connected / np.exp(distance_pos + distance_or) return weighted_distance, reached
def test_distance(self): q = Quaternion(scalar=0, vector=[1,0,0]) p = Quaternion(scalar=0, vector=[0,1,0]) self.assertEqual(pi/2, Quaternion.distance(q,p)) q = Quaternion(angle=pi/2, axis=[1,0,0]) p = Quaternion(angle=pi/2, axis=[0,1,0]) self.assertEqual(pi/3, Quaternion.distance(q,p)) q = Quaternion(scalar=1, vector=[1,1,1]) p = Quaternion(scalar=-1, vector=[-1,-1,-1]) p._normalise() q._normalise() self.assertAlmostEqual(0, Quaternion.distance(q,p), places=8)
def potential(self): # check if goal exists if self.goal_quat is None: return 0 # compute difference between current and goal pose diff_pos = self.curr_pos - self.curr_pos # no difference diff_quat = T.quat_multiply(T.quat_inverse(self.curr_quat), self.goal_quat) diff = np.hstack([diff_pos, diff_quat]) # compute distance to goal dist_pos = np.linalg.norm(diff[:3]) # no difference dist_quat = Quaternion.distance(Quaternion(self.curr_quat), Quaternion(self.goal_quat)) dist_quat = min(dist_quat, math.pi - dist_quat) dist = dist_pos + dist_quat # compute potential pot = 0.5 * dist * dist if self.verbose or ((not self.suppress_output) and ((self.num_iters % self.num_iters_print) == 0)): print("BaxterRotationController: potential %f" % pot) if self.verbose: print( "BaxterRotationController: position distance %f, rotation distance %f" % (dist_pos, dist_quat)) return min(pot, self.max_potential)
def sort_by_minimum_Qdistances(rotation_tuples, reference_vector): """ Sort a list of quaternion rotation tuples. By its distance to a ``reference_vector``. Designed to receive the output of :py:func:`generate_quaternion_rotations`. If you use this function you should cite `PyQuaternion package <http://kieranwynn.github.io/pyquaternion/>`_. Parameters ---------- rotation_tuples : list of tuples List of tuples containing Rotation quaternions and resulting rotated vector. In other words, the output from :py:func:`generate_quaternion_rotations`. reference_vector : tuple, list or array-like The XYZ 3D coordinates of the reference vector. The quaternion distance between vectors in ``rotation_tuples`` and this vector will be computed. Returns ------- list The list sorted according to the creterion. """ ref_u = Q(vector=reference_vector).unit minimum = sorted( rotation_tuples, key=lambda x: math.degrees(Q.distance(x[1], ref_u)) % 360, ) return minimum
def extract_angular_displacement(head_pose_initial, head_pose_final, **options): # return length of arc between q1 and q2 # return PyQuaternion.distance(q1, q2) if options.get("argument") == "euler angle": return abs(head_pose_final[1] - head_pose_initial[1]) # function is adaptable to both quaternions and euler angles as input # just need to specify (ex. extract_angular_displacement(.., .., argument=" ")) if options.get("argument") == "quaternion": return PyQuaternion.distance(head_pose_initial, head_pose_final)
def or_dist(rwdobj: DetailsForReward): """Take orientation as distance metric. Reason: position can easily be manipulated by the hands position.""" distance_or = Quaternion.distance(Quaternion(rwdobj.new_obj_or), Quaternion(rwdobj.init_obj_or)) obj_fell = not all_action_fingers_have_contact(rwdobj) if obj_fell: return rwdobj.extrinsic_reward, obj_fell else: distance = 1 / np.exp(distance_or) return distance, obj_fell
def weighted_or_dist(rwdobj: DetailsForReward): """Take orientation as distance metric. Reason: position can easily be manipulated by the hands position.""" distance_or = Quaternion.distance(Quaternion(rwdobj.new_obj_or), Quaternion(rwdobj.init_obj_or)) num_fingers_connected, obj_fell = for_weighted_connect(rwdobj) if obj_fell: return rwdobj.extrinsic_reward, obj_fell else: weighted_distance = num_fingers_connected / np.exp(distance_or) return weighted_distance, obj_fell
def calculateRotMask(self, rot_thr=np.pi / 3.0): both_have_rot = self.image_have_rot[:, None] * self.image_have_rot rot_mask = np.logical_not(both_have_rot) rot_idx = np.where(both_have_rot) for idx in range(0, len(rot_idx[0])): id1 = rot_idx[0][idx] id2 = rot_idx[1][idx] rot1 = self.image_rot[id1] rot2 = self.image_rot[id2] dist = Quaternion.distance(rot1, rot2) if dist < rot_thr: rot_mask[id1, id2] = True return rot_mask.reshape(rot_mask.shape[0], rot_mask.shape[0])
def weiszfeld_interpolation(quaternions, iterations=1000): y = Quaternion(quaternions[0].tolist()) for i in range(iterations): newy = Quaternion() dividend = 0 for j in range(1, len(quaternions)): distance = Quaternion.distance(Quaternion(quaternions[j]), y) if distance < 1e-4: continue newy = newy + quaternions[j] / distance dividend += 1 / distance newy /= dividend y = newy return y
def calculateDeltaPose(quaternionLocalPose, quaternionPriorPose): # Finds the length of the chord of the shortest arc. We don't want this. #deltaPoseChord = Quaternion.absolute_distance( quaternionLocalPose, quaternionPriorPose ) # Finds the length of the geodesic curve between poses # Returns positive value deltaPoseGeodesic = Quaternion.distance(quaternionLocalPose, quaternionPriorPose) # Finds the length of the geodesic curve between poses #deltaPoseSym = Quaternion.sym_distance( quaternionLocalPose, quaternionPriorPose ) return deltaPoseGeodesic
def compute_metrics(self, zs_pos, zs_rot, preds_pos, preds_rot): # Compute Eucliden and angular distances self.euc_dists = np.linalg.norm(zs_pos - preds_pos, axis=1) self.ang_dists = np.array( [Quaternion.distance(q1, q2) for q1, q2 in zip(zs_rot, preds_rot)]) # Mean Absolute Error (MAE) self.metrics['mae_euc'] = np.sum( self.euc_dists) / self.euc_dists.shape[0] logging.info("MAE position = %s", self.metrics['mae_euc']) self.metrics['mae_ang'] = np.rad2deg( np.sum(self.ang_dists) / self.ang_dists.shape[0]) logging.info("MAE rotation = %s", self.metrics['mae_ang']) # Root Mean Squared Error (RMSE) self.metrics['rmse_euc'] = np.sqrt((self.euc_dists**2).mean()) logging.info("RMSE position = %s", self.metrics['rmse_euc']) self.metrics['rmse_ang'] = np.rad2deg( np.sqrt((self.ang_dists**2).mean())) logging.info("RMSE rotation = %s", self.metrics['rmse_ang'])
def potential(self): # compute difference between current and goal pose diff_pos = self.object_curr_pos - self.object_goal_pos diff_quat = T.quat_multiply(T.quat_inverse(self.object_curr_quat), self.object_goal_quat) diff = np.hstack([diff_pos, diff_quat]) # compute distance to goal dist_pos = np.linalg.norm(diff[:3]) dist_quat = Quaternion.distance(Quaternion(self.object_curr_quat), Quaternion(self.object_goal_quat)) dist = dist_pos + dist_quat # compute potential pot = 0.5 * dist * dist print("BaxterObject6DPoseController: potential %f" % pot) if True: #self.verbose: # TODO print( "BaxterObject6DPoseController: position distance %f, rotation distance %f" % (dist_pos, dist_quat)) return min(pot, self.max_potential)
def rotation(): go_to(True) target = [ -0.09108, -0.51868, 0.47657, -0.49215, -0.48348, 0.51335, -0.5104 ] target_quaternion = Quaternion(np.roll(target[3:], 1)) current_quaternion = arm.end_effector()[3:] from_quaternion = Quaternion(np.roll(current_quaternion, 1)) rotate = Quaternion(axis=[1, 0, 0], degrees=00.0) to_quaternion = from_quaternion * rotate # rotate = Quaternion(axis=[0,1,0], degrees=-50.0) # to_quaternion *= rotate rotate = Quaternion(axis=[0, 0, 1], degrees=50.0) to_quaternion *= rotate old_q = from_quaternion for q in Quaternion.intermediates(q0=from_quaternion, q1=target_quaternion, n=10): w = transformations.angular_velocity_from_quaternions( old_q, q, 1.0 / 10.0) old_q = q delta = np.concatenate((np.zeros(3), w.vector)) to_pose = transformations.pose_euler_to_quaternion(arm.end_effector(), delta, dt=1.0 / 10.0) arm.set_target_pose_flex(to_pose, t=1.0 / 10.0) rospy.sleep(1.0 / 10.0) dist = Quaternion.distance(target_quaternion, Quaternion(np.roll(arm.end_effector()[3:], 1))) print(dist)
# set default orientation to be normal previous_orientation = Quaternion(1, 0, 0, 0) for index, row in data.iterrows(): # create objects for orientation and inverse orientation orientation = Quaternion(row["orientation_w"], row["orientation_x"], row["orientation_y"], row["orientation_z"]) inverse_orientation = orientation.inverse data.at[index, "inverse_orientation_w"] = inverse_orientation.w data.at[index, "inverse_orientation_x"] = inverse_orientation.x data.at[index, "inverse_orientation_y"] = inverse_orientation.y data.at[index, "inverse_orientation_z"] = inverse_orientation.z # determine if the orientation is assumed to be flipped data.at[index, "is_flipped"] = Quaternion.distance( inverse_orientation, previous_orientation) < 0.05 # set iteration values if (data.at[index, "is_flipped"]): orientation = inverse_orientation previous_orientation = orientation data.at[index, "corrected_orientation_w"] = orientation.w data.at[index, "corrected_orientation_x"] = orientation.x data.at[index, "corrected_orientation_y"] = orientation.y data.at[index, "corrected_orientation_z"] = orientation.z filename_elements = filename.split(".") new_filename = filename_elements[0] + "_fixed.csv" data.to_csv(new_filename)
def distanceBB(box1, box2): eucl = np.linalg.norm(box1.center - box2.center) angl = Quaternion.distance(Quaternion(matrix=box1.rotation_matrix), Quaternion(matrix=box2.rotation_matrix)) return eucl + angl
def distance_to(self, other): return pyQuaternion.distance(self, other)
def process(self, df, participant=None, video=None): print("Process LowPassFilter", participant, video) df = df.sort_values(by="time") ## get all sensors sensors = list( set( df.columns.map(lambda row: row if row == "time" else (row.split("_")[0])))) sensors.remove("time") low = max(min(self.hbias - (self.hrange / 2), 1), 0) high = max(min(self.hbias + (self.hrange / 2), 1), 0) hrangeLimited = high - low y = {} result = {} for sensor in sensors: yrow = df.iloc[[0]][[ "%s_orientation_q_w" % sensor, "%s_orientation_q_x" % sensor, "%s_orientation_q_y" % sensor, "%s_orientation_q_z" % sensor ]] y[sensor] = Quaternion(yrow["%s_orientation_q_w" % sensor], yrow["%s_orientation_q_x" % sensor], yrow["%s_orientation_q_y" % sensor], yrow["%s_orientation_q_z" % sensor]) if y[sensor].norm == 0.0: y[sensor] = Quaternion(1, 0, 0, 0) if sensor not in result: result[sensor] = [] result[sensor].append(list(y[sensor])) for i in range(1, len(df)): rowresult = [] for sensor in sensors: xrow = df.iloc[[i]][[ "%s_orientation_q_w" % sensor, "%s_orientation_q_x" % sensor, "%s_orientation_q_y" % sensor, "%s_orientation_q_z" % sensor ]] x = Quaternion(xrow["%s_orientation_q_w" % sensor], xrow["%s_orientation_q_x" % sensor], xrow["%s_orientation_q_y" % sensor], xrow["%s_orientation_q_z" % sensor]) if x.norm == 0.0: x = Quaternion(1, 0, 0, 0) d = Quaternion.distance(y[sensor], x) hlpf = d / math.pi * hrangeLimited + low y[sensor] = Quaternion.slerp(y[sensor], x, amount=hlpf) result[sensor].append(list(y[sensor])) dfs = [] dfs.append(pd.DataFrame({"time": df.time})) for sensor in sensors: newdf = pd.DataFrame(result[sensor], columns=[ "%s_orientation_q_%s" % (sensor, s) for s in list("wxyz") ]) dfs.append(newdf) newdf = pd.concat(dfs, axis=1, join='inner') return newdf
axs[1].plot(t, result_quat_array[:, 1], label='DMP') #axs[1].set_xlabel('t (s)') axs[1].set_ylabel('i') axs[2].plot(t, demo_quat_array[:, 2], label='Demonstration') axs[2].plot(t, result_quat_array[:, 2], label='DMP') #axs[2].set_xlabel('t (s)') axs[2].set_ylabel('j') axs[3].plot(t, demo_quat_array[:, 3], label='Demonstration') axs[3].plot(t, result_quat_array[:, 3], label='DMP') #axs[3].set_xlabel('t (s)') axs[3].set_ylabel('k') quat_error = [ Quaternion.distance(Quaternion(q1), Quaternion(q2)) for (q1, q2) in zip(demo_quat_array, result_quat_array) ] axs[4].plot(t, quat_error, label='Error', c='r') axs[4].set_xlabel('t (s)') axs[4].set_ylabel('e') axs[4].legend() def update(i, x, y, z, x2, y2, z2): x_data = Quaternion(demo_quat_array[i]).conjugate * Quaternion( [0, 1, 0, 0]) * demo_quat_array[i] x.set_data([0, x_data[1]], [0, x_data[2]]) x.set_3d_properties([0, x_data[3]]) x_data = Quaternion(result_quat_array[i]).conjugate * Quaternion( [0, 1, 0, 0]) * result_quat_array[i]