def change_task(self, task):
    task = {'goal': np.array([0, 1.0, 0.05]), 'height': 0.06, 'obj_init_pos': np.array([0, 0.6, 0.04])}
    self.grasp = False
    self.pickCompleted = False
    if (len(task['goal']) == 3):
        self._state_goal = task['goal']
    else:
        self._state_goal = np.concatenate([task['goal'], [0.02]])
    self._set_goal_marker(self._state_goal)
    if (len(task['obj_init_pos']) == 3):
        self.obj_init_pos = task['obj_init_pos']
    else:
        self.obj_init_pos = np.concatenate([task['obj_init_pos'], [0.02]])
def change_task(self, task):
    task = {'goal': np.array([0, 1.0, 0.05]), 'height': 0.06, 'obj_init_pos': np.array([0, 0.6, 0.04])}
    self.grasp = False
    self.pickCompleted = False
    if (len(task['goal']) == 3):
        self._state_goal = task['goal']
    else:
        self._state_goal = np.concatenate([task['goal'], [0.02]])
    self._set_goal_marker(self._state_goal)
    if (len(task['obj_init_pos']) == 3):
        self.obj_init_pos = task['obj_init_pos']
    else:
        self.obj_init_pos = np.concatenate([task['obj_init_pos'], [0.02]])