# Close eventual old connections
vrep.simxFinish(-1)
# Connect to V-REP remote server
clientID = vrep.simxStart('192.168.9.15', 19997, True, True, 5000, 5)
if clientID != -1:
print('Connected to remote API server')
res = vrep.simxAddStatusbarMessage(clientID, "40823217",
vrep.simx_opmode_oneshot)
if res not in (vrep.simx_return_ok, vrep.simx_return_novalue_flag):
print("Could not add a message to the status bar.")
opmode = vrep.simx_opmode_oneshot_wait
vrep.simxStartSimulation(clientID, opmode)
ret, front_handle = vrep.simxGetObjectHandle(clientID, "front_joint",
opmode)
ret, left_handle = vrep.simxGetObjectHandle(clientID, "left_joint", opmode)
ret, right_handle = vrep.simxGetObjectHandle(clientID, "right_joint",
opmode)
ret, gear_handle = vrep.simxGetObjectHandle(clientID, "gear_joint", opmode)
#ret,main = vrep.simxGetObjectHandle(clientID, "main", opmode)
while True:
#keyboard "w"
if keyboard.is_pressed("w"):
vrep.simxSetJointTargetVelocity(clientID, front_handle, 1000,
opmode)
vrep.simxSetJointTargetVelocity(clientID, left_handle, 1000,
opmode)
vrep.simxSetJointTargetVelocity(clientID, right_handle, 1000,
print('\n[ERROR] maze0' + str(file_num) +
'.jpg was not read correctly, something went wrong!')
print()
sys.exit()
# Check if connected to Remote API server and maze array has been generated successfully
if ((client_id != -1) and (maze_array_generated_flag == 1)):
try:
# Send maze array data to CoppeliaSim via Remote API
return_code = send_data(maze_array)
if (return_code == sim.simx_return_ok):
# Start the simulation
return_code = sim.simxStartSimulation(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 started correctly in CoppeliaSim.')
time.sleep(2)
try:
# Get image array and its resolution from Vision Sensor in ComppeliaSim scene
vision_sensor_image, image_resolution, return_code = get_vision_sensor_image(
)
epoch * steps + step)
return losses.avg
if __name__ == '__main__':
ip = '127.0.0.1'
port = 19997
sim.simxFinish(-1) # just in case, close all opened connections
clientID = sim.simxStart(ip, port, True, True, -5000, 5)
# Connect to V-REP
if clientID == -1:
import sys
sys.exit('\nV-REP remote API server connection failed (' + ip + ':' +
str(port) + '). Is V-REP running?')
print('Connected to Remote API Server') # show in the terminal
sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot)
Body = {}
get_first_handles(clientID, Body)
errorCode, acc = sim.simxGetStringSignal(clientID, 'Acceleration',
sim.simx_opmode_streaming)
returnCode, position = sim.simxGetObjectPosition(clientID, Body['HeadYaw'],
-1,
sim.simx_opmode_streaming)
converter = coordinate2angle()
converter.set_bound(np.load('bound_range.npy'), np.load('step.npy'), 32,
16)
primitives = np.load('primitive_data.npy')
model = CapabilityWithAttention(time_dim=32)
#model.load_state_dict(torch.load('./params-capability/Fri Jul 17 01-17-05 2020/best_fitted_params.pt')['model_state_dict'])
def init(_clientID, robotname):
global robot, robo, robotLeftMotor, robotRightMotor
global clientID
global paEsquerda, paDireita
global elevador, irRight, irLeft
global color_sensor_Left, color_sensor_Right
global positionrobot, angLeft, angRight, orientationrobot
clientID = _clientID
sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait)
print('Connected to remote API server')
sim.simxAddStatusbarMessage(clientID, 'Funcionando...',
sim.simx_opmode_oneshot_wait)
time.sleep(0.02)
# Coletar handles
#Robô
erro, robot = sim.simxGetObjectHandle(clientID, robotname,
sim.simx_opmode_blocking)
erro, robo = sim.simxGetObjectHandle(clientID, 'S_Base',
sim.simx_opmode_blocking)
#Motores
[erro,
robotLeftMotor] = sim.simxGetObjectHandle(clientID, 'Revolute_joint2',
sim.simx_opmode_blocking)
[erro,
robotRightMotor] = sim.simxGetObjectHandle(clientID, 'Revolute_joint1',
sim.simx_opmode_blocking)
#Garra
erro, paEsquerda = sim.simxGetObjectHandle(clientID,
'joint_pa_esquerda_garra',
sim.simx_opmode_blocking)
erro, paDireita = sim.simxGetObjectHandle(clientID,
'joint_pa_direita_garra',
sim.simx_opmode_blocking)
erro, elevador = sim.simxGetObjectHandle(clientID, 'joint_acoplador_garra',
sim.simx_opmode_blocking)
#Sensores
erro, irRight = sim.simxGetObjectHandle(clientID, 'Sensor_IR_direito',
sim.simx_opmode_blocking)
erro, irLeft = sim.simxGetObjectHandle(clientID, 'Sensor_IR_esquerdo',
sim.simx_opmode_blocking)
erro, color_sensor_Left = sim.simxGetObjectHandle(clientID,
'Sensor_cor_esquerda',
sim.simx_opmode_blocking)
erro, color_sensor_Right = sim.simxGetObjectHandle(
clientID, 'Sensor_cor_direita', sim.simx_opmode_blocking)
# Criar stream de dados
[erro,
positionrobot] = sim.simxGetObjectPosition(clientID, robot, -1,
sim.simx_opmode_streaming)
erro, angLeft = sim.simxGetJointPosition(clientID, robotLeftMotor,
sim.simx_opmode_streaming)
erro, angRight = sim.simxGetJointPosition(clientID, robotRightMotor,
sim.simx_opmode_streaming)
[erro, orientationrobot
] = sim.simxGetObjectOrientation(clientID, robot, -1,
sim.simx_opmode_streaming)
sim.simxReadProximitySensor(clientID, irRight, sim.simx_opmode_streaming)
sim.simxReadProximitySensor(clientID, irLeft, sim.simx_opmode_streaming)
sim.simxGetVisionSensorImage(clientID, color_sensor_Left, 0,
sim.simx_opmode_streaming)
sim.simxGetVisionSensorImage(clientID, color_sensor_Right, 0,
sim.simx_opmode_streaming)
j2=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.140,0.185,0.237,0.296,0.361,0.431,0.507,0.587,0.670,0.754,0.838,0.924,1.009,1.092,1.171,1.243,1.308,1.365,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.569,1.570,1.571,1.571,1.571]
j3=[0.000,0.000,-0.002,-0.009,-0.022,-0.042,-0.068,-0.100,-0.139,-0.185,-0.237,-0.296,-0.361,-0.433,-0.511,-0.595,-0.681,-0.767,-0.854,-0.942,-1.027,-1.107,-1.182,-1.249,-1.311,-1.365,-1.414,-1.455,-1.491,-1.519,-1.541,-1.557,-1.566,-1.569,-1.570,-1.571,-1.571,-1.571]
j4=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.027,1.107,1.181,1.249,1.310,1.365,1.413,1.455,1.490,1.519,1.541,1.556,1.565,1.569,1.570,1.570,1.570,1.571]
j5=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.028,1.108,1.182,1.250,1.311,1.366,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.569,1.570,1.571,1.571,1.571]
j6=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.027,1.108,1.182,1.249,1.311,1.365,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.569,1.570,1.571,1.571,1.571]
times=[0.000,0.050,0.100,0.150,0.200,0.250,0.300,0.350,0.400,0.450,0.500,0.550,0.600,0.650,0.700,0.750,0.800,0.850,0.900,0.950,1.000,1.050,1.100,1.150,1.200,1.250,1.300,1.350,1.400,1.450,1.500,1.550,1.600,1.650,1.700,1.750,1.800,1.850,1.900,1.950,2.000,2.050,2.100,2.150,2.200,2.250,2.300,2.350,2.400,2.450,2.500,2.550,2.600,2.650,2.700,2.750,2.800,2.850,2.900,2.950,3.000,3.050,3.100,3.150,3.200,3.250,3.300,3.350,3.400,3.450,3.500,3.550,3.600,3.650,3.700,3.750,3.800,3.850,3.900,3.950,4.000,4.050,4.100,4.150,4.200,4.250,4.300,4.350,4.400,4.450,4.500,4.550,4.600,4.650,4.700,4.750,4.800,4.850,4.900,4.950,5.000,5.050,5.100,5.150,5.200,5.250,5.300,5.350,5.400,5.450,5.500,5.550,5.600,5.650,5.700,5.750,5.800,5.850,5.900,5.950,6.000,6.050,6.100,6.150,6.200,6.250,6.300,6.350]
j1=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.360,0.431,0.506,0.587,0.669,0.753,0.838,0.923,1.008,1.091,1.170,1.243,1.308,1.365,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.564,1.556,1.543,1.524,1.498,1.465,1.426,1.380,1.328,1.270,1.205,1.136,1.065,0.994,0.922,0.849,0.777,0.705,0.632,0.560,0.487,0.415,0.342,0.270,0.197,0.125,0.053,-0.020,-0.092,-0.165,-0.237,-0.309,-0.382,-0.454,-0.527,-0.599,-0.671,-0.744,-0.816,-0.888,-0.961,-1.033,-1.106,-1.178,-1.250,-1.323,-1.394,-1.462,-1.523,-1.556,-1.595,-1.632,-1.664,-1.690,-1.709,-1.723,-1.729,-1.730,-1.723,-1.710,-1.691,-1.665,-1.632,-1.593,-1.548,-1.495,-1.437,-1.372,-1.302,-1.226,-1.146,-1.064,-0.980,-0.895,-0.810,-0.724,-0.638,-0.552,-0.469,-0.390,-0.318,-0.254,-0.199,-0.151,-0.110,-0.076,-0.048,-0.027,-0.012,-0.004,-0.001,-0.001,-0.000,-0.000,-0.000]
j2=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.140,0.185,0.237,0.296,0.361,0.431,0.507,0.587,0.670,0.754,0.838,0.924,1.009,1.092,1.171,1.243,1.308,1.365,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.564,1.557,1.544,1.529,1.513,1.497,1.481,1.465,1.449,1.432,1.416,1.400,1.384,1.367,1.351,1.335,1.319,1.303,1.286,1.270,1.254,1.238,1.221,1.205,1.189,1.173,1.157,1.140,1.124,1.108,1.092,1.075,1.059,1.043,1.027,1.010,0.994,0.978,0.961,0.945,0.929,0.913,0.896,0.880,0.864,0.848,0.831,0.815,0.799,0.786,0.769,0.749,0.730,0.710,0.689,0.669,0.649,0.629,0.609,0.589,0.569,0.548,0.528,0.508,0.488,0.468,0.448,0.427,0.407,0.387,0.367,0.347,0.327,0.306,0.286,0.266,0.246,0.226,0.206,0.186,0.166,0.146,0.125,0.105,0.084,0.064,0.044,0.025,0.012,0.004,0.001,0.000,0.000,0.000,0.000]
j3=[0.000,0.000,-0.002,-0.009,-0.022,-0.042,-0.068,-0.100,-0.139,-0.185,-0.237,-0.296,-0.361,-0.433,-0.511,-0.595,-0.681,-0.767,-0.854,-0.942,-1.027,-1.107,-1.182,-1.249,-1.311,-1.365,-1.414,-1.455,-1.491,-1.519,-1.541,-1.557,-1.566,-1.564,-1.556,-1.543,-1.524,-1.498,-1.465,-1.426,-1.381,-1.328,-1.270,-1.205,-1.133,-1.055,-0.971,-0.885,-0.798,-0.711,-0.624,-0.537,-0.450,-0.362,-0.275,-0.188,-0.101,-0.013,0.074,0.161,0.249,0.336,0.423,0.510,0.598,0.685,0.772,0.859,0.947,1.032,1.112,1.186,1.253,1.314,1.369,1.416,1.458,1.492,1.521,1.542,1.557,1.566,1.564,1.557,1.544,1.524,1.498,1.466,1.427,1.383,1.338,1.293,1.247,1.201,1.155,1.110,1.064,1.018,0.972,0.926,0.881,0.835,0.789,0.743,0.697,0.652,0.606,0.560,0.514,0.468,0.423,0.377,0.331,0.285,0.239,0.194,0.149,0.109,0.076,0.048,0.027,0.012,0.004,0.002,0.001,0.000,0.000,0.000]
j4=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.027,1.107,1.181,1.249,1.310,1.365,1.413,1.455,1.490,1.519,1.541,1.556,1.565,1.567,1.574,1.587,1.603,1.619,1.636,1.653,1.670,1.686,1.703,1.720,1.737,1.754,1.771,1.788,1.805,1.822,1.839,1.856,1.873,1.890,1.907,1.923,1.940,1.957,1.974,1.991,2.008,2.025,2.042,2.059,2.076,2.093,2.110,2.127,2.144,2.161,2.178,2.195,2.212,2.229,2.246,2.263,2.280,2.297,2.314,2.331,2.344,2.352,2.350,2.343,2.330,2.310,2.284,2.252,2.212,2.167,2.115,2.056,1.991,1.919,1.841,1.760,1.679,1.597,1.515,1.433,1.350,1.268,1.186,1.104,1.022,0.940,0.858,0.776,0.694,0.612,0.530,0.452,0.379,0.312,0.252,0.198,0.151,0.110,0.076,0.048,0.027,0.012,0.004,0.002,0.001,0.000,0.000,0.000]
j5=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.028,1.108,1.182,1.250,1.311,1.366,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.571,1.571,1.568,1.561,1.548,1.529,1.503,1.470,1.431,1.388,1.342,1.297,1.251,1.205,1.159,1.113,1.067,1.021,0.975,0.929,0.883,0.837,0.791,0.745,0.699,0.653,0.607,0.561,0.516,0.470,0.424,0.378,0.332,0.286,0.240,0.194,0.149,0.109,0.076,0.048,0.027,0.012,0.004,0.002,0.001,0.000,0.000,0.000]
j6=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.027,1.108,1.182,1.249,1.311,1.365,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.571,1.571,1.571,1.569,1.561,1.548,1.529,1.503,1.470,1.431,1.388,1.343,1.297,1.251,1.205,1.159,1.113,1.067,1.021,0.975,0.929,0.883,0.837,0.791,0.745,0.699,0.653,0.607,0.561,0.515,0.470,0.424,0.378,0.332,0.286,0.240,0.194,0.149,0.109,0.076,0.048,0.027,0.012,0.004,0.002,0.001,0.000,0.000,0.000]
# Start simulation:
sim.simxStartSimulation(clientID,sim.simx_opmode_blocking)
# Wait until ready:
waitForMovementExecuted('ready')
# Send the movement sequence:
targetConfig=[90*math.pi/180,90*math.pi/180,-90*math.pi/180,90*math.pi/180,90*math.pi/180,90*math.pi/180]
movementData={"id":"movSeq1","type":"pts","times":times,"j1":j1,"j2":j2,"j3":j3,"j4":j4,"j5":j5,"j6":j6}
packedMovementData=msgpack.packb(movementData)
sim.simxCallScriptFunction(clientID,targetArm,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot)
# Execute movement sequence:
sim.simxCallScriptFunction(clientID,targetArm,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq1',sim.simx_opmode_oneshot)
# Wait until above movement sequence finished executing:
waitForMovementExecuted('movSeq1')
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):
t = time.time() - t0
for j in range(3):
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)
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 for Edge Robot "
Phix1 = u2[0][0] # 1x1
Phiy1 = u2[1][0] # 1x1
Mx1 = (self.x2 - self.x1) # 1x1
My1 = (self.y2 - self.y1) # 1x1
" Data Transformation into M_x, M_y, Phi_x, Phi_y for Middle Robot "
Phix2 = (u1[0][0] + u3[0][0]) / 2 # 1x1
Phiy2 = (u1[1][0] + u3[1][0]) / 2 # 1x1
Mx2 = ((self.x1 - self.x2) + (self.x3 - self.x2)) / 2 # 1x1
My2 = ((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('edge_robot.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.i1 == 0: # write header value once
thewriter.writeheader()
self.i1 = 1
if self.j1 != 0:
thewriter.writerow({
'M_x': Mx1,
'M_y': My1,
'Phi_x': Phix1,
'Phi_y': Phiy1,
'U_x': u1[0][0],
'U_y': u1[1][0]
})
if self.j1 == 0: # skip first value because it's noisy
self.j1 = 1
with open('middle_robot.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': Mx2,
'M_y': My2,
'Phi_x': Phix2,
'Phi_y': Phiy2,
'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 += 0.5 # 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
# Take handles of the other robotos
ErrLocM3, LocM3 = sim.simxGetObjectHandle(
clientID, 'robot3#1', sim.simx_opmode_oneshot_wait)
if (not ErrLocM3 == sim.simx_return_ok):
pass
ErrLocM4, LocM4 = sim.simxGetObjectHandle(
clientID, 'robot4#2', sim.simx_opmode_oneshot_wait)
if (not ErrLocM4 == sim.simx_return_ok):
pass
ErrLocM5, LocM5 = sim.simxGetObjectHandle(
clientID, 'robot5#3', sim.simx_opmode_oneshot_wait)
if (not ErrLocM5 == sim.simx_return_ok):
pass
ErrLocM6, LocM6 = sim.simxGetObjectHandle(
clientID, 'robot6#4', sim.simx_opmode_oneshot_wait)
if (not ErrLocM6 == 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]
# Check if robot1 is close to any other robot (except robot2)
#for i in range(1,4):
too_close_dist = 0.4
ErrLoc3, Loc3 = sim.simxGetObjectPosition(
clientID, LocM3, -1, sim.simx_opmode_oneshot_wait)
if (not ErrLocM3 == sim.simx_return_ok):
pass
ErrLoc4, Loc4 = sim.simxGetObjectPosition(
clientID, LocM4, -1, sim.simx_opmode_oneshot_wait)
if (not ErrLocM4 == sim.simx_return_ok):
pass
ErrLoc5, Loc5 = sim.simxGetObjectPosition(
clientID, LocM5, -1, sim.simx_opmode_oneshot_wait)
if (not ErrLocM5 == sim.simx_return_ok):
pass
ErrLoc6, Loc6 = sim.simxGetObjectPosition(
clientID, LocM6, -1, sim.simx_opmode_oneshot_wait)
if (not ErrLocM6 == sim.simx_return_ok):
pass
exit_cond = 0
print(" -------- ELEMENT 1 ------------")
print(Loc1)
print(" --------- ELEMENT 2 -----------")
print(Loc3)
while exit_cond == 0:
print(Loc1)
distr13 = np.sqrt(
pow(Loc1[0] - Loc3[0], 2) + pow(Loc1[1] - Loc3[1], 2))
distr14 = np.sqrt(
pow(Loc1[0] - Loc4[0], 2) + pow(Loc1[1] - Loc4[1], 2))
distr15 = np.sqrt(
pow(Loc1[0] - Loc5[0], 2) + pow(Loc1[1] - Loc5[1], 2))
distr16 = np.sqrt(
pow(Loc1[0] - Loc6[0], 2) + pow(Loc1[1] - Loc6[1], 2))
if (distr13 < too_close_dist) or (
distr14 < too_close_dist
) or (distr15 < too_close_dist) or (distr16 < too_close_dist):
# TOO close. Select another starting point for robot 1
Loc1 = np.random.uniform(-2, 2, size=(1, 2))[0]
print(" Robot 1 is close!")
else:
exit_cond = 1
# In Loc1 there is here a valid inizialization of robot1
scenes[self.scene][0] = Loc1[0]
scenes[self.scene][1] = Loc1[1]
# Check if the robot2 is too close to other robots
exit_cond = 0
while exit_cond == 0:
dist21 = np.sqrt(
pow(Loc2[0] - Loc1[0], 2) + pow(Loc2[1] - Loc1[1], 2))
dist23 = np.sqrt(
pow(Loc2[0] - Loc3[0], 2) + pow(Loc2[1] - Loc3[1], 2))
dist24 = np.sqrt(
pow(Loc2[0] - Loc4[0], 2) + pow(Loc2[1] - Loc4[1], 2))
dist25 = np.sqrt(
pow(Loc2[0] - Loc5[0], 2) + pow(Loc2[1] - Loc5[1], 2))
dist26 = np.sqrt(
pow(Loc2[0] - Loc6[0], 2) + pow(Loc2[1] - Loc6[1], 2))
if dist21 < too_close_dist or dist23 < too_close_dist or dist24 < too_close_dist or dist25 < too_close_dist or dist26 < too_close_dist:
# TOO close. Select another starting point for robot 2
Loc2 = np.random.uniform(-2, 2, size=(1, 2))[0]
print(" Robot 2 is close!")
else:
exit_cond = 1
# In Loc2 there is here a valid inizialization of robot2
scenes[self.scene][3] = Loc2[0]
scenes[self.scene][4] = Loc2[1]
# 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 main():
print('Program started')
sim.simxFinish(-1) # just in case, close all opened connections
clientID = sim.simxStart(COPPELIA_SIM_IP_ADDRESS, COPPELIA_SIM_PORT, True,
True, 2000, 5) # Connect to CoppeliaSim
# lower e upper são vetores pra detecção da cor vermelha, cada elemento do vetor é um dos canais RGB (invertido por causa do opencv).
# então o limite inferior pra detecção seria o valor 0 para a cor azul, 0 para a cor verde e 100 para a cor vermelha.
# a análise é análoga para o vetor upper.
lower = np.uint8([0, 0, 100])
upper = np.uint8([60, 60, 255])
# nós temos 8 targets, portanto para pegar as suas referências (handles), é preciso iterar sobre eles, daí vem o targetIterator.
targetIterator = 1
if (clientID != -1):
print('Connection Successful')
# pega as referências de todos os elementos a serem manipulados.
# perceba que só pega a referência do primeiro alvo porque só pegamos a referência de um alvo por vez.
err1, ddRobotHandle = sim.simxGetObjectHandle(
clientID, 'RobotFrame#', sim.simx_opmode_oneshot_wait)
err2, leftMotorHandle = sim.simxGetObjectHandle(
clientID, 'LeftMotor#', sim.simx_opmode_oneshot_wait)
err3, rightMotorHandle = sim.simxGetObjectHandle(
clientID, 'RightMotor#', sim.simx_opmode_oneshot_wait)
err4, revoluteJointHandle = sim.simxGetObjectHandle(
clientID, 'RevoluteJoint#', sim.simx_opmode_oneshot_wait)
err5, visionSensorHandle = sim.simxGetObjectHandle(
clientID, 'VisionSensor#', sim.simx_opmode_oneshot_wait)
err6, targetHandle = sim.simxGetObjectHandle(
clientID, 'Target' + str(targetIterator) + '#',
sim.simx_opmode_oneshot_wait)
targetIterator += 1
# pega a referência dos sensores ultrassônicos (usamos 7 sensores).
# atenção: sensores ultrassônicos != vision sensor
usensors = []
for i in range(1, 8):
err, usensor = sim.simxGetObjectHandle(
clientID, 'Proximity_sensor' + str(i) + '#',
sim.simx_opmode_oneshot_wait)
usensors.append(usensor)
# a leitura dos sensores não pode bloquear o loop de execução do robô (while mais abaixo), portanto o coppelia oferece o opmode streaming,
# isto é, quando você manda a leitura começar a ser feita com streaming, ele mantém-se fazendo a leitura e colocando os resultados em um buffer
# exclusivo para aquele handle (7 handles 7 buffers). Portanto só precisamos a cada iteração ler do buffer, e aí não é preciso pedir que o simulador
# faça uma nova leitura, economizando tempo e não bloqueando o loop de execução do robô.
for i in range(7):
x, a, b, c, d = sim.simxReadProximitySensor(
clientID, usensors[i], sim.simx_opmode_streaming)
# a mesma regra dos sensores ultrassônicos descrita acima se aplica ao vision sensor (e a qualquer sensor que poderia vir a ser usado).
a, b, image = sim.simxGetVisionSensorImage(clientID,
visionSensorHandle, 0,
sim.simx_opmode_streaming)
print(f'RobotFrame: {ddRobotHandle}')
print(f'LeftMotor: {leftMotorHandle}')
print(f'RightMotor: {rightMotorHandle}')
print(f'RevoluteJoint: {revoluteJointHandle}')
print(f'VisionSensor: {visionSensorHandle}')
ret = sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait)
if (ret == -1):
print('Connection Failed.')
exit(-1)
print('Simulation Started')
while (sim.simxGetConnectionId(clientID) != -1):
# posicao atual do robo
pos = getPosition(clientID, ddRobotHandle)
#print(f'posX: {pos[0]:.3f} posY: {pos[1]:.3f}', end=' ')
# target atual do robo
goal = getPosition(clientID, targetHandle)
#print(f'goalX: {goal[0]:.3f} goalY: {goal[1]:.3f}', end=' ')
# observe o simx_opmode_buffer. nós não pedimos uma nova leitura do vision sensor, nós simplesmente pegamos a leitura do buffer.
a, b, image = sim.simxGetVisionSensorImage(clientID,
visionSensorHandle, 0,
sim.simx_opmode_buffer)
# operações feitas para organizar a imagem do vision sensor.
newImage = np.uint8(image).reshape((b[0], b[0], 3))
# definitiveImage é autoexplicativa, mas atenção, pois ela se encontra com a codificação BGR.
definitiveImage = cv2.cvtColor(np.flip(newImage, 0),
cv2.COLOR_RGB2BGR)
# mask é simplesmente uma outra imagem com a mesma resolução de definitiveImage, porém só mostrando objetos vermelhos, isto é,
# valor 255 nos pixels correspondentes a pixels vermelhos na imagem original, e 0 em todos os outros pixels.
mask = cv2.inRange(definitiveImage, lower, upper)
# distancia do alvo para o robô.
dx = goal[0] - pos[0]
dy = goal[1] - pos[1]
euclid = math.sqrt(dx * dx + dy * dy)
# print(euclid)
# se o alvo está perto o suficiente faça:
if (euclid <= 0.23):
# se na mask só tem pixels diferentes de 0, portanto só pixels vermelhos faça:
if (0.0 not in mask):
# para o robô e manda a hélice girar no sentido anti-horário por aproximadamente três segundos.
phiL, phiR, phiH = 0, 0, 2
setTargetSpeed(clientID, phiL, phiR, phiH, leftMotorHandle,
rightMotorHandle, revoluteJointHandle)
time.sleep(3)
setTargetSpeed(clientID, phiL, phiR, 0, leftMotorHandle,
rightMotorHandle, revoluteJointHandle)
# se não, o alvo é azul.
else:
# então para o robô e manda a hélice girar no sentido horário por aproximadamente três segundos.
phiL, phiR, phiH = 0, 0, -2
setTargetSpeed(clientID, phiL, phiR, phiH, leftMotorHandle,
rightMotorHandle, revoluteJointHandle)
time.sleep(3)
setTargetSpeed(clientID, phiL, phiR, 0, leftMotorHandle,
rightMotorHandle, revoluteJointHandle)
#se ainda há alvos
if (targetIterator <= 8):
# pega a referência do próximo alvo.
err6, targetHandle = sim.simxGetObjectHandle(
clientID, 'Target' + str(targetIterator) + '#',
sim.simx_opmode_oneshot_wait)
targetIterator += 1
# se não encerra o programa e pausa a simulação.
else:
break
# se o alvo ainda está longe, continua indo até ele com a hélice parada.
else:
phiL, phiR = movementControl(pos, goal, clientID, usensors)
phiH = 0
setTargetSpeed(clientID, phiL, phiR, phiH, leftMotorHandle,
rightMotorHandle, revoluteJointHandle)
# descomente estas duas linhas se você quiser salvar as imagens capturadas.
# imageItarator = 0
#cv2.imwrite('capture' + str(imageIterator) + '.png', definitiveImage)
#imageIterator += 1
setTargetSpeed(clientID, phiL, phiR, phiH, leftMotorHandle,
rightMotorHandle, revoluteJointHandle)
sim.simxPauseSimulation(clientID, sim.simx_opmode_oneshot_wait)
sim.simxFinish(clientID)
else:
print('Connection failed.')
exit(-2)
## faça uma nova leitura, economizando tempo e não bloqueando o loop de execução do robô.
for i in range(7):
x, a, b, c, d = sim.simxReadProximitySensor(clientID, usensors[i],
sim.simx_opmode_streaming)
## a mesma regra dos sensores ultrassônicos descrita acima se aplica ao vision sensor (e a qualquer sensor que poderia vir a ser usado).
a, b, image = sim.simxGetVisionSensorImage(clientID, visionSensorHandle, 0,
sim.simx_opmode_streaming)
print(f'RobotFrame: {ddRobotHandle}')
print(f'LeftMotor: {leftMotorHandle}')
print(f'RightMotor: {rightMotorHandle}')
print(f'RevoluteJoint: {revoluteJointHandle}')
print(f'VisionSensor: {visionSensorHandle}')
ret = sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait)
if (ret == -1):
print('Connection Failed.')
exit(-1)
print('Simulation Started')
while (sim.simxGetConnectionId(clientID) != -1):
pos = getPosition(clientID, ddRobotHandle)
#print(f'posX: {pos[0]:.3f} posY: {pos[1]:.3f}', end=' ')
goal = getPosition(clientID, targetHandle)
#print(f'goalX: {goal[0]:.3f} goalY: {goal[1]:.3f}', end=' ')
## observe o simx_opmode_buffer. nós não pedimos uma nova leitura do vision sensor, nós simplesmente pegamos a leitura do buffer.
a, b, image = sim.simxGetVisionSensorImage(clientID,
visionSensorHandle, 0,
sim.simx_opmode_buffer)
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
break
else:
time.sleep(0.2)
print("Failed connecting to remote API server!")
print("Connection success!")
RAD2DEG = 180 / math.pi # 常数，弧度转度数
tstep = 0.005 # 定义仿真步长
# 设置仿真步长，为了保持API端与V-rep端相同步长
vrep.simxSetFloatingParameter(clientID, vrep.sim_floatparam_simulation_time_step, tstep, vrep.simx_opmode_oneshot)
# 然后打开同步模式
vrep.simxSynchronous(clientID, True)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
baseName = 'Body'
jointName = ['Tran', 'RobotHip', 'RobotKnee']
print(len(jointName))
_, baseHandle = vrep.simxGetObjectHandle(clientID, baseName, vrep.simx_opmode_blocking)
jointHandle = [0,0,0]
for k in range(len(jointName)):
_, jointHandle[k] = vrep.simxGetObjectHandle(clientID, jointName[k], vrep.simx_opmode_blocking)
print('Handles available!')
import sim as vrep
import math
import random
import time
import math
def moving(x,y):
a=0.4
b=0.4
c=math.pow((math.pow(x,2)+math.pow(y,2)),0.5)
s=(a+b+c)/2
area=math.pow((s*(s-a)*(s-b)*(s-c)),0.5)
h=area/(2*c)
deg1_base=math.atan(x/y)
if x<0 and y<0 :
deg_base1=deg1_base+math.pi
deg1_tri=math.asin(h/a)
deg1=deg1_base+deg1_tri
deg2=math.pi-(0.5*math.pi-deg1_tri)-math.acos(h/b)
deg3=deg2-deg1
vrep.simxSetJointTargetPosition(clientID,joint01,deg1,opmode)
vrep.simxSetJointTargetPosition(clientID,joint02,-deg2,opmode)
vrep.simxSetJointTargetPosition(clientID,joint03,deg3,opmode)
print ('Start')
vrep.simxFinish(-1)
clientID = vrep.simxStart('192.168.0.10', 19997, True, True, 5000, 5)
if clientID != -1:
print ('Connected to remote API server')
def start(self):
s.simxStartSimulation(self._id, self._def_op_mode)
def start_sim(self): sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot_wait) return
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
if counter == 150:
saveData(ros_handle.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)