如何在Python中同时迭代两个数组?

时间:2019-07-03 22:21:53

标签: python arrays pandas iteration

我已经在python中编写了以下函数,该函数在给定开始和结束方向时,使用SLERP方法对方向进行插值:

 def slerp(self, start_O, target_O, t_array):
                ....(other code)
                return (s0[:,np.newaxis] * start_O[np.newaxis,:]) + (s1[:,np.newaxis] * target_O[np.newaxis,:])

在trajectoryMove函数中调用SLERP函数时,该函数返回500个数组的列表,每个数组包含4个元素。我试图同时遍历由SLERP函数生成的数组列表和由cubicSplineInterpolate函数生成的x,y和z值列表。因此,例如,对于第一个姿势,由cubicSplineInterpolate函数确定第一个姿势的位置的x,y和z元素,并由第一个姿势的取向确定x,y,z和w元素样条函数。然后,trajectoryMove函数应同时循环到slerp和cubicSplineInterpolate函数的第二个数组。如何在python中执行此操作?我是否需要在新数组中存储调用slerp函数的结果?

def trajectoryMove(self):
        newPose = Pose()
        arr1 = []
        arr2 = []
        arr3 = []
        x_axis = [0.001, 0.0039, 0.0160, 0.0334]
        y_axis = [0.009, 0.0239, 0.0121, 0.0034]
        z_axis = [0.009, 0.0199, 0.0821, 0.1034]
        start_orientation = [0.707106781172, 0.707106781191, 2.59734823723e-06, 0]
        end_orientation = [0.151231412, 0.5112315134, 0.0051534141, 0.5]
        self.cubicSplineInterpolate(x_axis,y_axis,z_axis)
        self.slerp(start_orientation, end_orientation, np.arange(0,1,0.001))
        arr1 = self.xLinespace
        arr2 = self.yLinespace
        arr3 = self.zLinespace

        for x, y, z in zip(arr1, arr2, arr3):

            newPose.position.x = x
            newPose.position.y = y
            newPose.position.z = z
            newPose.orientation.x = 0.707106781172
            newPose.orientation.y = 0.707106781191
            newPose.orientation.z = 2.59734823723e-06
            newPose.orientation.w = 0
            self.set_position_cartesian.publish(newPose)
            rospy.loginfo(newPose)
            rospy.sleep(0.05)

谢谢。

0 个答案:

没有答案