def make_simulation_synchronous(self, sync):
if not self.sim_running:
print(
'(vrepper) simulation doesn\'t seem to be running. starting up'
)
self.start_simulation(sync)
else:
check_ret(self.simxSynchronous(sync))
def load_scene(self, fullpathname):
print('(vrepper) loading scene from', fullpathname)
try:
check_ret(
self.simxLoadScene(
fullpathname,
0, # assume file is at server side
blocking))
except:
print('(vrepper) scene loading failure')
raise
print('(vrepper) scene successfully loaded')
def call_script_function(self,
function_name,
params,
script_name="remoteApiCommandServer"):
"""
Calls a function in a script that is mounted as child in the scene
:param str script_name: the name of the script that contains the function
:param str function_name: the name of the function to call
:param tuple params: the parameters to call the function with (must be 4 parameters: list of integers, list of floats, list of string, and bytearray
:returns: tuple (res_ints, res_floats, res_strs, res_bytes)
WHERE
list res_ints is a list of integer results
list res_floats is a list of floating point results
list res_strs is a list of string results
bytearray res_bytes is a bytearray containing the resulting bytes
"""
assert type(params) is tuple
assert len(params) == 4
return check_ret(
self.simxCallScriptFunction(
script_name,
sim_scripttype_childscript,
function_name,
params[0], # integers
params[1], # floats
params[2], # strings
params[3], # bytes
blocking))
def get_position(self, relative_to=None, operation_mode="blocking"):
position, = check_ret(
self.env.simxGetObjectPosition(
self.handle, -1 if relative_to is None else relative_to.handle,
blocking
if operation_mode == "blocking" else simx_opmode_buffer))
return position
def get_joint_velocity(self):
self._check_joint()
vel = check_ret(
self.env.simxGetObjectFloatParameter(self.handle,
sim_jointfloatparam_velocity,
blocking))
return vel
def read_force_sensor(self):
state, forceVector, torqueVector = check_ret(
self.env.simxReadForceSensor(self.handle, blocking))
if state & 1 == 1:
return None # sensor data not ready
else:
return forceVector, torqueVector
def set_position(self, position):
"""
Set object to specific position (should never be done with joints)
:param pos: tuple or list with 3 coordinates
:return: None
"""
return check_ret(
self.env.simxSetObjectPosition(self.handle, -1, position,
blocking))
def check_collision(self, collision_handle):
""" At any point in time call this function to get a boolean value if the
collision object is currently detecting a collision. True for collision.
:param collision_handle: integer, the handle that you obtaind from
"get_collision_handle(name_of_collision_obj)"
:return: boolean
"""
return check_ret(self.simxReadCollision(collision_handle, blocking))[0]
def force_position(self, angle):
"""
Force desired position of a servo
:param int angle: target servo angle in degrees
:return: None if successful, otherwise raises exception
"""
self._check_joint()
return check_ret(
self.env.simxSetJointPosition(self.handle, -np.deg2rad(angle),
blocking))
def get_vision_image(self):
resolution, image = check_ret(
self.env.simxGetVisionSensorImage(
self.handle,
0, # options=0 -> RGB
blocking,
))
dim, im = resolution, image
nim = np.array(im, dtype='uint8')
nim = np.reshape(nim, (dim[1], dim[0], 3))
nim = np.flip(nim, 0) # LR flip
nim = np.flip(nim, 2) # RGB -> BGR
return nim
def get_collision_object(self, name_of_collision_obj):
""" this is effectively the same as "get_collision_handle" but instead of an
integer (the handle) it instead returns an object that has a ".is_colliding()"
function, which is super marginally more convenient.
:param name_of_collision_obj: string, name of the collision object in V-Rep
:return: Collision object that you can check with ".is_colliding()->bool"
"""
handle = check_ret(
self.simxGetCollisionHandle(name_of_collision_obj + "#",
blocking))[0]
col = Collision(env=self, handle=handle)
return col
def start(self):
if self.started == True:
raise RuntimeError('you should not call start() more than once')
print('(vrepper) starting an instance of V-REP...')
self.instance.start()
# try to connect to V-REP instance via socket
retries = 0
while True:
print('(vrepper) trying to connect to server on port',
self.port_num, 'retry:', retries)
# vrep.simxFinish(-1) # just in case, close all opened connections
self.cid = self.simxStart(
'127.0.0.1',
self.port_num,
waitUntilConnected=True,
doNotReconnectOnceDisconnected=True,
timeOutInMs=1000,
commThreadCycleInMs=0) # Connect to V-REP
if self.cid != -1:
print('(vrepper) Connected to remote API server!')
break
else:
retries += 1
if retries > 15:
self.end()
raise RuntimeError(
'(vrepper) Unable to connect to V-REP after 15 retries.'
)
# Now try to retrieve data in a blocking fashion (i.e. a service call):
objs, = check_ret(self.simxGetObjects(sim_handle_all, blocking))
print('(vrepper) Number of objects in the scene: ', len(objs))
# Now send some data to V-REP in a non-blocking fashion:
self.simxAddStatusbarMessage('(vrepper)Hello V-REP!', oneshot)
# setup a useless signal
self.simxSetIntegerSignal('asdf', 1, blocking)
print(
'(vrepper) V-REP instance started, remote API connection created. Everything seems to be ready.'
)
self.started = True
return self
def get_collision_handle(self, name_of_collision_obj):
""" In order to use this you first have to open the scene in V-Rep, then
click on "calculation module properties" on the left side (the button
that looks like "f(x)"), then click "add new collision object", chose the
two things between which you want to check for collision (one of them can be a collection
which you can create in yet another window), and finally double click on the new
collision object in order to rename it to something more catchy than "Collision".
You can find more information here:
http://www.coppeliarobotics.com/helpFiles/en/collisionDetection.htm
Also don't forget to save the scene after adding the collision object.
:param name_of_collision_obj: the "#" is added automatically at the end
:return: collision_handle (this is an integer that you need for check_collision)
"""
return check_ret(
self.simxGetCollisionHandle(name_of_collision_obj + "#",
blocking))[0]
def start_simulation(self, is_sync):
# IMPORTANT
# you should poll the server state to make sure
# the simulation completely stops before starting a new one
while True:
# poll the useless signal (to receive a message from server)
check_ret(self.simxGetIntegerSignal('asdf', blocking))
# check server state (within the received message)
e = self.simxGetInMessageInfo(simx_headeroffset_server_state)
# check bit0
not_stopped = e[1] & 1
if not not_stopped:
break
# enter sync mode
check_ret(self.simxSynchronous(is_sync))
check_ret(self.simxStartSimulation(blocking))
self.sim_running = True
def get_position(self, relative_to=None):
position, = check_ret(
self.env.simxGetObjectPosition(
self.handle, -1 if relative_to is None else relative_to.handle,
blocking))
return position
def get_joint_force(self):
self._check_joint()
force = check_ret(self.env.simxGetJointForce(self.handle, blocking))
return force
def get_joint_angle(self):
self._check_joint()
angle = check_ret(self.env.simxGetJointPosition(self.handle, blocking))
return -np.rad2deg(angle[0])
def stop_simulation(self):
check_ret(self.simxStopSimulation(oneshot), ignore_one=True)
self.sim_running = False
def set_force(self, f):
self._check_joint()
return check_ret(self.env.simxSetJointForce(self.handle, f, blocking))
def set_velocity(self, v):
self._check_joint()
return check_ret(
self.env.simxSetJointTargetVelocity(self.handle, v, blocking))
def get_velocity(self):
return check_ret(
self.env.simxGetObjectVelocity(
self.handle,
# -1 if relative_to is None else relative_to.handle,
blocking))
def step_blocking_simulation(self):
check_ret(self.simxSynchronousTrigger())
def get_object_handle(self, name):
handle, = check_ret(self.simxGetObjectHandle(name, blocking))
return handle
def get_orientation(self, relative_to=None):
eulerAngles, = check_ret(
self.env.simxGetObjectOrientation(
self.handle, -1 if relative_to is None else relative_to.handle,
blocking))
return eulerAngles
def get_image(self, object_id):
res, img = check_ret(
self.simxGetVisionSensorImage(object_id, 0, blocking))
return self._convert_byte_image_to_color(res, img)
def is_colliding(self):
return check_ret(self.env.simxReadCollision(self.handle, blocking))[0]
def get_depth_image_as_rgb(self, object_id):
res, depth = check_ret(
self.simxGetVisionSensorDepthBuffer(object_id, blocking))
return self._convert_depth_to_rgb(res, depth)
