def simulation(self):
sim.simxSynchronous(self.clientID, 1)
#同步模式
sim.simxSynchronousTrigger(self.clientID)
# Start simulation
sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot_wait)
print("Simulation start")
def stepSimulation():
currentStep=client.stepCounter
sim.simxSynchronousTrigger(client.id);
while client.stepCounter==currentStep:
retCode,s=sim.simxGetIntegerSignal(client.id,client.intSignalName,sim.simx_opmode_buffer)
if retCode==sim.simx_return_ok:
client.stepCounter=s
def stepSimulation():
if client.runInSynchronousMode:
currentStep = client.stepCounter
sim.simxSynchronousTrigger(client.id)
while client.stepCounter == currentStep:
retCode, s = sim.simxGetIntegerSignal(
client.id, client.intSignalName,
sim.simx_opmode_buffer)
if retCode == sim.simx_return_ok:
client.stepCounter = s
retCode, res, img = sim.simxGetVisionSensorImage(
client.id, client.visionSensorHandle, 0,
sim.simx_opmode_buffer)
client.lastImageAcquisitionTime = sim.simxGetLastCmdTime(
client.id)
if retCode == sim.simx_return_ok:
sim.simxSetVisionSensorImage(
client.id, client.passiveVisionSensorHandle, img, 0,
sim.simx_opmode_oneshot)
else:
retCode, res, img = sim.simxGetVisionSensorImage(
client.id, client.visionSensorHandle, 0,
sim.simx_opmode_buffer)
if retCode == sim.simx_return_ok:
imageSimTime = sim.simxGetLastCmdTime(client.id)
if client.lastImageAcquisitionTime != imageSimTime:
client.lastImageAcquisitionTime = imageSimTime
sim.simxSetVisionSensorImage(
client.id, client.passiveVisionSensorHandle, img,
0, sim.simx_opmode_oneshot)
def step(self, action, reset):
if sys.version_info[0] == 3:
_ = sim.simxSetObjectPosition(self.clientID, self.target, -1,
self.target_position,
sim.simx_opmode_oneshot)
sensor_data = np.zeros((self.total_sensors), dtype=np.float32)
vel_reading = np.zeros((2), dtype=np.float32)
angular_reading = 0
collision = np.zeros((2), dtype=np.float32)
target_location = np.zeros((3), dtype=np.float32)
target_location = np.round_(
np.subtract(np.array(self.position[:2]),
np.array(self.target_position[:2])), 3)
if (reset == False):
if (self.flag):
_, target_location = sim.simxGetObjectPosition(
self.clientID, self.target, -1, sim.simx_opmode_buffer)
_, bot_location = sim.simxGetObjectPosition(
self.clientID, self.bot, -1, sim.simx_opmode_buffer)
target_location = np.round_([
bot_location[0] - target_location[0],
bot_location[1] - target_location[1]
], 3)
self.flag = True
speed = (self.velocity * action[0]) / 100
turn = (self.angular_velocity * action[1]) / 100
l_wheel_vel = round(
(speed - self.wheel_basis * turn) / self.wheel_radius, 4)
r_wheel_vel = round(
(speed + self.wheel_basis * turn) / self.wheel_radius, 4)
_ = sim.simxSetJointTargetVelocity(self.clientID,
self.left_wheel,
l_wheel_vel,
sim.simx_opmode_streaming)
_ = sim.simxSetJointTargetVelocity(self.clientID,
self.right_wheel,
r_wheel_vel,
sim.simx_opmode_streaming)
#Collision
_, collision[0] = sim.simxGetIntegerSignal(
self.clientID, "collision_wall", sim.simx_opmode_buffer)
_, collision[1] = sim.simxGetIntegerSignal(
self.clientID, "collision_target", sim.simx_opmode_buffer)
sensor_data = self._readsensor_continue()
vel_reading, angular_reading = self._get_velocity_reading_continue(
)
else:
self._create(self.position)
#_ = sim.simxSetObjectPosition(self.clientID, self.target, self.bot, self.target_position, sim.simx_opmode_oneshot)
#target_location = self.position - self.target_position
sim.simxSynchronousTrigger(self.clientID)
return sensor_data, vel_reading, angular_reading, target_location, collision
else:
raw_input('Press <enter> key to step the !')
def step(self, action):
d, Oc = self.get_obs()
observation = np.array([d, Oc])
reward = -(d**2)
if d >= 0.05:
done = False
Vl, Vr = action
else:
Vl = 0
Vr = 0
done = True
reward = self.MaxSteps - self.steps
self.updateVelocities(Vl, Vr)
if d >= 2 or self.steps > (self.MaxSteps - 1):
done = True
if not done:
sim.simxSynchronousTrigger(self.clientID)
self.steps += 1
xc, yc, theta = self.getPositionRobot()
info = {'xc': xc, 'yc': yc}
return observation, reward, done, info
def step(self):
if sys.version_info[0] == 3:
print('enter your code here')
else:
raw_input('Press <enter> key to step the simulation!')
sim.simxSynchronousTrigger(self.clientID)
def velocity(self,max1,max2):
#res,joint0 = sim.simxGetObjectHandle(clientID, "Revolute_joint0", sim.simx_opmode_oneshot_wait)
#res,joint1 = sim.simxGetObjectHandle(clientID, "Revolute_joint1", sim.simx_opmode_oneshot_wait)
res,main_handle = sim.simxGetObjectHandle(self.clientID, 'Cuboid0', sim.simx_opmode_blocking)
res,joint0 = sim.simxGetObjectHandle(self.clientID, "motor", sim.simx_opmode_oneshot_wait)
#res,joint1 = sim.simxGetObjectHandle(self.clientID, "right_motor", sim.simx_opmode_oneshot_wait)
sim.simxSynchronousTrigger(self.clientID)
left_Code =sim.simxSetJointTargetVelocity(self.clientID,joint0 ,max1,sim.simx_opmode_oneshot_wait)
#right_Code = sim.simxSetJointTargetVelocity(self.clientID,joint1 ,max2,sim.simx_opmode_oneshot_wait)
#right_Code = sim.simxSetJointTargetVelocity(clientID,joint1 ,max3,sim.simx_opmode_oneshot_wait)
print('finish velocity setting')
def main_loop(self):
"""
this is looped over in the sub process
"""
if self.sync:
sim.simxSynchronousTrigger(self.clientID)
sim.simxPauseCommunication(self.clientID, True)
# get position data
_, orientation = sim.simxGetObjectOrientation(self.clientID,
self.camHandle,
-1,
sim.simx_opmode_buffer)
_, position = sim.simxGetObjectPosition(self.clientID,
self.camHandle,
-1,
sim.simx_opmode_buffer)
# retrieve images
self.images = self.getImages()
# only send or receive once images are not none
if self.images is None:
time.sleep(1 / 100)
return
# send images to depth pipeline
self.images["display"] = self.Depth2Color(self.images["depth"], 1)
self.images["display"] = np.vstack([self.images["RGB"], self.images["display"]])
message = {"position": position, "orientation": orientation, "images": self.images}
self.queues["output"].send(message, method="recent")
speeds = self.queues["input"].retrieve(method="no_wait")
# check keyboard to set wheel directions
speed_L, speed_R = self.keyboardInput()
if (speeds is not None) and (not self.keyboard_controlled):
speed_L = speeds["left"]
speed_R = speeds["right"]
# activate gripper
#if self.gripper_key.click():
# self.setGripper("left", -0.05)
# self.setGripper("right", 0.05)
#else:
# self.setGripper("left", 0.05)
# self.setGripper("right", -0.05)
# set wheel speeds
threads = {"left": Thread(target=self.setSpeed, args=("left", speed_L,)),
"right": Thread(target=self.setSpeed, args=("right", speed_R,))}
self.startThreads(threads)
self.joinThreads(threads)
sim.simxPauseCommunication(self.clientID, False)
def step(self): # Now step a few times:
if sys.version_info[0] == 3:
self._get_pos()
_ = sim.simxSetObjectPosition(self.clientID, self.target, -1, self.target_position, sim.simx_opmode_oneshot)
# error, value = sim.simxGetIntegerSignal(self.clientID, "data", sim.simx_opmode_streaming)
# print(value)
# self.set_pos() +8.0299e-02
else:
raw_input('Press <enter> key to step the !')
sim.simxSynchronousTrigger(self.clientID)
def step(self, action):
[d, Oc, alpha, v, w], Sensors = self.get_obs()
observation = np.append([d, Oc, v, w], Sensors)
done = False
if d >= 0.05:
Vl, Vr = self.action_translation(action)
reward = -(d**2)
else:
Vl, Vr = [0, 0]
done = True
reward = 100
if max(Sensors) > 0.97:
self.problem = True
if self.problem or self.steps == (self.MaxSteps - 1):
done = True
reward = -100
Vl, Vr = [0, 0]
if not done:
sim.simxSynchronousTrigger(self.clientID)
self.steps += 1
self.updateVelocities(Vl, Vr)
v = (Vr + Vl) / 96
w = 5 * (Vr - Vl) / 24
xc, yc, theta = self.getPositionRobot()
#self.save_images()
info = {
'Distance': d,
'Oc': Oc,
'Lineal': v,
'Angular': w,
'xc': xc,
'yc': yc
}
return observation, reward, done, info
if not done:
sim.simxSynchronousTrigger(self.clientID)
info = {'Distance': d, 'Oc': Oc, 'Lineal': V, 'Angular': W}
return observation, reward, done, info
def reset(self):
# reset objects dynamically in lua script
result, collision_state, retFloats, retStrings, retBuffer = sim.simxCallScriptFunction(
self.clientID, "hexapod", sim.sim_scripttype_childscript,
"reset_robot", [], [], [], emptyBuff, blocking)
for link, angle in zip(self.links, [0, -30, 120]):
for handle in link.values():
sim.simxSetJointTargetPosition(self.clientID, handle,
angle * (3.14 / 180), oneshot)
sim.simxSynchronousTrigger(self.clientID)
self.prev_pos = sim.simxGetObjectPosition(self.clientID,
self.hexapod_handle, -1,
buffer)[1]
self.state = np.ones(self.state_size) * 120 * (3.14 / 180)
return self.state # hope coppeliaSim set them properly
def step(self, action):
[d, Oc, alpha, v, w], Sensors = self.get_obs()
observation = np.append([d, Oc, v, w], Sensors)
done = False
reward = -(d**2)
if d >= 0.05:
V, W = self.check_action(action)
else:
V, W = [0, 0]
done = True
reward = 100
if max(Sensors) > 0.97 or self.steps == self.MaxSteps - 1:
self.problem = True
if self.problem:
done = True
V, W = [0, 0]
reward = -100
Vl = (V - (W * 0.1) / 2) * 48
Vr = (V + (W * 0.1) / 2) * 48
self.Velocities = [V, W]
self.updateVelocities(Vl, Vr)
if not done:
self.steps += 1
sim.simxSynchronousTrigger(self.clientID)
xc, yc, theta = self.getPositionRobot()
info = {
'Distance': d,
'Oc': Oc,
'Lineal': V,
'Angular': W,
'xc': xc,
'yc': yc
}
return observation, reward, done, info
def doTracking(self):
self.resetSim()
self.prepareSim()
pathIsDone = sim.simxGetFloatSignal(self.clientID, 'movePathDone',
sim.simx_opmode_streaming)[1]
posX = []
posY = []
posZ = []
tactile1 = []
reward = []
while pathIsDone == 0:
X, Y, Z = self.getKinectXYZ(False)
# forces = self.getForce(False)
# diffForce = (forces[0][0] + ((forces[1][0] + forces[2][0]) / 2))/2
# print('[DATA] diff force: {:.4f}'.format(diffForce))
actionX = X # + 0.005*diffForce
actionY = Y
actionZ = Z
# print('[DATA] X:{:.4f}, Y:{:.4f}, Z:{:.4f}'.format(actionX,actionY,actionZ))
sim.simxSetFloatSignal(self.clientID, 'actionX', actionX,
sim.simx_opmode_oneshot)
sim.simxSetFloatSignal(self.clientID, 'actionY', actionY,
sim.simx_opmode_oneshot)
sim.simxSetFloatSignal(self.clientID, 'actionZ', actionZ,
sim.simx_opmode_oneshot)
sim.simxGetPingTime(self.clientID)
sim.simxSynchronousTrigger(self.clientID)
forces = self.getForceMagnitude(False)
posX.append(X)
posY.append(Y)
posZ.append(Z)
tactile1.append(forces[0])
reward.append(self._getReward())
sim.simxGetPingTime(self.clientID)
pathIsDone = sim.simxGetFloatSignal(self.clientID, 'movePathDone',
sim.simx_opmode_buffer)[1]
# time.sleep(0.5)
# print('[INFO] tracking is done.')
# print('[DATA] accumulated reward: {}'.format(np.sum(np.array(reward))))
sim.simxStopSimulation(self.clientID, sim.simx_opmode_blocking)
def step(self, action):
assert self.action_space.contains(action)
joint = int(action / 2)
handle = list(self.joint_handles.values())[joint]
parent_handle = list(self.links[1].values())[joint]
value = self.action_map[action % 2]
if joint in self.faulty_joints:
value = 0
cur_pos = sim.simxGetJointPosition(self.clientID, handle, buffer)[1]
new_pos = cur_pos + value * (3.14 / 180)
if np.abs(
new_pos
) <= MAX_ANGLE: # Limiting it from rotating beyond |30| degree
sim.simxSetJointTargetPosition(self.clientID, parent_handle,
-45 * (3.14 / 180), oneshot)
sim.simxSynchronousTrigger(self.clientID)
sim.simxSetJointTargetPosition(self.clientID, handle, new_pos,
oneshot)
sim.simxSynchronousTrigger(self.clientID)
sim.simxSetJointTargetPosition(self.clientID, parent_handle,
-30 * (3.14 / 180), oneshot)
sim.simxSynchronousTrigger(self.clientID)
# update state, reward, done
reward, done = self.calc_reward()
self.state[joint] = new_pos
else:
reward = -1
done = False
return self.state, reward, done, {}
def prepareSim(self):
self.initializeFunctions()
isTracking = sim.simxGetFloatSignal(self.clientID, 'isTracking',
sim.simx_opmode_streaming)[1]
while isTracking != 1:
sim.simxSynchronousTrigger(self.clientID)
isTracking = sim.simxGetFloatSignal(self.clientID, 'isTracking',
sim.simx_opmode_buffer)[1]
sim.simxGetPingTime(self.clientID)
X, Y, Z = self.getKinectXYZ(True)
actionX = X
actionY = Y
actionZ = Z
# print(actionX,actionY,actionZ)
# print('[DATA] X:{:.4f}, Y:{:.4f}, Z:{:.4f}'.format(actionX,actionY,actionZ))
sim.simxSetFloatSignal(self.clientID, 'actionX', actionX,
sim.simx_opmode_oneshot)
sim.simxSetFloatSignal(self.clientID, 'actionY', actionY,
sim.simx_opmode_oneshot)
sim.simxSetFloatSignal(self.clientID, 'actionZ', actionZ,
sim.simx_opmode_oneshot)
sim.simxGetPingTime(self.clientID)
def _step_simulation(self):
"""Шаг симуляции для режима синхронизации
"""
sim.simxSynchronousTrigger(self.client)
sim.simxGetPingTime(self.client)
#
#control_force = 400
if np.sign(control_force) >= 0:
set_force = control_force
set_velocity = 9999
else:
set_force = -control_force
set_velocity = -9999
# 控制命令需要同时方式，故暂停通信，用于存储所有控制命令一起发送
vrep.simxPauseCommunication(clientID, True)
vrep.simxSetJointTargetVelocity(clientID, jointHandle, set_velocity,
vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID, jointHandle, set_force,
vrep.simx_opmode_oneshot)
#vrep.simxSetJointForce(clientID,jointHandle,set_force,vrep.simx_opmode_oneshot);
# vrep.simxSetJointTargetPosition(clientID, jointHandle, 100, vrep.simx_opmode_oneshot)
# vrep.simxSetJointTargetPosition(clientID, jointHandle[1], 5/RAD2DEG, vrep.simx_opmode_oneshot)
# vrep.simxSetJointTargetVelocity(clientID, jointHandle[2], 500/RAD2DEG, vrep.simx_opmode_oneshot)
# vrep.simxSetJointTargetVelocity(clientID, jointHandle[3], 500/RAD2DEG, vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
# ***
lastCmdTime = currCmdTime # 记录当前时间
vrep.simxSynchronousTrigger(clientID) # 进行下一步
vrep.simxGetPingTime(clientID) # 使得该仿真步走完
print(data_len)
for i in range(data_len):
UR5_sim_model.setJointAngle('UR5_joint3', desired_q[i, 0])
UR5_sim_model.setJointAngle('UR5_joint4', desired_q[i, 1])
UR5_sim_model.setJointAngle('UR5_joint5', desired_q[i, 2])
desired_q1_record.append(desired_q[i, 0])
desired_q2_record.append(desired_q[i, 1])
desired_q3_record.append(desired_q[i, 2])
q = UR5_sim_model.getAllJointAngles()
current_q1_record.append(q[2])
current_q2_record.append(q[3])
current_q3_record.append(q[4])
vrep_sim.simxSynchronousTrigger(
client_ID) # trigger one simulation step
vrep_sim.simxStopSimulation(
client_ID, vrep_sim.simx_opmode_blocking) # stop the simulation
vrep_sim.simxFinish(-1) # Close the connection
print('Program terminated')
plt.figure(figsize=(10, 5))
plt.plot(desired_q1_record, 'g', label='desired q 1')
plt.plot(current_q1_record, 'r--', label='current q 1')
plt.plot(desired_q2_record, 'b', label='desired q 2')
plt.plot(current_q2_record, 'm--', label='current q 2')
plt.plot(desired_q3_record, 'k', label='desired q 3')
plt.plot(current_q3_record, 'y--', label='current q 3')
plt.legend()
plt.grid()
_,MotorHandle_Right = sim.simxGetObjectHandle(clientID,'Revolute_right',sim.simx_opmode_blocking)
_,TCP = sim.simxGetObjectHandle(clientID,'TCP',sim.simx_opmode_blocking)
#_, baseHandle = sim.simxGetObjectHandle(clientID, 'CarBody', sim.simx_opmode_blocking)
print('Handles Found!')
# read the initial value of joints
jointConfig = np.zeros((jointNum,))
for i in range(jointNum):
_, jpos = sim.simxGetJointPosition(clientID, jointHandle[i], sim.simx_opmode_streaming)
jointConfig[i] = jpos
lastCmdTime=sim.simxGetLastCmdTime(clientID) # record the current time
sim.simxSynchronousTrigger(clientID)
#iii = 0 #loop index
#increase_flag = 0
# trigger the sim
while sim.simxGetConnectionId(clientID) != -1:
currCmdTime=sim.simxGetLastCmdTime(clientID)
dt = currCmdTime - lastCmdTime # delta time
# read the current status of joints for every step
for i in range(jointNum):
_, jpos = sim.simxGetJointPosition(clientID, jointHandle[i], sim.simx_opmode_buffer)
# print(round(jpos * 180 / math.pi, 2))
jointConfig[i] = jpos
_,MotorHandle_Left = sim.simxGetObjectHandle(clientID,'Revolute_left',sim.simx_opmode_blocking)
def main(argv):
# https://www.coppeliarobotics.com/helpFiles/en/remoteApiClientSide.htm --> how to remote API
# Refer to https://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxStart
sim.simxFinish(-1) # just in case, close all opened connections
clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID == -1:
print('Failed connecting to remote API server')
else:
print('Connected to remote API client')
numberOfRuns = 10
numberOfMeasurements = 1
variationSize = 0.001
for i in range(numberOfRuns):
count = 0
dt = 0.0167
sim.simxSynchronous(clientID, True)
################################################### init variables ###################################################
ros_handle = simROS.SIMROS(argv)
targetAngle = 0
#targetAngles = list(np.random.randint(-25, 26, numberOfMeasurements))
targetAngles = list(np.zeros(numberOfMeasurements))
print(targetAngles)
randomVar = []
for i in range(18):
randomVar.append(
list(
np.random.uniform(-variationSize, variationSize,
numberOfMeasurements)))
if newFile:
with open(dataFilePath, 'w+') as myfile:
wr = csv.writer(myfile, quoting=csv.QUOTE_ALL)
wr.writerow([
"Inclination", "LFx", "LFy", "LFz", "LMx", "LMy",
"LMz", "LHx", "LHy", "LHz", "RFx", "RFy", "RFz", "RMx",
"RMy", "RMz", "RHx", "RHy", "RHz"
])
counter = 0
Done = False
#Start the simulation
sim.simxStartSimulation(clientID, sim.simx_opmode_blocking)
################################################### Control Loop ###################################################
while not Done:
#code here
#print(sim.simxReadForceSensor(clientID, "3D_force0", sim.simx_opmode_blocking))
if counter == 150:
saveData(force, targetAngle)
print("Runs remaining: ", len(targetAngles))
if len(targetAngles) <= 0:
Done = True
else:
targetAngle = targetAngles.pop(0)
motor_positions = [
0, randomVar[1].pop() + 2.059, randomVar[2].pop(),
0, randomVar[4].pop() + 2.059, randomVar[5].pop(),
0, randomVar[7].pop() + 2.059, randomVar[8].pop(),
0, randomVar[10].pop() + 2.059,
randomVar[11].pop(), 0,
randomVar[13].pop() + 2.059, randomVar[14].pop(),
0, randomVar[16].pop() + 2.059,
randomVar[17].pop()
]
counter = 0
ros_handle.setSlopeAngle(targetAngle)
ros_handle.setLegMotorPosition(motor_positions)
counter = counter + 1
#Step the simulation
sim.simxSynchronousTrigger(clientID)
sim.simxStopSimulation(clientID, sim.simx_opmode_blocking)
sim.simxGetPingTime(clientID)
sim.simxFinish(clientID)
def _simulation_step(self):
#for _ in range(self.n_substeps):
sim.simxSynchronousTrigger(self.clientID)
sim.simxGetPingTime(self.clientID)
sim.simxFinish(-1)
clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
print("Connection success")
if clientID!=-1:
print ('Connected to remote API server')
res,left_motor = sim.simxGetObjectHandle(clientID, "left_motor", sim.simx_opmode_oneshot_wait)
sim.simxSynchronous(clientID,1)# -啟用同步模式（客戶端）。服務器端（即CoppeliaSim）也需要啟用。
sim.simxStartSimulation(clientID,sim.simx_opmode_oneshot)# //開始仿真
sim.simxSetJointForce(clientID,left_motor,1.0f,sim.simx_opmode_oneshot）;#//設置聯合力/扭矩
sim.simxSetJointTargetVelocity(clientID,left_motor,180.0f * 3.1415f / 180.0f,sim.simx_opmode_oneshot)# //設置聯合目標速度
sim.simxSynchronousTrigger(clientID);#//觸發下一個模擬步驟。上面的命令將被應用
sim.simxSetJointForce(clientID,left_motor,0.5f,sim.simx_opmode_oneshot)# //設置聯合力/扭矩
sim.simxSetJointTargetVelocity(clientID,left_motor,180.0f * 3.1415f / 180.0f,sim.simx_opmode_oneshot)# //設置聯合目標速度
sim.simxSynchronousTrigger(clientID)# //下一個模擬步驟執行。上面的命令將被應用
sim.simxSetJointForce(clientID,left_motor,2.0f,sim.simx_opmode_oneshot)# //設置聯合力/扭矩
sim.simxSetJointTargetVelocity(clientID,left_motor,180.0f * 3.1415f / 180.0f,sim.simx_opmode_oneshot)# //設置關節目標速度
sys.exit("Error: no se puede conectar") #Terminar este script
except:
print('Check if CoppeliaSim is open')
UR3_joints_IP=get_joint_handle(clientID) #Getting the reference
for i in UR3_joints_IP:
UR3_joints_IP[i].append(get_joint_orientation(clientID, UR3_joints_IP[i][0]))
#print(get_joint_orientation(clientID, UR3_joints_IP[i]))
#J1z,J2z,J3Z
#Practica 30 grados j1, 15 grados j2, 30 grados j3, 20 grados j4, 15 grados j5, 10 grados j6
new_angles=[60,25,10,30,15,50]
c=0
sim.simxSynchronous(clientID,True)
sim.simxSynchronousTrigger(clientID)
sim.simxStartSimulation(clientID, sim.simx_opmode_blocking)
for i in UR3_joints_IP:
set_joint_orientation(clientID, UR3_joints_IP[i][0],new_angles[c], mode=sim.simx_opmode_blocking) #Todas las juntas posicion inicial
c=c+1
time.sleep(5)
sim.simxStopSimulation(clientID, sim.simx_opmode_blocking)
sim.simxFinish(clientID)
# Start simulation
sim.simxSynchronous(client_id, True)
sim.simxStartSimulation(client_id, sim.simx_opmode_blocking)
# Initial and final locations
start = (2, -3, -0 * math.pi / 2)
goal = (4, 4)
# Create the robot
_, robot_handle = create_robot(client_id, start[0], start[1], start[2])
sim.simxGetObjectPosition(
client_id, robot_handle, -1,
sim.simx_opmode_streaming) # Initialize real position streaming
# Execute a simulation step to get initial sensor readings
sim.simxSynchronousTrigger(client_id)
sim.simxGetPingTime(
client_id) # Make sure the simulation step has finished
# Write initialization code here
dt = 0.05
steps = 0
robot = RobotP3DX(client_id, dt)
z_us, z_v, z_w = robot.sense()
navigation = Navigation(dt, z_us)
localized = False
#particle filter
m = Map('map_project.json',
sensor_range=robot.SENSOR_RANGE,
compiled_intersect=True,
