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
Esempio n. 2
0
 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
Esempio n. 3
0
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]
Esempio n. 4
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)
Esempio n. 6
0
 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
Esempio n. 7
0
 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)
Esempio n. 9
0
    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
Esempio n. 10
0
    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)
Esempio n. 11
0
    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)
Esempio n. 12
0
 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
Esempio n. 13
0
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
Esempio n. 14
0
    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
Esempio n. 15
0
 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
Esempio n. 17
0
    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
Esempio n. 18
0
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
Esempio n. 19
0
 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)]
Esempio n. 20
0
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
Esempio n. 21
0
 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)]
Esempio n. 22
0
    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))
Esempio n. 23
0
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)
Esempio n. 25
0
    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
Esempio n. 26
0
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]
Esempio n. 28
0
        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)

Esempio n. 29
0
 def raw_getObjectGroupData(self, objectType, dataType, operationMode = vrep.simx_opmode_oneshot_wait):
     return vrep.simxGetObjectGroupData(self.clientID, objectType, dataType, operationMode)
Esempio n. 30
0
    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
Esempio n. 32
0
        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 ================'
Esempio n. 33
0
    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]
Esempio n. 34
0
        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 ================'
Esempio n. 36
0
    _, 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,
Esempio n. 37
0
    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()