def make_obs(self): ''' State vector of length 17 contains two poses, and length,width and height of object bb ''' state = None if self.state_rep == "state": objs = self.machine.get_objects(True) gripper_pose = list( self.machine.env._scene.get_observation().gripper_pose) gripper_quat = gripper_pose[3:] gripper_quat[0], gripper_quat[-1] = gripper_quat[-1], gripper_quat[ 0] quat = quaternion(*gripper_quat) g_vec = list(as_rotation_vector(quat)) obj_pose = list(objs[OBJECT].get_pose()) obj_quat = obj_pose[3:] obj_quat[0], obj_quat[-1] = obj_quat[-1], obj_quat[0] quat = quaternion(*obj_quat) r_vec = list(as_rotation_vector(quat)) obj_bb = objs[OBJECT].get_bounding_box() x_diff = obj_bb[0] - obj_bb[3] y_diff = obj_bb[1] - obj_bb[4] z_diff = obj_bb[2] - obj_bb[5] state = np.array(gripper_pose[:3]+g_vec+ \ obj_pose[:3]+r_vec+[x_diff,y_diff,z_diff]).reshape(1,-1) elif self.state_rep == "vision": obs = self.machine.env._scene.get_observation() state = obs.wrist_rgb # plt.imsave("image.png",state) return state
def st_induced_axial_rot_swing_twist(st: PoseTrajectory, ht: PoseTrajectory) -> np.ndarray: """Compute ST-induced HT axial rotation using swing-twist decomposition.""" num_frames = st.rot_matrix.shape[0] # rotational difference between frames expressed in torso coordinate system st_diff = st.quat[1:] * np.conjugate(st.quat[:-1]) induced_axial_rot_delta = np.empty((num_frames, 4), dtype=np.float) induced_axial_rot_delta[0, :] = 0 for i in range(num_frames - 1): hum_axis = ht.rot_matrix[i, :, 1] # this computes the induced axial rotation from the ST rotation in its entirety rot_vec = q.as_rotation_vector(quat_project(st_diff[i], hum_axis)) rot_vec_theta = np.linalg.norm(rot_vec) rot_vec_axis = rot_vec / rot_vec_theta # Note that rot_vec_theta will always be + because of np.linalg.norm. But a rotation about an axis v by an angle # theta is the same as a rotation about -v by an angle -theta. So here the humeral axis sets our direction. That # is, we always rotate around hum_axis (and not -hum_axis) and adjust the sign of rot_vec_theta accordingly induced_axial_rot_delta[i + 1, 3] = rot_vec_theta * ( 1 if np.dot(rot_vec_axis, hum_axis) > 0 else -1) # this computes it for each individual axis of the scapula for j in range(3): # first project the scapula rotation onto one of its axis st_axis_proj = quat_project(st_diff[i], st.rot_matrix[i, :, j]) # then proceed as above rot_vec = q.as_rotation_vector(quat_project( st_axis_proj, hum_axis)) rot_vec_theta = np.linalg.norm(rot_vec) rot_vec_axis = rot_vec / rot_vec_theta induced_axial_rot_delta[i + 1, j] = rot_vec_theta * ( 1 if np.dot(rot_vec_axis, hum_axis) > 0 else -1) return np.add.accumulate(induced_axial_rot_delta)
def get_obs(self, get_space=False): """ Get observation of state for RL. If get_space is true return space description. Space is defined as: 0: time [1] 1:7 hand pos + rotation vector [6] 7: gripper state [1] 8:14 obj pos + rotation vector [6], 14: obj height 15: obj radius In case of two objects: 16:22 obj pos + rotation vector [6], 22: obj height 23: obj radius """ if get_space: if self.two_objects: return FloatBox(low=[0.] + [-1.] * 6 + [0.] + [-1.] * 6 + [0.] * 2 + [-1.] * 6 + [0.] * 2, high=[1.] + [1.] * 6 + [self.FINGER_OPEN_POS] + [1.] * 6 + [0.2] * 2 + [1.] * 6 + [0.2] * 2) return FloatBox(low=[0.] + [-1.] * 6 + [0.] + [-1.] * 6 + [0.] * 2, high=[1.] + [1.] * 6 + [self.FINGER_OPEN_POS] + [1.] * 6 + [0.2] * 2) t = self.scene.simulation_time hand_pose = self.hand_actors[0].get_global_pose() grip = self.get_grip() obj_pose = self.obj.get_global_pose() if self.two_objects: obj2_pose = self.obj2.get_global_pose() return np.concatenate([ [t / 10.], hand_pose[0], npq.as_rotation_vector(hand_pose[1]), [grip], obj_pose[0], npq.as_rotation_vector(obj_pose[1]), [self.obj_height, self.obj_radius], obj2_pose[0], npq.as_rotation_vector(obj2_pose[1]), [self.obj2_height, self.obj2_radius], ]).astype(np.float32) return np.concatenate([ [t / 10.], hand_pose[0], npq.as_rotation_vector(hand_pose[1]), [grip], obj_pose[0], npq.as_rotation_vector(obj_pose[1]), [self.obj_height, self.obj_radius], ]).astype(np.float32)
def load_eval_data(eval_data_path, data_shape='asus'): """Read the data from the real world stored in jpgs""" imgs = [] labels = [] pickle_path = os.path.join(eval_data_path, "ep_data.pkl") with open(pickle_path, "rb") as f: context = pickle.load(f) files = sorted(os.listdir(eval_data_path)) obj_world_pose = context["obj_world_pose"] obj_world_pos = obj_world_pose[:3] obj_world_quat = quaternion.as_quat_array(obj_world_pose[3:]) zrot = quaternion.as_rotation_vector(obj_world_quat)[-1] pose = np.zeros((3,)) pose[:2] = obj_world_pos[:2] pose[2] = zrot for f in files: if not f.endswith('.png'): continue labels.append(pose.copy()) filename = os.path.join(eval_data_path, f) img = plt.imread(filename) if data_shape == 'asus': img = preproc_image(img, dtype=np.float32) # plt.imshow(img); plt.show() # img = (img / 127.5) - 1.0 imgs.append(img) return np.array(imgs, dtype=np.float32), np.array(labels, dtype=np.float32)
def queue_handler(self): try: command = self.queue.get(block=False) if command['type'] is 'ack': if self.state is self.State.connecting: self.state = self.State.connected logging.info('Got ack - connected') if command['type'] is 'rx': orientation = command['quat'] if self.state is not self.State.disconnected and self.state is not self.State.quitting: self.update_status(command['fingers'], orientation.w, orientation.x, orientation.y, orientation.z, command['freq']) fingers = command['fingers'] hand_id = fingers[0] * 0b1000 + fingers[ 1] * 0b0100 + fingers[2] * 0b0010 + fingers[3] * 0b0001 if hand_id != self.last_hand_id: self.hand_display.create_image( (0, 0), image=self.hand_icons[hand_id], anchor=N + W) self.last_hand_id = hand_id if self.state is self.State.recording: roll, yaw, pitch = quaternion.as_rotation_vector( orientation) x_coord = np.tan(pitch) * 125 + 125 y_coord = np.tan(yaw) * -125 + 125 logging.info('Rendering ({}, {})'.format(x_coord, y_coord)) if self.last_coordinate_visualized: logging.info(self.last_coordinate_visualized) self.path_display.create_line( self.last_coordinate_visualized[0], self.last_coordinate_visualized[1], x_coord, y_coord, width=2, fill='blue') self.last_coordinate_visualized = [x_coord, y_coord] self.path_display.create_oval( (x_coord - 2, y_coord - 2, x_coord + 2, y_coord + 2), fill='SeaGreen1') if command['type'] is 'quit': self.master.destroy() return except Empty: pass self.master.after(10, self.queue_handler)
def load_sample_sequence(self, filename): if filename.endswith('.pkl'): # This is a pickle file that contains a motion sequence stored in angle-axis format sample_sequence = pkl.load(open(filename, 'rb')) poses = np.array(sample_sequence) elif filename.endswith('.npz'): # This is a numpy file that was produced using the evaluation code preds = np.load(filename)['prediction'] # Choose the first sample in the file to be displayed pred = preds[1] # This is in rotation matrix format, so convert to angle-axis poses = np.reshape(pred, [pred.shape[0], -1, 3, 3]) poses = quaternion.as_rotation_vector( quaternion.from_rotation_matrix(poses)) poses = np.reshape(poses, [poses.shape[0], -1]) else: raise ValueError( "do not recognize file format of file {}".format(filename)) # Unity can also display orientations and accelerations, but they are not available here oris = np.zeros([poses.shape[0], 6 * 3]) accs = np.zeros([poses.shape[0], 6 * 3]) self.sample_sequence = {'gt': poses, 'ori': oris, 'acc': accs} self.next_frame = 0
def test_avg(self): # distribute a bunch of rotation around I, # such that the average should be I: # at random axis but constant angle, nb_rotations = 1000 angles = np.linspace(0.0, np.deg2rad(45), 100) averages_q = [] np.random.seed(1) for angle in angles: rotation_axis = np.random.uniform(-1., 1., size=(nb_rotations, 3)) rotation_axis /= np.linalg.norm(rotation_axis, axis=1, keepdims=True) rotation_axis *= angle rotation_qs = quaternion.from_rotation_vector(rotation_axis) rotation_qs = quaternion.as_float_array(rotation_qs) # print(rotation_qs) r = average_quaternion(rotation_qs) averages_q.append(r.tolist()) averages_q = np.array(averages_q) # normalizes rotation (resolve the opposite ambiguity) averages_q *= np.sign(averages_q[:, 0]).reshape((-1, 1)) # convert quaternions to angle-axis representation averages_q = quaternion.from_float_array(averages_q) averages_v = quaternion.as_rotation_vector(averages_q) # retrieve angle amplitudes. errors_rad = np.linalg.norm(averages_v, axis=1) # # plot for debug # import matplotlib.pyplot as plt # plt.plot(np.rad2deg(errors_rad), 'x', label='Errors (in deg.) as a function of angle amplitude (in deg.)') # plt.show() maximum_error_deg = np.rad2deg(np.max(errors_rad)) self.assertLess(maximum_error_deg, 3.6)
def st_induced_axial_rot_fha(st: PoseTrajectory, ht: PoseTrajectory) -> np.ndarray: """Compute ST-induced HT axial rotation using finite helical axes.""" num_frames = st.rot_matrix.shape[0] st_diff = st.quat[1:] * np.conjugate(st.quat[:-1]) st_fha = q.as_rotation_vector(st_diff) st_fha_x = ( extended_dot(st_fha, st.rot_matrix[:-1, :, 0])[..., np.newaxis] * st.rot_matrix[:-1, :, 0]) st_fha_y = ( extended_dot(st_fha, st.rot_matrix[:-1, :, 1])[..., np.newaxis] * st.rot_matrix[:-1, :, 1]) st_fha_z = ( extended_dot(st_fha, st.rot_matrix[:-1, :, 2])[..., np.newaxis] * st.rot_matrix[:-1, :, 2]) st_fha_proj_x = extended_dot(st_fha_x, ht.rot_matrix[:-1, :, 1]) st_fha_proj_y = extended_dot(st_fha_y, ht.rot_matrix[:-1, :, 1]) st_fha_proj_z = extended_dot(st_fha_z, ht.rot_matrix[:-1, :, 1]) st_fha_proj = extended_dot(st_fha, ht.rot_matrix[:-1, :, 1]) induced_axial_rot = np.empty((num_frames, 4), dtype=np.float) induced_axial_rot[0, :] = 0 induced_axial_rot[1:, 0] = np.add.accumulate(st_fha_proj_x) induced_axial_rot[1:, 1] = np.add.accumulate(st_fha_proj_y) induced_axial_rot[1:, 2] = np.add.accumulate(st_fha_proj_z) induced_axial_rot[1:, 3] = np.add.accumulate(st_fha_proj) return induced_axial_rot
def arr_quat_to_rots(quats): ret = q.as_rotation_vector(q.as_quat_array(quats)) ret = ret.reshape(-1, 3) ret = ret[:, 1] - 3.14 ret[np.where(ret > 3.14)] -= 6.28 ret[np.where(ret < -3.14)] += 6.28 return ret
def mySlerp(t, q0, qf): teta = np.linalg.norm(quaternion.as_rotation_vector((q0.conjugate() * qf))) t0 = 0 tf = 10 dt = (t - t0) / (tf - t0) quat_slerp = (sin(teta / 2 * (1 - dt)) * q0 + sin(teta / 2 * dt) * qf) / sin(teta / 2) return quat_slerp
def rot_matrix_to_axan(data): """ Converts rotation matrices to axis angles :param data: Rotation matrices. Shape: (Persons, Seq, 24, 3, 3) :return: Axis angle representation of inpute matrices. Shape: (Persons, Seq, 24, 3) """ aa = quaternion.as_rotation_vector(quaternion.from_rotation_matrix(data)) return aa
def __mul__(self, q: _np.quaternion) -> Frame: if not _np.all( _np.isclose( _quaternion.as_rotation_vector(q)[1:], _np.array( [0.0, 0.0]))): raise FrameException( "Frenet frames cannot be multiplied (rotated) by arbitrary quaternions." ) super().__mul__(q)
def test_rotation_vector(): quat = quaternion.from_float_array( np.array([np.cos(np.pi / 3), 0, np.sin(np.pi / 3), 0])) print("quaterion angle pi*2/3 about y-axis", quat) rvec = quaternion.as_rotation_vector(quat) print("rotation vector:", rvec) assert (np.isclose(np.linalg.norm(rvec), np.pi * 2 / 3)) print("!!! test_rotation_vector passed")
def move(self, pose: Pose, move_type: MoveType, velocity: float = 50.0, acceleration: float = 50.0) -> None: """Moves the robot's end-effector to a specific pose. :param pose: Target pose. :move_type: Move type. :param velocity: Speed of move (percent). :param acceleration: Acceleration of move (percent). :return: """ if not (0.0 <= velocity <= 100.0): raise DobotException("Invalid velocity.") if not (0.0 <= acceleration <= 100.0): raise DobotException("Invalid acceleration.") with self._move_lock: rp = tr.make_pose_rel(self.pose, pose) # prevent Dobot from moving when given an unreachable goal try: jv = self._inverse_kinematics(rp) except Arcor2NotImplemented: # TODO remove this once M1 has IK pass if self.simulator: self._joint_values = jv time.sleep((100.0 - velocity) * 0.05) return self._handle_pose_in(rp) try: self._dobot.clear_alarms() unrotated = self.UNROTATE_EEF * rp.orientation rotation = math.degrees( quaternion.as_rotation_vector( unrotated.as_quaternion())[2]) self._dobot.speed(velocity, acceleration) self._dobot.wait_for_cmd( self._dobot.move_to( rp.position.x * 1000.0, rp.position.y * 1000.0, rp.position.z * 1000.0, rotation, MOVE_TYPE_MAPPING[move_type], )) except DobotApiException as e: raise DobotException("Move failed.") from e self.alarms_to_exception()
def get_rotation_vector(self, ref: Optional[Frame] = None) -> _np.ndarray: """ Args: ref: Returns: """ return _quaternion.as_rotation_vector(self.get_quaternion(ref))
def frame_diff(df_row): pos_diff = df_row['v3d'][0:3, 3] - df_row['isb'][0:3, 3] matrix_diff = df_row['isb'][0:3, 0:3].T @ df_row['v3d'][0:3, 0:3] pose_diff = np.full((6, ), np.nan) pose_diff[0:3] = pos_diff pose_diff[3:] = np.rad2deg( quaternion.as_rotation_vector( quaternion.from_rotation_matrix(matrix_diff, nonorthogonal=False))) return pose_diff
def quat_pw(q, rho): theta = np.nan_to_num( np.linalg.norm(quat.as_rotation_vector(q.normalized()))) n = normalize(q.vec) scale = (np.sqrt(q.norm()))**rho #theta = np.arccos(q.w) sc_part = scale * np.cos(theta * rho) vec_part = scale * n * np.sin(theta * rho) q_pw = np.quaternion(sc_part, vec_part[0], vec_part[1], vec_part[2]) return q_pw
def test_as_rotation_vector(): np.random.seed(1234) n_tests = 1000 vecs = np.random.uniform(high=math.pi/math.sqrt(3), size=n_tests*3).reshape((n_tests, 3)) quats = np.zeros(vecs.shape[:-1]+(4,)) quats[..., 1:] = vecs[...] quats = quaternion.as_quat_array(quats) quats = np.exp(quats/2) quat_vecs = quaternion.as_rotation_vector(quats) assert allclose(quat_vecs, vecs)
def load_image_data(image_filename, table_file): cols = [ "OBSERVATION_END_MET", "IMAGE_FILENAME", "OBSERVATION_END_TIME", "SPC_X", "SPC_Y", "SPC_Z", "AST_J2_X", "AST_J2_Y", "AST_J2_Z", "SPC_J2_X", "SPC_J2_Y", "SPC_J2_Z", "BODY_SURFACE_DISTANCE", "CENTER_LON", "CENTER_LAT", "CENTER_PIXEL_RES", "CELESTIAL_N_CLOCK_ANGLE", "BODY_POLE_CLOCK_ANGLE", "BODY_POLE_ASPECT_ANGLE", "SUN_DIR_CLOCK_ANGLE", "RIGHT_ASCENSION", "DECLINATION", "SUBSOLAR_LON", "SUBSOLAR_LAT", "INCIDENCE_ANGLE", "EMISSION_ANGLE", "PHASE_ANGLE", "SOLAR_ELONGATION", "SUB_SC_LON", "SUB_SC_LAT", "BODY_CENTER_DISTANCE", "PIXEL_OFFSET_X", "PIXEL_OFFSET_Y", "AST_SUN_ROT_ANGLE" ] idx = dict(zip(cols, range(len(cols)))) with open(table_file, 'r') as fh: alldata = [re.split(r'\s+', row)[1:] for row in fh] d = None for row in alldata: if row[idx['IMAGE_FILENAME']] == image_filename: d = row break assert d is not None, 'data for image %s not found' % image_filename # spacecraft orientation, equatorial J2000 sc_rot_ra = float(d[idx['RIGHT_ASCENSION']]) sc_rot_dec = float(d[idx['DECLINATION']]) sc_rot_cnca = float(d[idx['CELESTIAL_N_CLOCK_ANGLE']]) sc_igrf_q = tools.ypr_to_q( rads(sc_rot_dec), rads(sc_rot_ra), -rads(sc_rot_cnca)) # same with rosetta lbls also sun_ast_igrf_r = np.array( [d[idx['AST_J2_X']], d[idx['AST_J2_Y']], d[idx['AST_J2_Z']]]).astype(np.float) sun_sc_igrf_r = np.array( [d[idx['SPC_J2_X']], d[idx['SPC_J2_Y']], d[idx['SPC_J2_Z']]]).astype(np.float) arr2str = lambda arr: '[%s]' % ', '.join(['%f' % v for v in arr]) print('sun_ast_igrf_r: %s' % arr2str(sun_ast_igrf_r * 1e3)) print('sun_sc_igrf_r: %s' % arr2str(sun_sc_igrf_r * 1e3)) print('ast_sc_igrf_r: %s' % arr2str( (sun_sc_igrf_r - sun_ast_igrf_r) * 1e3)) # print('ast_axis_ra: %f' % degs(ast_axis_ra)) # print('ast_axis_dec: %f' % degs(ast_axis_dec)) # print('ast_zlon_ra: %f' % degs(ast_zlon_ra)) aa = quaternion.as_rotation_vector(sc_igrf_q) angle = np.linalg.norm(aa) sc_angleaxis = [angle] + list(aa / angle) print('sc_angleaxis [rad]: %s' % arr2str(sc_angleaxis))
def orientationControl(self, target): r = quater.from_float_array(target) r_vect = quater.as_rotation_vector(r) cop_vect = copy.copy(r_vect) r_vect[0] = cop_vect[1] r_vect[1] = cop_vect[0] r_vect[2] *= -1 r = quater.from_rotation_vector(r_vect) out = quater.as_float_array(r) return out
def accumulate_egomotion(rots, vels): # Accumulate translation and rotation egos = [] qa = np.quaternion(1, 0, 0, 0) va = np.array([0., 0., 0.]) for rot, vel in zip(rots, vels): vel_rot = quaternion.rotate_vectors(qa, vel) va += vel_rot qa = qa * quaternion.from_rotation_vector(rot) egos.append( np.concatenate((quaternion.as_rotation_vector(qa), va), axis=0)) return egos
def rot_to_aa_representation(sample_list): """ Args: sample_list (list): of motion samples with shape of (seq_len, SMPL_NR_JOINTS*9). Returns: """ out = [np.reshape(s, [-1, SMPL_NR_JOINTS, 3, 3]) for s in sample_list] aa = [quaternion.as_rotation_vector(quaternion.from_rotation_matrix(s)) for s in out] aa = [np.reshape(s, (-1, 72)) for s in aa] return aa
def transform(self, R, H, S, numbers, positions=None, forces=None): q = quaternion.from_rotation_matrix(R) vR = quaternion.as_rotation_vector(q) * self.phase q = quaternion.from_rotation_vector(vR) UDUs = self._calc_UDUs(q) M = chain(*[UDUs[self.lvec[z]] for z in numbers]) M = la.block_diag(*M) H = M.T @ H @ M S = M.T @ S @ M pos_rot = positions @ R.T if forces is not None: force_rot = forces @ R.T return H, S, pos_rot, force_rot return H, S, pos_rot
def _get_ground_truth(self): """ Return x, y, and z rotation 3 dim total """ obj_gid = self.sim.model.geom_name2id('object') obj_bid = self.sim.model.geom_name2id('object') # only x and y pos needed # obj_world_pos = self.sim.model.geom_pos[obj_gid] # obj_world_quat = quaternion.as_quat_array(self.sim.model.geom_quat[obj_gid].copy()) obj_world_pose = self.sim.data.get_joint_qpos('object:joint') obj_world_pos = obj_world_pose[:3] obj_world_quat = quaternion.as_quat_array(obj_world_pose[3:]) zrot = quaternion.as_rotation_vector(obj_world_quat)[-1] pose = np.zeros((3, )) pose[:2] = obj_world_pos[:2] pose[2] = zrot print(pose) return pose
def update_tf_log(time_event): # type:(rospy.timer.TimerEvent)->None global diffs, tf_child, tf_parent try: (trans, rot) = get_tf_transform(tf_parent, tf_child, time_event.current_expected, rospy.Duration(0.1)) rot_axis = qt.as_rotation_vector(rot) angle = rot_axis[0] * rot_axis[0] + \ rot_axis[1] * rot_axis[1] + \ rot_axis[2] * rot_axis[2] except Exception: rospy.logwarn("No tf found!") trans = (0, 0, 0, 0) angle = 0 added = np.array([[trans[0], trans[1], trans[2], math.sqrt(angle)]]).T diffs = np.append(diffs, added, axis=1)
def find_final_angle_axis(pulse_sequence, n): ''' Finds the final angle-axis representation of the rotation on each of the first n subspaces after the sequence of pulses given by pulse_sequence Inputs: pulse_sequence (list): a list of angle-axis tuples specifying the pulse sequence to be performed Returns: list of axis-angle tuples representing the composite rotations on the first n subspaces ''' final_quaternions = find_final_quaternions(pulse_sequence, n) final_rotations = [ quaternion.as_rotation_vector(q) for q in final_quaternions ] return [(np.linalg.norm(vec), vec / np.linalg.norm(vec)) if np.linalg.norm(vec) > 1e-12 else (0, np.array([0.0, 0.0, 0.0])) for vec in final_rotations]
def quat_to_angle_axis(quat: qt.quaternion) -> Tuple[float, np.ndarray]: r"""Converts a quaternion to angle axis format :param quat: The quaternion :return: - `float` --- The angle to rotate about the axis by - `numpy.ndarray` --- The axis to rotate about. If :math:`\theta = 0`, then this is harded coded to be the +x axis """ rot_vec = qt.as_rotation_vector(quat) theta = np.linalg.norm(rot_vec) if np.abs(theta) < 1e-5: w = np.array([1, 0, 0]) theta = 0.0 else: w = rot_vec / theta return (theta, w)
def quat_to_angle_axis(quat: np.quantile) -> (float, np.array): r"""Converts a quaternion to angle axis format Args: quat (np.quaternion): The quaternion Returns: float: The angle to rotate about the axis by np.array: The axis to rotate about. If theta = 0, then this is harded coded to be the +x axis """ rot_vec = quaternion.as_rotation_vector(quat) theta = np.linalg.norm(rot_vec) if np.abs(theta) < 1e-5: w = np.array([1, 0, 0]) theta = 0.0 else: w = rot_vec / theta return (theta, w)
def rudders(self): if self.event['start_movement'] is not None: orientation = self.Λ angular_velocity = self.angular_velocity('SSK') a0 = np.array([B0, A0(self.movement_time), A0(self.movement_time)]) a1 = np.array([B1, A1(self.movement_time), A1(self.movement_time)]) program_orientation, program_angular_velocity = program_trajectory( self.movement_time) delta_angle = qn.as_rotation_vector(program_orientation.conj() * orientation) delta_angular_velocity = angular_velocity - program_angular_velocity signal = a0 * (delta_angle + a1 * delta_angular_velocity) signal = np.array([[1, +np.sqrt(2), +np.sqrt(2)], [1, -np.sqrt(2), +np.sqrt(2)], [1, -np.sqrt(2), -np.sqrt(2)], [1, +np.sqrt(2), -np.sqrt(2)]]) @ signal res = np.concatenate( [self.dδ, (signal - TAU_2 * self.dδ - self.δ) / TAU_1]) else: res = np.zeros(8) return res
def _path_to_line_points(path, height, width, xpad=0, ypad=0): dots = [] for quat in path: roll, yaw, pitch = quaternion.as_rotation_vector(quat) x_coord = np.tan(pitch) * 120 + 122 y_coord = np.tan(yaw) * -120 + 122 dots.append([x_coord, y_coord]) min_x = min([coord[0] for coord in dots]) max_x = max([coord[0] for coord in dots]) min_y = min([coord[1] for coord in dots]) max_y = max([coord[1] for coord in dots]) output = [] for x, y in dots: output.append((_scale(x, min_x, max_x, xpad, width - xpad * 2), _scale(y, min_y, max_y, ypad, height - ypad * 2))) return output
def sim_map_to_sim_continuous(self, coords): """Converts ground-truth 2D Map coordinates to absolute Habitat simulator position and rotation. """ agent_state = self._env.sim.get_agent_state(0) y, x = coords min_x, min_y = self.map_obj_origin / 100.0 cont_x = x / 20. + min_x cont_y = y / 20. + min_y agent_state.position[0] = cont_y agent_state.position[2] = cont_x rotation = agent_state.rotation rvec = quaternion.as_rotation_vector(rotation) if self.args.train_single_eps: rvec[1] = 0.0 else: rvec[1] = np.random.rand() * 2 * np.pi rot = quaternion.from_rotation_vector(rvec) return agent_state.position, rot