def test_incremental_rotation(dev_str, call): if call in [helpers.np_call, helpers.jnp_call, helpers.mx_call]: # convolutions not yet implemented in numpy or jax # mxnet is unable to stack or expand zero-dimensional tensors pytest.skip() batch_size = 1 num_timesteps = 1 num_cams = 1 num_feature_channels = 3 image_dims = [3, 3] esm = ESM(omni_image_dims=[10, 20], smooth_mean=False) empty_memory = esm.empty_memory(batch_size, num_timesteps) empty_obs = _get_dummy_obs(batch_size, num_timesteps, num_cams, image_dims, num_feature_channels, empty=True) rel_rot_vec_pose = ivy.array([[[0., 0., 0., 0., 0.1, 0.]]]) empty_obs['control_mean'] = rel_rot_vec_pose empty_obs['agent_rel_mat'] = ivy_mech.rot_vec_pose_to_mat_pose(rel_rot_vec_pose) first_obs = _get_dummy_obs(batch_size, num_timesteps, num_cams, image_dims, num_feature_channels, ones=True) memory_1 = esm(first_obs, empty_memory, batch_size=batch_size, num_timesteps=num_timesteps, num_cams=num_cams, image_dims=image_dims) memory_2 = esm(empty_obs, memory_1, batch_size=batch_size, num_timesteps=num_timesteps, num_cams=num_cams, image_dims=image_dims) memory_3 = esm(empty_obs, memory_2, batch_size=batch_size, num_timesteps=num_timesteps, num_cams=num_cams, image_dims=image_dims) assert not np.allclose(memory_1.mean, memory_3.mean)
def __init__(self): rot_vec_pose = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6]) self.inv_ext_mat = ivy_mech.rot_vec_pose_to_mat_pose(rot_vec_pose) self.rel_body_points = np.array([[0., 0., 0.], [-0.2, -0.2, 0.], [-0.2, 0.2, 0.], [0.2, -0.2, 0.], [0.2, 0.2, 0.]]) self.sampled_body = np.array([[0.1, 0.2, 0.3], [0.04361792, -0.0751835, 0.26690764], [-0.12924806, 0.22732089, 0.46339797], [0.32924806, 0.17267911, 0.13660203], [0.15638208, 0.4751835, 0.33309236]])
def compute_cost_and_sdfs(learnable_anchor_vals, anchor_points, start_anchor_val, end_anchor_val, query_points, sim): anchor_vals = ivy.concatenate( (ivy.expand_dims(start_anchor_val, 0), learnable_anchor_vals, ivy.expand_dims(end_anchor_val, 0)), 0) poses = ivy_robot.sample_spline_path(anchor_points, anchor_vals, query_points) inv_ext_mat_query_vals = ivy_mech.rot_vec_pose_to_mat_pose(poses) body_positions = ivy.transpose( sim.ivy_drone.sample_body(inv_ext_mat_query_vals), (1, 0, 2)) length_cost = compute_length(body_positions) sdf_vals = sim.sdf(ivy.reshape(body_positions, (-1, 3))) coll_cost = -ivy.reduce_mean(sdf_vals) total_cost = length_cost + coll_cost * 10 return total_cost[0], poses, body_positions, ivy.reshape( sdf_vals, (-1, 100, 1))
def execute_motion(self, poses): print('\nSpline path optimized.\n') if self._interactive: input('\nPress enter in the terminal to execute motion.\n') print('\nExecuting motion...\n') if self.with_pyrep: for i in range(100): pose = poses[i] inv_ext_mat = ivy_mech.rot_vec_pose_to_mat_pose(pose) self._drone.set_matrix( ivy.to_numpy(inv_ext_mat).reshape((-1, )).tolist()) time.sleep(0.05) elif self._interactive: this_dir = os.path.dirname(os.path.realpath(__file__)) for i in range(11): plt.ion() plt.imshow( mpimg.imread( os.path.join(this_dir, 'dsp_no_sim', 'motion_{}.png'.format(i)))) plt.show() plt.pause(0.1) plt.ioff()
def __init__(self, interactive, try_use_sim): super().__init__(interactive, try_use_sim) # ivy robot rel_body_points = ivy.array([[0., 0., 0.], [-0.15, 0., -0.15], [-0.15, 0., 0.15], [0.15, 0., -0.15], [0.15, 0., 0.15]]) self.ivy_drone = RigidMobile(rel_body_points) # initialize scene if self.with_pyrep: self._spherical_vision_sensor.remove() for i in range(6): self._vision_sensors[i].remove() self._vision_sensor_bodies[i].remove() [ray.remove() for ray in self._vision_sensor_rays[i]] drone_start_pos = np.array([-1.15, -1.028, 0.6]) target_pos = np.array([1.025, 1.125, 0.6]) self._drone.set_position(drone_start_pos) self._drone.set_orientation( np.array([-90, 50, -180]) * np.pi / 180) self._target.set_position(target_pos) self._target.set_orientation( np.array([-90, 50, -180]) * np.pi / 180) self._default_camera.set_position( np.array([-3.2835, -0.88753, 1.3773])) self._default_camera.set_orientation( np.array([-151.07, 70.079, -120.45]) * np.pi / 180) input( '\nScene initialized.\n\n' 'The simulator visualizer can be translated and rotated by clicking either the left mouse button or the wheel, ' 'and then dragging the mouse.\n' 'Scrolling the mouse wheel zooms the view in and out.\n\n' 'You can click on any object either in the scene or the left hand panel, ' 'then select the box icon with four arrows in the top panel of the simulator, ' 'and then drag the object around dynamically.\n' 'Starting to drag and then holding ctrl allows you to also drag the object up and down.\n' 'Clicking the top icon with a box and two rotating arrows similarly allows rotation of the object.\n\n' 'Once you have aranged the scene as desired, press enter in the terminal to continue with the demo...\n' ) # primitive scene self.setup_primitive_scene() # public objects drone_starting_inv_ext_mat = ivy.array( np.reshape(self._drone.get_matrix(), (3, 4)), 'float32') drone_start_rot_vec_pose = ivy_mech.mat_pose_to_rot_vec_pose( drone_starting_inv_ext_mat) self.drone_start_pose = drone_start_rot_vec_pose target_inv_ext_mat = ivy.array( np.reshape(self._target.get_matrix(), (3, 4)), 'float32') target_rot_vec_pose = ivy_mech.mat_pose_to_rot_vec_pose( target_inv_ext_mat) self.drone_target_pose = target_rot_vec_pose # spline path drone_start_to_target_poses = ivy.transpose( ivy.linspace(self.drone_start_pose, self.drone_target_pose, 100), (1, 0)) drone_start_to_target_inv_ext_mats = ivy_mech.rot_vec_pose_to_mat_pose( drone_start_to_target_poses) drone_start_to_target_positions =\ ivy.transpose(self.ivy_drone.sample_body(drone_start_to_target_inv_ext_mats), (1, 0, 2)) initil_sdf_vals = ivy.reshape( self.sdf( ivy.reshape( ivy.cast(drone_start_to_target_positions, 'float32'), (-1, 3))), (-1, 100, 1)) self.update_path_visualization(drone_start_to_target_positions, initil_sdf_vals, None) # wait for user input self._user_prompt( '\nInitialized scene with a drone and a target position to reach.' '\nPress enter in the terminal to use method ivy_robot.interpolate_spline_points ' 'to plan a spline path which reaches the target whilst avoiding the objects in the scene...\n' ) else: # primitive scene self.setup_primitive_scene_no_sim() # public objects self.drone_start_pose = ivy.array( [-1.1500, -1.0280, 0.6000, 0.7937, 1.7021, 1.7021]) self.drone_target_pose = ivy.array( [1.0250, 1.1250, 0.6000, 0.7937, 1.7021, 1.7021]) # message print( '\nInitialized dummy scene with a drone and a target position to reach.' '\nClose the visualization window to use method ivy_robot.interpolate_spline_points ' 'to plan a spline path which reaches the target whilst avoiding the objects in the scene...\n' ) # plot scene before rotation if interactive: plt.imshow( mpimg.imread( os.path.join( os.path.dirname(os.path.realpath(__file__)), 'dsp_no_sim', 'path_0.png'))) plt.show() print('\nOptimizing spline path...\n')
def main(f=None): # Framework Setup # # ----------------# # choose random framework f = choose_random_framework() if f is None else f set_framework(f) # Orientation # # ------------# # rotation representations # 3 rot_vec = ivy.array([0., 1., 0.]) # 3 x 3 rot_mat = ivy_mech.rot_vec_to_rot_mat(rot_vec) # 3 euler_angles = ivy_mech.rot_mat_to_euler(rot_mat, 'zyx') # 4 quat = ivy_mech.euler_to_quaternion(euler_angles) # 4 axis_and_angle = ivy_mech.quaternion_to_axis_angle(quat) # 3 rot_vec_again = axis_and_angle[..., :-1] * axis_and_angle[..., -1:] # Pose # # -----# # pose representations # 3 position = ivy.ones_like(rot_vec) # 6 rot_vec_pose = ivy.concatenate((position, rot_vec), 0) # 3 x 4 mat_pose = ivy_mech.rot_vec_pose_to_mat_pose(rot_vec_pose) # 6 euler_pose = ivy_mech.mat_pose_to_euler_pose(mat_pose) # 7 quat_pose = ivy_mech.euler_pose_to_quaternion_pose(euler_pose) # 6 rot_vec_pose_again = ivy_mech.quaternion_pose_to_rot_vec_pose(quat_pose) # Position # # ---------# # conversions of positional representation # 3 cartesian_coord = ivy.random_uniform(0., 1., (3, )) # 3 polar_coord = ivy_mech.cartesian_to_polar_coords(cartesian_coord) # 3 cartesian_coord_again = ivy_mech.polar_to_cartesian_coords(polar_coord) # cartesian co-ordinate frame-of-reference transformations # 3 x 4 trans_mat = ivy.random_uniform(0., 1., (3, 4)) # 4 cartesian_coord_homo = ivy_mech.make_coordinates_homogeneous( cartesian_coord) # 3 trans_cartesian_coord = ivy.matmul( trans_mat, ivy.expand_dims(cartesian_coord_homo, -1))[:, 0] # 4 trans_cartesian_coord_homo = ivy_mech.make_coordinates_homogeneous( trans_cartesian_coord) # 4 x 4 trans_mat_homo = ivy_mech.make_transformation_homogeneous(trans_mat) # 3 x 4 inv_trans_mat = ivy.inv(trans_mat_homo)[0:3] # 3 cartesian_coord_again = ivy.matmul( inv_trans_mat, ivy.expand_dims(trans_cartesian_coord_homo, -1))[:, 0] # message print('End of Run Through Demo!')
def main(interactive=True, f=None): global INTERACTIVE INTERACTIVE = interactive # Framework Setup # # ----------------# # choose random framework set_framework(choose_random_framework() if f is None else f) # Spline Interpolation # # ---------------------# # config num_free_anchors = 3 num_samples = 100 constant_rot_vec = ivy.array([[0., 0., 0.]]) constant_z = ivy.array([[0.]]) # xy positions # 1 x 2 start_xy = ivy.array([[0., 0.]]) target_xy = ivy.array([[1., 1.]]) # 1 x 2 anchor1_xy = ivy.array([[0.6, 0.2]]) anchor2_xy = ivy.array([[0.5, 0.5]]) anchor3_xy = ivy.array([[0.4, 0.8]]) # as 6DOF poses # 1 x 6 start_pose = ivy.concatenate((start_xy, constant_z, constant_rot_vec), -1) anchor1_pose = ivy.concatenate((anchor1_xy, constant_z, constant_rot_vec), -1) anchor2_pose = ivy.concatenate((anchor2_xy, constant_z, constant_rot_vec), -1) anchor3_pose = ivy.concatenate((anchor3_xy, constant_z, constant_rot_vec), -1) target_pose = ivy.concatenate((target_xy, constant_z, constant_rot_vec), -1) num_anchors = num_free_anchors + 2 # num_anchors x 6 anchor_poses = ivy.concatenate( (start_pose, anchor1_pose, anchor2_pose, anchor3_pose, target_pose), 0) # uniform sampling for spline # num_anchors x 1 anchor_points = ivy.expand_dims(ivy.linspace(0., 1., num_anchors), -1) # num_samples x 1 query_points = ivy.expand_dims(ivy.linspace(0., 1., num_samples), -1) # interpolated spline poses # num_samples x 6 interpolated_poses = ivy_robot.sample_spline_path(anchor_points, anchor_poses, query_points) # xy motion # num_samples x 2 anchor_xy_positions = anchor_poses[..., 0:2] # num_samples x 2 interpolated_xy_positions = interpolated_poses[..., 0:2] # show xy path show_2d_spline_path(anchor_xy_positions, interpolated_xy_positions, [-0.095, 0.055], [0.638, 0.171], [0.544, 0.486], [0.445, 0.766], [0.9, 0.9], 'x', 'y', 'Interpolated Drone Pose Spline in XY Plane', 'start point', 'target point') # Rigid Mobile # # -------------# # drone relative body points rel_body_points = ivy.array([[0., 0., 0.], [-0.1, -0.1, 0.], [-0.1, 0.1, 0.], [0.1, -0.1, 0.], [0.1, 0.1, 0.]]) # create drone as ivy rigid mobile robot drone = RigidMobile(rel_body_points) # rotatin vectors # 1 x 3 start_rot_vec = ivy.array([[0., 0., 0.]]) target_rot_vec = ivy.array([[0., 0., np.pi]]) # 1 x 3 anchor1_rot_vec = ivy.array([[0., 0., np.pi / 4]]) anchor2_rot_vec = ivy.array([[0., 0., 2 * np.pi / 4]]) anchor3_rot_vec = ivy.array([[0., 0., 3 * np.pi / 4]]) # as 6DOF poses # 1 x 6 start_pose = ivy.concatenate((start_xy, constant_z, start_rot_vec), -1) anchor1_pose = ivy.concatenate((anchor1_xy, constant_z, anchor1_rot_vec), -1) anchor2_pose = ivy.concatenate((anchor2_xy, constant_z, anchor2_rot_vec), -1) anchor3_pose = ivy.concatenate((anchor3_xy, constant_z, anchor3_rot_vec), -1) target_pose = ivy.concatenate((target_xy, constant_z, target_rot_vec), -1) # num_anchors x 6 anchor_poses = ivy.concatenate( (start_pose, anchor1_pose, anchor2_pose, anchor3_pose, target_pose), 0) # interpolated spline poses # num_samples x 6 interpolated_poses = ivy_robot.sample_spline_path(anchor_points, anchor_poses, query_points) # as matrices # num_anchors x 3 x 4 anchor_matrices = ivy_mech.rot_vec_pose_to_mat_pose(anchor_poses) # num_samples x 3 x 4 interpolated_matrices = ivy_mech.rot_vec_pose_to_mat_pose( interpolated_poses) # sample drone body # num_anchors x num_body_points x 3 anchor_body_points = drone.sample_body(anchor_matrices) # num_samples x num_body_points x 3 interpolated_body_points = drone.sample_body(interpolated_matrices) # show show_full_spline_path(anchor_body_points, interpolated_body_points, [-0.168, 0.19], [0.88, 0.73], 'x', 'y', 'Sampled Drone Body Positions in XY Plane', 'start points', 'target points', False) # Manipulator # # ------------# class SimpleManipulator(Manipulator): # noinspection PyShadowingNames def __init__(self, base_inv_ext_mat=None): a_s = ivy.array([0.5, 0.5]) d_s = ivy.array([0., 0.]) alpha_s = ivy.array([0., 0.]) dh_joint_scales = ivy.ones((2, )) dh_joint_offsets = ivy.array([-np.pi / 2, 0.]) super().__init__(a_s, d_s, alpha_s, dh_joint_scales, dh_joint_offsets, base_inv_ext_mat) # create manipulator as ivy manipulator manipulator = SimpleManipulator() # joint angles # 1 x 2 start_joint_angles = ivy.array([[0., 0.]]) target_joint_angles = ivy.array([[-np.pi / 4, -np.pi / 4]]) # 1 x 2 anchor1_joint_angles = -ivy.array([[0.2, 0.6]]) * np.pi / 4 anchor2_joint_angles = -ivy.array([[0.5, 0.5]]) * np.pi / 4 anchor3_joint_angles = -ivy.array([[0.8, 0.4]]) * np.pi / 4 # num_anchors x 2 anchor_joint_angles = ivy.concatenate( (start_joint_angles, anchor1_joint_angles, anchor2_joint_angles, anchor3_joint_angles, target_joint_angles), 0) # interpolated joint angles # num_anchors x 2 interpolated_joint_angles = ivy_robot.sample_spline_path( anchor_points, anchor_joint_angles, query_points) # sample links # num_anchors x num_link_points x 3 anchor_link_points = manipulator.sample_links(anchor_joint_angles, samples_per_metre=5) # num_anchors x num_link_points x 3 interpolated_link_points = manipulator.sample_links( interpolated_joint_angles, samples_per_metre=5) # show show_2d_spline_path(anchor_joint_angles, interpolated_joint_angles, [-0.222, -0.015], [-0.147, -0.52], [-0.38, -0.366], [-0.64, -0.263], [-0.752, -0.79], r'$\theta_1$', r'$\theta_2$', 'Interpolated Manipulator Joint Angles Spline', 'start angles', 'target angles') show_full_spline_path(anchor_link_points, interpolated_link_points, [0.026, 0.8], [0.542, 0.26], 'x', 'y', 'Sampled Manipulator Links in XY Plane', 'start config', 'target config', True)