Пример #1
0
# Close eventual old connections
vrep.simxFinish(-1)
# Connect to V-REP remote server
clientID = vrep.simxStart('192.168.9.15', 19997, True, True, 5000, 5)

if clientID != -1:
    print('Connected to remote API server')

    res = vrep.simxAddStatusbarMessage(clientID, "40823217",
                                       vrep.simx_opmode_oneshot)
    if res not in (vrep.simx_return_ok, vrep.simx_return_novalue_flag):
        print("Could not add a message to the status bar.")

    opmode = vrep.simx_opmode_oneshot_wait

    vrep.simxStartSimulation(clientID, opmode)
    ret, front_handle = vrep.simxGetObjectHandle(clientID, "front_joint",
                                                 opmode)
    ret, left_handle = vrep.simxGetObjectHandle(clientID, "left_joint", opmode)
    ret, right_handle = vrep.simxGetObjectHandle(clientID, "right_joint",
                                                 opmode)
    ret, gear_handle = vrep.simxGetObjectHandle(clientID, "gear_joint", opmode)
    #ret,main = vrep.simxGetObjectHandle(clientID, "main", opmode)
    while True:
        #keyboard "w"
        if keyboard.is_pressed("w"):
            vrep.simxSetJointTargetVelocity(clientID, front_handle, 1000,
                                            opmode)
            vrep.simxSetJointTargetVelocity(clientID, left_handle, 1000,
                                            opmode)
            vrep.simxSetJointTargetVelocity(clientID, right_handle, 1000,
Пример #2
0
        print('\n[ERROR] maze0' + str(file_num) +
              '.jpg was not read correctly, something went wrong!')
        print()
        sys.exit()

    # Check if connected to Remote API server and maze array has been generated successfully
    if ((client_id != -1) and (maze_array_generated_flag == 1)):

        try:
            # Send maze array data to CoppeliaSim via Remote API
            return_code = send_data(maze_array)

            if (return_code == sim.simx_return_ok):

                # Start the simulation
                return_code = sim.simxStartSimulation(client_id,
                                                      sim.simx_opmode_oneshot)

                # Making sure that last command sent out had time to arrive
                sim.simxGetPingTime(client_id)

                if ((return_code == sim.simx_return_novalue_flag)
                        or (return_code == sim.simx_return_ok)):
                    print('\nSimulation started correctly in CoppeliaSim.')

                time.sleep(2)

                try:
                    # Get image array and its resolution from Vision Sensor in ComppeliaSim scene
                    vision_sensor_image, image_resolution, return_code = get_vision_sensor_image(
                    )
Пример #3
0
                          epoch * steps + step)
    return losses.avg


if __name__ == '__main__':
    ip = '127.0.0.1'
    port = 19997
    sim.simxFinish(-1)  # just in case, close all opened connections
    clientID = sim.simxStart(ip, port, True, True, -5000, 5)
    # Connect to V-REP
    if clientID == -1:
        import sys
        sys.exit('\nV-REP remote API server connection failed (' + ip + ':' +
                 str(port) + '). Is V-REP running?')
    print('Connected to Remote API Server')  # show in the terminal
    sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot)
    Body = {}
    get_first_handles(clientID, Body)
    errorCode, acc = sim.simxGetStringSignal(clientID, 'Acceleration',
                                             sim.simx_opmode_streaming)
    returnCode, position = sim.simxGetObjectPosition(clientID, Body['HeadYaw'],
                                                     -1,
                                                     sim.simx_opmode_streaming)

    converter = coordinate2angle()
    converter.set_bound(np.load('bound_range.npy'), np.load('step.npy'), 32,
                        16)
    primitives = np.load('primitive_data.npy')

    model = CapabilityWithAttention(time_dim=32)
    #model.load_state_dict(torch.load('./params-capability/Fri Jul 17 01-17-05 2020/best_fitted_params.pt')['model_state_dict'])
