示例#1
0
This test file is dependent on vrep.

@author: cz
"""

from scene import Scene
from sceneplot import ScenePlot
from robot import Robot
import numpy as np

try:
    sc = Scene()
    sp = ScenePlot(sc)
    dynamics = 11
    sc.dynamics = dynamics
    sc.addRobot(np.float32([[-2, 0, 0], [0, 2 / 2, 0]]))
    sc.addRobot(np.float32([[1, 3, 0], [1.732 / 2, -1 / 2, 0]]))
    sc.addRobot(np.float32([[0, 0, 0], [-1.732 / 2, -1 / 2, 0]]))

    sc.setADjMatrix(np.uint8([[0, 1, 1], [1, 0, 1], [1, 1, 0]]))
    # vrep related
    sc.initVrep()
    # Choose sensor type
    sc.SENSOR_TYPE = "VPL16"  # None, 2d, VPL16, kinect
    sc.objectNames = [
        'Pioneer_p3dx', 'Pioneer_p3dx_leftMotor', 'Pioneer_p3dx_rightMotor'
    ]

    if sc.SENSOR_TYPE == "None":
        pass
    elif sc.SENSOR_TYPE == "2d":
def generateData():
    sc = Scene(recordData=True)
    sp = ScenePlot(sc)
    #sc.occupancyMapType = sc.OCCUPANCY_MAP_THREE_CHANNEL
    sc.occupancyMapType = sc.OCCUPANCY_MAP_BINARY
    sc.dynamics = sc.DYNAMICS_MODEL_BASED_LINEAR  # robot dynamics
    try:
        sc.addRobot(np.float32([[-2, 0, 0], [0.0, 0.0, 0.0]]),
                    role=sc.ROLE_LEADER)
        sc.addRobot(np.float32([[1, 3, 0], [-1.0, 0.0, 0.0]]),
                    role=sc.ROLE_FOLLOWER)
        #        sc.addRobot(np.float32([[1, 3, 0], [-1.0, 0.0, 0.0]]), role = sc.ROLE_FOLLOWER, learnedController = fcl.test)
        #==============================================================================
        #         sc.addRobot(np.float32([[1, 3, 0], [0, -1, 0]]),
        #                     dynamics = sc.DYNAMICS_LEARNED,
        #                     learnedController = fcl.test)
        #==============================================================================

        # No leader
        sc.setADjMatrix(np.uint8([[0, 0], [1, 0]]))
        # Set robot 0 as the leader.

        # vrep related
        sc.initVrep()
        # Choose sensor type
        sc.SENSOR_TYPE = "VPL16"  # None, 2d, VPL16, kinect
        #sc.SENSOR_TYPE = "None" # None, 2d, VPL16, kinect
        sc.objectNames = [
            'Pioneer_p3dx', 'Pioneer_p3dx_leftMotor', 'Pioneer_p3dx_rightMotor'
        ]

        if sc.SENSOR_TYPE == "None":
            sc.setVrepHandles(0, '')
            sc.setVrepHandles(1, '#0')
        elif sc.SENSOR_TYPE == "2d":
            sc.objectNames.append('LaserScanner_2D_front')
            sc.objectNames.append('LaserScanner_2D_rear')
            sc.setVrepHandles(0, '')
            sc.setVrepHandles(1, '#0')
        elif sc.SENSOR_TYPE == "VPL16":
            sc.objectNames.append('velodyneVPL_16')  # _ptCloud
            sc.setVrepHandles(0, '')
            sc.setVrepHandles(1, '#0')
        elif sc.SENSOR_TYPE == "kinect":
            sc.objectNames.append('kinect_depth')
            sc.objectNames.append('kinect_rgb')
            sc.setVrepHandles(0, '')
            sc.setVrepHandles(1, '#0')

        #sc.renderScene(waitTime = 3000)
        tf = 40  # must be greater than 1
        sc.resetPosition()
        sp.plot(3, tf)

        while sc.simulate():
            #sc.renderScene(waitTime = int(sc.dt * 1000))
            sc.showOccupancyMap(waitTime=int(sc.dt * 1000))

            #print("---------------------")
            #print("t = %.3f" % sc.t, "s")

            if sc.t > 1:
                maxAbsError = sc.getMaxFormationError()
                if maxAbsError < 0.01:
                    pass  #tf = sc.t - 0.01

            #sp.plot(0, tf)
            sp.plot(2, tf)
            #sp.plot(1, tf)
            sp.plot(3, tf)
            sp.plot(4, tf)
            sp.plot(6, tf)

            if sc.t > tf:
                print('maxAbsError = ', maxAbsError)
                break

            #print('robot 0: ', sc.robots[0].xi.x, ', ', sc.robots[0].xi.y, ', ', sc.robots[0].xi.theta)
            #print('robot 1: ', sc.robots[1].xi.x, ', ', sc.robots[1].xi.y, ', ', sc.robots[1].xi.theta)
            #print('robot 2: ', sc.robots[2].xi.x, ', ', sc.robots[2].xi.y, ', ', sc.robots[2].xi.theta)
            #print('y01: ' + str(sc.robots[1].xi.y - sc.robots[0].xi.y))
            #print('x02: ' + str(sc.robots[2].xi.x - sc.robots[0].xi.x))
        sc.deallocate()
    except KeyboardInterrupt:
        x = input('Quit?(y/n)')
        sc.deallocate()
        if x == 'y' or x == 'Y':
            tf = sc.t - 0.01

            sp.plot(2, tf)
            sp.plot(3, tf)
            sp.plot(4, tf)
            sp.plot(6, tf)

            raise Exception('Aborted.')

    except:
        sc.deallocate()
        raise

    if True:  #maxAbsError < 0.01:
        return sc
    else:
        return None
示例#3
0
def generateData():
    sc = Scene(recordData=True)
    sp = ScenePlot(sc)
    try:
        dynamics = 11
        sc.dynamics = dynamics
        sc.addRobot(np.float32([[-2, 0, 0], [0, 2 / 2, 0]]))
        sc.addRobot(np.float32([[1, 3, 0], [1.732 / 2, -1 / 2, 0]]))
        sc.addRobot(np.float32([[0, 0, 0], [-1.732 / 2, -1 / 2, 0]]))

        # No leader
        sc.setADjMatrix(np.uint8([[0, 1, 1], [1, 0, 1], [1, 1, 0]]))
        # Set robot 0 as the leader.
        # sc.setADjMatrix(np.uint8([[0, 0, 0], [1, 0, 1], [1, 1, 0]]))

        # vrep related
        sc.initVrep()
        # Choose sensor type
        sc.SENSOR_TYPE = "VPL16"  # None, 2d, VPL16, kinect
        sc.objectNames = [
            'Pioneer_p3dx', 'Pioneer_p3dx_leftMotor', 'Pioneer_p3dx_rightMotor'
        ]

        if sc.SENSOR_TYPE == "None":
            pass
        elif sc.SENSOR_TYPE == "2d":
            sc.objectNames.append('LaserScanner_2D_front')
            sc.objectNames.append('LaserScanner_2D_rear')
            sc.setVrepHandles(0, '')
            sc.setVrepHandles(1, '#0')
            sc.setVrepHandles(2, '#1')
        elif sc.SENSOR_TYPE == "VPL16":
            sc.objectNames.append('velodyneVPL_16')  # _ptCloud
            sc.setVrepHandles(0, '')
            sc.setVrepHandles(1, '#0')
            sc.setVrepHandles(2, '#1')
        elif sc.SENSOR_TYPE == "kinect":
            sc.objectNames.append('kinect_depth')
            sc.objectNames.append('kinect_rgb')
            sc.setVrepHandles(0, '')
            sc.setVrepHandles(1, '#0')
            sc.setVrepHandles(2, '#1')

        #sc.renderScene(waitTime = 3000)
        tf = 10
        sc.resetPosition()
        sp.plot(3, tf)
        while sc.simulate():
            #sc.renderScene(waitTime = int(sc.dt * 1000))
            sc.showOccupancyMap(waitTime=int(sc.dt * 1000))

            #print("---------------------")
            #print("t = %.3f" % sc.t, "s")

            #sp.plot(0, tf)
            sp.plot(2, tf)
            #sp.plot(1, tf)
            sp.plot(3, tf)
            sp.plot(4, tf)
            sp.plot(5, tf)
            sp.plot(6, tf)
            if sc.t > tf:
                break

            #print('robot 0: ', sc.robots[0].xi.x, ', ', sc.robots[0].xi.y, ', ', sc.robots[0].xi.theta)
            #print('robot 1: ', sc.robots[1].xi.x, ', ', sc.robots[1].xi.y, ', ', sc.robots[1].xi.theta)
            #print('robot 2: ', sc.robots[2].xi.x, ', ', sc.robots[2].xi.y, ', ', sc.robots[2].xi.theta)
            #print('y01: ' + str(sc.robots[1].xi.y - sc.robots[0].xi.y))
            #print('x02: ' + str(sc.robots[2].xi.x - sc.robots[0].xi.x))
        sc.deallocate()
    except KeyboardInterrupt:
        x = input('Quit?(y/n)')
        sc.deallocate()
        if x == 'y' or x == 'Y':
            raise Exception('Aborted.')

    except:
        sc.deallocate()
        raise

    # check max formation error
    maxAbsError = 0
    for key in sc.ydict[2]:
        absError = abs(sc.ydict[2][key][-1])
        if absError > maxAbsError:
            maxAbsError = absError
    print('maxAbsError = ', maxAbsError)

    if maxAbsError < 0.5:
        return sc
    else:
        return None
示例#4
0
def generateData(i):
    sc = Scene(fileName=__file__, recordData=True, runNum=i)
    sp = ScenePlot(sc)
    sp.saveEnabled = True  # save plots?
    #sc.occupancyMapType = sc.OCCUPANCY_MAP_THREE_CHANNEL
    sc.occupancyMapType = sc.OCCUPANCY_MAP_BINARY
    sc.dynamics = sc.DYNAMICS_MODEL_BASED_DISTANCE2_REFVEL  # robot dynamics
    sc.errorType = 0
    try:
        for i in range(robotNum):
            sc.addRobot(np.float32([[-2, 0, 1], [0.0, 0.0, 0.0]]),
                        role=sc.ROLE_PEER)
#==============================================================================
#         sc.addRobot(np.float32([[1, 3, 0], [0, -1, 0]]),
#                     dynamics = sc.DYNAMICS_LEARNED,
#                     learnedController = fcl.test)
#==============================================================================

# No leader
        I = np.identity(robotNum, dtype=np.int8)
        M = np.ones(robotNum, dtype=np.int8)
        sc.setADjMatrix(M - I)

        # Set robot 0 as the leader.

        # vrep related
        sc.initVrep()
        # Choose sensor type
        #sc.SENSOR_TYPE = "VPL16" # None, 2d, VPL16, kinect
        sc.SENSOR_TYPE = "None"  # None, 2d, VPL16, kinect
        sc.objectNames = [
            'Pioneer_p3dx', 'Pioneer_p3dx_leftMotor', 'Pioneer_p3dx_rightMotor'
        ]

        if sc.SENSOR_TYPE == "None":
            sc.setVrepHandles(0, '')
            sc.setVrepHandles(1, '#0')
            sc.setVrepHandles(2, '#1')
            sc.setVrepHandles(3, '#2')
        elif sc.SENSOR_TYPE == "VPL16":
            sc.objectNames.append('velodyneVPL_16')  # _ptCloud
            sc.setVrepHandles(0, '')
            sc.setVrepHandles(1, '#0')
            sc.setVrepHandles(2, '#1')
            sc.setVrepHandles(3, '#2')

        #sc.renderScene(waitTime = 3000)
        tf = 15  # must be greater than 1
        errorCheckerEnabled = False
        initRef(sc, i)
        sc.resetPosition(4)  # Random initial position
        # Fixed initial position
        #sc.robots[0].setPosition([0.0, 0.0, math.pi/2])
        #sc.robots[1].setPosition([-2.2, -1.0, 0.3])
        sp.plot(4, tf)
        while sc.simulate():
            #sc.renderScene(waitTime = int(sc.dt * 1000/2))
            #sc.showOccupancyMap(waitTime = int(sc.dt * 1000))

            #print("---------------------")
            #print("t = %.3f" % sc.t, "s")
            #print(sc.robots[2].xid0.y)
            if sc.t > 1:
                maxAbsError = sc.getMaxFormationError()
                if maxAbsError < 0.01 and errorCheckerEnabled:
                    #tf = sc.t - 0.01
                    # set for how many seconds after convergence the simulator shall run
                    tExtra = 30
                    #tf = sc.t + tExtra
                    errorCheckerEnabled = False
                    print('Ending in ', str(tExtra), ' seconds...')

            plot(sp, tf)
            if sc.t > tf:
                message = "maxAbsError = {0:.3f} m".format(maxAbsError)
                sc.log(message)
                print(message)
                break

    except KeyboardInterrupt:
        x = input('Quit?(y/n)')
        if x == 'y' or x == 'Y':
            tf = sc.t - 0.01
            plot(sp, tf)
            raise Exception('Aborted.')

    except VrepError as err:
        sc.log(err.message)
        print(err.message)
        return None
    except:
        raise
    finally:
        sc.deallocate()

    if True:  #maxAbsError < 0.01:
        return sc
    else:
        return None
def generateData(**kwargs):
    sc = Scene(recordData=True)
    sp = ScenePlot(sc)
    try:
        #dynamics = 20
        arg2 = np.float32([.5, .5])
        for name, value in kwargs.items():
            if name == "dynamics":
                dynamics = value
            elif name == "arg2":
                arg2 = value
        sc.dynamics = dynamics
        sc.addRobot(np.float32([[0, 0, 0], [0, 2 / 2, 0]]), arg2)

        # No leader
        sc.setADjMatrix(np.uint8([[0]]))
        # Set robot 0 as the leader.
        # sc.setADjMatrix(np.uint8([[0, 0, 0], [1, 0, 1], [1, 1, 0]]))

        # vrep related
        sc.initVrep()
        # Choose sensor type
        sc.SENSOR_TYPE = "None"  # None, 2d, VPL16, kinect
        sc.objectNames = [
            'Pioneer_p3dx', 'Pioneer_p3dx_leftMotor', 'Pioneer_p3dx_rightMotor'
        ]

        if sc.SENSOR_TYPE == "None":
            sc.setVrepHandles(0, '')
        elif sc.SENSOR_TYPE == "2d":
            sc.objectNames.append('LaserScanner_2D_front')
            sc.objectNames.append('LaserScanner_2D_rear')
            sc.setVrepHandles(0, '')
            sc.setVrepHandles(1, '#0')
            sc.setVrepHandles(2, '#1')
        elif sc.SENSOR_TYPE == "VPL16":
            sc.objectNames.append('velodyneVPL_16')  # _ptCloud
            sc.setVrepHandles(0, '')
            sc.setVrepHandles(1, '#0')
            sc.setVrepHandles(2, '#1')
        elif sc.SENSOR_TYPE == "kinect":
            sc.objectNames.append('kinect_depth')
            sc.objectNames.append('kinect_rgb')
            sc.setVrepHandles(0, '')
            sc.setVrepHandles(1, '#0')
            sc.setVrepHandles(2, '#1')

        #sc.renderScene(waitTime = 3000)
        tf = 30
        sp.plot(3, tf)
        while sc.simulate():
            #sc.renderScene(waitTime = int(sc.dt * 1000))
            #sc.showOccupancyMap(waitTime = int(sc.dt * 1000))

            #print("---------------------")
            #print("t = %.3f" % sc.t, "s")

            #sp.plot(0, tf)
            #sp.plot(1, tf)
            #sp.plot(2, tf)
            sp.plot(3, tf)
            sp.plot(4, tf)
            sp.plot(5, tf)
            sp.plot(6, tf)
            if sc.t > tf:
                break
        sc.deallocate()
    except KeyboardInterrupt:
        x = input('Quit?(y/n)')
        sc.deallocate()
        if x == 'y' or x == 'Y':
            raise Exception('Aborted.')

    except:
        sc.deallocate()
        raise

    return None
def generateData(i):
    sc = Scene(fileName=__file__, recordData=False, runNum=i)
    sp = ScenePlot(sc)
    sp.saveEnabled = True  # save plots?
    #sc.occupancyMapType = sc.OCCUPANCY_MAP_THREE_CHANNEL
    sc.occupancyMapType = sc.OCCUPANCY_MAP_BINARY
    sc.dynamics = 5  # robot dynamics
    sc.errorType = 0
    try:
        sc.addRobot(np.float32([[-.0, 0, 0], [0.0, 0.0, 0.0]]),
                    role=sc.ROLE_PEER)
        sc.addRobot(np.float32([[-3, 4, 0], [1.0, 0.0, 0.0]]),
                    role=sc.ROLE_PEER)
        sc.addRobot(np.float32([[2, 1, 0], [0.0, 1.732, 0.0]]),
                    role=sc.ROLE_PEER)
        #==============================================================================
        #         sc.addRobot(np.float32([[1, 3, 0], [0, -1, 0]]),
        #                     dynamics = sc.DYNAMICS_LEARNED,
        #                     learnedController = fcl.test)
        #==============================================================================

        # No leader
        sc.setADjMatrix(np.uint8([[0, 1, 1], [1, 0, 1], [1, 1, 0]]))
        # Set robot 0 as the leader.

        #sc.renderScene(waitTime = 3000)
        tf = 15  # must be greater than 1
        errorCheckerEnabled = False
        initRef(sc, i)
        #sc.resetPosition(2) # Random initial position
        sc.robots[0].setPosition([.0, .0, .0])
        sc.robots[1].setPosition([-2.0, 0.001, 0.0])
        sc.robots[2].setPosition([2.0, 0.0, 0.0])

        #sc.robots[0].setPosition([.0, .0, .0])
        #sc.robots[1].setPosition([-3.0, 4.0, 0.0])
        #sc.robots[2].setPosition([2.0, 1.0, 0.0])

        # Fixed initial position
        #sc.robots[0].setPosition([0.0, 0.0, math.pi/2])
        #sc.robots[1].setPosition([-2.2, -1.0, 0.3])
        sp.plot(4, tf)
        while sc.simulate():
            sc.renderScene(waitTime=int(sc.dt * 1000), mode=1)
            #sc.showOccupancyMap(waitTime = int(sc.dt * 1000))

            #print("---------------------")
            #print("t = %.3f" % sc.t, "s")
            #print(sc.robots[2].xid0.y)
            if sc.t > 1:
                maxAbsError = sc.getMaxFormationError()
                if maxAbsError < 0.01 and errorCheckerEnabled:
                    #tf = sc.t - 0.01
                    # set for how many seconds after convergence the simulator shall run
                    tExtra = 30
                    #tf = sc.t + tExtra
                    errorCheckerEnabled = False
                    print('Ending in ', str(tExtra), ' seconds...')

            plot(sp, tf)
            if sc.t > tf:
                message = "maxAbsError = {0:.3f} m".format(maxAbsError)
                sc.log(message)
                print(message)
                break

    except KeyboardInterrupt:
        x = input('Quit?(y/n)')
        if x == 'y' or x == 'Y':
            tf = sc.t - 0.01
            plot(sp, tf)
            raise Exception('Aborted.')

    except VrepError as err:
        sc.log(err.message)
        print(err.message)
        return None
    except:
        raise
    finally:
        sc.deallocate()

    if True:  #maxAbsError < 0.01:
        return sc
    else:
        return None