I've got an end effector and I'm setting up the camera on the end effector (length is 0.618)
So far I've taken some code from a grasping project, and added 0.3 to the Z height, which is a bit better, since the camera is sitting on the link.
But I would prefer if the camera were translated from its position, along the orientation, by 0.618
Anyone with some basic matrix skills able to help?
I presume it's like the
Code: Select all
pos = end_effector_state ori = end_effector_state #translate a bit higher pos = list(pos) pos += 0.3 pos = tuple(pos) rot_matrix = self._pybullet_client.getMatrixFromQuaternion(ori) rot_matrix = np.array(rot_matrix).reshape(3, 3) # Initial vectors #init_camera_vector = (0, 0, 1) # x-axis init_camera_vector = (0, 0, -1) # x-axis - oops it was looking backwards init_up_vector = (0, 1, 0) # y-axis # Rotated vectors camera_vector = rot_matrix.dot(init_camera_vector) up_vector = rot_matrix.dot(init_up_vector) #end_effector_length_vector = (0.618, 0, 0) #camera_vector = rot_matrix.dot(end_effector_length_vector) self.view_matrix_gripper = self._pybullet_client.computeViewMatrix(pos, pos + 0.1 * camera_vector, up_vector) #why 0.1? img = self._pybullet_client.getCameraImage(256, 256, self.view_matrix_gripper, self.projectionMatrix, shadow=0, flags = self._pybullet_client.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX, renderer=self._pybullet_client.ER_BULLET_HARDWARE_OPENGL)