def act(self, obs, info): """Run inference and return best action.""" act = {'camera_config': self.camera_config, 'primitive': None} # Get observations and run pick prediction gt_obs = self.info_to_gt_obs(info) pick_prediction = self.pick_model(gt_obs[None, Ellipsis]) if self.use_mdn: pi, mu, var = pick_prediction # prediction = mdn_utils.pick_max_mean(pi, mu, var) pick_prediction = mdn_utils.sample_from_pdf(pi, mu, var) pick_prediction = pick_prediction[:, 0, :] pick_prediction = pick_prediction[0] # unbatch # Get observations and run place prediction obs_with_pick = np.hstack((gt_obs, pick_prediction)) # since the pick at train time is always 0.0, # the predictions are unstable if not exactly 0 obs_with_pick[-1] = 0.0 place_prediction = self.place_model(obs_with_pick[None, Ellipsis]) if self.use_mdn: pi, mu, var = place_prediction # prediction = mdn_utils.pick_max_mean(pi, mu, var) place_prediction = mdn_utils.sample_from_pdf(pi, mu, var) place_prediction = place_prediction[:, 0, :] place_prediction = place_prediction[0] prediction = np.hstack((pick_prediction, place_prediction)) # just go exactly to objects, from observations # p0_position = np.hstack((gt_obs[3:5], 0.02)) # p0_rotation = utils.eulerXYZ_to_quatXYZW( # (0, 0, -gt_obs[5]*self.theta_scale)) # p1_position = np.hstack((gt_obs[0:2], 0.02)) # p1_rotation = utils.eulerXYZ_to_quatXYZW( # (0, 0, -gt_obs[2]*self.theta_scale)) # just go exactly to objects, predicted p0_position = np.hstack((prediction[0:2], 0.02)) p0_rotation = utils.eulerXYZ_to_quatXYZW( (0, 0, -prediction[2] * self.theta_scale)) p1_position = np.hstack((prediction[3:5], 0.02)) p1_rotation = utils.eulerXYZ_to_quatXYZW( (0, 0, -prediction[5] * self.theta_scale)) # Select task-specific motion primitive. act['primitive'] = 'pick_place' if self.task == 'sweeping': act['primitive'] = 'sweep' elif self.task == 'pushing': act['primitive'] = 'push' params = { 'pose0': (p0_position, p0_rotation), 'pose1': (p1_position, p1_rotation) } act['params'] = params return act
def act(self, obs, gt_act, info): """Run inference and return best action given visual observations.""" del gt_act del info self.regression_model.set_batch_size(1) act = {'camera_config': self.camera_config, 'primitive': None} if not obs: return act # Get heightmap from RGB-D images. colormap, heightmap = self.get_heightmap(obs, self.camera_config) # Concatenate color with depth images. input_image = np.concatenate( (colormap, heightmap[Ellipsis, None], heightmap[Ellipsis, None], heightmap[Ellipsis, None]), axis=2)[None, Ellipsis] # or just use rgb # input_image = colormap[None, ...] # Regression prediction = self.regression_model.forward(input_image) if self.use_mdn: mdn_prediction = prediction pi, mu, var = mdn_prediction # prediction = mdn_utils.pick_max_mean(pi, mu, var) prediction = mdn_utils.sample_from_pdf(pi, mu, var) prediction = prediction[:, 0, :] prediction = prediction[0] p0_position = np.hstack((prediction[0:2], 0.02)) p1_position = np.hstack((prediction[3:5], 0.02)) p0_rotation = utils.get_pybullet_quaternion_from_rot( (0, 0, -prediction[2] * self.theta_scale)) p1_rotation = utils.get_pybullet_quaternion_from_rot( (0, 0, -prediction[5] * self.theta_scale)) act['primitive'] = 'pick_place' if self.task == 'sweeping': act['primitive'] = 'sweep' elif self.task == 'pushing': act['primitive'] = 'push' params = { 'pose0': (p0_position, p0_rotation), 'pose1': (p1_position, p1_rotation) } act['params'] = params self.regression_model.set_batch_size(self.batch_size) return act
def act(self, obs, info): """Run inference and return best action.""" del obs act = {'camera_config': self.camera_config, 'primitive': None} # Get observations and run predictions. gt_obs = self.info_to_gt_obs(info) # just for visualization gt_act_center = self.info_to_gt_obs(info) # pylint: disable=unused-variable prediction = self.model(gt_obs[None, Ellipsis]) if self.use_mdn: mdn_prediction = prediction pi, mu, var = mdn_prediction # prediction = mdn_utils.pick_max_mean(pi, mu, var) prediction = mdn_utils.sample_from_pdf(pi, mu, var) prediction = prediction[:, 0, :] prediction = prediction[0] # unbatch # self.plot_act_mdn(gt_act_center[None, ...], mdn_prediction) # just go exactly to objects, from observations # p0_position = np.hstack((gt_obs[3:5], 0.02)) # p0_rotation = utils.eulerXYZ_to_quatXYZW( # (0, 0, -gt_obs[5] * self.theta_scale)) # p1_position = np.hstack((gt_obs[0:2], 0.02)) # p1_rotation = utils.eulerXYZ_to_quatXYZW( # (0, 0, -gt_obs[2] * self.theta_scale)) # just go exactly to objects, predicted p0_position = np.hstack((prediction[0:2], 0.02)) p0_rotation = utils.eulerXYZ_to_quatXYZW( (0, 0, -prediction[2] * self.theta_scale)) p1_position = np.hstack((prediction[3:5], 0.02)) p1_rotation = utils.eulerXYZ_to_quatXYZW( (0, 0, -prediction[5] * self.theta_scale)) # Select task-specific motion primitive. act['primitive'] = 'pick_place' if self.task == 'sweeping': act['primitive'] = 'sweep' elif self.task == 'pushing': act['primitive'] = 'push' params = { 'pose0': (p0_position, p0_rotation), 'pose1': (p1_position, p1_rotation) } act['params'] = params return act
def act(self, obs, info, goal=None): """Run inference and return best action.""" act = {'camera_config': self.camera_config, 'primitive': None} # Get observations and run predictions (second part just for visualization). if self.goal_conditioned: gt_obs = self.info_to_gt_obs(info, goal=goal) gt_act_center = self.info_to_gt_obs(info, goal=goal) else: gt_obs = self.info_to_gt_obs(info) gt_act_center = self.info_to_gt_obs(info) prediction = self.model(gt_obs[None, ...]) if self.USE_MDN: mdn_prediction = prediction pi, mu, var = mdn_prediction #prediction = mdn_utils.pick_max_mean(pi, mu, var) prediction = mdn_utils.sample_from_pdf(pi, mu, var) prediction = prediction[:, 0, :] prediction = prediction[0] # unbatch # Just go exactly to objects, predicted. Daniel: adding 1 rotation inference case. p0_position = np.hstack((prediction[0:2], 0.02)) p0_pred_rot = 0.0 if self.one_rot_inf else -prediction[ 2] * self.THETA_SCALE # idx 2 p0_rotation = utils.get_pybullet_quaternion_from_rot( (0, 0, p0_pred_rot)) p1_position = np.hstack((prediction[3:5], 0.02)) p1_pred_rot = 0.0 if self.one_rot_inf else -prediction[ 5] * self.THETA_SCALE # idx 5 p1_rotation = utils.get_pybullet_quaternion_from_rot( (0, 0, p1_pred_rot)) # Select task-specific motion primitive. act['primitive'] = 'pick_place' if self.task == 'sweeping': act['primitive'] = 'sweep' elif self.task == 'pushing': act['primitive'] = 'push' params = { 'pose0': (p0_position, p0_rotation), 'pose1': (p1_position, p1_rotation) } act['params'] = params # Daniel: like transporters, determine the task stage if applicable. (AND if loading only) if self.task in ['bag-items-easy', 'bag-items-hard', 'bag-color-goal']: self._determine_task_stage(p0_position, p1_position) return act
def act(self, obs, gt_act, info): """Run inference and return best action.""" del gt_act assert False, 'this needs to have the ordering switched for act inference -- is now xytheta, rpz' # pylint: disable=line-too-long act = {'camera_config': self.camera_config, 'primitive': None} # Get observations and run predictions. gt_obs = self.info_to_gt_obs(info) prediction = self.model(gt_obs[None, Ellipsis]) if self.use_mdn: mdn_prediction = prediction pi, mu, var = mdn_prediction # prediction = mdn_utils.pick_max_mean(pi, mu, var) prediction = mdn_utils.sample_from_pdf(pi, mu, var) prediction = prediction[:, 0, :] prediction = prediction[0] # unbatch p0_position = np.hstack((prediction[0:2], 0.02)) p0_rotation = utils.eulerXYZ_to_quatXYZW( (0, 0, -prediction[2] * self.theta_scale)) p1_position = prediction[3:6] p1_rotation = utils.eulerXYZ_to_quatXYZW( (prediction[6] * self.theta_scale, prediction[7] * self.theta_scale, -prediction[8] * self.theta_scale)) # Select task-specific motion primitive. act['primitive'] = 'pick_place_6dof' params = { 'pose0': (p0_position, p0_rotation), 'pose1': (p1_position, p1_rotation) } act['params'] = params return act
def act(self, obs, info): """Run inference and return best action.""" act = {'camera_config': self.camera_config, 'primitive': None} # Get observations and run pick prediction gt_obs = self.info_to_gt_obs(info) pick_prediction = self.pick_model(gt_obs[None, Ellipsis]) if self.use_mdn: pi, mu, var = pick_prediction # prediction = mdn_utils.pick_max_mean(pi, mu, var) pick_prediction = mdn_utils.sample_from_pdf(pi, mu, var) pick_prediction = pick_prediction[:, 0, :] pick_prediction = pick_prediction[0] # unbatch # Get observations and run place prediction obs_with_pick = np.hstack((gt_obs, pick_prediction)).astype(np.float32) # since the pick at train time is always 0.0, # the predictions are unstable if not exactly 0 obs_with_pick[-1] = 0.0 place_se2_prediction = self.place_se2_model(obs_with_pick[None, Ellipsis]) if self.use_mdn: pi, mu, var = place_se2_prediction # prediction = mdn_utils.pick_max_mean(pi, mu, var) place_se2_prediction = mdn_utils.sample_from_pdf(pi, mu, var) place_se2_prediction = place_se2_prediction[:, 0, :] place_se2_prediction = place_se2_prediction[0] # Get observations and run rpz prediction obs_with_pick_place_se2 = np.hstack( (obs_with_pick, place_se2_prediction)).astype(np.float32) place_rpz_prediction = self.place_rpz_model(obs_with_pick_place_se2[None, Ellipsis]) if self.use_mdn: pi, mu, var = place_rpz_prediction # prediction = mdn_utils.pick_max_mean(pi, mu, var) place_rpz_prediction = mdn_utils.sample_from_pdf(pi, mu, var) place_rpz_prediction = place_rpz_prediction[:, 0, :] place_rpz_prediction = place_rpz_prediction[0] p0_position = np.hstack((pick_prediction[0:2], 0.02)) p0_rotation = utils.eulerXYZ_to_quatXYZW((0, 0, 0)) p1_position = np.hstack( (place_se2_prediction[0:2], place_rpz_prediction[2])) p1_rotation = utils.eulerXYZ_to_quatXYZW( (place_rpz_prediction[0] * self.theta_scale, place_rpz_prediction[1] * self.theta_scale, -place_se2_prediction[2] * self.theta_scale)) # Select task-specific motion primitive. act['primitive'] = 'pick_place_6dof' params = { 'pose0': (p0_position, p0_rotation), 'pose1': (p1_position, p1_rotation) } act['params'] = params return act
def act(self, obs, info, goal=None): """Run inference and return best action.""" act = {'camera_config': self.camera_config, 'primitive': None} # Get observations and run pick prediction if self.goal_conditioned: gt_obs = self.info_to_gt_obs(info, goal=goal) else: gt_obs = self.info_to_gt_obs(info) pick_prediction = self.pick_model(gt_obs[None, ...]) if self.USE_MDN: pi, mu, var = pick_prediction #prediction = mdn_utils.pick_max_mean(pi, mu, var) pick_prediction = mdn_utils.sample_from_pdf(pi, mu, var) pick_prediction = pick_prediction[:, 0, :] pick_prediction = pick_prediction[0] # unbatch # Get observations and run place prediction obs_with_pick = np.hstack((gt_obs, pick_prediction)) # since the pick at train time is always 0.0, # the predictions are unstable if not exactly 0 obs_with_pick[-1] = 0.0 place_prediction = self.place_model(obs_with_pick[None, ...]) if self.USE_MDN: pi, mu, var = place_prediction #prediction = mdn_utils.pick_max_mean(pi, mu, var) place_prediction = mdn_utils.sample_from_pdf(pi, mu, var) place_prediction = place_prediction[:, 0, :] place_prediction = place_prediction[0] prediction = np.hstack((pick_prediction, place_prediction)) # Daniel: like with gt_state, guessing this is just for insertion. # just go exactly to objects, from observations # p0_position = np.hstack((gt_obs[3:5], 0.02)) # p0_rotation = utils.get_pybullet_quaternion_from_rot((0, 0, -gt_obs[5]*self.THETA_SCALE)) # p1_position = np.hstack((gt_obs[0:2], 0.02)) # p1_rotation = utils.get_pybullet_quaternion_from_rot((0, 0, -gt_obs[2]*self.THETA_SCALE)) # Just go exactly to objects, predicted. Daniel: adding 1 rotation inference case. p0_position = np.hstack((prediction[0:2], 0.02)) p0_pred_rot = 0.0 if self.one_rot_inf else -prediction[ 2] * self.THETA_SCALE # idx 2 p0_rotation = utils.get_pybullet_quaternion_from_rot( (0, 0, p0_pred_rot)) p1_position = np.hstack((prediction[3:5], 0.02)) p1_pred_rot = 0.0 if self.one_rot_inf else -prediction[ 5] * self.THETA_SCALE # idx 5 p1_rotation = utils.get_pybullet_quaternion_from_rot( (0, 0, p1_pred_rot)) # Select task-specific motion primitive. act['primitive'] = 'pick_place' if self.task == 'sweeping': act['primitive'] = 'sweep' elif self.task == 'pushing': act['primitive'] = 'push' params = { 'pose0': (p0_position, p0_rotation), 'pose1': (p1_position, p1_rotation) } act['params'] = params # Daniel: like transporters, determine the task stage if applicable. (AND if loading only) if self.task in ['bag-items-easy', 'bag-items-hard']: self._determine_task_stage(p0_position, p1_position) return act