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 ur5moveto(self, x, y, z):
"""
Move ur5 to location (x,y,z)
"""
vrep.simxSynchronousTrigger(self.clientID) # 让仿真走一步
self.targetQuaternion[0] = 0.707
self.targetQuaternion[1] = 0
self.targetQuaternion[2] = 0.707
self.targetQuaternion[3] = 0 #四元数
self.targetPosition[0] = x
self.targetPosition[1] = y
self.targetPosition[2] = z
vrep.simxPauseCommunication(self.clientID, True) #开启仿真
vrep.simxSetIntegerSignal(self.clientID, 'ICECUBE_0', 21,
vrep.simx_opmode_oneshot)
for i in range(3):
vrep.simxSetFloatSignal(self.clientID, 'ICECUBE_' + str(i + 1),
self.targetPosition[i],
vrep.simx_opmode_oneshot)
for i in range(4):
vrep.simxSetFloatSignal(self.clientID, 'ICECUBE_' + str(i + 4),
self.targetQuaternion[i],
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(self.clientID, False)
vrep.simxSynchronousTrigger(self.clientID) # 进行下一步
vrep.simxGetPingTime(self.clientID) # 使得该仿真步走完
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 close_hand(self):
vrep.simxSetJointForce(self.client_id, self.motor_handle, 100, \
vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(self.client_id, self.motor_handle, \
0.5, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
def step(self, action):
# set actions
self.__MODE.set(action)
vrep.simxSynchronousTrigger(self.__ID)
vrep.simxGetPingTime(self.__ID)
# get new states
return self.__MODE.get()
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 step_simulation(self, torque_commands):
vrep.simxSetJointTargetVelocity(
self.clientID, self.jointHandles[0],
90000 if torque_commands[0] > 0 else -9000,
vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(
self.clientID, self.jointHandles[0], torque_commands[0]
if torque_commands[0] > 0 else -torque_commands[0],
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(
self.clientID, self.jointHandles[1],
90000 if torque_commands[1] > 0 else -9000,
vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(
self.clientID, self.jointHandles[1], torque_commands[1]
if torque_commands[1] > 0 else -torque_commands[1],
vrep.simx_opmode_oneshot)
vrep.simxSynchronousTrigger(
self.clientID
) # Trigger next simulation step (Blocking function call)
vrep.simxGetPingTime(
self.clientID
) # Ensure simulation step has completed (Blocking function call)
def update_sim(self):
for handle, velocity in zip(self.joint_handles, self.target_velocities):
return_code = vrep.simxSetJointTargetVelocity(self.cid,
int(handle), velocity, vrep.simx_opmode_oneshot)
check_for_errors(return_code)
vrep.simxSynchronousTrigger(self.cid)
vrep.simxGetPingTime(self.cid)
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 main():
thetas = [[4, 10, 0, 2, 45, 6, 9],[-3, 12, 3, 5, 7, 9, 1],[-2, 3, 5, 7, 8, 10],[-1, 2, 4, 6, 1, 2, -56],[-0.25, 1, 2, 3, 4, 15, 25]]
print "This is a collision detection simulation for the Baxter robot"
clientID = initialize_sim()
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
joint_library, Larm_joints, Rarm_joints, body_joints, joint_bodynames, joint_Rarm, joint_Larm = declarejointvar(clientID)
collision_library = getCollisionlib(clientID)
for theta_set in thetas:
movebody(clientID, Larm_joints, joint_Larm, theta_set, collision_library[0])
movebody(clientID, Rarm_joints, joint_Rarm, theta_set, collision_library[1])
movebody(clientID, Larm_joints, joint_Larm, [0, 0, 0, 0, 0, 0], collision_library[0])
movebody(clientID, Rarm_joints, joint_Rarm, [0, 0, 0, 0, 0, 0], collision_library[1])
# stop simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
# Before closing the connection to V-REP, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
vrep.simxGetPingTime(clientID)
# Close the connection to V-REP
vrep.simxFinish(clientID)
def close_connection(client_id):
# Before closing the connection to V-REP, make sure that the last command sent out had time to arrive.
# You can guarantee this with (for example):
vrep.simxGetPingTime(client_id)
# Now close the connection to V-REP:
vrep.simxFinish(client_id)
print('Connection to remote API server closed')
def control_vrep(clientID, count):
# 读取Base 和 joint 的handle
jointHandle = np.zeros((jointNum, ), dtype=np.int)
for i in range(jointNum):
_, returnHandle = vrep.simxGetObjectHandle(clientID,
jointName + str(i + 1),
vrep.simx_opmode_blocking)
jointHandle[i] = returnHandle
_, baseHandle = vrep.simxGetObjectHandle(clientID, baseName,
vrep.simx_opmode_blocking)
print('Handles available')
# 首次读取关节的初始值，以streaming的形式
jointConfigure = np.zeros((jointNum, ))
for i in range(jointNum):
_, jpos = vrep.simxGetJointPosition(clientID, jointHandle[i],
vrep.simx_opmode_streaming)
jointConfigure[i] = jpos
vrep.simxSynchronousTrigger(clientID) # 让仿真走一步
### 控制指定关节 ####
vrep.simxPauseCommunication(clientID, True)
vrep.simxSetJointTargetPosition(clientID, jointHandle[count],
jointConfigure[count] + 1 / RAD2EDG,
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
vrep.simxSynchronousTrigger(clientID)
vrep.simxGetPingTime(clientID) # 使得该仿真步
def step(self, action): # execute one step in env
self._set_joint_effort(action)
# torque = [vrep.simxGetJointForce(self.clientID, self.handles['joint'][i], vrep.simx_opmode_blocking)[1]
# for i in range(self.action_dims)] # retrieve the current torque from 7 joints
# print('now=', torque, '\nnext=', action)
# send a synchronization trigger signal to inform V-REP to execute the next simulation step
vrep.simxSynchronousTrigger(self.clientID)
vrep.simxGetPingTime(self.clientID)
Reward, ter_flag, env_info = self._reward(
action) # ter_flag: True if finish the task
# TODO: define the scaled reward
if self.t < self._max_episode_length * 0.8:
scaled_reward = (self.t / self._max_episode_length) * Reward
else:
scaled_reward = (self.final_phase_scaler * self.t /
self._max_episode_length) * Reward
self.t += 1
next_observation = self._get_observation()
self.cur_obs = next_observation
return next_observation, scaled_reward, ter_flag, env_info
def render(self, robot, tables, done=False):
if self.reset:
# self.robot.render([robot.get_feature("x").value-3, robot.get_feature("y").value-3, 2])
self.robot.render([
robot.get_feature("x").value - env_margin,
robot.get_feature("y").value - env_margin, 0
]) #quad
returnCode, envHandle = vrep.simxGetObjectHandle(
self.clientID, "Environment", vrep.simx_opmode_blocking)
self.render_tables(tables, envHandle)
vrep.simxGetPingTime(self.clientID)
self.reset = False
dt = .01
# vrep.simxSetFloatingParameter(clientID, vrep.sim_floatparam_simulation_time_step, dt, vrep.simx_opmode_oneshot)
# vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot)
# set_trace()
else:
# self.robot.render([robot.get_feature("x").value-3, robot.get_feature("y").value-3, 2])
pass
if done:
# vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
vrep.simxFinish(self.clientID)
def run_simulation(self):
self.set_num_steps()
vrep.simxSynchronous(self._clientID,True);
vrep.simxStartSimulation(self._clientID,vrep.simx_opmode_oneshot);
for simStep in range (self._num_steps):
self._pid = set_target_thetas(self._num_steps, self._pid,self._experiment,self._simulator,simStep)
for joint in range(6):
errorCode, self._theta[joint] = vrep.simxGetJointPosition(self._clientID,self._arm_joints[joint],vrep.simx_opmode_blocking)
self._linearVelocity[joint] = self._pid[joint].get_velocity(math.degrees(self._theta[joint]))/self._convertdeg2rad
vrep.simxSetJointTargetVelocity(self._clientID,self._arm_joints[joint],self._linearVelocity[joint],vrep.simx_opmode_oneshot)
vrep.simxSynchronousTrigger(self._clientID)
vrep.simxGetPingTime(self._clientID)
self._positions.append(vrep.simxGetObjectPosition(self._clientID,self._arm_joints[5],-1,vrep.simx_opmode_blocking)[1] + vrep.simxGetObjectOrientation(self._clientID,self._arm_joints[5],-1,vrep.simx_opmode_blocking)[1] + vrep.simxGetObjectPosition(self._clientID,self._cube,-1,vrep.simx_opmode_blocking)[1] + vrep.simxGetObjectQuaternion(self._clientID,self._cube,-1,vrep.simx_opmode_blocking)[1])
vrep.simxStopSimulation(self._clientID, vrep.simx_opmode_blocking)
vrep.simxFinish(self._clientID)
saveStats(self._experiment,self._current_iteration, self._physics_engine,self._simulator, self._positions)
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 exit_gracefully(signum, frame):
running = False
global clientID
# stop the simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
# Before closing the connection to V-REP,
# make sure that the last command sent out had time to arrive.
vrep.simxGetPingTime(clientID)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
print('connection closed...')
original_sigint = signal.getsignal(signal.SIGINT)
# restore the original signal handler as otherwise evil things will happen
# in raw_input when CTRL+C is pressed, and our signal handler is not re-entrant
signal.signal(signal.SIGINT, original_sigint)
try:
if input("\nReally quit? (y/n)> ").lower().startswith('y'):
time.sleep(1)
sys.exit(1)
except KeyboardInterrupt:
print("Ok ok, quitting")
sys.exit(1)
# restore the exit gracefully handler here
signal.signal(signal.SIGINT, exit_gracefully)
def end_simulation(clientID):
# Stop simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
# Before closing the connection to V-REP, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
vrep.simxGetPingTime(clientID)
# Close the connection to V-REP
vrep.simxFinish(clientID)
print("End of simulation")
def update_sim(self):
for handle, velocity in zip(self.joint_handles, self.curr_action):
catch_errors(
vrep.simxSetJointTargetVelocity(self.cid, int(handle),
velocity,
vrep.simx_opmode_oneshot))
vrep.simxSynchronousTrigger(self.cid)
vrep.simxGetPingTime(self.cid)
def sim_stop(self):
if self.clientID != -1:
print("Simulation complete.")
vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_blocking)
vrep.simxGetPingTime(self.clientID)
vrep.simxFinish(self.clientID)
else:
print("Failed connecting to remote API server")
def close_connection(self):
if self.clientID != -1:
# BeforeQSize closing the connection to V-REP, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
vrep.simxGetPingTime(self.clientID)
# Now close the connection to V-REP:
vrep.simxFinish(self.clientID)
print ('Program ended')
def TerminateRemoteAPI(clientId):
# Before closing the connection to V-REP, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
vrep.simxGetPingTime(clientId)
# Now close the connection to V-REP:
vrep.simxFinish(clientId)
print('Program ended')
def disconnect():
# Make sure that the last command sent has arrived
vrep.simxGetPingTime(clientID)
show_msg('RoboNav-DRL: Bye')
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
time.sleep(0.5)
return
def disconnect():
""" Disconnect from the simulator"""
# Make sure that the last command sent has arrived
vrep.simxGetPingTime(clientID)
show_msg('ROBOT: Bye')
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
time.sleep(0.5)
return
def close(self):
'''
Close connection with the robot simulation
:return:
'''
# Before closing the connection to V-REP, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
vrep.simxGetPingTime(self.__clientID)
# Now close the connection to V-REP:
vrep.simxFinish(self.__clientID)
def streamVisionSensor(visionSensorName, clientID, pause=0.0001):
# enable the synchronous mode on the client:
vrep.simxSynchronousTrigger(clientID)
vrep.simxGetPingTime(clientID)
# start the simulation:
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
# enable streaming of the iteration counter:
res, iteration1 = vrep.simxGetIntegerSignal(clientID, "iteration",
vrep.simx_opmode_streaming)
#Get the handle of the vision sensor
res1, visionSensorHandle = vrep.simxGetObjectHandle(
clientID, visionSensorName, vrep.simx_opmode_oneshot_wait)
#Get the image
res2, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_streaming)
#Initialiazation of the figure
time.sleep(0.5)
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer)
im = I.new("RGB", (resolution[0], resolution[1]), "white")
# Init figure
plt.ion()
fig = plt.figure(1)
plotimg = plt.imshow(im, origin='lower')
#Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash
time.sleep(1)
c = 0
while (vrep.simxGetConnectionId(clientID) != -1):
res, iteration1 = vrep.simxGetIntegerSignal(clientID, "iteration",
vrep.simx_opmode_buffer)
print(iteration1)
# if res!=vrep.simx_return_ok:
# iteration1=-1;
# vrep.simxSynchronousTrigger(clientID)
# iteration2=iteration1
# while iteration2==iteration1: # wait until the iteration counter has changed
# res, iteration2 = vrep.simxGetIntegerSignal(clientID,"iteration",vrep.simx_opmode_buffer)
# if res!=vrep.simx_return_ok:
# iteration2=-1
#Get the image of the vision sensor
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer)
#Transform the image so it can be displayed
img = np.array(image, dtype=np.uint8).reshape(
[resolution[1], resolution[0], 3])
#Update the image
fig.canvas.set_window_title(visionSensorName + '_' + str(c))
plotimg.set_data(img)
plt.draw()
plt.pause(pause)
c += 1
print('End of Simulation')
def main():
ur5 = UR5()
ur5.connect()
ur5.ankleinit()
clientID = ur5.get_clientID()
camera = Camera(clientID)
lastCmdTime = vrep.simxGetLastCmdTime(clientID)
vrep.simxSynchronousTrigger(clientID)
count = 0
while vrep.simxGetConnectionId(clientID) != -1 and count < Simu_STEP:
currCmdTime = vrep.simxGetLastCmdTime(clientID)
dt = currCmdTime - lastCmdTime
# Camera achieve data
cur_depth, cur_color = camera.get_camera_data()
cur_depth_path, cur_img_path = camera.save_image(
cur_depth, cur_color, currCmdTime)
# Call affordance map function
affordance_cmd = 'th ' + Lua_PATH + ' -imgColorPath ' + cur_img_path + ' -imgDepthPath ' + cur_depth_path + ' -resultPath ' + Save_PATH_RES + str(
currCmdTime) + '_results.h5'
try:
os.system(affordance_cmd)
print('-- Successfully create affordance map!')
except:
print(
'!!!!!!!!!!!!!!!!!!!!!!!! Error occurred during creating affordance map'
)
hdf2affimg(Save_PATH_RES + str(currCmdTime) + '_results.h5',
currCmdTime)
# TODO: get the push u,v coordinate
u = 256
v = 212
camera_coor = pixel2camera(u, v, cur_depth, camera.intri,
camera.Dis_FAR)
_, ur5_position = vrep.simxGetObjectPosition(
clientID, ur5.get_handle(), -1, vrep.simx_opmode_oneshot_wait)
location = camera2ur5(camera_coor, ur5_position, camera.cam_position)
# Move ur5
# location = random.randint(100, 200, (1, 3))
# location = location[0] / 400
print("UR5 Move to %s" % (location))
ur5.ur5moveto(location[0], location[1], location[2] - 0.05)
count += 1
lastCmdTime = currCmdTime
vrep.simxSynchronousTrigger(clientID)
vrep.simxGetPingTime(clientID)
ur5.disconnect()
def __get(self):
vrep.simxGetPingTime(self.__ID)
self.state = np.array(
vrep.simxUnpackFloats(
vrep.simxGetStringSignal(self.__ID, "states",
vrep.simx_opmode_buffer)[1]))
self.reward = vrep.simxGetFloatSignal(self.__ID, "reward",
vrep.simx_opmode_buffer)[1]
self.done = bool(
vrep.simxGetIntegerSignal(self.__ID, "done",
vrep.simx_opmode_buffer)[1])
def finalize_vrep():
# stop the simulation
vrep.simxStopSimulation(RC.GB_CLIENT_ID(), vrep.simx_opmode_blocking)
# Before closing the connection to V-REP,
#make sure that the last command sent out had time to arrive.
vrep.simxGetPingTime(RC.GB_CLIENT_ID())
# Now close the connection to V-REP:
vrep.simxFinish(RC.GB_CLIENT_ID())
print('V-REP Server Connection closed...')
def step(self):
self.endpoint = self.endpoint + self.v_endpoint*self.dt
self.yaw = self.yaw + self.omega*self.dt
if self.yaw > np.pi:
self.yaw-=2*np.pi
if self.yaw < -np.pi:
self.yaw+=2*np.pi
self.move_endpoint_to(self.endpoint[0],self.endpoint[1],self.endpoint[2])
self.set_endpoint_yawrate(self.omega)
# this code triggers the next simulation step
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
def step(self, actions):
if actions:
vrep.simxCallScriptFunction(self._client_id, 'engine',
vrep.sim_scripttype_childscript, 'act',
actions, [], [], bytearray(),
vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self._client_id)
vrep.simxGetPingTime(self._client_id)
state = self.get_state()
reward = self.get_reward()
goals = self.get_goals(state)
return goals, state, reward
plt.show()
plt.draw()
if __name__ == '__main__':
try:
client_id
except NameError:
client_id = -1
e = vrep.simxStopSimulation(client_id, vrep.simx_opmode_oneshot_wait)
vrep.simxFinish(-1)
client_id = vrep.simxStart('127.0.0.1', 19998, True, True, 5000, 5)
assert client_id != -1, 'Failed connecting to remote API server'
# print ping time
sec, msec = vrep.simxGetPingTime(client_id)
print "Ping time: %f" % (sec + msec / 1000.0)
robot = Robot(client_id)
agent = Agent(robot, alpha=0.1, gamma=0.9, epsilon=0.05, q_init=0.3)
num_episodes = 500
len_episode = 100
return_history = []
try:
for episode in range(num_episodes):
print "start simulation # %d" % episode
with contexttimer.Timer() as timer:
agent.initialize_episode()
body_trajectory = []
# Now try to retrieve data in a blocking fashion (i.e. a service call):
res,objs=vrep.simxGetObjects(clientID,vrep.sim_handle_all,vrep.simx_opmode_blocking)
if res==vrep.simx_return_ok:
print ('Number of objects in the scene: ',len(objs))
else:
print ('Remote API function call returned with error code: ',res)
time.sleep(2)
# Now retrieve streaming data (i.e. in a non-blocking fashion):
startTime=time.time()
vrep.simxGetIntegerParameter(clientID,vrep.sim_intparam_mouse_x,vrep.simx_opmode_streaming) # Initialize streaming
while time.time()-startTime < 5:
returnCode,data=vrep.simxGetIntegerParameter(clientID,vrep.sim_intparam_mouse_x,vrep.simx_opmode_buffer) # Try to retrieve the streamed data
if returnCode==vrep.simx_return_ok: # After initialization of streaming, it will take a few ms before the first value arrives, so check the return code
print ('Mouse position x: ',data) # Mouse position x is actualized when the cursor is over V-REP's window
time.sleep(0.005)
# Now send some data to V-REP in a non-blocking fashion:
vrep.simxAddStatusbarMessage(clientID,'Hello V-REP!',vrep.simx_opmode_oneshot)
# Before closing the connection to V-REP, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
vrep.simxGetPingTime(clientID)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print ('Failed connecting to remote API server')
print ('Program ended')
def disconnect(self):
self.stop_sim()
vrep.simxGetPingTime(self.clientID)
vrep.simxFinish(self.clientID)
