def getJointRawAngles():
returnCode, _, _, floatData, _ = vrep.simxGetObjectGroupData(
clientID, jointsCollectionHandle, 15, vrep.simx_opmode_blocking
) # or simx_opmode_blocking (not recommended)
jointPositions = np.array(
floatData[0::2]
) # take elements at odd positions (even correspond to torques)
return jointPositions
def get_object_data(client_id, object_type, data_type):
"""
Get V-REP object group data by type
"""
res, handles, int_data, float_data, string_data = vrep.simxGetObjectGroupData(
client_id, object_type, data_type, vrep.simx_opmode_blocking)
if res == vrep.simx_return_ok:
return handles, int_data, float_data, string_data
def get_object_name(clientID, objectHandle):
ret, stringData = vrep.simxGetObjectGroupData(
clientID=clientID,
objectType=vrep.simx_appobj_object_type,
dataType=0, # object name
operationMode=vrep.simx_opmode_oneshot_wait)
check_return_ok(ret)
return stringData[0]
def printJointPositions(self, clientID):
error, handlers, intData, floatData, stringData=vrep.simxGetObjectGroupData(clientID,vrep.sim_appobj_object_type,0,vrep.simx_opmode_oneshot_wait)
itemHandle=0
print stringData
for name in stringData:
error, position = vrep.simxGetJointPosition(clientID,handlers[itemHandle], vrep.simx_opmode_streaming)
itemHandle=itemHandle+1
print name + ":" + str(position)
def GetObjectGroupData(self, object_name, parameter): #does not work
if self.clientID == None:
print("Robot not attached to VREP environment")
return -1
else:
return vrep.simxGetObjectGroupData(self.clientID,
object_name,
parameter,
operationMode=self.opmode)
def _get_info_about_joint(self, handle):
obj_type_code = vc.sim_object_joint_type
data_type_code = 16
code, handles, types_and_mode, limits_and_ranges, string_data = v.simxGetObjectGroupData(
self._id, obj_type_code, data_type_code, self._def_op_mode)
if handles.__contains__(handle):
return types_and_mode[0], types_and_mode[1], limits_and_ranges[
0], limits_and_ranges[1]
else:
return None
def getJointRawAngles(self):
# NOTE: default initial state: 180 degrees (=pi radians) for all angles
# update joint angles, normalize to ]0,1]
returnCode, _, _, floatData, _ = vrep.simxGetObjectGroupData(
self.clientID, self.jointsCollectionHandle, 15, vrep.
simx_opmode_blocking) # or simx_opmode_blocking (not recommended)
jointPositions = np.array(
floatData[0::2]
) # take elements at odd positions (even correspond to torques)
return jointPositions
def setup(self):
if self.clientID != -1:
errorCode, handles, intData, floatData, array = vrep.simxGetObjectGroupData(self.clientID,
vrep.sim_appobj_object_type,
0,
vrep.simx_opmode_oneshot_wait)
data = dict(zip(array, handles))
self.camera = [value for key, value in data.items() if "Vision_sensor" in key][0]
self.target = [value for key, value in data.items() if "Quadricopter_target" == key][0]
vrep.simxGetVisionSensorImage(self.clientID, self.camera, 1, vrep.simx_opmode_streaming)
def readAllSensors(self, numberOfSensorsToRead = 16, proximityValueForUnactivatedSensors = 1.0):
s = vrep.simxGetObjectGroupData(self.clientID, vrep.sim_object_proximitysensor_type, 13, vrep.simx_opmode_blocking)
proximity = []
for i in range(numberOfSensorsToRead):
if s[2][2 * i] == 1:
proximity.append(s[3][6 * i + 2])
else:
proximity.append(proximityValueForUnactivatedSensors)
return proximity
def __update_all_object_positions(self):
"""Invokes API in V-REP environment to fetch position of all objects in the scene.
"""
time.sleep(self.sleep_sec)
err, handles, ints, floats, strings = vrep.simxGetObjectGroupData(
self.clientID, vrep.sim_appobj_object_type, 3,
vrep.simx_opmode_blocking)
positions = np.array(floats)
self.objects = handles
self.object_positions = positions.reshape(len(handles), 3)
def __update_all_object_positions(self):
"""Invokes API in V-REP environment to fetch position of all objects in the scene.
"""
time.sleep(self.sleep_sec)
err, handles, ints, floats, strings = vrep.simxGetObjectGroupData(self.clientID,
vrep.sim_appobj_object_type, 3,
vrep.simx_opmode_blocking)
positions = np.array(floats)
self.objects = handles
self.object_positions = positions.reshape(len(handles), 3)
def GetJointInfo(self):
'''
Reference
http://www.coppeliarobotics.com/helpFiles/en/jointDescription.htm
'''
joint_mode = [
'nth', 'Passive', 'IK', 'Dependent', 'Motion', 'Torque/force'
]
joint_type = {10: 'Revolute', 11: 'Prismatic'}
obj_type_code = 1
'''
data_type = 0:
retrieves the object names (in stringData.)
'''
code, handles, a, b, obj_name = vrep.simxGetObjectGroupData(
self.ID, obj_type_code, 0, vrep.simx_opmode_oneshot_wait)
'''data_type = 16:
retrieves joint properties data
(in intData (2 values): joint type, joint mode (bit16=hybid operation).
In floatData (2 values): joint limit low, joint range (-1.0 if joint is cyclic))
'''
code, handles, types_and_mode, limits_and_ranges, string_data = vrep.simxGetObjectGroupData(
self.ID, obj_type_code, 16, vrep.simx_opmode_oneshot_wait)
types = [types_and_mode[x] for x in range(0, len(types_and_mode), 2)]
mode = [types_and_mode[x] for x in range(1, len(types_and_mode), 2)]
lower_limit = [
limits_and_ranges[x] for x in range(0, len(limits_and_ranges), 2)
]
upper_limit = [
limits_and_ranges[x] for x in range(1, len(limits_and_ranges), 2)
]
t = PrettyTable([
'Object Name', 'Joint Types', 'Joint Modes', 'Lower Limit',
'Upper Limit'
])
for i in range(len(obj_name)):
t.add_row([
obj_name[i], joint_type[types[i]], joint_mode[mode[i]],
lower_limit[i], upper_limit[i]
])
print t
def get_joints(robot_handle):
'''
This is a very costly function due to recursive blocking calls.
Need to be optimized
normally, when using the regular API, you could use function simGetObjectsInTree. That function is not (yet) implemented in the remote API.
As a workaround, you have several possibilities:
You could modify the remote API in order to support that function. This is rather difficult and not recommended.
You could use function simxGetObjectGroupData. You would retrieve all objects and all parent objects. From there you could rebuild the desired tree hierarchy.
You can use simxGetObjectChild as you mention it, in an iterative way, until you have retrieved all objects in the tree.
You can retrieve (or continuously stream) the tree objects: on the V-REP side, use simGetObjectsInTree. Then pack the returned table in a string signal.
Then send that signal to the remote API client, that will decode the string and retrieve the handles. On the V-REP side, inside of a non-threaded child script:
:param robot_handle:
:return: robot_joints: map of joint handles to their names
'''
try:
return __get_cached(get_joints, robot_handle)
except:
pass
# 0: Return names
return_code, handles, intData, floatData, stringData = vrep.simxGetObjectGroupData(
clientID, vrep.sim_object_joint_type, 0, vrep.simx_opmode_blocking)
handes_name_map = {}
for i, h in enumerate(handles):
handes_name_map[h] = stringData[i]
queue = []
queue.append(robot_handle)
robot_joint_handle_name_map = {}
while len(queue) > 0:
parent_handle = queue.pop(0)
child_handle = 0
child_index = 0
while child_handle != -1:
return_code, child_handle = vrep.simxGetObjectChild(
clientID, parent_handle, child_index,
vrep.simx_opmode_blocking)
__check_resp(return_code, vrep.simx_return_ok)
if child_handle != -1:
child_index = child_index + 1
queue.append(child_handle)
if child_handle in handes_name_map:
robot_joint_handle_name_map[
child_handle] = handes_name_map.get(child_handle)
__cache(get_joints, robot_handle, robot_joint_handle_name_map)
return robot_joint_handle_name_map
def get_all_objects_data(self, name_to_handle):
temp_dict = {}
rc, handles, intData, floatData, stringData = vrep.simxGetObjectGroupData(
self.client_id, vrep.sim_appobj_object_type, 0, blocking)
if name_to_handle:
for i, j in zip(stringData, handles):
temp_dict[i] = j
else:
for i, j in zip(handles, stringData):
temp_dict[i] = j
return temp_dict
def get_names(self):
"""Retrieve names of component scene objects."""
client_id = self.client_id
if client_id is None:
raise ConnectionError(
"Could not retrieve names of {}: not connected to V-REP "
"remote API server.".format(self._name))
res, _, _, _, names = vrep.simxGetObjectGroupData(
client_id, self._handle, 0, vrep.simx_opmode_blocking)
if res != vrep.simx_return_ok:
raise ServerError("Could not retrieve names of {}.".format(
self._name))
return names
def get_maze_segments(wallHandles):
## Get handles to:
# 1. The Maze collection:
res, mazeHandle = vrep.simxGetCollectionHandle(clientID, "Maze",
vrep.simx_opmode_blocking)
# Get the handles associated with each wall and the absolute position of the center of each wall:
res, wallHandles, intData, absPositions, stringData = vrep.simxGetObjectGroupData(
clientID, mazeHandle, 3, vrep.simx_opmode_blocking)
mazeSegments = []
if res == vrep.simx_return_ok:
count = 1
for wall in wallHandles:
res, wallCenterAbsPos = vrep.simxGetObjectPosition(
clientID, wall, -1, vrep.simx_opmode_oneshot_wait)
res, wallMinY = vrep.simxGetObjectFloatParameter(
clientID, wall, vrep.sim_objfloatparam_objbbox_min_y,
vrep.simx_opmode_oneshot_wait)
res, wallMaxY = vrep.simxGetObjectFloatParameter(
clientID, wall, vrep.sim_objfloatparam_objbbox_max_y,
vrep.simx_opmode_oneshot_wait)
wallLength = abs(wallMaxY - wallMinY)
# Get the orientation of the wall: Third euler angle is the angle around z-axis:
res, wallOrient = vrep.simxGetObjectOrientation(
clientID, wall, -1, vrep.simx_opmode_oneshot_wait)
# Get the end points of the maze wall: A list containing two tuples: [ (xs,ys) , (xe,ye)]
wallSeg = get_wall_seg(
wallCenterAbsPos, wallLength, wallOrient[2]
) # Assuming all walls are on the ground and nearly flat:
print("Wall #", count, " -> ", wallSeg)
mazeSegments.append(wallSeg)
count += 1
else:
print(" Failed to get individual wall handles!")
return mazeSegments
def scenesinit(self,port):
vrep.simxFinish(-1)
clientID=vrep.simxStart('127.0.0.1',port,True,True,5000,5)
if clientID!=-1:
_,handles,_,_,stringData=vrep.simxGetObjectGroupData(clientID,vrep.sim_appobj_object_type,0,vrep.simx_opmode_oneshot_wait)
for i in range(len(stringData)):
if stringData[i].find("Cuboid")>=0:
print(stringData[i])
self.obstacles_handles.update({stringData[i]:handles[i]})
_,pos=vrep.simxGetObjectPosition(clientID,handles[i],-1,vrep.simx_opmode_oneshot_wait)
self.obstacles_initpos.update({stringData[i]:pos})
return self.obstacles_handles,self.obstacles_initpos
def read_all_sonar(clientID):
# simxGetObjectGroupData(clientID, objectType, dataType, operationMode)
s = vrep.simxGetObjectGroupData(clientID,
vrep.sim_object_proximitysensor_type, 13,
vrep.simx_opmode_blocking)
r = [1.0] * 16
if s[0] == 0:
for i in range(16):
if s[2][2 * i] == 1:
r[i] = s[3][6 * i + 2]
#print ('### Sonar readings:', r)
#print ('### Sonar readings:', r[1], r[3], r[4], r[6])
return r
def get_positions(self, prec=None):
"""Retrieve positions of component scene objects."""
client_id = self.client_id
if client_id is None:
raise ConnectionError(
"Could not retrieve positions of {}: not connected to V-REP "
"remote API server.".format(self._name))
res, _, _, positions, _ = vrep.simxGetObjectGroupData(
client_id, self._handle, 3, vrep.simx_opmode_blocking)
if res != vrep.simx_return_ok:
raise ServerError("Could not retrieve positions of {}.".format(
self._name))
if prec is not None:
positions = [round(coord, prec) for coord in positions]
return [positions[p:p + 3] for p in range(0, len(positions), 3)]
def get_bodies():
'''
TODO: Not working
Blocking call
:return: List of kinematic bodies
'''
res, h, i, f, s = vrep.simxGetObjectGroupData(clientID,
vrep.sim_object_shape_type,
0, vrep.simx_opmode_blocking)
# res, objs = vrep.simxGetObjects(clientID, vrep.sim_shape_multishape_subtype, vrep.simx_opmode_blocking)
print i
__check_resp(res, vrep.simx_return_ok)
return s
def get_orientations(self, prec=None):
"""Retrieve orientations of component scene objects, specified as Euler
angles about x, y, and z axes of the absolute reference frame, each
angle between -pi and pi.
"""
client_id = self.client_id
if client_id is None:
raise ConnectionError(
"Could not retrieve orientations of {}: not connected to "
"V-REP remote API server.".format(self._name))
res, _, _, orientations, _ = vrep.simxGetObjectGroupData(
client_id, self._handle, 5, vrep.simx_opmode_blocking)
if res != vrep.simx_return_ok:
raise ServerError("Could not retrieve orientations of {}.".format(
self._name))
if prec is not None:
orientations = [round(angle, prec) for angle in orientations]
return [orientations[o:o + 3] for o in range(0, len(orientations), 3)]
def get_data_multiple_objects(self):
rc, handles, intData, floatData, strData = vrep.simxGetObjectGroupData(
self.client_id, vrep.sim_object_joint_type, 15, blocking)
# 15 = returns 2 float values, [position and torques], Can get all sort of data, check documentation.
# print("Handles: " + str(handles))
# print("Int Data: " + str(intData))
# print("Float Data: " + str(floatData))
# print("String Data: " + str(strData))
for i in range(len(floatData)):
print(i)
if i % 2 == 0: # If Even
degrees = math.degrees(floatData[i])
degrees = round(degrees, 3)
print("Position: {}".format(degrees))
else:
torque = round(floatData[i], 3)
print("Torque: " + str(torque))
def sensorApp(clientID, queue, colhad, scene):
_, handles, intD, floatD, stringD = vrep.simxGetObjectGroupData(
clientID, colhad, 13, vrep.simx_opmode_blocking)
detection = np.reshape(intD, (SENSOR_COUNT, 2))[:, 0]
detection = 1 - detection ## 0 to 1 , 1 to 0
detectedPoint = np.reshape(floatD, (SENSOR_COUNT, 6))[:, 0:3]
detDist = np.sum(np.abs(detectedPoint)**2, axis=1)**(1. / 2)
for index in range(0, len(detection)):
if detection[index] == 1:
detDist[index] = scene.max_distance
nomDetDist = 1 / scene.sensor_distance * detDist
sensorData = np.append(nomDetDist, detection)
queue.append(sensorData)
return queue
def initialize_vrep():
print('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
global gbClientID
gbClientID = vrep.simxStart('127.0.0.1', CSERVER_PORT, True, True, 5000,
5) # Connect to V-REP
if gbClientID != -1:
print('Connected to remote API server', gbClientID)
# Init Robot Common
RC.init(gbClientID)
# Start the simulation:
RC.startSimulation(gbClientID)
# Load a robot instance: res,retInts,retFloats,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'remoteApiCommandServer',vrep.sim_scripttype_childscript,'loadRobot',[],[0,0,0,0],['d:/v_rep/qrelease/release/test.ttm'],emptyBuff,vrep.simx_opmode_oneshot_wait)
# robotHandle=retInts[0]
# Get scene objects data
res, objHandles, intData, floatData, objNames = vrep.simxGetObjectGroupData(
gbClientID, vrep.sim_appobj_object_type, 0,
vrep.simx_opmode_blocking)
if res == vrep.simx_return_ok:
print('Number of objects in the scene: ', len(objHandles),
len(objNames))
for i in range(len(objHandles)):
print('Obj:', objHandles[i], objNames[i])
else:
print('Remote API function call returned with error code: ', res)
# Retrieve some handles:
global gbRobotHandle
res, gbRobotHandle = vrep.simxGetObjectHandle(
gbClientID, RC.GB_CSERVER_ROBOT_NAME,
vrep.simx_opmode_oneshot_wait)
res,objs = vrep.simxGetObjects(clientID,vrep.sim_handle_all,vrep.simx_opmode_oneshot_wait)
if res == vrep.simx_return_ok:
print 'Number of objects in the scene: ',len(objs)
print 'Connection to remote API server is open number:', clientID
res = vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
else:
print 'Remote API function call returned with error code: ',res
else:
print 'Failed connecting to remote API server'
res, handles, intData, floatData, objectNames = vrep.simxGetObjectGroupData(clientID, vrep.sim_object_shape_type, 0, vrep.simx_opmode_oneshot_wait)
res, handles, intData, objectPositionsandOrient, stringData = vrep.simxGetObjectGroupData(clientID, vrep.sim_object_shape_type, 9, vrep.simx_opmode_oneshot_wait)
res, kinectHandle = vrep.simxGetObjectHandle(clientID, 'kinect', vrep.simx_opmode_oneshot_wait)
res, cupHandle = vrep.simxGetObjectHandle(clientID, 'Cup_visible_transparent', vrep.simx_opmode_oneshot_wait)
res, cupOrientation = vrep.simxGetObjectOrientation(clientID, cupHandle, kinectHandle, vrep.simx_opmode_streaming)
time.sleep(5) # pause needed for server side communication
res, cupOrientation = vrep.simxGetObjectOrientation(clientID, cupHandle, kinectHandle, vrep.simx_opmode_buffer)
text_output = open("output.txt", "w")
text_output.write("Object handle: \n{} \nObject orientations in Euler (alpha, beta, gamma): \n{}".format(cupHandle, cupOrientation))
text_output.close()
# stop vrep communication after all your work is done
import time
vrep.simxFinish(-1)
client_id = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if client_id != -1:
print('Connected to remote API server')
vrep.simxStartSimulation(client_id, vrep.simx_opmode_blocking)
else:
sys.exit('Failed connecting to remote API server')
# convention of naming joints:
# L - left, R - right
# F - front, M - middle, B - back
# 1 - coxa, 2 - femur, 3 - tibia
group = vrep.simxGetObjectGroupData(client_id, vrepConst.sim_object_joint_type, 0, vrep.simx_opmode_blocking)
names = group[4]
print("Names: ", names)
joints = []
legs = []
for i in range(0, 18):
joints.append(Joint(client_id, names[i]))
for i in range(0, len(joints), 3):
legs.append(Leg(joints[i], joints[i+1], joints[i+2]))
hexapod = Hexapod(legs)
time.sleep(1)
hexapod.body_shift(30)
def _get_object_name_and_handle(self):
"""
# When call vrep.simxGetObjectGroupData to abstract object name and handle
# choose appropriate objectType parameter:
# joint: vrep.sim_object_joint_type
# proximity sensor: vrep.sim_object_proximitysensor_type
# light: vrep.sim_object_light_type
# visitor target position: vrep.sim_object_dummy_type
# visitor body: vrep.sim_object_shape_type
"""
dataType = 0 # 0: retrieves the object names (in stringData.)
print("Get objects' names and handles ...")
# proximity sensor
proxSensorIndex = []
rc = vrep.simx_return_initialize_error_flag
while rc != vrep.simx_return_ok:
rc, proxSensorHandles, intData, floatData, proxSensorNames = vrep.simxGetObjectGroupData(
self.clientID, vrep.sim_object_proximitysensor_type, dataType,
self._def_op_mode)
if rc == vrep.simx_return_ok:
print(
'Get Prox Sensor Success!!!!!'
) # display the reply from V-REP (in this case, just a string)
for i, name in enumerate(proxSensorNames):
if "_node#" in name:
print("Proximity Sensor: {}, and handle: {}".format(
name, proxSensorHandles[i]))
proxSensorIndex.append(i)
break
else:
print('Fail to get proximity sensors!!!')
# light
lightIndex = []
rc = vrep.simx_return_initialize_error_flag
while rc != vrep.simx_return_ok:
rc, lightHandles, intData, floatData, lightNames = vrep.simxGetObjectGroupData(
self.clientID, vrep.sim_object_light_type, dataType,
self._def_op_mode)
if rc == vrep.simx_return_ok:
print(
'Get Lihgt Success!!!!!'
) # display the reply from V-REP (in this case, just a string)
for i, name in enumerate(lightNames):
if "_node#" in name:
print("Light: {}, and handle: {}".format(
name, lightHandles[i]))
lightIndex.append(i)
break
else:
print('Fail to get lights!!!')
# joint
jointIndex = []
rc = vrep.simx_return_initialize_error_flag
while rc != vrep.simx_return_ok:
rc, jointHandles, intData, floatData, jointNames = vrep.simxGetObjectGroupData(
self.clientID, vrep.sim_object_joint_type, dataType,
self._def_op_mode)
if rc == vrep.simx_return_ok:
print(
'Get Joint Success!!!!!'
) # display the reply from V-REP (in this case, just a string)
for i, name in enumerate(jointNames):
if "_node#" in name:
print("Joint: {}, and handle: {}".format(
name, jointHandles[i]))
jointIndex.append(i)
break
else:
print('Fail to get joints!!!')
# visitor targetPosition
visitorIndex = []
rc = vrep.simx_return_initialize_error_flag
while rc != vrep.simx_return_ok:
rc, visitorHandles, intData, floatData, visitorNames = vrep.simxGetObjectGroupData(
self.clientID, vrep.sim_object_dummy_type, dataType,
self._def_op_mode)
if rc == vrep.simx_return_ok:
print(
'Get Visitor Success!!!!!'
) # display the reply from V-REP (in this case, just a string)
for i, name in enumerate(visitorNames):
if "TargetPosition_Visitor#" in name:
print("Visitor: {}, and handle: {}".format(
name, visitorHandles[i]))
visitorIndex.append(i)
break
else:
print('Fail to get visitors!!!')
# visitor body
visitorBodyIndex = []
rc = vrep.simx_return_initialize_error_flag
while rc != vrep.simx_return_ok:
rc, visitorBodyHandles, intData, floatData, visitorBodyNames = vrep.simxGetObjectGroupData(
self.clientID, vrep.sim_object_shape_type, dataType,
self._def_op_mode)
if rc == vrep.simx_return_ok:
print(
'Get Visitor Body Success!!!!!'
) # display the reply from V-REP (in this case, just a string)
for i, name in enumerate(visitorBodyNames):
if "Body_Visitor#" in name:
print("Visitor body: {}, and handle: {}".format(
name, visitorBodyHandles[i]))
visitorBodyIndex.append(i)
break
else:
print('Fail to get visitors body!!!')
proxSensorHandles = np.array(proxSensorHandles)
proxSensorNames = np.array(proxSensorNames)
lightHandles = np.array(lightHandles)
lightNames = np.array(lightNames)
jointHandles = np.array(jointHandles)
jointNames = np.array(jointNames)
visitorHandles = np.array(visitorHandles)
visitorNames = np.array(visitorNames)
visitorBodyHandles = np.array(visitorBodyHandles)
visitorBodyNames = np.array(visitorBodyNames)
# All objects handels and names
self.proxSensorHandles = proxSensorHandles[proxSensorIndex]
self.proxSensorNames = proxSensorNames[proxSensorIndex]
self.lightHandles = lightHandles[lightIndex]
self.lightNames = lightNames[lightIndex]
self.jointHandles = jointHandles[jointIndex]
self.jointNames = jointNames[jointIndex]
self.visitorNames = visitorNames[visitorIndex]
self.visitorHandles = visitorHandles[visitorIndex]
self.visitorBodyNames = visitorBodyNames[visitorBodyIndex]
self.visitorBodyHandles = visitorBodyHandles[visitorBodyIndex]
if child != -1:
children.append(child)
return children
print('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID != -1:
print('Connected to remote API server')
# res, objs = vrep.simxGetObjects(clientID, vrep.sim_handle_all, vrep.simx_opmode_oneshot_wait)
# if res == vrep.simx_return_ok:
# print('Number of objects in the scene: ', len(objs))
# else:
# print('Remote API function call returned with error code: ', res)
errorCode, handles, intData, floatData, array = vrep.simxGetObjectGroupData(clientID,vrep.sim_appobj_object_type,0,vrep.simx_opmode_oneshot_wait)
data = dict(zip(array, handles))
joints = {}
for name in data:
if "joint" in name:
joints[name] = data[name]
print joints
fl_leg = dict((key,value) for key, value in data.iteritems() if "fl" in key)
fr_leg = dict((key,value) for key, value in data.iteritems() if "fr" in key)
rr_leg = dict((key,value) for key, value in data.iteritems() if "rr" in key)
rl_leg = dict((key,value) for key, value in data.iteritems() if "rl" in key)
def raw_getObjectGroupData(self, objectType, dataType, operationMode = vrep.simx_opmode_oneshot_wait):
return vrep.simxGetObjectGroupData(self.clientID, objectType, dataType, operationMode)
print('Connected to remote API server')
#--- Generate Scene graph from simulation ----------
# Now try to retrieve data in a blocking fashion (i.e. a service call):
res, objs = vrep.simxGetObjects(clientID, vrep.sim_handle_all,
vrep.simx_opmode_blocking)
if res == vrep.simx_return_ok:
print('Number of objects in the scene: ', len(objs))
#http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxGetStringParameter
objectType = vrep.sim_appobj_object_type
operationMode = vrep.simx_opmode_blocking
# retrieves the object names
dataType = 0
returnCode, objects_handles, intData, floatData, objects_names = vrep.simxGetObjectGroupData(
clientID, objectType, dataType, operationMode)
# retrieves the object types
dataType = 1
returnCode, objects_handles, objects_types, floatData, stringData = vrep.simxGetObjectGroupData(
clientID, objectType, dataType, operationMode)
# retrieves the absolute object positions
dataType = 3
returnCode, objects_handles, intData, objects_poses, stringData = vrep.simxGetObjectGroupData(
clientID, objectType, dataType, operationMode)
# retrieves the object orientations
dataType = 5
returnCode, objects_handles, intData, objects_orientations, stringData = vrep.simxGetObjectGroupData(
clientID, objectType, dataType, operationMode)
while running:
returnCode, ping = vrep.simxGetPingTime(clientID)
returnCode, serverState = vrep.simxGetInMessageInfo(
clientID, vrep.simx_headeroffset_server_state)
running = serverState
# Start simulation
returnCode = vrep.simxStartSimulation(clientID,
vrep.simx_opmode_blocking)
printlog('simxStartSimulation', returnCode)
elif c == 'q':
break
#testing getState
returnCode, _, _, floatData, _ = vrep.simxGetObjectGroupData(
clientID, jointsCollectionHandle, 15, vrep.simx_opmode_streaming
) # or simx_opmode_blocking (not recommended)
jointPositions = np.array(
floatData[0::2]
) #take elements at odd positions (even corresponds to torques)
jointPositions = jointPositions % (2 * np.pi
) #take values in [0, 2*pi]
#get reward (proximity sensor)
returnCode, distanceToGoal = vrep.simxReadDistance(
clientID, distToGoalHandle, vrep.simx_opmode_buffer)
print("distance to goal:", distanceToGoal)
# reward =
#checkGoalReached
if distanceToGoal < minDistance: goalReached = True
vrep.simxCopyPasteObjects(clientID, NAO_Handle,
vrep.simx_opmode_oneshot_wait)
# Get the handle of the new NAO
copyNAO = vrep.simxGetObjectHandle(clientID, 'NAO#' + str(i),
vrep.simx_opmode_oneshot_wait)
# Change the position of the new NAO so it won't collide with others
vrep.simxSetObjectPosition(clientID, copyNAO[1], -1, [0, y, 0.3518],
vrep.simx_opmode_oneshot)
y += 0.8
# Restart the simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
print '================ Posture Initialization ================'
# Go to the posture StandZero
print 'Posture Initialization : StandZero'
for i in range(0, nbrOfNao):
motionProxy[i].stiffnessInterpolation('Body', 1.0, 1.0)
postureProxy[i].goToPosture('StandZero', 1.0, 1.0)
print '================ Handle Creation for the new NAO ================'
a = vrep.simxGetObjectGroupData(clientID, vrep.sim_object_joint_type, 0,
vrep.simx_opmode_oneshot_wait)
get_new_nao_handles(nbrOfNao, clientID, Body)
thread = []
for i in range(0, nbrOfNao):
thread.append(
Thread(target=JointControl, args=(clientID, motionProxy[i], i, Body)))
thread[i].start()
print 'Nao number ' + str(i + 1) + ' initialized'
print '================ All the Nao are listening ================'
def _get_object_name_and_handle(self):
"""
# objectType:
# joint: sim_object_joint_type
# proximity sensor: sim_object_proximitysensor_type
# light: sim_object_light_type
"""
dataType = 0 # 0: retrieves the object names (in stringData.)
print("Get objects' names and handles ...")
proxSensorIndex = []
lightIndex = []
jointIndex = []
# proximity sensor
rc = vrep.simx_return_initialize_error_flag
while rc != vrep.simx_return_ok:
rc, proxSensorHandles, intData, floatData, proxSensorNames = vrep.simxGetObjectGroupData(
self.clientID, vrep.sim_object_proximitysensor_type, dataType,
self._def_op_mode)
if rc == vrep.simx_return_ok:
print(
'Get Prox Sensor Success!!!!!'
) # display the reply from V-REP (in this case, just a string)
for i, name in enumerate(proxSensorNames):
if "_node#" in name:
print("Proximity Sensor: {}, and handle: {}".format(
name, proxSensorHandles[i]))
proxSensorIndex.append(i)
break
else:
print('Fail to get proximity sensors!!!')
# light
rc = vrep.simx_return_initialize_error_flag
while rc != vrep.simx_return_ok:
rc, lightHandles, intData, floatData, lightNames = vrep.simxGetObjectGroupData(
self.clientID, vrep.sim_object_light_type, dataType,
self._def_op_mode)
if rc == vrep.simx_return_ok:
print(
'Get Lihgt Success!!!!!'
) # display the reply from V-REP (in this case, just a string)
for i, name in enumerate(lightNames):
if "_node#" in name:
print("Light: {}, and handle: {}".format(
name, lightHandles[i]))
lightIndex.append(i)
break
else:
print('Fail to get lights!!!')
# joint
rc = vrep.simx_return_initialize_error_flag
while rc != vrep.simx_return_ok:
rc, jointHandles, intData, floatData, jointNames = vrep.simxGetObjectGroupData(
self.clientID, vrep.sim_object_joint_type, dataType,
self._def_op_mode)
if rc == vrep.simx_return_ok:
print(
'Get Joint Success!!!!!'
) # display the reply from V-REP (in this case, just a string)
for i, name in enumerate(jointNames):
if "_node#" in name:
print("Joint: {}, and handle: {}".format(
name, jointHandles[i]))
jointIndex.append(i)
break
else:
print('Fail to get joints!!!')
proxSensorHandles = np.array(proxSensorHandles)
proxSensorNames = np.array(proxSensorNames)
lightHandles = np.array(lightHandles)
lightNames = np.array(lightNames)
jointHandles = np.array(jointHandles)
jointNames = np.array(jointNames)
self.proxSensorHandles = proxSensorHandles[proxSensorIndex]
self.proxSensorNames = proxSensorNames[proxSensorIndex]
self.lightHandles = lightHandles[lightIndex]
self.lightNames = lightNames[lightIndex]
self.jointHandles = jointHandles[jointIndex]
self.jointNames = jointNames[jointIndex]
print textOK
else:
print "Error code =", err, textNotOK
print 'Program started'
clientID=vrep.simxStart('127.0.0.1',19997,True,True,5000,5)
if clientID!=-1:
print('Connected to remote API server')
err,objs=vrep.simxGetObjects(clientID,vrep.sim_handle_all,vrep.simx_opmode_oneshot_wait)
printErr(err,'Number of objects in the scene: {}'.format(len(objs)),'Remote API function call returned error')
vrep.simxAddStatusbarMessage (clientID,"Connect from Python Client",vrep.simx_opmode_oneshot_wait)
err,allHandles,allIntData,allFloatData,allStringData = vrep.simxGetObjectGroupData(clientID,vrep.sim_appobj_object_type,0,vrep.simx_opmode_oneshot_wait)
print "Err ", err
print "allHandles: ", allHandles
print "allIntData: ", allIntData
print "allFloatData: ", allFloatData
print "allStringData: ", allStringData
err,bubbleRobHandle=vrep.simxGetObjectHandle(clientID,"remoteApiControlledBubbleRob",vrep.simx_opmode_oneshot_wait)
err,bubbleRobBodyHandle=vrep.simxGetObjectHandle(clientID,"remoteApiControlledBubbleRobBody",vrep.simx_opmode_oneshot_wait)
err,bubbleRobBodyRespHandle=vrep.simxGetObjectHandle(clientID,"remoteApiControlledBubbleRobBody_respondable",vrep.simx_opmode_oneshot_wait)
err,bubbleRobGraphHandle=vrep.simxGetObjectHandle(clientID,"remoteApiControlledBubbleRobGraph",vrep.simx_opmode_oneshot_wait)
printErr(err,'Get handle for BubbleRob: {}'.format(bubbleRobHandle),'Error by getting handle for BubbleRob: {}'.format(err))
err,leftMotorHandle=vrep.simxGetObjectHandle(clientID,"remoteApiControlledBubbleRobLeftMotor",vrep.simx_opmode_oneshot_wait)
printErr(err,'Get handle for left motor: {}'.format(leftMotorHandle),'Error by getting handle for left motor')
time.sleep(2)
for i in range(0,nbrOfNao-1):
motionProxy.append(ALProxy('ALMotion',naoIP[i+1],naoPort[i+1]))
postureProxy.append(ALProxy('ALRobotPosture', naoIP[i+1], naoPort[i+1]))
#Create new NAo in VRep
vrep.simxCopyPasteObjects(clientID,NAO_Handle,vrep.simx_opmode_oneshot_wait)
#Get the handle of the new NAO
copyNAO = vrep.simxGetObjectHandle(clientID,'NAO#'+str(i),vrep.simx_opmode_oneshot_wait)
#Change the position of the new NAO so it won't collide with others
vrep.simxSetObjectPosition(clientID,copyNAO[1],-1,[0,y,0.3518],vrep.simx_opmode_oneshot )
y+=0.8
#Restart the simulation
vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot)
print '================ Posture Initialization ================'
#Go to the posture StandZero
print 'Posture Initialization : StandZero'
for i in range(0,nbrOfNao):
motionProxy[i].stiffnessInterpolation('Body', 1.0, 1.0)
postureProxy[i].goToPosture('StandZero',1.0)
print '================ Handle Creation for the new NAO ================'
a=vrep.simxGetObjectGroupData(clientID,vrep.sim_object_joint_type,0,vrep.simx_opmode_oneshot_wait)
get_new_nao_handles(nbrOfNao,clientID,Body)
thread=[]
for i in range(0,nbrOfNao):
thread.append(Thread(target = JointControl, args=(clientID,motionProxy[i],i,Body)))
thread[i].start()
print 'Nao number ' + str(i+1) + ' initialized'
print '================ All the Nao are listening ================'
_, dhandle = vrep.simxGetObjectHandle(clientID,"Dummy"+str(100+i),vrep.simx_opmode_blocking)
_ = vrep.simxSetObjectPosition(clientID,dhandle,-1,[0,i*0.5,0],vrep.simx_opmode_streaming)
_ = vrep.simxSetObjectOrientation(clientID,dhandle,-1,[0,0,math.pi/2],vrep.simx_opmode_oneshot)
'''
'''
for theta in np.arange(0,2*math.pi,0.5/R):
_, dhandle = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_blocking)
_ = vrep.simxSetObjectParent(clientID, dhandle, dsethandle, True, vrep.simx_opmode_blocking)
x = R*math.cos(theta)
y = R*math.sin(theta)
_ = vrep.simxSetObjectPosition(clientID, dhandle, -1, [x, y, 0], vrep.simx_opmode_oneshot)
'''
_, colHandle = vrep.simxGetCollectionHandle(clientID, "Ref",
vrep.simx_opmode_blocking)
_, refHandle, intData, doubleData, strData = vrep.simxGetObjectGroupData(
clientID, colHandle, 3, vrep.simx_opmode_blocking)
refPos = np.reshape(doubleData, (len(refHandle), 3))
for index in range(0, len(refHandle) - 1):
curPos = refPos[index]
nextPos = refPos[index + 1]
relPos = nextPos - curPos
ang = math.atan2(relPos[1], relPos[0])
_ = vrep.simxSetObjectPosition(clientID, refHandle[index], -1,
[curPos[0], curPos[1], 0.1],
vrep.simx_opmode_blocking)
_ = vrep.simxSetObjectOrientation(clientID, refHandle[index], -1,
def _reset(self):
if self._firstSim:
self._connect()
vrep.simxLoadScene(self.clientID, self._scene, 0,
vrep.simx_opmode_blocking)
else:
vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
number_of_iteration_not_stopped = 0
while True:
vrep.simxGetIntegerSignal(self.clientID, 'asdf',
vrep.simx_opmode_blocking)
e = vrep.simxGetInMessageInfo(
self.clientID, vrep.simx_headeroffset_server_state)
not_stopped = e[1] & 1
if not not_stopped:
print("STOPPED")
break
else:
if number_of_iteration_not_stopped % 10 == 0:
print(number_of_iteration_not_stopped, ': not stopped')
number_of_iteration_not_stopped += 1
if number_of_iteration_not_stopped > 100:
self._firstSim = True
self._connect()
vrep.simxLoadScene(self.clientID, self._scene, 0,
vrep.simx_opmode_blocking)
errorCode, handles, intData, floatData, stringData = vrep.simxGetObjectGroupData(
self.clientID, vrep.sim_appobj_object_type, 0,
vrep.simx_opmode_blocking)
self._handles_dict = dict(zip(stringData, handles))
tmp = []
self.new_trajectory = {key: list(tmp) for key in self._order}
for el in self._order:
vrep.simxSetJointTargetPosition(self.clientID,
self._handles_dict[el], 0,
vrep.simx_opmode_oneshot)
vrep.simxGetJointPosition(self.clientID, self._handles_dict[el],
vrep.simx_opmode_streaming)
#
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot)
for i in range(self._range):
if self._firstSim:
print("FirStSim iteration: ", i)
for el in self._order:
vrep.simxSynchronousTrigger(self.clientID)
vrep.simxSetJointTargetPosition(
self.clientID, self._handles_dict[el],
self._rad(self._walking_data[el][i]),
vrep.simx_opmode_streaming)
else:
print("Reset iteration: ", i)
for el in self._order:
vrep.simxSynchronousTrigger(self.clientID)
vrep.simxSetJointTargetPosition(
self.clientID, self._handles_dict[el],
self._rad(self._walking_data[el][i]),
vrep.simx_opmode_streaming)
if i == (self._range - 2):
lastposlist = []
for jo in self._order:
errorCode, lastpostmp = vrep.simxGetJointPosition(
self.clientID, self._handles_dict[jo],
vrep.simx_opmode_streaming)
lastposlist.append(lastpostmp)
rightSenValues, leftSenValues = _GetSensorValues(
self.clientID, self._rightSen, self._leftSen, self._handles_dict)
# print("RightSensors: ", rightSenValues)
# print("LeftSensors: ", leftSenValues)
rightSenSum = np.sum(np.asarray(rightSenValues))
leftSenSum = np.sum(np.asarray(leftSenValues))
zmpX, zmpY = _GetZmp(self.clientID, self._handles_dict, self._floor,
rightSenValues, leftSenValues, rightSenSum,
leftSenSum)
# print("ZMPX: ", zmpX)
# print("ZMPy: ", zmpY)
errorCode, sphere = vrep.simxGetObjectPosition(
self.clientID, self._handles_dict['Sphere'],
self._handles_dict[self._floor], vrep.simx_opmode_streaming)
self._sphere_height = sphere[2]
# print("Sphere height: ", self._sphere_height)
CurrentPos, CurrentVel = _GetJointValues(self.clientID,
self._handles_dict,
self._order, lastposlist)
# print("Position: ", CurrentPos)
# print("Velocity: ", CurrentVel)
self._state.Reset()
self._state.SetPosVel(CurrentPos, CurrentVel)
if self._firstSim:
self._firstSim = False
self._range = 15
self._reset()
self._done = False
self._quit = False
return self._state.GetState()