Пример #4
0
def init(_clientID, robotname):
    global robot, robo, robotLeftMotor, robotRightMotor
    global clientID
    global paEsquerda, paDireita
    global elevador, irRight, irLeft
    global color_sensor_Left, color_sensor_Right
    global positionrobot, angLeft, angRight, orientationrobot

    clientID = _clientID

    sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait)
    print('Connected to remote API server')
    sim.simxAddStatusbarMessage(clientID, 'Funcionando...',
                                sim.simx_opmode_oneshot_wait)
    time.sleep(0.02)

    # Coletar handles
    #Robô
    erro, robot = sim.simxGetObjectHandle(clientID, robotname,
                                          sim.simx_opmode_blocking)
    erro, robo = sim.simxGetObjectHandle(clientID, 'S_Base',
                                         sim.simx_opmode_blocking)
    #Motores
    [erro,
     robotLeftMotor] = sim.simxGetObjectHandle(clientID, 'Revolute_joint2',
                                               sim.simx_opmode_blocking)
    [erro,
     robotRightMotor] = sim.simxGetObjectHandle(clientID, 'Revolute_joint1',
                                                sim.simx_opmode_blocking)
    #Garra
    erro, paEsquerda = sim.simxGetObjectHandle(clientID,
                                               'joint_pa_esquerda_garra',
                                               sim.simx_opmode_blocking)
    erro, paDireita = sim.simxGetObjectHandle(clientID,
                                              'joint_pa_direita_garra',
                                              sim.simx_opmode_blocking)
    erro, elevador = sim.simxGetObjectHandle(clientID, 'joint_acoplador_garra',
                                             sim.simx_opmode_blocking)
    #Sensores
    erro, irRight = sim.simxGetObjectHandle(clientID, 'Sensor_IR_direito',
                                            sim.simx_opmode_blocking)
    erro, irLeft = sim.simxGetObjectHandle(clientID, 'Sensor_IR_esquerdo',
                                           sim.simx_opmode_blocking)
    erro, color_sensor_Left = sim.simxGetObjectHandle(clientID,
                                                      'Sensor_cor_esquerda',
                                                      sim.simx_opmode_blocking)
    erro, color_sensor_Right = sim.simxGetObjectHandle(
        clientID, 'Sensor_cor_direita', sim.simx_opmode_blocking)

    # Criar stream de dados
    [erro,
     positionrobot] = sim.simxGetObjectPosition(clientID, robot, -1,
                                                sim.simx_opmode_streaming)

    erro, angLeft = sim.simxGetJointPosition(clientID, robotLeftMotor,
                                             sim.simx_opmode_streaming)
    erro, angRight = sim.simxGetJointPosition(clientID, robotRightMotor,
                                              sim.simx_opmode_streaming)
    [erro, orientationrobot
     ] = sim.simxGetObjectOrientation(clientID, robot, -1,
                                      sim.simx_opmode_streaming)

    sim.simxReadProximitySensor(clientID, irRight, sim.simx_opmode_streaming)
    sim.simxReadProximitySensor(clientID, irLeft, sim.simx_opmode_streaming)
    sim.simxGetVisionSensorImage(clientID, color_sensor_Left, 0,
                                 sim.simx_opmode_streaming)
    sim.simxGetVisionSensorImage(clientID, color_sensor_Right, 0,
                                 sim.simx_opmode_streaming)
    j2=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.140,0.185,0.237,0.296,0.361,0.431,0.507,0.587,0.670,0.754,0.838,0.924,1.009,1.092,1.171,1.243,1.308,1.365,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.569,1.570,1.571,1.571,1.571]
    j3=[0.000,0.000,-0.002,-0.009,-0.022,-0.042,-0.068,-0.100,-0.139,-0.185,-0.237,-0.296,-0.361,-0.433,-0.511,-0.595,-0.681,-0.767,-0.854,-0.942,-1.027,-1.107,-1.182,-1.249,-1.311,-1.365,-1.414,-1.455,-1.491,-1.519,-1.541,-1.557,-1.566,-1.569,-1.570,-1.571,-1.571,-1.571]
    j4=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.027,1.107,1.181,1.249,1.310,1.365,1.413,1.455,1.490,1.519,1.541,1.556,1.565,1.569,1.570,1.570,1.570,1.571]
    j5=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.028,1.108,1.182,1.250,1.311,1.366,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.569,1.570,1.571,1.571,1.571]
    j6=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.027,1.108,1.182,1.249,1.311,1.365,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.569,1.570,1.571,1.571,1.571]
    
    times=[0.000,0.050,0.100,0.150,0.200,0.250,0.300,0.350,0.400,0.450,0.500,0.550,0.600,0.650,0.700,0.750,0.800,0.850,0.900,0.950,1.000,1.050,1.100,1.150,1.200,1.250,1.300,1.350,1.400,1.450,1.500,1.550,1.600,1.650,1.700,1.750,1.800,1.850,1.900,1.950,2.000,2.050,2.100,2.150,2.200,2.250,2.300,2.350,2.400,2.450,2.500,2.550,2.600,2.650,2.700,2.750,2.800,2.850,2.900,2.950,3.000,3.050,3.100,3.150,3.200,3.250,3.300,3.350,3.400,3.450,3.500,3.550,3.600,3.650,3.700,3.750,3.800,3.850,3.900,3.950,4.000,4.050,4.100,4.150,4.200,4.250,4.300,4.350,4.400,4.450,4.500,4.550,4.600,4.650,4.700,4.750,4.800,4.850,4.900,4.950,5.000,5.050,5.100,5.150,5.200,5.250,5.300,5.350,5.400,5.450,5.500,5.550,5.600,5.650,5.700,5.750,5.800,5.850,5.900,5.950,6.000,6.050,6.100,6.150,6.200,6.250,6.300,6.350]
    j1=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.360,0.431,0.506,0.587,0.669,0.753,0.838,0.923,1.008,1.091,1.170,1.243,1.308,1.365,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.564,1.556,1.543,1.524,1.498,1.465,1.426,1.380,1.328,1.270,1.205,1.136,1.065,0.994,0.922,0.849,0.777,0.705,0.632,0.560,0.487,0.415,0.342,0.270,0.197,0.125,0.053,-0.020,-0.092,-0.165,-0.237,-0.309,-0.382,-0.454,-0.527,-0.599,-0.671,-0.744,-0.816,-0.888,-0.961,-1.033,-1.106,-1.178,-1.250,-1.323,-1.394,-1.462,-1.523,-1.556,-1.595,-1.632,-1.664,-1.690,-1.709,-1.723,-1.729,-1.730,-1.723,-1.710,-1.691,-1.665,-1.632,-1.593,-1.548,-1.495,-1.437,-1.372,-1.302,-1.226,-1.146,-1.064,-0.980,-0.895,-0.810,-0.724,-0.638,-0.552,-0.469,-0.390,-0.318,-0.254,-0.199,-0.151,-0.110,-0.076,-0.048,-0.027,-0.012,-0.004,-0.001,-0.001,-0.000,-0.000,-0.000]
    j2=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.140,0.185,0.237,0.296,0.361,0.431,0.507,0.587,0.670,0.754,0.838,0.924,1.009,1.092,1.171,1.243,1.308,1.365,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.564,1.557,1.544,1.529,1.513,1.497,1.481,1.465,1.449,1.432,1.416,1.400,1.384,1.367,1.351,1.335,1.319,1.303,1.286,1.270,1.254,1.238,1.221,1.205,1.189,1.173,1.157,1.140,1.124,1.108,1.092,1.075,1.059,1.043,1.027,1.010,0.994,0.978,0.961,0.945,0.929,0.913,0.896,0.880,0.864,0.848,0.831,0.815,0.799,0.786,0.769,0.749,0.730,0.710,0.689,0.669,0.649,0.629,0.609,0.589,0.569,0.548,0.528,0.508,0.488,0.468,0.448,0.427,0.407,0.387,0.367,0.347,0.327,0.306,0.286,0.266,0.246,0.226,0.206,0.186,0.166,0.146,0.125,0.105,0.084,0.064,0.044,0.025,0.012,0.004,0.001,0.000,0.000,0.000,0.000]
    j3=[0.000,0.000,-0.002,-0.009,-0.022,-0.042,-0.068,-0.100,-0.139,-0.185,-0.237,-0.296,-0.361,-0.433,-0.511,-0.595,-0.681,-0.767,-0.854,-0.942,-1.027,-1.107,-1.182,-1.249,-1.311,-1.365,-1.414,-1.455,-1.491,-1.519,-1.541,-1.557,-1.566,-1.564,-1.556,-1.543,-1.524,-1.498,-1.465,-1.426,-1.381,-1.328,-1.270,-1.205,-1.133,-1.055,-0.971,-0.885,-0.798,-0.711,-0.624,-0.537,-0.450,-0.362,-0.275,-0.188,-0.101,-0.013,0.074,0.161,0.249,0.336,0.423,0.510,0.598,0.685,0.772,0.859,0.947,1.032,1.112,1.186,1.253,1.314,1.369,1.416,1.458,1.492,1.521,1.542,1.557,1.566,1.564,1.557,1.544,1.524,1.498,1.466,1.427,1.383,1.338,1.293,1.247,1.201,1.155,1.110,1.064,1.018,0.972,0.926,0.881,0.835,0.789,0.743,0.697,0.652,0.606,0.560,0.514,0.468,0.423,0.377,0.331,0.285,0.239,0.194,0.149,0.109,0.076,0.048,0.027,0.012,0.004,0.002,0.001,0.000,0.000,0.000]
    j4=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.027,1.107,1.181,1.249,1.310,1.365,1.413,1.455,1.490,1.519,1.541,1.556,1.565,1.567,1.574,1.587,1.603,1.619,1.636,1.653,1.670,1.686,1.703,1.720,1.737,1.754,1.771,1.788,1.805,1.822,1.839,1.856,1.873,1.890,1.907,1.923,1.940,1.957,1.974,1.991,2.008,2.025,2.042,2.059,2.076,2.093,2.110,2.127,2.144,2.161,2.178,2.195,2.212,2.229,2.246,2.263,2.280,2.297,2.314,2.331,2.344,2.352,2.350,2.343,2.330,2.310,2.284,2.252,2.212,2.167,2.115,2.056,1.991,1.919,1.841,1.760,1.679,1.597,1.515,1.433,1.350,1.268,1.186,1.104,1.022,0.940,0.858,0.776,0.694,0.612,0.530,0.452,0.379,0.312,0.252,0.198,0.151,0.110,0.076,0.048,0.027,0.012,0.004,0.002,0.001,0.000,0.000,0.000]
    j5=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.028,1.108,1.182,1.250,1.311,1.366,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.571,1.571,1.568,1.561,1.548,1.529,1.503,1.470,1.431,1.388,1.342,1.297,1.251,1.205,1.159,1.113,1.067,1.021,0.975,0.929,0.883,0.837,0.791,0.745,0.699,0.653,0.607,0.561,0.516,0.470,0.424,0.378,0.332,0.286,0.240,0.194,0.149,0.109,0.076,0.048,0.027,0.012,0.004,0.002,0.001,0.000,0.000,0.000]
    j6=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.027,1.108,1.182,1.249,1.311,1.365,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.571,1.571,1.571,1.569,1.561,1.548,1.529,1.503,1.470,1.431,1.388,1.343,1.297,1.251,1.205,1.159,1.113,1.067,1.021,0.975,0.929,0.883,0.837,0.791,0.745,0.699,0.653,0.607,0.561,0.515,0.470,0.424,0.378,0.332,0.286,0.240,0.194,0.149,0.109,0.076,0.048,0.027,0.012,0.004,0.002,0.001,0.000,0.000,0.000]

    # Start simulation:
    sim.simxStartSimulation(clientID,sim.simx_opmode_blocking)

    # Wait until ready:
    waitForMovementExecuted('ready') 

    # Send the movement sequence:
    targetConfig=[90*math.pi/180,90*math.pi/180,-90*math.pi/180,90*math.pi/180,90*math.pi/180,90*math.pi/180]
    movementData={"id":"movSeq1","type":"pts","times":times,"j1":j1,"j2":j2,"j3":j3,"j4":j4,"j5":j5,"j6":j6}
    packedMovementData=msgpack.packb(movementData)
    sim.simxCallScriptFunction(clientID,targetArm,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot)

    # Execute movement sequence:
    sim.simxCallScriptFunction(clientID,targetArm,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq1',sim.simx_opmode_oneshot)
    
    # Wait until above movement sequence finished executing:
    waitForMovementExecuted('movSeq1')
Пример #6
0
                                            vrep.simx_opmode_oneshot_wait)

err, Qpos = vrep.simxGetObjectPosition(clientID, Quadricopter, -1,
                                       vrep.simx_opmode_oneshot_wait)

err = vrep.simxSetObjectPosition(clientID, QuadricopterT, -1,
                                 (Qpos[0], Qpos[1], Qpos[2]),
                                 vrep.simx_opmode_oneshot)

err, Qang = vrep.simxGetObjectOrientation(clientID, Quadricopter, -1,
                                          vrep.simx_opmode_oneshot_wait)

err = vrep.simxSetObjectOrientation(clientID, QuadricopterT, -1,
                                    (0, 0, Qang[2]), vrep.simx_opmode_oneshot)

vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
print("start simulation")

max_vel = 0.005 / 0.02


