def init_fitness_streaming(self):
"""
subscribe to the fitness streams associated with this client
"""
for genome in self.genomes:
genome_ID = genome.GetID()
sig_name = 'FitnessScore_' + str(genome_ID)
sim.simxGetFloatSignal(self.clientID, sig_name, mode4)
def stop_fitness_streaming(self):
"""
unsubscribe from streaming fitness signals at the end of every generation:
otherwise streaming will continue, subscriptions pile up and block python
"""
for genome in self.genomes:
genome_ID = genome.GetID()
sig_name = 'FitnessScore_' + str(genome_ID)
sim.simxGetFloatSignal(self.clientID, sig_name, mode5)
def backward(self, speed, distance):
res, previousPosition = sim.simxGetFloatSignal(
self.simulation, "signal", sim.simx_opmode_streaming)
drivenDistance = 0
distance = previousPosition - distance
while drivenDistance > distance:
res, drivenDistance = sim.simxGetFloatSignal(
self.simulation, "signal", sim.simx_opmode_streaming)
self.setMotors(speed, speed)
self.setMotors(0, 0)
def forward(self, speed, distance):
res, previousPosition = sim.simxGetFloatSignal(
self.simulation, "signal", sim.simx_opmode_streaming)
drivenDistance = 0
distance = distance + previousPosition
while drivenDistance < distance:
res, drivenDistance = sim.simxGetFloatSignal(
self.simulation, "signal", sim.simx_opmode_streaming)
self.setMotors(-speed, -speed)
self.setMotors(0, 0)
def get_simulation_time(self):
# We get the error code and floating signal back from the server
if self.first_time_list[0]:
_, simulation_time = sim.simxGetFloatSignal(
self.client_id, 'simulation_time', sim.simx_opmode_streaming)
self.first_time_list[0] = False
else:
_, simulation_time = sim.simxGetFloatSignal(
self.client_id, 'simulation_time', sim.simx_opmode_buffer)
# We return the simulation time
return simulation_time
def get_conveyor_4_speed(self):
# We get the error code and floating signal back from the server
if self.first_time_list[7]:
_, conveyor_4_speed = sim.simxGetFloatSignal(
self.client_id, 'active_conveyorBeltVelocity_4',
sim.simx_opmode_streaming)
self.first_time_list[7] = False
else:
_, conveyor_4_speed = sim.simxGetFloatSignal(
self.client_id, 'active_conveyorBeltVelocity_4',
sim.simx_opmode_buffer)
# We return the simulation time
return conveyor_4_speed
def gyro_read():
error = 1
error2 = 1
error3 = 1
while (error != 0):
error, x = sim.simxGetFloatSignal(0, 'gyroX',
sim.simx_opmode_streaming)
while (error2 != 0):
error2, y = sim.simxGetFloatSignal(0, 'gyroY',
sim.simx_opmode_streaming)
while (error3 != 0):
error3, z = sim.simxGetFloatSignal(0, 'gyroZ',
sim.simx_opmode_streaming)
return [x, y, z]
def imu_read():
error = 1
error2 = 1
error3 = 1
while (error != 0):
error, x = sim.simxGetFloatSignal(0, 'accelerometerX',
sim.simx_opmode_streaming)
while (error2 != 0):
error2, y = sim.simxGetFloatSignal(0, 'accelerometerY',
sim.simx_opmode_streaming)
while (error3 != 0):
error3, z = sim.simxGetFloatSignal(0, 'accelerometerZ',
sim.simx_opmode_streaming)
return [x, y, z]
def doTracking(self):
self.resetSim()
self.prepareSim()
pathIsDone = sim.simxGetFloatSignal(self.clientID, 'movePathDone',
sim.simx_opmode_streaming)[1]
posX = []
posY = []
posZ = []
tactile1 = []
reward = []
while pathIsDone == 0:
X, Y, Z = self.getKinectXYZ(False)
# forces = self.getForce(False)
# diffForce = (forces[0][0] + ((forces[1][0] + forces[2][0]) / 2))/2
# print('[DATA] diff force: {:.4f}'.format(diffForce))
actionX = X # + 0.005*diffForce
actionY = Y
actionZ = Z
# print('[DATA] X:{:.4f}, Y:{:.4f}, Z:{:.4f}'.format(actionX,actionY,actionZ))
sim.simxSetFloatSignal(self.clientID, 'actionX', actionX,
sim.simx_opmode_oneshot)
sim.simxSetFloatSignal(self.clientID, 'actionY', actionY,
sim.simx_opmode_oneshot)
sim.simxSetFloatSignal(self.clientID, 'actionZ', actionZ,
sim.simx_opmode_oneshot)
sim.simxGetPingTime(self.clientID)
sim.simxSynchronousTrigger(self.clientID)
forces = self.getForceMagnitude(False)
posX.append(X)
posY.append(Y)
posZ.append(Z)
tactile1.append(forces[0])
reward.append(self._getReward())
sim.simxGetPingTime(self.clientID)
pathIsDone = sim.simxGetFloatSignal(self.clientID, 'movePathDone',
sim.simx_opmode_buffer)[1]
# time.sleep(0.5)
# print('[INFO] tracking is done.')
# print('[DATA] accumulated reward: {}'.format(np.sum(np.array(reward))))
sim.simxStopSimulation(self.clientID, sim.simx_opmode_blocking)
def gyro_read():
error = 1
error2 = 1
error3 = 1
while (error != 0):
error, x = sim.simxGetFloatSignal(0, 'gyroX',
sim.simx_opmode_streaming)
print("LINEAR Y IS ")
print(y)
while (error2 != 0):
error2, y = sim.simxGetFloatSignal(0, 'gyroY',
sim.simx_opmode_streaming)
print("LINEAR Y IS ")
print(y)
while (error3 != 0):
error3, z = sim.simxGetFloatSignal(0, 'gyroZ',
sim.simx_opmode_streaming)
print("LINEAR Y IS ")
print(y)
return x, y, z
def prepareSim(self):
self.initializeFunctions()
isTracking = sim.simxGetFloatSignal(self.clientID, 'isTracking',
sim.simx_opmode_streaming)[1]
while isTracking != 1:
sim.simxSynchronousTrigger(self.clientID)
isTracking = sim.simxGetFloatSignal(self.clientID, 'isTracking',
sim.simx_opmode_buffer)[1]
sim.simxGetPingTime(self.clientID)
X, Y, Z = self.getKinectXYZ(True)
actionX = X
actionY = Y
actionZ = Z
# print(actionX,actionY,actionZ)
# print('[DATA] X:{:.4f}, Y:{:.4f}, Z:{:.4f}'.format(actionX,actionY,actionZ))
sim.simxSetFloatSignal(self.clientID, 'actionX', actionX,
sim.simx_opmode_oneshot)
sim.simxSetFloatSignal(self.clientID, 'actionY', actionY,
sim.simx_opmode_oneshot)
sim.simxSetFloatSignal(self.clientID, 'actionZ', actionZ,
sim.simx_opmode_oneshot)
sim.simxGetPingTime(self.clientID)
def laiser_pointer(listVar):
#sim.simxGetFloatSignal(client.id, "laserDistance", sim.simx_opmode_streaming)
while 1:
time.sleep(1.5)
try:
err_code, distance = sim.simxGetFloatSignal(
client.id, "laserPointerData", sim.simx_opmode_streaming)
print(f'Distancia {distance}')
listVar['distance'] = distance
if err_code != 0:
raise Exception("teste")
except Exception:
print("Code error is {}".format(err_code))
def _sense_encoders(self) -> Tuple[float, float]:
"""Solve forward differential kinematics from encoder readings.
Returns:
z_v: Linear velocity of the robot center [m/s].
z_w: Angular velocity of the robot center [rad/s].
"""
_, th_l = sim.simxGetFloatSignal(self._client_id, 'leftEncoder',
sim.simx_opmode_buffer)
_, th_r = sim.simxGetFloatSignal(self._client_id, 'rightEncoder',
sim.simx_opmode_buffer)
wr = th_r / self._dt
wl = th_l / self._dt
z_v = ((wr + wl) * self.WHEEL_RADIUS) / 2.0
z_w = ((wr - wl) * self.WHEEL_RADIUS) / self.TRACK
return z_v, z_w
def update_fitness_scores(self, print_report):
"""
fetch fitness values from vrep and save to python genomes
"""
for genome in self.genomes:
genome_ID = genome.GetID()
sig_name = 'FitnessScore_' + str(genome_ID)
ret, fitness = sim.simxGetFloatSignal(self.clientID, sig_name,
mode4)
if print_report:
print("client", self.clientID, ": genome", genome_ID, "has",
sig_name, ": ", fitness, "success: ", ret)
if ret == 0:
genome.SetEvaluated()
genome.SetFitness(fitness)
def get_data_from_simulation(id):
"""Connects to the simulation and gets a float signal value
Parameters
----------
id : str
The signal id in CoppeliaSim. Possible values are 'accelX', 'accelY' and 'accelZ'.
Returns
-------
data : float
The float value retrieved from the simulation. None if retrieval fails.
"""
if clientID != -1:
res, data = sim.simxGetFloatSignal(clientID, id,
sim.simx_opmode_blocking)
if res == sim.simx_return_ok:
return data
return None
def imu_read_firsttime():
error, x = sim.simxGetFloatSignal(0, 'accelerometerX',
sim.simx_opmode_buffer)
while (error != sim.simx_return_ok):
error, x = sim.simxGetFloatSignal(0, 'accelerometerX',
sim.simx_opmode_streaming)
error2, y = sim.simxGetFloatSignal(0, 'accelerometerY',
sim.simx_opmode_buffer)
while (error2 != sim.simx_return_ok):
error2, y = sim.simxGetFloatSignal(0, 'accelerometerY',
sim.simx_opmode_streaming)
error3, z = sim.simxGetFloatSignal(0, 'accelerometerZ',
sim.simx_opmode_buffer)
while (error3 != sim.simx_return_ok):
error3, z = sim.simxGetFloatSignal(0, 'accelerometerZ',
sim.simx_opmode_streaming)
def getKinectXYZ(self, initialize=True):
if initialize:
X = sim.simxGetFloatSignal(self.clientID, 'actX',
sim.simx_opmode_streaming)[1]
Y = sim.simxGetFloatSignal(self.clientID, 'actY',
sim.simx_opmode_streaming)[1]
Z = sim.simxGetFloatSignal(self.clientID, 'actZ',
sim.simx_opmode_streaming)[1]
else:
X = sim.simxGetFloatSignal(self.clientID, 'actX',
sim.simx_opmode_buffer)[1]
Y = sim.simxGetFloatSignal(self.clientID, 'actY',
sim.simx_opmode_buffer)[1]
Z = sim.simxGetFloatSignal(self.clientID, 'actZ',
sim.simx_opmode_buffer)[1]
sim.simxGetPingTime(self.clientID)
return X, Y, Z
def gyro_read_firsttime():
error, x = sim.simxGetFloatSignal(0, 'gyroX', sim.simx_opmode_buffer)
while (error != sim.simx_return_ok):
error, x = sim.simxGetFloatSignal(0, 'gyroX',
sim.simx_opmode_streaming)
print("Angular X IS ")
print(x)
error2, y = sim.simxGetFloatSignal(0, 'gyroY', sim.simx_opmode_buffer)
while (error2 != sim.simx_return_ok):
error2, y = sim.simxGetFloatSignal(0, 'gyroY',
sim.simx_opmode_streaming)
print("Angular Y IS ")
print(y)
error3, z = sim.simxGetFloatSignal(0, 'gyroZ', sim.simx_opmode_buffer)
while (error3 != sim.simx_return_ok):
error3, z = sim.simxGetFloatSignal(0, 'gyroZ',
sim.simx_opmode_streaming)
print("Angular Z IS ")
print(z)
print('Connected to remote API server')
# Now try to retrieve data in a blocking fashion (i.e. a service call):
# res,objs=sim.simxGetObjects(clientID,sim.sim_handle_all,sim.simx_opmode_blocking)
# if res==sim.simx_return_ok:
# print ('Number of objects in the scene: ',len(objs))
# else:
# print ('Remote API function call returned with error code: ',res)
time = 0
# retrieve motor handles
errorCode, left_motor_handle = sim.simxGetObjectHandle(
clientID, 'wheel_left_joint', sim.simx_opmode_blocking)
errorCode, right_motor_handle = sim.simxGetObjectHandle(
clientID, 'wheel_right_joint', sim.simx_opmode_blocking)
r, signalValue = sim.simxGetFloatSignal(clientID,
'Turtlebot2_simulation_time',
sim.simx_opmode_streaming)
video = [[5, 5], [10, 10], [10, 5], [5, 10], [10, 5], [10, 10], [10, 5],
[5, 10], [10, 0], [5, 10], [10, 5], [0, 10], [5, 10], [10, 5],
[5, 10], [10, 5], [5, 10], [5, 0], [0, 5], [5, 0], [0,
10], [10, 5],
[0, 10], [10, 0], [5, 0], [0, 5], [10, 5], [5, 10], [10, 10],
[10, 10], [10, 10], [10, 10], [10, 10], [10, 5], [0, 10]]
rev1 = video[::-1]
for k in rev1:
time = 0
err_code1 = 1
err_code2 = 2
# print(type(k[0]))
while (err_code1 != 0 and err_code2 != 0):
def color_sort():
color_list = []
result, cs_bookline = sim.simxGetFloatSignal(clientID, "cs_bookline",
sim.simx_opmode_streaming)
result, book = sim.simxGetFloatSignal(clientID, "book",
sim.simx_opmode_streaming)
#move to intial position
moveJoints(cs_robot, position0)
#set the robot as busy
sim.simxSetFloatSignal(clientID, 'cs_robotBusy', 1,
sim.simx_opmode_oneshot)
while (cs_bookline == 0):
#wait for book to be detected
while (book == 0):
result, book = sim.simxGetFloatSignal(clientID, "book",
sim.simx_opmode_buffer)
#move book to cs table
moveJoints(cs_robot, cs_left_conveyor)
sim.simxSetFloatSignal(clientID, 'cs_succ', 1,
sim.simx_opmode_oneshot) # pick up the book
time.sleep(0.5)
moveJoints(cs_robot, cs_table)
sim.simxSetFloatSignal(clientID, 'cs_succ', 0,
sim.simx_opmode_oneshot) # drop the book off
#check book color
result, res, image = sim.simxGetVisionSensorImage(
clientID, cs_vis, 0, sim.simx_opmode_blocking)
book_color = [image[0], image[1], image[2]] #RGB color value
book_order = len(color_list) #assumes color is new initially
rng = 10 #range to determine wheter to colors are the same or not
#loop that check RGB values of every color list with book_color to determine is the same as a color in color_list
for color in enumerate(color_list):
if (abs(color[1][0] - book_color[0]) <
rng) and (abs(color[1][1] - book_color[1]) < rng) and (
abs(color[1][2] - book_color[2]) < rng):
book_order = color[0]
#if color is new append it to the color_list
if (book_order == len(color_list)):
color_list.append(book_color)
#set the book position depending on its order
book_position = invk(cs_robot, -1.2952, -0.68 + 0.154 * (book_order),
0.25)
#move and place the book in the proper position
sim.simxSetFloatSignal(clientID, 'cs_succ', 1,
sim.simx_opmode_oneshot) # pick up the book\
time.sleep(0.5)
moveJoints(cs_robot, book_position)
time.sleep(1.1)
sim.simxSetFloatSignal(clientID, 'cs_succ', 0,
sim.simx_opmode_oneshot) # drop the book off
result, cs_bookline = sim.simxGetFloatSignal(clientID, "cs_bookline",
sim.simx_opmode_buffer)
time.sleep(1)
#set the robot as free
sim.simxSetFloatSignal(clientID, 'cs_robotBusy', 0,
sim.simx_opmode_oneshot)
moveJoints(cs_robot, position0)
return color_list
def _is_done(self):
pathIsDone = sim.simxGetFloatSignal(self.clientID, 'movePathDone',
sim.simx_opmode_blocking)[1]
sim.simxGetPingTime(self.clientID)
return True if pathIsDone == 1 else False
def size_sort():
result, stack = sim.simxGetFloatSignal(clientID, "stack",
sim.simx_opmode_streaming)
result, height = sim.simxGetFloatSignal(clientID, "height",
sim.simx_opmode_streaming)
result, size = sim.simxGetFloatSignal(clientID, "size",
sim.simx_opmode_streaming)
size_list = []
#wait for stack to be detected
while (stack == 0):
result, stack = sim.simxGetFloatSignal(clientID, "stack",
sim.simx_opmode_buffer)
#set the robot as busy
sim.simxSetFloatSignal(clientID, 'ss_robotBusy', 1,
sim.simx_opmode_oneshot)
while (stack > 0):
#get top book height
result, height = sim.simxGetFloatSignal(clientID, "height",
sim.simx_opmode_buffer)
#set book position depending on stack length
book_position = invk(ss_robot, -1.2952, 0.6111, height)
#move book to ss table
moveJoints(ss_robot, book_position)
sim.simxSetFloatSignal(clientID, 'ss_succ', 1,
sim.simx_opmode_oneshot) # pick up the book
time.sleep(1)
moveJoints(ss_robot, ss_table)
sim.simxSetFloatSignal(clientID, 'ss_succ', 0,
sim.simx_opmode_oneshot) # drop the book off
time.sleep(2)
#get book size
result, size = sim.simxGetFloatSignal(clientID, "size",
sim.simx_opmode_buffer)
size_list.append([len(size_list), size, height])
#separate stack
sim.simxSetFloatSignal(clientID, 'ss_succ', 1,
sim.simx_opmode_oneshot) # pick up the book
book_position = invk(ss_robot, -0.425,
0.6025 - (len(size_list) - 1) * 0.16, 0.17)
moveJoints(ss_robot, book_position) #move to appropriate position
sim.simxSetFloatSignal(clientID, 'ss_succ', 0,
sim.simx_opmode_oneshot) # drop the book off
time.sleep(1.1)
result, stack = sim.simxGetFloatSignal(clientID, "stack",
sim.simx_opmode_buffer)
#loop to get book thickness
for i in range(len(size_list) - 1):
size_list[i][2] = size_list[i][2] - size_list[i + 1][2]
size_list[len(size_list) - 1][2] = size_list[len(size_list) - 1][2] - 0.15
#sort the size list
size_list = sorted(size_list, key=lambda size: size[1], reverse=True)
#largest books
largest_book = size_list[0]
#remove largest book from size list
size_list.pop(0)
#intial stack height
height = largest_book[2] + 0.15
#stack the books depending on size
for i in range(len(size_list)):
intial_position = invk(ss_robot, -0.425,
0.6025 - (size_list[i][0]) * 0.15, 0.14)
moveJoints(ss_robot, intial_position)
sim.simxSetFloatSignal(clientID, 'ss_succ', 1,
sim.simx_opmode_oneshot) # pick up the book
time.sleep(1)
#define a mid position to stop the robot from knocking over other robots
mid_position = getJointAngles(ss_robot)
mid_position[1] = 0
moveJoints(ss_robot, mid_position)
#finding height so the robot knows where to put the robot
height = height + size_list[i][2]
#get the largest book's position
target_position = invk(ss_robot, -0.425,
0.6025 - (largest_book[0]) * 0.15, height)
#move to the smaller book on top of the bigger book
moveJoints(ss_robot, target_position)
time.sleep(1.5)
sim.simxSetFloatSignal(clientID, 'ss_succ', 0,
sim.simx_opmode_oneshot) # drop the book off
time.sleep(1)
time.sleep(1)
#set the robot as free
sim.simxSetFloatSignal(clientID, 'ss_robotBusy', 0,
sim.simx_opmode_oneshot)
time.sleep(3) #wait for book stack to reach the end
return size_list
def tiempoSimulador(self):
_, tiempo = sim.simxGetFloatSignal(self.clientID, "tiemposimulacion",
sim.simx_opmode_streaming)
return tiempo
def _init_encoders(self):
"""Initialize encoder streaming."""
sim.simxGetFloatSignal(self._client_id, 'leftEncoder',
sim.simx_opmode_streaming)
sim.simxGetFloatSignal(self._client_id, 'rightEncoder',
sim.simx_opmode_streaming)
rf_z = -0.482
lf_z = -0.482
rb_z = -0.482
if gait_state == 1: #trot步态
lb_x = -0.1
rb_x = 0.1
lf_x = 0.1
rf_x = -0.1
lb_z = -0.482
rf_z = -0.482
lf_z = -0.482
rb_z = -0.482
[rec, vrep_time
] = sim.simxGetFloatSignal(clientID, 'time',
sim.simx_opmode_oneshot) #获取仿真时间
else:
[rec, vrep_realtime
] = sim.simxGetFloatSignal(clientID, 'time',
sim.simx_opmode_oneshot) #获取仿真时间
if gait_state == 2: #慢步步态
T = 1
time1 = vrep_realtime - vrep_time
time2 = vrep_realtime - vrep_time + 0.25
time3 = vrep_realtime - vrep_time + 0.5
time4 = vrep_realtime - vrep_time + 0.75
T1 = time1 % T
T2 = time2 % T #mod 函数为取模运算
T3 = time3 % T
T4 = time4 % T
[lb_x, lb_z] = gait_plan2(T1, T)
