关于pybullet中invertTransform和multiplyTransforms用法

最近在学pybullet，遇到了一个比较尴尬的问题，就是根据大地坐标位置参数对物体施加力的时候，在转动90度的时候会出现错误，然后就开始找这两个用法，找了半天没啥收获，总结下最近遇道的问题

invertTransform相当于求解逆矩阵，与旋转矩阵求逆是没有本质区别的，但是需要注意一个问题

inv_hand_pos, inv_hand_orn = p.invertTransform(robot_observation[:3],
                                               p.getQuaternionFromEuler(robot_observation[3:6]))

这个invertTransform里面的参数，是相对于基座的位置参数，此时若只有旋转，矩阵中将只有

p.invertTransform((0, 0, 0), p.getBasePositionAndOrientation(robot, 0)[1])

（可以这么理解），当相对于基座存在位移并且有姿态变化时，可以设置为下面这种，invertTransform第二个参数是时刻在改变的，相当于旋转角度theta代入矩阵的数值

inv_hand_pos, inv_hand_orn = p.invertTransform((a, b, c), p.getBasePositionAndOrientation(robot, 0)[1])

然后再用multiplyTransforms计算矩阵的乘法，换句话说是将大地坐标系换算成机器人基座的坐标系下的位置参数，换算方式是后者换算成前者（但通过两次仿真发现，参数后者和前者并不会存在输出值的改变），negative_world_pos是原本的坐标系数值添加负号。

negative_world_pos = [-1 * float(x) for x in p.getBasePositionAndOrientation(robot, 0)[0]]

obj_pos_in_hand, obj_orn_in_hand = p.multiplyTransforms(inv_hand_pos, inv_hand_orn,negative_world_pos,p.getQuaternionFromEuler((0, 0, 0))

也可以反过来用，将大地坐标换算成基座下的坐标，这个在工程上一般用的会多些，利用相机定位获得世界坐标系的参数 在定位到机器人位置参数