def set_drone_position(point):
    err, Qpos0 = vrep.simxGetObjectPosition(clientID, Quadricopter, -1,
                                            vrep.simx_opmode_oneshot_wait)
    dist = sqrt((point[0] - Qpos[0])**2 + (point[1] - Qpos[1])**2 +
                (point[2] - Qpos[2])**2)
    tp = dist / max_vel
    t0 = time.time()
    while (time.time() - t0 < tp):
        t = time.time() - t0
        for j in range(3):
    def listener_callback(self, msg):
        
        if msg.transforms[0].child_frame_id == 'robot1' :  
            self.x1 = msg.transforms[0].transform.translation.x
            self.y1 = msg.transforms[0].transform.translation.y
            self.xr1 = msg.transforms[0].transform.rotation.x
            self.yr1 = msg.transforms[0].transform.rotation.y
            self.zr1 = msg.transforms[0].transform.rotation.z
            self.wr1 = msg.transforms[0].transform.rotation.w
            self.Theta1 = euler_from_quaternion(self.xr1,self.yr1,self.zr1,self.wr1)
   

        if  msg.transforms[0].child_frame_id == 'robot2' :
            self.x2 = msg.transforms[0].transform.translation.x
            self.y2 = msg.transforms[0].transform.translation.y
            self.xr2 = msg.transforms[0].transform.rotation.x
            self.yr2 = msg.transforms[0].transform.rotation.y
            self.zr2 = msg.transforms[0].transform.rotation.z
            self.wr2 = msg.transforms[0].transform.rotation.w
            self.Theta2 = euler_from_quaternion(self.xr2,self.yr2,self.zr2,self.wr2) 
        
        distance = abs(self.x2 - self.x1) + abs(self.y2 - self.y1)     
        
        print(distance)
        
        # Run Consensus Algorithm as long as they don't meet
        if distance > 1:

            
            " Calculate Control inputs u1 and u2 "
            
            u1 = np.array([[ self.k*(self.x2-self.x1)],[self.k*(self.y2-self.y1)]]) # 2x1 
            u2 = np.array([[ self.k*(self.x1-self.x2)],[self.k*(self.y1-self.y2)]]) # 2x1

            " Calculate V1/W1 and V2/W2 "

            S1 = np.array([[self.v1], [self.w1]]) #2x1
            G1 = np.array([[1,0], [0,1/L]]) #2x2
            R1 = np.array([[math.cos(self.Theta1),math.sin(self.Theta1)],[-math.sin(self.Theta1),math.cos(self.Theta1)]]) #2x2
            S1 = np.dot(np.dot(G1, R1), u1) #2x1
            print(S1)

           
            S2 = np.array([[self.v2], [self.w2]]) #2x1
            G2 = np.array([[1,0], [0,1/L]]) #2x2
            R2 = np.array([[math.cos(self.Theta2),math.sin(self.Theta2)],[-math.sin(self.Theta2),math.cos(self.Theta2)]]) #2x2
            S2 = np.dot(np.dot(G2, R2), u2) # 2x1

            " Calculate VL1/VR1 and VL2/VR2 "

            D = np.array([[1/2,1/2],[-1/(2*d),1/(2*d)]]) #2x2
            Di = np.linalg.inv(D) #2x2

            Speed_L1 = np.array([[self.vL1], [self.vR1]]) # Vector 2x1 for Speed of Robot 1
            Speed_L2 = np.array([[self.vL2], [self.vR2]]) # Vector 2x1 for Speed of Robot 2 
            M1 = np.array([[S1[0]],[S1[1]]]).reshape(2,1) #2x1
            M2 = np.array([[S2[0]], [S2[1]]]).reshape(2,1) #2x1
            Speed_L1 = np.dot(Di, M1) # 2x1 (VL1, VR1)
            Speed_L2 = np.dot(Di, M2) # 2x1 (VL2, VR2)

            VL1 = float(Speed_L1[0])
            VR1 = float(Speed_L1[1])
            VL2 = float(Speed_L2[0])
            VR2 = float(Speed_L2[1])


            " Calculate the Pose of Robot 2 w.r.t Robot 1 and Control input U1 "

            self.X1 = self.x2 - self.x1 # Relative Pose of Robot 2 wrt Robot 1 in Global frame for X coordinate of dimension 1x1
            self.Y1 = self.y2 -self.y1 # Relative Pose of Robot 2 wrt Robot 1 in Global frame for Y coordinate of dimension 1x1
            self.U1 = u1 # Control input U1 in Global frame for robot 1 of dimension 2x1


            " Calculate the Pose of Robot 1 w.r.t Robot 2 and Control input U2 "

            self.X2 = self.x1 - self.x2 # Relative Pose of Robot 1 wrt Robot 2 in Global frame for X coordinate of dimension 1x1
            self.Y2 = self.y1 -self.y2 # Relative Pose of Robot 1 wrt Robot 2 in Global frame for Y coordinate of dimension 1x1
            self.U2 = u2 # Control input U2 in Global frame for robot 2 of dimension 2x1

            " Transform Control Input U1 from Global to Local Reference Frame "
            
            U1L = np.dot(R1, self.U1) # Control input of Robot 1 in Local Frame of dimension 2x1
            U2L = np.dot(R2, self.U2) # Control input of Robot 2 in Local Frame of dimension 2x1

            " Transform Relative Pose from Global to Local Reference Frame "
            
            PoseG1 = np.array([[self.X1],[self.Y1]]) # Relative Pose of Robot 2 wrt Robot 1 in Global Frame of dimension 2x1
            PoseL1 = np.dot(R1, PoseG1) # Relative Pose of Robot 2 wrt Robot 2 in Local Frame of dimension 2x1 
            PoseG2 = np.array([[self.X2],[self.Y2]]) # Relative Pose of Robot 1 wrt Robot 1 in Global Frame of dimension 2x1
            PoseL2 = np.dot(R2, PoseG2) # Relative Pose of Robot 1 wrt Robot 2 in Local Frame of dimension 2x1 

            " Publish Speed Commands to Robot 1"

            msgl1 = Float32()    
            msgr1 = Float32()
            msgl1.data = VL1
            msgr1.data = VR1
            self.publisher_l1.publish(msgl1)
            self.publisher_r1.publish(msgr1)
            #self.get_logger().info('Publishing R1: "%s"' % msgr1.data)
 

            " Publish Speed Commands to Robot 2"

            msgl2 = Float32()
            msgr2 = Float32()
            msgl2.data = VL2
            msgr2.data = VR2
            self.publisher_l2.publish(msgl2)
            self.publisher_r2.publish(msgr2)

            " Write Values to CSV1 and CSV2 "

            
            if self.count % 2 == 0:

                with open('robot1.csv', 'a', newline='') as f:
                    fieldnames = ['Data_X', 'Data_Y', 'Angle', 'Label_X', 'Label_Y']
                    thewriter = csv.DictWriter(f, fieldnames=fieldnames)

                    if self.i1 == 0: # write header value once
                        thewriter.writeheader()
                        self.i1 = 1
    
                    if self.j1 != 0:    
                        thewriter.writerow({'Data_X' : PoseL1[0][0], 'Data_Y' : PoseL1[1][0], 'Angle' : self.Theta1, 'Label_X' : U1L[0][0], 'Label_Y' : U1L[1][0]})
    
                    if self.j1 == 0: # skip first value because it's noisy
                        self.j1 = 1

                with open('robot2.csv', 'a', newline='') as f:
                    fieldnames = ['Data_X', 'Data_Y', 'Angle', 'Label_X', 'Label_Y']
                    thewriter = csv.DictWriter(f, fieldnames=fieldnames)

                    if self.i2 == 0: # write header value once
                        thewriter.writeheader()
                        self.i2 = 1

                    if self.j2 != 0:
                        thewriter.writerow({'Data_X' : PoseL2[0][0], 'Data_Y' : PoseL2[1][0], 'Angle' : self.Theta2, 'Label_X' : U2L[0][0], 'Label_Y' : U2L[1][0]})

                    if self.j2 == 0: # skip first value because it's noisy
                        self.j2 = 1


            self.count += 1 # Counter to skip values while saving to csv file
            
        else:

            print(" Simulation ", self.scene)
            
            
            
            # Stop Simulation
            sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait)  
    
            # Retrieve some handles:
    
            ErrLocM1,LocM1 =sim.simxGetObjectHandle(clientID, 'robot1', sim.simx_opmode_oneshot_wait)
            
            if (not ErrLocM1==sim.simx_return_ok):
                pass
                
            ErrLocM2,LocM2 =sim.simxGetObjectHandle(clientID, 'robot2#0', sim.simx_opmode_oneshot_wait)
            
            if (not ErrLocM2==sim.simx_return_ok):
                pass           
    
            ErrLoc1,Loc1 =sim.simxGetObjectPosition(clientID, LocM1, -1, sim.simx_opmode_oneshot_wait)
            
            if (not ErrLoc1==sim.simx_return_ok):
                pass            
            
            ErrLoc2,Loc2 =sim.simxGetObjectPosition(clientID, LocM2, -1, sim.simx_opmode_oneshot_wait)

            if (not ErrLoc2==sim.simx_return_ok):
                pass     
    
            ErrLocO1,OriRobo1 =sim.simxGetObjectOrientation(clientID,LocM1, -1, sim.simx_opmode_oneshot_wait)
            
            if (not ErrLocO1==sim.simx_return_ok):
                pass             
            
            ErrLocO2,OriRobo2 =sim.simxGetObjectOrientation(clientID,LocM2, -1, sim.simx_opmode_oneshot_wait)

            if (not ErrLocO2==sim.simx_return_ok):
                pass     
    
            OriRobo1[2] = scenes[self.scene][2]
            OriRobo2[2] = scenes[self.scene][5]
    
    
            # Set Robot Orientation
    
            sim.simxSetObjectOrientation(clientID, LocM1, -1, OriRobo1, sim.simx_opmode_oneshot_wait) 
            sim.simxSetObjectOrientation(clientID, LocM2, -1, OriRobo2, sim.simx_opmode_oneshot_wait)
    
    
            Loc1[0] = scenes[self.scene][0]
            Loc2[0] = scenes[self.scene][3]
    
    
            Loc1[1] = scenes[self.scene][1]
            Loc2[1] = scenes[self.scene][4]
    
            # Set Robot Position
    
            sim.simxSetObjectPosition(clientID, LocM1, -1, Loc1, sim.simx_opmode_oneshot)
            sim.simxSetObjectPosition(clientID, LocM2, -1, Loc2, sim.simx_opmode_oneshot)
    
            # Print Positions and Orientation
    
            #print("Robot1 Position:", Loc1)
            #print("Robot2 Position:", Loc2)
    
            #print("Robot1 Orientation:", OriRobo1)
            #print("Robot2 Orientation:", OriRobo2)
                        
            # Nb of Scene Counter
            self.scene += 1
            
            # Start Simulation
            sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait)
            
            time.sleep(3)
            
        if self.scene == scenes.shape[0]-1:
            # Stop Simulation 
            sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait)
            # End Connection to V-Rep
            sim.simxFinish(clientID)                         
