夹爪坐标系不同怎么处理呢?
songhat opened this issue · 7 comments
我目前在用仿真环境进行graspnet的测试。我知道这是一个简单的坐标变换问题,但是就是在不知道哪里出了问题,图中左边是rviz我自己的机械臂的tf信息,图中间是机械臂现在的状态,图右边是graspnet估计的位姿。目前是把位姿转换到世界坐标下了,按道理进行一个y轴顺时针旋转机械臂的方向就对了,但是我实践的时候总是出问题,好像并不是拿世界坐标下的位姿做旋转?
我的变换代码:
# graspnet 估计的位姿(世界坐标下)
grasp_position = [0.50,0.02,0.6]
grasp_orientation = [0.68,-0.68,-0.19,-0.19]
R = Rotation.from_quat(grasp_orientation)
T = np.eye(4,4)
T[:3,:3] = R.as_matrix()
T[:3,3] = grasp_position
# 作y轴旋转
R2 = Rotation.from_euler("zyx",[0 ,1.57, 0],degrees=False)
T2 = np.eye(4,4)
T2[:3,:3] = R2.as_matrix()
T3 = np.dot(T2,T)
T:
array([[ 0. , -1. , 0. , 0.5 ],
[-0.855, 0. , 0.518, 0.02 ],
[-0.518, 0. , -0.855, 0.6 ],
[ 0. , 0. , 0. , 1. ]]
T2:
array([[ 0.001, 0. , 1. , 0. ],
[ 0. , 1. , 0. , 0. ],
[-1. , 0. , 0.001, 0. ],
[ 0. , 0. , 0. , 1. ]])
T3:
array([[-0.518, -0.001, -0.855, 0.6 ],
[-0.855, 0. , 0.518, 0.02 ],
[-0. , 1. , -0.001, -0.5 ],
[ 0. , 0. , 0. , 1. ]])
因为我的基础不是很好,不知道是哪个知识点出了问题,希望大佬们能赐教
信息不大全,一般是grasp pose右乘一个你自己夹爪坐标系到graspnet定义夹爪坐标系的转换矩阵,也许你可以试试 T3 = np.dot(T,T2) ?
@chenxi-wang 我按照你说的改了,确实有效!但是想知道为什么是右乘,因为好多教课书上做的变换都是左乘。
import numpy as np
import open3d as o3d
from scipy.spatial.transform import Rotation
# graspnet 估计的位姿(世界坐标下)
grasp_position = [0.50,0.02,0.6]
grasp_orientation = [0.68,-0.68,-0.19,-0.19]
R = Rotation.from_quat(grasp_orientation)
T = np.eye(4,4)
T[:3,:3] = R.as_matrix()
T[:3,3] = grasp_position
# 作y轴旋转
R2 = Rotation.from_euler("yzx",[np.pi/2 , 0, 0],degrees=False)
T2 = np.eye(4,4)
T2[:3,:3] = R2.as_matrix()
T3 = np.dot(T,T2)
# 起作用了,Y轴旋转了
Rotation.from_matrix(T[:3,:3]).as_euler("yzx")
# array([2.59666635, 1.57079633, 0. ])
Rotation.from_matrix(T3[:3,:3]).as_euler("yzx")
#array([-2.11572263, 1.57079633, 0. ])
左乘右乘都可以,不是固定的,取决于矩阵有没有转置,多个api一起用容易搞混,调试的时候可以先把translation调对,再调rotation
位置有偏差是因为两个夹爪坐标系的转换少了一个平移量,可以参考这里的定义改一下
@chenxi-wang 我尝试着修改平移量,按照定义,我只要相对夹爪坐标系x轴平移一点距离就可以了。但是实际变换的时候,它总是相对相机x轴平移
代码:
gg = gg[:3]
T = np.eye(4)
T[:3, 3] = np.array((-0.1, -0.1, -0.1))
T[:3, :3] = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
gg.transform(T)
经过我的测试,想要沿着夹爪坐标系进行位移,就要先把位移左乘坐标系旋转矩阵。
T = np.eye(4)
T[:3, 3] = np.array((-0.08, 0, 0)) # 沿着x轴移动
T[:3, 3] = np.dot(gg[0].rotation_matrix,T[:3, 3]) # 旋转之后的位移才是相对夹爪坐标系的位移
g.transform(T)
@chenxi-wang 我按照你说的改了,确实有效!但是想知道为什么是右乘,因为好多教课书上做的变换都是左乘。
import numpy as np import open3d as o3d from scipy.spatial.transform import Rotation # graspnet 估计的位姿(世界坐标下) grasp_position = [0.50,0.02,0.6] grasp_orientation = [0.68,-0.68,-0.19,-0.19] R = Rotation.from_quat(grasp_orientation) T = np.eye(4,4) T[:3,:3] = R.as_matrix() T[:3,3] = grasp_position # 作y轴旋转 R2 = Rotation.from_euler("yzx",[np.pi/2 , 0, 0],degrees=False) T2 = np.eye(4,4) T2[:3,:3] = R2.as_matrix() T3 = np.dot(T,T2) # 起作用了,Y轴旋转了 Rotation.from_matrix(T[:3,:3]).as_euler("yzx") # array([2.59666635, 1.57079633, 0. ]) Rotation.from_matrix(T3[:3,:3]).as_euler("yzx") #array([-2.11572263, 1.57079633, 0. ])
请问一下,这个是什么模拟器?
@chenxi-wang 我按照你说的改了,确实有效!但是想知道为什么是右乘,因为好多教课书上做的变换都是左乘。
import numpy as np import open3d as o3d from scipy.spatial.transform import Rotation # graspnet 估计的位姿(世界坐标下) grasp_position = [0.50,0.02,0.6] grasp_orientation = [0.68,-0.68,-0.19,-0.19] R = Rotation.from_quat(grasp_orientation) T = np.eye(4,4) T[:3,:3] = R.as_matrix() T[:3,3] = grasp_position # 作y轴旋转 R2 = Rotation.from_euler("yzx",[np.pi/2 , 0, 0],degrees=False) T2 = np.eye(4,4) T2[:3,:3] = R2.as_matrix() T3 = np.dot(T,T2) # 起作用了,Y轴旋转了 Rotation.from_matrix(T[:3,:3]).as_euler("yzx") # array([2.59666635, 1.57079633, 0. ]) Rotation.from_matrix(T3[:3,:3]).as_euler("yzx") #array([-2.11572263, 1.57079633, 0. ])请问一下,这个是什么模拟器?
gazebo