def run_threaded_drop(self, frame_work2obj):
"""Gets an initial object pose by 'dropping' it WRT frame_work2obj.
This function launches a threaded script that sets an object position,
enables the object to be dynamically simulated, and allows it to fall
to the ground and come to a resting pose. This is usually the first
step when collecting grasps.
Note that we also set the gripper to be non-collidable / respondable,
so it doesn't interfere with the drop process
"""
self._clear_signals()
self.set_gripper_properties(visible=True,
dynamic=True,
collidable=True)
frame = self._format_matrix(frame_work2obj)
# Launch the threaded script
vrep.simxSetStringSignal(self.clientID, 'run_drop_object',
vrep.simxPackFloats(frame),
vrep.simx_opmode_oneshot)
r = -1
while r != vrep.simx_return_ok:
r, success = vrep.simxGetIntegerSignal(
self.clientID, 'object_resting', vrep.simx_opmode_oneshot_wait)
if success == 0:
raise Exception('Error dropping object! Return code: ', r)
def JacoHandGrap():
global grap_state
# input
input_invalid = 1
while (input_invalid):
input_msg = input("grap or release? (please enter grap or release)")
if ((input_msg == "grap") | (input_msg == "release")):
input_invalid = 0
else:
print("invalid input!\n")
# grap
if (input_msg == "grap"):
if (grap_state == 0):
vrep.simxSetStringSignal(clientID, 'jacoHand', 'true',
vrep.simx_opmode_oneshot)
time.sleep(1)
grap_state = 1
else:
print("Already grapped. Ignore your command.")
return
# release
if (input_msg == "release"):
if (grap_state == 1):
vrep.simxSetStringSignal(clientID, 'jacoHand', 'false',
vrep.simx_opmode_oneshot)
grap_state = 0
else:
print("Already release. Ignore your command.")
return
def search_object():
print('Searching...')
vrep.simxSetStringSignal(clientID, 'IKCommands', 'LookPos',
vrep.simx_opmode_oneshot) # "Looking" position
center_res = 10
robot_joints.velocity[0] = 0.5
robot_joint_pub.publish(robot_joints)
# vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], 0.5, vrep.simx_opmode_streaming)
tic_so = time.clock()
objectFound = False
while objectFound is False:
toc_so = time.clock()
robot_joints.velocity[0] = 0.5
robot_joint_pub.publish(robot_joints)
rgb_image.resize([resolution[0], resolution[1], 3])
[grasp_type, im_x, im_y,
obj_pix_prop] = image_process(rgb_image, resolution)
#
if grasp_type > 0:
print('Object Found!')
print('Grasp Type: ', grasp_type)
full_center(im_x, im_y, center_res, resolution)
# lock_all_joints()
# center_object(0.0025, im_x, im_y, resolution)
return 1
elif toc_so - tic_so > 20:
return 6
def calcTheForce(self):
# расчет сил
dx = self.XYZ_d[0] - self.XYZ[0]
dy = self.XYZ_d[1] - self.XYZ[1]
self.ABG_d[2] = np.arctan2(dy, dx)
Fx_d = 0
if -0.05 <= self.ABG_d[2] - self.ABG[2] <= 0.05:
if dx < 0 and dy < 0:
if dx > dy:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0],
self.XYZ[0]) * -1
else:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1],
self.XYZ[1]) * -1
if dx > 0 and dy > 0:
if dx > dy:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0])
else:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1])
if dx < 0 and dy > 0:
if dx > dy:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0],
self.XYZ[0]) * -1
else:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1])
if dx > 0 and dy < 0:
if dx > dy:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0])
else:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1],
self.XYZ[1]) * -1
# перемещения вдоль игрек
if -0.5 <= dx <= 0.5 and (self.XYZ[1] <= self.XYZ_d[1]):
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1])
if (-0.5 <= dx <= 0.5) and (self.XYZ[1] > self.XYZ_d[1]):
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1]) * -1
# перемещения вдоль икс
if -0.5 <= dy <= 0.5 and (self.XYZ[0] <= self.XYZ_d[0]):
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0])
if (-0.5 <= dy <= 0.5) and (self.XYZ[0] > self.XYZ_d[0]):
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0]) * -1
else:
Fx_d = 0
Fy_d = 0
Fz_d = PIDc.pidUpdate(self.stab_ctrl, self.XYZ_d[2], self.XYZ[2])
# повороты: рыскание, тангаж и крен
Mx_d = PIDc.pidUpdate(self.rotation_ctrl, self.ABG_d[2],
self.ABG[2]) # вокруг Z, рыскание
My_d = PIDc.pidUpdate(self.pitchroll_ctrl, self.ABG_d[1], self.ABG[1])
Mz_d = PIDc.pidUpdate(self.pitchroll_ctrl, self.ABG_d[0], self.ABG[0])
forceD = [Fx_d, Fy_d, Fz_d, Mx_d, My_d, Mz_d]
s_force = vrep.simxPackFloats(forceD)
vrep.simxSetStringSignal(self.clientID, "ForceD", s_force,
vrep.simx_opmode_oneshot)
def search_object_2():
print('Searching...')
vrep.simxSetStringSignal(clientID, 'IKCommands', 'LookPos',
vrep.simx_opmode_oneshot) # "Looking" position
center_res = 10
tic_so = time.clock()
objectFound = False
while objectFound is False:
toc_so = time.clock()
errorCode2, resolution, image = vrep.simxGetVisionSensorImage(
clientID, cam_Handle, 0, vrep.simx_opmode_buffer)
im = np.array(image, dtype=np.uint8)
if len(resolution) > 0:
im.resize([resolution[0], resolution[1], 3])
obj_list = image_process_2(im, resolution)
if len(obj_list) > 0:
print(obj_list[0])
if len(obj_list[0]) > 1:
im_x = obj_list[0][1]
im_y = obj_list[0][2]
print('Object Found!')
full_center(im_x, im_y, center_res, resolution)
return 1
errorCode, joint_ang_0 = vrep.simxGetJointPosition(
clientID, joint_handles[0], vrep.simx_opmode_buffer)
errorCode, joint_ang_4 = vrep.simxGetJointPosition(
clientID, joint_handles[4], vrep.simx_opmode_buffer)
if joint_ang_0 > 70 * math.pi / 180:
vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], 0,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, joint_handles[4], -0.2,
vrep.simx_opmode_streaming)
if joint_ang_4 < 100 * math.pi / 180:
vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], -0.2,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, joint_handles[4], 0,
vrep.simx_opmode_streaming)
if joint_ang_0 < -70 * math.pi / 180:
vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], 0,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, joint_handles[4], 0.2,
vrep.simx_opmode_streaming)
if joint_ang_4 > 118 * math.pi / 180:
vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], 0.2,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, joint_handles[4], 0,
vrep.simx_opmode_streaming)
if toc_so - tic_so > 20:
return 6
def final_grasp():
# print('Final Grasp!')
gripper_type = 2
if gripper_type == 2:
vrep.simxSetJointTargetVelocity(clientID, joint_handles[4], 0.04,
vrep.simx_opmode_oneshot)
time.sleep(2)
time.sleep(1)
vrep.simxSetStringSignal(clientID, 'Hand', 'true',
vrep.simx_opmode_oneshot)
time.sleep(5)
return 6
def set_led_light(self, led_number, intensity=0):
if led_number > 7 or led_number < 0:
if self.debug:
err_print(prefix="SET LED LIGHT",
message=["LED NUMBER SHOULD BE BETWEEN 0 AND 7"])
elif intensity not in (0, 1):
pass
else:
self.led_lights[led_number] = intensity
pack = vrep.simxPackInts([led_number, intensity])
vrep.simxSetStringSignal(self.client_id, 'EPUCK_LED' + self.suffix,
pack, vrep.simx_opmode_oneshot_wait)
def _connect(self):
self.__clientID = vrep.simxStart(self.__host_url, self.__port, True, True, 5000, 5) # Connect to V-REP
if self.__clientID != -1:
print ('Connected to remote API server of Vrep with clientID: ', self.__clientID)
self._connectionStatus = True
vrep.simxGetStringSignal(self.__clientID, self.__signalName+'_allSens', vrep. simx_opmode_streaming) #first time call
vrep.simxGetStringSignal(self.__clientID, self.__signalName+'_camera', vrep. simx_opmode_streaming) #first time call
vrep.simxSetStringSignal(self.__clientID, self.__signalName+'_velocities', vrep.simxPackFloats([0.0, 0.0]), vrep.simx_opmode_streaming) #first time call
# if self.__synchronous:
# self.startsim()
else:
logging.error('Failed connecting to remote API server of Vrep')
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 show_path(path, clientID):
#make a signal for each path-component
datax = path[0]
datay = path[1]
dataz = path[2]
packedDatax = vrep.simxPackFloats(datax)
vrep.simxClearStringSignal(clientID, 'Path_Signalx',
vrep.simx_opmode_oneshot)
vrep.simxSetStringSignal(clientID, 'Path_Signalx', packedDatax,
vrep.simx_opmode_oneshot)
packedDatay = vrep.simxPackFloats(datay)
vrep.simxClearStringSignal(clientID, 'Path_Signaly',
vrep.simx_opmode_oneshot)
vrep.simxSetStringSignal(clientID, 'Path_Signaly', packedDatay,
vrep.simx_opmode_oneshot)
packedDataz = vrep.simxPackFloats(dataz)
vrep.simxClearStringSignal(clientID, 'Path_Signalz',
vrep.simx_opmode_oneshot)
vrep.simxSetStringSignal(clientID, 'Path_Signalz', packedDataz,
vrep.simx_opmode_oneshot)
#signals that the data of the path is ready to be read
data2 = [1]
packedData2 = vrep.simxPackFloats(data2)
vrep.simxClearStringSignal(clientID, 'Command_Path_Ready',
vrep.simx_opmode_oneshot)
vrep.simxSetStringSignal(clientID, 'Command_Path_Ready', packedData2,
vrep.simx_opmode_oneshot)
def _setMotorSpeeds(self, leftSpeed, rightSpeed):
"""
set the desired speeds of the left and right wheel of the robot
:param
leftSpeed: float
speed in rad/sec of left motor
:param
rightSpeed: float
speed in rad/sec of right motor
"""
"""
res, ret_ints, ret_floats, ret_strings, ret_buffer = vrep.simxCallScriptFunction(self.__clientID, self._name,
vrep.sim_scripttype_childscript,
'setVelocitiesForRemote', [],
[leftSpeed, rightSpeed], [],
self.__empty_buff, vrep.simx_opmode_blocking)
"""
velocities = vrep.simxPackFloats([leftSpeed, rightSpeed])
res = vrep.simxSetStringSignal(self.__clientID, self.__signalName+'_velocities', velocities, vrep.simx_opmode_oneshot)
# res = vrep.simx_return_ok
# res = vrep.simxSetFloatSignal(self.__clientID, self.__signalName+'_velLeft', 0.0, vrep.simx_opmode_oneshot)
# res = vrep.simxSetFloatSignal(self.__clientID, self.__signalName+'_velRight', 0.0, vrep.simx_opmode_oneshot)
if res == vrep.simx_return_ok:
logging.debug(' motor values', leftSpeed, rightSpeed)
else:
logging.error('Remote function call simxSetStringSignal for signal EPUCK_velLeft/-Right failed')
def dvs(cid,
handle=None,
signal_name="dataFromThisTimestep",
dim_x=32,
dim_y=32,
offset=0,
magnitude=1):
empty_str = ""
raw_bytes = (ctypes.c_ubyte * len(empty_str)).from_buffer_copy(empty_str)
err, data = vrep.simxGetStringSignal(cid, "dataFromThisTimeStep",
vrep.simx_opmode_oneshot)
err = vrep.simxSetStringSignal(cid, "dataFromThisTimeStep", raw_bytes,
vrep.simx_opmode_oneshot_wait)
image = np.zeros((dim_x, dim_y), dtype='uint8') + offset
l = len(data)
for i in range(int(l / 4)):
b = list(bytearray(data[i * 4:i * 4 + 4]))
x_coord = b[0]
y_coord = b[1]
if x_coord >= 128:
x_coord -= 128
polarity = 1
else:
#polarity = 0
polarity = -1
timestamp = (b[3] * 256) + b[2]
# Distinguish between different polarities
#image[127-y_coord][127-x_coord] = polarity * magnitude
image[dim_y - 1 - y_coord][dim_x - 1 - x_coord] = polarity * magnitude
# No Distinction between different polarities
#image[127-y_coord][127-x_coord] = magnitude
return image.ravel()
def __init__( self, sim_dt=0.01, max_target_distance=3, noise=False,
noise_std=[0,0,0,0,0,0],
):
# Call the superclass of Quadcopter, which is Robot
super(Quadcopter, self).__init__(sim_dt)
err, self.copter = vrep.simxGetObjectHandle(self.cid, "Quadricopter_base",
vrep.simx_opmode_oneshot_wait )
# Reset the motor commands to zero
packedData=vrep.simxPackFloats([0,0,0,0])
raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities",
raw_bytes,
vrep_mode)
self.pos = [0,0,0]
self.pos_err = [0,0,0]
self.t_pos = [0,0,0]
self.lin = [0,0,0]
self.ori = [0,0,0]
self.ori_err = [0,0,0]
self.t_ori = [0,0,0]
self.ang = [0,0,0]
# Maximum target distance error that can be returned
self.max_target_distance = max_target_distance
# If noise is being modelled
self.noise = noise
# Standard Deviation of the noise for the 4 state variables
self.noise_std = noise_std
def dvs(cid, handle=None, signal_name="dataFromThisTimestep",
dim_x=32, dim_y=32, offset=0, magnitude=1):
empty_str=""
raw_bytes = (ctypes.c_ubyte * len(empty_str)).from_buffer_copy(empty_str)
err, data = vrep.simxGetStringSignal(cid, "dataFromThisTimeStep",
vrep.simx_opmode_oneshot)
err = vrep.simxSetStringSignal(cid, "dataFromThisTimeStep", raw_bytes,
vrep.simx_opmode_oneshot_wait)
image = np.zeros( (dim_x, dim_y), dtype='uint8' ) + offset
l = len(data)
for i in range( int(l/4) ):
b = list(bytearray(data[i*4:i*4+4]))
x_coord = b[0]
y_coord = b[1]
if x_coord >= 128:
x_coord -= 128
polarity = 1
else:
#polarity = 0
polarity = -1
timestamp = (b[3] * 256) + b[2]
# Distinguish between different polarities
#image[127-y_coord][127-x_coord] = polarity * magnitude
image[dim_y-1-y_coord][dim_x-1-x_coord] = polarity * magnitude
# No Distinction between different polarities
#image[127-y_coord][127-x_coord] = magnitude
return image.ravel()
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 __init__( self, sim_dt=0.01, max_target_distance=3, noise=False,
noise_std=[0,0,0,0,0,0],
target_func=None,
):
super(Quadcopter, self).__init__(sim_dt)
err, self.copter = vrep.simxGetObjectHandle(self.cid, "Quadricopter_base",
vrep.simx_opmode_oneshot_wait )
err, self.target = vrep.simxGetObjectHandle(self.cid, "Quadricopter_target",
vrep.simx_opmode_oneshot_wait )
# Reset the motor commands to zero
packedData=vrep.simxPackFloats([0,0,0,0])
raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities",
raw_bytes,
vrep_mode)
self.pos = [0,0,0]
self.pos_err = [0,0,0]
self.t_pos = [0,0,0]
self.lin = [0,0,0]
self.ori = [0,0,0]
self.ori_err = [0,0,0]
self.t_ori = [0,0,0]
self.ang = [0,0,0]
self.vert_prox_dist = 0
self.left_prox_dist = 0
self.right_prox_dist = 0
# Distance reading recorded when nothing is in range
self.max_vert_dist = 1.5
self.max_left_dist = 1.0
self.max_right_dist = 1.0
# Maximum target distance error that can be returned
self.max_target_distance = max_target_distance
# If noise is being modelled
self.noise = noise
# Standard Deviation of the noise for the 4 state variables
self.noise_std = noise_std
# Overwrite the get_target method if the target is to be controlled by a
# function instead of by V-REP
if target_func is not None:
self.step = 0
self.target_func = target_func
def get_target():
self.t_pos, self.t_ori = self.target_func( self.step )
self.step += 1
self.get_target = get_target
def set_swarm_data(self, val):
signal_name = "signal"
res = vrep.simxSetStringSignal(self.client_id, signal_name, val,
vrep.simx_opmode_blocking)
if res != 0:
err_print("SET WARM DATA", parse_error(res))
raise Exception("ERROR IN SET WARM DATA")
def __call__(self, t, values):
if self.size is None:
packedData = vrep.simxPackFloats(values.flatten())
else:
packedData = vrep.simxPackFloats(values[:self.size].flatten())
raw_bytes = (ctypes.c_ubyte *
len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, self.signal_name, raw_bytes,
self.mode)
def __call__( self, t, values ):
if self.size is None:
packedData=vrep.simxPackFloats(values.flatten())
else:
packedData=vrep.simxPackFloats(values[:self.size].flatten())
raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, self.signal_name,
raw_bytes,
self.mode)
def quadcopter_rotors(cid, handle, val):
# This function does not use handle, it just exists in the signature
# to make it consistent
motor_values = np.zeros(4)
for i in range(4):
motor_values[i] = val[i]
packedData = vrep.simxPackFloats(motor_values.flatten())
raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(cid, "rotorTargetVelocities", raw_bytes,
vrep_mode)
def send_motor_commands( self, values ):
motor_values = np.zeros(4)
for i in range(4):
motor_values[i] = values[i]
packedData=vrep.simxPackFloats(motor_values.flatten())
raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities",
raw_bytes,
vrep_mode)
def forward(self, action=None):
controller.target_move(self.cid,self.target,self.function,self.args)
if not(action is None):
packedData = vrep.simxPackFloats([x for x in action])
raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities",
packedData,
vrep.simx_opmode_oneshot)
firstPass = False
else:
controller.controller_motor(self.cid,self.copter,self.target)
def quadcopter_rotors(cid, handle, val):
# This function does not use handle, it just exists in the signature
# to make it consistent
motor_values = np.zeros(4)
for i in range(4):
motor_values[i] = val[i]
packedData=vrep.simxPackFloats(motor_values.flatten())
raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(cid, "rotorTargetVelocities",
raw_bytes,
vrep_mode)
def forward(self, action=None):
controller.target_move(self.cid, self.target, self.function, self.args)
if not (action is None):
packedData = vrep.simxPackFloats([x for x in action])
raw_bytes = (ctypes.c_ubyte *
len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities",
packedData,
vrep.simx_opmode_oneshot)
firstPass = False
else:
controller.controller_motor(self.cid, self.copter, self.target)
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_setpos():
global objHandles
objHandles = np.zeros(3)
theta = 45 * math.pi / 180
distance = 0.5
# print('Place All 3 Object Set Positions!!!!!')
OP_x_cube, OP_y_cube = -0.1035, distance
OP_x_cyl, OP_y_cyl = distance * math.sin(
theta) - 0.1035, distance * math.cos(theta)
OP_x_sphere, OP_y_sphere = -distance * math.sin(
theta) - 0.1035, distance * math.cos(theta)
# OP_x_sphere, OP_y_sphere = -0.1035, distance
# OP_x_cyl, OP_y_cyl = distance * math.sin(theta) - 0.1035, distance * math.cos(theta)
# OP_x_cube, OP_y_cube = -distance * math.sin(theta) - 0.1035, distance * math.cos(theta)
vrep.simxSetFloatSignal(clientID, 'ObjPos_x', OP_x_cube,
vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, 'ObjPos_y', OP_y_cube,
vrep.simx_opmode_oneshot)
vrep.simxSetStringSignal(clientID, 'Obj_type', 'Cube',
vrep.simx_opmode_oneshot)
errorCode, objHandles[0] = vrep.simxGetObjectHandle(
clientID, 'Cuboid', vrep.simx_opmode_blocking)
vrep.simxSetFloatSignal(clientID, 'ObjPos_x', OP_x_cyl,
vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, 'ObjPos_y', OP_y_cyl,
vrep.simx_opmode_oneshot)
vrep.simxSetStringSignal(clientID, 'Obj_type', 'Cylinder',
vrep.simx_opmode_oneshot)
errorCode, objHandles[1] = vrep.simxGetObjectHandle(
clientID, 'Cylinder', vrep.simx_opmode_blocking)
vrep.simxSetFloatSignal(clientID, 'ObjPos_x', OP_x_sphere,
vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, 'ObjPos_y', OP_y_sphere,
vrep.simx_opmode_oneshot)
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)
return objHandles
def show_path(path, clientID):
# make a signal for each path-component
datax = path[0]
datay = path[1]
dataz = path[2]
packedDatax = vrep.simxPackFloats(datax)
vrep.simxClearStringSignal(clientID, "Path_Signalx", vrep.simx_opmode_oneshot)
vrep.simxSetStringSignal(clientID, "Path_Signalx", packedDatax, vrep.simx_opmode_oneshot)
packedDatay = vrep.simxPackFloats(datay)
vrep.simxClearStringSignal(clientID, "Path_Signaly", vrep.simx_opmode_oneshot)
vrep.simxSetStringSignal(clientID, "Path_Signaly", packedDatay, vrep.simx_opmode_oneshot)
packedDataz = vrep.simxPackFloats(dataz)
vrep.simxClearStringSignal(clientID, "Path_Signalz", vrep.simx_opmode_oneshot)
vrep.simxSetStringSignal(clientID, "Path_Signalz", packedDataz, vrep.simx_opmode_oneshot)
# signals that the data of the path is ready to be read
data2 = [1]
packedData2 = vrep.simxPackFloats(data2)
vrep.simxClearStringSignal(clientID, "Command_Path_Ready", vrep.simx_opmode_oneshot)
vrep.simxSetStringSignal(clientID, "Command_Path_Ready", packedData2, vrep.simx_opmode_oneshot)
def send_motor_commands(self, values):
# Limit motors by max and min values
motor_values = np.zeros(4)
for i in range(4):
# if values[i] > 30:
# motor_values[i] = 30
# elif values[i] < 0:
# motor_values[i] = 0
# else:
# motor_values[i] = values[i]
motor_values[i] = values[i]
# print('motor_value', motor_values)
packedData = vrep.simxPackFloats(motor_values.flatten())
raw_bytes = (ctypes.c_ubyte *
len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities",
raw_bytes, vrep_mode)
def send_motor_commands( self, values ):
# Limit motors by max and min values
motor_values = np.zeros(4)
for i in range(4):
"""
if values[i] > 30:
motor_values[i] = 30
elif values[i] < 0:
motor_values[i] = 0
else:
motor_values[i] = values[i]
"""
motor_values[i] = values[i]
packedData=vrep.simxPackFloats(motor_values.flatten())
raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities",
raw_bytes,
vrep_mode)
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 traversePath(clientID, path, robotHandle, signalname, signalval):
#ret = vrep.simxSetStringSignal(clientID,signalname,'change',vrepConst.simx_opmode_oneshot)
#ret = True
startTime = time.time()
while time.time() - startTime < 4:
x = True
ret, robotHandle = vrep.simxGetObjectHandle(clientID, robotHandle,
vrep.simx_opmode_oneshot_wait)
if (ret == 0):
for i in range(0, len(path), 3):
# print("dinchak")
pos = (path[i], path[i + 1], path[i + 2])
ret = setRobotPosition(clientID, robotHandle, pos)
startTime = time.time()
while time.time() - startTime < 6:
x = True
print(signalname)
print(signalval)
ret = vrep.simxSetStringSignal(clientID, signalname, signalval,
vrepConst.simx_opmode_oneshot_wait)
print(ret)
return ret
def show_path2(path,clientID):
errorCode,Graph=vrep.simxGetObjectHandle(clientID,'Graph',vrep.simx_opmode_oneshot_wait)
print ("sending path to VREP")
datax=path[0]
datay=path[1]
dataz=path[2]
packedDatax=vrep.simxPackFloats(datax)
vrep.simxClearStringSignal(clientID,'Path_Signalx',vrep.simx_opmode_oneshot)
vrep.simxSetStringSignal(clientID,'Path_Signalx',packedDatax,vrep.simx_opmode_oneshot)
packedDatay=vrep.simxPackFloats(datay)
vrep.simxClearStringSignal(clientID,'Path_Signaly',vrep.simx_opmode_oneshot)
vrep.simxSetStringSignal(clientID,'Path_Signaly',packedDatay,vrep.simx_opmode_oneshot)
packedDataz=vrep.simxPackFloats(dataz)
vrep.simxClearStringSignal(clientID,'Path_Signalz',vrep.simx_opmode_oneshot)
vrep.simxSetStringSignal(clientID,'Path_Signalz',packedDataz,vrep.simx_opmode_oneshot)
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_signal(self, signal, val):
res = vrep.simxSetStringSignal(self.client_id, signal, val,
vrep.simx_opmode_blocking)
if res != 0:
err_print("SET SIGNAL", parse_error(res))
raise Exception("ERROR IN SET SIGNAL")
initialCall=True
print ('VREP Simulation Program')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to V-REP
if clientID!=-1:
print ('Connected to remote API server')
joystick = quadCTRL()
# Object handle
# _,Quadbase=vrep.simxGetObjectHandle(clientID,'Quadricopter_base',vrep.simx_opmode_oneshot_wait)
else:
print('Connection Failure')
sys.exit('Abort Connection')
while True:
# Code for testing...
if initialCall:
mode = vrep.simx_opmode_streaming
initialCall = False
else:
mode = vrep.simx_opmode_buffer
# Poll controller
demands = joystick.DetectAction()
print("R: " + str(demands[0]) + " " +str(demands[1]) + " " +
"L: " + str(demands[2]) + " " +str(demands[3]))
demandString=vrep.simxPackFloats(demands)
vrep.simxSetStringSignal(clientID,'demandstring',demandString,vrep.simx_opmode_oneshot)
print('EOS')
def followPath(clientID, path, goal):
# arrange the data given to this function
pathx = path[0]
pathy = path[1]
pathz = path[2]
# get the start infos for the following
errorCode, UAV = vrep.simxGetObjectHandle(clientID, "UAV", vrep.simx_opmode_oneshot_wait)
errorCode, pos = vrep.simxGetObjectPosition(clientID, UAV, -1, vrep.simx_opmode_streaming)
errorCode, orientation = vrep.simxGetObjectOrientation(clientID, UAV, -1, vrep.simx_opmode_streaming)
time.sleep(0.1)
errorCode, pos = vrep.simxGetObjectPosition(clientID, UAV, -1, vrep.simx_opmode_buffer)
errorCode, orientation = vrep.simxGetObjectOrientation(clientID, UAV, -1, vrep.simx_opmode_buffer)
# some initialization
xPosition = pos[0]
yPosition = pos[1]
zPosition = pos[2]
xp = []
yp = []
zp = []
xpnear = []
ypnear = []
zpnear = []
xerror = []
yerror = []
zerror = []
# define the radius around the goal, in which the UAV is slowed to lower the error between UAV and path
slowvelo_dis = 4
absolut_dis = 1
# path-following loop
# here is the goal defined, in this case reach the goal <5cm, <2cm also works in nearly all case, but needs some more time at the end
while absolut_dis > 0.05:
# define the max possible velocities
xvelomax = 1.5
yvelomax = 1.5
# define the max possible turnrate
turnmax = 8
# refresh the UAV information
errorCode, pos = vrep.simxGetObjectPosition(clientID, UAV, -1, vrep.simx_opmode_buffer)
errorCode, orientation = vrep.simxGetObjectOrientation(clientID, UAV, -1, vrep.simx_opmode_buffer)
# arrange the information
xPosition = pos[0]
yPosition = pos[1]
zPosition = pos[2]
# calculate the distance to the goal
absolut_dis = math.sqrt(
(xPosition - pathx[(len(pathx) - 1)]) ** 2
+ (yPosition - pathy[(len(pathx) - 1)]) ** 2
+ (zPosition - pathz[(len(pathx) - 1)]) ** 2
)
# lower the max velocities in the defined radius, closer to the goal the UAV is more slowed
if absolut_dis < slowvelo_dis:
xvelomax = (xvelomax - 0.15) * absolut_dis / slowvelo_dis * absolut_dis / slowvelo_dis + 0.15
yvelomax = (yvelomax - 0.15) * absolut_dis / slowvelo_dis * absolut_dis / slowvelo_dis + 0.15
# save some information for the documentation after the following
xp.append(xPosition)
yp.append(yPosition)
zp.append(zPosition)
# get the direction the UAV should fly to follow the path
vec, vec_path, pnear, dis = pathfollowing.findnearst(pos, path)
# some more info for documentation
xpnear.append(pnear[0])
ypnear.append(pnear[1])
zpnear.append(pnear[2])
xerror.append(abs(xPosition - pnear[0]))
yerror.append(abs(yPosition - pnear[1]))
zerror.append(abs(zPosition - pnear[2]))
# calculate the velocities from the direction vector
absolut = math.sqrt(vec[0, 0] ** 2 + vec[0, 1] ** 2)
xvelo = 0
yvelo = 0
height = zPosition
xvelo_w = xvelomax * vec[0, 0] / absolut # ref_velx
yvelo_w = yvelomax * vec[0, 1] / absolut # ref_vely
height = zPosition + vec[0, 2] # e
# ref_angz, angle between (1/0/0) and (xvelo/yvelo/0)
a1 = [1, 0, 0]
b1 = [vec_path[0], vec_path[1], 0]
ref_angz = angle_calculation(a1, b1)
# arrange some info
angle = orientation[2]
dangle = ref_angz - angle
# angle correction to prevent the UAV from turning not into the shorter direction
if dangle > np.pi:
dangle = dangle - 2 * np.pi
if dangle < -np.pi:
dangle = 2 * np.pi + dangle
# calculate the turnrate, its lower, if the orientation error is lower, to prevent the UAV from doing a oszillation around the right orientation
veloz = turnmax * (dangle) / np.pi
# transfom the velocities into the UAV-coordinate-system from the world-coordinates
xvelo = xvelo_w * np.cos(angle) + yvelo_w * np.sin(angle)
yvelo = yvelo_w * np.cos(angle) - xvelo_w * np.sin(angle)
# modify the velocities to improve the path following, by using the current distances and errors to the path and his orientation
if abs(vec[0, 2]) > 0.2:
xvelo = 0
yvelo = 0
else:
if dis < 0.5:
xvelo = (
xvelo
* ((np.pi - abs(dangle)) / np.pi) ** 130
* (((0.2 - abs(vec[0, 2])) / 0.2) ** 6)
* ((0.5 - dis) / 0.5)
)
yvelo = (
yvelo
* ((np.pi - abs(dangle)) / np.pi) ** 15
* (((0.2 - abs(vec[0, 2])) / 0.2) ** 6)
* (dis + 0.2) ** 2
)
else:
xvelo = (
xvelo
* ((np.pi - abs(dangle)) / np.pi) ** 130
* (((0.2 - abs(vec[0, 2])) / 0.2) ** 6)
* ((0.5 - dis) / 0.5)
)
yvelo = yvelo * ((np.pi - abs(dangle)) / np.pi) ** 15 * (((0.2 - abs(vec[0, 2])) / 0.2) ** 6)
if abs(vec_path[2]) > 0.003:
print vec_path[2]
xvelo = xvelo * (0.003 / abs(vec_path[2])) ** 3
yvelo = yvelo * (0.003 / abs(vec_path[2]))
height = height + vec[0, 2] * 4
# create the data-signal, which is read to the LUA-script
data = [xvelo, yvelo, height, 0, 0, veloz]
packedData = vrep.simxPackFloats(data)
vrep.simxClearStringSignal(clientID, "Command_Twist_Quad", vrep.simx_opmode_oneshot)
vrep.simxSetStringSignal(clientID, "Command_Twist_Quad", packedData, vrep.simx_opmode_oneshot)
# trigger the next simulation step
vrep.simxSynchronousTrigger(clientID)
# clean up some signals
data = [0, 0, pathz[(len(pathx) - 1)], 0, 0, 0]
packedData = vrep.simxPackFloats(data)
vrep.simxClearStringSignal(clientID, "Command_Twist_Quad", vrep.simx_opmode_oneshot)
vrep.simxSetStringSignal(clientID, "Command_Twist_Quad", packedData, vrep.simx_opmode_oneshot)
data2 = [0]
packedData2 = vrep.simxPackFloats(data2)
vrep.simxClearStringSignal(clientID, "Command_Path_Ready", vrep.simx_opmode_oneshot)
vrep.simxSetStringSignal(clientID, "Command_Path_Ready", packedData2, vrep.simx_opmode_oneshot)
# prepare some data for the plot of the path
xyzparray = np.ndarray(shape=(3, len(xp)), dtype=float)
for next in range(len(xp)):
xyzparray[0, next] = xp[next]
xyzparray[1, next] = yp[next]
xyzparray[2, next] = zp[next]
xyzneararray = np.ndarray(shape=(3, len(xpnear)), dtype=float)
for next in range(len(xpnear)):
xyzneararray[0, next] = xpnear[next]
xyzneararray[1, next] = ypnear[next]
xyzneararray[2, next] = zpnear[next]
# return plot data
return xyzparray, xyzneararray
def __set(self, action):
self.action = np.clip(action, self.action_space.low,
self.action_space.high)
vrep.simxSetStringSignal(self.__ID, "actions",
vrep.simxPackFloats(self.action),
vrep.simx_opmode_oneshot)
def run_simulation(self, trajectory):
"""
Trajectory is a list 6 pairs of vectors, each of the same length.
For each pair:
1. The first vector is the position of the motor in rad.
2. The second vector is the max velocity of the motor in rad/s.
"""
self._com(remote_api.simxStopSimulation, get=False)
if self.verbose:
print("Setting parameters...")
traj = self._prepare_traj(trajectory)
packed_data = remote_api.simxPackFloats(traj)
raw_bytes = (ctypes.c_ubyte * len(packed_data)).from_buffer_copy(packed_data)
assert remote_api.simxSetStringSignal(self.api_id, 'trajectory', raw_bytes,
remote_api.simx_opmode_oneshot_wait) == 0
self._com(remote_api.simxSetIntegerSignal, 'state', 1, get=False)
self._com(remote_api.simxGetIntegerSignal, 'state', get=True,
mode=remote_api.simx_opmode_streaming, ret_ok=(0, 1))
time.sleep(0.1)
self._com(remote_api.simxStartSimulation, get=False)
if self.verbose:
print("Simulation started.")
time.sleep(0.01)
start_time = time.time()
while not self.simulation_paused():
if time.time() - start_time > 60.0:
print("Simulation seems hung. Exiting...")
sys.exit(1)
time.sleep(0.005)
time.sleep(1.0)
if self.verbose:
print("Getting resulting parameters.")
object_sensors = self._get_signal('object_sensors')
collide_data = self._get_signal('collide_data')
contact_data = self._get_signal('contact_data')
contact_type = self._get_signal('contact_type', int_type=True)
contacts = []
for i in range(0, len(contact_type), 3):
step = contact_type[i]
obj_names = [self.handles[contact_type[i+1]],self.handles[contact_type[i+2]]]
data = contact_data[2*i:2*i+6]
contacts.append(Contact(step, obj_names, data))
first_c, last_c = None, None
max_f, max_c = 0.0, None
for c in contacts:
if c.obj_names[0] in self.tracked_objects or c.obj_names[1] in self.tracked_objects:
if first_c is None:
first_c = c
last_c = c
if max_f < c.force_norm:
max_f, max_c = c.force_norm, c
salient_contacts = {'first': first_c, 'last': last_c, 'max': max_c}
if first_c is not None:
print('first contact: {} {:.2f} N'.format('|'.join(first_c.obj_names), first_c.force_norm))
if max_c is not None:
print(' max contact: {} {:.2f} N'.format('|'.join(max_c.obj_names), max_c.force_norm))
marker_sensors = None
if self.cfg.sprims.tip:
marker_sensors = np.array(remote_api.simxUnpackFloats(
remote_api.simxGetStringSignal(self.api_id, 'marker_sensors',
remote_api.simx_opmode_oneshot_wait)))
# assert len(positions) == len(quaternions) == len(velocities)
if self.verbose:
print("End of simulation.")
joint_sensors = None
return {'object_sensors' : object_sensors,
'joint_sensors' : joint_sensors,
'marker_sensors' : marker_sensors,
'collide_data' : collide_data,
'contacts' : contacts,
'salient_contacts': salient_contacts}
def setUtterance(self, utterance):
raw_ubytes = (ctypes.c_ubyte * len(utterance)).from_buffer_copy(utterance)
vrep.simxSetStringSignal(self.__clientID, self.__name + ":Utterance", raw_ubytes, vrep.simx_opmode_oneshot)
# ******************************** Your robot control code goes here ******************************** #
time.sleep(1)
pi = np.pi
Goal_joint_angles = np.array([-pi/2,0.,0.,0.,0.,pi/2])
SetJointPosition(Goal_joint_angles)
SetJointPosition(np.array([0.,0.,0.,0.,0.,pi/2]))
# Wait two seconds
time.sleep(3)
# *************************************************************************************************** #
command = np.array([0,0,0,0,0,0])
#close the Jaco hand at first#
vrep.simxSetStringSignal(clientID,'jacoHand','false',vrep.simx_opmode_oneshot)
j=0
# Decision making
hasItem = 0
while(hasItem == 0):
if (JacoHandHasItem()):
hasItem = 1
time.sleep(3)
JacoHandGrasp()
time.sleep(1)
SetJointPosition(np.array([-pi/2,0.,0.,0.,0.,pi/2]))
# JacoHandRelease()
break
print("No item, wait!")
time.sleep(1)
def JacoHandGrasp():
vrep.simxSetStringSignal(clientID,'jacoHand','true',vrep.simx_opmode_oneshot)
time.sleep(1)
print("Try to grasp.")
return
def controller_motor(cid, copter, targHandle):
global firstPass
global pParam
global iParam
global dParam
global vParam
global cumul
global lastE
global pAlphaE
global pBetaE
global psp2
global psp1
global prevEuler
global firstPass
global particlesTargetVelocities
global propellerScripts
if (firstPass):
propellerScripts=[-1,-1,-1,-1]
for i in range(4):
propellerScripts[i]= vrep.simxGetObjectHandle(cid,'Quadricopter_propeller_respondable'+str(i)+str(1),mode())
particlesTargetVelocities = [0]*4
pParam=.8
iParam=0
dParam=0
vParam= .6
cumul=0
lastE=0
pAlphaE=0
pBetaE=0
psp2=0
psp1=0
prevEuler=0
print("OMG FIRSTPASS")
print("PREV EULER")
print(prevEuler)
#Vertical control:
#d = vrep.simxGetObjectHandle(cid,'Quadricopter_base',mode())[1]
#targetPos=simGetObjectPosition(targetObj,-1)
targetPos = vrep.simxGetObjectPosition(cid,targHandle,-1,mode())[1]
print("Target_Pos IS")
print(targetPos)
#pos = simGetObjectPosition(d,-1)
pos = vrep.simxGetObjectPosition(cid, copter, -1, mode())[1]
l=vrep.simxGetObjectVelocity(cid, copter, mode())[1]
print("l is:{}".format(l))
e=(targetPos[2]-pos[2])
cumul=cumul+e
pv=pParam*e
#thrust=5.335+pv+iParam*cumul+dParam*(e-lastE)+l[2]*vParam
lastE=e
thrust =5.335+ pParam * e +vParam *(0- l[2])
print("THIS IS THRUST" + str(thrust))
#Horizontal control:
#sp=simGetObjectPosition(targetObj,d)
sp = vrep.simxGetObjectPosition(cid,targHandle,copter,mode())[1]
m=simGetObjectMatrix(cid,copter,-1)
#m = vrep.simxGetJointMatrix(cid,d,mode())
print(m)
vx=np.array([1,0,0,1], dtype = np.float64)
vx = np.reshape(vx,(4,1))
#vx=simMultiplyVector(m,vx)
print(m.shape)
print(m)
print(vx.shape)
print(vx)
vx = np.dot(m,vx)
print("CHECK KEK")
print(m.shape)
print(m)
print(vx.shape)
print(vx)
vy = np.array([0,1,0,1],dtype = np.float64)
vy = np.reshape(vy,(4,1))
vy=np.dot(m,vy)
m = m.flatten()
alphaE=(vy[2]-m[11])
alphaCorr=0.25*alphaE+2.1*(alphaE-pAlphaE)
betaE=(vx[2]-m[11])
betaCorr=-0.25*betaE-2.1*(betaE-pBetaE)
pAlphaE=alphaE
pBetaE=betaE
alphaCorr=alphaCorr+sp[1]*0.005+1*(sp[1]-psp2)
betaCorr=betaCorr-sp[0]*0.005-1*(sp[0]-psp1)
psp2=sp[1]
psp1=sp[0]
#-- Rotational control:
#euler=simGetObjectOrientation(d,targetObj)
euler = vrep.simxGetObjectOrientation(cid,copter,targHandle,mode())[1]
rotCorr=euler[2]*0.1+2*(euler[2]-prevEuler)
prevEuler=euler[2]
#-- Decide of the motor velocities:
particlesTargetVelocities[0]=thrust*(1-alphaCorr+betaCorr+rotCorr)
particlesTargetVelocities[1]=thrust*(1-alphaCorr-betaCorr-rotCorr)
particlesTargetVelocities[2]=thrust*(1+alphaCorr-betaCorr+rotCorr)
particlesTargetVelocities[3]=thrust*(1+alphaCorr+betaCorr-rotCorr)
#-- Send the desired motor velocities to the 4 rotors:
packedData = vrep.simxPackFloats([x for x in particlesTargetVelocities])
#packedData = vrep.simxPackFloats([100,100,100,100])
raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(cid, "rotorTargetVelocities",
packedData,
vrep.simx_opmode_oneshot)
firstPass = False
return particlesTargetVelocities
def JacoHandRelease():
vrep.simxSetStringSignal(clientID,'jacoHand','false',vrep.simx_opmode_oneshot)
time.sleep(1)
print("Release item.")
return
#!/usr/bin/env python
import vrep
import time
vrep.simxFinish(-1)
#clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
gripper_handler = 'JacoHand'
vrep.simxSetStringSignal(clientID, gripper_handler, 'true',
vrep.simx_opmode_oneshot)
time.sleep(0.6)
vrep.simxSetStringSignal(clientID, gripper_handler, 'false',
vrep.simx_opmode_oneshot)
time.sleep(0.6)
if clientID != -1:
print('Connected to remote API server')
res, v0 = vrep.simxGetObjectHandle(clientID, 'robot_view',
vrep.simx_opmode_oneshot_wait)
res, v1 = vrep.simxGetObjectHandle(clientID, 'passive_sensor',
vrep.simx_opmode_oneshot_wait)
err, resolution, image = vrep.simxGetVisionSensorImage(
clientID, v0, 0, vrep.simx_opmode_streaming_split)
time.sleep(1)
while vrep.simxGetConnectionId(clientID) != -1:
err, resolution, image = vrep.simxGetVisionSensorImage(
h, Xside, Yside, vel = cui.setStuff()
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 15000, 5)
if clientID != -1:
print('| Connected to AUV |')
res, auv = vrep.simxGetObjectHandle(clientID, "Sphere",
vrep.simx_opmode_oneshot_wait)
# создание объектов классов
path = PP(clientID, auv, vel)
camera = cam(clientID, "Vision_sensor")
# отправка нулевых сил
forceD = [0, 0, 0, 0]
s_force = vrep.simxPackFloats(forceD)
vrep.simxSetStringSignal(clientID, "ForceD", s_force, vrep.simx_opmode_oneshot)
# планировщик маршрута
path.setPolygon(h, Xside, Yside, vel)
path.makePath()
# ========== начало работы =============================================================================================
time.sleep(1)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
print("| start |")
print("| |")
while True:
path.stateOfMove()
camera.stateOfSearch(path.returnCoord(), auv)
time.sleep(0.05)
def followPath2(clientID,path,goal):
pathx=path[0]
pathy=path[1]
pathz=path[2]
errorCode,UAV=vrep.simxGetObjectHandle(clientID,'UAV',vrep.simx_opmode_oneshot_wait)
errorCode,pos=vrep.simxGetObjectPosition(clientID,UAV,-1,vrep.simx_opmode_streaming)
errorCode,orientation=vrep.simxGetObjectOrientation(clientID,UAV,-1,vrep.simx_opmode_streaming)
time.sleep(0.1)
errorCode,pos=vrep.simxGetObjectPosition(clientID,UAV,-1,vrep.simx_opmode_buffer)
errorCode,orientation=vrep.simxGetObjectOrientation(clientID,UAV,-1,vrep.simx_opmode_buffer)
xPosition=pos[0]
yPosition=pos[1]
zPosition=pos[2]
vecp=[0,0,0]
pdangle=0
pveloz=0
pangle=0
pref_angz=0
while (xPosition > pathx[199]+0.1) or (xPosition < pathx[199]-0.1) or (yPosition > pathy[199]+0.1) or (yPosition < pathy[199]-0.1) or (zPosition > pathz[199]+0.1) or (zPosition < pathz[199]-0.1):
xvelomax=0.6
yvelomax=0.6
zmax=1
absolut_dis=math.sqrt((xPosition-pathx[199])**2+(yPosition-pathy[199])**2+(zPosition-pathz[199])**2)
#print absolut_dis
slowvelo_dis=4
if absolut_dis < slowvelo_dis:
xvelomax=xvelomax*absolut_dis/slowvelo_dis
yvelomax=yvelomax*absolut_dis/slowvelo_dis
#zmax=zmax*absolut_dis/slowvelo_dis
start_time = time.time()
errorCode,pos=vrep.simxGetObjectPosition(clientID,UAV,-1,vrep.simx_opmode_buffer)
errorCode,orientation=vrep.simxGetObjectOrientation(clientID,UAV,-1,vrep.simx_opmode_buffer)
xPosition=pos[0]
yPosition=pos[1]
zPosition=pos[2]
# Find nearest point
pnear = pathfollowing.find_nearp(pos, path)
vec=pathfollowing.findnearst(pos,path)
absolut=math.sqrt(vec[0]**2+vec[1]**2+vec[2]**2)
xvelo=0
yvelo=0
height=zPosition
xvelo_w=xvelomax*vec[0]/absolut#ref_velx
yvelo_w=yvelomax*vec[1]/absolut#ref_vely
height = pnear[2]
#ref_angz, angle between (1/0/0) and (xvelo/yvelo/0)
a1=[1,0,0]
b1=[xvelo_w,yvelo_w,0]
ref_angz=angle_calculationx(a1,b1)
if orientation[2]<0:
angle=2*np.pi+orientation[2]
else:
angle=orientation[2]
if ref_angz<0:
ref_angz=2*np.pi+ref_angz
dangle=angle-ref_angz
if dangle>np.pi:
veloz=6*(2*np.pi-dangle)/np.pi
else:
if dangle<-np.pi:
veloz=-6*(2*np.pi+dangle)/np.pi
else:
veloz=-6*(dangle)/np.pi
if dangle-pdangle>1:
print (pdangle,dangle)
print (pangle,angle)
print (pref_angz,ref_angz)
print (pveloz,veloz)
pdangle=dangle
pveloz=veloz
pangle=angle
pref_angz=ref_angz
xvelo=xvelo_w*np.cos(-orientation[2])-yvelo_w*np.sin(-orientation[2])
yvelo=xvelo_w*np.sin(-orientation[2])+yvelo_w*np.cos(-orientation[2])
data=[xvelo,yvelo,height,0,0,veloz]
packedData=vrep.simxPackFloats(data)
vrep.simxClearStringSignal(clientID,'Command_Twist_Quad',vrep.simx_opmode_oneshot)
vrep.simxSetStringSignal(clientID,'Command_Twist_Quad',packedData,vrep.simx_opmode_oneshot)
data=[0,0,height,0,0,0]
packedData=vrep.simxPackFloats(data)
vrep.simxClearStringSignal(clientID,'Command_Twist_Quad',vrep.simx_opmode_oneshot)
vrep.simxSetStringSignal(clientID,'Command_Twist_Quad',packedData,vrep.simx_opmode_oneshot)
detection_display.set_data(img)
detection_display.axes.figure.canvas.draw()
cid = vrep.simxStart('127.0.0.1',20002,True,True,5000,5)
if cid != -1:
print ('Connected to V-REP remote API server, client id: %s' % cid)
vrep.simxStartSimulation( cid, vrep.simx_opmode_oneshot )
else:
print ('Failed connecting to V-REP remote API server')
exit(1)
while True:
err, data = vrep.simxGetStringSignal(cid, "dataFromThisTimeStep",
vrep.simx_opmode_oneshot)
err = vrep.simxSetStringSignal(cid, "dataFromThisTimeStep", raw_bytes,
vrep.simx_opmode_oneshot_wait)
l = len(data)
for i in range( int(l/4) ):
b = list(bytearray(data[i*4:i*4+4]))
x_coord = b[0]
y_coord = b[1]
if x_coord > 128:
x_coord -= 128
polarity = 1
else:
polarity = 0
timestamp = (b[3] * 256) + b[2]
# Distinguish between different polarities
image[127-y_coord][127-x_coord] = polarity*127
def __init__( self, max_target_distance=4, noise=False,
noise_std=None, dodging=True,
target_func=None, cid=None
):
# If a cid is specified, assume the connection has already been
# established and should remain open
if cid is None:
vrep.simxFinish(-1) # just in case, close all opened connections
self.cid = vrep.simxStart('127.0.0.1',19997,True,True,5000,5)
else:
self.cid = cid
if self.cid != -1:
print ('Connected to V-REP remote API server, client id: %s' % self.cid)
vrep.simxStartSimulation( self.cid, vrep.simx_opmode_oneshot )
if SYNC:
vrep.simxSynchronous( self.cid, True )
else:
print ('Failed connecting to V-REP remote API server')
self.exit()
err, self.copter = vrep.simxGetObjectHandle(self.cid, "Quadricopter_base",
vrep.simx_opmode_oneshot_wait )
err, self.target = vrep.simxGetObjectHandle(self.cid, "Quadricopter_target",
vrep.simx_opmode_oneshot_wait )
# Reset the motor commands to zero
packedData=vrep.simxPackFloats([0,0,0,0])
raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities",
raw_bytes,
vrep_mode)
self.pos = [0,0,0]
self.pos_err = [0,0,0]
self.t_pos = [0,0,0]
self.lin = [0,0,0]
self.ori = [0,0,0]
self.ori_err = [0,0,0]
self.t_ori = [0,0,0]
self.ang = [0,0,0]
self.count = 0
# Maximum target distance error that can be returned
self.max_target_distance = max_target_distance
# If noise is being modelled
if noise_std is not None:
self.noise = True
else:
self.noise = False
# Standard Deviation of the noise for the 4 state variables
self.noise_std = noise_std
# Overwrite the get_target method if the target is to be controlled by a
# function instead of by V-REP
if target_func is not None:
self.step = 0
self.target_func = target_func
def get_target():
self.t_pos, self.t_ori = self.target_func( self.step )
self.step += 1
self.get_target = get_target
def braiten2a(self):
"Seek light source"
"PARAMETERS"
intens = 100
ambientIntensRatio = 0
attVect = [0,0,1]
HOMEODIR = '/home/stefano/Documents/Projects/Homeostat/Simulator/Python-port/Homeo/'
dataDir = 'SimsData-'+strftime("%Y-%m-%d-%H-%M-%S", localtime(time()))
simsDataDir = os.path.join(HOMEODIR,"SimulationsData",dataDir)
os.mkdir(simsDataDir)
print "Saving to: ", simsDataDir
e = vrep.simxSetStringSignal(self.simulID,"HOMEO_SIGNAL_SIM_DATA_DIR" ,asByteArray(simsDataDir), vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
print "Message sent, error code: ", e
"END PARAMETERS"
for run in xrange(self.noRuns):
eCode = vrep.simxStartSimulation(self.simulID, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
e = vrep.simxSetStringSignal(self.simulID,"HOMEO_SIGNAL_SIM_DATA_DIR" ,asByteArray(simsDataDir), vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
print "Simulation started: run number %d, error code: %d"% (run+1, eCode)
"Wait until simulation is ready, otherwise we will miss a few movement commands"
# sleep(2)
np.random.seed(64)
# resetRobotInitPose(initPose, self.simulID, ePuckHandle)
eCode = vrep.simxSetStringSignal(self.simulID, self.trajStateSignalName, asByteArray("NEWFILE"), vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
if eCode == 0:
print "Starting a new trajectory file"
else:
print "ERROR: Could not start a new trajectory file"
timeStart = time()
for step in xrange(self.noSteps):
rightLight = vrep.simxGetFloatSignal(self.simulID, "HOMEO_SIGNAL_rightEye_LIGHT_READING", vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
leftLight = vrep.simxGetFloatSignal(self.simulID, "HOMEO_SIGNAL_leftEye_LIGHT_READING", vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
# print "rightLight %.3f\t left light: %.3f" %(rightLight[1],leftLight[1])
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.rightMotor, clip(leftLight[1],0,self.maxSpeed), vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.leftMotor, clip(rightLight[1],0, self.maxSpeed), vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
sleep(0)
timeElapsed = time() - timeStart
"Stop the robot"
self.stopRobot(self.simulID, [self.rightMotor, self.leftMotor])
eCode = vrep.simxSetStringSignal(self.simulID, self.trajStateSignalName, asByteArray("SAVE"), vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
if eCode == 0:
print "Saving trajectory file"
else:
print "ERROR: Could not save a new trajectory file"
sleep(.5)
robotPose = vrep.simxGetObjectPosition(self.simulID, self.robotHandle, -1, vrep.simx_opmode_oneshot_wait)[1][:2]
vrep.simxSynchronousTrigger(self.simulID)
print "%d: Robot is at: %.3f, %.3f Distance from target is: %.4f. Run took exactly %.3f seconds" % (run,
robotPose[0],
robotPose[1],
self.computeDistance(self.targetPose, robotPose),
timeElapsed) #
eCode = vrep.simxStopSimulation(self.simulID, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
sleep(1)
# eCode = vrep.simxStartSimulation(self.simulID, vrep.simx_opmode_oneshot_wait)
# vrep.simxSynchronousTrigger(self.simulID)
eCode = vrep.simxSetStringSignal(self.simulID, self.trajStateSignalName, asByteArray("CLOSEFILE"), vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
if eCode == 0:
print "Starting a new trajectory file"
else:
print "ERROR: Could not close a new trajectory file"
print "Done"
#UAV_main.py
import vrep
import UAV_mapgen
import UAV_pathfinding
import UAV_VREP
import numpy as np
import time
from scipy import ndimage
#start Connection to V-REP
vrep.simxFinish(-1) # just in case, close all opened connections
clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to V-REP
data=[0,0,1,0,0,0]
packedData=vrep.simxPackFloats(data)
vrep.simxClearStringSignal(clientID,'Command_Twist_Quad',vrep.simx_opmode_oneshot)
vrep.simxSetStringSignal(clientID,'Command_Twist_Quad',packedData,vrep.simx_opmode_oneshot)
#generate mapdata, load data if mapdata for scene exist
mapdata=UAV_mapgen.mapgen_fast("columns_and_blocks",16,16,10,clientID)
# Extending obstacles boundaries using dilation
mapdata = ndimage.binary_dilation(mapdata).astype(np.int64)
#mapdata[:,:,1] = np.ones_like(mapdata[:,:,1])
#mapdata[:,:,2] = np.ones_like(mapdata[:,:,2])
# Get start and goal data from v-REP
start_position=UAV_VREP.getPosition(clientID,'UAV_target')
goal_position=UAV_VREP.getPosition(clientID,'goal_new')
print ("Start position:", start_position)
print ("Goal position:", goal_position)
def moveRandomly(self):
"Set trajectory data directory and communicate to V-REP"
HOMEODIR = '/home/stefano/Documents/Projects/Homeostat/Simulator/Python-port/Homeo/'
dataDir = 'SimsData-'+strftime("%Y-%m-%d-%H-%M-%S", localtime(time()))
simsDataDir = os.path.join(HOMEODIR,"SimulationsData",dataDir)
os.mkdir(simsDataDir)
print "Saving to: ", simsDataDir
e = vrep.simxSetStringSignal(self.simulID,"HOMEO_SIGNAL_SIM_DATA_DIR" ,asByteArray(simsDataDir), vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
print "Message sent, error code: ", e
for run in xrange(self.noRuns):
eCode = vrep.simxStartSimulation(self.simulID, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
e = vrep.simxSetStringSignal(self.simulID,"HOMEO_SIGNAL_SIM_DATA_DIR" ,asByteArray(simsDataDir), vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
print "Simulation started: run number %d, error code: %d"% (run+1, eCode)
"Wait until simulation is ready, otherwise we will miss a few movement commands"
# sleep(2)
np.random.seed(64)
# resetRobotInitPose(initPose, self.simulID, ePuckHandle)
eCode = vrep.simxSetStringSignal(self.simulID, self.trajStateSignalName, asByteArray("NEWFILE"), vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
if eCode == 0:
print "Starting a new trajectory file"
else:
print "ERROR: Could not start a new trajectory file"
for step in xrange(self.noSteps):
timeStart = time()
rightSpeed = np.random.uniform(self.maxSpeed * 2) # - self.maxSpeed
leftSpeed = np.random.uniform(self.maxSpeed * 2) # -maxSpeed
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.rightMotor, rightSpeed, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.leftMotor, leftSpeed, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
for i in xrange(self.betwCmdDelays):
vrep.simxSynchronousTrigger(self.simulID)
timeElapsed = time() - timeStart
"Stop the robot"
self.stopRobot(self.simulID, [self.rightMotor, self.leftMotor])
eCode = vrep.simxSetStringSignal(self.simulID, self.trajStateSignalName, asByteArray("SAVE"), vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
if eCode == 0:
print "Saving trajectory file"
else:
print "ERROR: Could not save a new trajectory file"
sleep(.5)
robotPose = vrep.simxGetObjectPosition(self.simulID, self.robotHandle, -1, vrep.simx_opmode_oneshot_wait)[1][:2]
vrep.simxSynchronousTrigger(self.simulID)
print "%d: Robot is at: %.3f, %.3f Distance from target is: %.4f. Run took exactly %.3f seconds" % (run,
robotPose[0],
robotPose[1],
self.computeDistance(self.targetPose, robotPose),
timeElapsed) #
eCode = vrep.simxStopSimulation(self.simulID, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
sleep(1)
# eCode = vrep.simxStartSimulation(self.simulID, vrep.simx_opmode_oneshot_wait)
# vrep.simxSynchronousTrigger(self.simulID)
eCode = vrep.simxSetStringSignal(self.simulID, self.trajStateSignalName, asByteArray("CLOSEFILE"), vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
if eCode == 0:
print "Starting a new trajectory file"
else:
print "ERROR: Could not close a new trajectory file"
print "Done"
def __init__(self,
max_target_distance=4,
noise=False,
noise_std=None,
dodging=True,
target_func=None,
cid=None):
# If a cid is specified, assume the connection has already been
# established and should remain open
if cid is None:
vrep.simxFinish(-1) # just in case, close all opened connections
self.cid = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
else:
self.cid = cid
if self.cid != -1:
print('Connected to V-REP remote API server, client id: %s' %
self.cid)
vrep.simxStartSimulation(self.cid, vrep.simx_opmode_oneshot)
if SYNC:
vrep.simxSynchronous(self.cid, True)
else:
print('Failed connecting to V-REP remote API server')
self.exit()
err, self.copter = vrep.simxGetObjectHandle(
self.cid, "Quadricopter_base", vrep.simx_opmode_oneshot_wait)
err, self.target = vrep.simxGetObjectHandle(
self.cid, "Quadricopter_target", vrep.simx_opmode_oneshot_wait)
err, self.laser_signal = vrep.simxReadStringStream(
self.cid, 'laser_data', vrep.simx_opmode_streaming)
err, self.dis_signal = vrep.simxReadStringStream(
self.cid, 'dis_data', vrep.simx_opmode_streaming)
# Reset the motor commands to zero
packedData = vrep.simxPackFloats([0, 0, 0, 0])
raw_bytes = (ctypes.c_ubyte *
len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities",
raw_bytes, vrep_mode)
self.pos = [0, 0, 0]
self.pos_err = [0, 0, 0]
self.t_pos = [0, 0, 0]
self.lin = [0, 0, 0]
self.ori = [0, 0, 0]
self.ori_err = [0, 0, 0]
self.t_ori = [0, 0, 0]
self.ang = [0, 0, 0]
self.count = 0
self.first = True
self.figure = plt.figure()
self.x_min = 999
self.y_min = 999 #min distance to wall
# Maximum target distance error that can be returned
self.max_target_distance = max_target_distance
# If noise is being modelled
if noise_std is not None:
self.noise = True
else:
self.noise = False
# Standard Deviation of the noise for the 4 state variables
self.noise_std = noise_std
def _set_Action(self, action, prefix=""):
vrep.simxSetStringSignal(self.id_, prefix + "actions",
vrep.simxPackFloats(action),
vrep.simx_opmode_oneshot)