Пример #8
0
    def listener_callback(self, msg):

        if msg.transforms[0].child_frame_id == 'robot1':
            self.x1 = msg.transforms[0].transform.translation.x
            self.y1 = msg.transforms[0].transform.translation.y
            self.xr1 = msg.transforms[0].transform.rotation.x
            self.yr1 = msg.transforms[0].transform.rotation.y
            self.zr1 = msg.transforms[0].transform.rotation.z
            self.wr1 = msg.transforms[0].transform.rotation.w
            self.Theta1 = euler_from_quaternion(self.xr1, self.yr1, self.zr1,
                                                self.wr1)

        if msg.transforms[0].child_frame_id == 'robot2':

            self.x2 = msg.transforms[0].transform.translation.x
            self.y2 = msg.transforms[0].transform.translation.y
            self.xr2 = msg.transforms[0].transform.rotation.x
            self.yr2 = msg.transforms[0].transform.rotation.y
            self.zr2 = msg.transforms[0].transform.rotation.z
            self.wr2 = msg.transforms[0].transform.rotation.w
            self.Theta2 = euler_from_quaternion(self.xr2, self.yr2, self.zr2,
                                                self.wr2)

        if msg.transforms[0].child_frame_id == 'robot3':

            self.x3 = msg.transforms[0].transform.translation.x
            self.y3 = msg.transforms[0].transform.translation.y
            self.xr3 = msg.transforms[0].transform.rotation.x
            self.yr3 = msg.transforms[0].transform.rotation.y
            self.zr3 = msg.transforms[0].transform.rotation.z
            self.wr3 = msg.transforms[0].transform.rotation.w
            self.Theta3 = euler_from_quaternion(self.xr3, self.yr3, self.zr3,
                                                self.wr3)

        if msg.transforms[0].child_frame_id == 'robot4':

            self.x4 = msg.transforms[0].transform.translation.x
            self.y4 = msg.transforms[0].transform.translation.y
            self.xr4 = msg.transforms[0].transform.rotation.x
            self.yr4 = msg.transforms[0].transform.rotation.y
            self.zr4 = msg.transforms[0].transform.rotation.z
            self.wr4 = msg.transforms[0].transform.rotation.w
            self.Theta4 = euler_from_quaternion(self.xr4, self.yr4, self.zr4,
                                                self.wr4)

        if msg.transforms[0].child_frame_id == 'robot5':

            self.x5 = msg.transforms[0].transform.translation.x
            self.y5 = msg.transforms[0].transform.translation.y
            self.xr5 = msg.transforms[0].transform.rotation.x
            self.yr5 = msg.transforms[0].transform.rotation.y
            self.zr5 = msg.transforms[0].transform.rotation.z
            self.wr5 = msg.transforms[0].transform.rotation.w
            self.Theta5 = euler_from_quaternion(self.xr5, self.yr5, self.zr5,
                                                self.wr5)

        if msg.transforms[0].child_frame_id == 'robot6':

            self.x6 = msg.transforms[0].transform.translation.x
            self.y6 = msg.transforms[0].transform.translation.y
            self.xr6 = msg.transforms[0].transform.rotation.x
            self.yr6 = msg.transforms[0].transform.rotation.y
            self.zr6 = msg.transforms[0].transform.rotation.z
            self.wr6 = msg.transforms[0].transform.rotation.w
            self.Theta6 = euler_from_quaternion(self.xr6, self.yr6, self.zr6,
                                                self.wr6)

        self.distance = abs(self.x1 - self.x2) + abs(self.y1 - self.y2) + abs(
            self.x1 -
            self.x3) + abs(self.y1 - self.y3) + abs(self.x1 - self.x4) + abs(
                self.y1 - self.y4) + abs(self.x1 - self.x5) + abs(
                    self.y1 - self.y5) + abs(self.x1 - self.x6) + abs(self.y1 -
                                                                      self.y6)

        print(self.distance)

        if self.distance > 2.2:

            " Calculate Control inputs u1, u2, u3, u4, u5, u6 "

            A = np.ones(6) - np.identity(6)  # Adjancency Matrix

            self.X = np.array([[self.x1], [self.x2], [self.x3], [self.x4],
                               [self.x5], [self.x6]])  #6x1
            self.Y = np.array([[self.y1], [self.y2], [self.y3], [self.y4],
                               [self.y5], [self.y6]])  #6x1

            ux = np.zeros((6, 1))  # 6x1
            uy = np.zeros((6, 1))  # 6x1

            for i in range(1, 7):
                for j in range(1, 7):
                    ux[i - 1] += -(A[i - 1][j - 1]) * (
                        self.X[i - 1] - self.X[j - 1])  # 1x1 each
                    uy[i - 1] += -(A[i - 1][j - 1]) * (
                        self.Y[i - 1] - self.Y[j - 1])  # 1x1 each

            u1 = np.array([[float(ux[0])], [float(uy[0])]])  # 2x1
            u2 = np.array([[float(ux[1])], [float(uy[1])]])  # 2x1
            u3 = np.array([[float(ux[2])], [float(uy[2])]])  # 2x1
            u4 = np.array([[float(ux[3])], [float(uy[3])]])  # 2x1
            u5 = np.array([[float(ux[4])], [float(uy[4])]])  # 2x1
            u6 = np.array([[float(ux[5])], [float(uy[5])]])  # 2x1

            " Data Transformation into M_x, M_y, Phi_x, Phi_y for Edge Robot "

            Phix1 = u2[0][0]  # 1x1
            Phiy1 = u2[1][0]  # 1x1

            Mx1 = (self.x2 - self.x1)  # 1x1
            My1 = (self.y2 - self.y1)  # 1x1

            " Data Transformation into M_x, M_y, Phi_x, Phi_y for Middle Robot "

            Phix2 = (u1[0][0] + u3[0][0]) / 2  # 1x1
            Phiy2 = (u1[1][0] + u3[1][0]) / 2  # 1x1

            Mx2 = ((self.x1 - self.x2) + (self.x3 - self.x2)) / 2  # 1x1
            My2 = ((self.y1 - self.y2) + (self.y3 - self.y2)) / 2  # 1x1

            " Calculate V1/W1, V2/W2, V3/W3, V4/W4, V5/W5, V6/W6 "

            S1 = np.array([[self.v1], [self.w1]])  #2x1
            G1 = np.array([[1, 0], [0, 1 / L]])  #2x2
            R1 = np.array([[math.cos(self.Theta1),
                            math.sin(self.Theta1)],
                           [-math.sin(self.Theta1),
                            math.cos(self.Theta1)]])  #2x2
            S1 = np.dot(np.dot(G1, R1), u1)  #2x1

            S2 = np.array([[self.v2], [self.w2]])  #2x1
            G2 = np.array([[1, 0], [0, 1 / L]])  #2x2
            R2 = np.array([[math.cos(self.Theta2),
                            math.sin(self.Theta2)],
                           [-math.sin(self.Theta2),
                            math.cos(self.Theta2)]])  #2x2
            S2 = np.dot(np.dot(G2, R2), u2)  # 2x1

            S3 = np.array([[self.v3], [self.w3]])  #2x1
            G3 = np.array([[1, 0], [0, 1 / L]])  #2x2
            R3 = np.array([[math.cos(self.Theta3),
                            math.sin(self.Theta3)],
                           [-math.sin(self.Theta3),
                            math.cos(self.Theta3)]])  #2x2
            S3 = np.dot(np.dot(G3, R3), u3)  #2x1

            S4 = np.array([[self.v4], [self.w4]])  #2x1
            G4 = np.array([[1, 0], [0, 1 / L]])  #2x2
            R4 = np.array([[math.cos(self.Theta4),
                            math.sin(self.Theta4)],
                           [-math.sin(self.Theta4),
                            math.cos(self.Theta4)]])  #2x2
            S4 = np.dot(np.dot(G4, R4), u4)  #2x1

            S5 = np.array([[self.v5], [self.w5]])  #2x1
            G5 = np.array([[1, 0], [0, 1 / L]])  #2x2
            R5 = np.array([[math.cos(self.Theta5),
                            math.sin(self.Theta5)],
                           [-math.sin(self.Theta5),
                            math.cos(self.Theta5)]])  #2x2
            S5 = np.dot(np.dot(G5, R5), u5)  #2x1

            S6 = np.array([[self.v6], [self.w6]])  #2x1
            G6 = np.array([[1, 0], [0, 1 / L]])  #2x2
            R6 = np.array([[math.cos(self.Theta6),
                            math.sin(self.Theta6)],
                           [-math.sin(self.Theta6),
                            math.cos(self.Theta6)]])  #2x2
            S6 = np.dot(np.dot(G6, R6), u6)  #2x1

            " Calculate VL1/VR1, VL2/VR2, VL3/VR3, VL4/VR4, VL5/VR5, VL6/VR6 "

            D = np.array([[1 / 2, 1 / 2], [-1 / (2 * d), 1 / (2 * d)]])  #2x2
            Di = np.linalg.inv(D)  #2x2

            Speed_L1 = np.array([[self.vL1], [self.vR1]
                                 ])  # Vector 2x1 for Speed of Robot 1
            Speed_L2 = np.array([[self.vL2], [self.vR2]
                                 ])  # Vector 2x1 for Speed of Robot 2
            Speed_L3 = np.array([[self.vL3], [self.vR3]
                                 ])  # Vector 2x1 for Speed of Robot 3
            Speed_L4 = np.array([[self.vL4], [self.vR4]
                                 ])  # Vector 2x1 for Speed of Robot 4
            Speed_L5 = np.array([[self.vL5], [self.vR5]
                                 ])  # Vector 2x1 for Speed of Robot 5
            Speed_L6 = np.array([[self.vL6], [self.vR6]
                                 ])  # Vector 2x1 for Speed of Robot 6

            M1 = np.array([[S1[0]], [S1[1]]]).reshape(2, 1)  #2x1
            M2 = np.array([[S2[0]], [S2[1]]]).reshape(2, 1)  #2x1
            M3 = np.array([[S3[0]], [S3[1]]]).reshape(2, 1)  #2x1
            M4 = np.array([[S4[0]], [S4[1]]]).reshape(2, 1)  #2x1
            M5 = np.array([[S5[0]], [S5[1]]]).reshape(2, 1)  #2x1
            M6 = np.array([[S6[0]], [S6[1]]]).reshape(2, 1)  #2x1

            Speed_L1 = np.dot(Di, M1)  # 2x1 (VL1, VR1)
            Speed_L2 = np.dot(Di, M2)  # 2x1 (VL2, VR2)
            Speed_L3 = np.dot(Di, M3)  # 2x1 (VL3, VR3)
            Speed_L4 = np.dot(Di, M4)  # 2x1 (VL4, VR4)
            Speed_L5 = np.dot(Di, M5)  # 2x1 (VL5, VR5)
            Speed_L6 = np.dot(Di, M6)  # 2x1 (VL6, VR6)

            VL1 = float(Speed_L1[0])
            VR1 = float(Speed_L1[1])
            VL2 = float(Speed_L2[0])
            VR2 = float(Speed_L2[1])
            VL3 = float(Speed_L3[0])
            VR3 = float(Speed_L3[1])
            VL4 = float(Speed_L4[0])
            VR4 = float(Speed_L4[1])
            VL5 = float(Speed_L5[0])
            VR5 = float(Speed_L5[1])
            VL6 = float(Speed_L6[0])
            VR6 = float(Speed_L6[1])

            " Publish Speed Commands to Robot 1 "

            msgl1 = Float32()
            msgr1 = Float32()
            msgl1.data = VL1
            msgr1.data = VR1
            self.publisher_l1.publish(msgl1)
            self.publisher_r1.publish(msgr1)
            #self.get_logger().info('Publishing R1: "%s"' % msgr1.data)

            " Publish Speed Commands to Robot 2 "

            msgl2 = Float32()
            msgr2 = Float32()
            msgl2.data = VL2
            msgr2.data = VR2
            self.publisher_l2.publish(msgl2)
            self.publisher_r2.publish(msgr2)

            " Publish Speed Commands to Robot 3 "

            msgl3 = Float32()
            msgr3 = Float32()
            msgl3.data = VL3
            msgr3.data = VR3
            self.publisher_l3.publish(msgl3)
            self.publisher_r3.publish(msgr3)

            " Publish Speed Commands to Robot 4 "

            msgl4 = Float32()
            msgr4 = Float32()
            msgl4.data = VL4
            msgr4.data = VR4
            self.publisher_l4.publish(msgl4)
            self.publisher_r4.publish(msgr4)

            " Publish Speed Commands to Robot 5 "

            msgl5 = Float32()
            msgr5 = Float32()
            msgl5.data = VL5
            msgr5.data = VR5
            self.publisher_l5.publish(msgl5)
            self.publisher_r5.publish(msgr5)

            " Publish Speed Commands to Robot 6 "

            msgl6 = Float32()
            msgr6 = Float32()
            msgl6.data = VL6
            msgr6.data = VR6
            self.publisher_l6.publish(msgl6)
            self.publisher_r6.publish(msgr6)

            " Write Values to CSV1 and CSV2 "

            if self.count % 2 == 0:

                with open('edge_robot.csv', 'a', newline='') as f:
                    fieldnames = ['M_x', 'M_y', 'Phi_x', 'Phi_y', 'U_x', 'U_y']
                    thewriter = csv.DictWriter(f, fieldnames=fieldnames)

                    if self.i1 == 0:  # write header value once
                        thewriter.writeheader()
                        self.i1 = 1

                    if self.j1 != 0:
                        thewriter.writerow({
                            'M_x': Mx1,
                            'M_y': My1,
                            'Phi_x': Phix1,
                            'Phi_y': Phiy1,
                            'U_x': u1[0][0],
                            'U_y': u1[1][0]
                        })

                    if self.j1 == 0:  # skip first value because it's noisy
                        self.j1 = 1

                with open('middle_robot.csv', 'a', newline='') as f:
                    fieldnames = ['M_x', 'M_y', 'Phi_x', 'Phi_y', 'U_x', 'U_y']
                    thewriter = csv.DictWriter(f, fieldnames=fieldnames)

                    if self.i2 == 0:  # write header value once
                        thewriter.writeheader()
                        self.i2 = 1

                    if self.j2 != 0:
                        thewriter.writerow({
                            'M_x': Mx2,
                            'M_y': My2,
                            'Phi_x': Phix2,
                            'Phi_y': Phiy2,
                            'U_x': u2[0][0],
                            'U_y': u2[1][0]
                        })

                    if self.j2 == 0:  # skip first value because it's noisy
                        self.j2 = 1

            self.count += 0.5  # Counter to skip values while saving to csv file

        else:

            print(" Simulation ", self.scene)

            # Stop Simulation
            sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait)

            # Retrieve some handles:

            ErrLocM1, LocM1 = sim.simxGetObjectHandle(
                clientID, 'robot1', sim.simx_opmode_oneshot_wait)

            if (not ErrLocM1 == sim.simx_return_ok):
                pass

            ErrLocM2, LocM2 = sim.simxGetObjectHandle(
                clientID, 'robot2#0', sim.simx_opmode_oneshot_wait)

            if (not ErrLocM2 == sim.simx_return_ok):
                pass

            ErrLoc1, Loc1 = sim.simxGetObjectPosition(
                clientID, LocM1, -1, sim.simx_opmode_oneshot_wait)

            if (not ErrLoc1 == sim.simx_return_ok):
                pass

            ErrLoc2, Loc2 = sim.simxGetObjectPosition(
                clientID, LocM2, -1, sim.simx_opmode_oneshot_wait)

            if (not ErrLoc2 == sim.simx_return_ok):
                pass

            ErrLocO1, OriRobo1 = sim.simxGetObjectOrientation(
                clientID, LocM1, -1, sim.simx_opmode_oneshot_wait)

            if (not ErrLocO1 == sim.simx_return_ok):
                pass

            ErrLocO2, OriRobo2 = sim.simxGetObjectOrientation(
                clientID, LocM2, -1, sim.simx_opmode_oneshot_wait)

            if (not ErrLocO2 == sim.simx_return_ok):
                pass

            # Take handles of the other robotos
            ErrLocM3, LocM3 = sim.simxGetObjectHandle(
                clientID, 'robot3#1', sim.simx_opmode_oneshot_wait)
            if (not ErrLocM3 == sim.simx_return_ok):
                pass

            ErrLocM4, LocM4 = sim.simxGetObjectHandle(
                clientID, 'robot4#2', sim.simx_opmode_oneshot_wait)
            if (not ErrLocM4 == sim.simx_return_ok):
                pass

            ErrLocM5, LocM5 = sim.simxGetObjectHandle(
                clientID, 'robot5#3', sim.simx_opmode_oneshot_wait)
            if (not ErrLocM5 == sim.simx_return_ok):
                pass

            ErrLocM6, LocM6 = sim.simxGetObjectHandle(
                clientID, 'robot6#4', sim.simx_opmode_oneshot_wait)
            if (not ErrLocM6 == sim.simx_return_ok):
                pass

            OriRobo1[2] = scenes[self.scene][2]
            OriRobo2[2] = scenes[self.scene][5]

            # Set Robot Orientation

            sim.simxSetObjectOrientation(clientID, LocM1, -1, OriRobo1,
                                         sim.simx_opmode_oneshot_wait)
            sim.simxSetObjectOrientation(clientID, LocM2, -1, OriRobo2,
                                         sim.simx_opmode_oneshot_wait)

            Loc1[0] = scenes[self.scene][0]
            Loc2[0] = scenes[self.scene][3]

            Loc1[1] = scenes[self.scene][1]
            Loc2[1] = scenes[self.scene][4]

            # Check if robot1 is close to any other robot (except robot2)
            #for i in range(1,4):
            too_close_dist = 0.4
            ErrLoc3, Loc3 = sim.simxGetObjectPosition(
                clientID, LocM3, -1, sim.simx_opmode_oneshot_wait)
            if (not ErrLocM3 == sim.simx_return_ok):
                pass
            ErrLoc4, Loc4 = sim.simxGetObjectPosition(
                clientID, LocM4, -1, sim.simx_opmode_oneshot_wait)
            if (not ErrLocM4 == sim.simx_return_ok):
                pass
            ErrLoc5, Loc5 = sim.simxGetObjectPosition(
                clientID, LocM5, -1, sim.simx_opmode_oneshot_wait)
            if (not ErrLocM5 == sim.simx_return_ok):
                pass
            ErrLoc6, Loc6 = sim.simxGetObjectPosition(
                clientID, LocM6, -1, sim.simx_opmode_oneshot_wait)
            if (not ErrLocM6 == sim.simx_return_ok):
                pass
            exit_cond = 0
            print(" -------- ELEMENT 1 ------------")
            print(Loc1)
            print(" --------- ELEMENT 2 -----------")
            print(Loc3)
            while exit_cond == 0:
                print(Loc1)
                distr13 = np.sqrt(
                    pow(Loc1[0] - Loc3[0], 2) + pow(Loc1[1] - Loc3[1], 2))
                distr14 = np.sqrt(
                    pow(Loc1[0] - Loc4[0], 2) + pow(Loc1[1] - Loc4[1], 2))
                distr15 = np.sqrt(
                    pow(Loc1[0] - Loc5[0], 2) + pow(Loc1[1] - Loc5[1], 2))
                distr16 = np.sqrt(
                    pow(Loc1[0] - Loc6[0], 2) + pow(Loc1[1] - Loc6[1], 2))
                if (distr13 < too_close_dist) or (
                        distr14 < too_close_dist
                ) or (distr15 < too_close_dist) or (distr16 < too_close_dist):
                    # TOO close. Select another starting point for robot 1
                    Loc1 = np.random.uniform(-2, 2, size=(1, 2))[0]
                    print(" Robot 1 is close!")
                else:
                    exit_cond = 1

            # In Loc1 there is here a valid inizialization of robot1
            scenes[self.scene][0] = Loc1[0]
            scenes[self.scene][1] = Loc1[1]

            # Check if the robot2 is too close to other robots
            exit_cond = 0
            while exit_cond == 0:
                dist21 = np.sqrt(
                    pow(Loc2[0] - Loc1[0], 2) + pow(Loc2[1] - Loc1[1], 2))
                dist23 = np.sqrt(
                    pow(Loc2[0] - Loc3[0], 2) + pow(Loc2[1] - Loc3[1], 2))
                dist24 = np.sqrt(
                    pow(Loc2[0] - Loc4[0], 2) + pow(Loc2[1] - Loc4[1], 2))
                dist25 = np.sqrt(
                    pow(Loc2[0] - Loc5[0], 2) + pow(Loc2[1] - Loc5[1], 2))
                dist26 = np.sqrt(
                    pow(Loc2[0] - Loc6[0], 2) + pow(Loc2[1] - Loc6[1], 2))

                if dist21 < too_close_dist or dist23 < too_close_dist or dist24 < too_close_dist or dist25 < too_close_dist or dist26 < too_close_dist:
                    # TOO close. Select another starting point for robot 2
                    Loc2 = np.random.uniform(-2, 2, size=(1, 2))[0]
                    print(" Robot 2 is close!")
                else:
                    exit_cond = 1

            # In Loc2 there is here a valid inizialization of robot2
            scenes[self.scene][3] = Loc2[0]
            scenes[self.scene][4] = Loc2[1]

            # Set Robot Position

            sim.simxSetObjectPosition(clientID, LocM1, -1, Loc1,
                                      sim.simx_opmode_oneshot)
            sim.simxSetObjectPosition(clientID, LocM2, -1, Loc2,
                                      sim.simx_opmode_oneshot)

            # Print Positions and Orientation

            #print("Robot1 Position:", Loc1)
            #print("Robot2 Position:", Loc2)

            #print("Robot1 Orientation:", OriRobo1)
            #print("Robot2 Orientation:", OriRobo2)

            # Nb of Scene Counter
            self.scene += 1

            # Start Simulation
            sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait)

            time.sleep(5)

        if self.scene == scenes.shape[0] - 1:
            # Stop Simulation
            sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait)
            # End Connection to V-Rep
            sim.simxFinish(clientID)
