def setEmotionTo(self, emotion):
inputIntegers = []
inputFloats = []
inputStrings = [emotion]
inputBuffer = bytearray()
sim.simxCallScriptFunction(self.clientID, 'Screen', sim.sim_scripttype_childscript, 'setEmotionTo',
inputIntegers, inputFloats, inputStrings, inputBuffer, sim.simx_opmode_blocking)
def moveTiltTo(self, degrees, speed):
inputIntegers = [degrees, speed]
inputFloats = []
inputStrings = []
inputBuffer = bytearray()
sim.simxCallScriptFunction(self.clientID, 'Tilt_Motor', sim.sim_scripttype_childscript, 'moveTiltTo',
inputIntegers, inputFloats, inputStrings, inputBuffer, sim.simx_opmode_blocking)
def resetWheelEncoders(self):
inputIntegers = []
inputFloats = []
inputStrings = []
inputBuffer = bytearray()
sim.simxCallScriptFunction(self.clientID, 'Left_Motor', sim.sim_scripttype_childscript, 'resetWheelEncoders',
inputIntegers, inputFloats, inputStrings, inputBuffer, sim.simx_opmode_blocking)
def moveWheelsByTime(self, rspeed, lspeed, duration):
inputIntegers = [rspeed, lspeed]
inputFloats = [duration]
inputStrings = []
inputBuffer = bytearray()
sim.simxCallScriptFunction(self.clientID, 'Left_Motor', sim.sim_scripttype_childscript, 'moveWheelsByTime',
inputIntegers, inputFloats, inputStrings, inputBuffer, sim.simx_opmode_blocking)
def setLEDColor(self, led, color):
inputIntegers = []
inputFloats = []
inputStrings = [led, color]
inputBuffer = bytearray()
sim.simxCallScriptFunction(self.clientID, 'Back_L', sim.sim_scripttype_childscript, 'setLEDColor',
inputIntegers, inputFloats, inputStrings, inputBuffer, sim.simx_opmode_blocking)
def moveWheelsByDegrees(self, wheel, degrees, speed):
inputIntegers = [degrees, speed]
inputFloats = []
inputStrings = [wheel]
inputBuffer = bytearray()
sim.simxCallScriptFunction(self.clientID, 'Left_Motor', sim.sim_scripttype_childscript, 'moveWheelsByDegrees',
inputIntegers, inputFloats, inputStrings, inputBuffer, sim.simx_opmode_blocking)
def commandMagnetOnOff(self):
if self.craneStatus and self.connectionStatus:
sim.simxCallScriptFunction(self.clientID, 'Base',
sim.sim_scripttype_childscript,
'AtuadorIma', [], [], [], bytearray(),
sim.simx_opmode_blocking)
self.magnetStatus = not self.magnetStatus
return self.magnetStatus
def ptp(self, joints):
ba = bytearray()
res = vrep.simxCallScriptFunction(self.clientId, 'UR10',
vrep.sim_scripttype_childscript,
'setRobotGoal', [], joints, ['ptp'],
ba, vrep.simx_opmode_blocking)
while True: # waiting for result
res = vrep.simxCallScriptFunction(self.clientId, 'UR10',
vrep.sim_scripttype_childscript,
'isReached', [], [], [''], ba,
vrep.simx_opmode_blocking)
if res[1][0] == 1: break
time.sleep(0.1)
def reset(self, is_dynamic=False, do_orientate=True):
"""Сбросить сцену
Объекты сцены приобретают случайное положение в рабочей зоне манипулятора.
Робот становится в начальную конфигурацию. Если do_orientate=True, то первая ось
направляется в сторону объекта, иначе - принимает случайное значение в рабочем диапазоне.
Parameters
----------
is_dynamic : bool, optional
Динамическая ли сцена, by default False
do_orientate : bool, optional
Следует ли повернуть робота в сторону объекта, by default True
"""
# phi - угол сектора, в котором был сгенерирован целевой объект
_, _, phi, _, _ = sim.simxCallScriptFunction(
self.client, SCENE_CONTROLLER_NAME, sim.sim_scripttype_childscript,
RESET_FUNC_NAME, [], [], [], bytearray(), sim.simx_opmode_blocking)
pos = self.default_pos.copy()
if do_orientate:
offset = random.random(
) * 0.2 - 0.1 # небольшое отклонение в зоне видимости объекта
pos[0] = phi[0] + offset
else:
pos[0] = random.random() * (self.joint_ranges[
0, 1] - self.joint_ranges[0, 0]) + self.joint_ranges[0, 0]
if is_dynamic:
self.move(pos)
else:
self.set_position(pos)
def computePath(handle, location):
# import pdb; pdb.set_trace()
initPos, initRot = getAbsolutePose(handle, 'block')
emptyBuff = bytearray()
res, retInts, retFloats, retStrings, retBuffer = sim.simxCallScriptFunction(
clientID, # client
'lumibot', # scriptDescription
sim.sim_scripttype_childscript, # scriptHandleOrType
'computePath', # functionName
[], # ints
[], # floats
[location], # strings
emptyBuff, # buffer
sim.simx_opmode_blocking,
)
# print(res, retInts, retFloats, retStrings, retBuffer)
path = []
for i in range(0, len(retFloats) - 3, 3):
pos = (retFloats[i], retFloats[i + 1], initPos[2])
rot = (initRot[0], initRot[1], retFloats[i + 2])
path.append((pos, rot))
return path, retFloats
def callback(msg):
pc = []
len_msg = len(msg.points)
for i in range(len_msg):
pc.append(msg.points[i].x)
pc.append(msg.points[i].y)
pc.append((msg.points[i].z)) #pc.append((msg.points[i].z+40))
emptyBuff = bytearray()
t = [1, 2, 3]
s = "envio concluido"
returnCode, outInts, outFloats, outStrings, outBuffer = sim.simxCallScriptFunction(
clientID=clientID,
scriptDescription='Dummy',
options=sim.sim_scripttype_childscript,
functionName='callback_PointCloud',
inputInts=[len_msg],
inputFloats=pc,
inputStrings=['Envio Concluido'],
inputBuffer=emptyBuff,
operationMode=sim.simx_opmode_blocking)
def moveArmToJointsPosition(targetPosition):
emptyBuff = bytearray()
res, retInts, target1Pose, retStrings, retBuffer = sim.simxCallScriptFunction(
clientID, 'LBR_iiwa_14_R820', sim.sim_scripttype_childscript,
'commandIIWA', [], targetPosition, [], emptyBuff,
sim.simx_opmode_oneshot_wait)
def setEffector(val):
# acciona el efector final
# val es Int con valor 0 ó 1 para desactivar o activar el actuador final.
res, retInts, retFloats, retStrings, retBuffer = sim.simxCallScriptFunction(
clientID, "suctionPad", sim.sim_scripttype_childscript, "setEffector",
[val], [], [], "", sim.simx_opmode_blocking)
return res
def get_simulation_time_2(self):
# We run a function in a child script in coppeliasim that returns the simulation time
error_code, _, result_float, _, _ = \
sim.simxCallScriptFunction(self.client_id, 'kuka_assembly', sim.sim_scripttype_childscript,
'python_get_sim_time', [], [], '', bytearray(), sim.simx_opmode_blocking)
# We return the simulation time
return result_float[0]
def setPose(client_id, display_name, transform=None, parent_handle=-1):
"""Set the pose of an object in the simulation
# Arguments
transform_display_name: name string to use for the object in the sim scene
transform: 3 cartesian (x, y, z) and 4 quaternion (x, y, z, w) elements, same as sim
parent_handle: -1 is the world frame, any other int should be a sim object handle
"""
if transform is None:
transform = np.array([0., 0., 0., 0., 0., 0., 1.])
# 2. Now create a dummy object at coordinate 0.1,0.2,0.3 with name 'MyDummyName':
empty_buffer = bytearray()
res, ret_ints, ret_floats, ret_strings, ret_buffer = sim.simxCallScriptFunction(
client_id, 'remoteApiCommandServer', sim.sim_scripttype_childscript,
'createDummy_function', [parent_handle], transform, [display_name],
empty_buffer, sim.simx_opmode_blocking)
if res == sim.simx_return_ok:
# display the reply from CoppeliaSim (in this case, the handle of the created dummy)
print('SetPose object name:', display_name, ' handle: ', ret_ints[0],
' transform: ', transform)
else:
print('setPose remote function call failed.')
print(''.join(traceback.format_stack()))
return -1
return ret_ints[0]
def readAccelerationSensor(self):
inputIntegers = []
inputFloats = []
inputStrings = []
inputBuffer = bytearray()
array = sim.simxCallScriptFunction(self.clientID, 'Smartphone_Respondable', sim.sim_scripttype_childscript,
'readAccelerationSensor', inputIntegers, inputFloats, inputStrings,
inputBuffer, sim.simx_opmode_blocking)
return array[2]
def readAllIRSensor(self):
inputIntegers = []
inputFloats = []
inputStrings = []
inputBuffer = bytearray()
array = sim.simxCallScriptFunction(self.clientID, 'IR_Back_C', sim.sim_scripttype_childscript,
'readAllIRSensor', inputIntegers, inputFloats, inputStrings, inputBuffer,
sim.simx_opmode_blocking)
return array[1]
def readTiltPosition(self):
inputIntegers = []
inputFloats = []
inputStrings = []
inputBuffer = bytearray()
array = sim.simxCallScriptFunction(self.clientID, 'Tilt_Motor', sim.sim_scripttype_childscript,
'readTiltPosition', inputIntegers, inputFloats, inputStrings,
inputBuffer, sim.simx_opmode_blocking)
return array[1][0]
def create_robot(client_id: int, x: float, y: float, theta: float):
current_path = os.path.dirname(os.path.abspath(__file__))
model_path = os.path.join(current_path, 'p3dx.ttm')
rc, out_ints, _, _, _ = sim.simxCallScriptFunction(
client_id, 'Maze', sim.sim_scripttype_childscript, 'createRobot', [],
[x, y, theta], [model_path], "", sim.simx_opmode_blocking)
robot_handle = out_ints[0]
return rc, robot_handle
def call_func(name, ints=[], floats=[], strings=[]):
'''
Call a function from the robot.
The functions can be found in the "functiones" file in the scene file
'''
return sim.simxCallScriptFunction(clientID, 'Funciones',
sim.sim_scripttype_childscript, name,
ints, floats, strings,
bytearray('1', 'utf-8'),
sim.simx_opmode_blocking)
def moveTargetFrame(targetPosition, clientID):
"""[summary]
Args:
targetPosition ([type]): [description]
"""
emptyBuff = bytearray()
res, retInts, target1Pose, retStrings, retBuffer = sim.simxCallScriptFunction(
clientID, 'target', sim.sim_scripttype_childscript, 'commandTarget',
[], targetPosition, [], emptyBuff, sim.simx_opmode_oneshot_wait)
print(res, retInts, target1Pose, retStrings, retBuffer)
def setPause(pause):
emptyBuff = bytearray()
res, retInts, retFloats, retStrings, retBuffer = sim.simxCallScriptFunction(
clientID, # client
'Bill', # scriptDescription
sim.sim_scripttype_childscript, # scriptHandleOrType
'setPause', # functionName
[pause], # ints
[], # floats
[], # strings
emptyBuff, # buffer
sim.simx_opmode_blocking,
)
def send_data(maze_array):
"""
Purpose:
---
Sends data to CoppeliaSim via Remote API.
Input Arguments:
---
`maze_array` : [ nested list of lists ]
encoded maze in the form of a 2D array returned by detectMaze() function
Returns:
---
`return_code` : [ integer ]
the return code generated from the call script function remote API
Example call:
---
send_data(maze_array)
NOTE: You might want to study this link to understand simx.callScriptFunction() better
https://www.coppeliarobotics.com/helpFiles/en/remoteApiExtension.htm
"""
global client_id
return_code = -1
############## ADD YOUR CODE HERE ##############
inputInts = []
cnt = 0
for i in range(10):
for j in range(10):
inputInts.append(int(np.binary_repr(maze_array[i][j])))
#print(inputInts[cnt])
cnt = cnt + 1
inputFloats = []
inputStrings = []
inputBuffer = ''
return_code, retInts, retFloats, retStrings, retBuffer = sim.simxCallScriptFunction(
client_id, 'Base', sim.sim_scripttype_customizationscript,
'receiveData', inputInts, inputFloats, inputStrings, inputBuffer,
sim.simx_opmode_blocking)
##################################################
return return_code
def humanStep():
# have the human move one step on its path
emptyBuff = bytearray()
res, retInts, retFloats, retStrings, retBuffer = sim.simxCallScriptFunction(
clientID, # client
'Bill', # scriptDescription
sim.sim_scripttype_childscript, # scriptHandleOrType
'step', # functionName
[], # ints
[], # floats
[], # strings
emptyBuff, # buffer
sim.simx_opmode_blocking,
)
def readSensors():
emptyBuff = bytearray()
res, retInts, retFloats, retStrings, retBuffer = sim.simxCallScriptFunction(
clientID, # client
'lumibot', # scriptDescription
sim.sim_scripttype_childscript, # scriptHandleOrType
'readSensors', # functionName
[], # ints
[], # floats
[], # strings
emptyBuff, # buffer
sim.simx_opmode_blocking,
)
# print(res, retInts, retFloats, retStrings, retBuffer)
return retFloats
def getPosOnPath(curpos):
emptyBuff = bytearray()
res, retInts, retFloats, retStrings, retBuffer = sim.simxCallScriptFunction(
clientID, # client
'Bill', # scriptDescription
sim.sim_scripttype_childscript, # scriptHandleOrType
'getPositionOnPath', # functionName
[], # ints
[curpos], # floats
[], # strings
emptyBuff, # buffer
sim.simx_opmode_blocking,
)
# print(res, retInts, retFloats, retStrings, retBuffer)
return list(retFloats)
def drawLines(client_id,
display_name,
lines,
parent_handle=-1,
transform=None,
debug=FLAGS.csimDebugMode,
operation_mode=sim.simx_opmode_blocking):
"""Create a line in the simulation.
Note that there are currently some quirks with this function. Only one line is accepted,
and sometimes CoppeliaSim fails to delete the object correctly and lines will fail to draw.
In that case you need to close and restart CoppeliaSim.
# Arguments
transform_display_name: name string to use for the object in the sim scene
transform: 3 cartesian (x, y, z) and 4 quaternion (x, y, z, w) elements, same as sim
parent_handle: -1 is the world frame, any other int should be a sim object handle
lines: array of line definitions using two endpoints (x0, y0, z0, x1, y1, z1).
Multiple lines can be defined but there should be 6 entries (two points) per line.
"""
# 2. Now create a dummy object at coordinate 0.1,0.2,0.3 with name 'MyDummyName':
empty_buffer = bytearray()
res, ret_ints, ret_floats, ret_strings, ret_buffer = sim.simxCallScriptFunction(
client_id,
'remoteApiCommandServer',
sim.sim_scripttype_childscript,
'addDrawingObject_function',
[parent_handle, int(lines.size / 6)],
# np.append(transform, lines),
lines,
[display_name],
empty_buffer,
operation_mode)
if res == sim.simx_return_ok:
# display the reply from CoppeliaSim (in this case, the handle of the created dummy)
if debug is not None and 'print_drawLines' in debug:
print('drawLines name:', display_name, ' handle: ', ret_ints[0],
' transform: ', transform)
if transform is not None:
# set the transform for the point cloud
setPose(client_id, display_name, transform, parent_handle)
else:
print('drawLines remote function call failed.')
print(''.join(traceback.format_stack()))
return -1
return ret_ints[0]
def call_remote(self,
function_name,
int_param=None,
float_param=None,
str_param=None,
mode=sim.simx_opmode_blocking):
if str_param is None:
str_param = []
if float_param is None:
float_param = []
if int_param is None:
int_param = []
res, retInts, retFloats, retStrings, retBuffer = sim.simxCallScriptFunction(
self.sim_client, 'remoteApiCommandServer',
sim.sim_scripttype_childscript, function_name, int_param,
float_param, str_param, self.empty_buff, mode)
return res, retInts, retFloats, retStrings, retBuffer
def reset(self):
# reset objects dynamically in lua script
result, collision_state, retFloats, retStrings, retBuffer = sim.simxCallScriptFunction(
self.clientID, "hexapod", sim.sim_scripttype_childscript,
"reset_robot", [], [], [], emptyBuff, blocking)
for link, angle in zip(self.links, [0, -30, 120]):
for handle in link.values():
sim.simxSetJointTargetPosition(self.clientID, handle,
angle * (3.14 / 180), oneshot)
sim.simxSynchronousTrigger(self.clientID)
self.prev_pos = sim.simxGetObjectPosition(self.clientID,
self.hexapod_handle, -1,
buffer)[1]
self.state = np.ones(self.state_size) * 120 * (3.14 / 180)
return self.state # hope coppeliaSim set them properly
def send_data(maze_array):
"""
Purpose:
---
Sends data to CoppeliaSim via Remote API.
Input Arguments:
---
`maze_array` : [ nested list of lists ]
encoded maze in the form of a 2D array returned by detectMaze() function
Returns:
---
`return_code` : [ integer ]
the return code generated from the call script function remote API
Example call:
---
send_data(maze_array)
NOTE: You might want to study this link to understand simx.callScriptFunction() better
https://www.coppeliarobotics.com/helpFiles/en/remoteApiExtension.htm
"""
global client_id
return_code = -1
#####n######### ADD YOUR CODE HERE ##############
maze_array = np.array(maze_array)
inputInts = maze_array.flatten()
inputFloats = []
inputStrings = []
inputBuffer = bytearray()
return_code, retInts, retFloats, retStrings, retBuffer = sim.simxCallScriptFunction(
client_id, 'Base', sim.sim_scripttype_customizationscript,
'receiveData', inputInts, inputFloats, inputStrings, inputBuffer,
sim.simx_opmode_blocking)
# if (return_code == sim.simx_return_ok):
# print(return_code,retInts,retFloats,retStrings,retBuffer)
##################################################
return return_code
