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)