def reset(self):
# stop the simulation:
sim.simxStopSimulation(self.clientID, sim.simx_opmode_blocking)
time.sleep(0.1)
sim.simxSynchronous(self.clientID, True)
# start the simulation:
sim.simxStartSimulation(self.clientID, sim.simx_opmode_blocking)
self.steps = 0
self.ResetSimulationScene()
if self.Randomize:
ran_angle = random.random() * 360
self.xp, self.yp = self.change_target_angle(ran_angle)
else:
self.xp, self.yp = self.getPositionTarget()
#self.xp, self.yp = self.change_target_angle(self.angle)
d, Oc = self.get_obs()
observation = np.array([d, Oc])
return observation
def reset(self):
# stop the simulation:
self.problem = False
sim.simxStopSimulation(self.clientID, sim.simx_opmode_blocking)
sim.simxSynchronous(self.clientID, True)
# start the simulation:
sim.simxStartSimulation(self.clientID, sim.simx_opmode_blocking)
self.ResetSimulationScene()
if self.Randomize:
self.xp, self.yp = self.change_target_position(radius=self.radius)
else:
self.xp, self.yp = self.getPositionTarget()
if self.RobotOrientationRand:
self.change_robot_orientation()
self.steps = 0
[d, Oc, alpha, v, w], Sensors = self.get_obs()
observation = np.append([d, Oc, v, w], Sensors)
return observation
def inputxmlfile(self):
sim.simxStopSimulation(self.clientID, sim.simx_opmode_oneshot)
#number returnCode = simxCloseScene（number clientID，number operationMode）
sim.simxCloseScene(self.clientID,sim.simx_opmode_blocking)
#number returnCode=simxLoadScene(number clientID,string scenePathAndName,number options,number operationMode)
sim.simxLoadScene(self.clientID,"D:\fall2020_v2\data\home\Desktop\40723103\add_stl.simscene.xml",0x00,sim.simx_opmode_blocking)
'''
# relative to remote API client location, relative path:
sim.simxLoadScene(clientID,'test/testScene.ttt',0xFF,vrep.simx_opmode_blocking)
# relative to V-REP executable location, relative path:
sim.simxLoadScene(clientID,'scenes/collisionDetectionDemo.ttt',0x00,vrep.simx_opmode_blocking)
# relative to remote API client location, absolute path:
sim.simxLoadScene(clientID,'c:/python27/test/testScene.ttt',0xFF,vrep.simx_opmode_blocking)
# relative to V-REP executable location, absolute path:
sim.simxLoadScene(clientID,'d:/v_rep/qrelease/release/scenes/collisionDetectionDemo.ttt',0x00,vrep.simx_opmode_blocking)
'''
print("successful input file")
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 reset(self):
"""
stop the simulation (will reset the scene in vrep)
clear signals and erase the client's local genome files
"""
ret = sim.simxStopSimulation(self.clientID, mode1)
for filename in self.genome_filenames:
sim.simxEraseFile(self.clientID, filename, mode1)
sim.simxClearIntegerSignal(self.clientID, '', mode1)
sim.simxClearFloatSignal(self.clientID, '', mode1)
sim.simxClearStringSignal(self.clientID, '', mode1)
def get_label(unit_data, converter, clientID,
Body): #1 for stable and 0 for fall down
labels = []
continue_flag = 0
duration = 0.03
for i in tqdm(range(unit_data.shape[0])):
sequence_sample = unit_data[i]
#for duration in [0.025, 0.035]:
for idx_f in range(sequence_sample.shape[0]):
angle_recon = converter.frameRecon(sequence_sample[idx_f])
# angles: LSP, LSR, LEY, LER, RSP, RSR, REY, RER, LHP, LHR, LHYP, LKP, RHP, RHR, RHYP, RKP, LAP, LAR, RAP, RAR
angles = converter.generateWholeJoints(angle_recon)
assert (len(angles) == 20)
transmit(clientID, Body, angles)
time.sleep(duration)
returnCode, position = sim.simxGetObjectPosition(
clientID, Body['HeadYaw'], -1, sim.simx_opmode_buffer)
if position[2] < 0.4 and position[2] > 0: #fall down
print('fall')
sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot)
print('stop')
time.sleep(.3)
sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot)
print('start')
time.sleep(.5)
labels.append(0)
continue_flag = 1
break
if continue_flag:
continue_flag = 0
continue
labels.append(1)
time.sleep(.3)
labels = np.array(labels)
print(labels.shape, np.sum(labels == 0))
np.save('unit_stability_labels.npy', np.array(labels))
def stop_simulation():
"""
Purpose:
---
This function should stop the running simulation in CoppeliaSim server.
NOTE: In this Task, do not call the exit_remote_api_server function in case of failed connection to the server.
The test_task_2a executable script will handle that condition.
Input Arguments:
---
None
Returns:
---
`return_code` : [ integer ]
the return code generated from the stop running simulation remote API
Example call:
---
return_code = stop_simulation()
NOTE: This function will be automatically called by test_task_2a executable at the end of simulation.
"""
global client_id
return_code = 0
############## ADD YOUR CODE HERE ##############
return_code = sim.simxStopSimulation(client_id, sim.simx_opmode_oneshot)
##################################################
return return_code
def stop_sim():
sim.simxStopSimulation(clientID, sim.simx_opmode_blocking)
print("Simulation stopped.")
def end(self):
sim.simxStopSimulation(self.clientID, sim.simx_opmode_blocking)
sim.simxFinish(self.clientID)
def get_image(self):
err, resolution, image = sim.simxGetVisionSensorImage(
self.clientID, self.v0, 0, sim.simx_opmode_streaming)
if err == sim.simx_return_ok:
img = numpy.array(image, dtype=numpy.uint8)
img.resize([resolution[1], resolution[0], 3])
img = imutils.rotate_bound(img, 180)
img = cv2.flip(img, 1)
cv2.putText(img, 'Image Recognition', (50, 500),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1,
cv2.LINE_AA)
global ret_blue
global ret_red
global ret_green
ret_green = color.track_green_object(img)
ret_red = color.track_red_object(img)
ret_blue = color.track_blue_object(img)
img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
if ret_green and ret_red and ret_blue:
#Use Rectangle and Text Mark Green Object
Rec_range = 6
cv2.rectangle(
img, (ret_green[0] - Rec_range, ret_green[1] - Rec_range),
(ret_green[0] + Rec_range, ret_green[1] + Rec_range),
(0, 255, 0), 1)
cv2.putText(img, 'Pla', (ret_green[0] - 10, ret_green[1] + 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1,
cv2.LINE_AA)
#Use Rectangle and Text Mark Red Object
cv2.rectangle(img,
(ret_red[0] - Rec_range, ret_red[1] - Rec_range),
(ret_red[0] + Rec_range, ret_red[1] + Rec_range),
(0, 0, 200), 1)
cv2.putText(img, 'Com', (ret_red[0] - 15, ret_red[1] - 10),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1,
cv2.LINE_AA)
#Use Rectangle and Text Mark Blue Object
cv2.rectangle(
img, (ret_blue[0] - Rec_range, ret_blue[1] - Rec_range),
(ret_blue[0] + Rec_range, ret_blue[1] + Rec_range),
(255, 0, 0), 1)
cv2.putText(img, 'Ball', (ret_blue[0] - 20, ret_blue[1] - 10),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 1,
cv2.LINE_AA)
print(threading.enumerate())
if threading.active_count() < 3:
thread1 = threading.Thread(
target=self.Computer1_thread_job, name='Computer_1')
thread1.start()
else:
if not ret_green:
print('not ret_green')
if not ret_red:
print('not ret_red')
if not ret_blue:
print('not ret_blue')
sim.simxStopSimulation(clientID2,
sim.simx_opmode_oneshot_wait)
time.sleep(1)
sim.simxStartSimulation(clientID2,
sim.simx_opmode_oneshot_wait)
time.sleep(0.5)
self.lastFrame = img
#self.lastFrame = numpy.hstack((image_ori,img))
return 1, self.lastFrame
elif err == sim.simx_return_novalue_flag:
return 0, None
else:
return err, None
def stopvrep():
sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait)
sim.simxFinish(-1) # just in case, close all opened connections
sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait)
return render_template('stopvrep.html')
y = Qpos[1] + obj_pos_pixel[1] * pixel_size
set_drone_position([x, y, 1])
img = get_image()
objects = get_objects(img)
objects = sorted(objects.items(), key=itemgetter(0), reverse=True)
obj_pos_pixel = objects[0][1]
pixel_size = (2 * (1 - 0.25) / sqrt(3)) / 256
err, cameraPos = vrep.simxGetObjectPosition(clientID, cameraID, -1,
vrep.simx_opmode_oneshot_wait)
x = cameraPos[0] + obj_pos_pixel[0] * pixel_size
y = cameraPos[1] + obj_pos_pixel[1] * pixel_size
objects = set_drone_position([x, y, 0.4])
time.sleep(2)
err, Qpos = vrep.simxGetObjectPosition(clientID, Quadricopter, -1,
vrep.simx_opmode_oneshot_wait)
# Throw ball
emptyBuff = bytearray()
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
clientID, 'Quadricopter', vrep.sim_scripttype_childscript,
'ThrowBallFunction', [], [], [""], emptyBuff, vrep.simx_opmode_blocking)
time.sleep(1)
img = get_image()
cv2.imwrite("ball.png", img)
# finish work
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
time.sleep(1)
vrep.simxFinish(clientID)
print("stop simulation")
def listener_callback(self, msg):
if msg.transforms[0].child_frame_id == 'robot1':
self.x1 = msg.transforms[0].transform.translation.x
self.y1 = msg.transforms[0].transform.translation.y
self.xr1 = msg.transforms[0].transform.rotation.x
self.yr1 = msg.transforms[0].transform.rotation.y
self.zr1 = msg.transforms[0].transform.rotation.z
self.wr1 = msg.transforms[0].transform.rotation.w
self.Theta1 = euler_from_quaternion(self.xr1, self.yr1, self.zr1,
self.wr1)
if msg.transforms[0].child_frame_id == 'robot2':
self.x2 = msg.transforms[0].transform.translation.x
self.y2 = msg.transforms[0].transform.translation.y
self.xr2 = msg.transforms[0].transform.rotation.x
self.yr2 = msg.transforms[0].transform.rotation.y
self.zr2 = msg.transforms[0].transform.rotation.z
self.wr2 = msg.transforms[0].transform.rotation.w
self.Theta2 = euler_from_quaternion(self.xr2, self.yr2, self.zr2,
self.wr2)
if msg.transforms[0].child_frame_id == 'robot3':
self.x3 = msg.transforms[0].transform.translation.x
self.y3 = msg.transforms[0].transform.translation.y
self.xr3 = msg.transforms[0].transform.rotation.x
self.yr3 = msg.transforms[0].transform.rotation.y
self.zr3 = msg.transforms[0].transform.rotation.z
self.wr3 = msg.transforms[0].transform.rotation.w
self.Theta3 = euler_from_quaternion(self.xr3, self.yr3, self.zr3,
self.wr3)
if msg.transforms[0].child_frame_id == 'robot4':
self.x4 = msg.transforms[0].transform.translation.x
self.y4 = msg.transforms[0].transform.translation.y
self.xr4 = msg.transforms[0].transform.rotation.x
self.yr4 = msg.transforms[0].transform.rotation.y
self.zr4 = msg.transforms[0].transform.rotation.z
self.wr4 = msg.transforms[0].transform.rotation.w
self.Theta4 = euler_from_quaternion(self.xr4, self.yr4, self.zr4,
self.wr4)
if msg.transforms[0].child_frame_id == 'robot5':
self.x5 = msg.transforms[0].transform.translation.x
self.y5 = msg.transforms[0].transform.translation.y
self.xr5 = msg.transforms[0].transform.rotation.x
self.yr5 = msg.transforms[0].transform.rotation.y
self.zr5 = msg.transforms[0].transform.rotation.z
self.wr5 = msg.transforms[0].transform.rotation.w
self.Theta5 = euler_from_quaternion(self.xr5, self.yr5, self.zr5,
self.wr5)
if msg.transforms[0].child_frame_id == 'robot6':
self.x6 = msg.transforms[0].transform.translation.x
self.y6 = msg.transforms[0].transform.translation.y
self.xr6 = msg.transforms[0].transform.rotation.x
self.yr6 = msg.transforms[0].transform.rotation.y
self.zr6 = msg.transforms[0].transform.rotation.z
self.wr6 = msg.transforms[0].transform.rotation.w
self.Theta6 = euler_from_quaternion(self.xr6, self.yr6, self.zr6,
self.wr6)
self.distance = abs(self.x1 - self.x2) + abs(self.y1 - self.y2) + abs(
self.x1 -
self.x3) + abs(self.y1 - self.y3) + abs(self.x1 - self.x4) + abs(
self.y1 - self.y4) + abs(self.x1 - self.x5) + abs(
self.y1 - self.y5) + abs(self.x1 - self.x6) + abs(self.y1 -
self.y6)
print(self.distance)
if self.distance > 2.2:
" Calculate Control inputs u1, u2, u3, u4, u5, u6 "
A = np.ones(6) - np.identity(6) # Adjancency Matrix
self.X = np.array([[self.x1], [self.x2], [self.x3], [self.x4],
[self.x5], [self.x6]]) #6x1
self.Y = np.array([[self.y1], [self.y2], [self.y3], [self.y4],
[self.y5], [self.y6]]) #6x1
ux = np.zeros((6, 1)) # 6x1
uy = np.zeros((6, 1)) # 6x1
for i in range(1, 7):
for j in range(1, 7):
ux[i - 1] += -(A[i - 1][j - 1]) * (
self.X[i - 1] - self.X[j - 1]) # 1x1 each
uy[i - 1] += -(A[i - 1][j - 1]) * (
self.Y[i - 1] - self.Y[j - 1]) # 1x1 each
u1 = np.array([[float(ux[0])], [float(uy[0])]]) # 2x1
u2 = np.array([[float(ux[1])], [float(uy[1])]]) # 2x1
u3 = np.array([[float(ux[2])], [float(uy[2])]]) # 2x1
u4 = np.array([[float(ux[3])], [float(uy[3])]]) # 2x1
u5 = np.array([[float(ux[4])], [float(uy[4])]]) # 2x1
u6 = np.array([[float(ux[5])], [float(uy[5])]]) # 2x1
" Data Transformation into M_x, M_y, Phi_x, Phi_y "
Phix = (u1[0][0] + u3[0][0]) / 2 # 1x1
Phiy = (u1[1][0] + u3[1][0]) / 2 # 1x1
Mx = ((self.x1 - self.x2) + (self.x3 - self.x2)) / 2 # 1x1
My = ((self.y1 - self.y2) + (self.y3 - self.y2)) / 2 # 1x1
" Calculate V1/W1, V2/W2, V3/W3, V4/W4, V5/W5, V6/W6 "
S1 = np.array([[self.v1], [self.w1]]) #2x1
G1 = np.array([[1, 0], [0, 1 / L]]) #2x2
R1 = np.array([[math.cos(self.Theta1),
math.sin(self.Theta1)],
[-math.sin(self.Theta1),
math.cos(self.Theta1)]]) #2x2
S1 = np.dot(np.dot(G1, R1), u1) #2x1
S2 = np.array([[self.v2], [self.w2]]) #2x1
G2 = np.array([[1, 0], [0, 1 / L]]) #2x2
R2 = np.array([[math.cos(self.Theta2),
math.sin(self.Theta2)],
[-math.sin(self.Theta2),
math.cos(self.Theta2)]]) #2x2
S2 = np.dot(np.dot(G2, R2), u2) # 2x1
S3 = np.array([[self.v3], [self.w3]]) #2x1
G3 = np.array([[1, 0], [0, 1 / L]]) #2x2
R3 = np.array([[math.cos(self.Theta3),
math.sin(self.Theta3)],
[-math.sin(self.Theta3),
math.cos(self.Theta3)]]) #2x2
S3 = np.dot(np.dot(G3, R3), u3) #2x1
S4 = np.array([[self.v4], [self.w4]]) #2x1
G4 = np.array([[1, 0], [0, 1 / L]]) #2x2
R4 = np.array([[math.cos(self.Theta4),
math.sin(self.Theta4)],
[-math.sin(self.Theta4),
math.cos(self.Theta4)]]) #2x2
S4 = np.dot(np.dot(G4, R4), u4) #2x1
S5 = np.array([[self.v5], [self.w5]]) #2x1
G5 = np.array([[1, 0], [0, 1 / L]]) #2x2
R5 = np.array([[math.cos(self.Theta5),
math.sin(self.Theta5)],
[-math.sin(self.Theta5),
math.cos(self.Theta5)]]) #2x2
S5 = np.dot(np.dot(G5, R5), u5) #2x1
S6 = np.array([[self.v6], [self.w6]]) #2x1
G6 = np.array([[1, 0], [0, 1 / L]]) #2x2
R6 = np.array([[math.cos(self.Theta6),
math.sin(self.Theta6)],
[-math.sin(self.Theta6),
math.cos(self.Theta6)]]) #2x2
S6 = np.dot(np.dot(G6, R6), u6) #2x1
" Calculate VL1/VR1, VL2/VR2, VL3/VR3, VL4/VR4, VL5/VR5, VL6/VR6 "
D = np.array([[1 / 2, 1 / 2], [-1 / (2 * d), 1 / (2 * d)]]) #2x2
Di = np.linalg.inv(D) #2x2
Speed_L1 = np.array([[self.vL1], [self.vR1]
]) # Vector 2x1 for Speed of Robot 1
Speed_L2 = np.array([[self.vL2], [self.vR2]
]) # Vector 2x1 for Speed of Robot 2
Speed_L3 = np.array([[self.vL3], [self.vR3]
]) # Vector 2x1 for Speed of Robot 3
Speed_L4 = np.array([[self.vL4], [self.vR4]
]) # Vector 2x1 for Speed of Robot 4
Speed_L5 = np.array([[self.vL5], [self.vR5]
]) # Vector 2x1 for Speed of Robot 5
Speed_L6 = np.array([[self.vL6], [self.vR6]
]) # Vector 2x1 for Speed of Robot 6
M1 = np.array([[S1[0]], [S1[1]]]).reshape(2, 1) #2x1
M2 = np.array([[S2[0]], [S2[1]]]).reshape(2, 1) #2x1
M3 = np.array([[S3[0]], [S3[1]]]).reshape(2, 1) #2x1
M4 = np.array([[S4[0]], [S4[1]]]).reshape(2, 1) #2x1
M5 = np.array([[S5[0]], [S5[1]]]).reshape(2, 1) #2x1
M6 = np.array([[S6[0]], [S6[1]]]).reshape(2, 1) #2x1
Speed_L1 = np.dot(Di, M1) # 2x1 (VL1, VR1)
Speed_L2 = np.dot(Di, M2) # 2x1 (VL2, VR2)
Speed_L3 = np.dot(Di, M3) # 2x1 (VL3, VR3)
Speed_L4 = np.dot(Di, M4) # 2x1 (VL4, VR4)
Speed_L5 = np.dot(Di, M5) # 2x1 (VL5, VR5)
Speed_L6 = np.dot(Di, M6) # 2x1 (VL6, VR6)
VL1 = float(Speed_L1[0])
VR1 = float(Speed_L1[1])
VL2 = float(Speed_L2[0])
VR2 = float(Speed_L2[1])
VL3 = float(Speed_L3[0])
VR3 = float(Speed_L3[1])
VL4 = float(Speed_L4[0])
VR4 = float(Speed_L4[1])
VL5 = float(Speed_L5[0])
VR5 = float(Speed_L5[1])
VL6 = float(Speed_L6[0])
VR6 = float(Speed_L6[1])
" Publish Speed Commands to Robot 1 "
msgl1 = Float32()
msgr1 = Float32()
msgl1.data = VL1
msgr1.data = VR1
self.publisher_l1.publish(msgl1)
self.publisher_r1.publish(msgr1)
#self.get_logger().info('Publishing R1: "%s"' % msgr1.data)
" Publish Speed Commands to Robot 2 "
msgl2 = Float32()
msgr2 = Float32()
msgl2.data = VL2
msgr2.data = VR2
self.publisher_l2.publish(msgl2)
self.publisher_r2.publish(msgr2)
" Publish Speed Commands to Robot 3 "
msgl3 = Float32()
msgr3 = Float32()
msgl3.data = VL3
msgr3.data = VR3
self.publisher_l3.publish(msgl3)
self.publisher_r3.publish(msgr3)
" Publish Speed Commands to Robot 4 "
msgl4 = Float32()
msgr4 = Float32()
msgl4.data = VL4
msgr4.data = VR4
self.publisher_l4.publish(msgl4)
self.publisher_r4.publish(msgr4)
" Publish Speed Commands to Robot 5 "
msgl5 = Float32()
msgr5 = Float32()
msgl5.data = VL5
msgr5.data = VR5
self.publisher_l5.publish(msgl5)
self.publisher_r5.publish(msgr5)
" Publish Speed Commands to Robot 6 "
msgl6 = Float32()
msgr6 = Float32()
msgl6.data = VL6
msgr6.data = VR6
self.publisher_l6.publish(msgl6)
self.publisher_r6.publish(msgr6)
" Write Values to CSV1 and CSV2 "
if self.count % 2 == 0:
with open('transformed_dataset.csv', 'a', newline='') as f:
fieldnames = ['M_x', 'M_y', 'Phi_x', 'Phi_y', 'U_x', 'U_y']
thewriter = csv.DictWriter(f, fieldnames=fieldnames)
if self.i2 == 0: # write header value once
thewriter.writeheader()
self.i2 = 1
if self.j2 != 0:
thewriter.writerow({
'M_x': Mx,
'M_y': My,
'Phi_x': Phix,
'Phi_y': Phiy,
'U_x': u2[0][0],
'U_y': u2[1][0]
})
if self.j2 == 0: # skip first value because it's noisy
self.j2 = 1
self.count += 1 # Counter to skip values while saving to csv file
else:
print(" Simulation ", self.scene)
# Stop Simulation
sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait)
# Retrieve some handles:
ErrLocM1, LocM1 = sim.simxGetObjectHandle(
clientID, 'robot1', sim.simx_opmode_oneshot_wait)
if (not ErrLocM1 == sim.simx_return_ok):
pass
ErrLocM2, LocM2 = sim.simxGetObjectHandle(
clientID, 'robot2#0', sim.simx_opmode_oneshot_wait)
if (not ErrLocM2 == sim.simx_return_ok):
pass
ErrLoc1, Loc1 = sim.simxGetObjectPosition(
clientID, LocM1, -1, sim.simx_opmode_oneshot_wait)
if (not ErrLoc1 == sim.simx_return_ok):
pass
ErrLoc2, Loc2 = sim.simxGetObjectPosition(
clientID, LocM2, -1, sim.simx_opmode_oneshot_wait)
if (not ErrLoc2 == sim.simx_return_ok):
pass
ErrLocO1, OriRobo1 = sim.simxGetObjectOrientation(
clientID, LocM1, -1, sim.simx_opmode_oneshot_wait)
if (not ErrLocO1 == sim.simx_return_ok):
pass
ErrLocO2, OriRobo2 = sim.simxGetObjectOrientation(
clientID, LocM2, -1, sim.simx_opmode_oneshot_wait)
if (not ErrLocO2 == sim.simx_return_ok):
pass
OriRobo1[2] = scenes[self.scene][2]
OriRobo2[2] = scenes[self.scene][5]
# Set Robot Orientation
sim.simxSetObjectOrientation(clientID, LocM1, -1, OriRobo1,
sim.simx_opmode_oneshot_wait)
sim.simxSetObjectOrientation(clientID, LocM2, -1, OriRobo2,
sim.simx_opmode_oneshot_wait)
Loc1[0] = scenes[self.scene][0]
Loc2[0] = scenes[self.scene][3]
Loc1[1] = scenes[self.scene][1]
Loc2[1] = scenes[self.scene][4]
# Set Robot Position
sim.simxSetObjectPosition(clientID, LocM1, -1, Loc1,
sim.simx_opmode_oneshot)
sim.simxSetObjectPosition(clientID, LocM2, -1, Loc2,
sim.simx_opmode_oneshot)
# Print Positions and Orientation
#print("Robot1 Position:", Loc1)
#print("Robot2 Position:", Loc2)
#print("Robot1 Orientation:", OriRobo1)
#print("Robot2 Orientation:", OriRobo2)
# Nb of Scene Counter
self.scene += 1
# Start Simulation
sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait)
time.sleep(5)
if self.scene == scenes.shape[0] - 1:
# Stop Simulation
sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait)
# End Connection to V-Rep
sim.simxFinish(clientID)
# Set Robot Position
sim.simxSetObjectPosition(clientID, LocM1, -1, Loc1,
sim.simx_opmode_oneshot)
sim.simxSetObjectPosition(clientID, LocM2, -1, Loc2,
sim.simx_opmode_oneshot)
# Start the Simulation
print("Robot1 Position:", Loc1)
print("Robot2 Position:", Loc2)
print("Robot1 Orientation:", OriRobo1)
print("Robot2 Orientation:", OriRobo2)
print("Simulation Running ...")
#sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait)
time.sleep(1)
sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait)
iter += 1
# Before closing the connection to CoppeliaSim, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
sim.simxGetPingTime(clientID)
# End Connection to V-Rep
sim.simxFinish(clientID)
else:
print("Failed connecting to remote API server")
print("Program Ended")
def stopSimulation(self):
vrep.simxStopSimulation(self.clientId, vrep.simx_opmode_blocking)
vrep.simxFinish(self.clientId)
def main():
obst_count = 68
targetCount = 5
obstaclePrefix = 'column'
targetPrefix = 'End'
sim.simxFinish(-1)
clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID != -1:
print('Connected to remote API server')
err, QuadricopterT = sim.simxGetObjectHandle(clientID,
'Quadricopter_target',
sim.simx_opmode_blocking)
if err == -1:
print("No Quadricopter")
err, Quadricopter = sim.simxGetObjectHandle(clientID, 'Quadricopter',
sim.simx_opmode_blocking)
if err == -1:
print("No Quadricopter")
#sim.simxStartSimulation(clientID, sim.simx_opmode_blocking)
# enable the synchronous mode on the client:
print("is connected!!!")
#retrieves the boxes from coppeliasim. This is slow (which is why it is commented out, we read from file instead)
#obstacle collection
#obst_list = []
bbox_list = []
if GENERATE_FILES:
for i in range(obst_count):
err, Obst = sim.simxGetObjectHandle(clientID,
obstaclePrefix + str(i),
sim.simx_opmode_blocking)
if err > 0:
print("could not retrieve column ", i)
obst_pose = flib.get_pos(clientID, Obst)
print("col ", i, "POSE: ", obst_pose)
obst_size = flib.get_size(clientID, Obst)
print("col ", i, "SIZE: ", obst_size)
obst_bbox = convert_bbox(obst_pose, obst_size)
print("col ", i, "BBOX: ", obst_bbox)
#obst = Obstacle(obst_pose, obst_size, obst_bbox)
bbox_list.append(obst_bbox)
#obst_list.append(obst)
#we write the boxes to a file to retrieve faster later.
write_boxes_file(bbox_list)
bbox_list = read_boxes_file()
#plot_boxes(bbox_list)
#target collection
deliveries = []
for i in range(targetCount):
err, targ = sim.simxGetObjectHandle(clientID,
targetPrefix + str(i + 1),
sim.simx_opmode_blocking)
tmp = flib.get_pos(clientID, targ)
print("Target ", i, "Location: ", tmp)
deliveries.append(np.array([tmp[0], tmp[1], tmp[2]]))
if GENERATE_FILES:
np.savetxt("targets.csv", deliveries, delimiter=",")
pose = flib.get_pos(clientID, Quadricopter)
if GENERATE_FILES:
np.savetxt("pose.csv", pose, delimiter=",")
print("Start position: ", pose)
print("Total distance: ", calc_total_distance(pose, deliveries))
#controller object
pathControl = quadsim_P2P(pose, bbox_list)
np.savetxt("pose.csv", pose, delimiter=",")
np.savetxt("targets.csv", np.array(deliveries), delimiter=",")
#plan route
before_rrt_t = datetime.datetime.now()
while not pathControl.plan(deliveries, clientID):
print("Retrying planning with: max iterations=",
pathControl.rrt.max_iter)
if pathControl.rrt.use_funnel:
print("search cone angle[Rad]=", pathControl.rrt.searchTheta)
print("the path is worthy! Calculation took: ",
(datetime.datetime.now() - before_rrt_t).total_seconds(),
" seconds.")
lastIter = -1
lastGoal = -1
#start simulation
pathControl.iterRun_start()
sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot)
sim.simxSynchronous(clientID, 1)
lastTime = datetime.datetime.now()
startTime = now = datetime.datetime.now()
while pathControl.iterRunGo:
pos, ori = pathControl.iterRun_move()
#pathControl.display()
sim.simxSetObjectPosition(clientID, Quadricopter, -1, pos,
sim.simx_opmode_streaming)
sim.simxSetObjectOrientation(clientID, Quadricopter, -1, ori,
sim.simx_opmode_streaming)
if (pathControl.pathIter != lastIter):
sim.simxSetObjectPosition(
clientID, QuadricopterT, -1,
pathControl.path[pathControl.goalIter][pathControl.pathIter],
sim.simx_opmode_streaming)
now = datetime.datetime.now()
print("Time between goals: ", (now - lastTime).total_seconds(),
"[s]")
lastIter = pathControl.pathIter
lastTime = now
if pathControl.goalIter != lastGoal:
now = datetime.datetime.now()
print("Time of flight: ", (now - startTime).total_seconds(), "[s]")
lastGoal = pathControl.goalIter
startTime = now
pathControl.iterRun_stop()
sim.simxStopSimulation(clientID, sim.simx_opmode_blocking)
sim.simxFinish(clientID)
vrep.simxSetJointTargetVelocity(clientID, slide1_handle, 0, opmode)
vrep.simxSetJointTargetVelocity(clientID, slide2_handle, 0, opmode)
vrep.simxSetJointTargetVelocity(clientID, Worm_2_handle, 0, opmode)
vrep.simxSetJointTargetVelocity(clientID, Board_handle, 0, opmode)
#keyboard "S"
if keyboard.is_pressed("S"):
vrep.simxSetJointTargetVelocity(clientID, slide1_handle, -1,
opmode)
vrep.simxSetJointTargetVelocity(clientID, slide2_handle, -1,
opmode)
vrep.simxSetJointTargetVelocity(clientID, Worm_2_handle, -1,
opmode)
vrep.simxSetJointTargetVelocity(clientID, Board_handle, -1, opmode)
#keyboard "C"
if keyboard.is_pressed("C"):
vrep.simxSetJointTargetVelocity(clientID, DOOR_handle, 3, opmode)
#keyboard "V"
if keyboard.is_pressed("V"):
vrep.simxSetJointTargetVelocity(clientID, DOOR_handle, 0, opmode)
#keyboard "esc"
if keyboard.is_pressed("esc"):
vrep.simxStopSimulation(clientID, opmode)
break
else:
print('Failed connecting to remote API server')
print('End')
generated_img_file_path, transformed_image)
if return_code == True:
print(
'\nTransformed maze image from CoppeliaSim: '
+ str(generated_img_file_path) +
' was saved in \'generated_images\' folder successfully!'
)
else:
print(
'\n[ERROR] Failed to save Transformed maze image from CoppeliaSim in \'generated_images\' folder.'
)
# Stop the simulation
return_code = sim.simxStopSimulation(
client_id, sim.simx_opmode_oneshot)
# Making sure that last command sent out had time to arrive
sim.simxGetPingTime(client_id)
if ((return_code
== sim.simx_return_novalue_flag) or
(return_code == sim.simx_return_ok)):
print('\nSimulation stopped correctly.')
time.sleep(2)
# Stop the Remote API connection with CoppeliaSim server
try:
exit_remote_api_server()
def listener_callback(self, msg):
if msg.transforms[0].child_frame_id == 'robot1' :
self.x1 = msg.transforms[0].transform.translation.x
self.y1 = msg.transforms[0].transform.translation.y
self.xr1 = msg.transforms[0].transform.rotation.x
self.yr1 = msg.transforms[0].transform.rotation.y
self.zr1 = msg.transforms[0].transform.rotation.z
self.wr1 = msg.transforms[0].transform.rotation.w
self.Theta1 = euler_from_quaternion(self.xr1,self.yr1,self.zr1,self.wr1)
if msg.transforms[0].child_frame_id == 'robot2' :
self.x2 = msg.transforms[0].transform.translation.x
self.y2 = msg.transforms[0].transform.translation.y
self.xr2 = msg.transforms[0].transform.rotation.x
self.yr2 = msg.transforms[0].transform.rotation.y
self.zr2 = msg.transforms[0].transform.rotation.z
self.wr2 = msg.transforms[0].transform.rotation.w
self.Theta2 = euler_from_quaternion(self.xr2,self.yr2,self.zr2,self.wr2)
distance = abs(self.x2 - self.x1) + abs(self.y2 - self.y1)
print(distance)
# Run Consensus Algorithm as long as they don't meet
if distance > 1:
" Calculate Control inputs u1 and u2 "
u1 = np.array([[ self.k*(self.x2-self.x1)],[self.k*(self.y2-self.y1)]]) # 2x1
u2 = np.array([[ self.k*(self.x1-self.x2)],[self.k*(self.y1-self.y2)]]) # 2x1
" Calculate V1/W1 and V2/W2 "
S1 = np.array([[self.v1], [self.w1]]) #2x1
G1 = np.array([[1,0], [0,1/L]]) #2x2
R1 = np.array([[math.cos(self.Theta1),math.sin(self.Theta1)],[-math.sin(self.Theta1),math.cos(self.Theta1)]]) #2x2
S1 = np.dot(np.dot(G1, R1), u1) #2x1
print(S1)
S2 = np.array([[self.v2], [self.w2]]) #2x1
G2 = np.array([[1,0], [0,1/L]]) #2x2
R2 = np.array([[math.cos(self.Theta2),math.sin(self.Theta2)],[-math.sin(self.Theta2),math.cos(self.Theta2)]]) #2x2
S2 = np.dot(np.dot(G2, R2), u2) # 2x1
" Calculate VL1/VR1 and VL2/VR2 "
D = np.array([[1/2,1/2],[-1/(2*d),1/(2*d)]]) #2x2
Di = np.linalg.inv(D) #2x2
Speed_L1 = np.array([[self.vL1], [self.vR1]]) # Vector 2x1 for Speed of Robot 1
Speed_L2 = np.array([[self.vL2], [self.vR2]]) # Vector 2x1 for Speed of Robot 2
M1 = np.array([[S1[0]],[S1[1]]]).reshape(2,1) #2x1
M2 = np.array([[S2[0]], [S2[1]]]).reshape(2,1) #2x1
Speed_L1 = np.dot(Di, M1) # 2x1 (VL1, VR1)
Speed_L2 = np.dot(Di, M2) # 2x1 (VL2, VR2)
VL1 = float(Speed_L1[0])
VR1 = float(Speed_L1[1])
VL2 = float(Speed_L2[0])
VR2 = float(Speed_L2[1])
" Calculate the Pose of Robot 2 w.r.t Robot 1 and Control input U1 "
self.X1 = self.x2 - self.x1 # Relative Pose of Robot 2 wrt Robot 1 in Global frame for X coordinate of dimension 1x1
self.Y1 = self.y2 -self.y1 # Relative Pose of Robot 2 wrt Robot 1 in Global frame for Y coordinate of dimension 1x1
self.U1 = u1 # Control input U1 in Global frame for robot 1 of dimension 2x1
" Calculate the Pose of Robot 1 w.r.t Robot 2 and Control input U2 "
self.X2 = self.x1 - self.x2 # Relative Pose of Robot 1 wrt Robot 2 in Global frame for X coordinate of dimension 1x1
self.Y2 = self.y1 -self.y2 # Relative Pose of Robot 1 wrt Robot 2 in Global frame for Y coordinate of dimension 1x1
self.U2 = u2 # Control input U2 in Global frame for robot 2 of dimension 2x1
" Transform Control Input U1 from Global to Local Reference Frame "
U1L = np.dot(R1, self.U1) # Control input of Robot 1 in Local Frame of dimension 2x1
U2L = np.dot(R2, self.U2) # Control input of Robot 2 in Local Frame of dimension 2x1
" Transform Relative Pose from Global to Local Reference Frame "
PoseG1 = np.array([[self.X1],[self.Y1]]) # Relative Pose of Robot 2 wrt Robot 1 in Global Frame of dimension 2x1
PoseL1 = np.dot(R1, PoseG1) # Relative Pose of Robot 2 wrt Robot 2 in Local Frame of dimension 2x1
PoseG2 = np.array([[self.X2],[self.Y2]]) # Relative Pose of Robot 1 wrt Robot 1 in Global Frame of dimension 2x1
PoseL2 = np.dot(R2, PoseG2) # Relative Pose of Robot 1 wrt Robot 2 in Local Frame of dimension 2x1
" Publish Speed Commands to Robot 1"
msgl1 = Float32()
msgr1 = Float32()
msgl1.data = VL1
msgr1.data = VR1
self.publisher_l1.publish(msgl1)
self.publisher_r1.publish(msgr1)
#self.get_logger().info('Publishing R1: "%s"' % msgr1.data)
" Publish Speed Commands to Robot 2"
msgl2 = Float32()
msgr2 = Float32()
msgl2.data = VL2
msgr2.data = VR2
self.publisher_l2.publish(msgl2)
self.publisher_r2.publish(msgr2)
" Write Values to CSV1 and CSV2 "
if self.count % 2 == 0:
with open('robot1.csv', 'a', newline='') as f:
fieldnames = ['Data_X', 'Data_Y', 'Angle', 'Label_X', 'Label_Y']
thewriter = csv.DictWriter(f, fieldnames=fieldnames)
if self.i1 == 0: # write header value once
thewriter.writeheader()
self.i1 = 1
if self.j1 != 0:
thewriter.writerow({'Data_X' : PoseL1[0][0], 'Data_Y' : PoseL1[1][0], 'Angle' : self.Theta1, 'Label_X' : U1L[0][0], 'Label_Y' : U1L[1][0]})
if self.j1 == 0: # skip first value because it's noisy
self.j1 = 1
with open('robot2.csv', 'a', newline='') as f:
fieldnames = ['Data_X', 'Data_Y', 'Angle', 'Label_X', 'Label_Y']
thewriter = csv.DictWriter(f, fieldnames=fieldnames)
if self.i2 == 0: # write header value once
thewriter.writeheader()
self.i2 = 1
if self.j2 != 0:
thewriter.writerow({'Data_X' : PoseL2[0][0], 'Data_Y' : PoseL2[1][0], 'Angle' : self.Theta2, 'Label_X' : U2L[0][0], 'Label_Y' : U2L[1][0]})
if self.j2 == 0: # skip first value because it's noisy
self.j2 = 1
self.count += 1 # Counter to skip values while saving to csv file
else:
print(" Simulation ", self.scene)
# Stop Simulation
sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait)
# Retrieve some handles:
ErrLocM1,LocM1 =sim.simxGetObjectHandle(clientID, 'robot1', sim.simx_opmode_oneshot_wait)
if (not ErrLocM1==sim.simx_return_ok):
pass
ErrLocM2,LocM2 =sim.simxGetObjectHandle(clientID, 'robot2#0', sim.simx_opmode_oneshot_wait)
if (not ErrLocM2==sim.simx_return_ok):
pass
ErrLoc1,Loc1 =sim.simxGetObjectPosition(clientID, LocM1, -1, sim.simx_opmode_oneshot_wait)
if (not ErrLoc1==sim.simx_return_ok):
pass
ErrLoc2,Loc2 =sim.simxGetObjectPosition(clientID, LocM2, -1, sim.simx_opmode_oneshot_wait)
if (not ErrLoc2==sim.simx_return_ok):
pass
ErrLocO1,OriRobo1 =sim.simxGetObjectOrientation(clientID,LocM1, -1, sim.simx_opmode_oneshot_wait)
if (not ErrLocO1==sim.simx_return_ok):
pass
ErrLocO2,OriRobo2 =sim.simxGetObjectOrientation(clientID,LocM2, -1, sim.simx_opmode_oneshot_wait)
if (not ErrLocO2==sim.simx_return_ok):
pass
OriRobo1[2] = scenes[self.scene][2]
OriRobo2[2] = scenes[self.scene][5]
# Set Robot Orientation
sim.simxSetObjectOrientation(clientID, LocM1, -1, OriRobo1, sim.simx_opmode_oneshot_wait)
sim.simxSetObjectOrientation(clientID, LocM2, -1, OriRobo2, sim.simx_opmode_oneshot_wait)
Loc1[0] = scenes[self.scene][0]
Loc2[0] = scenes[self.scene][3]
Loc1[1] = scenes[self.scene][1]
Loc2[1] = scenes[self.scene][4]
# Set Robot Position
sim.simxSetObjectPosition(clientID, LocM1, -1, Loc1, sim.simx_opmode_oneshot)
sim.simxSetObjectPosition(clientID, LocM2, -1, Loc2, sim.simx_opmode_oneshot)
# Print Positions and Orientation
#print("Robot1 Position:", Loc1)
#print("Robot2 Position:", Loc2)
#print("Robot1 Orientation:", OriRobo1)
#print("Robot2 Orientation:", OriRobo2)
# Nb of Scene Counter
self.scene += 1
# Start Simulation
sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait)
time.sleep(3)
if self.scene == scenes.shape[0]-1:
# Stop Simulation
sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait)
# End Connection to V-Rep
sim.simxFinish(clientID)
mean_x, mean_y = calMeanPos(cropImage)
#^ 计算大scale的平均黑色像素点位置，用于小车找不到方向时决定方向使用
dir_x, dir_y = calMeanPos(landscape)
#^ 增加一个函数calMeanPosOfBlackLine用于计算目标直线的平均黑点坐标
acc_x, acc_y = calMeanPosOfBlackLine(followLine)
# print(acc_x)
#^ 加速速率设为对数增长
accRate = max(
math.log((followLineX - acc_x) / (followLineX / 5) +
1), 1) if acc_x else 1
#^ 计算大尺度下的error
landscape_error = dir_y - bigY / 2 if dir_y else None
# print(landscape_error)
leftVelocity, rightVelocity = motor(mean_x, mean_y,
landscape_error, accRate)
#^ 同步触发
sim.simxSynchronousTrigger(clientID)
#^ 暂且使用try-except来处理控制无限循环，具体逻辑懒得讲了，自己试试Ctrl+C是什么效果吧
except KeyboardInterrupt:
print('=============================================')
print('Simulation stopped due to keyboard interrupt.')
print('=============================================')
except Exception as e:
traceback.print_exc()
# stop simulation and cleanup
status = sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot)
sim.simxGetPingTime(clientID)
sim.simxFinish(clientID)
#%%
def reset(self):
if self.sim_running:
self.stop_simulation()
# Stop Simulation
sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait)
# Retrieve some handles:
ErrLocM1, LocM1 = sim.simxGetObjectHandle(
clientID, 'robot1', sim.simx_opmode_oneshot_wait)
if (not ErrLocM1 == sim.simx_return_ok):
pass
ErrLocM2, LocM2 = sim.simxGetObjectHandle(
clientID, 'robot2#0', sim.simx_opmode_oneshot_wait)
if (not ErrLocM2 == sim.simx_return_ok):
pass
ErrLoc1, Loc1 = sim.simxGetObjectPosition(
clientID, LocM1, -1, sim.simx_opmode_oneshot_wait)
if (not ErrLoc1 == sim.simx_return_ok):
pass
ErrLoc2, Loc2 = sim.simxGetObjectPosition(
clientID, LocM2, -1, sim.simx_opmode_oneshot_wait)
if (not ErrLoc2 == sim.simx_return_ok):
pass
ErrLocO1, OriRobo1 = sim.simxGetObjectOrientation(
clientID, LocM1, -1, sim.simx_opmode_oneshot_wait)
if (not ErrLocO1 == sim.simx_return_ok):
pass
ErrLocO2, OriRobo2 = sim.simxGetObjectOrientation(
clientID, LocM2, -1, sim.simx_opmode_oneshot_wait)
if (not ErrLocO2 == sim.simx_return_ok):
pass
OriRobo1[2] = scenes[self.scene][2]
OriRobo2[2] = scenes[self.scene][5]
# Set Robot Orientation
sim.simxSetObjectOrientation(clientID, LocM1, -1, OriRobo1,
sim.simx_opmode_oneshot_wait)
sim.simxSetObjectOrientation(clientID, LocM2, -1, OriRobo2,
sim.simx_opmode_oneshot_wait)
Loc1[0] = scenes[self.scene][0]
Loc2[0] = scenes[self.scene][3]
Loc1[1] = scenes[self.scene][1]
Loc2[1] = scenes[self.scene][4]
# Set Robot Position
sim.simxSetObjectPosition(clientID, LocM1, -1, Loc1,
sim.simx_opmode_oneshot)
sim.simxSetObjectPosition(clientID, LocM2, -1, Loc2,
sim.simx_opmode_oneshot)
# Nb of Scene Counter
self.scene += 1
" Use Adjacency Matrix to find Mxy and Phi's "
A = np.ones(6) - np.identity(6) # Adjancency Matrix
self.X = np.array([[self.x1], [self.x2], [self.x3], [self.x4],
[self.x5], [self.x6]]) #6x1
self.Y = np.array([[self.y1], [self.y2], [self.y3], [self.y4],
[self.y5], [self.y6]]) #6x1
Mx = np.zeros((6, 1)) # 6x1
My = np.zeros((6, 1)) # 6x1
for i in range(1, 7):
for j in range(1, 7):
Mx[i - 1] += (A[i - 1][j - 1]) * (
self.X[j - 1] - self.X[i - 1]) # 1x1 each
My[i - 1] += (A[i - 1][j - 1]) * (
self.Y[j - 1] - self.Y[i - 1]) # 1x1 each
Mx1 = float(Mx[0]) / 5 # 1x1
My1 = float(My[0]) / 5 # 1x1
Mx2 = float(Mx[1]) / 5 # 1x1
My2 = float(My[1]) / 5 # 1x1
Mx3 = float(Mx[2]) / 5 # 1x1
My3 = float(My[2]) / 5 # 1x1
Mx4 = float(Mx[3]) / 5 # 1x1
My4 = float(My[3]) / 5 # 1x1
Mx5 = float(Mx[4]) / 5 # 1x1
My5 = float(My[4]) / 5 # 1x1
Mx6 = float(Mx[5]) / 5 # 1x1
My6 = float(My[5]) / 5 # 1x1
self.Phix1 = (Mx2 + Mx3 + Mx4 + Mx5 + Mx6) / 5 # 1x1
self.Phiy1 = (My2 + My3 + My4 + My5 + My6) / 5 # 1x1
self.Phix2 = (Mx1 + Mx3 + Mx4 + Mx5 + Mx6) / 5 # 1x1
self.Phiy2 = (My1 + My3 + My4 + My5 + My6) / 5 # 1x1
self.Phix3 = (Mx1 + Mx2 + Mx4 + Mx5 + Mx6) / 5 # 1x1
self.Phiy3 = (My1 + My2 + My4 + My5 + My6) / 5 # 1x1
self.Phix4 = (Mx1 + Mx2 + Mx3 + Mx5 + Mx6) / 5 # 1x1
self.Phiy4 = (My1 + My2 + My3 + My5 + My6) / 5 # 1x1
self.Phix5 = (Mx1 + Mx2 + Mx3 + Mx4 + Mx6) / 5 # 1x1
self.Phiy5 = (My1 + My2 + My3 + My4 + My6) / 5 # 1x1
self.Phix6 = (Mx1 + Mx2 + Mx3 + Mx4 + Mx5) / 5 # 1x1
self.Phiy6 = (My1 + My2 + My3 + My4 + My5) / 5 # 1x1
observation_DQN = np.array([Mx1, My1, self.Phix1, self.Phiy1])
# Start Simulation
sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait)
time.sleep(5)
return observation_DQN
def main():
sim.simxFinish(-1)
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID != -1:
print('Connected to remote API server')
else:
print('Failed connecting to remote API server')
sys.exit('Could not connect to remote API server')
sim.simxSynchronous(clientID, 1) #synchronous operation necessary for
sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot)
jointHandles = [-1, -1, -1, -1, -1, -1]
move_helper.getJointHandles(clientID, jointHandles)
#handle of UR3
base_handle = sim.simxGetObjectHandle(clientID, "UR3",
sim.simx_opmode_blocking)[1]
#handle for end-effector
end_effector_handle = sim.simxGetObjectHandle(clientID, "UR3_link7",
sim.simx_opmode_blocking)[1]
#position of UR3 end effector wrt UR3 base frame
end_effector_wrt_base = sim.simxGetObjectPosition(
clientID, end_effector_handle, base_handle,
sim.simx_opmode_blocking)[1]
#handle for the ball
ball_handle = sim.simxGetObjectHandle(clientID, 'Ball',
sim.simx_opmode_blocking)[1]
#handle for ball sensor/proximity sensor
sensorHandle = sim.simxGetObjectHandle(clientID, 'Ball_Sensor',
sim.simx_opmode_blocking)[1]
#position and orientation of proximity sensore wrt UR3 base frame
T_sensor_in_base = np.array( \
[[0, 0, -1, end_effector_wrt_base[0]],
[0, -1, 0, end_effector_wrt_base[1]], \
[-1, 0, 0, end_effector_wrt_base[2]],
[0, 0, 0, 1]])
#the actual end point of the ball based on path in the world frame
path = generate_path()
end_pt = path[path.shape[0] - 1]
detectedPoints = np.zeros((2, 3))
#the position and orientation of the UR3 base frame wrt world frame (T_wb)
T_base_in_world = np.array([[-1, 0, 0, -1.4001], [0, -1, 0, -0.000086],
[0, 0, 1, 0.043], [0, 0, 0, 1]])
#T_bw
T_world_in_base = np.linalg.inv(T_base_in_world)
#end pt coords in homog form
end_pt_homogenous_coords = np.array([[end_pt[0], end_pt[1], end_pt[2],
1]]).T
#T_bw * p_w = p_b (4,4) * (4, 1) -> (4, 1), left out final element -> (3, 1)
#the actual endpt of the ball in the base frame
end_pt_ball_in_base = np.matmul(T_world_in_base,
end_pt_homogenous_coords)[:3]
detection_thread = threading.Thread(target=utils.calc_ball_position,
args=(clientID, sensorHandle, path,
detectedPoints))
motion_thread = threading.Thread(target=utils.move_ball,
args=(clientID, ball_handle, path))
motion_thread.start()
detection_thread.start()
detection_thread.join()
#point_in_sensorX - homogeneous coords of detectedPoints in the frame of the proximity sensor
point_in_sensor1 = np.array([[detectedPoints[0][0]],
[detectedPoints[0][1]],
[detectedPoints[0][2]], [1]])
point_in_sensor2 = np.array([[detectedPoints[1][0]],
[detectedPoints[1][1]],
[detectedPoints[1][2]], [1]])
#T_bs * p_s = p_b
point1_ball_in_base = np.matmul(T_sensor_in_base, point_in_sensor1)[:3]
point2_ball_in_base = np.matmul(T_sensor_in_base, point_in_sensor2)[:3]
vector = (1.0 * point2_ball_in_base - point1_ball_in_base)
#convert to unit vector
vector /= np.linalg.norm(vector)
t = (end_effector_wrt_base[0] - point2_ball_in_base[0]) / vector[0]
final_x = end_effector_wrt_base[0]
final_y = point2_ball_in_base[1] + (t * vector[1])
final_z = point2_ball_in_base[2] + (t * vector[2])
predicted_point = np.array([[final_x], [final_y[0]], [final_z[0]]])
difference = predicted_point - end_pt_ball_in_base
print("Difference: " + str(difference))
#the predicted direction the ball is moving in, based off of predicted points 1 and 2
#if the difference between detected points is too small
if (vector[0] == 0):
vector[0] += 0.000000001
#find the estimated position of the ball when it is in the yz plane of the UR3
#the predicted point of the ball
print("Predicted Point: " + str(predicted_point))
print("End Point Ball in Base: " + str(end_pt_ball_in_base))
#compare prediction to actual end pt of ball
#our initial guess of what the UR3 joint angles should be to get to finalT
thetas = np.array([0.0, 0.0, 0.0])
#get a rough initial guess for theta of joint 2
predicted_y = predicted_point[1]
predicted_z = predicted_point[2]
position_joint2 = sim.simxGetObjectPosition(clientID, jointHandles[1],
base_handle,
sim.simx_opmode_blocking)[1]
print(position_joint2)
joint2_y = position_joint2[1]
joint2_z = position_joint2[2]
#distance from predicted point to joint2 in yz plane
d1 = np.sqrt((joint2_y - predicted_y)**2 + (joint2_z - predicted_z)**2)
z_length = predicted_z - joint2_z
y_length = predicted_y - joint2_y
#our thetas to be calculated
theta2_guess = 0.0
theta3_guess = 0.0
theta4_guess = 0.0
#define relevant link lengths
L1 = 0.244
L2 = 0.213
L3 = 0.083
#if it's in this range, it doesn't make sense to move others
if d1 < (L1 - L2):
theta2_guess = np.arctan2(y_length, z_length)
theta3_guess = 0.0
theta4_guess = 0.0
elif d1 <= (L1 + L2 - L3):
top2 = L2**2 - L1**2 - d1**2
bot2 = -2 * L1 * d1
theta2_guess = np.arctan2(y_length, z_length) - np.arccos(top2 / bot2)
top3 = d1**2 - L1**2 - L2**2
bot3 = -2 * L1 * L2
theta3_guess = np.pi - np.arccos(top3 / bot3)
#fold in on self
theta4_guess = np.pi / 2
else:
d2 = d1 - L1
theta2_guess = np.arctan2(y_length, z_length)
top3_2 = L3**2 - L2**2 - d2**2
bot3_2 = -2 * d2 * L2
print("top3_2/bot3_2: " + str(top3_2 / bot3_2))
theta3_guess = np.arccos(top3_2 / bot3_2)
top4 = d2**2 - L2**2 - L3**2
bot4 = -2 * L2 * L3
print("top4/bot4: " + str(top4 / bot4))
theta4_guess = np.arccos(top4 / bot4) - np.pi
thetas[0] = theta2_guess
thetas[1] = theta3_guess
thetas[2] = theta4_guess
print("thetas: " + str(thetas))
for i in range(1, 4):
sim.simxSetJointTargetPosition(clientID, jointHandles[i],
thetas[i - 1],
sim.simx_opmode_oneshot_wait)
motion_thread.join(
) #stop motion thread after robot has moved to defending position
print("Actual End Effector Position " + str(
sim.simxGetObjectPosition(clientID, end_effector_handle, base_handle,
sim.simx_opmode_blocking)[1]))
sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot)
sim.simxFinish(clientID)
return 1
def close(self):
sim.simxStopSimulation(self.clientID, sim.simx_opmode_oneshot)
sim.simxGetPingTime(self.clientID)
sim.simxFinish(self.clientID)
def stop(self):
s.simxStopSimulation(self._id, self._def_op_mode)
def endSimulation(clientID):
sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot)
sim.simxFinish(clientID)
def close_connection_to_server(self):
sim.simxGetPingTime(self.clientId)
error = sim.simxStopSimulation(self.clientId, sim.simx_opmode_oneshot)
if error == sim.simx_return_ok:
print(str(error) + '! ERROR: simxSetModelProperty pioneer')
sim.simxFinish(self.clientId)
avoid, ulb, urb = pc.braitenberg(clientID, usensor)
errp, ulc, urc, pos, rot = pc.continuosControl(
clientID, robot, (pointsx[step], pointsy[step]))
ul = ulb if avoid else ulc
ur = urb if avoid else urc
# Check achieved goals
achieved = achieved + 1 if pc.distance2p(
pos, goals[achieved]) <= 0.3 else achieved
# If an obstacle was avoided, replan the path. Only works when there are more than 2 goals left
if prev and not avoid:
path = pc.splinePath(p[:, 0][achieved:],
p[:,
1][achieved:]) # New path of remaining points
pointsx = np.linspace(min(p[:, 0][achieved:]),
max(p[:, 0][achieved:]),
num=(60 - step),
endpoint=True)
pointsy = path(pointsx)
step = 0 # Start at the beginning of new path
errf = vrep.simxSetJointTargetVelocity(clientID, motorL, ul,
vrep.simx_opmode_streaming)
errf = vrep.simxSetJointTargetVelocity(clientID, motorR, ur,
vrep.simx_opmode_streaming)
# The End
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
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)
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)
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()
plt.xlabel('time')
plt.ylabel('joint angles')
plt.show()