def publish(self):
"""
send msg to vrep
msg : the frequency of leg
"""
vrep.simxSetFloatSignal(self.clientID, self.LSignalName, self.LCycleFreq, vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(self.clientID, self.RSignalName, self.RCycleFreq, vrep.simx_opmode_oneshot)
def ur5moveto(self, x, y, z):
"""
Move ur5 to location (x,y,z)
"""
vrep.simxSynchronousTrigger(self.clientID) # 让仿真走一步
self.targetQuaternion[0] = 0.707
self.targetQuaternion[1] = 0
self.targetQuaternion[2] = 0.707
self.targetQuaternion[3] = 0 #四元数
self.targetPosition[0] = x
self.targetPosition[1] = y
self.targetPosition[2] = z
vrep.simxPauseCommunication(self.clientID, True) #开启仿真
vrep.simxSetIntegerSignal(self.clientID, 'ICECUBE_0', 21,
vrep.simx_opmode_oneshot)
for i in range(3):
vrep.simxSetFloatSignal(self.clientID, 'ICECUBE_' + str(i + 1),
self.targetPosition[i],
vrep.simx_opmode_oneshot)
for i in range(4):
vrep.simxSetFloatSignal(self.clientID, 'ICECUBE_' + str(i + 4),
self.targetQuaternion[i],
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(self.clientID, False)
vrep.simxSynchronousTrigger(self.clientID) # 进行下一步
vrep.simxGetPingTime(self.clientID) # 使得该仿真步走完
def setParams(self, params):
for key, value in params.items():
vrep.simxSetFloatSignal(self.clientID, key, value,
vrep.simx_opmode_oneshot)
vrep.simxSetIntegerSignal(self.clientID, 'clear', 1,
vrep.simx_opmode_oneshot)
vrep.simxSetIntegerSignal(self.clientID, 'stop', 1,
vrep.simx_opmode_oneshot)
def init_rotors(clientID):
# Clear all signals
for i in range(len(propellers)):
vrep.simxClearFloatSignal(clientID, propellers[i], vrep.simx_opmode_oneshot)
# Set all propellers to zero
for i in range(len(propellers)):
vrep.simxSetFloatSignal(clientID, propellers[i], 1e-8, vrep.simx_opmode_oneshot)
def _write_actuators(self):
"""
Write in the robot the actuators values. Don't use directly,
instead use 'step()'
"""
# We make a copy of the actuators list
actuators = self._actuators_to_write[:]
for m in actuators:
if m[0] == 'L':
# Leds
# Sent as packed string to ensure update of the wanted LED
test = vrep.simxPackInts([m[1] + 1, m[2]])
vrep.simxSetStringSignal(self._clientID, 'EPUCK_LED', test,
vrep.simx_opmode_oneshot_wait)
elif m[0] == 'D':
# Set motor speed
# maxVel = 120.0 * math.pi / 180.0
# maxVel of ePuck firmware is 1000
# => 120 * pi / (180*1000) = pi/1500
velLeft = m[1] * math.pi / 1500.0
velRight = m[2] * math.pi / 1500.0
if velLeft > 120.0 * math.pi / 180.0:
velLeft = 120.0 * math.pi / 180.0
self._debug("velLeft too high, thresholded")
if velLeft < -120.0 * math.pi / 180.0:
velLeft = -120.0 * math.pi / 180.0
self._debug("velLeft too high, thresholded")
if velRight > 120.0 * math.pi / 180.0:
velRight = 120.0 * math.pi / 180.0
self._debug("velRight too high, thresholded")
if velRight < -120.0 * math.pi / 180.0:
velRight = -120.0 * math.pi / 180.0
self._debug("velRRight too high, thresholded")
vrep.simxSetFloatSignal(self._clientID, 'EPUCK_velLeft',
velLeft, vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(self._clientID, 'EPUCK_velRight',
velRight, vrep.simx_opmode_oneshot)
elif m[0] == 'P':
# Motor position
self._debug('WARNING: Motor position not yet implemented!')
else:
self._debug('Unknown actuator to write')
self._actuators_to_write.remove(m)
return
def conduct_action(self, a):
self.action = a / 1000.0
if vrep.simxGetConnectionId(self.client_id) == -1:
self.connection()
#print("action2", a)
lasttime = time.time()
while True:
for i in range(3):
vrep.simxSetFloatSignal(self.client_id, "myTestValue" + str(i),
self.action[i],
vrep.simx_opmode_oneshot)
currtime = time.time()
if currtime - lasttime > 0.1:
break
def connection(self):
#关闭潜在的连接
vrep.simxFinish(-1)
#每隔0.2秒检测一次，直到连接上V-rep
while True:
self.client_id = vrep.simxStart(self.client_ip, 19999, True, True,
5000, 5)
if self.client_id > -1:
vrep.simxSetFloatSignal(self.client_id, "ConnectFlag", 19999,
vrep.simx_opmode_oneshot)
break
else:
time.sleep(0.2)
print('Failed connecting to remote API server')
print('Connection success!')
def controller(self):
self.LCycleFreq = self.BaseFreq
self.RCycleFreq = self.BaseFreq
error = vrep.simxSetFloatSignal(self.clientID, self.LSignalName,
self.LCycleFreq,
vrep.simx_opmode_oneshot)
error = error or vrep.simxSetFloatSignal(
self.clientID, self.RSignalName, self.RCycleFreq,
vrep.simx_opmode_oneshot)
if error != 0:
print("Function error: ", error)
time = vrep.simxGetLastCmdTime(clientID)
self.telemetryData['cycleFrequencyTime'] = time
self.telemetryData['leftCycleFrequency'] = self.LCycleFreq
self.telemetryData['rightCycleFrequency'] = self.RCycleFreq
def setJointVelocity(self,
jointIndex,
vel,
isHandJoint=False,
useSignalToJoint=0):
if (isHandJoint):
jointSignalPrefixes = GB_CROBOT_HAND_JOINT_SIGNAL_NAME_PREFIXES
else:
jointSignalPrefixes = GB_CROBOT_JOINT_SIGNAL_NAME_PREFIXES
# http://www.coppeliarobotics.com/helpFiles/en/jointDescription.htm
# Signal to the joint control callback script
if (useSignalToJoint):
jointSignalName = jointSignalPrefixes[jointIndex] + "TargetVel"
res = vrep.simxSetFloatSignal(
self._clientID, jointSignalName, vel,
vrep.simx_opmode_oneshot_wait
) # set the signal value: !!simx_opmode_oneshot_wait
else:
res = vrep.simxSetJointTargetVelocity(
self._clientID,
self._jointHandles[jointIndex],
vel, # target velocity
vrep.simx_opmode_blocking)
if (res != 0):
print('Set Joint Velocity Error', res)
return res
def takeAction(self,doubleAction):
"""
state = (basePos[0], basePos[1], basePos[2],
linVel[3], linVel[4], linVel[5],
angVel[6], angVel[7], angVel[8],
baseYaw[9], baseRoll[10], basePitch[11])
"""
# self.prevState = self.state
#print("oldState: "+str(self.state))
self.num_sim_steps += 1
action = np.asarray(doubleAction,dtype=np.float32)
#print("takeAction: "+str(doubleAction))
# Get altitude directly from position Z
altiMeters = self.state[2]
# Get motor thrusts from FMU model
thrusts = self.fmu.getMotors((self.state[11],self.state[10],self.state[9]),altiMeters,
action, self.stepSeconds)
# print("thrusts: " + str(thrusts[0]) + " " + str(thrusts[1]) + " " + \
# str(thrusts[2]) + " " + str(thrusts[3]))
#vrep.simxPauseCommunication(self.clientID,True)
for t in range(4):
errorFlag = vrep.simxSetFloatSignal(self.clientID,'thrusts'+str(t+1),
thrusts[t], vrep.simx_opmode_oneshot)
#vrep.simxPauseCommunication(self.clientID,False)
self.proceed_simulation()
def main():
print ('Program started')
emptybuf = bytearray()
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 2000, 5)
if clientID!=-1:
print ('Connected to remote API server')
# Start the simulation:
vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait)
# start init wheels --------------------------------------------------------------------------------------------
wheelJoints = np.empty(4, dtype=np.int)
wheelJoints.fill(-1) # front left, rear left, rear right, front right
res, wheelJoints[0] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_fl', vrep.simx_opmode_oneshot_wait)
res, wheelJoints[1] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_rl', vrep.simx_opmode_oneshot_wait)
res, wheelJoints[2] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_fr', vrep.simx_opmode_oneshot_wait)
res, wheelJoints[3] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_rr', vrep.simx_opmode_oneshot_wait)
# end init wheels ----------------------------------------------------------------------------------------------
# start init camera --------------------------------------------------------------------------------------------
# change the angle of the camera view (default is pi/4)
res = vrep.simxSetFloatSignal(clientID, 'rgbd_sensor_scan_angle', math.pi / 2, vrep.simx_opmode_oneshot_wait)
# turn on camera
res = vrep.simxSetIntegerSignal(clientID, 'handle_rgb_sensor', 2, vrep.simx_opmode_oneshot_wait);
# get camera object-handle
res, youBotCam = vrep.simxGetObjectHandle(clientID, 'rgbSensor', vrep.simx_opmode_oneshot_wait)
# get first image
err, resolution, image = vrep.simxGetVisionSensorImage(clientID, youBotCam, 0, vrep.simx_opmode_streaming)
time.sleep(1)
# end init camera ----------------------------------------------------------------------------------------------
# programmable space -------------------------------------------------------------------------------------------
colorDet.exercise4_action(clientID, youBotCam)
# end of programmable space --------------------------------------------------------------------------------------------
# Stop simulation
vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
# Close connection to V-REP:
vrep.simxFinish(clientID)
else:
print ('Failed connecting to remote API server')
print ('Program ended')
def setJointForce(self,
jointIndex,
force,
isHandJoint=False,
useSignalToJoint=0):
if (isHandJoint):
jointSignalPrefixes = GB_CROBOT_HAND_JOINT_SIGNAL_NAME_PREFIXES
else:
jointSignalPrefixes = GB_CROBOT_JOINT_SIGNAL_NAME_PREFIXES
# Signal to the joint control callback script
jointSignalName = jointSignalPrefixes[jointIndex] + "Force"
if (useSignalToJoint):
res = vrep.simxSetFloatSignal(
self._clientID, jointSignalName, force,
vrep.simx_opmode_oneshot_wait
) # set the signal value: !!simx_opmode_oneshot_wait
else:
res = vrep.simxSetJointForce(
self._clientID,
self._jointHandles[jointIndex],
force, # force to apply
vrep.simx_opmode_blocking)
if (res != 0):
print('Set Joint Force Error', res)
return res
def takeAction(self, doubleAction):
"""
state = (basePos[0], basePos[1], basePos[2],
linVel[3], linVel[4], linVel[5],
angVel[6], angVel[7], angVel[8],
baseYaw[9], baseRoll[10], basePitch[11])
"""
# self.prevState = self.state
#print("oldState: "+str(self.state))
self.num_sim_steps += 1
action = np.asarray(doubleAction, dtype=np.float32)
#print("takeAction: "+str(doubleAction))
# Get altitude directly from position Z
altiMeters = self.state[2]
# Get motor thrusts from FMU model
thrusts = self.fmu.getMotors(
(self.state[11], self.state[10], self.state[9]), altiMeters,
action, self.stepSeconds)
# print("thrusts: " + str(thrusts[0]) + " " + str(thrusts[1]) + " " + \
# str(thrusts[2]) + " " + str(thrusts[3]))
#vrep.simxPauseCommunication(self.clientID,True)
for t in range(4):
errorFlag = vrep.simxSetFloatSignal(self.clientID,
'thrusts' + str(t + 1),
thrusts[t],
vrep.simx_opmode_oneshot)
#vrep.simxPauseCommunication(self.clientID,False)
self.proceed_simulation()
def set_parameter(self, sensor_name, parameter_name, parameter_value):
"""Set sensor parameters. Sensor name can be fastHokuyo_sensor1, kinect_depth, kinect_rgb.
To get list of possible params call youbot.parameters_list"""
if parameter_name == 'perspective_angle':
parameter_value = parameter_value / (180 * 2) * math.pi
if parameter_name in self.params_f:
error = vrep.simxSetObjectFloatParameter(
self.client_id,
self.handles[sensor_name + self.postfix],
self.params_f[parameter_name],
parameter_value,
ONE_SHOT_MODE
)
vrep.simxSetFloatSignal(
self.client_id,
'change_params',
parameter_value,
ONE_SHOT_MODE
)
vrep.simxClearFloatSignal(
self.client_id,
'change_params',
ONE_SHOT_MODE
)
return error
elif parameter_name in self.params_i:
error = vrep.simxSetObjectFloatParameter(
self.client_id,
self.handles[sensor_name + self.postfix],
self.params_i[parameter_name],
parameter_value,
ONE_SHOT_MODE
)
vrep.simxSetFloatSignal(
self.client_id,
'change_params',
parameter_value,
ONE_SHOT_MODE
)
vrep.simxClearFloatSignal(
self.client_id,
'change_params',
ONE_SHOT_MODE
)
return error
else:
return 'Parameter not found'
def _write_actuators(self):
"""
Write in the robot the actuators values. Don't use directly,
instead use 'step()'
"""
# We make a copy of the actuators list
actuators = self._actuators_to_write[:]
for m in actuators:
if m[0] == 'L':
# Leds
# Sent as packed string to ensure update of the wanted LED
test = vrep.simxPackInts([m[1]+1, m[2]])
vrep.simxSetStringSignal(self._clientID, 'EPUCK_LED', test, vrep.simx_opmode_oneshot_wait)
elif m[0] == 'D':
# Set motor speed
# maxVel = 120.0 * math.pi / 180.0
# maxVel of ePuck firmware is 1000
# => 120 * pi / (180*1000) = pi/1500
velLeft = m[1] * math.pi / 1500.0
velRight = m[2] * math.pi / 1500.0
if velLeft > 120.0 * math.pi / 180.0:
velLeft = 120.0 * math.pi / 180.0
self._debug("velLeft too high, thresholded")
if velLeft < -120.0 * math.pi / 180.0:
velLeft = -120.0 * math.pi / 180.0
self._debug("velLeft too high, thresholded")
if velRight > 120.0 * math.pi / 180.0:
velRight = 120.0 * math.pi / 180.0
self._debug("velRight too high, thresholded")
if velRight < -120.0 * math.pi / 180.0:
velRight = -120.0 * math.pi / 180.0
self._debug("velRRight too high, thresholded")
vrep.simxSetFloatSignal(self._clientID, 'EPUCK_velLeft', velLeft, vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(self._clientID, 'EPUCK_velRight', velRight, vrep.simx_opmode_oneshot)
elif m[0] == 'P':
# Motor position
self._debug('WARNING: Motor position not yet implemented!')
else:
self._debug('Unknown actuator to write')
self._actuators_to_write.remove(m)
return
def move_rotors(clientID, propeller_vels):
vrep.simxSetFloatSignal(clientID, propellers[0], propeller_vels[0],
vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, propellers[1], propeller_vels[1],
vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, propellers[2], propeller_vels[2],
vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, propellers[3], propeller_vels[3],
vrep.simx_opmode_oneshot)
def vrep_change_properties(client_id, object_id, parameter_id,
parameter_value):
"""Changing properties of sensors in vrep
client_id (int): ID of current scene in vrep
object_id (int): ID of sensor to change
parameter_id (int): ID of parameter to change
parameter_value (int/double): value of parameter to change
"""
params_f = {
'near_clipping_plane': 1000,
'far_clipping_plane': 1001,
'perspective_angle': 1004
}
params_i = {
'vision_sensor_resolution_x': 1002,
'vision_sensor_resolution_y': 1003
}
if parameter_id == 'perspective_angle':
parameter_value = parameter_value / (180 * 2) * math.pi
if parameter_id in params_f:
error = vrep.simxSetObjectFloatParameter(client_id, object_id,
params_f[parameter_id],
parameter_value,
vrep.simx_opmode_blocking)
vrep.simxSetFloatSignal(client_id, 'change_params', parameter_value,
vrep.simx_opmode_blocking)
vrep.simxClearFloatSignal(client_id, 'change_params',
vrep.simx_opmode_blocking)
return error
elif parameter_id in params_i:
error = vrep.simxSetObjectIntParameter(client_id, object_id,
params_i[parameter_id],
parameter_value,
vrep.simx_opmode_blocking)
vrep.simxSetFloatSignal(client_id, 'change_params', parameter_value,
vrep.simx_opmode_blocking)
vrep.simxClearFloatSignal(client_id, 'change_params',
vrep.simx_opmode_blocking)
return error
else:
return 'parameter wasn\'t found'
def setRequiredParams(self):
# required_params = {
# 'pParam':6,
# 'iParam':0.04,
# 'dParam':0.08,
# 'vParam':-2,
# 'alphaE':0.3,
# 'dAlphaE':1.1, # 1.1
# 'alphaCumul': 0,
# 'betaE':0.6,
# 'dBetaE':1.1, # 2.5
# 'betaCumul':0.001,
# 'sp2':0.15,
# 'dsp2':4,
# 'sp2Cumul':0.005,
# 'sp1':0.08, #0.26
# 'dsp1':1, # 4.2
# 'sp1Cumul':0,
# }
required_params = {
'pParam': 30,
'iParam': 0,
'dParam': 20,
'vParam': -4,
'alphaE': 0.3,
'dAlphaE': 2, # 1.1
'alphaCumul': 0.01,
'betaE': 0.6,
'dBetaE': 3.5, # 2.5
'betaCumul': 0.02,
'sp2': 0.26,
'dsp2': 4.5,
'sp2Cumul': 0,
'sp1': 0.26, #0.26
'dsp1': 4.5, # 4.2
'sp1Cumul': 0,
}
# key to edit: betaE, dBetaE, betaZCumul, sp1, dsp1
for key, value in required_params.items():
vrep.simxSetFloatSignal(self.clientID, key, value,
vrep.simx_opmode_oneshot)
def sendSignalVREP(self,signalName, signalValue):
if type(signalValue) == str:
eCode = vrep.simxSetStringSignal(self._VREP_clientId, signalName, asByteArray(signalValue), vrep.simx_opmode_oneshot_wait)
elif type(signalValue) == int:
eCode = vrep.simxSetIntegerSignal(self._VREP_clientId, signalName, signalValue, vrep.simx_opmode_oneshot_wait)
elif type(signalValue) == float:
eCode = vrep.simxSetFloatSignal(self._VREP_clientId, signalName, signalValue, vrep.simx_opmode_oneshot_wait)
else:
raise Exception("Trying to send a signal of unknown data type. Only strings, floats and and ints are accepted")
if eCode != 0:
raise Exception("Could not send string signal", signalValue)
vrep.simxSynchronousTrigger(self._VREP_clientId)
def place_all_3_obj(low_thresh, up_thresh):
global objHandles
objHandles = np.zeros(3)
for i in range(0, 3):
# Assign new object position (simulation)
OP_x = (random.random() - 0.5) * 1.2
OP_y = abs(random.random() - 0.5) * 1.2
while math.sqrt(OP_x**2 + OP_y**2) < low_thresh or math.sqrt(
OP_x**2 + OP_y**2) > up_thresh:
OP_x = (random.random() - 0.5) * 1.2
OP_y = abs(random.random() - 0.5) * 1.2
vrep.simxSetFloatSignal(clientID, 'ObjPos_x', OP_x,
vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, 'ObjPos_y', OP_y,
vrep.simx_opmode_oneshot)
if i == 0:
vrep.simxSetStringSignal(clientID, 'Obj_type', 'Cube',
vrep.simx_opmode_oneshot)
errorCode, objHandles[0] = vrep.simxGetObjectHandle(
clientID, 'Cuboid', vrep.simx_opmode_blocking)
elif i == 1:
vrep.simxSetStringSignal(clientID, 'Obj_type', 'Cylinder',
vrep.simx_opmode_oneshot)
errorCode, objHandles[1] = vrep.simxGetObjectHandle(
clientID, 'Cylinder', vrep.simx_opmode_blocking)
elif i == 2:
vrep.simxSetStringSignal(clientID, 'Obj_type', 'Sphere',
vrep.simx_opmode_oneshot)
errorCode, objHandles[2] = vrep.simxGetObjectHandle(
clientID, 'Sphere', vrep.simx_opmode_blocking)
vrep.simxSetStringSignal(clientID, 'Obj_type', 'All',
vrep.simx_opmode_oneshot)
# get_20()
return objHandles
def place_object(OP_x, OP_y, obj_type):
global objHandle
vrep.simxSetFloatSignal(clientID, 'ObjPos_x', OP_x,
vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, 'ObjPos_y', OP_y,
vrep.simx_opmode_oneshot)
init_param = []
if obj_type == 'Cube':
vrep.simxSetStringSignal(clientID, 'Obj_type', 'Cube',
vrep.simx_opmode_oneshot)
errorCode, objHandle = vrep.simxGetObjectHandle(
clientID, 'Cuboid', vrep.simx_opmode_blocking)
paramreadfile = open('ParamFileCube.csv', 'rb+')
if obj_type == 'Cylinder':
vrep.simxSetStringSignal(clientID, 'Obj_type', 'Cylinder',
vrep.simx_opmode_oneshot)
errorCode, objHandle = vrep.simxGetObjectHandle(
clientID, 'Cylinder', vrep.simx_opmode_blocking)
paramreadfile = open('ParamFileCyl.csv', 'rb+')
if obj_type == 'Sphere':
vrep.simxSetStringSignal(clientID, 'Obj_type', 'Sphere',
vrep.simx_opmode_oneshot)
errorCode, objHandle = vrep.simxGetObjectHandle(
clientID, 'Sphere', vrep.simx_opmode_blocking)
paramreadfile = open('ParamFileSphere.csv', 'rb+')
param_reader = csv.reader(paramreadfile, delimiter=',', quotechar='|')
init_param_t = [list(map(float, rec)) for rec in param_reader]
if len(init_param_t) > 0:
init_param = init_param_t[0]
# print(init_param)
paramreadfile.close()
return [objHandle, init_param]
def set_ang_acc(self,ang_acc): mass_norm_acc = float(ang_acc[0]) roll_ang_acc = float(ang_acc[1]) pitch_ang_acc = float(ang_acc[2]) yaw_ang_acc = float(ang_acc[3]) print(mass_norm_acc) # print(roll_ang_acc) # print(pitch_ang_acc) # print(yaw_ang_acc) vrep.simxSetFloatSignal(self.clientID,'mass_norm_acc', mass_norm_acc,vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(self.clientID,'roll_ang_acc', roll_ang_acc,vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(self.clientID,'pitch_ang_acc', pitch_ang_acc,vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(self.clientID,'yaw_ang_acc', yaw_ang_acc,vrep.simx_opmode_oneshot) return
def update_paramter(d_position):
print(d_position)
x = d_position[0]
y = d_position[1]
dirs = np.array([x, -x, y, -y])
d = np.argmax(dirs)
print(dirs[d])
res, p1 = vrep.simxGetFloatSignal(clientID, "p1",
vrep.simx_opmode_blocking)
res, p2 = vrep.simxGetFloatSignal(clientID, "p2",
vrep.simx_opmode_blocking)
res, p3 = vrep.simxGetFloatSignal(clientID, "p3",
vrep.simx_opmode_blocking)
res, p4 = vrep.simxGetFloatSignal(clientID, "p4",
vrep.simx_opmode_blocking)
print(d)
if d == 0:
p1 += delta
p2 -= delta
p3 -= delta
p4 += delta
elif d == 1:
p1 -= delta
p2 += delta
p3 += delta
p4 -= delta
elif d == 2:
p1 += delta
p2 += delta
p3 -= delta
p4 -= delta
elif d == 3:
p1 -= delta
p2 -= delta
p3 += delta
p4 += delta
p = np.array([p1, p2, p3, p4])
[p1, p2, p3, p4] = p
print(p1, p2, p3, p4)
vrep.simxSetFloatSignal(clientID, "p1", p1, vrep.simx_opmode_blocking)
vrep.simxSetFloatSignal(clientID, "p2", p2, vrep.simx_opmode_blocking)
vrep.simxSetFloatSignal(clientID, "p3", p3, vrep.simx_opmode_blocking)
vrep.simxSetFloatSignal(clientID, "p4", p4, vrep.simx_opmode_blocking)
def envstep(action):
# Get old quadrotor state
err, pos_old = vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
err, euler_old = vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
# Set propeller thrusts
print("Setting propeller thrusts...")
propeller_vels = action
# Send propeller thrusts
print("Sending propeller thrusts...")
print(propeller_vels)
[vrep.simxSetFloatSignal(clientID, prop, vels, vrep.simx_opmode_oneshot) for prop, vels in
zip(propellers, propeller_vels)]
# Trigger simulator step
print("Stepping simulator...")
vrep.simxSynchronousTrigger(clientID)
# Get new quadrotor state
err, pos_new = vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
err, euler_new = vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
observation = np.array(euler_new)
reward_alpha = np.float32(-np.fabs(euler_new[0])) if not check_quad_flipped(euler_new) else np.float32(-10.)
reward_beta = np.float32(-np.fabs(euler_new[1])) if not check_quad_flipped(euler_new) else np.float32(-10.)
reward_gamma = np.float32(-np.fabs(euler_new[2])) if not check_quad_flipped(euler_new) else np.float32(-5.)
reward_alpha_acc = np.float32(-np.fabs(euler_new[0] - euler_old[0])) if not check_quad_flipped(
euler_new) else np.float32(-10.)
reward_beta_acc = np.float32(-np.fabs(euler_new[1] - euler_old[1])) if not check_quad_flipped(
euler_new) else np.float32(-10.)
reward_gamma_acc = np.float32(-np.fabs(euler_new[2] - euler_old[2])) if not check_quad_flipped(
euler_new) else np.float32(-5.)
reward = reward_alpha + reward_beta + reward_gamma + reward_alpha_acc * 10. + reward_beta_acc * 10. \
+ reward_gamma_acc * 10.
done = False
info = np.array(euler_new)
return observation, reward, done, info
def float_signal(cid, handle, val, signal_name):
err = vrep.simxSetFloatSignal(cid, signal_name, val, vrep_mode)
def step(self, actions):
# Set propeller thrusts
print("Setting propeller thrusts...")
# Only PD control bc can't find api function for getting simulation time
self.propeller_vels = pid(self.pos_error, self.euler_new,
self.euler_error[2], self.vel_error,
self.angvel_error)
self.propeller_vels += actions
# Send propeller thrusts
print("Sending propeller thrusts...")
[
vrep.simxSetFloatSignal(self.clientID, prop, vels,
vrep.simx_opmode_oneshot)
for prop, vels in zip(self.propellers, self.propeller_vels)
]
# Trigger simulator step
print("Stepping simulator...")
vrep.simxSynchronousTrigger(self.clientID)
# Get quadrotor initial position and orientation
err, self.pos_new = vrep.simxGetObjectPosition(self.clientID,
self.quad_handle, -1,
vrep.simx_opmode_buffer)
err, self.euler_new = vrep.simxGetObjectOrientation(
self.clientID, self.quad_handle, -1, vrep.simx_opmode_buffer)
err, self.vel_new, self.angvel_new = vrep.simxGetObjectVelocity(
self.clientID, self.quad_handle, vrep.simx_opmode_buffer)
self.pos_new = np.asarray(self.pos_new)
self.euler_new = np.asarray(self.euler_new) * 10
self.vel_new = np.asarray(self.vel_new)
self.angvel_new = np.asarray(self.angvel_new)
self.pos_old = self.pos_new
self.euler_old = self.euler_new
self.vel_old = self.vel_new
self.angvel_old = self.angvel_new
self.pos_error = (self.pos_start + self.setpoint_delta[0:3]) \
- self.pos_new
self.euler_error = (self.euler_start + self.setpoint_delta[3:6]) \
- self.euler_new
self.euler_error %= 2 * np.pi
for i in range(len(self.euler_error)):
if self.euler_error[i] > np.pi:
self.euler_error[i] -= 2 * np.pi
self.vel_error = (self.vel_start + self.setpoint_delta[6:9]) \
- self.vel_new
self.angvel_error = (self.angvel_start + self.setpoint_delta[9:12]) \
- self.angvel_new
valid = self.is_valid_state()
rew = self.get_reward()
self.timestep += 1
done = False
if self.timestep > self.episode_len or not valid:
done = True
return self.get_state(), rew, done, {}
time.sleep(0.2)
# Initialize simulation parameters
RobotNames = ["LineTracer", "LineTracer#0", "LineTracer#1"]
Params = [[[0.1, 0.2, 0, 0], [0.1, 0, 0, 0.2]],
[[0.2, 0.2, 0, 0], [0.2, 0, 0, 0.2]],
[[0.3, 0.3, 0, 0], [0.3, 0, 0, 0.4]]]
for i in range(len(RobotNames)):
rN = RobotNames[i]
Par = Params[i]
for j in range(len(Par)):
# For each motor
for k in range(len(Par[j])):
# For each sensor (+ base value)
SignalName = rN+"_"+str(j+1)+"_"+str(k+1)
res = vrep.simxSetFloatSignal(clientID, SignalName, Par[j][k],
vrep.simx_opmode_oneshot_wait)
rbt_names = ["LineTracer", "LineTracer#0", "LineTracer#1"]
init_pos = []
rbts = []
for rbt_name in rbt_names:
res, rbt_tmp = vrep.simxGetObjectHandle(clientID, rbt_name, vrep.simx_opmode_oneshot_wait)
rbts.append(rbt_tmp)
res, pos = vrep.simxGetObjectPosition(clientID, rbts[-1], -1, vrep.simx_opmode_streaming)
time.sleep(0.2)
for rbt in rbts:
res, pos = vrep.simxGetObjectPosition(clientID, rbt, -1, vrep.simx_opmode_buffer)
init_pos.append(pos)
# print pos
def float_signal(cid, handle, val, signal_name):
err = vrep.simxSetFloatSignal(cid, signal_name, val, vrep_mode)
def main():
print('Program started')
emptybuf = bytearray()
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 2000, 5)
if clientID != -1:
print('Connected to remote API server')
# Start the simulation:
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait)
# start init wheels --------------------------------------------------------------------------------------------
wheelJoints = np.empty(4, dtype=np.int)
wheelJoints.fill(-1) # front left, rear left, rear right, front right
res, wheelJoints[0] = vrep.simxGetObjectHandle(
clientID, 'rollingJoint_fl', vrep.simx_opmode_oneshot_wait)
res, wheelJoints[1] = vrep.simxGetObjectHandle(
clientID, 'rollingJoint_rl', vrep.simx_opmode_oneshot_wait)
res, wheelJoints[2] = vrep.simxGetObjectHandle(
clientID, 'rollingJoint_fr', vrep.simx_opmode_oneshot_wait)
res, wheelJoints[3] = vrep.simxGetObjectHandle(
clientID, 'rollingJoint_rr', vrep.simx_opmode_oneshot_wait)
# end init wheels ----------------------------------------------------------------------------------------------
# start init camera --------------------------------------------------------------------------------------------
# change the angle of the camera view (default is pi/4)
res = vrep.simxSetFloatSignal(clientID, 'rgbd_sensor_scan_angle',
math.pi / 2,
vrep.simx_opmode_oneshot_wait)
# turn on camera
res = vrep.simxSetIntegerSignal(clientID, 'handle_rgb_sensor', 2,
vrep.simx_opmode_oneshot_wait)
# get camera object-handle
res, youBotCam = vrep.simxGetObjectHandle(
clientID, 'rgbSensor', vrep.simx_opmode_oneshot_wait)
# get first image
err, resolution, image = vrep.simxGetVisionSensorImage(
clientID, youBotCam, 0, vrep.simx_opmode_streaming)
time.sleep(1)
# end init camera ----------------------------------------------------------------------------------------------
# programmable space -------------------------------------------------------------------------------------------
print("Begin calculation of H-matrix, please wait ...")
err, res, image = vrep.simxGetVisionSensorImage(
clientID, youBotCam, 0, vrep.simx_opmode_buffer)
image = colorDet.convertToCv2Format(image, res)
image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
found, prime_corners = cv2.findChessboardCorners(image, (3, 4))
prime_corners = addOne(prime_corners)
# gCX are the world coordinates for the points on the chessboards which are later used to construct the h-matrix
gCX = [[0.075, 0.55, 1.0], [0.075, 0.5, 1.0], [0.075, 0.45, 1.0],
[0.025, 0.55, 1.0], [0.025, 0.5, 1.0], [0.025, 0.45, 1.0],
[-0.025, 0.55, 1.0], [-0.025, 0.5, 1.0], [-0.025, 0.45, 1.0],
[-0.075, 0.55, 1.0], [-0.075, 0.5, 1.0], [-0.075, 0.45, 1.0]]
# convert all global corners of the chessboard in egocentric world space
ego_corners = []
for gc in gCX:
newCorner = colorDet.globalToEgocentric(gc, clientID)
ego_corners.append(newCorner)
# add a 1 in every row (globalToEgocentric only returns x,y coordinates
ego_corners = addOne(ego_corners)
# convert ego_corners in numpy array
ego_corners = np.asarray(ego_corners)
# calculate H-matrix
A = getA(prime_corners, ego_corners)
H = getH(A)
newH = cv2.findHomography(prime_corners, ego_corners)
print("H-matrix cv2:", newH)
print("H-matrix own:", H / H[2][2])
blobs = colorDet.findAllBlobs(clientID, youBotCam, H)
obstacleList = []
for b in blobs:
obstacleList.append(b[0])
print("Found blobs:")
# sort blob list and print it
blobs = sortBlobsByRad(blobs)
for blob in blobs:
print("Coordinate: ", blob[0],
" Upper and lower Bound of the color: ", blob[1])
# get position of youBot and goal
roboPos, ori = move.getPos(clientID)
res, objHandle = vrep.simxGetObjectHandle(
clientID, "goal", vrep.simx_opmode_oneshot_wait)
targetPosition = vrep.simxGetObjectPosition(
clientID, objHandle, -1, vrep.simx_opmode_oneshot_wait)
targetPosition = targetPosition[1][:2]
# drive to the goal with obstacles ahead
driveThroughPath(obstacleList, roboPos[:2], targetPosition, clientID)
# end of programmable space --------------------------------------------------------------------------------------------
# Stop simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
# Close connection to V-REP:
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
print('Program ended')
def setSteering(self, steering):
# Steering value [-1,1]
self.__steering = steering
#if self.getName()=="BubbleRob#1":
# print self.getName(), steering
vrep.simxSetFloatSignal(self.__clientID, self.__name+":Steering", self.__steering, vrep.simx_opmode_oneshot)
def setSteering(self, steering):
# Steering value [-1,1]
self.__steering = steering
vrep.simxSetFloatSignal(self.__clientID, self.__name+":Steering", self.__steering, vrep.simx_opmode_oneshot)
# Get "handle" to the end-effector of robot
result, end_handle = vrep.simxGetObjectHandle(clientID, 'UR10_link7_visible',
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for end effector')
# ==================================================================================================== #
# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
# ******************************** Your robot control code goes here ******************************** #
time.sleep(0.1)
counter = vrep.simxGetFloatSignal(clientID, "counter",
vrep.simx_opmode_streaming)[1]
vrep.simxSetFloatSignal(clientID, "RG2_open", 1, vrep.simx_opmode_oneshot)
previousCounter = counter
# Goal_joint_angles = np.array([[0,0,-0.5*np.pi,0.5*np.pi,-0.5*np.pi,-0.5*np.pi], \
# [0.5*np.pi,0,-0.5*np.pi,0.5*np.pi,0.5*np.pi,-0.5*np.pi],\
# [-0.5*np.pi,-0.5*np.pi,-0.5*np.pi,0,-0.5*np.pi,-0.5*np.pi]])
Goal_joint_angles = np.array([\
[0,0,0,0,0,0], \
[0.5*np.pi,0,0,0,0,0], \
[0,0.25*np.pi,0,0,0,0], \
[0,0,0.5*np.pi,0,0,0], \
[0,0,0,0.5*np.pi,0,0], \
[0,0,0,0,0.5*np.pi,0], \
[0,0,0,0,0,0.5*np.pi], \
# [0,0,0.45*np.pi,0,-0.5*np.pi,0], \
[0,0.1*np.pi,0.35*np.pi,0.05*np.pi,-0.5*np.pi,0], \
])
# errorCode, cam_handle = vrep.simxGetObjectHandle(clientID,
# 'cam1',
# vrep.simx_opmode_oneshot_wait)
#
# errorCode, resolution, image = \
# vrep.simxGetVisionSensorImage(clientID, cam_handle,
# 0, vrep.simx_opmode_streaming_split+4000)
now = time.time()
t0 = now-dT
while True:
if now > t0+dT:
# Set the float signal that tells the robot how fast to drive
res = vrep.simxSetFloatSignal(clientID, "Pioneer_p3dx_v0", speeds1[cur_speed],
vrep.simx_opmode_oneshot)
res = vrep.simxSetFloatSignal(clientID, "Pioneer_p3dx#0_v0", speeds2[cur_speed],
vrep.simx_opmode_oneshot)
res = vrep.simxSetFloatSignal(clientID, "Pioneer_p3dx#1_v0", speeds3[cur_speed],
vrep.simx_opmode_oneshot)
print 'New speeds: '+str(speeds1[cur_speed])+' '+str(speeds2[cur_speed])+' '+str(speeds3[cur_speed])
t0 = now
cur_speed += 1
if cur_speed >= len(speeds1):
cur_speed = 0
# Print elapsed time between each control step
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19997, True,
True, 5000, 5) # Connect to V-REP
res, drone = vrep.simxGetObjectHandle(clientID,"drone_zed",vrep.simx_opmode_blocking)
res, target = vrep.simxGetObjectHandle(clientID,"Quadricopter_target",vrep.simx_opmode_blocking)
assert drone != 0
assert target != 0
try:
ps = np.arange(0.004,0.02,0.002)
ds = np.arange(0.5,4,0.5)
with open("mesh.txt",'w') as f:
for p in ps:
for d in ds:
f.write("p: "+str(round(p,2))+"\td: "+str(round(d,2))+'\t')
print("p: "+str(round(p,2))+"\td: "+str(round(d,2)))
vrep.simxSetFloatSignal(clientID,"p",p,vrep.simx_opmode_blocking)
vrep.simxSetFloatSignal(clientID,"d",d,vrep.simx_opmode_blocking)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
start = time.time()
max_dis = 0
while True:
res, sp = vrep.simxGetObjectPosition(clientID,drone,target,vrep.simx_opmode_blocking)
max_dis =abs(sp[0]) if abs(sp[0]) > max_dis else max_dis
res, vel, _ = vrep.simxGetObjectVelocity(clientID,drone,vrep.simx_opmode_blocking)
if abs(sp[0])<0.1 and abs(vel[1])<0.1:
f.write("time: "+str(round(time.time() - start,2))+'\n')
f.write("dis: "+str(round(max_dis,2))+"\n\n")
print("time: "+str(round(time.time() - start,2)))
print("dis: "+str(round(max_dis,2)))
break
if max_dis > 2:
def sendSignal(self):
r = vrep.simxSetFloatSignal(self.clientID, self.LSignalName,
self.CycleFreqL, vrep.simx_opmode_oneshot)
r = vrep.simxSetFloatSignal(self.clientID, self.RSignalName,
self.CycleFreqR, vrep.simx_opmode_oneshot)
#float signals
Joint = [500, 500, 500, 500, 500, 500] #FK mode joint values
#IK mode position X
pos_X = 0
pos_Y = 0
pos_Z = 0
Alpha = 0
Beta = 0
Gamma = 0
vrep.simxSetIntegerSignal(clientID, "Apimode", Apimode,
vrep.simx_opmode_oneshot)
vrep.simxSetIntegerSignal(clientID, "movementMode", movementMode,
vrep.simx_opmode_oneshot)
if (movementMode):
vrep.simxSetFloatSignal(clientID, "pos_X", pos_X, vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, "pos_Y", pos_Y, vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, "pos_Z", pos_Z, vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, "Alpha", Alpha, vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, "Beta", Beta, vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, "Gamma", Gamma, vrep.simx_opmode_oneshot)
else:
vrep.simxSetFloatSignal(clientID, "Joint1", Joint[0],
vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, "Joint2", Joint[1],
vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, "Joint3", Joint[2],
vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, "Joint4", Joint[3],
vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, "Joint5", Joint[4],
def setThrust(self, thrust):
# Average wheel speed
self.__thrust = thrust
vrep.simxSetFloatSignal(self.__clientID, self.__name+":Acceleration", self.__thrust, vrep.simx_opmode_oneshot)
def setStepMode(stepVelocity, stepAmplitude, stepHeight, movementDirection,
rotationMode, movementStrength):
if (validConnection()):
vrep.simxSetFloatSignal(clientID, 'stepVelocity', stepVelocity,
vrep.simx_opmode_oneshot_wait)
vrep.simxSetFloatSignal(clientID, 'stepVelocity', stepAmplitude,
vrep.simx_opmode_oneshot_wait)
vrep.simxSetFloatSignal(clientID, 'stepHeight', stepHeight,
vrep.simx_opmode_oneshot_wait)
vrep.simxSetFloatSignal(clientID, 'movementDirection',
movementDirection,
vrep.simx_opmode_oneshot_wait)
vrep.simxSetFloatSignal(clientID, 'rotationMode', rotationMode,
vrep.simx_opmode_oneshot_wait)
vrep.simxSetFloatSignal(clientID, 'movementStrength', movementStrength,
vrep.simx_opmode_oneshot_wait)
def run_trial(self, genomes):
# Set the signals for each robot
for genome, robot in zip(genomes, self.robot_names):
par = [genome[:4], genome[4:]]
for j in range(len(par)):
# For each motor
for k in range(len(par[j])):
# For each sensor (+ base value)
signal_name = robot+"_"+str(j+1)+"_"+str(k+1)
res = vrep.simxSetFloatSignal(self.clientID, signal_name, par[j][k],
vrep.simx_opmode_oneshot)
if res > 1:
print 'Error setting signal '+signal_name+': '+str(res)
# Start running simulation
# print 'Running trial'
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot_wait)
# Initialize output arrays
sim_time = [[] for i in range(self.n_robots)]
robot_x = [[] for i in range(self.n_robots)]
robot_y = [[] for i in range(self.n_robots)]
init_pos = [[] for i in range(self.n_robots)]
fit_y = [0 for i in range(self.n_robots)]
t_step = 0.01 # how often to check the simulation's signals
t_flag = time.time()+t_step
once = True
new_data = [0 for i in range(self.n_robots)]
go_loop = True
while go_loop:
now = time.time()
if now > t_flag:
t_flag = now+t_step
# Get info from robot
for i in range(self.n_robots):
res1, in1 = vrep.simxGetFloatSignal(self.clientID,
self.robot_names[i]+'_1', vrep.simx_opmode_buffer)
res2, in2 = vrep.simxGetFloatSignal(self.clientID,
self.robot_names[i]+'_2', vrep.simx_opmode_buffer)
res3, in3 = vrep.simxGetFloatSignal(self.clientID,
self.robot_names[i]+'_3', vrep.simx_opmode_buffer)
if res1 == 0 and res2 == 0 and res3 == 0:
sim_time[i].append(in1)
robot_x[i].append(in2)
robot_y[i].append(in3)
new_data[i] = 1 # new data arrived
if once:
if sum(new_data) == self.n_robots:
for i in range(self.n_robots):
init_pos[i] = [robot_x[i][-1], robot_y[i][-1]]
once = False
else:
for i in range(self.n_robots):
if new_data[i] == 1:
fit_y[i] += abs(robot_y[i][-1])*(sim_time[i][-1]-sim_time[i][-2])
new_data[i] = 0
if sim_time[i][-1] > 3: # time limit for the simulation
go_loop = False
break
# # Pause the simulation
# vrep.simxPauseSimulation(self.clientID, vrep.simx_opmode_oneshot)
# time.sleep(0.15)
#
# trial_fitness = []
# # Get the trial results for each robot
# for i in range(len(self.robot_handles)):
# res, pos = vrep.simxGetObjectPosition(self.clientID, self.robot_handles[i], -1, vrep.simx_opmode_buffer)
# # print pos_tmp
#
# delta_x = pos[0] - self.robot_pos0[i][0]
# delta_y = pos[1] - self.robot_pos0[i][1]
#
# # trial_fitness.append(delta_x)
# trial_fitness.append([delta_x, 1./(1.+20*abs(delta_y))])
trial_fitness = []
for i in range(self.n_robots):
x_fit = robot_x[i][-1] - init_pos[i][0]
y_fit = 1/(1+20*fit_y[i])
trial_fitness.append([x_fit, y_fit])
# Stop and reset the simulation
vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
time.sleep(0.35)
return trial_fitness
