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

Thanks

Code: Select all

```
pos = end_effector_state[0]
ori = end_effector_state[1]
#translate a bit higher
pos = list(pos)
pos[2] += 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)
```