我已经在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)
谢谢。