translation along link

Official Python bindings with a focus on reinforcement learning and robotics.
Post Reply
djb
Posts: 3
Joined: Sun Jun 14, 2020 9:54 pm

translation along link

Post by djb »

Hi

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)

djb
Posts: 3
Joined: Sun Jun 14, 2020 9:54 pm

Re: translation along link

Post by djb »

Well I ended up doing this

Code: Select all

    pos = [pos[0] + 0.5*camera_vector[0], pos[1] + 0.5*camera_vector[1], pos[2] + 0.5*camera_vector[2]]
    pos = tuple(pos) 
If anyone has a better way let me know
Post Reply