def rotates_z(handle, deg):
rotates(handle, deg)
arc = cal_arc(45)
res, ori0 = vrep.simxGetObjectOrientation(clientID, v0, -1,
vrep.simx_opmode_oneshot_wait)
res = vrep.simxSetObjectOrientation(clientID, handle, -1,
(ori0[0], ori0[1], ori0[2] + arc),
vrep.simx_opmode_oneshot_wait)
rotates(handle, deg)
res = vrep.simxSetObjectOrientation(clientID, handle, -1,
(ori0[0], ori0[1], ori0[2]),
vrep.simx_opmode_oneshot_wait)
def grasp_object(self, object_handle, parent_handle):
res, rot = sim.simxGetObjectOrientation(self.clientID, object_handle,
parent_handle,
sim.simx_opmode_blocking)
sim.simxSetObjectParent(self.clientID, object_handle, self.endEffector,
False, sim.simx_opmode_blocking)
sim.simxSetObjectPosition(
self.clientID, object_handle, sim.sim_handle_parent, np.zeros(3),
sim.simx_opmode_blocking) # assume it has been grasped
sim.simxSetObjectOrientation(
self.clientID, object_handle, self.endEffector, rot,
sim.simx_opmode_blocking) # not changeing the object orientation
return rot
def put_object(self, object_handle, parent_handle):
res, rot = sim.simxGetObjectOrientation(self.clientID, object_handle,
-1, sim.simx_opmode_blocking)
res, trans = sim.simxGetObjectPosition(self.clientID, object_handle,
-1, sim.simx_opmode_blocking)
res = sim.simxSetObjectParent(self.clientID, object_handle,
parent_handle, False,
sim.simx_opmode_blocking)
sim.simxSetObjectOrientation(
self.clientID, object_handle, -1, rot, sim.simx_opmode_blocking
) # not changeing the object orientation after detaching
def change_robot_orientation(self):
ori_body = sim.simxGetObjectOrientation(self.clientID, self.khepera,
-1, sim.simx_opmode_buffer)
angles = ori_body[1][:]
angles[2] = 2 * np.pi * random.random()
errorCode = sim.simxSetObjectOrientation(self.clientID, self.khepera,
-1, angles,
sim.simx_opmode_oneshot)
def setAbsolutePose(handle, pos, rot):
res1 = sim.simxSetObjectPosition(clientID, handle, -1, pos,
sim.simx_opmode_oneshot)
# print(res1)
res2 = sim.simxSetObjectOrientation(clientID, handle, -1, rot,
sim.simx_opmode_oneshot)
# print(res2)
return res1, res2
def set_orientation(self, orientation, relative_object=-1):
# By default, get the position wrt the reference frame
if relative_object != -1:
relative_object = self._get_handler(relative_object)
err_code = sim.simxSetObjectOrientation(self.client_id, self.frame,
relative_object, orientation, sim.simx_opmode_oneshot)
if err_code != 0:
print("ERROR: CANNOT SET ORIENTATION W.R.T. {} TO {}".format(relative_object, orientation))
def reset_target_object(self, x, y, z):
error_code = vrep.simxSetObjectPosition(self.clientID,
self.main_target, -1,
[x, y, z],
vrep.simx_opmode_oneshot)
time.sleep(.1) # needs a brief pause or it is skipped.
error_code = vrep.simxSetObjectOrientation(self.clientID,
self.main_target, -1,
self.main_target_angles,
vrep.simx_opmode_oneshot)
def reset_target_object(self, x, y, z):
error_code = vrep.simxSetObjectPosition(self.clientID,
self.main_target, -1,
[x, y, z],
vrep.simx_opmode_oneshot)
time.sleep(
.2
) # needs a brief pause or it is skipped, don't make this less... it won't put it back right!
error_code = vrep.simxSetObjectOrientation(self.clientID,
self.main_target, -1,
self.main_target_angles,
vrep.simx_opmode_oneshot)
def set_drone_orientation(angle):
err, Qang = vrep.simxGetObjectOrientation(clientID, QuadricopterT, -1,
vrep.simx_opmode_oneshot_wait)
i = Qang[2]
while abs(Qang[2] - angle) > radians(2):
err, Qang = vrep.simxGetObjectOrientation(
clientID, QuadricopterT, -1, vrep.simx_opmode_oneshot_wait)
i += copysign(radians(1), angle - Qang[2])
err = vrep.simxSetObjectOrientation(clientID, QuadricopterT, -1,
(0, 0, i),
vrep.simx_opmode_oneshot)
time.sleep(0.1)
time.sleep(1)
def rotates(handle, deg):
arc = cal_arc(deg)
res, ori0 = vrep.simxGetObjectOrientation(clientID, v0, -1,
vrep.simx_opmode_oneshot_wait)
res = vrep.simxSetObjectOrientation(clientID, handle, -1,
(ori0[0] + arc, ori0[1], ori0[2]),
vrep.simx_opmode_oneshot_wait)
time.sleep(t)
res = vrep.simxSetObjectOrientation(
clientID, handle, -1, (ori0[0] + arc, ori0[1] + arc, ori0[2]),
vrep.simx_opmode_oneshot_wait)
time.sleep(t)
res = vrep.simxSetObjectOrientation(clientID, handle, -1,
(ori0[0], ori0[1] + arc, ori0[2]),
vrep.simx_opmode_oneshot_wait)
time.sleep(t)
res = vrep.simxSetObjectOrientation(
clientID, handle, -1, (ori0[0] - arc, ori0[1] + arc, ori0[2]),
vrep.simx_opmode_oneshot_wait)
time.sleep(t)
res = vrep.simxSetObjectOrientation(clientID, handle, -1,
(ori0[0] - arc, ori0[1], ori0[2]),
vrep.simx_opmode_oneshot_wait)
time.sleep(t)
res = vrep.simxSetObjectOrientation(
clientID, handle, -1, (ori0[0] - arc, ori0[1] - arc, ori0[2]),
vrep.simx_opmode_oneshot_wait)
time.sleep(t)
res = vrep.simxSetObjectOrientation(clientID, handle, -1,
(ori0[0], ori0[1] - arc, ori0[2]),
vrep.simx_opmode_oneshot_wait)
time.sleep(t)
res = vrep.simxSetObjectOrientation(
clientID, handle, -1, (ori0[0] + arc, ori0[1] - arc, ori0[2]),
vrep.simx_opmode_oneshot_wait)
time.sleep(t)
res = vrep.simxSetObjectOrientation(clientID, handle, -1, ori0,
vrep.simx_opmode_oneshot_wait)
vrep.simx_opmode_oneshot)
print(a)
if keyboard.is_pressed("home"):
a[2] += 0.0006
vrep.simxSetObjectPosition(clientID, objecthandle, -1, a,
vrep.simx_opmode_oneshot)
if keyboard.is_pressed("end"):
a[2] -= 0.0006
vrep.simxSetObjectPosition(clientID, objecthandle, -1, a,
vrep.simx_opmode_oneshot)
if keyboard.is_pressed("page up"):
rot[2] += 0.0004
vrep.simxSetObjectOrientation(clientID, objecthandle, -1, rot,
vrep.simx_opmode_oneshot)
if keyboard.is_pressed("page down"):
rot[2] -= 0.0004
vrep.simxSetObjectOrientation(clientID, objecthandle, -1, rot,
vrep.simx_opmode_oneshot)
if keyboard.is_pressed('q'):
break
#break # if user pressed a key other than the given key the loop will break
vrep.simxAddStatusbarMessage(clientID, 'Hello V-REP!',
vrep.simx_opmode_oneshot)
vrep.simxGetPingTime(clientID)
vrep.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 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)
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)
print("No camera")
err, cameraPos = vrep.simxGetObjectPosition(clientID, cameraID, -1,
vrep.simx_opmode_oneshot_wait)
err, Qpos = vrep.simxGetObjectPosition(clientID, Quadricopter, -1,
vrep.simx_opmode_oneshot_wait)
err = vrep.simxSetObjectPosition(clientID, QuadricopterT, -1,
(Qpos[0], Qpos[1], Qpos[2]),
vrep.simx_opmode_oneshot)
err, Qang = vrep.simxGetObjectOrientation(clientID, Quadricopter, -1,
vrep.simx_opmode_oneshot_wait)
err = vrep.simxSetObjectOrientation(clientID, QuadricopterT, -1,
(0, 0, Qang[2]), vrep.simx_opmode_oneshot)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
print("start simulation")
max_vel = 0.005 / 0.02
def set_drone_position(point):
err, Qpos0 = vrep.simxGetObjectPosition(clientID, Quadricopter, -1,
vrep.simx_opmode_oneshot_wait)
dist = sqrt((point[0] - Qpos[0])**2 + (point[1] - Qpos[1])**2 +
(point[2] - Qpos[2])**2)
tp = dist / max_vel
t0 = time.time()
while (time.time() - t0 < tp):
def set_orientation(self, a=0, b=0, g=90):
a = math.radians(a)
b = math.radians(b)
g = math.radians(g)
error = sim.simxSetModelProperty(self.clientId, self.pioneerHandle,
sim.sim_modelproperty_not_dynamic,
sim.simx_opmode_oneshot)
if error == sim.simx_return_ok:
print(str(error) + '! ERROR: simxSetModelProperty pioneer')
error = sim.simxSetModelProperty(self.clientId, self.leftMotorHandle,
sim.sim_modelproperty_not_dynamic,
sim.simx_opmode_oneshot)
if error == sim.simx_return_ok:
print(
str(error) +
'! ERROR: simxSetModelProperty self.leftMotorHandle')
error = sim.simxSetModelProperty(self.clientId, self.rightMotorHandle,
sim.sim_modelproperty_not_dynamic,
sim.simx_opmode_oneshot)
if error == sim.simx_return_ok:
print(
str(error) +
'! ERROR: simxSetModelProperty self.rightMotorHandle')
error = sim.simxSetModelProperty(self.clientId, self.casterFreeHandle,
sim.sim_modelproperty_not_dynamic,
sim.simx_opmode_oneshot)
if error == sim.simx_return_ok:
print(
str(error) +
'! ERROR: simxSetModelProperty self.casterFreeHandle')
error = sim.simxSetObjectOrientation(self.clientId, self.pioneerHandle,
-1, (a, b, g),
sim.simx_opmode_oneshot_wait)
if error != sim.simx_return_ok:
print(str(error) + '! ERROR: simxSetObjectPosition pioneer')
time.sleep(0.1)
error = sim.simxSetModelProperty(self.clientId, self.pioneerHandle,
not sim.sim_modelproperty_not_dynamic,
sim.simx_opmode_oneshot)
if error != sim.simx_return_ok:
print(str(error) + '! ERROR: simxSetModelProperty pioneer')
error = sim.simxSetModelProperty(self.clientId, self.leftMotorHandle,
not sim.sim_modelproperty_not_dynamic,
sim.simx_opmode_oneshot)
if error != sim.simx_return_ok:
print(
str(error) +
'! ERROR: simxSetModelProperty self.leftMotorHandle')
error = sim.simxSetModelProperty(self.clientId, self.rightMotorHandle,
not sim.sim_modelproperty_not_dynamic,
sim.simx_opmode_oneshot)
if error != sim.simx_return_ok:
print(
str(error) +
'! ERROR: simxSetModelProperty self.rightMotorHandle')
error = sim.simxSetModelProperty(self.clientId, self.casterFreeHandle,
not sim.sim_modelproperty_not_dynamic,
sim.simx_opmode_oneshot)
if error != sim.simx_return_ok:
print(
str(error) +
'! ERROR: simxSetModelProperty self.casterFreeHandle')
"change xc and yc to test the control law for yaw movement"
xc = -0.1
yc = -0.1
r = lambda z, y, x: -2 * (0.2 * (
(((xb - xt)**2 - (x - xt)**2)**2 * ((xb - xt)**2 * (th)**2 - y**2)**2 *
((xb - xt)**2 *
(tv)**2 - z**2)**2) / ((xb - xt)**12 * (th)**4 * (tv)**4)) - 4) * (
(((xb - xt)**2 - (x - xt)**2)**2 *
((xb - xt)**2 * (th)**2 - y**2)**2 * ((xb - xt)**2 *
(tv)**2 - z**2)**2) /
((xb - xt)**12 * (th)**4 *
(tv)**4)) * ((-(4 * ((y)**2 - th**2 * (xb - xt)**2)**2 * (
(z)**2 - tv**2 * (xb - xt)**2)**2 * ((xb - xt)**2 -
(x)**2) * (x - xt)) /
(th**4 * tv**4 * (xb - xt)**12)) * yc -
((4 * ((y)**2 - th**2 * (xb - xt)**2) *
((z)**2 - tv**2 * (xb - xt)**2)**2 * (
(xb - xt)**2 - (x - xt)**2)**2 * (y)) /
(th**4 * tv**4 * (xb - xt)**12)) * xc)
R = R + integrate.tplquad(r, 0.01, xc, lambda x: 0, lambda x: yc,
lambda x, y: 0, lambda x, y: 0.1)[0]
"set vision sensor position"
errorcode = sim.simxSetObjectOrientation(clientID, dummy, dummy, [0, 0, R],
sim.simx_opmode_oneshot)
"control loop end with time step"
time.sleep(0.05)
if (clientID != -1):
print('Connected to remote API server')
res, v0 = vrep.simxGetObjectHandle(clientID, 'Chessboard',
vrep.simx_opmode_oneshot_wait)
# res, vp = vrep.simxGetObjectParent(clientID, v0, vrep.simx_opmode_oneshot_wait)
res, pos0 = vrep.simxGetObjectPosition(clientID, v0, -1,
vrep.simx_opmode_oneshot_wait)
res, ori0 = vrep.simxGetObjectOrientation(clientID, v0, -1,
vrep.simx_opmode_oneshot_wait)
print(pos0, ori0)
while (vrep.simxGetConnectionId(clientID) != -1):
#############################original#############################
res = vrep.simxSetObjectPosition(clientID, v0, -1, pos0,
vrep.simx_opmode_oneshot_wait)
res = vrep.simxSetObjectOrientation(clientID, v0, -1, ori0,
vrep.simx_opmode_oneshot_wait)
time.sleep(t)
rotation_combinations(v0)
#############################layer 02#############################
res = vrep.simxSetObjectPosition(clientID, v0, v0, (0, 0, -0.2),
vrep.simx_opmode_oneshot_wait)
# res = vrep.simxSetObjectOrientation(clientID, v0, -1, ori0, vrep.simx_opmode_oneshot_wait)
time.sleep(t)
rotation_combinations(v0)
res = vrep.simxSetObjectPosition(clientID, v0, v0, (0.12, 0.18, 0),
vrep.simx_opmode_oneshot_wait)
time.sleep(t)
rotation_combinations(v0)
norm_goal_vec = np.linalg.norm(goal_vec)
goal_vec = goal_vec / norm_goal_vec
# print("goal_vec", goal_vec)
goal_vec *= speed
# формирование вектора отталкивания
# Получает позици близжайшего препятствия
obst_pose, dist_to_obst = get_near_obst(drone_pose, obst_list)
print("obst_pose", obst_pose, "dist_to_obst", dist_to_obst)
# rep_vec = np.array([0,0])
rep_vec = obst_pose - drone_pose
power = rep_force(dist_to_obst)
rep_vec = rep_vec * power
print("rep forces", rep_vec)
# публикация в сим
err = sim.simxSetObjectPosition(
clientID, QuadricopterT, -1,
(drone_pose[0] + goal_vec[0] - rep_vec[0], drone_pose[1] +
goal_vec[1] - rep_vec[1], drone_pose[2] + +goal_vec[2]),
sim.simx_opmode_blocking)
err = sim.simxSetObjectOrientation(clientID, QuadricopterT, -1,
(0, 0, rad(0)),
sim.simx_opmode_blocking)
time.sleep(0.01)
sim.simxFinish(clientID)
def grasp_and_put(self,
grasp_pos,
object_orientation,
put_pos,
object_handle,
parent_handle,
lift_padding=0.35):
# lift up before doing the grasping
cur_pose = self.cur_pos[len(self.cur_pos) - 1].copy().flatten()[0:3]
cur_pose[2] += lift_padding
self.inverse_kinematic(cur_pose,
object_orientation,
use_orientation=False,
num_iter=1200)
self.plan_motion(self.control)
# going to the top of grasping pose
grasp_pos[2] += lift_padding
self.inverse_kinematic(grasp_pos,
np.zeros(3),
use_orientation=False,
num_iter=1200)
self.plan_motion(self.control)
# lay down gripper
grasp_pos[2] -= lift_padding
self.inverse_kinematic(grasp_pos,
np.zeros(3),
use_orientation=False,
num_iter=1200)
self.plan_motion(self.control)
# grasping object
rotation = self.grasp_object(object_handle,
parent_handle) # put down the object
# lifting stage
grasp_pos[2] += lift_padding
self.inverse_kinematic(grasp_pos,
object_orientation,
use_orientation=False,
num_iter=1200)
self.plan_motion(self.control)
# put stage, first go to the top
put_pos[2] += lift_padding
self.inverse_kinematic(put_pos,
object_orientation,
use_orientation=False,
num_iter=1200)
self.plan_motion(self.control)
# lay down gripper
put_pos[2] -= lift_padding
self.inverse_kinematic(put_pos,
object_orientation,
use_orientation=False,
num_iter=1200)
self.plan_motion(self.control)
# put down object
self.put_object(object_handle, parent_handle)
# for faking the put down procedure
res = sim.simxSetObjectPosition(self.clientID, object_handle, -1,
put_pos, sim.simx_opmode_blocking)
sim.simxSetObjectOrientation(self.clientID, object_handle,
parent_handle, rotation,
sim.simx_opmode_blocking)
ErrLoc1, Loc1 = sim.simxGetObjectPosition(
clientID, LocM1, -1, sim.simx_opmode_oneshot_wait)
ErrLoc2, Loc2 = sim.simxGetObjectPosition(
clientID, LocM2, -1, sim.simx_opmode_oneshot_wait)
ErrLocO1, OriRobo1 = sim.simxGetObjectOrientation(
clientID, LocM1, -1, sim.simx_opmode_oneshot_wait)
ErrLocO2, OriRobo2 = sim.simxGetObjectOrientation(
clientID, LocM2, -1, sim.simx_opmode_oneshot_wait)
OriRobo1[2] = ((z_rot1[i][0] * math.pi) / 180)
OriRobo2[2] = ((z_rot2[i][0] * math.pi) / 180)
# 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] = x_disp1[j][0]
Loc2[0] = x_disp2[j][0]
Loc1[1] = y_disp1[k][0]
Loc2[1] = y_disp2[k][0]
# Set Robot Position
sim.simxSetObjectPosition(clientID, LocM1, -1, Loc1,
sim.simx_opmode_oneshot)
sim.simxSetObjectPosition(clientID, LocM2, -1, Loc2,
sim.simx_opmode_oneshot)
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)