Пример #9
0
def main():
    print('Program started')

    sim.simxFinish(-1)  # just in case, close all opened connections
    clientID = sim.simxStart(COPPELIA_SIM_IP_ADDRESS, COPPELIA_SIM_PORT, True,
                             True, 2000, 5)  # Connect to CoppeliaSim

    # lower e upper são vetores pra detecção da cor vermelha, cada elemento do vetor é um dos canais RGB (invertido por causa do opencv).
    # então o limite inferior pra detecção seria o valor 0 para a cor azul, 0 para a cor verde e 100 para a cor vermelha.
    # a análise é análoga para o vetor upper.
    lower = np.uint8([0, 0, 100])
    upper = np.uint8([60, 60, 255])

    # nós temos 8 targets, portanto para pegar as suas referências (handles), é preciso iterar sobre eles, daí vem o targetIterator.
    targetIterator = 1

    if (clientID != -1):

        print('Connection Successful')

        # pega as referências de todos os elementos a serem manipulados.
        # perceba que só pega a referência do primeiro alvo porque só pegamos a referência de um alvo por vez.
        err1, ddRobotHandle = sim.simxGetObjectHandle(
            clientID, 'RobotFrame#', sim.simx_opmode_oneshot_wait)
        err2, leftMotorHandle = sim.simxGetObjectHandle(
            clientID, 'LeftMotor#', sim.simx_opmode_oneshot_wait)
        err3, rightMotorHandle = sim.simxGetObjectHandle(
            clientID, 'RightMotor#', sim.simx_opmode_oneshot_wait)
        err4, revoluteJointHandle = sim.simxGetObjectHandle(
            clientID, 'RevoluteJoint#', sim.simx_opmode_oneshot_wait)
        err5, visionSensorHandle = sim.simxGetObjectHandle(
            clientID, 'VisionSensor#', sim.simx_opmode_oneshot_wait)
        err6, targetHandle = sim.simxGetObjectHandle(
            clientID, 'Target' + str(targetIterator) + '#',
            sim.simx_opmode_oneshot_wait)
        targetIterator += 1

        # pega a referência dos sensores ultrassônicos (usamos 7 sensores).
        # atenção: sensores ultrassônicos != vision sensor
        usensors = []
        for i in range(1, 8):
            err, usensor = sim.simxGetObjectHandle(
                clientID, 'Proximity_sensor' + str(i) + '#',
                sim.simx_opmode_oneshot_wait)
            usensors.append(usensor)

        # a leitura dos sensores não pode bloquear o loop de execução do robô (while mais abaixo), portanto o coppelia oferece o opmode streaming,
        # isto é, quando você manda a leitura começar a ser feita com streaming, ele mantém-se fazendo a leitura e colocando os resultados em um buffer
        # exclusivo para aquele handle (7 handles 7 buffers). Portanto só precisamos a cada iteração ler do buffer, e aí não é preciso pedir que o simulador
        # faça uma nova leitura, economizando tempo e não bloqueando o loop de execução do robô.
        for i in range(7):
            x, a, b, c, d = sim.simxReadProximitySensor(
                clientID, usensors[i], sim.simx_opmode_streaming)

        # a mesma regra dos sensores ultrassônicos descrita acima se aplica ao vision sensor (e a qualquer sensor que poderia vir a ser usado).
        a, b, image = sim.simxGetVisionSensorImage(clientID,
                                                   visionSensorHandle, 0,
                                                   sim.simx_opmode_streaming)

        print(f'RobotFrame: {ddRobotHandle}')
        print(f'LeftMotor: {leftMotorHandle}')
        print(f'RightMotor: {rightMotorHandle}')
        print(f'RevoluteJoint: {revoluteJointHandle}')
        print(f'VisionSensor: {visionSensorHandle}')

        ret = sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait)
        if (ret == -1):
            print('Connection Failed.')
            exit(-1)
        print('Simulation Started')

        while (sim.simxGetConnectionId(clientID) != -1):
            # posicao atual do robo
            pos = getPosition(clientID, ddRobotHandle)
            #print(f'posX: {pos[0]:.3f} posY: {pos[1]:.3f}', end=' ')
            # target atual do robo
            goal = getPosition(clientID, targetHandle)
            #print(f'goalX: {goal[0]:.3f} goalY: {goal[1]:.3f}', end=' ')

            # observe o simx_opmode_buffer. nós não pedimos uma nova leitura do vision sensor, nós simplesmente pegamos a leitura do buffer.
            a, b, image = sim.simxGetVisionSensorImage(clientID,
                                                       visionSensorHandle, 0,
                                                       sim.simx_opmode_buffer)
            # operações feitas para organizar a imagem do vision sensor.
            newImage = np.uint8(image).reshape((b[0], b[0], 3))
            # definitiveImage é autoexplicativa, mas atenção, pois ela se encontra com a codificação BGR.
            definitiveImage = cv2.cvtColor(np.flip(newImage, 0),
                                           cv2.COLOR_RGB2BGR)
            # mask é simplesmente uma outra imagem com a mesma resolução de definitiveImage, porém só mostrando objetos vermelhos, isto é,
            # valor 255 nos pixels correspondentes a pixels vermelhos na imagem original, e 0 em todos os outros pixels.
            mask = cv2.inRange(definitiveImage, lower, upper)

            # distancia do alvo para o robô.
            dx = goal[0] - pos[0]
            dy = goal[1] - pos[1]
            euclid = math.sqrt(dx * dx + dy * dy)
            # print(euclid)

            # se o alvo está perto o suficiente faça:
            if (euclid <= 0.23):
                # se na mask só tem pixels diferentes de 0, portanto só pixels vermelhos faça:
                if (0.0 not in mask):
                    # para o robô e manda a hélice girar no sentido anti-horário por aproximadamente três segundos.
                    phiL, phiR, phiH = 0, 0, 2
                    setTargetSpeed(clientID, phiL, phiR, phiH, leftMotorHandle,
                                   rightMotorHandle, revoluteJointHandle)
                    time.sleep(3)
                    setTargetSpeed(clientID, phiL, phiR, 0, leftMotorHandle,
                                   rightMotorHandle, revoluteJointHandle)
                # se não, o alvo é azul.
                else:
                    # então para o robô e manda a hélice girar no sentido horário por aproximadamente três segundos.
                    phiL, phiR, phiH = 0, 0, -2
                    setTargetSpeed(clientID, phiL, phiR, phiH, leftMotorHandle,
                                   rightMotorHandle, revoluteJointHandle)
                    time.sleep(3)
                    setTargetSpeed(clientID, phiL, phiR, 0, leftMotorHandle,
                                   rightMotorHandle, revoluteJointHandle)

                #se ainda há alvos
                if (targetIterator <= 8):
                    # pega a referência do próximo alvo.
                    err6, targetHandle = sim.simxGetObjectHandle(
                        clientID, 'Target' + str(targetIterator) + '#',
                        sim.simx_opmode_oneshot_wait)
                    targetIterator += 1
                # se não encerra o programa e pausa a simulação.
                else:
                    break
            # se o alvo ainda está longe, continua indo até ele com a hélice parada.
            else:
                phiL, phiR = movementControl(pos, goal, clientID, usensors)
                phiH = 0
                setTargetSpeed(clientID, phiL, phiR, phiH, leftMotorHandle,
                               rightMotorHandle, revoluteJointHandle)

        # descomente estas duas linhas se você quiser salvar as imagens capturadas.
        # imageItarator = 0
        #cv2.imwrite('capture' + str(imageIterator) + '.png', definitiveImage)
        #imageIterator += 1

        setTargetSpeed(clientID, phiL, phiR, phiH, leftMotorHandle,
                       rightMotorHandle, revoluteJointHandle)
        sim.simxPauseSimulation(clientID, sim.simx_opmode_oneshot_wait)
        sim.simxFinish(clientID)

    else:
        print('Connection failed.')
        exit(-2)
Пример #10
0
    ## faça uma nova leitura, economizando tempo e não bloqueando o loop de execução do robô.
    for i in range(7):
        x, a, b, c, d = sim.simxReadProximitySensor(clientID, usensors[i],
                                                    sim.simx_opmode_streaming)

    ## a mesma regra dos sensores ultrassônicos descrita acima se aplica ao vision sensor (e a qualquer sensor que poderia vir a ser usado).
    a, b, image = sim.simxGetVisionSensorImage(clientID, visionSensorHandle, 0,
                                               sim.simx_opmode_streaming)

    print(f'RobotFrame: {ddRobotHandle}')
    print(f'LeftMotor: {leftMotorHandle}')
    print(f'RightMotor: {rightMotorHandle}')
    print(f'RevoluteJoint: {revoluteJointHandle}')
    print(f'VisionSensor: {visionSensorHandle}')

    ret = sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait)
    if (ret == -1):
        print('Connection Failed.')
        exit(-1)
    print('Simulation Started')

    while (sim.simxGetConnectionId(clientID) != -1):
        pos = getPosition(clientID, ddRobotHandle)
        #print(f'posX: {pos[0]:.3f} posY: {pos[1]:.3f}', end=' ')
        goal = getPosition(clientID, targetHandle)
        #print(f'goalX: {goal[0]:.3f} goalY: {goal[1]:.3f}', end=' ')

        ## observe o simx_opmode_buffer. nós não pedimos uma nova leitura do vision sensor, nós simplesmente pegamos a leitura do buffer.
        a, b, image = sim.simxGetVisionSensorImage(clientID,
                                                   visionSensorHandle, 0,
                                                   sim.simx_opmode_buffer)
Пример #11
0
def main():
    sim.simxFinish(-1)

    clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
    if clientID != -1:
        print('Connected to remote API server')
    else:
        print('Failed connecting to remote API server')
        sys.exit('Could not connect to remote API server')
    sim.simxSynchronous(clientID, 1)  #synchronous operation necessary for
    sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot)

    jointHandles = [-1, -1, -1, -1, -1, -1]

    move_helper.getJointHandles(clientID, jointHandles)

    #handle of UR3
    base_handle = sim.simxGetObjectHandle(clientID, "UR3",
                                          sim.simx_opmode_blocking)[1]

    #handle for end-effector
    end_effector_handle = sim.simxGetObjectHandle(clientID, "UR3_link7",
                                                  sim.simx_opmode_blocking)[1]

    #position of UR3 end effector wrt UR3 base frame
    end_effector_wrt_base = sim.simxGetObjectPosition(
        clientID, end_effector_handle, base_handle,
        sim.simx_opmode_blocking)[1]

    #handle for the ball
    ball_handle = sim.simxGetObjectHandle(clientID, 'Ball',
                                          sim.simx_opmode_blocking)[1]

    #handle for ball sensor/proximity sensor
    sensorHandle = sim.simxGetObjectHandle(clientID, 'Ball_Sensor',
                                           sim.simx_opmode_blocking)[1]

    #position and orientation of proximity sensore wrt UR3 base frame
    T_sensor_in_base = np.array( \
        [[0, 0, -1, end_effector_wrt_base[0]],
        [0, -1, 0,  end_effector_wrt_base[1]], \
        [-1, 0, 0,  end_effector_wrt_base[2]],
        [0, 0, 0, 1]])

    #the actual end point of the ball based on path in the world frame

    path = generate_path()
    end_pt = path[path.shape[0] - 1]
    detectedPoints = np.zeros((2, 3))
    #the position and orientation of the UR3 base frame wrt world frame (T_wb)
    T_base_in_world = np.array([[-1, 0, 0, -1.4001], [0, -1, 0, -0.000086],
                                [0, 0, 1, 0.043], [0, 0, 0, 1]])
    #T_bw
    T_world_in_base = np.linalg.inv(T_base_in_world)
    #end pt coords in homog form
    end_pt_homogenous_coords = np.array([[end_pt[0], end_pt[1], end_pt[2],
                                          1]]).T
    #T_bw * p_w = p_b (4,4) * (4, 1) -> (4, 1), left out final element -> (3, 1)
    #the actual endpt of the ball in the base frame
    end_pt_ball_in_base = np.matmul(T_world_in_base,
                                    end_pt_homogenous_coords)[:3]

    detection_thread = threading.Thread(target=utils.calc_ball_position,
                                        args=(clientID, sensorHandle, path,
                                              detectedPoints))
    motion_thread = threading.Thread(target=utils.move_ball,
                                     args=(clientID, ball_handle, path))
    motion_thread.start()
    detection_thread.start()
    detection_thread.join()
    #point_in_sensorX - homogeneous coords of detectedPoints in the frame of the proximity sensor
    point_in_sensor1 = np.array([[detectedPoints[0][0]],
                                 [detectedPoints[0][1]],
                                 [detectedPoints[0][2]], [1]])
    point_in_sensor2 = np.array([[detectedPoints[1][0]],
                                 [detectedPoints[1][1]],
                                 [detectedPoints[1][2]], [1]])

    #T_bs * p_s = p_b
    point1_ball_in_base = np.matmul(T_sensor_in_base, point_in_sensor1)[:3]
    point2_ball_in_base = np.matmul(T_sensor_in_base, point_in_sensor2)[:3]
    vector = (1.0 * point2_ball_in_base - point1_ball_in_base)

    #convert to unit vector
    vector /= np.linalg.norm(vector)
    t = (end_effector_wrt_base[0] - point2_ball_in_base[0]) / vector[0]
    final_x = end_effector_wrt_base[0]
    final_y = point2_ball_in_base[1] + (t * vector[1])
    final_z = point2_ball_in_base[2] + (t * vector[2])
    predicted_point = np.array([[final_x], [final_y[0]], [final_z[0]]])
    difference = predicted_point - end_pt_ball_in_base
    print("Difference: " + str(difference))

    #the predicted direction the ball is moving in, based off of predicted points 1 and 2

    #if the difference between detected points is too small
    if (vector[0] == 0):
        vector[0] += 0.000000001

    #find the estimated position of the ball when it is in the yz plane of the UR3

    #the predicted point of the ball

    print("Predicted Point: " + str(predicted_point))
    print("End Point Ball in Base: " + str(end_pt_ball_in_base))
    #compare prediction to actual end pt of ball

    #our initial guess of what the UR3 joint angles should be to get to finalT
    thetas = np.array([0.0, 0.0, 0.0])

    #get a rough initial guess for theta of joint 2
    predicted_y = predicted_point[1]
    predicted_z = predicted_point[2]

    position_joint2 = sim.simxGetObjectPosition(clientID, jointHandles[1],
                                                base_handle,
                                                sim.simx_opmode_blocking)[1]
    print(position_joint2)
    joint2_y = position_joint2[1]
    joint2_z = position_joint2[2]

    #distance from predicted point to joint2 in yz plane
    d1 = np.sqrt((joint2_y - predicted_y)**2 + (joint2_z - predicted_z)**2)
    z_length = predicted_z - joint2_z
    y_length = predicted_y - joint2_y

    #our thetas to be calculated
    theta2_guess = 0.0
    theta3_guess = 0.0
    theta4_guess = 0.0

    #define relevant link lengths
    L1 = 0.244
    L2 = 0.213
    L3 = 0.083

    #if it's in this range, it doesn't make sense to move others
    if d1 < (L1 - L2):
        theta2_guess = np.arctan2(y_length, z_length)
        theta3_guess = 0.0
        theta4_guess = 0.0
    elif d1 <= (L1 + L2 - L3):
        top2 = L2**2 - L1**2 - d1**2
        bot2 = -2 * L1 * d1
        theta2_guess = np.arctan2(y_length, z_length) - np.arccos(top2 / bot2)

        top3 = d1**2 - L1**2 - L2**2
        bot3 = -2 * L1 * L2
        theta3_guess = np.pi - np.arccos(top3 / bot3)

        #fold in on self
        theta4_guess = np.pi / 2

    else:
        d2 = d1 - L1
        theta2_guess = np.arctan2(y_length, z_length)

        top3_2 = L3**2 - L2**2 - d2**2
        bot3_2 = -2 * d2 * L2
        print("top3_2/bot3_2: " + str(top3_2 / bot3_2))
        theta3_guess = np.arccos(top3_2 / bot3_2)

        top4 = d2**2 - L2**2 - L3**2
        bot4 = -2 * L2 * L3
        print("top4/bot4: " + str(top4 / bot4))
        theta4_guess = np.arccos(top4 / bot4) - np.pi

    thetas[0] = theta2_guess
    thetas[1] = theta3_guess
    thetas[2] = theta4_guess
    print("thetas: " + str(thetas))

    for i in range(1, 4):
        sim.simxSetJointTargetPosition(clientID, jointHandles[i],
                                       thetas[i - 1],
                                       sim.simx_opmode_oneshot_wait)

    motion_thread.join(
    )  #stop motion thread after robot has moved to defending position

    print("Actual End Effector Position " + str(
        sim.simxGetObjectPosition(clientID, end_effector_handle, base_handle,
                                  sim.simx_opmode_blocking)[1]))
    sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot)
    sim.simxFinish(clientID)
    return 1
Пример #12
0
        break
    else:
        time.sleep(0.2)
        print("Failed connecting to remote API server!")
print("Connection success!")



RAD2DEG = 180 / math.pi   # 常数,弧度转度数
tstep = 0.005             # 定义仿真步长

# 设置仿真步长,为了保持API端与V-rep端相同步长
vrep.simxSetFloatingParameter(clientID, vrep.sim_floatparam_simulation_time_step, tstep, vrep.simx_opmode_oneshot)
# 然后打开同步模式
vrep.simxSynchronous(clientID, True) 
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)



baseName = 'Body'
jointName = ['Tran', 'RobotHip', 'RobotKnee']

print(len(jointName))

_, baseHandle = vrep.simxGetObjectHandle(clientID, baseName, vrep.simx_opmode_blocking)
jointHandle = [0,0,0]
for k in range(len(jointName)):
    _, jointHandle[k] = vrep.simxGetObjectHandle(clientID, jointName[k], vrep.simx_opmode_blocking)

print('Handles available!')
Пример #13
0
import sim as vrep
import math
import random
import time
import math
def moving(x,y):
    a=0.4
    b=0.4
    c=math.pow((math.pow(x,2)+math.pow(y,2)),0.5)
    s=(a+b+c)/2
    area=math.pow((s*(s-a)*(s-b)*(s-c)),0.5)
    h=area/(2*c)
    deg1_base=math.atan(x/y)
    if x<0 and y<0 :
        deg_base1=deg1_base+math.pi
    
    deg1_tri=math.asin(h/a)
    deg1=deg1_base+deg1_tri
    deg2=math.pi-(0.5*math.pi-deg1_tri)-math.acos(h/b)
    deg3=deg2-deg1
    vrep.simxSetJointTargetPosition(clientID,joint01,deg1,opmode)
    vrep.simxSetJointTargetPosition(clientID,joint02,-deg2,opmode)
    vrep.simxSetJointTargetPosition(clientID,joint03,deg3,opmode)
print ('Start')
 
vrep.simxFinish(-1)
 
clientID = vrep.simxStart('192.168.0.10', 19997, True, True, 5000, 5)
if clientID != -1:
    print ('Connected to remote API server')
Пример #14
0
 def start(self):
     s.simxStartSimulation(self._id, self._def_op_mode)
Пример #15
0
	def start_sim(self):
		sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot_wait)
		return
Пример #16
0
def main(argv):
    # https://www.coppeliarobotics.com/helpFiles/en/remoteApiClientSide.htm --> how to remote API
    # Refer to https://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxStart

    sim.simxFinish(-1)  # just in case, close all opened connections
    clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5)

    if clientID == -1:
        print('Failed connecting to remote API server')
    else:
        print('Connected to remote API client')

        numberOfRuns = 10
        numberOfMeasurements = 1

        variationSize = 0.001

        for i in range(numberOfRuns):

            count = 0
            dt = 0.0167
            sim.simxSynchronous(clientID, True)

            ################################################### init variables ###################################################

            ros_handle = simROS.SIMROS(argv)

            targetAngle = 0
            #targetAngles = list(np.random.randint(-25, 26, numberOfMeasurements))
            targetAngles = list(np.zeros(numberOfMeasurements))
            print(targetAngles)

            randomVar = []
            for i in range(18):
                randomVar.append(
                    list(
                        np.random.uniform(-variationSize, variationSize,
                                          numberOfMeasurements)))

            if newFile:
                with open(dataFilePath, 'w+') as myfile:
                    wr = csv.writer(myfile, quoting=csv.QUOTE_ALL)
                    wr.writerow([
                        "Inclination", "LFx", "LFy", "LFz", "LMx", "LMy",
                        "LMz", "LHx", "LHy", "LHz", "RFx", "RFy", "RFz", "RMx",
                        "RMy", "RMz", "RHx", "RHy", "RHz"
                    ])

            counter = 0

            Done = False
            #Start the simulation
            sim.simxStartSimulation(clientID, sim.simx_opmode_blocking)
            ################################################### Control Loop ###################################################
            while not Done:
                #code here

                if counter == 150:
                    saveData(ros_handle.force, targetAngle)
                    print("Runs remaining: ", len(targetAngles))
                    if len(targetAngles) <= 0:
                        Done = True
                    else:
                        targetAngle = targetAngles.pop(0)
                        motor_positions = [
                            0, randomVar[1].pop() + 2.059, randomVar[2].pop(),
                            0, randomVar[4].pop() + 2.059, randomVar[5].pop(),
                            0, randomVar[7].pop() + 2.059, randomVar[8].pop(),
                            0, randomVar[10].pop() + 2.059,
                            randomVar[11].pop(), 0,
                            randomVar[13].pop() + 2.059, randomVar[14].pop(),
                            0, randomVar[16].pop() + 2.059,
                            randomVar[17].pop()
                        ]
                        counter = 0
                        ros_handle.setSlopeAngle(targetAngle)
                        ros_handle.setLegMotorPosition(motor_positions)

                counter = counter + 1

                #Step the simulation
                sim.simxSynchronousTrigger(clientID)

            sim.simxStopSimulation(clientID, sim.simx_opmode_blocking)
            sim.simxGetPingTime(clientID)

        sim.simxFinish(clientID)