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]]

 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

    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)

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)

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 )

    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)

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()

 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]

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

    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])

 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)

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 
           

 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 )

 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

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,:]

 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 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

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)