def _set_all_visitor_position(self, position):
visitorNum = len(self.visitorHandles)
for i in range(visitorNum):
vrep.simxSetObjectPosition(
self.clientID, self.visitorHandles[i], -1,
[position[i * 2], position[i * 2 + 1], 0],
self._set_visitor_op_mode)
def add_multiple_objects(cid, nb_obj):
object_name_list = []
object_handle_list = []
object_number = nb_obj
# object_list = ['imported_part_0','imported_part_1','imported_part_2','imported_part_3','imported_part_4','imported_part_5','imported_part_6','imported_part_7']
object_list = [
'imported_part_0', 'imported_part_1', 'imported_part_2',
'imported_part_3', 'imported_part_4', 'imported_part_5'
]
for i in range(object_number):
object_name = random.choice(object_list)
object_list.remove(object_name)
object_name_list.append(object_name)
print('Adding %s' % object_name)
res, object_handle = vrep.simxGetObjectHandle(
cid, object_name, vrep.simx_opmode_oneshot_wait)
object_handle_list.append(object_handle)
object_pos = [0, 0, 0.3]
a = random.uniform(-90, 90)
b = random.uniform(-90, 90)
g = random.uniform(-90, 90)
object_angle = [a, b, g]
vrep.simxSetObjectPosition(cid, object_handle, -1, object_pos,
vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectOrientation(cid, object_handle, -1, object_angle,
vrep.simx_opmode_oneshot_wait)
return object_name_list, object_handle_list
def joy_control_cb(self, data):
returnCode, self.target_sphere_handle = vrep.simxGetObjectHandle(
self.clientID, 'Target_sphere', vrep.simx_opmode_oneshot_wait)
position = []
if returnCode == 0:
returnCode, target_sphere_position = vrep.simxGetObjectPosition(
self.clientID, self.target_sphere_handle, -1,
vrep.simx_opmode_oneshot_wait)
returnCode, target_sphere_orentation = vrep.simxGetObjectOrientation(
self.clientID, self.target_sphere_handle, -1,
vrep.simx_opmode_oneshot_wait)
position[0] = target_sphere_position[
0] + 0.001 * data.buttons[3] - 0.001 * data.buttons[2]
position[1] = target_sphere_position[
1] + 0.001 * data.buttons[4] - 0.001 * data.buttons[1]
position[2] = target_sphere_position[
2] + 0.001 * data.buttons[6] - 0.001 * data.buttons[5]
orentation[0] = target_sphere_orentation[
0] + 0.01 * data.buttons[14] - 0.01 * data.buttons[15]
orentation[1] = target_sphere_orentation[
1] + 0.001 * data.buttons[4] - 0.001 * data.buttons[1]
orentation[2] = target_sphere_orentation[
2] + 0.01 * data.buttons[12] - 0.01 * data.buttons[13]
vrep.simxSetObjectPosition(self.clientID, self.target_sphere_handle,
-1, position, vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectOrientation(self.clientID, self.target_sphere_handle,
-1, orentation,
vrep.simx_opmode_oneshot_wait)
def two_legs_recovery(n=N):
print("Start recovering legs!")
init_position = np.zeros((2, 3))
Lx = np.zeros((2, n + 1))
Ly = np.zeros((2, n + 1))
Lz = np.zeros((2, n + 1))
res, init_position[0] = vrep.simxGetObjectPosition(
clientID, S1[1], BCS, vrep.simx_opmode_oneshot_wait)
res, init_position[1] = vrep.simxGetObjectPosition(
clientID, S1[4], BCS, vrep.simx_opmode_oneshot_wait)
for i in range(n + 1):
Lx[0][i] = init_position[0][0] + i * (INIT_POSITION[1][0] -
init_position[0][0]) / n
Ly[0][i] = init_position[0][1] + i * (INIT_POSITION[1][1] - 0.2 -
init_position[0][1]) / n
Lz[0][i] = init_position[0][2] + i * (INIT_POSITION[1][2] -
init_position[0][2]) / n
for i in range(n + 1):
Lx[1][i] = init_position[1][0] + i * (INIT_POSITION[4][0] -
init_position[1][0]) / n
Ly[1][i] = init_position[1][1] + i * (INIT_POSITION[4][1] + 0.2 -
init_position[1][1]) / n
Lz[1][i] = init_position[1][2] + i * (INIT_POSITION[4][2] -
init_position[1][2]) / n
for i in range(n):
vrep.simxSynchronousTrigger(clientID)
vrep.simxSetObjectPosition(clientID, Tip_target[1], BCS,
[Lx[0][i], Ly[0][i], Lz[0][i]],
vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectPosition(clientID, Tip_target[4], BCS,
[Lx[1][i], Ly[1][i], Lz[1][i]],
vrep.simx_opmode_oneshot_wait)
def randomize_gripper_location(self):
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
_, sawyer_target_orientation = vrep.simxGetObjectOrientation(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
x_pos = np.random.uniform(low=1.028, high=1.242, size=1)[0]
y_pos = np.random.uniform(low=1.1, high=1.278, size=1)[0]
new_position = np.array([x_pos, y_pos, sawyer_target_position[2]])
orientation = np.random.uniform(low=0.01745329252, high=1.5533430343, size=1)[0]
new_orientation = np.array([sawyer_target_orientation[0], \
orientation, sawyer_target_orientation[2]])
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
move_direction = np.asarray([new_position[0] - sawyer_target_position[0], \
new_position[1] - sawyer_target_position[1], new_position[2] - \
sawyer_target_position[2]])
move_magnitude = np.linalg.norm(move_direction)
move_step = 0.01*move_direction/move_magnitude
num_move_steps = int(np.floor(move_magnitude/0.01))
remaining_magnitude = -num_move_steps * 0.01 + move_magnitude
remaining_distance = remaining_magnitude * move_direction/move_magnitude
for step_iter in range(num_move_steps): #selects action and executes action
vrep.simxSetObjectPosition(self.client_id, self.sawyer_target_handle, -1, \
(sawyer_target_position[0] + move_step[0], sawyer_target_position[1] + \
move_step[1], sawyer_target_position[2] + move_step[2]), vrep.simx_opmode_blocking)
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
vrep.simxSetObjectPosition(self.client_id, self.sawyer_target_handle, -1, \
(sawyer_target_position[0] + remaining_distance[0], sawyer_target_position[1] + \
remaining_distance[1], sawyer_target_position[2] + remaining_distance[2]), \
vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
_, sawyer_orientation = vrep.simxGetObjectOrientation(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
rotation_step = 0.3 if (new_orientation[1] - sawyer_orientation[1] > 0) else -0.3
num_rotation_steps = int(np.floor((new_orientation[1] - \
sawyer_orientation[1]) / rotation_step))
for step_iter in range(num_rotation_steps):
vrep.simxSetObjectOrientation(self.client_id, self.sawyer_target_handle, \
-1, (sawyer_orientation[0], sawyer_orientation[1] + rotation_step, \
sawyer_orientation[2]), vrep.simx_opmode_blocking)
_, sawyer_orientation = vrep.simxGetObjectOrientation(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
vrep.simxSetObjectOrientation(self.client_id, self.sawyer_target_handle, \
-1, (sawyer_orientation[0], new_orientation[1], sawyer_orientation[2]), \
vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
def getDroneImage(clientID, height):
position = np.asarray([0, 0, height])
# Get Drone Handle
errorCode, droneHandle = vrep.simxGetObjectHandle(
clientID, 'drone', vrep.simx_opmode_oneshot_wait)
# Set Drone Height (in meters)
vrep.simxSetObjectPosition(clientID, droneHandle, -1, position,
vrep.simx_opmode_oneshot_wait)
# Get Drone Camera Image (First Call)
errorDrone, resolution, droneImage = vrep.simxGetVisionSensorImage(
clientID, droneHandle, 0, vrep.simx_opmode_streaming)
count = 0
while (vrep.simxGetConnectionId(clientID) != -1):
# Read in drone camera image (Note operating mode changed to buffer)
errorDrone, resolution, droneImage = vrep.simxGetVisionSensorImage(
clientID, droneHandle, 0, vrep.simx_opmode_buffer)
count += 1
# quit after 5 images read (first images may be empty)
if (errorDrone == vrep.simx_return_ok and count >= 1):
droneImage = np.array(droneImage, dtype=np.uint8)
droneImage.resize([resolution[1], resolution[0], 3])
break
return resolution, droneImage
def robotMove(clientId, robotTargetHandle, vectorXYZ, stepSize):
res, currentTargetPos = vrep.simxGetObjectPosition(
clientId, robotTargetHandle, -1, vrep.simx_opmode_oneshot_wait)
finalPosX = currentTargetPos[0] + vectorXYZ[0]
finalPosY = currentTargetPos[1] + vectorXYZ[1]
finalPosZ = currentTargetPos[2] + vectorXYZ[2]
# Limit the boundary of Y & Z axis of the UR5 robot
# by checking the desired position
if finalPosY > -0.425 and finalPosY < 0.425:
movingPermissionY = 1
else:
movingPermissionY = 0
if finalPosZ > 0.3 and finalPosZ < 0.9:
movingPermissionZ = 1
else:
movingPermissionZ = 0
# Moving robot to the desired position in every point of the path
for i in range(0, stepSize):
xPos = currentTargetPos[0] + (vectorXYZ[0] * (i + 1) / stepSize)
yPos = currentTargetPos[1] + ((vectorXYZ[1] *
(i + 1) / stepSize) * movingPermissionY)
zPos = currentTargetPos[2] + ((vectorXYZ[2] *
(i + 1) / stepSize) * movingPermissionZ)
vrep.simxSetObjectPosition(clientId, robotTargetHandle, -1,
[xPos, yPos, zPos],
vrep.simx_opmode_oneshot_wait)
def collectNearestBlock():
blockHandle,blockName,distance,direction = findEnergyBlocks()[0]
if distance <= 0.5:
vrep.simxSetObjectPosition(robot.clientID, blockHandle, -1, [1000,1000,-2], vrep.simx_opmode_oneshot)
blockHandleArray[blockName][-1] = [1000,1000,-2]
return('Energy collected :)')
return('No blocks nearby :(')
def remove_obj(self, remove_obj_idx):
assert self.clientID != -1, 'Failed to connect to the V-rep server'
vrep.simxSetObjectPosition(self.clientID,
self.handle_obj[remove_obj_idx],
self.handle_robot, [0.8, 0, 0],
vrep.simx_opmode_oneshot)
def checkCollision(self, start, goal):
# m=simGetObjectMatrix(handle,-1)
# mRot=simBuildMatrix({0,0,0},{0,0,rot})
# newM=simMultiplyMatrices(m,mRot)
# simSetObjectMatrix(handle,-1,newM)
# print ("check collision: ", start, goal)
vrep.simxSetObjectPosition(self.clientID, self.robot.target, -1, goal,
vrep.simx_opmode_oneshot_wait)
collision_handles = []
for table in self.tables:
collision_handles.append(table.sim_table)
for chair in table.chairs:
collision_handles.append(chair)
for human in table.humans:
collision_handles.append(human)
returnCode,collision,_,_,_ = vrep.simxCallScriptFunction(self.clientID,"target",vrep.sim_scripttype_childscript,"isInCollision", \
collision_handles,[],[],bytearray(),vrep.simx_opmode_oneshot_wait)
#vrep.simxSetObjectPosition(self.clientID, self.robot.target, -1, start, vrep.simx_opmode_oneshot_wait)
if len(collision) > 0 and collision[0] == 1:
print("inCollision")
return 1
return 0
def shake_arm(self):
self.move_to_positions = []
for i in range(4):
self.move_to_positions.append([np.random.uniform(low=1.018, high=1.223, size=1)[0], \
np.random.uniform(low=0.951, high=1.294, size=1)[0], np.random.uniform(low=0.1, \
high=0.18, size=1)[0]])
for i in range(len(self.move_to_positions)):
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
move_position = self.move_to_positions[i]
move_direction = np.asarray([move_position[0] - sawyer_target_position[0], \
move_position[1] - sawyer_target_position[1], move_position[2] - \
sawyer_target_position[2]])
move_magnitude = np.linalg.norm(move_direction)
move_step = 0.01 * move_direction / move_magnitude
num_move_steps = int(np.floor(move_magnitude / 0.01))
for step_iter in range(num_move_steps):
vrep.simxSetObjectPosition(self.client_id, self.sawyer_target_handle, -1, \
(sawyer_target_position[0]+move_step[0], sawyer_target_position[1] + \
move_step[1], sawyer_target_position[2] + move_step[2]), \
vrep.simx_opmode_blocking)
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
_ = vrep.simxSetObjectPosition(self.client_id, self.sawyer_target_handle, -1, \
move_position, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
def locate_pen(x, y, z, target, plane, clientID):
'''
VREP locate pen function on the position x, y, z on the space
taking as reference targer and plane
'''
vrep.simxSetObjectPosition(clientID, target, plane, [x, y, z],
vrep.simx_opmode_oneshot_wait)
def set_initial_position(self):
self.open_hand()
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
move_direction = np.asarray([self.init_endpoint_pos[0] - sawyer_target_position[0], \
self.init_endpoint_pos[1] - sawyer_target_position[1], self.init_endpoint_pos[2] - \
sawyer_target_position[2]])
move_magnitude = np.linalg.norm(move_direction)
move_step = 0.01 * move_direction / move_magnitude
num_move_steps = int(np.floor(move_magnitude / 0.01))
for step_iter in range(num_move_steps):
vrep.simxSetObjectPosition(self.client_id, self.sawyer_target_handle, \
-1, (sawyer_target_position[0] + move_step[0], sawyer_target_position[1] + \
move_step[1], sawyer_target_position[2] + move_step[2]), vrep.simx_opmode_blocking)
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
vrep.simxSetObjectPosition(self.client_id, self.sawyer_target_handle, -1, \
self.init_endpoint_pos, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
vrep.simxSetObjectOrientation(self.client_id, self.sawyer_target_handle, -1, \
(self.init_endpoint_ori[0], self.init_endpoint_ori[1], self.init_endpoint_ori[2]), \
vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
for i in range(len(self.joint_handles)):
vrep.simxSetJointPosition(self.client_id, self.joint_handles[i], \
self.joint_positions[i], vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(self.client_id, self.joint_handles[i], -1, \
self.joint_orientation[i], vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
def set_gripper_position(self):
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.robot.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
self.image = self.robot.get_image()
move_direction = np.asarray([self.pickup_position[0] - sawyer_target_position[0], \
self.pickup_position[1] - sawyer_target_position[1], self.pickup_position[2] - \
sawyer_target_position[2]])
move_magnitude = np.linalg.norm(move_direction)
move_step = 0.03 * move_direction / move_magnitude
num_move_steps = int(np.floor(move_magnitude / 0.03))
remaining_magnitude = -num_move_steps * 0.03 + move_magnitude
remaining_distance = remaining_magnitude * move_direction / move_magnitude
for step_iter in range(
num_move_steps): #selects action and executes action
vrep.simxSetObjectPosition(self.client_id, self.robot.sawyer_target_handle, -1, \
(sawyer_target_position[0] + move_step[0], sawyer_target_position[1] + \
move_step[1], sawyer_target_position[2] + move_step[2]), vrep.simx_opmode_blocking)
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.robot.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
vrep.simxSetObjectPosition(self.robot.client_id, self.robot.sawyer_target_handle, -1, \
(sawyer_target_position[0] + remaining_distance[0], sawyer_target_position[1] + \
remaining_distance[1], sawyer_target_position[2]+ remaining_distance[2]), \
vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
def reset(self):
super(DishRackEnv, self).reset()
self.rack_pos[0] = self.np_random.uniform(rack_lower[0], rack_upper[0])
self.rack_pos[1] = self.np_random.uniform(rack_lower[1], rack_upper[1])
self.rack_rot[0] = self.np_random.uniform(rack_lower[2], rack_upper[2])
vrep.simxSetObjectPosition(self.cid, self.rack_handle, -1,
self.rack_pos, vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(self.cid, self.rack_handle,
self.rack_rot_ref, self.rack_rot,
vrep.simx_opmode_blocking)
trg_rot = catch_errors(
vrep.simxGetObjectOrientation(self.cid, self.target_handle, -1,
vrep.simx_opmode_blocking))
vrep.simxSetObjectOrientation(self.cid, self.mv_trg_handle, -1,
trg_rot, vrep.simx_opmode_blocking)
if self.vis_mode:
# OTHER POSES
stand_height_diff = np.append([0, 0],
self.np_random.uniform(
-self.max_height_displacement,
self.max_height_displacement, 1))
vrep.simxSetObjectPosition(self.cid, self.stand_h, -1,
self.init_stand_pos + stand_height_diff,
vrep.simx_opmode_blocking)
self.randomise_domain()
return self._get_obs()
def reset(self):
self.get_point = False
self.grab_counter = 0
self.get_obstacles = False
self.obstacles_counter = 0
_ = vrep.simxSetObjectPosition(self.clientID,self.ball1Handle, -1,self.pos1 , vrep.simx_opmode_oneshot)
_ = vrep.simxSetObjectPosition(self.clientID,self.ball2Handle , -1,self.pos2 , vrep.simx_opmode_oneshot)
# for i in range(self.jointNum):
# vrep.simxSetJointPosition(self.clientID,self.robot1_jointHandle[i], self.config1[i], vrep.simx_opmode_oneshot)
# for i in range(self.jointNum):
# vrep.simxSetJointPosition(self.clientID,self.robot2_jointHandle[i], self.config2[i],vrep.simx_opmode_oneshot)
time.sleep(0.1)
jointConfig1 = np.zeros((self.jointNum,))
jointConfig2 = np.zeros((self.jointNum,))
for i in range(self.jointNum):
_, jpos = vrep.simxGetJointPosition(self.clientID, self.robot1_jointHandle[i], vrep.simx_opmode_blocking)
jointConfig1[i] = jpos
for i in range(self.jointNum):
_, jpos = vrep.simxGetJointPosition(self.clientID, self.robot2_jointHandle[i], vrep.simx_opmode_blocking)
jointConfig2[i] = jpos
s = np.hstack((jointConfig1, jointConfig2))
_, end1 = vrep.simxGetObjectPosition(self.clientID, self.end1_Handle,self.base1Handle, vrep.simx_opmode_blocking)
_, end2 = vrep.simxGetObjectPosition(self.clientID, self.end2_Handle,self.base2Handle, vrep.simx_opmode_blocking)
del end1[2]
del end2[2]
end1 = np.array(end1)
end2 = np.array(end2)
d1 = self.goal_1-end1
d2 = self.goal_2-end2
s = np.hstack((s, np.hstack((d1,d2))))
return s
def lift_arm(self):
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
lift_position_x = 1.137
lift_position_y = 1.2151
lift_position_z = 0.18
self.lift_position = np.array(
[lift_position_x, lift_position_y, lift_position_z])
move_direction = np.asarray([self.lift_position[0] - sawyer_target_position[0], \
self.lift_position[1] - sawyer_target_position[1], self.lift_position[2] - \
sawyer_target_position[2]])
move_magnitude = np.linalg.norm(move_direction)
move_step = 0.01 * move_direction / move_magnitude
num_move_steps = int(np.floor(move_magnitude / 0.01))
for step_iter in range(num_move_steps):
vrep.simxSetObjectPosition(self.client_id, self.sawyer_target_handle, -1, \
(sawyer_target_position[0] + move_step[0], sawyer_target_position[1] + \
move_step[1], sawyer_target_position[2] + move_step[2]), vrep.simx_opmode_blocking)
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
_ = vrep.simxSetObjectPosition(self.client_id, self.sawyer_target_handle, -1, \
self.lift_position, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
def setPosition(self, stateVector=None):
# stateVector = [x, y, theta]
z0 = 0.1587
if stateVector == None:
x0 = self.xi.x
y0 = self.xi.y
theta0 = self.xi.theta
elif len(stateVector) == 3:
x0 = stateVector[0]
y0 = stateVector[1]
theta0 = stateVector[2]
self.xi.x = x0
self.xi.y = y0
self.xi.theta = theta0
else:
raise Exception('Argument error!')
if self.scene.vrepConnected == False:
return
vrep.simxSetObjectPosition(self.scene.clientID, self.robotHandle, -1,
[x0, y0, z0], vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(self.scene.clientID, self.robotHandle,
-1, [0, 0, theta0],
vrep.simx_opmode_oneshot)
message = "Robot #" + str(self.index) + "'s pose is set to "
message += "[{0:.3f}, {1:.3f}, {2:.3f}]".format(x0, y0, theta0)
self.scene.log(message)
def collectNearestBlock(clientID, blockHandleArray, pioneerRobotHandle):
dist2block, id, pioneer2BlockPos = getNearestConcretBlockDist(clientID, blockHandleArray, pioneerRobotHandle)
if dist2block <= 0.35:
vrep.simxSetObjectPosition(clientID, blockHandleArray[id][1], -1, [0,0,-2], vrep.simx_opmode_oneshot)
print('Collect succeed ;)')
else:
print('No blocks nearby')
def render(self, position):
margin = -0.35
# print ("robot pose: ", position)
if self.robot_sim is None:
# x, self.robot_sim = vrep.simxGetObjectHandle(self.clientID, "Quadricopter", vrep.simx_opmode_blocking)
# x, self.robot_sim = vrep.simxLoadModel(self.clientID, self.models_path + "Quadricopter.ttm", 0, vrep.simx_opmode_blocking)
x, self.robot_sim = vrep.simxGetObjectHandle(
self.clientID, "youBot", vrep.simx_opmode_blocking)
# x, self.target = vrep.simxLoadModel(self.clientID, self.models_path + "KUKA YouBot.ttm", 0, vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(self.clientID, self.robot_sim, -1, (position[0], position[1], position[2] + margin + \
math.fabs(vrep.simxGetObjectFloatParameter (self.clientID, self.robot_sim, vrep.sim_objfloatparam_modelbbox_min_z, vrep.simx_opmode_blocking)[1])), vrep.simx_opmode_oneshot)
#x, self.target = vrep.simxGetObjectHandle(self.clientID, "Quadricopter_target", vrep.simx_opmode_blocking) ##quad
x, self.target = vrep.simxGetObjectHandle(
self.clientID, "target", vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(self.clientID, self.target, -1, (position[0], position[1], position[2] + margin + \
math.fabs(vrep.simxGetObjectFloatParameter (self.clientID, self.robot_sim, vrep.sim_objfloatparam_modelbbox_min_z, vrep.simx_opmode_blocking)[1])), vrep.simx_opmode_oneshot)
x, self.target_hall = vrep.simxGetObjectHandle(
self.clientID, "inflated_convex_hall_target",
vrep.simx_opmode_blocking)
# vrep.simxSetObjectPosition(self.clientID, self.target, -1, (position[0], position[1] + 1, position[2] + margin + \
# math.fabs(vrep.simxGetObjectFloatParameter (self.clientID, self.robot_sim, vrep.sim_objfloatparam_modelbbox_min_z, vrep.simx_opmode_blocking)[1])), vrep.simx_opmode_oneshot)
# vrep.simxSetObjectParent(self.clientID,self.target,-1,True, vrep.simx_opmode_blocking)
# x, self.goal_robot = vrep.simxLoadModel(self.clientID, self.models_path + "KUKA YouBot.ttm", 0, vrep.simx_opmode_blocking)
# x, self.goal_robot = vrep.simxGetObjectHandle(self.clientID, "Quadricopter#0", vrep.simx_opmode_blocking)
# simxInt simxSetModelProperty(self.clientID,self.goal_robot,vrep.sim_modelproperty_not_collidable,vrep.simx_opmode_oneshot)
##sim.modelproperty_not_visible
else:
pass
def move_down(self): #3 time-steps
_, object_position = vrep.simxGetObjectPosition(self.client_id, \
self.robot.object_handle[0], -1, vrep.simx_opmode_blocking)
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.robot.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
move_direction = np.asarray([self.pickup_position[0] - sawyer_target_position[0], \
self.pickup_position[1] - sawyer_target_position[1], object_position[2] + 0.01 \
- sawyer_target_position[2]])
move_magnitude = np.linalg.norm(move_direction)
move_step = 0.03 * move_direction / move_magnitude
num_move_steps = int(np.floor(move_magnitude / 0.03))
remaining_magnitude = -num_move_steps * 0.03 + move_magnitude
remaining_distance = remaining_magnitude * move_direction / move_magnitude
for step_iter in range(num_move_steps):
vrep.simxSetObjectPosition(self.client_id, self.robot.sawyer_target_handle,\
-1, (sawyer_target_position[0] + move_step[0], sawyer_target_position[1] \
+ move_step[1], sawyer_target_position[2] + move_step[2]), \
vrep.simx_opmode_blocking)
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id,\
self.robot.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
vrep.simxSetObjectPosition(self.robot.client_id, self.robot.sawyer_target_handle, \
-1, (sawyer_target_position[0] + remaining_distance[0], sawyer_target_position[1] \
+ remaining_distance[1], sawyer_target_position[2]+ remaining_distance[2]), \
vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
def render_chair(self, table, position, offset):
x, chair = vrep.simxLoadModel(self.clientID,
self.models_path + "dining chair.ttm", 0,
vrep.simx_opmode_blocking)
x = vrep.simxSetObjectParent(self.clientID, chair, table, True,
vrep.simx_opmode_blocking)
offset_z = math.fabs(
vrep.simxGetObjectFloatParameter(
self.clientID, chair, vrep.sim_objfloatparam_modelbbox_min_z,
vrep.simx_opmode_blocking)[1])
vrep.simxSetObjectPosition(
self.clientID, chair, -1,
(position[0] + offset[0], position[1] + offset[1],
position[2] + offset_z + offset[2]), vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(self.clientID, chair, -1,
(offset[3], offset[4], offset[5]),
vrep.simx_opmode_oneshot)
#returnCode, prop = vrep.simxGetModelProperty(self.clientID,chair,vrep.simx_opmode_blocking)
# print (prop, prop + vrep.sim_modelproperty_not_respondable + vrep.sim_modelproperty_not_collidable)
#returnCode2 = vrep.simxSetModelProperty(self.clientID, chair, prop + vrep.sim_modelproperty_not_respondable, vrep.simx_opmode_blocking)
#returnCode3, prop = vrep.simxGetModelProperty(self.clientID,chair,vrep.simx_opmode_blocking)
# print (returnCode, returnCode2, returnCode3, prop)
# vrep.simxSetModelProperty(self.clientID,chair,vrep.sim_modelproperty_not_collidable,vrep.simx_opmode_oneshot)
return chair
def render_human(self, chair, position, offset):
x, person = vrep.simxLoadModel(self.clientID,
self.models_path + "Sitting Bill.ttm",
0, vrep.simx_opmode_blocking)
x = vrep.simxSetObjectParent(self.clientID, person, chair, True,
vrep.simx_opmode_blocking)
offset_z = vrep.simxGetObjectFloatParameter(
self.clientID, person, vrep.sim_objfloatparam_modelbbox_max_z,
vrep.simx_opmode_blocking)[1]
vrep.simxSetObjectPosition(self.clientID, person, -1,
(position[0] + offset[0], position[1] +
offset[1], position[2] + offset[2]),
vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(self.clientID, person, -1,
(offset[3], offset[4], offset[5]),
vrep.simx_opmode_oneshot)
# vrep.simxSetModelProperty(self.clientID,person,vrep.sim_modelproperty_not_collidable,vrep.simx_opmode_oneshot)
# x, objs = vrep.simxGetObjects(self.clientID, vrep.sim_handle_all, vrep.simx_opmode_oneshot_wait)
# x, joint = vrep.simxGetObjectHandle(self.clientID, 'Bill_leftElbowJoint', vrep.simx_opmode_oneshot_wait)
# x, shoulder = vrep.simxGetObjectHandle(self.clientID, 'Bill_leftShoulderJoint', vrep.simx_opmode_oneshot_wait)
# rotationMatrix = (-1,0,0,0,0,-1,0,0,0,0,-1,0)
# x = vrep.simxSetSphericalJointMatrix(self.clientID, shoulder, rotationMatrix,vrep.simx_opmode_streaming)
#Moving Rotational Joint
# ang = -math.pi/2
# x = vrep.simxSetJointPosition(self.clientID, joint, ang, vrep.simx_opmode_oneshot)
# x, pos = vrep.simxGetJointPosition(self.clientID, joint, vrep.simx_opmode_streaming)
return person
def goForOffset(dummy, hand, base, maxVel, maxAccel, maxJerk, ikSteps, emptyBuff, pThreshold, oThreshold, offsetX, offsetY):
randomX = random.uniform(-offsetX, offsetX) / 2
randomY = random.uniform(-offsetY, offsetY) / 2
res, dummyPosition = vrep.simxGetObjectPosition(clientID, dummy, base, vrep.simx_opmode_blocking)
dummyPosition = [dummyPosition[0]+randomX, dummyPosition[1]+randomY, dummyPosition[2]]
vrep.simxSetObjectPosition(clientID, dummy, base, dummyPosition, vrep.simx_opmode_blocking)
moveToDummy(dummy, hand, base, maxVel, maxAccel, maxJerk, ikSteps, emptyBuff, pThreshold, oThreshold)
def seat(
self, data, chair
): #Seats person at desired chair. In VREP, switches model from a standing bill to a sitting bill
data.bills.remove(self.billID)
data.billID = getBillID(data.bills)
self.type = "Seated"
self.location = chair.location
self.vrepOrientation = [0, 0, chair.vrepOrientation[2] - math.pi / 2]
if self.vrepID != None:
vrep.simxRemoveModel(clientID, self.vrepID,
vrep.simx_opmode_oneshot)
self.vrepPos = [
chair.vrepPos[0],
chair.vrepPos[1] - math.cos(chair.vrepOrientation[2]) * -0.05, 0
]
error, self.vrepID = vrep.simxLoadModel(
clientID, "/home/steelshot/vrep/models/people/IK Bill.ttm", 0,
vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(
clientID, self.vrepID, -1,
(self.vrepPos[0], self.vrepPos[1], self.vrepPos[2] - 0.4),
vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(clientID, self.vrepID, -1,
self.vrepOrientation,
vrep.simx_opmode_oneshot)
error, self.rightLegID = vrep.simxGetObjectHandle(
clientID, 'Bill_rightLegJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, rLeg = vrep.simxGetJointPosition(clientID, self.rightLegID,
vrep.simx_opmode_streaming)
error, self.leftLegID = vrep.simxGetObjectHandle(
clientID, 'Bill_leftLegJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.rightKneeID = vrep.simxGetObjectHandle(
clientID, 'Bill_rightKneeJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.leftKneeID = vrep.simxGetObjectHandle(
clientID, 'Bill_leftKneeJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.rightAnkleID = vrep.simxGetObjectHandle(
clientID, 'Bill_rightAnkleJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.leftAnkleID = vrep.simxGetObjectHandle(
clientID, 'Bill_leftAnkleJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.neckID = vrep.simxGetObjectHandle(
clientID, 'Bill_neck' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.rightArmID = vrep.simxGetObjectHandle(
clientID, 'Bill_rightShoulderJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.leftArmID = vrep.simxGetObjectHandle(
clientID, 'Bill_leftShoulderJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.rightElbowID = vrep.simxGetObjectHandle(
clientID, 'Bill_rightElbowJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.leftElbowID = vrep.simxGetObjectHandle(
clientID, 'Bill_leftElbowJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
def goAndPutDown(startP, regionY, regionZ, box, hand, base, maxVel, maxAccel, maxJerk, ikSteps, emptyBuff, pThreshold, oThreshold):
# create point above final put down position randomly
aboveFinalP1 = createDummy(startP, box)
#randomY = random.uniform(regionY[0], regionY[1])
#randomZ = random.uniform(regionZ[0], regionZ[1])
#res, aboveFinalPPosition = vrep.simxGetObjectPosition(clientID, aboveFinalP, box, vrep.simx_opmode_blocking)
#aboveFinalPPosition = [aboveFinalPPosition[0], randomY, randomZ]
#vrep.simxSetObjectPosition(clientID, aboveFinalP, box, aboveFinalPPosition, vrep.simx_opmode_blocking)
# go to aboveFinalP
#moveToDummy(aboveFinalP, hand, base, maxVel, maxAccel, maxJerk, ikSteps, emptyBuff, pThreshold, oThreshold)
# rotate
angle, configBeforeRotate = goForRandomRotate(aboveFinalP1, hand, base, maxVel, maxAccel, maxJerk, ikSteps, emptyBuff, oThreshold)
aboveFinalP2 = createDummy(hand, base)
# create put down point
putDownP1 = createDummy(aboveFinalP2, -1)
res, putDownPPosition = vrep.simxGetObjectPosition(clientID, putDownP1, -1, vrep.simx_opmode_blocking)
putDownPPosition = [putDownPPosition[0], putDownPPosition[1], tableHeight + gripperLength + 0.01]
vrep.simxSetObjectPosition(clientID, putDownP1, -1, putDownPPosition, vrep.simx_opmode_blocking)
# down to putDownP
putDownP2 = createDummy(putDownP1,base)
moveToDummy(putDownP2, hand, base, maxVel, maxAccel, maxJerk, ikSteps, emptyBuff, pThreshold, oThreshold)
# open
inInts = [motorForce]
inFloats = [motorVelocity1, motorVelocity2]
res,retInts,retFloats,retStrings,retBuffer = vrep.simxCallScriptFunction(clientID, 'Baxter_rightArm_joint1', vrep.sim_scripttype_childscript, 'pyGripper', inInts, inFloats, [], emptyBuff, vrep.simx_opmode_oneshot_wait)
time.sleep(1.5)
# up to aboveFinal
moveToDummy(aboveFinalP2, hand, base, maxVel, maxAccel, maxJerk, ikSteps, emptyBuff, pThreshold, oThreshold)
#return configBeforeRotate
res=vrep.simxRemoveObject(clientID,aboveFinalP1,vrep.simx_opmode_blocking)
res=vrep.simxRemoveObject(clientID,aboveFinalP2,vrep.simx_opmode_blocking)
res=vrep.simxRemoveObject(clientID,putDownP1,vrep.simx_opmode_blocking)
res=vrep.simxRemoveObject(clientID,putDownP2,vrep.simx_opmode_blocking)
def set_position(self, name, x, y):
try:
vrep.simxSetObjectPosition(self.clientID, self.models[name].handle,
-1, (x, 2 - y, self.models[name].z),
vrep.simx_opmode_oneshot_wait)
except KeyError:
print("Could not set Poistion for ", name)
def moveObj(T, clientID, objHandle):
R = T[0:3, 0:3]
p = T[0:3, 3]
cy_thresh = 0
_FLOAT_EPS_4 = np.finfo(float).eps * 4.0
try:
cy_thresh = np.finfo(R.dtype).eps * 4
except ValueError:
cy_thresh = _FLOAT_EPS_4
r11, r12, r13, r21, r22, r23, r31, r32, r33 = R.flat
cy = math.sqrt(r33 * r33 + r23 * r23)
if cy > cy_thresh: # cos(y) not close to zero, standard form
z = math.atan2(-r12, r11) # atan2(cos(y)*sin(z), cos(y)*cos(z))
y = math.atan2(r13, cy) # atan2(sin(y), cy)
x = math.atan2(-r23, r33) # atan2(cos(y)*sin(x), cos(x)*cos(y))
else: # cos(y) (close to) zero, so x -> 0.0 (see above)
# so r21 -> sin(z), r22 -> cos(z) and
z = math.atan2(r21, r22)
y = math.atan2(r13, cy) # atan2(sin(y), cy)
x = 0.0
E = np.array([x, y, z])
vrep.simxSetObjectPosition(clientID, objHandle, -1, p,
vrep.simx_opmode_streaming)
vrep.simxSetObjectOrientation(clientID, objHandle, -1, E,
vrep.simx_opmode_oneshot)
def reset(self):
vrep.simxSetObjectOrientation(self.cid, self.mv_trg_handle, -1,
self.start_rot,
vrep.simx_opmode_blocking)
success = False
while not success:
super(ShelfStackEnv, self).reset()
start_pos = [0, 0, 0]
start_pos[0] = self.np_random.uniform(start_lower[0],
start_upper[0])
start_pos[1] = self.np_random.uniform(start_lower[1],
start_upper[1])
start_pos[2] = self.np_random.uniform(start_lower[2],
start_upper[2])
vrep.simxSetObjectPosition(self.cid, self.mv_trg_handle, -1,
start_pos, vrep.simx_opmode_blocking)
_, pose, _, _ = self.call_lua_function('solve_ik')
for handle, pos in zip(self.joint_handles, pose):
vrep.simxSetJointPosition(self.cid, handle, pos,
vrep.simx_opmode_blocking)
displacement = np.abs(
self.get_vector(self.mv_trg_handle, self.subject_handle))
orientation_diff = np.abs(self.get_mug_orientation())
success = np.all(orientation_diff <= max_rot) and np.all(
displacement <= 0.01)
vrep.simxSetObjectPosition(self.cid, self.anchor_handle,
self.mv_trg_handle, [0., 0., 0.],
vrep.simx_opmode_blocking)
return self._get_obs()
def three_end_step(Lx, Ly, Lz, n=N):
for i in range(1, n + 1):
vrep.simxSynchronousTrigger(clientID)
for j in range(1, 6, 2):
vrep.simxSetObjectPosition(clientID, Tip_target[j], BCS,
[Lx[j][i], Ly[j][i], Lz[j][i]],
vrep.simx_opmode_oneshot_wait)
def copyObject(self,name,position,relativeName=None):
"""
Copy and paste an object in a specific position
"""
if name in self.handles.keys():
pass
else:
self.initHandle(name)
objectHandle=self.handles[name]
if relativeName:
if relativeName in self.handles.keys():
pass
else:
self.initHandle(relativeName)
relativeHandle=self.handles[relativeName]
else:
relativeHandle = -1
returnCode, newObjectHandles=vrep.simxCopyPasteObjects(self.clientID, [objectHandle], vrep.simx_opmode_oneshot_wait)
print("newOH",newObjectHandles)
newObjectHandle=newObjectHandles[0]
vrep.simxSetObjectPosition(self.clientID,newObjectHandle,relativeHandle,position,vrep.simx_opmode_oneshot)
return newObjectHandle
def set_back(self):
vrep.simxSetObjectPosition(self.client_id, self.object_handle[0], -1, \
self.object_position, vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(self.client_id, self.object_handle[0], \
self.robot_handle, self.object_orientation, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
def set_position(self, position):
"""Set the position (x,y,theta) of the robot
Args:
position (list): the position [x,y,theta]
"""
vrep.simxSetObjectPosition(self.client_id, self.pioneer, -1, [position[0], position[1], 0.13879], vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectOrientation(self.client_id, self.pioneer, -1, [0, 0, to_deg(position[2])], vrep.simx_opmode_oneshot_wait)
def movePlant():
dist2Plant = math.sqrt(pos2Plant[1][0]*pos2Plant[1][0] + pos2Plant[1][1]*pos2Plant[1][1] + pos2Plant[1][2]*pos2Plant[1][2])
currentPlantPosition = vrep.simxGetObjectPosition(clientID, indoorPlantHandle, -1, vrep.simx_opmode_oneshot)
plantPosition = [[2, 2, 0.1], [-2, -2, 0.1]]
if dist2Plant < 0.8:
if currentPlantPosition[0] > 0:
PosIdx = 1
else:
PosIdx = 0
vrep.simxSetObjectPosition(clientID, indoorPlantHandle, -1, plantPosition[PosIdx], vrep.simx_opmode_oneshot)
def run(path, handles):
files = []
for i in range(len(handles)):
files.append(open('path_%d.csv' % i, 'w'))
for state in path:
for i in range(len(handles)):
handle = handles[i]
ret, pos = vrep.simxGetObjectPosition(clientID, handle, -1, vrep.simx_opmode_oneshot_wait)
# wait until it's almost in the position
# ------
if (not ret == vrep.simx_return_ok):
print('Failed to retrieve position for Quadricopter_target')
sys.exit(1)
#print pos
new_pos = pos
new_pos[0] = state.positions[i].as_tuple()[0]
new_pos[1] = state.positions[i].as_tuple()[1]
files[i].write(state.positions[i].as_vrep_path_point())
ret = vrep.simxSetObjectPosition(clientID, handle, -1, new_pos, vrep.simx_opmode_oneshot_wait)
if (not ret == vrep.simx_return_ok):
print('Failed to set position for Quadricopter_target')
sys.exit(1)
for i in range(len(handles)):
files[i].close()
def mapgen(scene_name,x,y,z,clientID):
#part which loads
if os.path.isfile('./Mapdata/'+ scene_name +'.npy'):
arr=np.load('./Mapdata/'+ scene_name +'.npy')
#part which creates
else:
#initialize the 2 sensors
errorCode,sensor1=vrep.simxGetObjectHandle(clientID,'Sensor_1',vrep.simx_opmode_oneshot_wait)
errorCode,detectionState,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor (clientID,sensor1,vrep.simx_opmode_streaming)
errorCode,sensor2=vrep.simxGetObjectHandle(clientID,'Sensor_2',vrep.simx_opmode_oneshot_wait)
errorCode,detectionState,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor (clientID,sensor2,vrep.simx_opmode_streaming)
#init some values
time.sleep(3)
xmax=x/0.4
ymax=y/0.4
zmax=z/0.4
xmax=int(math.ceil(xmax))
ymax=int(math.ceil(ymax))
zmax=int(math.ceil(zmax))
arr=np.ndarray(shape=(xmax,ymax,zmax),dtype=int)
#move the sensors through the map and create the data
for index in range(xmax):
for index2 in range(ymax):
for index3 in range(zmax):
#move sensor
x=0.4+0.4*index
y=0.2+0.4*index2
z=0.3+0.4*index3
vrep.simxSetObjectPosition (clientID,sensor1,-1,(x,y,z),vrep.simx_opmode_oneshot)
vrep.simxSetObjectPosition (clientID,sensor2,-1,(x-0.4,y,z),vrep.simx_opmode_oneshot)
time.sleep(0.2)
#check for collision
errorCode,detectionState1,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor (clientID,sensor1,vrep.simx_opmode_buffer)
errorCode,detectionState2,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor (clientID,sensor2,vrep.simx_opmode_buffer)
#save the collision result in an array
if (detectionState1 or detectionState2):
arr[index,index2,index3]=1
else:
arr[index,index2,index3]=0
#save the final array as file
np.save('./Mapdata/'+scene_name, np.ndarray(arr))
return arr
def setPositionObject(self, name, position, relativeName=None):
"""
Set the position of an object
"""
if name in self.handles.keys():
pass
else:
self.initHandle(name)
objectHandle=self.handles[name]
if relativeName:
if relativeName in self.handles.keys():
pass
else:
self.initHandle(relativeName)
relativeHandle=self.handles[relativeName]
else:
relativeHandle = -1
vrep.simxSetObjectPosition(self.clientID,objectHandle,relativeHandle,position,vrep.simx_opmode_oneshot)
def _vrep_set_pos(self, handle, handle_rel, pos, tries=10, fail=True):
r, trycount = -1, 0
while r == -1 and trycount < tries:
if trycount > 0:
time.sleep(0.2)
trycount += 1
r = remote_api.simxSetObjectPosition(self.api_id, handle, handle_rel, [p/100.0 for p in pos],
remote_api.simx_opmode_oneshot_wait)
if fail and r == -1:
raise IOError("could not set position for object (handle: '{}')".format(handle))
return r
def moveHelperArm(self, distTraveled):
SET_OBJECT_POSITION_ERROR_THRESHOLD = 2
MOVE_HELPER_ARM_ERROR = 1
newArmPositionX = float(distTraveled * 1.2)
newArmPosition = (newArmPositionX, 0, 0)
#vrep.simxSetBooleanParameter(self.clientId,sim_boolparam_dynamics_handling_enabled,False,vrep.simx_opmode_oneshot)
#moves relative to the robot itself
error = vrep.simxSetObjectPosition(self.clientId, self.armHandler, self.armHandler, newArmPosition, vrep.simx_opmode_oneshot)
#vrep.simxSetBooleanParameter(self.clientId,sim_boolparam_dynamics_handling_enabled,True,vrep.simx_opmode_oneshot)
if error < SET_OBJECT_POSITION_ERROR_THRESHOLD:
self.armPositionXAxis = newArmPosition
else:
return MOVE_HELPER_ARM_ERROR
def move_hand(x,y,z,object_handle): res,position = vrep.simxGetObjectPosition(clientID,object_handle,-1,vrep.simx_opmode_buffer) print object_handle,position x_pos = position[0] y_pos = position[1] z_pos = position[2] x_pos += x y_pos += y z_pos += z move_pos = [x_pos,y_pos,z_pos] print move_pos res = vrep.simxSetObjectPosition(clientID,object_handle,-1,move_pos,vrep.simx_opmode_oneshot) time.sleep(1.0)
def reset_rob(self): """ Sets the bubbleRob to his starting position; mind the hack, simulation has to be stopped """ ##### Set absolute position #stop simulation vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait) #100ms delay, this is a hack because server isn't ready either time.sleep(0.3) #set on absolute position err = vrep.simxSetObjectPosition(self.clientID, self.bubbleRobHandle, -1, self.bubbleRobStartPosition, vrep.simx_opmode_oneshot_wait) #start simulation again vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait) time.sleep(0.3)
def robotCode(self):
""" OUR ROBOT CODE GOES HERE """
# Shooting Test
vrep.simxSetObjectPosition(
self.clientID, self.ball,-1,[0, -0.35], vrep.simx_opmode_streaming)
target = self.aim()
vrep.simxSetObjectPosition(
self.clientID, self.player2, -1, target, vrep.simx_opmode_streaming)
position = []
position = self.kickingPose(target)
kickingPosition = [position[0], position[1], z]
vrep.simxSetObjectPosition(
self.clientID, self.bot, -1, kickingPosition, vrep.simx_opmode_streaming)
vrep.simxSetObjectOrientation(
self.clientID, self.bot, -1, [self.eulerAngles[0], self.eulerAngles[1], position[2]], vrep.simx_opmode_streaming)
time.sleep(2)
vrep.simx_opmode_blocking)
count = 0
track_hand = []
track_target = []
target_index = 0
change_target_time = dt*300
# NOTE: main loop starts here ---------------------------------------------
while count < len(target_positions) * change_target_time: # run this many seconds
# every so often move the target
if (count % change_target_time) < dt:
vrep.simxSetObjectPosition(clientID,
target_handle,
-1, # set absolute, not relative position
target_positions[target_index],
vrep.simx_opmode_blocking)
target_index += 1
# get the (x,y,z) position of the target
_, target_xyz = vrep.simxGetObjectPosition(clientID,
target_handle,
-1, # retrieve absolute, not relative, position
vrep.simx_opmode_blocking)
if _ !=0 : raise Exception()
track_target.append(np.copy(target_xyz)) # store for plotting
target_xyz = np.asarray(target_xyz)
for ii,joint_handle in enumerate(joint_handles):
old_q = np.copy(q)
def mapgen_fast(scene_name,x,y,z,clientID):
#load mapdata if file exits
if os.path.isfile('./Mapdata/'+scene_name+'.txt'):
fo = open('./Mapdata/'+scene_name+'.txt', "r")
data_string = fo.read();
print "Read String is : ", str
# Close opend file
fo.close()
arr = np.loads(data_string)
return arr
#otherwise generate mapdata
else:
x0=x
y0=y
#generate the 100 sensors and put them to theirstarting position
errorCode,sensor1=vrep.simxGetObjectHandle(clientID,'Sensor_1',vrep.simx_opmode_oneshot_wait)
objectHandles=np.array([sensor1])
sensor_data=np.zeros(shape=(10,10,1),dtype=int)
for x in range(10):
for y in range(10):
returnCode,new_sensor_handle=vrep.simxCopyPasteObjects(clientID,objectHandles,vrep.simx_opmode_oneshot_wait)
vrep.simxReadProximitySensor(clientID,new_sensor_handle[0],vrep.simx_opmode_streaming)
sensor_data[x,y]=new_sensor_handle[0]
x1=0.4+x*0.4
y1=0.2+y*0.4
z1=0.21
vrep.simxSetObjectPosition (clientID,sensor_data[x,y],-1,(x1,y1,z1),vrep.simx_opmode_oneshot)
#init some values
xmax=x0/4
ymax=y0/4
zmax=z/0.4
xmax=int(math.ceil(xmax))
ymax=int(math.ceil(ymax))
zmax=int(math.ceil(zmax))
arr=np.zeros(shape=(xmax*10,ymax*10,zmax),dtype=int)
#loop to detect the whole area
for index in range(xmax):
for index2 in range(ymax):
for index3 in range(zmax):
for x in range(10):
for y in range(10):
errorCode,detectionState1,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor (clientID,sensor_data[x,y],vrep.simx_opmode_buffer)
#save result
if (detectionState1):
arr[index*10+x,index2*10+y,index3]=1
else:
arr[index*10+x,index2*10+y,index3]=0
for x in range(10):
for y in range(10):
x1=0.4+4*(index)+x*0.4
y1=0.2+4*(index2)+y*0.4
z1=0.4*index3+0.21
vrep.simxSetObjectPosition (clientID,sensor_data[x,y],-1,(x1,y1,z1),vrep.simx_opmode_oneshot)
time.sleep(1)
#save mapdata to file
arr=save(arr)
data_string=np.ndarray.dumps(arr)
print data_string
fo = open('./Mapdata/'+scene_name+'.txt', "w")
fo.write(data_string);
fo.close()
return arr
def set_target(self,x,y,z,rot):
vrep.simxSetObjectPosition(self.clientID,self.target,-1,(x,y,z), vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(self.clientID,self.target,-1,(0,0,rot), vrep.simx_opmode_oneshot)
self.oldtarget = (x,y,z,rot)
def set_target_position(self,target_position): 'Set the target position in the simulation' rc = vrep.simxSetObjectPosition(self.clientID, self.target, -1, target_position, vrep.simx_opmode_oneshot)
else:
motionProxy.append(ALProxy('ALMotion',naoIP[0],naoPort[0]))
postureProxy.append(ALProxy('ALRobotPosture', naoIP[0], naoPort[0]))
y=0.8
#Pause the simulation to avoid collision problems
vrep.simxPauseSimulation(clientID,vrep.simx_opmode_oneshot)
time.sleep(2)
for i in range(0,nbrOfNao-1):
motionProxy.append(ALProxy('ALMotion',naoIP[i+1],naoPort[i+1]))
postureProxy.append(ALProxy('ALRobotPosture', naoIP[i+1], naoPort[i+1]))
#Create new NAo in VRep
vrep.simxCopyPasteObjects(clientID,NAO_Handle,vrep.simx_opmode_oneshot_wait)
#Get the handle of the new NAO
copyNAO = vrep.simxGetObjectHandle(clientID,'NAO#'+str(i),vrep.simx_opmode_oneshot_wait)
#Change the position of the new NAO so it won't collide with others
vrep.simxSetObjectPosition(clientID,copyNAO[1],-1,[0,y,0.3518],vrep.simx_opmode_oneshot )
y+=0.8
#Restart the simulation
vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot)
print '================ Posture Initialization ================'
#Go to the posture StandZero
print 'Posture Initialization : StandZero'
for i in range(0,nbrOfNao):
motionProxy[i].stiffnessInterpolation('Body', 1.0, 1.0)
postureProxy[i].goToPosture('StandZero',1.0)
print '================ Handle Creation for the new NAO ================'
a=vrep.simxGetObjectGroupData(clientID,vrep.sim_object_joint_type,0,vrep.simx_opmode_oneshot_wait)
get_new_nao_handles(nbrOfNao,clientID,Body)
thread=[]
def initConcretBlockPosition(clientID, blockHandleArray):
import random
for i_block in range(7,12):
rand_loc = [random.random()*6-1.5, random.random()*7-2.5, 0.0537] # x[-1.5,4.5] y[-2.5,-4.5]
vrep.simxSetObjectPosition(clientID, blockHandleArray[i_block][1], -1, rand_loc, vrep.simx_opmode_oneshot)
angle = np.arccos(cos_angle) # angle in radiant
if a[2]>0:
return angle
else:
return -angle
errorCode,Ball=vrep.simxGetObjectHandle(clientID,'A_star_points',vrep.simx_opmode_oneshot_wait)
objectHandles=np.array([Ball])
for next in path2:
(a,b,c)=next
x=0.4+0.4*a
y=0.2+0.4*b
z=0.3+0.4*c
returnCode,newObjectHandles=vrep.simxCopyPasteObjects(clientID,objectHandles,vrep.simx_opmode_oneshot_wait)
Ball_new=newObjectHandles[0]
vrep.simxSetObjectPosition (clientID,Ball_new,-1,(x,y,z),vrep.simx_opmode_oneshot)
#interpolation
#1. step elimination of unnecessary nodes in the path, makes the path shorter, because of more direct movements
in_progress=1
while in_progress>0:
in_progress=0
i=0
while i <(len(path2)-2):
if collision(path2[i],path2[i+2]):
path2.pop(i+1)
in_progress=1
i=i+1
#Mark the points in the simulation
errorCode,Ball=vrep.simxGetObjectHandle(clientID,'A_star_points_filtered',vrep.simx_opmode_oneshot_wait)
def mapgen_fast(scene_name,x,y,z,clientID):
if os.path.isfile('./Mapdata/'+scene_name+'.txt'):
fo = open('./Mapdata/'+scene_name+'.txt', "rb")
arr = np.load(fo)['arr_0']
#data_string = fo.read();
#print ("Read String is : ", str)
# Close opend file
fo.close()
#arr = np.loads(data_string)
return arr
else:
x0=x
y0=y
errorCode,sensor1=vrep.simxGetObjectHandle(clientID,'Sensor_1',vrep.simx_opmode_oneshot_wait)
objectHandles=np.array([sensor1])
#create hundred sensors
sensor_data=np.zeros(shape=(10,10,1),dtype=int)
for x in range(10):
for y in range(10):
returnCode,new_sensor_handle=vrep.simxCopyPasteObjects(clientID,objectHandles,vrep.simx_opmode_oneshot_wait)
vrep.simxReadProximitySensor(clientID,new_sensor_handle[0],vrep.simx_opmode_streaming)
sensor_data[x,y]=new_sensor_handle[0]
x1=0.0+x*0.2
y1=0.0+y*0.2
z1=0.0
vrep.simxSetObjectPosition (clientID,sensor_data[x,y],-1,(x1,y1,z1),vrep.simx_opmode_oneshot)
#move sensors to every point of map
xmax=x0/2
ymax=y0/2
zmax=z/0.2
xmax=int(math.ceil(xmax))
ymax=int(math.ceil(ymax))
zmax=int(math.ceil(zmax))
arr=np.zeros(shape=(xmax*10,ymax*10,zmax),dtype=int)
for index in range(xmax):
for index2 in range(ymax):
for index3 in range(zmax):
for x in range(10):
for y in range(10):
#judge barrier
errorCode,detectionState1,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor (clientID,sensor_data[x,y],vrep.simx_opmode_buffer)
if (detectionState1):
arr[index*10+x,index2*10+y,index3]=1 #save result
else:
arr[index*10+x,index2*10+y,index3]=0
for x in range(10):
for y in range(10):
x1=0.0 + 2*index + x*0.2
y1=0.0 + 2*index2 + y*0.2
z1=0.0 + 0.2*index3
vrep.simxSetObjectPosition (clientID,sensor_data[x,y],-1,(x1,y1,z1),vrep.simx_opmode_oneshot)
time.sleep(0.2)
#arr=save(arr)
data_string=np.ndarray.dumps(arr)
#print ("Data string is:", data_string)
fo = open('./Mapdata/'+scene_name+'.txt', "wb")
np.savez(fo,arr)
#fo.write(data_string);
fo.close()
return arr
# Choix de l'expert associe a l'action choisie T= utilities.getTheGoodTree(Pbdd,actionChoisie) if len(T.data) > varKppv: S_predicted = kppv.kppv(actionChoisie,T.data,varKppv) else: S_predicted = 0 pass # On realise l'action dans VREP vrep.simxSetJointTargetVelocity(clientID,leftHandle,actionChoisie[0],vrep.simx_opmode_oneshot) vrep.simxSetJointTargetVelocity(clientID,rightHandle,actionChoisie[1],vrep.simx_opmode_oneshot) vec = toy.toy_controller(actionChoisie[2],epuck_position) if isinstance(vec,type([])) and len(vec)!=0: vrep.simxSetObjectPosition(clientID,toyHandle,-1,vec,vrep.simx_opmode_oneshot) pass # On calcule E(t) if t ==0: err_toy, toyPosition = vrep.simxGetObjectPosition(clientID,toyHandle, -1, vrep.simx_opmode_streaming) else: err_toy, toyPosition = vrep.simxGetObjectPosition(clientID,toyHandle, -1, vrep.simx_opmode_buffer) # Calcul de l'erreur de prediction S_calculated = sqrt((epuck_position[0] - toyPosition[0])**2 + (epuck_position[1] - toyPosition[1])**2) E = abs(S_predicted - S_calculated) # Ajout a la base de donnees de MP # Retires S(t) du vecteur actionChoisie actionChoisie.pop()
#err = vrep.simxSetObjectPosition(clientID, bubbleRobHandle, bubbleRobBodyRespHandle, [1,0,0], vrep.simx_opmode_oneshot_wait)
#printErr(err,"setting bubbleRob position ","err while setting bubbleRob position ")
#restart the simulation
#vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait)
###### Set absolute position
#stop simulation
vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
err,bubbleRobPosition = vrep.simxGetObjectPosition(clientID, bubbleRobBodyRespHandle, -1, vrep.simx_opmode_oneshot_wait)
printErr(err,"get bubbleRob position ","err while get bubbleRob position")
#set on absolute position
err = vrep.simxSetObjectPosition(clientID, bubbleRobBodyRespHandle, -1, [2,2,2], vrep.simx_opmode_oneshot_wait)
printErr(err,"setting bubbleRob position ","err while setting bubbleRob position ")
vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait)
counter = 0
while True:
counter += 1
# move the robot somehow
vrep.simxSetJointTargetVelocity(clientID,leftMotorHandle,3.1415*0.1,vrep.simx_opmode_oneshot);
vrep.simxSetJointTargetVelocity(clientID,rightMotorHandle,3.1415*0.1,vrep.simx_opmode_oneshot);
time.sleep(1)
if counter % 5 == 0:
vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
time.sleep(1)
def setObjectPosition(self, obj, position):
return vrep.simxSetObjectPosition(self.clientID, obj, -1, position, vrep.simx_opmode_streaming)
def position(cid, handle, val):
err = vrep.simxSetObjectPosition(cid, handle, -1, val, vrep_mode)
def __init__(self, initRobotPosition=[2.5, -6.5], boundaries=[[-3, 7],[-7, 7]], redPenalty = 0, rewardStrategy = 'Differential', actionStrategy = 'Absolute',verbose = False):
vrep.simxFinish(-1)
clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,2)
if clientID==-1:
raise Exception('Could not connect to API server')
errorCodeCar,car = vrep.simxGetObjectHandle(clientID,'nakedAckermannSteeringCar',vrep.simx_opmode_oneshot_wait)
errorCodeFrontRight,fr = vrep.simxGetObjectHandle(clientID,'Cylinder7',vrep.simx_opmode_oneshot_wait)
errorCodeBackRight,br = vrep.simxGetObjectHandle(clientID,'Cylinder3',vrep.simx_opmode_oneshot_wait)
if (errorCodeCar!=0):
raise Exception('Could not get car handle')
returnCode=vrep.simxSetObjectPosition(clientID,car,-1,[initRobotPosition[0],initRobotPosition[1],0.1],vrep.simx_opmode_oneshot_wait)
returnCode,position=vrep.simxGetObjectPosition(clientID,car,-1,vrep.simx_opmode_oneshot_wait)
returnCode,positionFR=vrep.simxGetObjectPosition(clientID,fr,-1,vrep.simx_opmode_oneshot_wait)
returnCode,positionBR=vrep.simxGetObjectPosition(clientID,br,-1,vrep.simx_opmode_oneshot_wait)
theta=math.atan2(positionFR[1]-positionBR[1],positionFR[0]-positionBR[0])
errorCodeTarget,target = vrep.simxGetObjectHandle(clientID,'Target',vrep.simx_opmode_oneshot_wait)
if (errorCodeTarget!=0):
raise Exception('Could not get target handle')
returnCode,targetPosition=vrep.simxGetObjectPosition(clientID,target,-1,vrep.simx_opmode_oneshot_wait)
if (returnCode!=0):
raise Exception('Could not get target position')
self.verbose = verbose
self.car = car
self.fr = fr
self.br = br
self.clientID = clientID
self.boundaries = boundaries
self.redBoundaries=[1.5,2.5]
self.robot = Robot(clientID)
self.target = [targetPosition[0],targetPosition[1]]
#self.initState = [position[0],position[1],theta,1*(self.redBoundaries[0]>position[0] or position[0]>self.redBoundaries[1]),0,0]
self.initState = [position[0],position[1],theta]
self.state = self.initState
self.wininngRadius = 0.5
self.redPenalty = redPenalty
self.rewardStrategy = rewardStrategy
self.actionStrategy = actionStrategy
def returnToInitPosition(self):
vrep.simxSetObjectPosition(self.__clientID, self.__bodyHandle, -1, self.__initPosition, vrep.simx_opmode_oneshot)
def target_move(cid,obj,function,*args):
args = cleanse(args[0])
pos = function(args)
vrep.simxSetObjectPosition(cid,obj,-1,pos,mode())
ret, handle = vrep.simxGetObjectHandle( clientID, 'Quadricopter_target', vrep.simx_opmode_oneshot_wait )
if ( not ret == vrep.simx_return_ok ):
print('Failed to retrieve handle for Quadricopter_target')
sys.exit( 1 )
quad = None
while ( 1 ):
ret, pos = vrep.simxGetObjectPosition( clientID, handle, -1, vrep.simx_opmode_oneshot_wait )
if ( not ret == vrep.simx_return_ok ):
print('Failed to retrieve position for Quadricopter_target')
sys.exit( 1 )
print pos
if quad is None:
quad = Quadcopter(pos)
new_pos = quad.update(pos)
# new_pos = pos
# new_pos[0] = new_pos[0]+.01
ret = vrep.simxSetObjectPosition( clientID, handle, -1, new_pos, vrep.simx_opmode_oneshot_wait )
if ( not ret == vrep.simx_return_ok ):
print('Failed to set position for Quadricopter_target')
sys.exit( 1 )
def setBallPose(self, positionxy):
position = np.concatenate((np.asarray(positionxy), np.array([z])))
_ = vrep.simxSetObjectPosition(
self.clientID, self.handle, -1, position, vrep.simx_opmode_oneshot
)
alpha = [1/4,1/4,1/(math.pi)]
print ('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
client_id=vrep.simxStart(ip,port,True,True,5000,5) # Connect to V-REP
if client_id!=-1:
print ('Connected to remote API server on %s:%s' % (ip, port))
res = vrep.simxLoadScene(client_id, scene, 1, vrep.simx_opmode_oneshot_wait)
res, pioneer = vrep.simxGetObjectHandle(client_id, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
res, left_motor = vrep.simxGetObjectHandle(client_id, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
res, right_motor = vrep.simxGetObjectHandle(client_id, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)
# set initial position, -1 is absolute
vrep.simxSetObjectPosition(client_id, pioneer, -1, [position_init[0], position_init[1], 0.13879], vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectOrientation(client_id, pioneer, -1, [0, 0, to_deg(position_init[2])], vrep.simx_opmode_oneshot_wait)
network = NN(ni,nj,nk)
vrep.simxStartSimulation(client_id, vrep.simx_opmode_oneshot_wait)
position = position_init # [x,y,theta]
network_input = [0, 0, 0]
for i in range(ni):
network_input[i] = position[i]-target[i]
prev_error = 100000
#while(distance(position, target)>0.001 or position[2]-target[2]>0.1):
while(True):
