Python vrep.simxGetObjectPosition函数代码示例

    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 )