def get_new_other_obj_position(self, anchor_args, anchor_pos, anchor_lb, anchor_ub, other_handle, oriented_other_lb, oriented_other_ub): '''Get position of the other object around the main object. anchor_lb: Relative lower bound of bounding box. anchor_ub: Relative upper bound of bounding box. oriented_other_lb: Absolute lower bound of bounding box in world frame. oriented_other_ub: Absolute upper bound of bounding box in world frame. ''' args = self.args rel_pos = npri(1, 4) oriented_other_center = (oriented_other_lb + oriented_other_ub) / 2 oriented_bb_size = (oriented_other_ub - oriented_other_lb) oriented_other_center = oriented_other_center - INIT_POS_ARR other_obj_ht = oriented_other_ub[-1] - oriented_other_lb[-1] if args.sample_on_top == 1: th = -0.05 ht = anchor_pos[-1] + anchor_ub[-1] + other_obj_ht / 2.0 else: th = 0.01 ht = other_obj_ht / 2.0 logging.info("Other obj new ht: {:.3f}".format(ht)) if rel_pos == 0: # Left new_y = npu(anchor_lb[1], anchor_ub[1]) new_pos = [ anchor_lb[0] - oriented_bb_size[0] / 2.0 - th, new_y, ht ] elif rel_pos == 1: # right new_y = npu(anchor_lb[1], anchor_ub[1]) new_pos = [ anchor_ub[0] + oriented_bb_size[0] / 2.0 + th, new_y, ht ] elif rel_pos == 2: # infront new_x = npu(anchor_lb[0], anchor_ub[0]) new_pos = [ new_x, anchor_lb[1] - oriented_bb_size[1] / 2.0 - th, ht ] elif rel_pos == 3: # back new_x = npu(anchor_lb[0], anchor_ub[0]) new_pos = [ new_x, anchor_ub[1] + oriented_bb_size[1] / 2.0 + th, ht ] else: raise ValueError("Invalid rel_pos: {}".format(rel_pos)) logging.info("Anchor lb: {}, \t ub: {}".format(anchor_lb, anchor_ub)) # import ipdb; ipdb.set_trace() # Set new object position resp = vrep.simxSetObjectPosition(self.client_id, other_handle, -1, new_pos, vrep.simx_opmode_blocking) if resp != vrep.simx_return_ok: logging.error("Cannot set new obj position: {}".format(resp)) return resp == vrep.simx_return_ok, new_pos
def set_object_position(self, obj_handle, position): resp = vrep.simxSetObjectPosition( self.client_id, obj_handle, -1, # Sets absolute position position, vrep.simx_opmode_oneshot) if resp != vrep.simx_return_ok: logging.error("Error in setting object position: {}".format(resp))
def setObjPos(self): e, handle = vrep.simxGetObjectHandle(self.CLIENT_ID, 'MP_BODY_respondable', vrep.simx_opmode_oneshot_wait) vrep.simxSetObjectPosition(self.CLIENT_ID, handle, -1, [0.6, 0.6, 0.6], vrep.simx_opmode_oneshot_wait)