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)