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]
