def interpolate(self, b):
current_model_state = self.get_model_state(model_name="piano2")
quat = Q(current_model_state.pose.orientation.x, current_model_state.pose.orientation.y,\
current_model_state.pose.orientation.z, current_model_state.pose.orientation.w)
a = SE3(current_model_state.pose.position.x, current_model_state.pose.position.y,\
current_model_state.pose.position.z, quat)
x_inc = b.X - a.X
y_inc = b.Y - a.Y
z_inc = b.Z - a.Z
mag = sqrt(x_inc * x_inc + y_inc * y_inc + z_inc * z_inc)
x_inc = x_inc / mag * inc
y_inc = y_inc / mag * inc
z_inc = z_inc / mag * inc
x = a.X
y = a.Y
z = a.Z
q = a.q
cur = a
while SE3.euclid_dist(cur, b) > inc\
or fabs(Q.sym_distance(q, b.q)) > steering_inc:
self.set_position(cur)
self.set_steering_angle(cur.q)
if SE3.euclid_dist(cur, b) > inc:
x += x_inc
y += y_inc
z += z_inc
if Q.sym_distance(q, b.q) > steering_inc:
q = Q.slerp(q, b.q, amount=steering_inc)
cur = SE3(x, y, z, q)
sleep(0.05)
self.set_state(b)
def test_sym_distance(self):
q = Quaternion(scalar=0, vector=[1,0,0])
p = Quaternion(scalar=0, vector=[0,1,0])
self.assertEqual(pi/2, Quaternion.sym_distance(q,p))
q = Quaternion(angle=pi/2, axis=[1,0,0])
p = Quaternion(angle=pi/2, axis=[0,1,0])
self.assertAlmostEqual(pi/3, Quaternion.sym_distance(q,p), places=6)
q = Quaternion(scalar=0, vector=[1,0,0])
p = Quaternion(scalar=0, vector=[0,-1,0])
self.assertEqual(pi/2, Quaternion.sym_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(pi, Quaternion.sym_distance(q,p), places=8)
def move_connection(rwdobj: DetailsForReward):
""" high reward, if finger tips connect to bar. """
dis_traveled = euclidean(rwdobj.new_obj_pos, rwdobj.init_obj_pos)
or_traveled = Quaternion.sym_distance(Quaternion(rwdobj.new_obj_or),
Quaternion(rwdobj.init_obj_or))
# symmetric orientation scaling
or_traveled /= 1000
obj_fell = sum(rwdobj.collision_dict.values()) < 4
stuck = False
if len(rwdobj.tactile_state) > 2:
sames = np.round(rwdobj.current_joint_position,
2) == np.round(rwdobj.current_minus1_joint_position,
2)
stuck = all(sames)
if obj_fell or stuck:
return rwdobj.extrinsic_reward, (obj_fell or stuck)
else:
return np.exp(dis_traveled + or_traveled), obj_fell
def check_collide(a, b):
x_inc = b.X - a.X
y_inc = b.Y - a.Y
z_inc = b.Z - a.Z
mag = sqrt(x_inc * x_inc + y_inc * y_inc + z_inc * z_inc)
x_inc = x_inc / mag * inc
y_inc = y_inc / mag * inc
z_inc = z_inc / mag * inc
x = a.X
y = a.Y
z = a.Z
q = a.q
cur = SE3(x, y, z, q)
T, R = b.get_transition_rotation()
currlerp = 0
if pqp_client(T, R).result:
return False
looping = True
while looping:
looping = False
T, R = cur.get_transition_rotation()
if pqp_client(T, R).result:
# Collision
return False
if SE3.euclid_dist(cur, b) > inc:
looping = True
cur.X += x_inc
cur.Y += y_inc
cur.Z += z_inc
if fabs(Q.sym_distance(cur.q, b.q)) > .1:
looping = True
cur.q = Q.slerp(a.q, b.q, amount=currlerp)
currlerp += steering_inc
T, R = cur.get_transition_rotation()
if pqp_client(T, R).result:
return False
return True
def weighted_distance(a, b):
return Q.sym_distance(a.q, b.q) + 3 * SE3.euclid_dist(a, b)
def distance(a, b):
return Q.sym_distance(a.q, b.q) + SE3.euclid_dist(a, b)
