如何计算两个对象之间的相对姿势并将其放入转换矩阵中

我有两个物体的姿势。它们分别是x，y，z和横摇，俯仰和偏航的角度（我用四元数表示它们，然后将它们转换为横摇，俯仰，偏航）。我需要做的是记录两个对象之间的相对姿势，并记录它们，因此，当一个对象旋转时，另一个对象的平移和旋转方式完全相同。我从物体的质心得到姿势。

该视频将说明我打算做什么。

https://drive.google.com/file/d/1NKtS9fv-FasloVwCKqYIAV1Uc2i9_PN0/view

我已访问以下网站寻求帮助：

http://planning.cs.uiuc.edu/node102.html

请帮助我，我困扰了很长时间，我需要一个方向，我是机器人技术的新手，并且我在计算机视觉方面没有任何背景。

@Vik，所以我已经实现了您想要的方式，但是它无法正常工作。我在python的ROS环境中工作，但是我给出了代码的逻辑部分。如果您可以看一下，那将非常有帮助。我从这里获得矩阵：http : //planning.cs.uiuc.edu/node104.html

#This is object2
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.90
p.pose.position.y = 0.30
p.pose.position.z = 1.2
p.pose.orientation.x=0.0
p.pose.orientation.y=0.0
p.pose.orientation.z=0.0
p.pose.orientation.w=1.0
scene.add_box("object2", p, (0.1, 0.1, 0.2))

rospy.sleep(2)

quaternion1 =   (p.pose.orientation.x,p.pose.orientation.y,p.pose.orientation.z,p.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion1, axes='sxyz') # will provide result in x, y,z sequence
roll=euler[0]
pitch=euler[1]
yaw=euler[2]

C00=math.cos(yaw)*math.cos(pitch)
C01=math.cos(yaw)*math.sin(pitch)*math.sin(roll) - math.sin(yaw)*math.sin(roll)
C02=math.cos(yaw)*math.sin(pitch)*math.cos(roll) + math.sin(yaw)*math.sin(roll)
C03=p.pose.position.x
C10=math.sin(yaw)*math.cos(pitch)
C11=math.sin(yaw)*math.sin(pitch)*math.sin(roll) + math.cos(yaw)*math.cos(roll)
C12=math.sin(yaw)*math.sin(pitch)*math.cos(roll) -math.cos(yaw)*math.sin(roll)
C13=p.pose.position.y
C20= -math.sin(pitch)
C21=math.cos(pitch)*math.sin(roll)
C22=math.cos(pitch)*math.cos(roll)
C23=p.pose.position.z
C30=0
C31=0
C32=0
C33=1

obj2_mat=np.array([[C00, C01, C02, C03],[C10, C11, C12, C13],[C20, C21, C22, C23],[C30, C31, C32, C33]])

#This is object1
p1 = PoseStamped()
p1.header.frame_id = robot.get_planning_frame()
p1.pose.position.x = 0.9
p1.pose.position.y = 0.30
p1.pose.position.z = 0.7
p1.pose.orientation.x=0.0
p1.pose.orientation.y=0.0
p1.pose.orientation.z=0.0
p1.pose.orientation.w=1.0


scene.add_box("object1", p1, (0.1, 0.1, 0.5))


rospy.sleep(2)

quaternion2 = (p1.pose.orientation.x,p1.pose.orientation.y,p1.pose.orientation.z,p1.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion2, axes='sxyz')
roll=euler[0]
pitch=euler[1]
yaw=euler[2]

C00=math.cos(yaw)*math.cos(pitch)
C01=math.cos(yaw)*math.sin(pitch)*math.sin(roll) - math.sin(yaw)*math.sin(roll)
C02=math.cos(yaw)*math.sin(pitch)*math.cos(roll) + math.sin(yaw)*math.sin(roll)
C03=p1.pose.position.x
C10=math.sin(yaw)*math.cos(pitch)
C11=math.sin(yaw)*math.sin(pitch)*math.sin(roll) + math.cos(yaw)*math.cos(roll)
C12=math.sin(yaw)*math.sin(pitch)*math.cos(roll) -math.cos(yaw)*math.sin(roll)
C13=p1.pose.position.y
C20= -math.sin(pitch)
C21=math.cos(pitch)*math.sin(roll)
C22=math.cos(pitch)*math.cos(roll)
C23=p1.pose.position.z
C30=0
C31=0
C32=0
C33=1

obj1_mat=np.array([[C00, C01, C02, C03],[C10, C11, C12, C13],[C20, C21, C22, C23],[C30, C31, C32, C33]])

transformation_mat=np.dot(inv(obj2_mat), obj1_mat) #generating the transformation

rospy.sleep(10)

#This is object1 in second pose
p2 = PoseStamped()
p2.header.frame_id = robot.get_planning_frame()
p2.pose.position.x = 0.70
p2.pose.position.y = -0.9
p2.pose.position.z = 0.7
p2.pose.orientation.x=0.3826834
p2.pose.orientation.y=0.0
p2.pose.orientation.z=0.0
p2.pose.orientation.w=-0.9238795
scene.add_box("object1", p2, (0.1, 0.1, 0.5))

object_position_mat=np.array([[p2.pose.position.x],[p2.pose.position.y],[p2.pose.position.z],[1]]) # (x,y,z,1) position matrix for object1 in its second position

rospy.sleep(2)

Final_position=np.dot(transformation_mat, object_position_mat) #Getting the new position of object2 by multiplying transformation matrix with position of object1


print "============ Generating plan 2"


#This is object2 in second pose
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = Final_position[0]
p.pose.position.y = Final_position[1]
p.pose.position.z = Final_position[2]
p.pose.orientation.x=p2.pose.orientation.x #Kept the same orientation values of object1 in second pose, did not do any calculation as it is logical
p.pose.orientation.y=p2.pose.orientation.y
p.pose.orientation.z=p2.pose.orientation.z
p.pose.orientation.w=p2.pose.orientation.w

scene.add_box("object2", p, (0.1, 0.1, 0.2))