通过choregraph利用python获取naoqi机器人某些部位的位置位姿（如摄像头）

版权声明：未经原作者允许不得转载本文内容，否则将视为侵权 https://blog.csdn.net/springhammer/article/details/88539704

获取机器人某些部位的位置位姿（如摄像头）

①数据记录，记录30行，每0.1秒记录一个位姿数据。

def recordData(memory_service):

    """ Record the data from ALMemory.

    Returns a matrix of values

    """

    print "Recording data ..."

    data = list()

    for range_counter in range (1, 31):#记录30行

        line = list()

        for key in ALMEMORY_KEY_NAMES:

            value = memory_service.getData(key)

            line.append(value)

        data.append(line)

        time.sleep(0.1)#每0.1秒记录一次

return data

 

②做转头动作，在2秒钟内转1弧度。

def main(session):

    """ Parse command line arguments, run recordData

    and write the results into a csv file.

    """

    # Get the services ALMemory and ALMotion.

    memory_service = session.service("ALMemory")

    motion_service = session.service("ALMotion")

    # Set stiffness on for Head motors

    motion_service.setStiffnesses("Head", 1.0)

    # Will go to 1.0 then 0 radian  in two seconds

    motion_service.angleInterpolation(

        ["HeadYaw"],

        [1.0,0],#转头1弧度

        [1  , 2],#2秒钟内转完

        False,

        _async=True

    )

    data = recordData(memory_service)

    # Gently set stiff off for Head motors

    motion_service.setStiffnesses("Head", 1.0)

    output = os.path.abspath("record.csv")

    with open(output, "w") as fp:

        for line in data:

            fp.write("; ".join(str(x) for x in line))

            fp.write("\n")

print "Results written to", output

 

③运行后将记录的数据写入record.csv中

④record.csv中头部位姿的X,Y值