graspnet/graspnet-baseline

夹爪坐标系不同怎么处理呢?

songhat opened this issue · 7 comments

我目前在用仿真环境进行graspnet的测试。我知道这是一个简单的坐标变换问题,但是就是在不知道哪里出了问题,图中左边是rviz我自己的机械臂的tf信息,图中间是机械臂现在的状态,图右边是graspnet估计的位姿。目前是把位姿转换到世界坐标下了,按道理进行一个y轴顺时针旋转机械臂的方向就对了,但是我实践的时候总是出问题,好像并不是拿世界坐标下的位姿做旋转?
0bc56120bf1f97aa8c50c536ae8f597
我的变换代码:

# 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.        ])

另外我的机械臂夹爪的原点和graspnet的原点好像也不同,造成了夹爪碰撞物体,这怎么解决呢?
image

左乘右乘都可以,不是固定的,取决于矩阵有没有转置,多个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)

原始抓取:
image
变换之后:
image

经过我的测试,想要沿着夹爪坐标系进行位移,就要先把位移左乘坐标系旋转矩阵。

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.        ])

另外我的机械臂夹爪的原点和graspnet的原点好像也不同,造成了夹爪碰撞物体,这怎么解决呢? image

请问一下,这个是什么模拟器?

@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.        ])

另外我的机械臂夹爪的原点和graspnet的原点好像也不同,造成了夹爪碰撞物体,这怎么解决呢? image

请问一下,这个是什么模拟器?

gazebo