while True:
client_ID = vrep_sim.simxStart('127.0.0.1', 19999, True, False, 5000,
5) # Connect to CoppeliaSim
if client_ID > -1: # connected
print('Connect to remote API server.')
break
else:
print('Failed connecting to remote API server! Try it again ...')
# Pause the simulation
# res = vrep_sim.simxPauseSimulation(client_ID, vrep_sim.simx_opmode_blocking)
delta_t = 0.005 # simulation time step
# Set the simulation step size for VREP
vrep_sim.simxSetFloatingParameter(client_ID,
vrep_sim.sim_floatparam_simulation_time_step,
delta_t, vrep_sim.simx_opmode_oneshot)
# Open synchronous mode
vrep_sim.simxSynchronous(client_ID, True)
# Start simulation
vrep_sim.simxStartSimulation(client_ID, vrep_sim.simx_opmode_oneshot)
# ------------------------------- Initialize simulation model -------------------------------
UR5_sim_model = UR5SimModel()
UR5_sim_model.initializeSimModel(client_ID)
time.sleep(0.1)
t = np.linspace(0, 2 * np.pi, 800)
data_len = len(t)
reference_q = np.zeros((3, data_len))
# 每隔0.2s检测一次，直到连接上V-rep
while True:
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID > -1:
break
else:
time.sleep(0.2)
print("Failed connecting to remote API server!")
print("Connection success!")
RAD2DEG = 180 / math.pi # 常数，弧度转度数
tstep = 0.01 # 定义仿真步长
# 设置仿真步长，为了保持API端与V-rep端相同步长
vrep.simxSetFloatingParameter(clientID,
vrep.sim_floatparam_simulation_time_step, tstep,
vrep.simx_opmode_oneshot)
# 然后打开同步模式
vrep.simxSynchronous(clientID, True)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
baseName = 'Body'
jointName = 'Tran'
_, baseHandle = vrep.simxGetObjectHandle(clientID, baseName,
vrep.simx_opmode_blocking)
_, jointHandle = vrep.simxGetObjectHandle(clientID, jointName,
vrep.simx_opmode_blocking)
print('Handles available!')
jointNames = [
"Pioneer_p3dx", "Pioneer_p3dx_rightMotor", "Pioneer_p3dx_leftMotor"
]
# Get handles for the P3dx and its wheels
jointHandles = getP3dxHandles(jointNames)
# initReads()
# Number of seconds the simulation is going to run
np = 600
simulationTotalTime = 0.15
# Use a custom timestep since we're using the physics engine
hd = 0.005
sim.simxSetFloatingParameter(clientID, sim.sim_floatparam_simulation_time_step,
hd, sim.simx_opmode_oneshot)
# Define initial values for the inputs
tau_r = -0.75
tau_l = -0.75
torqueStep = 0.50
listOfV = []
listOfW = []
t = []
for i in range(9):
# Start simulation
print("Starting!")
sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait)