예제 #1
0
파일: Simulator.py 프로젝트: dtbinh/lucy
 def getObjectPositionWrapper(self, clientID, LSP_Handle):
     if self.getObjectPositionFirstTime:
         error, ret = vrep.simxGetObjectPosition(clientID, LSP_Handle, -1, vrep.simx_opmode_streaming)
         self.getObjectPositionFirstTime = False
     else:
         error, ret = vrep.simxGetObjectPosition(clientID, LSP_Handle, -1, vrep.simx_opmode_buffer) 
     return error, ret
예제 #2
0
    def handle_output(self):
        # return whatever state information you need to get the error you want
        # this will get you the information for the center of the object. If you
        # want something like the position of the end of an arm, you will need
        # to do some calculations, or just make a dummy object, attach it to
        # the point you want, and get the position of the dummy object
        
        #err, hand_pos = vrep.simxGetObjectPosition(self.cid, self.hand, -1,
        #                                           vrep.simx_opmode_oneshot)
        #err, hand_ori = vrep.simxGetObjectOrientation(self.cid, self.hand, -1,
        #                                           vrep.simx_opmode_oneshot)
        err, hand_ori = vrep.simxGetJointPosition(self.cid, self.hand_joint,
                                                   vrep.simx_opmode_oneshot)
        err, arm_ori = vrep.simxGetJointPosition(self.cid, self.arm_joint,
                                                   vrep.simx_opmode_oneshot)
        err, hand_vel = vrep.simxGetObjectFloatParameter(self.cid, self.hand_joint,
                                                         2012, vrep.simx_opmode_oneshot)
        err, arm_vel = vrep.simxGetObjectFloatParameter(self.cid, self.arm_joint,
                                                        2012, vrep.simx_opmode_oneshot)
        err, hand_pos = vrep.simxGetObjectPosition(self.cid, self.hand, -1,
                                                   vrep.simx_opmode_oneshot)
        err, target_pos = vrep.simxGetObjectPosition(self.cid, self.target, -1,
                                                   vrep.simx_opmode_oneshot)

        return [arm_ori, hand_ori, arm_vel, hand_vel, hand_pos[0], hand_pos[2],
                target_pos[0], target_pos[1]]
예제 #3
0
파일: BugBase.py 프로젝트: dtbinh/Learning
    def _init_values(self):

        error_code, _ = vrep.simxGetObjectPosition(self.client_id, self.target_handle, -1, vrep.simx_opmode_oneshot)

        error_code, _ = vrep.simxGetObjectPosition(self.client_id, self.bot_handle, -1, vrep.simx_opmode_oneshot)

        error_code, _ = vrep.simxGetObjectOrientation(self.client_id, self.bot_handle, -1, vrep.simx_opmode_streaming)
예제 #4
0
def simple():
    # obtem os handlers. Um Handler eh um numero que identifica um componente, como, por exemplo, uma junta
    res, nao = vrep.simxGetObjectHandle(clientID, "NAO", vrep.simx_opmode_blocking)
    res, shL = vrep.simxGetObjectHandle(clientID, "LShoulderPitch3", vrep.simx_opmode_blocking)
    res, shR = vrep.simxGetObjectHandle(clientID, "RShoulderPitch3", vrep.simx_opmode_blocking)
    res, kneeR = vrep.simxGetObjectHandle(clientID, "RKneePitch3", vrep.simx_opmode_blocking)
    res, kneeL = vrep.simxGetObjectHandle(clientID, "LKneePitch3", vrep.simx_opmode_blocking)
    res, hipPitchL = vrep.simxGetObjectHandle(clientID, "LHipPitch3", vrep.simx_opmode_blocking)
    res, hipPitchR = vrep.simxGetObjectHandle(clientID, "RHipPitch3", vrep.simx_opmode_blocking)
    res, hipYawPitchL = vrep.simxGetObjectHandle(clientID, "LHipYawPitch3", vrep.simx_opmode_blocking)
    res, hipYawPitchR = vrep.simxGetObjectHandle(clientID, "RHipYawPitch3", vrep.simx_opmode_blocking)
    res, anklePitchL = vrep.simxGetObjectHandle(clientID, "LAnklePitch3", vrep.simx_opmode_blocking)
    res, anklePitchR = vrep.simxGetObjectHandle(clientID, "RAnklePitch3", vrep.simx_opmode_blocking)


    while True:
    	# Envia comandos para as juntas
    	vrep.simxSetJointTargetPosition(clientID, kneeR, 1.2,vrep.simx_opmode_oneshot)
    	vrep.simxSetJointTargetPosition(clientID, anklePitchR, 0.4,vrep.simx_opmode_oneshot)

    	time.sleep(1)
    	vrep.simxSetJointTargetPosition(clientID, hipYawPitchL, -0.3,vrep.simx_opmode_oneshot)

    	vrep.simxSetJointTargetPosition(clientID, hipPitchR, -0.1,vrep.simx_opmode_oneshot)
    	vrep.simxSetJointTargetPosition(clientID, hipYawPitchR, -0.3,vrep.simx_opmode_oneshot)
    	vrep.simxSetJointTargetPosition(clientID, kneeR, -0.1,vrep.simx_opmode_oneshot)
    	vrep.simxSetJointTargetPosition(clientID, anklePitchR, 0,vrep.simx_opmode_oneshot)

    	print vrep.simxGetObjectPosition(clientID, nao, -1, vrep.simx_opmode_blocking)
    	reset_simulation(clientID)
예제 #5
0
    def configure_handles(self):
        # Handles of body and joints
        rc, body = vrep.simxGetObjectHandle(self.client_id, 'NAO', vrep.simx_opmode_oneshot_wait)
        assert rc == 0, rc

        rc, joint_0 = vrep.simxGetObjectHandle(self.client_id, 'LShoulderPitch3', vrep.simx_opmode_oneshot_wait)
        assert rc == 0, rc
        rc, joint_1 = vrep.simxGetObjectHandle(self.client_id, 'RShoulderPitch3', vrep.simx_opmode_oneshot_wait)
        assert rc == 0, rc

        rc, joint_2 = vrep.simxGetObjectHandle(self.client_id, 'LHipPitch3', vrep.simx_opmode_oneshot_wait)
        assert rc == 0, rc
        rc, joint_3 = vrep.simxGetObjectHandle(self.client_id, 'RHipPitch3', vrep.simx_opmode_oneshot_wait)
        assert rc == 0, rc
        rc, joint_4 = vrep.simxGetObjectHandle(self.client_id, 'LKneePitch3', vrep.simx_opmode_oneshot_wait)
        assert rc == 0, rc
        rc, joint_5 = vrep.simxGetObjectHandle(self.client_id, 'RKneePitch3', vrep.simx_opmode_oneshot_wait)
        assert rc == 0, rc
        rc, joint_6 = vrep.simxGetObjectHandle(self.client_id, 'LAnklePitch3', vrep.simx_opmode_oneshot_wait)
        assert rc == 0, rc
        rc, joint_7 = vrep.simxGetObjectHandle(self.client_id, 'RAnklePitch3', vrep.simx_opmode_oneshot_wait)
        assert rc == 0, rc

        rc, joint_8 = vrep.simxGetObjectHandle(self.client_id, 'LHipYawPitch3', vrep.simx_opmode_oneshot_wait)
        assert rc == 0, rc

        rc, self.vase = vrep.simxGetObjectHandle(self.client_id, 'indoorPlant', vrep.simx_opmode_oneshot_wait)
        assert rc == 0, rc

        self.body = body
        self.joints = [joint_0, joint_1, joint_2, joint_3, joint_4, joint_5, joint_6, joint_7, joint_8]
        self.get_body_position(initial=True)
        self.get_joint_angles(initial=True)
        vrep.simxGetObjectPosition(self.client_id, self.body, self.vase, vrep.simx_opmode_streaming)
        time.sleep(0.5)
예제 #6
0
def getPosition(clientID,goal_new):
    errorcode,newgoal_handle=vrep.simxGetObjectHandle(clientID,goal_new,vrep.simx_opmode_oneshot_wait)
    #time.sleep(1) 
    errorCode,newgoal_position=vrep.simxGetObjectPosition(clientID,newgoal_handle,-1,vrep.simx_opmode_streaming)
    time.sleep(1) 
    errorCode,newgoal_position=vrep.simxGetObjectPosition(clientID,newgoal_handle,-1,vrep.simx_opmode_buffer)
    return newgoal_position
    def initializeVrepApi(self):
        # initialize bot handles and variables
        _, self.leftMotor=vrep.simxGetObjectHandle(
            self.clientID, '%s_leftJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
        _, self.rightMotor=vrep.simxGetObjectHandle(
            self.clientID, '%s_rightJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
        _, self.bot=vrep.simxGetObjectHandle(
            self.clientID, self.bot_name, vrep.simx_opmode_oneshot_wait)

        # initialize proximity sensors
        self.proxSensors = prox_sens_initialize(self.clientID, self.bot_name)

        # initialize odom of bot
        _, self.xyz = vrep.simxGetObjectPosition(
            self.clientID, self.bot, -1, vrep.simx_opmode_streaming )
        _, self.eulerAngles = vrep.simxGetObjectOrientation(
            self.clientID, self.bot, -1, vrep.simx_opmode_streaming )

        # FIXME: striker handles shouldn't be part of the default base class
        # FIXME: should be better way to have information regarding all the bots (Central Control?) (Publishers/Subscribers...)?
        # initialize bot handles and variables for Red1
        _, self.leftMotorStriker=vrep.simxGetObjectHandle(
            self.clientID, '%s_leftJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
        _, self.rightMotorStriker=vrep.simxGetObjectHandle(
            self.clientID, '%s_rightJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
        _, self.botStriker = vrep.simxGetObjectHandle(
            self.clientID, self.bot_nameStriker, vrep.simx_opmode_oneshot_wait)
        _, xyzStriker = vrep.simxGetObjectPosition(
            self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming )
        _, eulerAnglesStriker = vrep.simxGetObjectOrientation(
            self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming )
예제 #8
0
def getDif(cid,copter,target):
    copter_pos = vrep.simxGetObjectPosition(cid, copter, -1, mode())[1]
    copter_vel = vrep.simxGetObjectVelocity(cid, copter, mode())[1]
    copter_orientation = vrep.simxGetObjectOrientation(cid,copter,-1,mode())[1]
    target_pos = vrep.simxGetObjectPosition(cid, target, -1, mode())[1]
    target_vel = vrep.simxGetObjectVelocity(cid, target, mode())[1]

    return np.asarray([(-np.asarray(copter_pos) + np.asarray(target_pos)),(-np.asarray(copter_vel) + np.asarray(target_vel)),np.asarray(copter_orientation)]).flatten()
예제 #9
0
 def getState(self):
     returnCode,position=vrep.simxGetObjectPosition(self.clientID,self.car,-1,vrep.simx_opmode_oneshot_wait)
     returnCode,positionFR=vrep.simxGetObjectPosition(self.clientID,self.fr,-1,vrep.simx_opmode_oneshot_wait)
     returnCode,positionBR=vrep.simxGetObjectPosition(self.clientID,self.br,-1,vrep.simx_opmode_oneshot_wait)
     
     theta=math.atan2(positionFR[1]-positionBR[1],positionFR[0]-positionBR[0])
     #return [position[0],position[1],theta,1*(self.redBoundaries[0]>position[0] or position[0]>self.redBoundaries[1]),self.robot.desiredWheelRotSpeed,self.robot.desiredSteeringAngle]
     return [position[0],position[1],theta]
예제 #10
0
def getPosition(clientID, object_name):
    # get the handle of the object, which position is needed
    errorcode, object_handle = vrep.simxGetObjectHandle(clientID, object_name, vrep.simx_opmode_oneshot_wait)
    # get the position
    errorCode, object_position = vrep.simxGetObjectPosition(clientID, object_handle, -1, vrep.simx_opmode_streaming)
    # some waiting time is needed to get the right data
    time.sleep(1)
    errorCode, object_position = vrep.simxGetObjectPosition(clientID, object_handle, -1, vrep.simx_opmode_buffer)
    return object_position
예제 #11
0
파일: BugBase.py 프로젝트: dtbinh/Learning
    def read_values(self):

        error_code, target_pos = vrep.simxGetObjectPosition(self.client_id, self.target_handle, -1, vrep.simx_opmode_streaming)
        self.target_pos = Vector3(x=target_pos[0], y=target_pos[1], z=target_pos[2])

        error_code, bot_pos = vrep.simxGetObjectPosition(self.client_id, self.bot_handle, -1, vrep.simx_opmode_streaming)
        self.bot_pos = Vector3(x=bot_pos[0], y=bot_pos[1], z=bot_pos[2])

        error_code, bot_euler_angles = vrep.simxGetObjectOrientation(self.client_id, self.bot_handle, -1, vrep.simx_opmode_streaming)
        self.bot_euler_angles = Vector3(x=bot_euler_angles[0], y=bot_euler_angles[1], z=bot_euler_angles[2])
예제 #12
0
파일: ttt_files.py 프로젝트: humm/dovecot
 def load(self, com, base_h):
     self.handle = com._vrep_get_handle(self.name)
     res, pos = remote_api.simxGetObjectPosition(com.api_id, self.handle, base_h,
                                                 remote_api.simx_opmode_oneshot_wait)
     assert res == 0
     self.pos    = tuple(100*p for p in pos)
     res, pos_w = remote_api.simxGetObjectPosition(com.api_id, self.handle, -1,
                                                   remote_api.simx_opmode_oneshot_wait)
     assert res == 0
     self.pos_w  = tuple(100*p for p in pos_w)
     self.dim, self.mass = self.object_properties(com, self.handle)
예제 #13
0
def odometryData():
    err_pos_o, pos_o = vrep.simxGetObjectPosition(Variables.clientID, 
                                              Variables.bodyHandle,
                                              -1, vrep.simx_opmode_streaming)
    
    err_pos_n, pos = vrep.simxGetObjectPosition(Variables.clientID, 
                                              Variables.bodyHandle,
                                              -1, vrep.simx_opmode_buffer)
    if Variables.robot_pos_new != pos:
        Variables.robot_pos_old = Variables.robot_pos_new
        Variables.robot_pos_new = pos 
           
예제 #14
0
 def getDistanceBetwObjects(self, objectNameA, objectNameB):
     """Get the distance between two named objects in V-REP.
        Raise exception if either does not exist"""
        
     eCode, handleA = vrep.simxGetObjectHandle(self._VREP_clientId, objectNameA, vrep.simx_opmode_oneshot_wait)
     if eCode != 0:
         raise Exception("Could not get handle of object", objectNameA)
     eCode, poseA =  vrep.simxGetObjectPosition(self._VREP_clientId, handleA, -1, vrep.simx_opmode_oneshot_wait)
     eCode, handleB = vrep.simxGetObjectHandle(self._VREP_clientId, objectNameB, vrep.simx_opmode_oneshot_wait)
     if eCode != 0:
         raise Exception("Could not get handle of object", objectNameB)
     eCode, poseB =  vrep.simxGetObjectPosition(self._VREP_clientId, handleB, -1, vrep.simx_opmode_oneshot_wait)
     return distance(poseA,poseB)
 def __init__(self, clientID):
     self.clientID = clientID
     _, self.handle=vrep.simxGetObjectHandle(
         self.clientID, 'Ball', vrep.simx_opmode_oneshot_wait)
     vrep.simxGetObjectPosition(
         self.clientID, self.handle, -1, vrep.simx_opmode_streaming)
     self.posm2 = self.getBallPose()
     self.tm2 = time.time()
     self.posm1 = self.getBallPose()
     self.tm1 = time.time()
     self.pos = self.getBallPose()
     self.t =  time.time()
     self.T = 2.15   # time constant in [s]
     vrep.simxGetFloatSignal(self.clientID, 'simTime', vrep.simx_opmode_streaming )
예제 #16
0
def getTargetStart(cid,target):
    start_pos = [0]
    while (max(start_pos) == 0):
        controller.controller_motor(cid, copter, target)
        err, start_pos = vrep.simxGetObjectPosition(cid, target, -1, mode())

    return err, np.asarray(start_pos)
예제 #17
0
	def __init__(self, clientID):
		self.clientID = clientID
		
		err,leftMotorHandle=vrep.simxGetObjectHandle(clientID,"remoteApiControlledBubbleRobLeftMotor",vrep.simx_opmode_oneshot_wait)
		if err==vrep.simx_error_noerror:
			self.leftMotorHandle = leftMotorHandle
			print('Get handle for left motor: {}'.format(leftMotorHandle))
		else:
			print('Error by getting handle for left motor: {}'.format(err))
			
		err,rightMotorHandle=vrep.simxGetObjectHandle(clientID,"remoteApiControlledBubbleRobRightMotor",vrep.simx_opmode_oneshot_wait)
		if err==vrep.simx_error_noerror:
			self.rightMotorHandle = rightMotorHandle
			print('Get handle for right motor: {}'.format(rightMotorHandle))
		else:
			print('Error by getting handle for right motor: {}'.format(err))

		err,visionSensorHandle=vrep.simxGetObjectHandle(clientID,"Vision_sensor",vrep.simx_opmode_oneshot_wait)
		if err==vrep.simx_error_noerror:
			self.visionSensorHandle = visionSensorHandle
			print('Get handle for vision sensor: {}'.format(visionSensorHandle))
		else:
			print('Error by getting handle for vision sensor: {}'.format(err))

		err,bubbleRobHandle=vrep.simxGetObjectHandle(clientID,"remoteApiControlledBubbleRob",vrep.simx_opmode_oneshot_wait) #getHandle
		if err==vrep.simx_error_noerror:
			self.bubbleRobHandle = bubbleRobHandle
			print('Get handle for bubbleRob: {}'.format(bubbleRobHandle))
		else:
			print('Error by getting handle for bubbleRob: {}'.format(err))

		err,self.bubbleRobStartPosition = vrep.simxGetObjectPosition(clientID, bubbleRobHandle, -1, vrep.simx_opmode_oneshot_wait) #getStartPosition
예제 #18
0
 def getState(self, initial=False):
     if initial:
         mode = vrep.simx_opmode_streaming
     else:
         mode = vrep.simx_opmode_buffer
         
     # Retrieve IMU data
     errorSignal, self.stepSeconds = vrep.simxGetFloatSignal(self.clientID,
                                                             'stepSeconds', mode)      
     errorOrien, baseEuler = vrep.simxGetObjectOrientation(self.clientID,
                                                           self.Quadbase, -1, mode) 
     errorPos, basePos = vrep.simxGetObjectPosition(self.clientID,
                                                    self.Quadbase,-1, mode)
     errorVel, linVel, angVel = vrep.simxGetObjectVelocity(self.clientID,
                                                           self.Quadbase, mode)         
                                                            
     if initial:
         if (errorSignal or errorOrien or errorPos or errorVel != vrep.simx_return_ok):
             time.sleep(0.05)
         pass
     else:       
         # Convert Euler angles to pitch, roll, yaw
         rollRad, pitchRad = rotate((baseEuler[0], baseEuler[1]), baseEuler[2])
         pitchRad = -pitchRad
         yawRad   = -baseEuler[2]
     
         baseRad = np.array([yawRad,rollRad,pitchRad])+0.0   
         self.state = np.asarray(np.concatenate((basePos,linVel,angVel,baseRad)),dtype=np.float32)
         #print("data_core: " + str(self.state))
         return self.state
예제 #19
0
def execute_action(clientID, RobotHandle, LMotorHandle, RMotorHandle, raw_pose_data, next_action):
    linearVelo, angularVelo, goal_pose = compute_control(raw_pose_data, next_action)
    l_ang_v, r_ang_v = dif_drive(linearVelo, angularVelo)
    #----------
    dist_bound = 0.15
    ang_bound = 0.1*PI
    # retDia, DialogHandle, uiHandle = vrep.simxDisplayDialog(clientID, 'Current Action', next_action, vrep.sim_dlgstyle_message, 'None', None, None, vrep.simx_opmode_blocking)
    #----------
    while (distance(raw_pose_data, goal_pose)>= dist_bound) or (abs(raw_pose_data[2]-goal_pose[2])>=ang_bound):
        # print '--line_distance--', distance(raw_pose_data, goal_pose)
        # print '--angle distance--', abs(raw_pose_data[2]-goal_pose[2])
        pret, RobotPos = vrep.simxGetObjectPosition(clientID, RobotHandle, -1, vrep.simx_opmode_blocking)
        #print "robot position: (x = " + str(RobotPos[0]) + ", y = " + str(RobotPos[1]) + ")"
        oret, RobotOrient = vrep.simxGetObjectOrientation(clientID, RobotHandle, -1, vrep.simx_opmode_blocking)
        #print "robot orientation: (a = " + str(RobotOrient[0]) + ", b = " + str(RobotOrient[1]) +", g = " + str(RobotOrient[2]) + ")"
        raw_pose_data = shift_raw_pose([RobotPos[0], RobotPos[1], RobotOrient[2]])
        #------------------------------
        linearVelo, angularVelo = Find_Control(raw_pose_data, next_action, goal_pose)
        l_ang_v, r_ang_v = dif_drive(linearVelo, angularVelo)
        #print 'raw_pose_data', raw_pose_data
        vrep.simxSetJointTargetVelocity(clientID, LMotorHandle, l_ang_v, vrep.simx_opmode_streaming)
        vrep.simxSetJointTargetVelocity(clientID, RMotorHandle, r_ang_v, vrep.simx_opmode_streaming)
        #print 'distance_x_y:%s; angle_dif:%s' %(str(distance(raw_pose_data, goal_pose)),str(abs(raw_pose_data[2]-goal_pose[2])))
    print 'Goal pose reached!'
    # vrep.simxSetJointTargetVelocity(clientID, LMotorHandle, 0, vrep.simx_opmode_blocking)
    # vrep.simxSetJointTargetVelocity(clientID, RMotorHandle, 0, vrep.simx_opmode_blocking)
    # time.sleep(0.5)
    # retEnd = vrep.simxEndDialog(clientID, DialogHandle, vrep.simx_opmode_blocking)
    return raw_pose_data
예제 #20
0
def simGetObjectMatrix(cid,obj,relative):
    err, pos = vrep.simxGetObjectPosition(cid,obj,-1,mode())
    x,y,z = pos
    print(err)
    print("Values are {} {} {} {}".format(x,y,z,pos))
    err, angles = vrep.simxGetObjectOrientation(cid,obj,-1,mode())
    a,b,g = angles
    print(err)
    print("Angles are {} {} {} {}".format(a,b,g,angles))

    op = np.array([[0]*4]*4, dtype =np.float64)
    A = float(cos(a))
    B = float(sin(a))
    C = float(cos(b))
    D = float(sin(b))
    E = float(cos(g))
    F = float(sin(g))
    AD = float(A*D)
    BD = float(B*D)
    op[0][0]=float(C)*E
    op[0][1]=-float(C)*F
    op[0][2]=float(D)
    op[1][0]=float(BD)*E+A*F
    op[1][1]=float(-BD)*F+A*E
    op[1][2]=float(-B)*C
    op[2][0]=float(-AD)*E+B*F
    op[2][1]=float(AD)*F+B*E
    op[2][2]=float(A)*C
    op[0][3]=float(x)
    op[1][3]=float(y)
    op[2][3]=float(z)
    return op[0:3,:]
예제 #21
0
 def loop(self):
     operationMode = vrep.simx_opmode_streaming
     if self.__initLoop:
         self.__initLoop = False
     else:
         operationMode = vrep.simx_opmode_buffer
     returnCode, position = vrep.simxGetObjectPosition(self.__clientID, self.__bodyHandle, -1, operationMode)
     if returnCode == vrep.simx_return_ok:
         self.__position = [0.0, 0.0]
         self.__position[0] = position[0]  # X
         self.__position[1] = position[1]  # Y
     else:
         self.__position = None
         # print >> sys.stderr, "Error in VRepBubbleRob.getPosition()"
     returnCode, linearVelocity, angularVelocity = vrep.simxGetObjectVelocity(
         self.__clientID, self.__bodyHandle, operationMode
     )
     if returnCode == vrep.simx_return_ok:
         try:
             self.__velocity = linearVelocity[0] * math.cos(self.__orientation) + linearVelocity[1] * math.sin(
                 self.__orientation
             )
             self.__mind.setInput("velocity", self.__velocity)
         except TypeError:
             pass
             # if self.__name=="BubbleRob#1":
             #    print self.__velocity, linearVelocity[0], math.cos(self.__orientation), linearVelocity[1], math.sin(self.__orientation)
     else:
         self.__velocity = None
         # print >> sys.stderr, "Error in VRepBubbleRob.getPosition()"
     self.__cnt = self.__cnt + 1
def run(path, handles):
    files = []
    for i in range(len(handles)):
        files.append(open('path_%d.csv' % i, 'w'))
    for state in path:
        for i in range(len(handles)):
            handle = handles[i]
            ret, pos = vrep.simxGetObjectPosition(clientID, handle, -1, vrep.simx_opmode_oneshot_wait)

            # wait until it's almost in the position
            # ------

            if (not ret == vrep.simx_return_ok):
                print('Failed to retrieve position for Quadricopter_target')
                sys.exit(1)

            #print pos
            new_pos = pos
            new_pos[0] = state.positions[i].as_tuple()[0]
            new_pos[1] = state.positions[i].as_tuple()[1]

            files[i].write(state.positions[i].as_vrep_path_point())

            ret = vrep.simxSetObjectPosition(clientID, handle, -1, new_pos, vrep.simx_opmode_oneshot_wait)
            if (not ret == vrep.simx_return_ok):
                print('Failed to set position for Quadricopter_target')
                sys.exit(1)

    for i in range(len(handles)):
        files[i].close()
예제 #23
0
 def loop(self):
     operationMode=vrep.simx_opmode_streaming
     if self.__initLoop:
         self.__initLoop=False
     else:
         operationMode=vrep.simx_opmode_buffer
     returnCode, orientation = vrep.simxGetObjectOrientation(self.__clientID, self.__bodyHandle, -1, operationMode)
     if returnCode==vrep.simx_return_ok:
         self.__orientation=orientation[2]
     else:
         self.__orientation = None
         # print >> sys.stderr, "Error in VRepBubbleRob.getOrientation()"
     self.__mind.setInput("orientation", self.__orientation)
     returnCode, position = vrep.simxGetObjectPosition(self.__clientID, self.__bodyHandle, -1, operationMode)
     if returnCode==vrep.simx_return_ok:
         self.__position=[0.0,0.0]
         self.__position[0]=position[0]  #X
         self.__position[1]=position[1]  #Y
     else:
         self.__position=None
         # print >> sys.stderr, "Error in VRepBubbleRob.getPosition()"
     returnCode, linearVelocity, angularVelocity = vrep.simxGetObjectVelocity(self.__clientID, self.__bodyHandle, operationMode)
     if returnCode==vrep.simx_return_ok:
         try:
             # self.__velocity=linearVelocity[0]*math.cos(self.__orientation)+linearVelocity[1]*math.sin(self.__orientation)
             self.__velocity=math.sqrt(linearVelocity[0]**2+linearVelocity[1]**2)
             self.__mind.setInput("velocity", self.__velocity)
         except TypeError:
             pass
             # if self.__name=="BubbleRob#1":
             #    print self.__velocity, linearVelocity[0], math.cos(self.__orientation), linearVelocity[1], math.sin(self.__orientation)
     else:
         self.__velocity=None
         # print >> sys.stderr, "Error in VRepBubbleRob.getPosition()"
     returnCode, sensorTrigger, dp, doh, dsnv = vrep.simxReadProximitySensor(self.__clientID, self.__sensorHandle, operationMode)
     if returnCode==vrep.simx_return_ok:
         # We succeeded at reading the proximity sensor
         self.__mind.setInput("lastProximitySensorTime", vrep.simxGetLastCmdTime(self.__clientID))
         self.__mind.setInput("sensorTrigger", sensorTrigger)
     self.__mind.setInput("mostSalientItem", self.__mind.selectMostSalient("I"))
     # print self.__name, self.__mind.getAttendedItem(self.__mind.getOutput("attendedItemName"))
     self.__mind.setInput("attendedItem", self.__mind.getAttendedItem(self.__mind.getOutput("attendedItemName")))
     self.__mind.applyRules()
     self.__mind.setStates()
     if self.__mind.getOutput("steering")!=None:
         self.setSteering(self.__mind.getOutput("steering"))
     if self.__mind.getOutput("thrust")!=None:
         self.setThrust(self.__mind.getOutput("thrust"))
     if self.__mind.getOutput("reward")!=None:
         if self.__mind.getOutput("reward")>0.5:
             self.setEmotionalExpression(1)
         elif self.__mind.getOutput("reward")<-0.5:
             self.setEmotionalExpression(-1)
         else:
             self.setEmotionalExpression(0)
     getSignalReturnCode, dMessage = vrep.simxGetStringSignal(self.__clientID, "Debug", vrep.simx_opmode_streaming)
     if dMessage!="":
         print("Debug:"+dMessage)
     self.__cnt=self.__cnt+1
예제 #24
0
파일: env.py 프로젝트: trummisen54/AI
def getNearestConcretBlockDist(clientID, blockHandleArray, pioneerRobotHandle):
    distances = [0. for ix in range(len(blockHandleArray))] # initialize a container
    pioneer2BlockPos = [[] for ix in range(len(blockHandleArray))] # initialize a container
    for i_block in range(len(blockHandleArray)):
        pioneer2BlockPos[i_block] = vrep.simxGetObjectPosition(clientID, pioneerRobotHandle, blockHandleArray[i_block][1], vrep.simx_opmode_oneshot_wait) # get relative position to robot
        distances[i_block] = math.sqrt(pioneer2BlockPos[i_block][1][0]**2 + pioneer2BlockPos[i_block][1][1]**2) # compute Euclidean distance
        #print(i_block, distances[i_block])
    return min(distances), distances.index(min(distances)), pioneer2BlockPos[distances.index(min(distances))]
예제 #25
0
    def get_target( self ):
        err, self.t_ori = vrep.simxGetObjectOrientation(self.cid, self.target, -1,
                                                    vrep_mode )
        err, self.t_pos = vrep.simxGetObjectPosition(self.cid, self.target, -1,
                                                vrep_mode )

        # Convert orientations to z-y-x convention
        self.t_ori = convert_angles(self.t_ori)
예제 #26
0
def isRobotUp(clientID):
    error, LSP_Handle=vrep.simxGetObjectHandle(clientID,"Bioloid", vrep.simx_opmode_oneshot_wait)
    error, bioloid_position = vrep.simxGetObjectPosition(clientID, LSP_Handle, -1, vrep.simx_opmode_streaming)
    #in case of problems consulting the robot position, consider that the robot is up
    if error:
        return error, True
    else:
        return error, bioloid_position[2]>FALL_THRESHOLD
예제 #27
0
    def __init__(self, n_robots, base_name):
        self.name = "line follower tester"
        self.n_robots = n_robots
        self.base_name = base_name

        self.robot_names = [self.base_name]
        for i in range(self.n_robots-1):
            self.robot_names.append(self.base_name+'#'+str(i))

        # Establish connection to V-REP
        vrep.simxFinish(-1)  # just in case, close all opened connections

        # Connect to the simulation using V-REP's remote API (configured in V-REP, not scene specific)
        # http://www.coppeliarobotics.com/helpFiles/en/remoteApiServerSide.htm
        self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)

        # Use port 19999 and add simExtRemoteApiStart(19999) to some child-script in your scene for scene specific API
        # (requires the simulation to be running)

        if self.clientID != -1:
            print ('Connected to remote API server')
        else:
            print ('Failed connecting to remote API server')
            sys.exit('Could not connect')

        # Reset running simulation
        vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
        time.sleep(0.2)

        # Get initial robots' positions
        self.robot_handles = []
        self.robot_pos0 = []
        for rbt_name in self.robot_names:
            res, rbt_tmp = vrep.simxGetObjectHandle(self.clientID, rbt_name, vrep.simx_opmode_oneshot_wait)
            self.robot_handles.append(rbt_tmp)
            # Initialize data stream
            vrep.simxGetObjectPosition(self.clientID, self.robot_handles[-1], -1, vrep.simx_opmode_streaming)
            vrep.simxGetFloatSignal(self.clientID, rbt_name+'_1', vrep.simx_opmode_streaming)
            vrep.simxGetFloatSignal(self.clientID, rbt_name+'_2', vrep.simx_opmode_streaming)
            vrep.simxGetFloatSignal(self.clientID, rbt_name+'_3', vrep.simx_opmode_streaming)

        time.sleep(0.2)
        for rbt in self.robot_handles:
            res, pos = vrep.simxGetObjectPosition(self.clientID, rbt, -1, vrep.simx_opmode_buffer)
            self.robot_pos0.append(pos)
    def getRobotPose(self, robot_handle):
        _, xyz = vrep.simxGetObjectPosition(
            self.clientID, robot_handle, -1, vrep.simx_opmode_buffer)
        _, eulerAngles = vrep.simxGetObjectOrientation(
            self.clientID, robot_handle, -1, vrep.simx_opmode_buffer)
        x, y, z = xyz
        theta = eulerAngles[2]

        return (x, y, theta)
    def get_target(self):
        time.sleep(0.1)
        ret, xyz =vrep.simxGetObjectPosition(self.clientID, self.target, -1,vrep.simx_opmode_oneshot)
        print(ret, xyz)
        x,y,z = xyz
        ret, orientation = vrep.simxGetObjectOrientation(self.clientID, self.target,-1, vrep.simx_opmode_oneshot)
        rot = orientation[2]

        return (x, y, z, rot)
예제 #30
0
 def get_body_position(self, initial=False):
     if initial:
         mode = vrep.simx_opmode_streaming
     else:
         mode = vrep.simx_opmode_buffer
     rc, body_position = vrep.simxGetObjectPosition(
         self.client_id, self.body, -1, mode)
     assert rc == (1 if initial else 0), rc
     return np.array(body_position)
def QC_controller(Quadricopter_target, Quadricopter_base, Quadricopter,
                  Quadricopter_target2, Quadricopter_base2, Quadricopter2,
                  Quadricopter_target0, Quadricopter_base0, Quadricopter0,
                  Quadricopter_target1, Quadricopter_base1, Quadricopter1):
    try:
        import vrep
    except:
        print('--------------------------------------------------------------')
        print('"vrep.py" could not be imported. This means very probably that')
        print('either "vrep.py" or the remoteApi library could not be found.')
        print('Make sure both are in the same folder as this file,')
        print('or appropriately adjust the file "vrep.py"')
        print('--------------------------------------------------------------')
        print('')
    """ =========================================================== """
    """ ============ Imported Libraries: ============ """
    import time
    import numpy as np
    from captains_log_v1 import log_data
    import os
    import cost
    import csv
    import matplotlib.pyplot as plt
    from mpl_toolkits.mplot3d import Axes3D
    import main_PID_controller
    """ =================================================================== """
    # output limits:
    #min_output=0
    #max_output=8.335
    # program parameters:
    global i
    i = 0
    xl = []
    yl = []
    yf = []
    zl = []
    xf2 = []
    yf2 = []
    zf2 = []

    xf0 = []
    yf0 = []
    zf0 = []

    xf1 = []
    yf1 = []
    zf1 = []
    #    xe = []
    #    ye = []
    ze = []
    #    xs = []
    #    ys = []
    #    zs = []
    x_qc = []
    y_qc = []
    z_qc = []
    x_qc2 = []
    y_qc2 = []
    z_qc2 = []
    #    u = []
    v1 = []
    v2 = []
    v3 = []
    v4 = []
    w1 = []
    w2 = []
    w3 = []
    w4 = []
    v12 = []
    v22 = []
    v32 = []
    v42 = []
    #global variables:
    cumul = 0
    last_e = 0
    pAlphaE = 0
    pBetaE = 0
    psp2 = 0
    psp1 = 0
    prevEuler = 0

    cumulAlpha = 0
    cumulBeta = 0

    cumulAlphaPos = 0
    cumulBetaPos = 0

    cumul2 = 0
    last_e2 = 0
    pAlphaE2 = 0
    pBetaE2 = 0
    psp22 = 0
    psp12 = 0
    prevEuler2 = 0

    cumulAlpha2 = 0
    cumulBeta2 = 0

    cumulAlphaPos2 = 0
    cumulBetaPos2 = 0

    cumul0 = 0
    last_e0 = 0
    pAlphaE0 = 0
    pBetaE0 = 0
    psp20 = 0
    psp10 = 0
    prevEuler0 = 0

    cumulAlpha0 = 0
    cumulBeta0 = 0

    cumulAlphaPos0 = 0
    cumulBetaPos0 = 0

    cumul1 = 0
    last_e1 = 0
    pAlphaE1 = 0
    pBetaE1 = 0
    psp21 = 0
    psp11 = 0
    prevEuler1 = 0

    cumulAlpha1 = 0
    cumulBeta1 = 0

    cumulAlphaPos1 = 0
    cumulBetaPos1 = 0

    cumulPOS = 0
    cumulVY = 0
    cumulVX = 0
    cumulSP0 = 0
    cumulSP1 = 0
    cumulEULER = 0
    cumulPOS2 = 0
    cumulVY2 = 0
    cumulVX2 = 0
    cumulSP02 = 0
    cumulSP12 = 0
    cumulEULER2 = 0
    cumulPOS0 = 0
    cumulVY0 = 0
    cumulVX0 = 0
    cumulSP00 = 0
    cumulSP10 = 0
    cumulEULER0 = 0
    cumulPOS1 = 0
    cumulVY1 = 0
    cumulVX1 = 0
    cumulSP01 = 0
    cumulSP11 = 0
    cumulEULER1 = 0

    particlesTargetVelocities = [0, 0, 0, 0]
    particlesTargetVelocities2 = [0, 0, 0, 0]
    particlesTargetVelocities0 = [0, 0, 0, 0]
    particlesTargetVelocities1 = [0, 0, 0, 0]
    #speed weight:
    vParam = -2
    #parameters for vertical control
    Kpv = 2
    Kiv = 0
    Kdv = 2
    #parameters for horizontal control:
    Kph = 0.4  # TBD
    Kih = 0.1  # TBD
    Kdh = 1.5
    Kph_pos1 = 0.4
    Kih_pos1 = 0.001
    Kdh_pos1 = 0.05
    Kph_pos0 = 0.4
    Kih_pos0 = 0.001
    Kdh_pos0 = 0.05
    #parameters for rotational control:
    Kpr = 0.05
    #    Kir=0
    Kdr = 0.9
    """ =========================================================== """
    gd = 0
    """ =========================================================== """
    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)  # Connect to V-REP

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

        # enable the synchronous mode on the client:
        vrep.simxSynchronous(clientID, True)

        # start the simulation:
        vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)

        #functional/handle code:
        emptyBuff = bytearray()
        [returnCode,
         targetObj] = vrep.simxGetObjectHandle(clientID, Quadricopter_target,
                                               vrep.simx_opmode_blocking)
        [returnCode,
         qc_base_handle] = vrep.simxGetObjectHandle(clientID,
                                                    Quadricopter_base,
                                                    vrep.simx_opmode_blocking)
        [returnCode,
         qc_handle] = vrep.simxGetObjectHandle(clientID, Quadricopter,
                                               vrep.simx_opmode_blocking)

        [returnCode,
         targetObj2] = vrep.simxGetObjectHandle(clientID, Quadricopter_target2,
                                                vrep.simx_opmode_blocking)
        [returnCode,
         qc_base_handle2] = vrep.simxGetObjectHandle(clientID,
                                                     Quadricopter_base2,
                                                     vrep.simx_opmode_blocking)
        [returnCode,
         qc_handle2] = vrep.simxGetObjectHandle(clientID, Quadricopter2,
                                                vrep.simx_opmode_blocking)

        [returnCode,
         targetObj0] = vrep.simxGetObjectHandle(clientID, Quadricopter_target0,
                                                vrep.simx_opmode_blocking)
        [returnCode,
         qc_base_handle0] = vrep.simxGetObjectHandle(clientID,
                                                     Quadricopter_base0,
                                                     vrep.simx_opmode_blocking)
        [returnCode,
         qc_handle0] = vrep.simxGetObjectHandle(clientID, Quadricopter0,
                                                vrep.simx_opmode_blocking)

        [returnCode,
         targetObj1] = vrep.simxGetObjectHandle(clientID, Quadricopter_target1,
                                                vrep.simx_opmode_blocking)
        [returnCode,
         qc_base_handle1] = vrep.simxGetObjectHandle(clientID,
                                                     Quadricopter_base1,
                                                     vrep.simx_opmode_blocking)
        [returnCode,
         qc_handle1] = vrep.simxGetObjectHandle(clientID, Quadricopter1,
                                                vrep.simx_opmode_blocking)
        # main loop:
        while True:
            """ ========== vertical control: ========== """
            [returnCode,
             targetPos] = vrep.simxGetObjectPosition(clientID, targetObj, -1,
                                                     vrep.simx_opmode_blocking)
            [returnCode,
             pos] = vrep.simxGetObjectPosition(clientID, qc_base_handle, -1,
                                               vrep.simx_opmode_blocking)
            [returnCode, l,
             w] = vrep.simxGetObjectVelocity(clientID, qc_base_handle,
                                             vrep.simx_opmode_blocking)

            [returnCode, targetPos2
             ] = vrep.simxGetObjectPosition(clientID, targetObj2, -1,
                                            vrep.simx_opmode_blocking)
            [returnCode,
             pos2] = vrep.simxGetObjectPosition(clientID, qc_base_handle2, -1,
                                                vrep.simx_opmode_blocking)
            [returnCode, l2,
             w2] = vrep.simxGetObjectVelocity(clientID, qc_base_handle2,
                                              vrep.simx_opmode_blocking)

            [returnCode, targetPos0
             ] = vrep.simxGetObjectPosition(clientID, targetObj0, -1,
                                            vrep.simx_opmode_blocking)
            [returnCode,
             pos0] = vrep.simxGetObjectPosition(clientID, qc_base_handle0, -1,
                                                vrep.simx_opmode_blocking)
            [returnCode, l0,
             w0] = vrep.simxGetObjectVelocity(clientID, qc_base_handle0,
                                              vrep.simx_opmode_blocking)

            [returnCode, targetPos1
             ] = vrep.simxGetObjectPosition(clientID, targetObj1, -1,
                                            vrep.simx_opmode_blocking)
            [returnCode,
             pos1] = vrep.simxGetObjectPosition(clientID, qc_base_handle1, -1,
                                                vrep.simx_opmode_blocking)
            [returnCode, l1,
             w1] = vrep.simxGetObjectVelocity(clientID, qc_base_handle1,
                                              vrep.simx_opmode_blocking)
            """ =========================================================== """
            """ ========== horizontal control: ========== """
            [returnCode,
             sp] = vrep.simxGetObjectPosition(clientID, targetObj,
                                              qc_base_handle,
                                              vrep.simx_opmode_blocking)
            [rc, rc, vx, rc, rc] = vrep.simxCallScriptFunction(
                clientID, Quadricopter, vrep.sim_scripttype_childscript,
                'qc_Get_vx', [], [], [], emptyBuff, vrep.simx_opmode_blocking)
            [rc, rc, vy, rc, rc] = vrep.simxCallScriptFunction(
                clientID, Quadricopter, vrep.sim_scripttype_childscript,
                'qc_Get_vy', [], [], [], emptyBuff, vrep.simx_opmode_blocking)
            [rc, rc, rc, rc,
             rc] = vrep.simxCallScriptFunction(clientID, Quadricopter,
                                               vrep.sim_scripttype_childscript,
                                               'qc_Get_Object_Matrix', [], [],
                                               [], emptyBuff,
                                               vrep.simx_opmode_blocking)
            [errorCode,
             M] = vrep.simxGetStringSignal(clientID, 'mtable',
                                           vrep.simx_opmode_oneshot_wait)
            if (errorCode == vrep.simx_return_ok):
                m = vrep.simxUnpackFloats(M)

            m1 = m[0]
            m2 = m[1]
            m0 = m[2]
            m11 = m[3]
            vx1 = [vx[0], vx[1], vx[2]]
            vx2 = [vx[3], vx[4], vx[5]]
            vx0 = [vx[6], vx[7], vx[8]]
            vx11 = [vx[9], vx[10], vx[11]]
            vy1 = [vy[0], vy[1], vy[2]]
            vy2 = [vy[3], vy[4], vy[5]]
            vy0 = [vy[6], vy[7], vy[8]]
            vy11 = [vy[9], vy[10], vy[11]]

            [returnCode,
             sp2] = vrep.simxGetObjectPosition(clientID, targetObj2,
                                               qc_base_handle2,
                                               vrep.simx_opmode_blocking)

            [returnCode,
             sp0] = vrep.simxGetObjectPosition(clientID, targetObj0,
                                               qc_base_handle0,
                                               vrep.simx_opmode_blocking)

            [returnCode,
             sp1] = vrep.simxGetObjectPosition(clientID, targetObj1,
                                               qc_base_handle1,
                                               vrep.simx_opmode_blocking)
            """ =========================================================== """
            """ ========== rotational control: ========== """
            [returnCode,
             euler] = vrep.simxGetObjectOrientation(clientID, targetObj,
                                                    qc_base_handle,
                                                    vrep.simx_opmode_blocking)
            [returnCode, orientation
             ] = vrep.simxGetObjectOrientation(clientID, qc_base_handle, -1,
                                               vrep.simx_opmode_blocking)

            [returnCode,
             euler2] = vrep.simxGetObjectOrientation(clientID, targetObj2,
                                                     qc_base_handle2,
                                                     vrep.simx_opmode_blocking)
            [returnCode, orientation2
             ] = vrep.simxGetObjectOrientation(clientID, qc_base_handle2, -1,
                                               vrep.simx_opmode_blocking)

            [returnCode,
             euler0] = vrep.simxGetObjectOrientation(clientID, targetObj0,
                                                     qc_base_handle0,
                                                     vrep.simx_opmode_blocking)
            [returnCode, orientation0
             ] = vrep.simxGetObjectOrientation(clientID, qc_base_handle0, -1,
                                               vrep.simx_opmode_blocking)

            [returnCode,
             euler1] = vrep.simxGetObjectOrientation(clientID, targetObj1,
                                                     qc_base_handle1,
                                                     vrep.simx_opmode_blocking)
            [returnCode, orientation1
             ] = vrep.simxGetObjectOrientation(clientID, qc_base_handle1, -1,
                                               vrep.simx_opmode_blocking)
            """ =========================================================== """
            """ ========== set propeller velocities: ========== """
            particlesTargetVelocities, cumul, last_e, cumulAlpha, pAlphaE, cumulBeta, pBetaE, cumulAlphaPos, cumulBetaPos, psp2, psp1, prevEuler, cumulPOS, cumulVY, cumulVX, cumulSP0, cumulSP1, cumulEULER = main_PID_controller.PID(
                targetPos, pos, cumul, last_e, Kpv, Kiv, Kdv, vParam, l, vy1,
                m1, cumulAlpha, pAlphaE, Kph, Kih, Kdh, vx1, cumulBeta, pBetaE,
                cumulAlphaPos, cumulBetaPos, sp, Kph_pos1, Kih_pos1, Kdh_pos1,
                Kph_pos0, Kih_pos0, Kdh_pos0, psp2, psp1, Kpr, Kdr, euler,
                prevEuler, cumulPOS, cumulVY, cumulVX, cumulSP0, cumulSP1,
                cumulEULER, vx, vy, pos, sp, euler)
            particlesTargetVelocities2, cumul2, last_e2, cumulAlpha2, pAlphaE2, cumulBeta2, pBetaE2, cumulAlphaPos2, cumulBetaPos2, psp22, psp12, prevEuler2, cumulPOS2, cumulVY2, cumulVX2, cumulSP02, cumulSP12, cumulEULER2 = main_PID_controller.PID(
                targetPos2, pos2, cumul2, last_e2, Kpv, Kiv, Kdv, vParam, l2,
                vy2, m2, cumulAlpha2, pAlphaE2, Kph, Kih, Kdh, vx2, cumulBeta2,
                pBetaE2, cumulAlphaPos2, cumulBetaPos2, sp2, Kph_pos1,
                Kih_pos1, Kdh_pos1, Kph_pos0, Kih_pos0, Kdh_pos0, psp22, psp12,
                Kpr, Kdr, euler2, prevEuler2, cumulPOS2, cumulVY2, cumulVX2,
                cumulSP02, cumulSP12, cumulEULER2, vx, vy, pos, sp, euler)
            particlesTargetVelocities0, cumul0, last_e0, cumulAlpha0, pAlphaE0, cumulBeta0, pBetaE0, cumulAlphaPos0, cumulBetaPos0, psp20, psp10, prevEuler0, cumulPOS0, cumulVY0, cumulVX0, cumulSP00, cumulSP10, cumulEULER0 = main_PID_controller.PID(
                targetPos0, pos0, cumul0, last_e0, Kpv, Kiv, Kdv, vParam, l0,
                vy0, m0, cumulAlpha0, pAlphaE0, Kph, Kih, Kdh, vx0, cumulBeta0,
                pBetaE0, cumulAlphaPos0, cumulBetaPos0, sp0, Kph_pos1,
                Kih_pos1, Kdh_pos1, Kph_pos0, Kih_pos0, Kdh_pos0, psp20, psp10,
                Kpr, Kdr, euler0, prevEuler0, cumulPOS0, cumulVY0, cumulVX0,
                cumulSP00, cumulSP10, cumulEULER0, vx, vy, pos, sp, euler)
            particlesTargetVelocities1, cumul1, last_e1, cumulAlpha1, pAlphaE1, cumulBeta1, pBetaE1, cumulAlphaPos1, cumulBetaPos1, psp21, psp11, prevEuler1, cumulPOS1, cumulVY1, cumulVX1, cumulSP01, cumulSP11, cumulEULER1 = main_PID_controller.PID(
                targetPos1, pos1, cumul1, last_e1, Kpv, Kiv, Kdv, vParam, l1,
                vy11, m11, cumulAlpha1, pAlphaE1, Kph, Kih, Kdh, vx11,
                cumulBeta1, pBetaE1, cumulAlphaPos1, cumulBetaPos1, sp1,
                Kph_pos1, Kih_pos1, Kdh_pos1, Kph_pos0, Kih_pos0, Kdh_pos0,
                psp21, psp11, Kpr, Kdr, euler1, prevEuler1, cumulPOS1,
                cumulVY1, cumulVX1, cumulSP01, cumulSP11, cumulEULER1, vx, vy,
                pos, sp, euler)

            particlesTargetVelocities_s = [
                particlesTargetVelocities[0], particlesTargetVelocities[1],
                particlesTargetVelocities[2], particlesTargetVelocities[3],
                particlesTargetVelocities2[0], particlesTargetVelocities2[1],
                particlesTargetVelocities2[2], particlesTargetVelocities2[3],
                particlesTargetVelocities0[0], particlesTargetVelocities0[1],
                particlesTargetVelocities0[2], particlesTargetVelocities0[3],
                particlesTargetVelocities1[0], particlesTargetVelocities1[1],
                particlesTargetVelocities1[2], particlesTargetVelocities1[3]
            ]
            """ =========================================================== """
            """ =========================================================== """
            """ ========== PLOTTING: ========== """
            ## PLOTTING:
            #            ze.append(targetPos[2]-pos[2]) # otstapuvanje od z-oska
            #            xe.append(sp[0]) # otstapuvanje od x-oska
            #            ye.append(sp[1]) # otstapuvawe od y-oska
            #            xs.append(targetPos[0])
            #            ys.append(targetPos[1])
            #            zs.append(targetPos[2])
            #            x_qc.append(pos[0])
            #            y_qc.append(pos[1])
            #            z_qc.append(pos[2])
            #            x_qc2.append(pos2[0])
            #            y_qc2.append(pos2[1])
            #            z_qc2.append(pos2[2])
            #            fig = plt.figure()
            #            ax = fig.add_subplot(111, projection='3d')
            #            ax.plot_wireframe(x_qc,y_qc,z_qc,color='blue')
            #            ax.plot_wireframe(x_qc2,y_qc2,z_qc2,color='red')
            #            ax.set_xlim3d(-2.1,2.1)
            #            ax.set_ylim3d(-2.1,2.1)
            #            ax.set_zlim3d(0,1.9)
            #            plt.show()

            #            u.append(thrust)
            ''' !!! treba da se nacrta i pozicijata i da se sporedi so i bez consensus !!! '''
            xl.append(pos[0])
            yl.append(pos[1])
            zl.append(pos[2])

            xf2.append(pos2[0])
            yf2.append(pos2[1])
            zf2.append(pos2[2])

            xf0.append(pos0[0])
            yf0.append(pos0[1])
            zf0.append(pos0[2])

            xf1.append(pos1[0])
            yf1.append(pos1[1])
            zf1.append(pos1[2])

            plt.plot(zl, color="blue")
            plt.hold(True)
            plt.plot(zf2, color="red")
            plt.hold(True)
            plt.plot(zf0, color="pink")
            plt.hold(True)
            plt.plot(zf1, color="green")
            plt.hold(True)
            plt.axis([0, 199, 0, 1.4])
            plt.show()
            plt.plot(xl, color="blue")
            plt.hold(True)
            plt.plot(xf2, color="red")
            plt.hold(True)
            plt.plot(xf0, color="pink")
            plt.hold(True)
            plt.plot(xf1, color="green")
            plt.hold(True)
            plt.axis([0, 199, -2, 2])
            plt.show()
            plt.plot(yl, color="blue")
            plt.hold(True)
            plt.plot(yf2, color="red")
            plt.hold(True)
            plt.plot(yf0, color="pink")
            plt.hold(True)
            plt.plot(yf1, color="green")
            plt.hold(True)
            plt.axis([0, 199, -2, 1.5])
            plt.show()

            v1.append(l[2])
            v2.append(l2[2])
            v3.append(l0[2])
            v4.append(l1[2])
            plt.plot(v1, color="blue")
            plt.hold(True)
            plt.plot(v2, color="red")
            plt.hold(True)
            plt.plot(v3, color="pink")
            plt.hold(True)
            plt.plot(v4, color="green")
            plt.hold(True)
            plt.axis([0, 199, -0.1, 1])
            plt.show()

            w1.append(w[2])
            w2.append(w2[2])
            w3.append(w0[2])
            w4.append(w1[2])
            plt.plot(w1, color="blue")
            plt.hold(True)
            plt.plot(w2, color="red")
            plt.hold(True)
            plt.plot(w3, color="pink")
            plt.hold(True)
            plt.plot(w4, color="green")
            plt.hold(True)
            plt.axis([0, 199, -0.5, 0.5])
            plt.show()
            #            v12.append(particlesTargetVelocities2[0])
            #            v22.append(particlesTargetVelocities2[1])
            #            v32.append(particlesTargetVelocities2[2])
            #            v42.append(particlesTargetVelocities2[3])
            #            plt.plot(yl,color='blue')
            #            plt.hold(True)
            #            plt.plot(yf,color='red')
            #            plt.plot(euler_l,color='blue')
            #            plt.plot(euler_f,color='red')
            #            plt.plot(yl,color='green')
            #            plt.hold(True)
            #            plt.plot(yf,color='pink')
            #            plt.hold(True)
            #            plt.plot(v3,color='green')
            #            plt.hold(True)
            #            plt.plot(v4,color='pink')
            #            plt.hold(True)
            #            plt.axis([0,149,-3,3])
            #            plt.show()
            #            print " Zaxis error between drones: ", pos[2]-pos2[2]
            ze.append(pos[2] - pos2[2])
            #            ze.append(abs(euler[2])-abs(euler2[2]))
            #            plt.plot(ze,color='green')
            #            plt.axis([0,149,-3,3])
            #            plt.show()
            """ =========================================================== """
            """ ========== Gradient descent: ========== """
            #            sum_h_alpha.append(cumulAlpha)
            #            sum_h_beta.append(cumulBeta)
            #            sum_h_pos1.append(cumulAlphaPos)
            #            sum_h_pos0.append(cumulBetaPos)

            #            J_h_alpha.append((1/(tf - t0))*(5*cumulAlpha**2 + delta_alpha_angle[gd]**2)*dt)
            #            J_h_beta.append((1/(tf - t0))*(5*cumulBeta**2 + delta_beta_angle[gd]**2)*dt)
            #            J_h_pos1.append((1/(tf - t0))*(5*cumulAlphaPos**2 + delta_alpha_pos[gd]**2)*dt)
            #            J_h_pos0.append((1/(tf - t0))*(5*cumulBetaPos**2 + delta_beta_pos[gd]**2)*dt)

            #            paramX.append(targetPos[0])
            #            paramY.append(targetPos[1])

            #            print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
            #            print "theta_h_angles : ", theta_h_angles
            #            print "theta_h_pos : ", theta_h_pos
            #
            #            print "sum_h_alpha : ", sum_h_alpha[gd]
            #            print "sum_h_beta : ", sum_h_beta[gd]
            #            print "sum_h_pos1 : ", sum_h_pos1[gd]
            #            print "sum_h_pos0 : ", sum_h_pos0[gd]

            #            print "J_h_alpha : ", J_h_alpha[gd]
            #            print "J_h_beta : ", J_h_beta[gd]
            #            print "J_h_pos1 : ", J_h_pos1[gd]
            #            print "J_h_pos0 : ", J_h_pos0[gd]

            #            print "paramX : ", paramX[gd]
            #            print "paramY : ", paramY[gd]
            #            print "cumulPOS : ", cumulPOS2
            #            print "cumulVY : ", cumulVY2
            #            print "cumulVX : ", cumulVX2
            #            print "cumulSP0 : ", cumulSP02
            #            print "cumulSP1 : ", cumulSP12
            #            print "cumulEULER : ", cumulEULER2
            #            print "cumul : ", cumul
            print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
            gd = gd + 1
            print "gd (iterations) : ", gd
            print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
            """ =========================================================== """
            if gd == 200:
                #                with open('GD_43.csv','wb') as f1:
                #                    writer=csv.writer(f1)
                #                    for i in range(0, gd):
                #                        writer.writerow([paramX[i],paramY[i],theta_h_angles[0],theta_h_angles[1],theta_h_angles[2],theta_h_pos[0],theta_h_pos[1],theta_h_pos[2],J_h_alpha[i],J_h_beta[i],J_h_pos1[i],J_h_pos0[i]])
                break

#            """ =========================================================== """
            """ ========== WRITE TO TEXT: ========== """
            ## WRITE TO TEXT:
            #            log_data(C_parameters_vert, C_parameters_horr, C_parameters_rot, vert_comp, horr_comp, rot_comp, particlesTargetVelocities, i)
            #            i +=1
            """ =========================================================== """
            # send propeller velocities to output:
            [res, retInts, retFloats, retStrings,
             retBuffer] = vrep.simxCallScriptFunction(
                 clientID, Quadricopter, vrep.sim_scripttype_childscript,
                 'qc_propeller_v', [], particlesTargetVelocities_s, [],
                 emptyBuff, vrep.simx_opmode_blocking)
            #            [res,retInts,retFloats,retStrings,retBuffer]=vrep.simxCallScriptFunction(clientID,Quadricopter2,vrep.sim_scripttype_childscript,'qc_propeller_v',[],particlesTargetVelocities2,[],emptyBuff,vrep.simx_opmode_blocking)
            vrep.simxSynchronousTrigger(clientID)
            """ =========================================================== """
#        print " Zaxis error between drones: ", ze
# stop the simulation:
        vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)

        # Now close the connection to V-REP:
        vrep.simxFinish(clientID)

    else:
        print('Failed connecting to remote API server')
예제 #32
0
    def initialize_handles(self):
        self._RightMotor = self._vrep_get_object_handle(
            'Right_Motor{}'.format(self._value_number),
            vrep.simx_opmode_blocking)
        self._LeftMotor = self._vrep_get_object_handle(
            'Left_Motor{}'.format(self._value_number),
            vrep.simx_opmode_blocking)
        self._Robobo = self._vrep_get_object_handle(
            'Robobo{}'.format(self._value_number), vrep.simx_opmode_blocking)

        self._IrBackC = self._vrep_get_object_handle(
            'Ir_Back_C{}'.format(self._value_number),
            vrep.simx_opmode_blocking)
        self._IrFrontC = self._vrep_get_object_handle(
            'Ir_Front_C{}'.format(self._value_number),
            vrep.simx_opmode_blocking)
        self._IrFrontLL = self._vrep_get_object_handle(
            'Ir_Front_LL{}'.format(self._value_number),
            vrep.simx_opmode_blocking)
        self._IrFrontRR = self._vrep_get_object_handle(
            'Ir_Front_RR{}'.format(self._value_number),
            vrep.simx_opmode_blocking)
        self._IrBackL = self._vrep_get_object_handle(
            'Ir_Back_L{}'.format(self._value_number),
            vrep.simx_opmode_blocking)
        self._IrBackLFloor = self._vrep_get_object_handle(
            'Ir_Back_L_Floor{}'.format(self._value_number),
            vrep.simx_opmode_blocking)
        self._IrBackR = self._vrep_get_object_handle(
            'Ir_Back_R{}'.format(self._value_number),
            vrep.simx_opmode_blocking)
        self._IrBackRFloor = self._vrep_get_object_handle(
            'Ir_Back_R_Floor{}'.format(self._value_number),
            vrep.simx_opmode_blocking)
        self._IrFrontL = self._vrep_get_object_handle(
            'Ir_Front_L{}'.format(self._value_number),
            vrep.simx_opmode_blocking)
        self._IrFrontLFloor = self._vrep_get_object_handle(
            'Ir_Front_L_Floor{}'.format(self._value_number),
            vrep.simx_opmode_blocking)
        self._IrFrontR = self._vrep_get_object_handle(
            'Ir_Front_R{}'.format(self._value_number),
            vrep.simx_opmode_blocking)
        self._IrFrontRFloor = self._vrep_get_object_handle(
            'Ir_Front_R_Floor{}'.format(self._value_number),
            vrep.simx_opmode_blocking)
        self._TiltMotor = self._vrep_get_object_handle(
            'Tilt_Motor{}'.format(self._value_number),
            vrep.simx_opmode_blocking)
        self._PanMotor = self._vrep_get_object_handle(
            'Pan_Motor{}'.format(self._value_number),
            vrep.simx_opmode_blocking)
        self._FrontalCamera = self._vrep_get_object_handle(
            'Frontal_Camera{}'.format(self._value_number),
            vrep.simx_opmode_blocking)

        # read a first value in streaming mode
        self._vrep_read_proximity_sensor_ignore_error(self._IrFrontC)
        self._vrep_read_proximity_sensor_ignore_error(self._IrBackC)
        self._vrep_read_proximity_sensor_ignore_error(self._IrFrontLL)
        self._vrep_read_proximity_sensor_ignore_error(self._IrFrontRR)
        self._vrep_read_proximity_sensor_ignore_error(self._IrBackL)
        self._vrep_read_proximity_sensor_ignore_error(self._IrBackLFloor)
        self._vrep_read_proximity_sensor_ignore_error(self._IrBackR)
        self._vrep_read_proximity_sensor_ignore_error(self._IrBackRFloor)
        self._vrep_read_proximity_sensor_ignore_error(self._IrFrontR)
        self._vrep_read_proximity_sensor_ignore_error(self._IrFrontRFloor)
        self._vrep_read_proximity_sensor_ignore_error(self._IrFrontL)
        self._vrep_read_proximity_sensor_ignore_error(self._IrFrontLFloor)

        # setup join positions
        vrep.simxGetJointPosition(self._clientID, self._RightMotor,
                                  vrep.simx_opmode_buffer)
        vrep.simxGetJointPosition(self._clientID, self._LeftMotor,
                                  vrep.simx_opmode_buffer)
        vrep.simxGetObjectPosition(self._clientID, self._Robobo, -1,
                                   vrep.simx_opmode_buffer)

        # read a first value in buffer mode
        self._vrep_get_vision_sensor_image_ignore_error(
            self._FrontalCamera, vrep.simx_opmode_streaming)

        self.wait_for_ping()
예제 #33
0
def GetSuctionAbsPosition():
    result, position = vrep.simxGetObjectPosition(clientID, suction, -1,
                                                  vrep.simx_opmode_blocking)
    if result != vrep.simx_return_ok:
        raise Exception('could not get suction position')
    return position
예제 #34
0
def run_sim(model, clientID):
	vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait) # start simulation
	
	previousPosition = [-2.71879,-1.90075,36.263,1.35473,0.780061,-35.7055,-0.225083,-3.02226,1.82057,0.258116,-2.12325,1.061]
	
	#RhipX,RhipY,RhipZ,LhipX,LhipY,LhipZ,RKneeZ,LKneeZ,RAnkleX,RAnkleZ,LAnkleX,LAnkleZ
	legJoints = [0,0,0,0,0,0,0,0,0,0,0,0]
	legJointInv = [1,1,1,-1,-1,1,1,1,1,1,-1,1]
	hipJointInv = [1,1,1,1,1,-1,1,1,1,1,1,1]
	
	returnCode01, legJoints[0] = vrep.simxGetObjectHandle(clientID, "rightLegJoint0", vrep.simx_opmode_blocking)
	returnCode02, legJoints[1] = vrep.simxGetObjectHandle(clientID, "rightLegJoint1", vrep.simx_opmode_blocking)
	returnCode03, legJoints[2] = vrep.simxGetObjectHandle(clientID, "rightLegJoint2", vrep.simx_opmode_blocking)
	returnCode04, legJoints[6] = vrep.simxGetObjectHandle(clientID, "rightLegJoint3", vrep.simx_opmode_blocking) 
	returnCode05, legJoints[8] = vrep.simxGetObjectHandle(clientID, "rightLegJoint5", vrep.simx_opmode_blocking)
	returnCode06, legJoints[9] = vrep.simxGetObjectHandle(clientID, "rightLegJoint4", vrep.simx_opmode_blocking)
	
	returnCode07, legJoints[3] = vrep.simxGetObjectHandle(clientID, "leftLegJoint0", vrep.simx_opmode_blocking)
	returnCode08, legJoints[4] = vrep.simxGetObjectHandle(clientID, "leftLegJoint1", vrep.simx_opmode_blocking)
	returnCode09, legJoints[5] = vrep.simxGetObjectHandle(clientID, "leftLegJoint2", vrep.simx_opmode_blocking)
	returnCode010, legJoints[7] = vrep.simxGetObjectHandle(clientID, "leftLegJoint3", vrep.simx_opmode_blocking) 
	returnCode011, legJoints[10] = vrep.simxGetObjectHandle(clientID, "leftLegJoint5", vrep.simx_opmode_blocking)
	returnCode012, legJoints[11] = vrep.simxGetObjectHandle(clientID, "leftLegJoint4", vrep.simx_opmode_blocking)
	
	returnCode013, head = vrep.simxGetObjectHandle(clientID, "Asti", vrep.simx_opmode_blocking)
	returnCode013, r_foot = vrep.simxGetObjectHandle(clientID, "rightFoot", vrep.simx_opmode_blocking)
	returnCode013, l_foot = vrep.simxGetObjectHandle(clientID, "leftFoot", vrep.simx_opmode_blocking)
	
	#RhipX,RhipY,RhipZ,LhipX,LhipY,LhipZ,RKneeZ,LKneeZ,RAnkleX,RAnkleZ,LAnkleX,LAnkleZ
	legPositions = [0,0,0,0,0,0,0,0,0,0,0,0]
	
	headLocation = [1,1,1];
	begin_r = [0,0,0]
	begin_l = [0,0,0]
	end_r = [0,0,0]
	end_r = [0,0,0]
	
	for i in range(12):
		returnCode, legPositions[i] = vrep.simxGetJointPosition(clientID, legJoints[i], vrep.simx_opmode_streaming)
		returncode, headLocation = vrep.simxGetObjectPosition(clientID, head, -1, vrep.simx_opmode_streaming)
		returnCode, begin_r =  vrep.simxGetObjectPosition(clientID, r_foot, -1, vrep.simx_opmode_streaming)
		returnCode, begin_l =  vrep.simxGetObjectPosition(clientID, l_foot, -1, vrep.simx_opmode_streaming)
	
	start_time = int(round(time.time()))
	while vrep.simxGetConnectionId(clientID) != -1 and ((headLocation[2] > 0.0 or headLocation[2] == 0.0) and ((int(round(time.time())) - start_time) < 100)):
		for i in range(12):
			returnCode, legPositions[i] = vrep.simxGetJointPosition(clientID, legJoints[i], vrep.simx_opmode_buffer)
		
		# do something to the joints dummy right now
		#remember to convert to radians and back
		degreeLegPositions = [0,0,0,0,0,0,0,0,0,0,0,0]
		for i in range(len(legPositions)):
			degreeLegPositions[i] = legPositions[i] * legJointInv[i] * 180 / math.pi
		
		inputData = []
		for element in degreeLegPositions:
			inputData.append(element)
		
		newJointPositions = model_iterator.runModel(model,[tuple(inputData)])
		
		newJointPositions = newJointPositions[0]
		
		for i in range(len(newJointPositions)):
			newJointPositions[i] = newJointPositions[i] * math.pi / 180
		
		vrep.simxPauseCommunication(clientID,1)
		for i in range(12):
			vrep.simxSetJointTargetPosition(clientID,legJoints[i],newJointPositions[i] * legJointInv[i] * hipJointInv[i],vrep.simx_opmode_oneshot)
		vrep.simxPauseCommunication(clientID,0)
		time.sleep(0.001)
		
		returncode, headLocation = vrep.simxGetObjectPosition(clientID, head, -1, vrep.simx_opmode_buffer)
	
	returnCode, end_r =  vrep.simxGetObjectPosition(clientID, r_foot, -1, vrep.simx_opmode_buffer)
	returnCode, end_l =  vrep.simxGetObjectPosition(clientID, l_foot, -1, vrep.simx_opmode_buffer)
	
	distanceTraveled = (((end_r[1] + end_l[1])/2) - ((begin_r[1] + begin_l[1])/2))
			
	vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
	time.sleep(0.25)
	
	return distanceTraveled
예제 #35
0
def run_onlyimu_training(sim_number, sim_time):
    print("start running onlyIMU model")
    prefs.codegen.target = "numpy"
    start_scope()

    # number of sensor in robot
    N_sensor = 8
    N_vision = 2

    # robot radius
    r = 0.0586

    # CD neurons number
    N_CD = 36
    # vision detection_neurons number
    N_VD = 10

    # HD neuron number
    N_HD = 72
    # coordinate neuron
    N_x_axis, N_y_axis = 32, 32
    N_PI = N_x_axis * N_y_axis

    # Simulation time
    # Speed cell number
    N_speed = 6

    m = 5  # 60
    iter = 0

    # width and length of the arena x=width/ y=lenght
    # 1 square in vrep = 0.5*0.5
    x_scale = 2
    y_scale = 2
    # distance unit per neuron
    N = 200 / (N_x_axis * N_y_axis)

    # define the initial mismatch encounter
    last_mismatch = 0

    ##--------------------------------------------------------------------------------------------------------------------##
    ##--------------------Collision detection Neural Architecture--------------------------------------------##
    ##--------------------------------------------------------------------------------------------------------------------##
    Notred_red_inh, spikemon_red, spikemon_notred, spikemon_estimatedlandmark, spikemon_nonlandmark, wall_landmark_synapse, landmark_wall_synapse, Poisson_group, Poisson_vision, Collision_neuron, Collision_or_not, Color, Wall, Landmark, Poisson_synapse, PoissonVision_synapse, Self_ex_synapse, Self_ex_synapse_color, Self_in_synapse, Self_in_synapse_color, Collide_or_not_synapse, Red_landmark_ex, Red_wall_inh, Notred_landmark_inh, spikemon_CD, spikemon_collision, spikemon_landmark, spikemon_poisson, spikemon_wall, Poisson_non_collision, Non_collision_neuron, Poisson_non_synapse, CON_noncollision_synapse, Non_collision_color_synapses, spikemon_noncollison, Estimated_landmark_neuron, Non_landmark_neuron, Poisson_nonlandmark_synapse, Landmark_nonlandmark_synapse, CON_landmark_ex, CON_wall_ex, Notred_wall_ex, Red, Notred, color_notred_synapses, color_red_synapses = CD_net(
        N_CD, N_vision, N_VD)

    # gaussian input for 8 collison sensors
    stimuli = np.array([
        gaussian_spike(N_CD, j * N_CD / N_sensor, 10, 0.2)
        for j in range(N_sensor)
    ])

    # gaussion distribution for 2 vision input(wall or landmark)
    stimuli_vision = np.array([
        gaussian_spike(N_VD, j * N_VD / N_vision, 10, 0.2)
        for j in range(N_vision)
    ])

    ##--------------------------------------------------------------------------------------------------------------------##
    ##--------------------Head Direction and position Neural Architecture----------------------------------------------------------##
    ##--------------------------------------------------------------------------------------------------------------------##
    IHD_in_synapse, str_inh_synapse, Poisson_straight, Go_straight_neuron, Go_straight, HD_str_synapse, str_IHD_synapse, sti_PI, sti, Poisson_Left, Poisson_Right, Poisson_Compass, Left_drive, Right_drive, HD_Neuron, IHD_Neuron, Left_shift_neuron, Left_shift_speed_neuron, Right_shift_neuron, Right_shift_speed_neuron, HD_Left_synapse, Left_IHD_synapse, HD_Right_synapse, Left_drive_synapse, Right_drive_synapse, Right_IHD_synapse, HD_IHD_synapse, Reset_synapse, HD_ex_synapse, HD_in_synapse, spikemon_HD, spikemon_IHD, Poisson_PI, PI_Neurons, IPI_Neurons, Directional_Neurons, HD_directional_synapse, directional_PI_synapse, PI_shifting_neurons, North_shifting_neurons, South_shifting_neurons, East_shifting_neurons, West_shifting_neurons, PI_ex_synapse, PI_in_synapse, PI_N_synapse, PI_S_synapse, PI_E_synapse, PI_W_synapse, IPI_N_synapse, IPI_S_synapse, IPI_E_synapse, IPI_W_synapse, IPI_PI_synapse, PI_Reset_synapse, spikemon_PI, spikemon_IPI, NE_shifting_neurons, SE_shifting_neurons, WS_shifting_neurons, WN_shifting_neurons, PI_NE_synapse, PI_SE_synapse, PI_WS_synapse, PI_WN_synapse, IPI_NE_synapse, IPI_SE_synapse, IPI_WS_synapse, IPI_WN_synapse, Left_inh_synapse, Right_IHD_synapse, IPI_in_synapse, IPI_stay_synapse, Stay_stay_layer, Stay_layer, Stay, PI_stay_synapse = HD_PI_integrated_net(
        N_HD, N_speed, N_x_axis, N_y_axis, sim_time)

    ##-----------------head direction correcting matrix network-------------#
    # Poisson_IMU,IMU_Neuron,HD_errormatrix_neurons,HD_positive_error,HD_negative_error,IMU_poi_synapses,IMU_errors_connecting_synapses,HD_error_connect_synapses,Error_negative_synapses,Error_positive_synapses,Ex_speed_pool_Neurons,Inh_speed_pool_Neurons,Positive_ex_pool_synapses,Negative_inh_pool_synapses,statemon_positiveHD_error,statemon_negativeHD_error,spikemon_positiveHD_error,spikemon_negativeHD_error,spiketrain_pos=HD_error_correcting(N_HD,HD_Neuron)

    ##--------------------------------------------------------------------------------------------------------------------##
    ##--------------------Fusi Synapse between Position - Landmark Neuron----------------------------------------------------------##
    ##--------------------------------------------------------------------------------------------------------------------##
    '''measure collision weight by fusi synapse landmark'''
    landmark_PI_plastic = fusi_landmark(PI_Neurons, Landmark)
    w_landmark_plas_shape = np.shape(landmark_PI_plastic.w_fusi_landmark)[0]
    w_plastic_landmark = landmark_PI_plastic.w_fusi_landmark
    all_fusi_weights_landmark = landmark_PI_plastic.w_fusi_landmark

    ##--------------------------------------------------------------------------------------------------------------------##
    ##--------------------Fusi Synapse between Position - Wall Neuron----------------------------------------------------------##
    ##--------------------------------------------------------------------------------------------------------------------##
    '''measure collision weight by fusi synapse landmark'''
    wall_PI_plastic = fusi_wall(PI_Neurons, Wall)
    w_plas_shape_wall = np.shape(wall_PI_plastic.w_fusi_wall)[0]
    w_plastic_wall = wall_PI_plastic.w_fusi_wall
    all_fusi_weights_wall = wall_PI_plastic.w_fusi_wall

    ##--------------------------------------------------------------------------------------------------------------------##
    ##-------------------------------------------Mismatch Network--------------------------------------------------------------##
    ##--------------------------------------------------------------------------------------------------------------------##
    '''store previously estimated landmark position by fusi synapse estimatedlandmark'''

    preeq = '''
            v_post += 1*(w_piest >= 0.5)
            '''
    PI_Neurons_est_synapse = Synapses(PI_Neurons,
                                      Estimated_landmark_neuron,
                                      'w_piest : 1',
                                      on_pre=preeq)
    PI_Neurons_est_synapse.connect()

    Mismatch_landmark_inh_synpase, landmark_mismatch_inh_synapse, spikemon_nonestimatedlandmark, NonEst_landmark_poisson, Non_estimatedlandmark_neuron, Mismatch_neuron, Non_Mismatch_neuron, Nonestimatedlandmark_poi_synapse, Est_nonest_inh_synapse, NonCol_mismatch_synapse, NonCol_nonmismatch_synapse, NonEst_mistmatch_inh_synapse, Est_mismatch_ex_synapse, Est_nonmismatch_inh_synapses, Mismatch_est_inh_synpase, spikemon_Mismatch = mismatch_net(
        Non_landmark_neuron, Estimated_landmark_neuron, Landmark)

    ##--------------------------------------------------------------------------------------------------------------------##
    ##----------------------------------------------Network--------------------------------------------------------------##
    ##--------------------------------------------------------------------------------------------------------------------##
    print("adding network")
    # Network
    PINet = Network()
    # add collision network
    PINet.add(Notred_red_inh, spikemon_red, spikemon_notred,
              spikemon_estimatedlandmark, spikemon_nonlandmark,
              wall_landmark_synapse, landmark_wall_synapse, Poisson_group,
              Poisson_vision, Collision_neuron, Collision_or_not, Color, Wall,
              Landmark, Poisson_synapse, PoissonVision_synapse,
              Self_ex_synapse, Self_ex_synapse_color, Self_in_synapse,
              Self_in_synapse_color, Collide_or_not_synapse, Red_landmark_ex,
              Red_wall_inh, Notred_landmark_inh, spikemon_CD,
              spikemon_collision, spikemon_landmark, spikemon_poisson,
              spikemon_wall, Poisson_non_collision, Non_collision_neuron,
              Poisson_non_synapse, CON_noncollision_synapse,
              Non_collision_color_synapses, spikemon_noncollison,
              Estimated_landmark_neuron, Non_landmark_neuron,
              Poisson_nonlandmark_synapse, Landmark_nonlandmark_synapse,
              CON_landmark_ex, CON_wall_ex, Notred_wall_ex, Red, Notred,
              color_notred_synapses, color_red_synapses)

    # add position network
    PINet.add(
        IHD_in_synapse, str_inh_synapse, Poisson_straight, Go_straight_neuron,
        Go_straight, HD_str_synapse, str_IHD_synapse, Poisson_Left,
        Poisson_Right, Poisson_Compass, Left_drive, Right_drive, HD_Neuron,
        IHD_Neuron, Left_shift_neuron, Left_shift_speed_neuron,
        Right_shift_neuron, Right_shift_speed_neuron, HD_Left_synapse,
        Left_IHD_synapse, HD_Right_synapse, Left_drive_synapse,
        Right_drive_synapse, Right_IHD_synapse, HD_IHD_synapse, Reset_synapse,
        HD_ex_synapse, HD_in_synapse, spikemon_HD, spikemon_IHD, Poisson_PI,
        PI_Neurons, IPI_Neurons, Directional_Neurons, HD_directional_synapse,
        directional_PI_synapse, PI_shifting_neurons, North_shifting_neurons,
        South_shifting_neurons, East_shifting_neurons, West_shifting_neurons,
        PI_ex_synapse, PI_in_synapse, PI_N_synapse, PI_S_synapse, PI_E_synapse,
        PI_W_synapse, IPI_N_synapse, IPI_S_synapse, IPI_E_synapse,
        IPI_W_synapse, IPI_PI_synapse, PI_Reset_synapse, spikemon_PI,
        spikemon_IPI, NE_shifting_neurons, SE_shifting_neurons,
        WS_shifting_neurons, WN_shifting_neurons, PI_NE_synapse, PI_SE_synapse,
        PI_WS_synapse, PI_WN_synapse, IPI_NE_synapse, IPI_SE_synapse,
        IPI_WS_synapse, IPI_WN_synapse, Left_inh_synapse, Right_IHD_synapse,
        IPI_in_synapse, IPI_stay_synapse, Stay_stay_layer, Stay_layer, Stay,
        PI_stay_synapse)
    # add fusi plastic synapses for landmark,wall neurons with PI neurons
    PINet.add(landmark_PI_plastic)
    PINet.add(wall_PI_plastic)
    # add mismatch network
    PINet.add(PI_Neurons_est_synapse, Mismatch_landmark_inh_synpase,
              landmark_mismatch_inh_synapse, spikemon_nonestimatedlandmark,
              NonEst_landmark_poisson, Non_estimatedlandmark_neuron,
              Mismatch_neuron, Non_Mismatch_neuron,
              Nonestimatedlandmark_poi_synapse, Est_nonest_inh_synapse,
              NonCol_mismatch_synapse, NonCol_nonmismatch_synapse,
              NonEst_mistmatch_inh_synapse, Est_mismatch_ex_synapse,
              Est_nonmismatch_inh_synapses, Mismatch_est_inh_synpase,
              spikemon_Mismatch)

    ##-----------------------------------------------------------------------------------------##
    ##---------------------------------Robot controller----------------------------------------##
    ##-----------------------------------------------------------------------------------------##
    # Connect to the server
    print('finished adding network')

    vrep.simxFinish(-1)  # just in case, close all opened connections
    clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
    if clientID != -1:
        print("Connected to remote API server")
    else:
        print("Not connected to remote API server")
        sys.exit("Could not connect")
    vrep.simxSynchronous(clientID, 1)

    ##----------------------------------Controller initialized------------------------------------------##
    # set motor
    err_code, l_motor_handle = vrep.simxGetObjectHandle(
        clientID, "KJunior_motorLeft", vrep.simx_opmode_blocking)
    err_code, r_motor_handle = vrep.simxGetObjectHandle(
        clientID, "KJunior_motorRight", vrep.simx_opmode_blocking)

    # Compass output=orientation
    # define robot
    err_code, robot = vrep.simxGetObjectHandle(clientID, 'KJunior',
                                               vrep.simx_opmode_blocking)

    # define Angles
    err_code, Angles = vrep.simxGetObjectOrientation(
        clientID, robot, -1, vrep.simx_opmode_streaming)

    # define object position
    err_code, Position = vrep.simxGetObjectPosition(clientID, robot, -1,
                                                    vrep.simx_opmode_streaming)

    # get sensor
    sensor1, sensor2, sensor3, sensor4, sensor5, sensor6, sensor7, sensor8 = getSensor(
        clientID)
    # read point
    detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4, detectedPoint5, detectedPoint6, detectedPoint7, detectedPoint8 = getDetectedpoint(
        sensor1, sensor2, sensor3, sensor4, sensor5, sensor6, sensor7, sensor8,
        clientID)
    # Distance from sensor to obstacle
    sensor_val1, sensor_val2, sensor_val3, sensor_val4, sensor_val5, sensor_val6, sensor_val7, sensor_val8 = getSensorDistance(
        detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4,
        detectedPoint5, detectedPoint6, detectedPoint7, detectedPoint8)
    # get sensor for mismatch landmark detection
    err_code, sensorMismatch = vrep.simxGetObjectHandle(
        clientID, "Proximity_sensor", vrep.simx_opmode_blocking)
    err_code, detectionStateMismatch, detectedPointMismatch, detectedObjectHandleMismatch, detectedSurfaceNormalVectorMismatch = vrep.simxReadProximitySensor(
        clientID, sensorMismatch, vrep.simx_opmode_streaming)
    mismatch_sensordistance = np.linalg.norm(detectedPointMismatch)

    # get vision sensor
    err_code, camera = vrep.simxGetObjectHandle(clientID, "Vision_sensor",
                                                vrep.simx_opmode_blocking)
    err_code, resolution, image = vrep.simxGetVisionSensorImage(
        clientID, camera, 1, vrep.simx_opmode_streaming)

    ##---------------------------------Sensor initial condition (for CD)------------------------------------##
    # sensor value of every sensor for every neuron
    # Use inv_filter to filter out noise
    # sensor_val = np.array([np.repeat(inv_filter(sensor_val1),N_CD), np.repeat(inv_filter(sensor_val2),N_CD),np.repeat(inv_filter(sensor_val3),N_CD),np.repeat(inv_filter(sensor_val7),N_CD),np.repeat(inv_filter(sensor_val8),N_CD)])
    sensor_val = np.array([
        np.repeat(inv_filter(sensor_val1), N_CD),
        np.repeat(inv_filter(sensor_val2), N_CD),
        np.repeat(inv_filter(sensor_val3), N_CD),
        np.repeat(inv_filter(sensor_val4), N_CD),
        np.repeat(inv_filter(sensor_val5), N_CD),
        np.repeat(inv_filter(sensor_val6), N_CD),
        np.repeat(inv_filter(sensor_val7), N_CD),
        np.repeat(inv_filter(sensor_val8), N_CD)
    ])
    sensor_val_vision = np.zeros([N_vision, N_VD])
    # sum of each sensor value * its gaussian distribution --> sum to find all activity for each neurons --> WTA
    All_stimuli = np.sum(sensor_val * stimuli, axis=0)
    All_stimuli_vision = np.sum(sensor_val_vision * stimuli_vision, axis=0)

    # find the winner
    winner = WTA(All_stimuli)
    for w in winner:
        Poisson_synapse.w_poi[w] = All_stimuli[w]
    winner_vision = WTA(All_stimuli_vision)
    for w in winner_vision:
        PoissonVision_synapse.w_vision_poi[w] = All_stimuli_vision[w]

    ##------------------------------------------------------------------------------------------------##

    # initial speed of wheel
    l_steer = 0.24
    r_steer = 0.24
    # record the initial time
    t_int = time.time()
    # record compass angle
    compass_angle = np.array([])
    # record x and y axis at each time
    x_axis = np.array([])
    y_axis = np.array([])
    # path that is passing
    r = np.array([])
    # all_time
    all_time = np.array([])
    # collision index that the robot collided
    collision_index = np.array([])
    collision_index_during_run = []

    # record weight parameter
    # record every m sec
    weight_record_time = 1
    weight_during_run_landmark = np.zeros(
        [np.shape(w_plastic_landmark)[0], 3 * int(sim_time / 5)])
    weight_during_run_wall = np.zeros(
        [np.shape(w_plastic_wall)[0], 3 * int(sim_time / 5)])
    time_step_list = np.array([])

    # start simulation
    while (time.time() - t_int) < sim_time:

        t1 = time.time() - t_int
        # record proximity sensor at each time step
        sensor_val1, sensor_val2, sensor_val3, sensor_val4, sensor_val5, sensor_val6, sensor_val7, sensor_val8 = getSensorDistance(
            detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4,
            detectedPoint5, detectedPoint6, detectedPoint7, detectedPoint8)
        all_sensor = np.array(
            [sensor_val1, sensor_val2, sensor_val3, sensor_val4, sensor_val5])
        all_sensor[all_sensor < 4.1e-20] = np.infty
        activated_sensor = np.argmin(all_sensor)

        # obtain vision sensor values
        pixelimage = set(image)

        if all_sensor[activated_sensor] < 0.1:
            if activated_sensor == 3:
                r_steer, l_steer, zeta = TurnRight(r_steer, l_steer, delta_t)
            elif activated_sensor == 4:
                r_steer, l_steer, zeta = TurnRight(r_steer, l_steer, delta_t)
            elif activated_sensor == 0:
                r_steer, l_steer, zeta = TurnLeft(r_steer, l_steer, delta_t)
            elif activated_sensor == 1:
                r_steer, l_steer, zeta = TurnLeft(r_steer, l_steer, delta_t)
            elif activated_sensor == 2:
                r_steer, l_steer, zeta = TurnLeft(r_steer, l_steer, delta_t)
            else:
                l_steer = 0.24
                r_steer = 0.24
                zeta = 0
        else:
            l_steer = 0.24
            r_steer = 0.24
            zeta = 0

        ####-------------------- Record weight------------------------####
        weight_during_run_landmark[:, iter] = w_plastic_landmark
        weight_during_run_wall[:, iter] = w_plastic_wall
        collision_index_during_run.append(collision_index)

        ####-------------------- Start recording spike (CD) ------------------------####
        # cleaning noises for the collision distance sensor value
        sensor_val = np.array([
            np.repeat(inv_filter(sensor_val1), N_CD),
            np.repeat(inv_filter(sensor_val2), N_CD),
            np.repeat(inv_filter(sensor_val3), N_CD),
            np.repeat(inv_filter(sensor_val4), N_CD),
            np.repeat(inv_filter(sensor_val5), N_CD),
            np.repeat(inv_filter(sensor_val6), N_CD),
            np.repeat(inv_filter(sensor_val7), N_CD),
            np.repeat(inv_filter(sensor_val8), N_CD)
        ])
        # get the vision sensor value
        sensor_val_vision = generate_vision_val(pixelimage, N_VD)

        # reset weight to 0 again
        Poisson_synapse.w_poi = 0
        PoissonVision_synapse.w_vision_poi = 0

        err_code = vrep.simxSetJointTargetVelocity(clientID, l_motor_handle,
                                                   l_steer,
                                                   vrep.simx_opmode_streaming)
        err_code = vrep.simxSetJointTargetVelocity(clientID, r_motor_handle,
                                                   r_steer,
                                                   vrep.simx_opmode_streaming)

        # All stimuli and WTA
        All_stimuli = (np.sum(sensor_val * stimuli, axis=0))
        winner = WTA(All_stimuli)
        for w in winner:
            Poisson_synapse.w_poi[w] = All_stimuli[w]

        All_stimuli_vision = np.sum(sensor_val_vision * stimuli_vision, axis=0)
        winner_vision = WTA(All_stimuli_vision)
        for w in winner_vision:
            PoissonVision_synapse.w_vision_poi[w] = All_stimuli_vision[w] / 10

        # --------mismatching detecting---------##
        PI_Neurons_est_synapse.w_piest = landmark_PI_plastic.w_fusi_landmark

        ####-------------------- End recording spike ----------------------------####

        ####-------------------- Start recording Head direction ------------------------####

        # Choose neuron that is the nearest to the turning speed/direction
        # if turn left
        if r_steer == 0:
            neuron_index = nearest_neuron_speed(zeta, N_speed)
            for syn in range(N_speed):
                Left_drive_synapse[syn].w_left_drive = 0
                Right_drive_synapse[syn].w_right_drive = 0
            Left_drive_synapse[neuron_index].w_left_drive = 5
            Right_drive_synapse[neuron_index].w_right_drive = 0
            Go_straight.w_str = -3
            directional_PI_synapse.w_dir_PI = 0
            Stay_stay_layer.w_stay_stay = 2
        # if turn right
        elif l_steer == 0:
            neuron_index = nearest_neuron_speed(zeta, N_speed)
            for syn in range(N_speed):
                Left_drive_synapse[syn].w_left_drive = 0
                Right_drive_synapse[syn].w_right_drive = 0
            Left_drive_synapse[neuron_index].w_left_drive = 0
            Right_drive_synapse[neuron_index].w_right_drive = 5
            Go_straight.w_str = -3
            directional_PI_synapse.w_dir_PI = 0  # if turn position PI stay
            Stay_stay_layer.w_stay_stay = 2
        # if go straight
        else:
            for syn in range(N_speed):
                Left_drive_synapse[syn].w_left_drive = 0
                Right_drive_synapse[syn].w_right_drive = 0
            Go_straight.w_str = 10
            directional_PI_synapse.w_dir_PI = 4  # if move position PI run
            Stay_stay_layer.w_stay_stay = -4

        ##-----------------reset IHD (Compass) -----------------##
        # Get actual heading direction
        err_code, Angles = vrep.simxGetObjectOrientation(
            clientID, robot, -1, vrep.simx_opmode_streaming)
        heading_dir = getHeadingdirection(Angles)
        compass_angle = np.append(compass_angle, heading_dir)

        # recalibrate head direction to nearest neuron
        recal = nearest_neuron_head(heading_dir, N_HD)

        # set reset by compass weight upon angle atm
        Reset_synapse.w_reset = np.array(gaussian_spike(N_HD, recal, 30, 0.03))
        # print("reset synapse",Reset_synapse.w_reset)

        ##-----------------head direction error correction -----------------##
        if len(spikemon_Mismatch) > last_mismatch:
            print('encounter mismatch')
            err_code, detectionStateMismatch, detectedPointMismatch, detectedObjectHandleMismatch, detectedSurfaceNormalVectorMismatch = vrep.simxReadProximitySensor(
                clientID, sensorMismatch, vrep.simx_opmode_streaming)
            mismatch_sensordistance = np.linalg.norm(detectedPointMismatch)

            while mismatch_sensordistance <= 0.01:
                l_steer = 0
                r_steer = 0.5
                err_code = vrep.simxSetJointTargetVelocity(
                    clientID, l_motor_handle, l_steer,
                    vrep.simx_opmode_streaming)
                err_code = vrep.simxSetJointTargetVelocity(
                    clientID, r_motor_handle, r_steer,
                    vrep.simx_opmode_streaming)
                err_code, detectionStateMismatch, detectedPointMismatch, detectedObjectHandleMismatch, detectedSurfaceNormalVectorMismatch = vrep.simxReadProximitySensor(
                    clientID, sensorMismatch, vrep.simx_opmode_streaming)
                mismatch_sensordistance = np.linalg.norm(detectedPointMismatch)
            current_error_distance = mismatch_sensordistance
            print("mismatch distance", mismatch_sensordistance)
            l_steer = 0.24
            r_steer = 0.24
            err_code = vrep.simxSetJointTargetVelocity(
                clientID, l_motor_handle, l_steer, vrep.simx_opmode_streaming)
            err_code = vrep.simxSetJointTargetVelocity(
                clientID, r_motor_handle, r_steer, vrep.simx_opmode_streaming)
        last_mismatch = len(spikemon_Mismatch)

        ##----------------- Read position -----------------##
        err_code, Position = vrep.simxGetObjectPosition(
            clientID, robot, -1, vrep.simx_opmode_streaming)
        x_pos = Position[0]
        y_pos = Position[1]

        # recalibrate head direction to nearest neuron
        x_axis = np.append(x_axis, x_pos)
        y_axis = np.append(y_axis, y_pos)

        # recalibrate
        recal_x_axis = nearest_neuron_x_axis(x_pos, N_x_axis, x_scale)
        recal_y_axis = nearest_neuron_y_axis(y_pos, N_y_axis, y_scale)
        recal_index = N_x_axis * recal_y_axis + recal_x_axis

        r = np.append(
            r, recal_index
        )  # is an array keeping all index that neuron fire during the run

        # set reset weight
        ##----------------- Recording Position Index when collision -----------------##
        if l_steer == 0 or r_steer == 0:
            collision_index = np.append(collision_index, recal_index)

        ##----------------- Collect time-----------------##
        all_time = np.append(all_time, time.time() - t_int)

        # run
        PINet.run(15 * ms)

        print("current plas landmark synapese index",
              np.where(w_plastic_landmark >= 0.5))
        print('wall synpases values index bigger',
              np.where(w_plastic_wall >= 0.5))

        print(
            '####-------------------- Read sensor (new round) ----------------------------####'
        )

        # slow down the controller for more stability
        # time.sleep(4.2)
        # Start new measurement in the next time step
        detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4, detectedPoint5, detectedPoint6, detectedPoint7, detectedPoint8 = getDetectedpoint(
            sensor1, sensor2, sensor3, sensor4, sensor5, sensor6, sensor7,
            sensor8, clientID)
        err_code, detectionStateMismatch, detectedPointMismatch, detectedObjectHandleMismatch, detectedSurfaceNormalVectorMismatch = vrep.simxReadProximitySensor(
            clientID, sensorMismatch, vrep.simx_opmode_streaming)
        err_code, resolution, image = vrep.simxGetVisionSensorImage(
            clientID, camera, 1, vrep.simx_opmode_streaming)

        # record time step and final time
        t2 = time.time() - t_int
        delta_t = t2 - t1
        final_time = time.time()
        time_step_list = np.append(time_step_list, delta_t)
        print("delta-t", delta_t, iter)
        print("final time t2", int(t2))
        iter += 1

    ##---------------end of simulation ---------------##

    print("Done")

    # saving data
    # setting pathway
    pathway = "/Users/jieyab/SNN/for_plotting/"

    exp_number = str(sim_number)
    np.save(pathway + "r" + exp_number, r)
    np.save(pathway + "spikemon_CD_i" + exp_number, spikemon_CD.i)
    np.save(pathway + "spikemon_CD_t" + exp_number, spikemon_CD.t / ms)
    np.save(pathway + "spikemon_HD_i" + exp_number, spikemon_HD.i)
    np.save(pathway + "spikemon_HD_t" + exp_number, spikemon_HD.t / ms)
    np.save(pathway + "spikemon_PI_i" + exp_number, spikemon_PI.i)
    np.save(pathway + "spikemon_PI_t" + exp_number, spikemon_PI.t / ms)

    np.save(pathway + "spikemon_noncollision_i" + exp_number,
            spikemon_noncollison.i)
    np.save(pathway + "spikemon_noncollision_t" + exp_number,
            spikemon_noncollison.t / ms)

    np.save(pathway + "spikemon_wall_i" + exp_number, spikemon_wall.i)
    np.save(pathway + "spikemon_wall_t" + exp_number, spikemon_wall.t / ms)
    np.save(pathway + "spikemon_nonlandmark_i" + exp_number,
            spikemon_nonlandmark.i)
    np.save(pathway + "spikemon_nonlandmark_t" + exp_number,
            spikemon_nonlandmark.t / ms)
    np.save(pathway + "spikemon_landmark_i" + exp_number, spikemon_landmark.i)
    np.save(pathway + "spikemon_landmark_t" + exp_number,
            spikemon_landmark.t / ms)
    np.save(pathway + "weight_during_run_landmark" + exp_number,
            weight_during_run_landmark)
    np.save(pathway + "weight_during_run_wall" + exp_number,
            weight_during_run_wall)
    np.save(pathway + "weight_landmark" + exp_number, w_plastic_landmark)
    np.save(pathway + "weight__wall" + exp_number, w_plastic_wall)
    np.save(pathway + "spikemon_mismatch_t" + exp_number,
            spikemon_Mismatch.t / ms)
    np.save(pathway + "spikemon_mismatch_v" + exp_number, spikemon_Mismatch.v)
    np.save(pathway + "collision_index" + exp_number, collision_index)
    np.save(pathway + "collision_index_during_run" + exp_number,
            collision_index_during_run)

    np.save(pathway + "compass_angle" + exp_number, compass_angle)
    np.save(pathway + "all_time" + exp_number, all_time)

    np.save(pathway + "spikemon_estimatedlandmark_i" + exp_number,
            spikemon_estimatedlandmark.i)
    np.save(pathway + "spikemon_estimatedlandmark_t" + exp_number,
            spikemon_estimatedlandmark.t / ms)
    np.save(pathway + "spikemon_nonestimatedlandmark_i" + exp_number,
            spikemon_nonestimatedlandmark.i)
    np.save(pathway + "spikemon_nonestimatedlandmark_t" + exp_number,
            spikemon_nonestimatedlandmark.t / ms)
    np.save(pathway + "spikemon_red_i" + exp_number, spikemon_red.i)
    np.save(pathway + "spikemon_red_t" + exp_number, spikemon_red.t / ms)
    np.save(pathway + "spikemon_notred_i" + exp_number, spikemon_notred.i)
    np.save(pathway + "spikemon_notred_t" + exp_number, spikemon_notred.t / ms)
    np.save(pathway + "step_time" + exp_number, time_step_list)

    return clientID
예제 #36
0
def JointControl(clientID, motionProxy, postureProxy, i, Body):
    # Head
    # velocity_x = vrep.simxGetObjectFloatParameter(clientID,vrep.simxGetObjectHandle(clientID,'NAO#',vrep.simx_opmode_oneshot_wait)[1],11,vrep.simx_opmode_streaming)
    pos = vrep.simxGetObjectPosition(
        clientID,
        vrep.simxGetObjectHandle(clientID, 'NAO#',
                                 vrep.simx_opmode_oneshot_wait)[1], -1,
        vrep.simx_opmode_streaming)[1]
    angularPos = vrep.simxGetObjectOrientation(
        clientID,
        vrep.simxGetObjectHandle(clientID, 'NAO#',
                                 vrep.simx_opmode_oneshot_wait)[1], -1,
        vrep.simx_opmode_streaming)[1]
    alphaPosRegister = []
    betaPosRegister = []
    gamaPosRegister = []
    j = 0
    li = []
    while vrep.simxGetConnectionId(clientID) != -1:
        for x in range(0, 150):
            commandAngles = motionProxy.getAngles('Body', False)
            j += 1
            pos = vrep.simxGetObjectPosition(
                clientID,
                vrep.simxGetObjectHandle(clientID, 'NAO#',
                                         vrep.simx_opmode_oneshot_wait)[1], -1,
                vrep.simx_opmode_buffer)[1]
            angularPos = vrep.simxGetObjectOrientation(
                clientID,
                vrep.simxGetObjectHandle(clientID, 'NAO#',
                                         vrep.simx_opmode_oneshot_wait)[1], -1,
                vrep.simx_opmode_buffer)[1]
            alphaPosRegister.append(angularPos[0])
            betaPosRegister.append(angularPos[1])
            gamaPosRegister.append(angularPos[2])

            vrep.simxSetJointTargetPosition(clientID, Body[0][i],
                                            commandAngles[0],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[1][i],
                                            commandAngles[1],
                                            vrep.simx_opmode_streaming)
            # Left Leg
            vrep.simxSetJointTargetPosition(clientID, Body[2][i],
                                            commandAngles[8],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[3][i],
                                            commandAngles[9],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[4][i],
                                            commandAngles[10],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[5][i],
                                            commandAngles[11],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[6][i],
                                            commandAngles[12],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[7][i],
                                            commandAngles[13],
                                            vrep.simx_opmode_streaming)
            # Right Leg
            vrep.simxSetJointTargetPosition(clientID, Body[8][i],
                                            commandAngles[14],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[9][i],
                                            commandAngles[15],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[10][i],
                                            commandAngles[16],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[11][i],
                                            commandAngles[17],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[12][i],
                                            commandAngles[18],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[13][i],
                                            commandAngles[19],
                                            vrep.simx_opmode_streaming)
            # Left Arm
            vrep.simxSetJointTargetPosition(clientID, Body[14][i],
                                            commandAngles[2],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[15][i],
                                            commandAngles[3],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[16][i],
                                            commandAngles[4],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[17][i],
                                            commandAngles[5],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[18][i],
                                            commandAngles[6],
                                            vrep.simx_opmode_streaming)
            # Right Arm
            vrep.simxSetJointTargetPosition(clientID, Body[19][i],
                                            commandAngles[20],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[20][i],
                                            commandAngles[21],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[21][i],
                                            commandAngles[22],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[22][i],
                                            commandAngles[23],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[23][i],
                                            commandAngles[24],
                                            vrep.simx_opmode_streaming)
            # Left Fingers
            vrep.simxSetJointTargetPosition(clientID, Body[25][i][0],
                                            1.0 - commandAngles[7],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[25][i][1],
                                            1.0 - commandAngles[7],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[25][i][2],
                                            1.0 - commandAngles[7],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[25][i][3],
                                            1.0 - commandAngles[7],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[25][i][4],
                                            1.0 - commandAngles[7],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[25][i][5],
                                            1.0 - commandAngles[7],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[25][i][6],
                                            1.0 - commandAngles[7],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[25][i][7],
                                            1.0 - commandAngles[7],
                                            vrep.simx_opmode_streaming)
            # Right Fingers
            vrep.simxSetJointTargetPosition(clientID, Body[27][i][0],
                                            1.0 - commandAngles[25],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[27][i][1],
                                            1.0 - commandAngles[25],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[27][i][2],
                                            1.0 - commandAngles[25],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[27][i][3],
                                            1.0 - commandAngles[25],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[27][i][4],
                                            1.0 - commandAngles[25],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[27][i][5],
                                            1.0 - commandAngles[25],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[27][i][6],
                                            1.0 - commandAngles[25],
                                            vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID, Body[27][i][7],
                                            1.0 - commandAngles[25],
                                            vrep.simx_opmode_streaming)
        motionProxy.stopMove()
        postureProxy.stopMove()
        # postureProxy.goToPosture("Stand",0.5)
        vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
        break
    print 'End of simulation'
    print 'Final x pos'
    print pos
    calculos = {}
    calcular(alphaPosRegister, calculos)
    desvpadA = calculos['desvio_padrao']
    calculos = {}
    calcular(betaPosRegister, calculos)
    desvpadB = calculos['desvio_padrao']
    calculos = {}
    calcular(gamaPosRegister, calculos)
    desvpadG = calculos['desvio_padrao']
    return [(10 * pos[0] - (desvpadA + desvpadB + desvpadG) / 3), pos[0]]
예제 #37
0
def main():
    # global variables
    global velocity
    global armJoints
    global wheels
    global MAX_FORCE
    global tcp_handle
    global bodyHandle
    global proxSensor
    global holdingCube
    # get transformation data
    M = transformation_data.M
    S = transformation_data.S
    T_b_s = transformation_data.T_b_s
    zero_pose = transformation_data.zero_pose
    plate_pose = transformation_data.plate_pose
    front_pose = transformation_data.front_pose

    # VREP stuff
    print('Program started')
    vrep.simxFinish(-1)  # just in case, close all opened connections
    clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000,
                              5)  # Connect to V-REP
    if clientID != -1:
        print('Connected to remote API server')

        # gets handle for TCP - used in printing comparison
        e, tcp_handle = vrep.simxGetObjectHandle(clientID,
                                                 'youBotGripperJoint1',
                                                 vrep.simx_opmode_oneshot_wait)
        e, bodyHandle = vrep.simxGetObjectHandle(clientID, "youBot",
                                                 vrep.simx_opmode_blocking)
        e, cubeHandle = vrep.simxGetObjectHandle(clientID, "Rectangle16",
                                                 vrep.simx_opmode_blocking)
        e, proxSensor = vrep.simxGetObjectHandle(clientID, "Proximity_sensor",
                                                 vrep.simx_opmode_blocking)
        #e, visionSensorHandle = vrep.simxGetObjectHandle(clientID, "Vision_sensor", vrep.simx_opmode_blocking)

        # initialize wheel motors
        e1, wheels[0] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_fl',
                                                 vrep.simx_opmode_oneshot_wait)
        e2, wheels[1] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_rl',
                                                 vrep.simx_opmode_oneshot_wait)
        e3, wheels[2] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_rr',
                                                 vrep.simx_opmode_oneshot_wait)
        e4, wheels[3] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_fr',
                                                 vrep.simx_opmode_oneshot_wait)

        #initialize arm joints
        arm_poses = [0, 0, 0, 0, 0]
        for i in range(5):
            # get object handle
            e, armJoints[i] = vrep.simxGetObjectHandle(
                clientID, 'youBotArmJoint' + str(i),
                vrep.simx_opmode_oneshot_wait)
            # set max force
            vrep.simxSetJointForce(clientID, armJoints[i], MAX_FORCE,
                                   vrep.simx_opmode_oneshot_wait)
        # MAKE A CALL WITH simx_opmode_streaming TO INIT DATA AQUISITION
        vrep.simxGetObjectPosition(clientID, bodyHandle, -1,
                                   vrep.simx_opmode_streaming)
        vrep.simxGetObjectOrientation(clientID, bodyHandle, -1,
                                      vrep.simx_opmode_streaming)
        vrep.simxGetObjectPosition(clientID, bodyHandle, -1,
                                   vrep.simx_opmode_buffer)
        vrep.simxGetObjectOrientation(clientID, bodyHandle, -1,
                                      vrep.simx_opmode_buffer)
        #IMAGE PROCESSING
        res, vsHandle = vrep.simxGetObjectHandle(clientID, 'Vision_sensor',
                                                 vrep.simx_opmode_oneshot_wait)
        res, bsHandle = vrep.simxGetObjectHandle(clientID, 'Has_Block_sensor',
                                                 vrep.simx_opmode_oneshot_wait)
        res, cubeHandle = vrep.simxGetObjectHandle(
            clientID, 'Rectangle14', vrep.simx_opmode_oneshot_wait)
        err, resolution, image = vrep.simxGetVisionSensorImage(
            clientID, vsHandle, 0, vrep.simx_opmode_streaming)
        err, resolution, image = vrep.simxGetVisionSensorImage(
            clientID, bsHandle, 0, vrep.simx_opmode_streaming)

        #TESTING if we can move the arm between two poses
        '''
        DO NOT DELETE
        NEEDED TO MAKE ROBOT GET CORRECT POSITION AT START (wait until buffer is cleared)
        '''
        print("Initializing robot...\n\n")
        time.sleep(2)

        stopWheels()
        moveArm(transformation_data.plate_pose, [])
        release()
        ### INSERT YOUR CODE HERE ###

        # MEANS NO CUBE -0.046000331640244
        '''
        while True:
            grab()
            release()
        
        '''

        continue_running = True
        while continue_running:  # This will happen as long as the robot is alive

            print("Start of main loop")
            start_time = time.time()
            # STATE 1
            #Search for a cube
            dist, blob_center = getCubeProperties(clientID, vsHandle)
            while (dist < 0):
                turnRight(0)  #spins
                #print(dist)
                dist, blob_center = getCubeProperties(clientID, vsHandle)
                if (time.time() - start_time >
                        10):  #If we've been searching for 15 seconds
                    #Give up
                    continue_running = False
                    break
            stopWheels()

            if (not continue_running):  #no cubes found by front camera
                print("Giving up...")
                break

            # STATE 2
            #Navigate to cube
            # TODO: instead of drifting, we want rotation so that it is a lot more natural
            # could look at the move to position function
            print("Navigating to cube")
            while (len(blob_center) != 0 and
                   blob_center[0] > 140):  # MOVE RIGHT UNTIL BLOCK IS CENTERED
                moveRight()
                dist, blob_center = getCubeProperties(clientID, vsHandle)
            while (len(blob_center) != 0 and
                   blob_center[0] < 110):  # MOVE LEFT UNTIL BLOCK IS CENTERED
                moveLeft()
                dist, blob_center = getCubeProperties(clientID, vsHandle)
            while (len(blob_center) != 0 and blob_center[1] < 90
                   ):  # MOVE FORWARD UNTIL BLOCK IS WITHIN REACH
                moveForward()
                dist, blob_center = getCubeProperties(clientID, vsHandle)
            stopWheels()

            # STATE 3
            #Grab cube
            print("Grabbing cube")
            detectionState, yaw, cube_pose = detectCube()
            grabCube_success = grabCube(cube_pose, yaw)

            # Check if we have cube
            if (grabCube_success):
                time.sleep(0.5)
                #print('---------Start Debugging!!!!!!!!---------')
                dist, blob_center = getRearCubeProperties(clientID, bsHandle)
                if (len(blob_center) == 0):
                    grabCube_success = False

            print("Successfully grabbed cube: " + str(grabCube_success))
            #STATE 4
            #Move to destination
            if (grabCube_success):
                returnCube()
            release()
        time.sleep(5)

        # Now close the connection to V-REP:
        vrep.simxFinish(clientID)
    else:
        print('Failed connecting to remote API server')
    print('Program ended')
예제 #38
0
 def getPosition(self, objectHandle):
     return vrep.simxGetObjectPosition(self.clientID, objectHandle, -1,
                                       vrep.simx_opmode_oneshot)
 def position(self):
     return vrep.unwrap_vrep(
         vrep.simxGetObjectPosition(self._clientID, self._Robobo, -1, vrep.simx_opmode_blocking)
     )
예제 #40
0
print 'Python program started'
vrep.simxFinish(-1)  # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19998, True, True, 5000,
                          5)  #Timeout=5000ms, Threadcycle=5ms
if clientID != -1:
    print 'Connected to V-REP'
else:
    print 'Failed connecting to V-REP'
    vrep.simxFinish(clientID)

if clientID != -1:

    #ball position
    ErrorCode, ballPos = vrep.simxGetObjectHandle(
        clientID, 'Ball', vrep.simx_opmode_oneshot_wait)
    ErrorCode, ballXYZ = vrep.simxGetObjectPosition(clientID, ballPos, -1,
                                                    vrep.simx_opmode_streaming)
    #ErrorCode, ballPos = vrep.simxGetObjectPosition (clientID,ball,-1,vrep.simx_opmode_streaming)
    #ErrorCode, ballPos = vrep.simxGetObjectPosition (clientID,ball,-1,vrep.simx_opmode_buffer)

    #motors here
    ErrorCode, LeftJointHandle = vrep.simxGetObjectHandle(
        clientID, 'Blue1_leftJoint',
        vrep.simx_opmode_oneshot_wait)  #Left motor
    ErrorCode, RightJointHandle = vrep.simxGetObjectHandle(
        clientID, 'Blue1_rightJoint',
        vrep.simx_opmode_oneshot_wait)  #Right motor
    ErrorCode = vrep.simxSetJointTargetVelocity(clientID, LeftJointHandle, 0,
                                                vrep.simx_opmode_oneshot_wait)
    ErrorCode = vrep.simxSetJointTargetVelocity(clientID, RightJointHandle, 0,
                                                vrep.simx_opmode_oneshot_wait)
예제 #41
0
        print("Erro em obter o handle")

    # # Armazena o posicionamento angular dos sensores
    # sensor_angle = np.array([PI/2, 50/180.0*PI, 30/180.0*PI, 10/180.0*PI,
    #                    -10/180.0*PI, -30/180.0*PI, -50/180.0*PI, -PI/2, -PI/2,
    #                    -130/180.0*PI, -150/180.0*PI, -170/180.0*PI, 170/180.0*PI,
    #                    150/180.0*PI, 130/180.0*PI, PI/2])
    # Posicionamento angular dos sensores
    sensor_angle = np.array([
        90, 50, 30, 10, -10, -30, -50, -90, -90, -130, -150, -170, 170, 150,
        130, 90
    ])
    sensor_angle = sensor_angle * PI / 180.0

    # recupera a posição do robô
    returnCode, position = vrep.simxGetObjectPosition(
        clientID, robot_handle, -1, vrep.simx_opmode_streaming)

    returnCode, eulerAngles = vrep.simxGetObjectOrientation(
        clientID, robot_handle, -1, vrep.simx_opmode_streaming)

    startTime = time.time()

    sensor_handle_list = []

    for i in range(16):
        errCode, sensor_handle = vrep.simxGetObjectHandle(
            clientID, 'Pioneer_p3dx_ultrasonicSensor' + str(i + 1),
            vrep.simx_opmode_oneshot_wait)
        sensor_handle_list.append(sensor_handle)
        errCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor(
            clientID, sensor_handle, vrep.simx_opmode_streaming)
예제 #42
0
 def position(self):
     err_code, position = vrep.simxGetObjectPosition(
         self.simulation.clientID, self.robotnik, -1,
         vrep.simx_opmode_buffer)
     return position
예제 #43
0
			print(instruction[0])
			if(instruction[0] == 1 or instruction[0] == 5):
				print("contraer pierna")
			elif(instruction[0] == 2 or instruction[0] == 6):
				print("Estirar pierna")
			elif(instruction[0] == 3 or instruction[0] == 7):
				print("pierna hacia adelante")
			elif(instruction[0] == 4 or instruction[0] == 8):
				print("Pierna hacia atrás")
			#This is what makes the simulation Synchronous
			initialTime = vrep.simxGetLastCmdTime(clientID)
			actualTime = initialTime
			#Condition to make a new movement
			while(actualTime - initialTime < instruction[1]/10):
				#Retrive head position
				headPosition = vrep.simxGetObjectPosition(clientID, head, -1, vrep.simx_opmode_oneshot)
				headTrace.append(headPosition)
				#print(headPosition)
				#print(contador, " ", actualTime, " ", initialTime)#, " ",actualTime-initialTime," - ", instruction[1]," - ",contador,vrep.simxGetObjectPosition(clientID, head, -1, vrep.simx_opmode_oneshot))
				#Advance time in my internal counter
				actualTime+=dt
				#Make de simulation run one step (dt detemines how many seconds pass by between step and step)
				vrep.simxSynchronousTrigger(clientID)

				'''
				TODO Tengo que cambiar el sleep por controlar el tiempo en le simulación
				Entonces voy a hacer un loop infinito en el cual pregunto el tiempo, 
				luego guardo el tiempo y la diferencia entre el t inicial y el actual 
				debe ser igual o mayor que el de la la próxima instrucción. 
				De esa manera entre vuelta y vuelta del while que espera a que 
				pase el tiempo voy a poder preguntar por las posiciones de la 
예제 #44
0
errorCode,P_handle=vrep.simxGetObjectHandle(clientID,'P',vrep.simx_opmode_oneshot_wait)
errorCode,L_handle=vrep.simxGetObjectHandle(clientID,'L',vrep.simx_opmode_oneshot_wait)
errorCode,R_handle=vrep.simxGetObjectHandle(clientID,'R',vrep.simx_opmode_oneshot_wait)


vrep.simxSetJointTargetVelocity(clientID,P_handle,0,vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetVelocity(clientID,L_handle,0,vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetVelocity(clientID,R_handle,0,vrep.simx_opmode_oneshot_wait)

def speed(handle,speed):
    errorCode = vrep.simxSetJointTargetVelocity(clientID,handle,speed,vrep.simx_opmode_oneshot_wait)


vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait)
while True:
    errorCode,position_S=vrep.simxGetObjectPosition(clientID,Sphere_handle,-1,vrep.simx_opmode_oneshot)
    
    if position_S[1] <= 0.195 and position_S[1] >0.075 and position_S[0]<-0.154 and position_S[0]>-0.194:
        speed(L_handle,R_KickBallVel)
    else:
        speed(L_handle,B_KickBallVel)

    if position_S[1] <= 0.195 and position_S[1] >0.075 and position_S[0]<-0.1 and position_S[0]>-0.121:
        speed(R_handle,B_KickBallVel)
    else:
        speed(R_handle,R_KickBallVel)
    
    if position_S[0]>-0.0226 and position_S[0]<-0.0176 and position_S[1]<0.0250 and position_S[1]>=0.0212:
        speed(P_handle,-1500)
    else: 
        speed(P_handle,1000)
 def get_end_pose(self):
     pose = vrep.simxGetObjectPosition(self.cid, self.end_handle, -1,
                                       vrep.simx_opmode_blocking)[1]
     return np.array(pose)
예제 #46
0
    # Start the simulation:
    vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait)

    # ???
    res, U4 = vrep.simxGetObjectHandle(clientID, 'J11R',
                                       vrep.simx_opmode_blocking)
    res, U5 = vrep.simxGetObjectHandle(clientID, 'shaft19',
                                       vrep.simx_opmode_blocking)
    res, robotHandle = vrep.simxGetObjectHandle(clientID, 'bodyDyn',
                                                vrep.simx_opmode_blocking)
    res, endHandle = vrep.simxGetObjectHandle(clientID, 'Destination',
                                              vrep.simx_opmode_blocking)

    res, robotPosition = vrep.simxGetObjectPosition(
        clientID=clientID,
        objectHandle=robotHandle,
        relativeToObjectHandle=endHandle,
        operationMode=vrep.simx_opmode_oneshot_wait)

    # camera
    res, kinect_depth_camera = vrep.simxGetObjectHandle(
        clientID, 'kinect_depth', vrep.simx_opmode_blocking)
    res, kinect_rgb_camera = vrep.simxGetObjectHandle(
        clientID, 'kinect_rgb', vrep.simx_opmode_blocking)
    res, kinect_joint = vrep.simxGetObjectHandle(clientID, 'kinect_joint',
                                                 vrep.simx_opmode_blocking)

    #Retrive the body coordinate system
    # coordinate ????
    res, GCS = vrep.simxGetObjectHandle(clientID, 'GCS',
                                        vrep.simx_opmode_blocking)
예제 #47
0
    def setUp(self):
        '''////////// CONNECT //////////'''
        vrep.simxFinish(-1)  # just in case, close all opened connections
        self.clientID = vrep.simxStart('127.0.0.1', 19998, True, True, 5000,
                                       5)  # Connect to V-REP

        if self.clientID != -1:
            pass
            #print ('Connected to remote API server')
        else:
            print('Connection Failed')
            sys.exit('Could not connect')

        self.done = False
        '''////////// HANDLE //////////'''
        _, self.goal = vrep.simxGetObjectHandle(self.clientID, 'Goal',
                                                vrep.simx_opmode_oneshot_wait)
        _, self.robot = vrep.simxGetObjectHandle(self.clientID, 'Omnirob',
                                                 vrep.simx_opmode_oneshot_wait)
        for i in range(0, 4):
            self.motor_handle.append(
                vrep.simxGetObjectHandle(
                    self.clientID,
                    'Omnirob_' + self.motor_name[i] + 'wheel_motor',
                    vrep.simx_opmode_oneshot_wait)[1])

        for i in range(0, 9 + 1):
            self.cube_handle.append(
                vrep.simxGetObjectHandle(self.clientID, 'Cuboid' + str(i),
                                         vrep.simx_opmode_oneshot_wait)[1])

        for i in range(0, 18):
            self.sensor_handle.append(
                vrep.simxGetObjectHandle(self.clientID, 'sensor' + str(i + 1),
                                         vrep.simx_opmode_oneshot_wait)[1])
        '''////////// SET POSITION & ANGLE //////////'''
        vrep.simxSetObjectOrientation(self.clientID, self.robot, -1,
                                      [0, 0, math.pi],
                                      vrep.simx_opmode_oneshot)
        vrep.simxSetObjectOrientation(
            self.clientID, self.goal, -1,
            [0, 0, random.uniform(0, 2 * math.pi)], vrep.simx_opmode_oneshot)

        self.l = []
        self.randomPos()

        z = random.randint(0, 10)
        z = -10.25 if z in [1, 5, 8, 9] else 0.25
        vrep.simxSetObjectPosition(self.clientID, self.robot, -1,
                                   [self.l[10][1], self.l[10][2], 0.28],
                                   vrep.simx_opmode_oneshot)
        vrep.simxSetObjectPosition(self.clientID, self.goal, -1,
                                   [self.l[11][1], self.l[11][2], 0.00],
                                   vrep.simx_opmode_oneshot)

        for i in range(0, 9 + 1):
            vrep.simxSetObjectPosition(self.clientID, self.cube_handle[i], -1,
                                       [self.l[i][1], self.l[i][2], z],
                                       vrep.simx_opmode_oneshot)
            vrep.simxSetObjectOrientation(
                self.clientID, self.cube_handle[i], -1,
                [0, 0, random.uniform(0, 2 * math.pi)],
                vrep.simx_opmode_oneshot)

        _, self.velocity_robot, self.angvelocity_robot = vrep.simxGetObjectVelocity(
            self.clientID, self.robot, -1)
        _, self.position_robot = vrep.simxGetObjectPosition(
            self.clientID, self.robot, -1, vrep.simx_opmode_streaming)
        _, self.position_goal = vrep.simxGetObjectPosition(
            self.clientID, self.goal, -1, vrep.simx_opmode_streaming)
        _, self.angle_robot = vrep.simxGetObjectOrientation(
            self.clientID, self.robot, -1, vrep.simx_opmode_streaming)
        '''////////// START //////////'''
        vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_continuous)
        self.pre_distance2goal = 0.0
예제 #48
0
def setGBS():
    res, BCS_position = vrep.simxGetObjectPosition(
        clientID, BCS, -1, vrep.simx_opmode_oneshot_wait)
    [x, y, z] = [BCS_position[0], BCS_position[1], BCS_position[2]]
    vrep.simxSetObjectPosition(clientID, GCS, -1, [x, y, 0],
                               vrep.simx_opmode_oneshot_wait)
예제 #49
0
            gain_divisor = 0.5 * (np.dot(d_lm.T, (m * d_lm - g))) + 1e-10

            gain = grain_numerator / gain_divisor

            if gain > 0:
                self.vector = vector_new
                m = m * np.max([1 / 3, 1 - (2 * gain - 1)**3])
            else:
                m = m * v
                v = v * 2

            i += 1

        t1 = time.time()
        print('i:', i, '    time:', t1 - t0, '    F:', self.F0(self.vector))
        # print self.vector[8] + 1.0389e-1


stew = Platform()

i = 0
while True:
    i += 0.05
    stew.move(0.2 * sin(i) + 0.2)
    pos = vrep.simxGetObjectPosition(client_id, pos_h, -1,
                                     vrep.simx_opmode_oneshot)

    stew.get_position()
    print('error:', stew.vector[8] - pos[1][2] + 1.0389e-1)
예제 #50
0
    def train(self):
        vrep.simxFinish(-1)  #clean up the previous stuff
        clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
        if clientID == -1:
            print("Could not connect to server")
            sys.exit()

        first = True
        for i_episode in range(100):

            vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)

            observation1 = robot1.observation_space
            observation2 = robot2.observation_space
            ep_r = 0
            self.steps = 0
            while True:

                action1 = self.RL1.choose_action(observation1)  # To check
                # print(action1)
                observation1_, done1 = robot1.step(action1)  # To check
                #print(observation1_)
                observation2_ = robot2.observation_space
                done2 = False

                x1, y1, z1, vx1, vy1, vz1, theta1_f, theta2_f, theta3_f = observation1_  # To check
                x2, y2, z2, vx2, vy2, vz2, theta1_b, theta2_b, theta3_b = observation2_

                error, self.r1 = vrep.simxGetObjectHandle(
                    clientID, 'body#1', vrep.simx_opmode_blocking)
                error, self.r2 = vrep.simxGetObjectHandle(
                    clientID, 'body#13', vrep.simx_opmode_blocking)
                error, self.o1 = vrep.simxGetObjectHandle(
                    clientID, 'Obstacle#1', vrep.simx_opmode_blocking)

                error, position_object = vrep.simxGetObjectPosition(
                    clientID, self.o1, -1, vrep.simx_opmode_blocking)
                x1_ = position_object[0]
                y1_ = position_object[1]
                z1_ = position_object[2]

                error, position_hexa_base1 = vrep.simxGetObjectPosition(
                    clientID, self.r1, -1, vrep.simx_opmode_blocking)
                x1 = position_hexa_base1[0]
                y1 = position_hexa_base1[1]
                z1 = position_hexa_base1[2]

                error, position_hexa_base2 = vrep.simxGetObjectPosition(
                    clientID, self.r2, -1, vrep.simx_opmode_blocking)
                x2 = position_hexa_base2[0]
                y2 = position_hexa_base2[1]
                z2 = position_hexa_base2[2]

                distance = np.sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) *
                                   (y1 - y2))
                distance_object = np.sqrt((x1 - x1_) * (x1 - x1_) +
                                          (y1 - y1_) * (y1 - y1_) +
                                          (z1 - z1_) * (z1 - z1_))
                ###########################
                if distance > 1 or distance < 0.15 or distance_object < 0.3:
                    done1 = True

                ######################################

                #########reward function############
                reward1 = self.rsrvl + (
                    vx1 + vx2) - 0.5 * (np.abs(vy1) + np.abs(vy2))
                reward = 1 * (distance < 0.15) - 10 * (
                    distance > 1 or
                    distance_object < 0.3) - 0.1 * self.steps - 0.1 * distance
                #################################
                #print("R: ", reward)
                print("distance: ", distance)
                print("distance_object", distance_object)
                # print("z1:",z1)

                self.RL1.store_transition(observation1, action1, reward,
                                          observation1_)

                if self.total_steps > 200 and self.total_steps % 5 == 0:
                    self.RL1.learn()

                ep_r += reward
                if done1:
                    #print(done1)
                    print('episode: ', i_episode, 'ep_r: ', round(ep_r, 2),
                          ' epsilon: ', round(self.RL1.epsilon, 2))
                    break

                observation1 = observation1_
                observation2 = observation2_
                self.total_steps += 1
                self.steps += 1
                done1 = False

            first = False
            vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
            time.sleep(1)
        self.RL1.plot_cost()
예제 #51
0
    raise Exception('could not get object handle for whelel')
result, wheel4 = vrep.simxGetObjectHandle(clientID,
                                          'OmniWheel_regularRotation#2',
                                          vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
    raise Exception('could not get object handle for whelel')

# ==================================================================================================== #

# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)

# ******************************** Your robot control code goes here  ******************************** #

SetJointPosition(np.array([0, 0, 0, 0, 0, 0]))
result, p1 = vrep.simxGetObjectPosition(clientID, joint_one_handle, car,
                                        vrep.simx_opmode_blocking)
result, p2 = vrep.simxGetObjectPosition(clientID, joint_two_handle, car,
                                        vrep.simx_opmode_blocking)
result, p3 = vrep.simxGetObjectPosition(clientID, joint_three_handle, car,
                                        vrep.simx_opmode_blocking)
result, p4 = vrep.simxGetObjectPosition(clientID, joint_four_handle, car,
                                        vrep.simx_opmode_blocking)
result, p5 = vrep.simxGetObjectPosition(clientID, joint_five_handle, car,
                                        vrep.simx_opmode_blocking)
result, p6 = vrep.simxGetObjectPosition(clientID, joint_six_handle, car,
                                        vrep.simx_opmode_blocking)
result, r1 = vrep.simxGetObjectOrientation(clientID, joint_one_handle, car,
                                           vrep.simx_opmode_blocking)
result, r2 = vrep.simxGetObjectOrientation(clientID, joint_two_handle, car,
                                           vrep.simx_opmode_blocking)
result, r3 = vrep.simxGetObjectOrientation(clientID, joint_three_handle, car,
예제 #52
0
    def getPosition(self):
        ret, position = vrep.simxGetObjectPosition(self.clientID, self.robotHandle, -1, vrep.simx_opmode_oneshot)

        return position
예제 #53
0
print('Program started')
vrep.simxFinish(-1)  # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000,
                          5)  # Connect to V-REP
if clientID != -1:
    print('Connected to remote API server')

    input = []
    output = []

    #ping pong ball
    #initialization of ball
    res, ball0 = vrep.simxGetObjectHandle(clientID, 'Sphere',
                                          vrep.simx_opmode_blocking)
    res, ballpos = vrep.simxGetObjectPosition(clientID, ball0, -1,
                                              vrep.simx_opmode_streaming)
    time.sleep(1)
    res, ballOrigin = vrep.simxGetObjectPosition(clientID, ball0, -1,
                                                 vrep.simx_opmode_buffer)
    time.sleep(1)

    res, xval = vrep.simxGetFloatSignal(clientID, "xtarget",
                                        vrep.simx_opmode_streaming)
    res, yval = vrep.simxGetFloatSignal(clientID, "ytarget",
                                        vrep.simx_opmode_streaming)
    res, zval = vrep.simxGetFloatSignal(clientID, "ztarget",
                                        vrep.simx_opmode_streaming)

    res, xdest = vrep.simxGetFloatSignal(clientID, "xdest",
                                         vrep.simx_opmode_streaming)
    res, ydest = vrep.simxGetFloatSignal(clientID, "ydest",
예제 #54
0
파일: ddpg1.py 프로젝트: Fan806/Nao
def getState():

    #height
    res, handle = vrep.simxGetObjectHandle(clientID, 'LHipYawPitch3',
                                           vrep.simx_opmode_blocking)
    res, pos = vrep.simxGetObjectPosition(clientID, handle, -1,
                                          vrep.simx_opmode_blocking)

    height = pos[2]
    print("initial height:", height)

    res, handlhead = vrep.simxGetObjectHandle(clientID,
                                              "HeadPitch_link_respondable",
                                              vrep.simx_opmode_blocking)

    #torso
    res, handle = vrep.simxGetObjectHandle(clientID, "imported_part_20_sub0",
                                           vrep.simx_opmode_blocking)
    #res, Angle = vrep.simxGetObjectOrientation(clientID, handle, handlhead, vrep.simx_opmode_blocking)
    res, Angle = vrep.simxGetObjectOrientation(clientID, handle, -1,
                                               vrep.simx_opmode_blocking)
    res, Torsolv, Torsoav = vrep.simxGetObjectVelocity(
        clientID, handle, vrep.simx_opmode_blocking)
    res, Torsopos = vrep.simxGetObjectPosition(clientID, handle, -1,
                                               vrep.simx_opmode_blocking)
    TorsoAngle = Angle[2] / 180 * 3.14

    #Rthigh
    res, handle = vrep.simxGetObjectHandle(clientID, "imported_part_13_sub",
                                           vrep.simx_opmode_blocking)
    #res, Angle = vrep.simxGetObjectOrientation(clientID, handle, handlhead, vrep.simx_opmode_blocking)
    res, Angle = vrep.simxGetObjectOrientation(clientID, handle, -1,
                                               vrep.simx_opmode_blocking)
    res, Rthighlv, Rthighav = vrep.simxGetObjectVelocity(
        clientID, handle, vrep.simx_opmode_blocking)
    res, Rthighpos = vrep.simxGetObjectPosition(clientID, handle, -1,
                                                vrep.simx_opmode_blocking)
    RthighAngle = Angle[2] / 180 * 3.14

    #Lthigh
    res, handle = vrep.simxGetObjectHandle(clientID, "imported_part_10_sub",
                                           vrep.simx_opmode_blocking)
    #res, Angle = vrep.simxGetObjectOrientation(clientID, handle, handlhead, vrep.simx_opmode_blocking)
    res, Angle = vrep.simxGetObjectOrientation(clientID, handle, -1,
                                               vrep.simx_opmode_blocking)
    res, Lthighlv, Lthighav = vrep.simxGetObjectVelocity(
        clientID, handle, vrep.simx_opmode_blocking)
    res, Lthighpos = vrep.simxGetObjectPosition(clientID, handle, -1,
                                                vrep.simx_opmode_blocking)
    LthighAngle = Angle[2] / 180 * 3.14

    #Rcalf
    res, handle = vrep.simxGetObjectHandle(clientID, "r_knee_pitch_link_pure2",
                                           vrep.simx_opmode_blocking)
    #res, Angle = vrep.simxGetObjectOrientation(clientID, handle, handlhead, vrep.simx_opmode_blocking)
    res, Angle = vrep.simxGetObjectOrientation(clientID, handle, -1,
                                               vrep.simx_opmode_blocking)
    res, Rcalflv, Rcalfav = vrep.simxGetObjectVelocity(
        clientID, handle, vrep.simx_opmode_blocking)
    res, Rcalfpos = vrep.simxGetObjectPosition(clientID, handle, -1,
                                               vrep.simx_opmode_blocking)
    RcalfAngle = Angle[2] / 180 * 3.14

    #Lcalf
    res, handle = vrep.simxGetObjectHandle(clientID, "l_knee_pitch_link_pure2",
                                           vrep.simx_opmode_blocking)
    #res, Angle = vrep.simxGetObjectOrientation(clientID, handle, handlhead, vrep.simx_opmode_blocking)
    res, Angle = vrep.simxGetObjectOrientation(clientID, handle, -1,
                                               vrep.simx_opmode_blocking)
    res, Lcalflv, Lcalfav = vrep.simxGetObjectVelocity(
        clientID, handle, vrep.simx_opmode_blocking)
    res, Lcalfpos = vrep.simxGetObjectPosition(clientID, handle, -1,
                                               vrep.simx_opmode_blocking)
    LcalfAngle = Angle[2] / 180 * 3.14

    state = [height]

    state.append(TorsoAngle)
    state.extend(Torsolv)
    state.extend(Torsoav)
    state.extend(Torsopos)

    state.append(RthighAngle)
    state.extend(Rthighlv)
    state.extend(Rthighav)
    state.extend(Rthighpos)

    state.append(LthighAngle)
    state.extend(Lthighlv)
    state.extend(Lthighav)
    state.extend(Lthighpos)

    state.append(RcalfAngle)
    state.extend(Rcalflv)
    state.extend(Rcalfav)
    state.extend(Rcalfpos)

    state.append(LcalfAngle)
    state.extend(Lcalflv)
    state.extend(Lcalfav)
    state.extend(Lcalfpos)

    state = np.array(state)

    return state
예제 #55
0
errorCode, left_motor_handle_2 = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor#2',
                                                          vrep.simx_opmode_blocking)
print("Car 1 leftJoint", left_motor_handle_2, errorCode)
errorCode, right_motor_handle_2 = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor#2',
                                                           vrep.simx_opmode_blocking)
print("Car 1 rightJoint", right_motor_handle_2, errorCode)

# init sensors
sensors_0_dist = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
sensors_1_dist = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
sensors_2_dist = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
for i in range(16):
    errorCode, sensors_0_dist[i] = vrep.simxGetFloatSignal(clientID, 'Sensor_0_{}'.format(i), vrep.simx_opmode_streaming)

#init gps
errorCode, car0_pos = vrep.simxGetObjectPosition(clientID, car_0, -1, vrep.simx_opmode_streaming)
errorCode, car0_dir = vrep.simxGetObjectOrientation(clientID, car_0, -1, vrep.simx_opmode_streaming)
errorCode, car1_pos = vrep.simxGetObjectPosition(clientID, car_1, -1, vrep.simx_opmode_streaming)
errorCode, car1_dir = vrep.simxGetObjectOrientation(clientID, car_1, -1, vrep.simx_opmode_streaming)
errorCode, car2_pos = vrep.simxGetObjectPosition(clientID, car_2, -1, vrep.simx_opmode_streaming)
errorCode, car2_dir = vrep.simxGetObjectOrientation(clientID, car_2, -1, vrep.simx_opmode_streaming)



# sample codes
v0 = 2
vLeft_0 = v0
vRight_0 = v0
vLeft_1 = v0
vRight_1 = v0
vLeft_2 = v0
예제 #56
0
def init():
    global robot
    global blockHandleArray, connectionTime
    print('Program started')
    vrep.simxFinish(-1)  # just in case, close all opened connections
    int_portNb = 19999  # define port_nr
    clientID = vrep.simxStart('127.0.0.1', int_portNb, True, True, 5000,
                              5)  # connect to server
    if clientID != -1:
        print('Connected to remote API server')
        res, objs = vrep.simxGetObjects(
            clientID, vrep.sim_handle_all,
            vrep.simx_opmode_oneshot_wait)  # get all objects in the scene
        if res == vrep.simx_return_ok:  # Remote function call succeeded
            print('Number of objects in the scene: ',
                  len(objs))  # print number of object in the scene
            ret_lm, leftMotorHandle = vrep.simxGetObjectHandle(
                clientID, 'Pioneer_p3dx_leftMotor',
                vrep.simx_opmode_oneshot_wait)
            ret_rm, rightMotorHandle = vrep.simxGetObjectHandle(
                clientID, 'Pioneer_p3dx_rightMotor',
                vrep.simx_opmode_oneshot_wait)
            ret_pr, pioneerRobotHandle = vrep.simxGetObjectHandle(
                clientID, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
            ret_sl, ultraSonicSensorLeft = vrep.simxGetObjectHandle(
                clientID, 'Pioneer_p3dx_ultrasonicSensor3',
                vrep.simx_opmode_oneshot_wait)
            ret_sr, ultraSonicSensorRight = vrep.simxGetObjectHandle(
                clientID, 'Pioneer_p3dx_ultrasonicSensor5',
                vrep.simx_opmode_oneshot_wait)
            blockHandleArray = []
            for i_block in range(12):
                blockName = 'ConcretBlock#' + str(i_block)
                retCode, handle = vrep.simxGetObjectHandle(
                    clientID, blockName, vrep.simx_opmode_oneshot_wait)
                assert retCode == 0, retCode
                if i_block > 6:
                    rand_loc = [
                        random.random() * 6 - 1.5,
                        random.random() * 7 - 2.5, 0.0537
                    ]  # x[-1.5,4.5] y[-2.5,-4.5]
                    vrep.simxSetObjectPosition(clientID, handle, -1, rand_loc,
                                               vrep.simx_opmode_oneshot_wait)
                retCode, position = vrep.simxGetObjectPosition(
                    clientID, handle, -1, vrep.simx_opmode_oneshot_wait)
                assert retCode == 0, retCode
                blockHandleArray.append([handle, i_block, position])
            robot = EasyDict(clientID=clientID,
                             leftMotorHandle=leftMotorHandle,
                             rightMotorHandle=rightMotorHandle,
                             pioneerRobotHandle=pioneerRobotHandle,
                             ultraSonicSensorLeft=ultraSonicSensorLeft,
                             ultraSonicSensorRight=ultraSonicSensorRight,
                             energySensor=None)
            connectionTime = vrep.simxGetLastCmdTime(robot.clientID)
            return robot
        else:
            print('Remote API function call returned with error code: ', res)
        vrep.simxFinish(clientID)  # close all opened connections
    else:
        print('Failed connecting to remote API server')
        print('Program finished')
    return {}
예제 #57
0
                                                'ResizableFloor_5_25',
                                                vrep.simx_opmode_blocking)

    jointHandles.append(joint1)
    jointHandles.append(joint2)
    jointHandles.append(joint3)

    while True:
        try:
            vrep.simxPauseCommunication(clientID, 1)
            i = i + 1
            #Get Robot State
            if i == 1:
                errorCode, eulerAngles = vrep.simxGetObjectOrientation(
                    clientID, body, -1, vrep.simx_opmode_streaming)
                errorCode, position = vrep.simxGetObjectPosition(
                    clientID, body, -1, vrep.simx_opmode_streaming)
            else:
                errorCode, eulerAngles = vrep.simxGetObjectOrientation(
                    clientID, body, -1, vrep.simx_opmode_buffer)
                errorCode, position = vrep.simxGetObjectPosition(
                    clientID, body, -1, vrep.simx_opmode_buffer)

            #print('p1:'+str(position[0])+' p2:'+str(position[1])+' p3:'+str(position[2]))
            #print('p1:'+str(eulerAngles[0])+' p2:'+str(eulerAngles[1])+' p3:'+str(eulerAngles[2]))

            #PID Control
            # position_control = PID_Position.control(0.00005*i,position[0],1)
            #
            # if(eulerAngles[1]<(3*3.1416/180)):
            #     orientation_control = PID_Orientation.control(position_control,eulerAngles[1],1)
            # elif(eulerAngles[1]>=(3*3.1416/180)):
예제 #58
0
def main():
    # Start V-REP connection
    try:
        vrep.simxFinish(-1)
        print("Connecting to simulator...")
        clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
        if clientID == -1:
            print("Failed to connect to remote API Server")
            vrep_exit(clientID)
    except KeyboardInterrupt:
        vrep_exit(clientID)
        return

    # Setup V-REP simulation
    print("Setting simulator to async mode...")
    vrep.simxSynchronous(clientID, True)
    dt = .001
    vrep.simxSetFloatingParameter(
        clientID,
        vrep.sim_floatparam_simulation_time_step,
        dt,  # specify a simulation time step
        vrep.simx_opmode_oneshot)

    # Load V-REP scene
    print("Loading scene...")
    vrep.simxLoadScene(clientID, scene_name, 0xFF, vrep.simx_opmode_blocking)

    # Get quadrotor handle
    err, quad_handle = vrep.simxGetObjectHandle(clientID, quad_name,
                                                vrep.simx_opmode_blocking)

    # Initialize quadrotor position and orientation
    vrep.simxGetObjectPosition(clientID, quad_handle, -1,
                               vrep.simx_opmode_streaming)
    vrep.simxGetObjectOrientation(clientID, quad_handle, -1,
                                  vrep.simx_opmode_streaming)

    # Start simulation
    vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)

    # Initialize rotors
    print("Initializing propellers...")
    for i in range(len(propellers)):
        vrep.simxClearFloatSignal(clientID, propellers[i],
                                  vrep.simx_opmode_oneshot)

    # Get quadrotor initial position and orientation
    err, pos_old = vrep.simxGetObjectPosition(clientID, quad_handle, -1,
                                              vrep.simx_opmode_buffer)
    err, euler_old = vrep.simxGetObjectOrientation(clientID, quad_handle, -1,
                                                   vrep.simx_opmode_buffer)

    pos_old = np.asarray(pos_old)
    euler_old = np.asarray(euler_old) * 10.

    pos_start = pos_old

    # hyper parameters

    n_input = 6
    n_forces = 4
    #n_action = 8
    hidden = 256
    batch_size = 512
    learning_rate = .01
    eps = 0.1
    gamma = 0.9

    init_f = 7.

    net = DQN(n_input + n_forces, hidden, 1)

    optim = torch.optim.Adam(net.parameters(), lr=learning_rate)
    state = [0 for i in range(n_input)]
    state = torch.from_numpy(np.asarray(state,
                                        dtype=np.float32)).view(-1, n_input)

    propeller_vels = [init_f, init_f, init_f, init_f]

    extended_state = torch.cat(
        (state, torch.from_numpy(np.asarray([propeller_vels],
                                            dtype=np.float32))), 1)
    memory = ReplayMemory(10000)

    while (vrep.simxGetConnectionId(clientID) != -1):

        # epsilon greedy
        r = random.random()
        if r > eps:
            delta_forces = generate_forces(net, extended_state, learning_rate)
        else:
            delta_forces = [float(random.randint(-1, 1)) for i in range(4)]
        # Set propeller thrusts
        print("Setting propeller thrusts...")
        propeller_vels = apply_forces(propeller_vels, delta_forces)

        # Send propeller thrusts
        print("Sending propeller thrusts...")
        [
            vrep.simxSetFloatSignal(clientID, prop, vels,
                                    vrep.simx_opmode_oneshot)
            for prop, vels in zip(propellers, propeller_vels)
        ]

        # Trigger simulator step
        print("Stepping simulator...")
        vrep.simxSynchronousTrigger(clientID)

        # Get quadrotor initial position and orientation
        err, pos_new = vrep.simxGetObjectPosition(clientID, quad_handle, -1,
                                                  vrep.simx_opmode_buffer)
        err, euler_new = vrep.simxGetObjectOrientation(clientID, quad_handle,
                                                       -1,
                                                       vrep.simx_opmode_buffer)

        pos_new = np.asarray(pos_new)
        euler_new = np.asarray(euler_new) * 10
        #euler_new[2]/=100

        valid = is_valid_state(pos_start, pos_new, euler_new)
        if valid:
            next_state = torch.FloatTensor(
                np.concatenate([euler_new, pos_new - pos_old]))
            #next_state = torch.FloatTensor(euler_new )

            next_extended_state = torch.FloatTensor(
                [np.concatenate([next_state,
                                 np.asarray(propeller_vels)])])
        else:
            next_state = None
            next_extended_state = None

        reward_acc = np.float32(-np.linalg.norm(
            pos_new - pos_old)) if next_state is not None else np.float32(-10.)

        reward_alpha = np.float32(
            -np.fabs(euler_new[0])
        ) if not check_quad_flipped(euler_new) else np.float32(-10.)
        reward_beta = np.float32(
            -np.fabs(euler_new[1])
        ) if not check_quad_flipped(euler_new) else np.float32(-10.)
        reward_gamma = np.float32(
            -np.fabs(euler_new[2])
        ) if not check_quad_flipped(euler_new) else np.float32(-5.)

        reward_alpha_acc = np.float32(
            -np.fabs(euler_new[0] - euler_old[0])
        ) if not check_quad_flipped(euler_new) else np.float32(-10.)
        reward_beta_acc = np.float32(
            -np.fabs(euler_new[1] - euler_old[1])
        ) if not check_quad_flipped(euler_new) else np.float32(-10.)
        reward_gamma_acc = np.float32(
            -np.fabs(euler_new[2] - euler_old[2])
        ) if not check_quad_flipped(euler_new) else np.float32(-5.)

        reward = reward_alpha + reward_beta + reward_gamma + reward_acc + reward_alpha_acc * 10. + reward_beta_acc * 10. + reward_gamma_acc * 10.

        if not check_quad_flipped(euler_new) and not valid:
            print('Success.')
            reward = 10.

        #reward_pos=-np.linalg.norm(pos_new-pos_start)

        if reward < -10.:
            reward = -10.
        reward = np.float32(reward)
        memory.push(
            extended_state,
            torch.from_numpy(np.asarray([delta_forces], dtype=np.float32)),
            next_extended_state, torch.from_numpy(np.asarray([[reward]])))

        state = next_state
        extended_state = next_extended_state
        pos_old = pos_new
        euler_old = euler_new
        print(propeller_vels)
        print("\n")

        # Perform one step of the optimization (on the target network)

        #DQN_update2(net, memory, batch_size, gamma, optim)

        if not valid:
            DQN_update2(net, memory, batch_size, gamma, optim)
            print('reset')
            # reset
            reset(clientID)
            print("Loading scene...")
            vrep.simxLoadScene(clientID, scene_name, 0xFF,
                               vrep.simx_opmode_blocking)

            # Setup V-REP simulation
            print("Setting simulator to async mode...")
            vrep.simxSynchronous(clientID, True)
            dt = .0005
            vrep.simxSetFloatingParameter(
                clientID,
                vrep.sim_floatparam_simulation_time_step,
                dt,  # specify a simulation time step
                vrep.simx_opmode_oneshot)
            # Get quadrotor handle
            err, quad_handle = vrep.simxGetObjectHandle(
                clientID, quad_name, vrep.simx_opmode_blocking)

            # Initialize quadrotor position and orientation
            vrep.simxGetObjectPosition(clientID, quad_handle, -1,
                                       vrep.simx_opmode_streaming)
            vrep.simxGetObjectOrientation(clientID, quad_handle, -1,
                                          vrep.simx_opmode_streaming)

            # Start simulation
            vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)

            # Get quadrotor initial position and orientation
            err, pos_old = vrep.simxGetObjectPosition(clientID, quad_handle,
                                                      -1,
                                                      vrep.simx_opmode_buffer)
            err, euler_old = vrep.simxGetObjectOrientation(
                clientID, quad_handle, -1, vrep.simx_opmode_buffer)
            euler_old = euler_old
            pos_old = np.asarray(pos_old)
            euler_old = np.asarray(euler_old) * 10.
            pos_start = pos_old

            state = [0 for i in range(n_input)]
            state = torch.FloatTensor(np.asarray(state)).view(-1, n_input)

            propeller_vels = [init_f, init_f, init_f, init_f]

            extended_state = torch.cat(
                (state, torch.FloatTensor(np.asarray([propeller_vels]))), 1)
            print('duration: ', len(memory))
            memory = ReplayMemory(10000)
예제 #59
0
 def get_position(self):
     position = vrep.simxGetObjectPosition(
         self.client_id, self.handles['youBot_positionTarget'],
         -1, BLOCKING_MODE
     )[1]
     return position
예제 #60
0
 print('Connected to remote API server')
 #获得相机句柄
 _, sensorHandle = vrep.simxGetObjectHandle(clientID, 'Vision_sensor#',
                                            vrep.simx_opmode_oneshot_wait)
 _, sphereHandle = vrep.simxGetObjectHandle(clientID, 'Plane',
                                            vrep.simx_opmode_oneshot_wait)
 #获得深度相机的近景和远景距离
 _, nearClip = vrep.simxGetObjectFloatParameter(
     clientID, sensorHandle, vrep.sim_visionfloatparam_near_clipping,
     vrep.simx_opmode_oneshot_wait)
 _, farClip = vrep.simxGetObjectFloatParameter(
     clientID, sensorHandle, vrep.sim_visionfloatparam_far_clipping,
     vrep.simx_opmode_oneshot_wait)
 print("farclip:%s  nearclip:%s" % (farClip, nearClip))
 #深度相机位置
 _, sensorPostion = vrep.simxGetObjectPosition(
     clientID, sensorHandle, -1, vrep.simx_opmode_oneshot_wait)
 #发送流传送命令
 if IMG_TYPE == IMG_RGB:
     _, resolution, image = vrep.simxGetVisionSensorImage(
         clientID, sensorHandle, 0, vrep.simx_opmode_streaming)
 else:
     _, resolution, image = vrep.simxGetVisionSensorImage(
         clientID, sensorHandle, 1, vrep.simx_opmode_streaming)
 time.sleep(2)
 while (vrep.simxGetConnectionId(clientID) != -1):
     #参数2,options为1时表示灰度图,为0时RGB图,官方教程错误
     err, resolution, img = vrep.simxGetVisionSensorImage(
         clientID, sensorHandle, 0, vrep.simx_opmode_buffer)
     if err == vrep.simx_return_ok:
         if IMG_TYPE == IMG_RGB:  #RGB图
             img = np.array(img, dtype=np.uint8)