Make BallJoint point to target

I have a box (projectile) floating in the world with a arbitrary transformation in between. Attached to this box is a cylinder (rope) which is mounted using a ball joint with no parent/child transformation. I now want to let the cylinder point from the projectile to a specific body p given by its three coordinates.

I tried the following snippet:

axis = self.projectile.getCOM(p)
matrix = util.matrix_from_vectors(np.asarray([1, 0, 0]), axis)
angles = matrixToEulerXYZ(matrix)
joint.setPositions(angles)

But this gives me some weird values which do not seem to be correct. util.matrix_from_vectors calculates the rotation matrix to align the first vector to the second. The code is as follows (taken somewehere from stackoverflow):

def matrix_from_vectors(vec1, vec2):
    """ Find the rotation matrix that aligns vec1 to vec2
    :param vec1: A 3d "source" vector
    :param vec2: A 3d "destination" vector
    :return mat: A transform matrix (3x3) which when applied to vec1, aligns it with vec2.
    """
    a, b = (vec1 / np.linalg.norm(vec1)).reshape(3), (vec2 / np.linalg.norm(vec2)).reshape(3)
    v = np.cross(a, b)
    c = np.dot(a, b)
    s = np.linalg.norm(v)

    if c == -1:
        return -np.eye(3)
    elif c == 1:
        return np.eye(3)

    kmat = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
    rotation_matrix = np.eye(3) + kmat + kmat.dot(kmat) * ((1 - c) / (s ** 2))
    return rotation_matrix

I have the feeling I am fundamentally wrong with something here, but I just cannot get the hang of it. Help is appreciated :slight_smile:

After some experimenting with custom values set to the BallJoint rotations, I think my problem is that I do not understand the meaning of the position values.

I believe the BallJoint uses a log map representation for its positions, not Euler Angles. That can make it tough to directly compute the position values that will get you what you want (I wouldn’t know how to do so myself).

If you instead use the Jacobian of the joint to iterate the positions until they give you the alignment that you want, that should work. The InverseKinematics class could help you with this.

Thanks for the hint with inverse kinematics, that seems suitable. I will have a look into this.

To estimate the complexity of calculations I would like to have a look into log map representations. I could not find the term log map rotation, but exponential rotation (e.g. here at the bottom). I guess, this is equivalent?

Yes, the log map is the inverse of exponential map.

You could use dartpy.dynamics.BallJoint.convertToPositions(rotation_matrix) and dartpy.dynamics.BallJoint.convertToRotation(logarithm_coordinates) for the conversions.

Thanks to your comments, I got it working. The convertToPositions() function was quite straight forward to use. This is the final snippet:

axis = self.p.getCOM(projectile)
matrix = util.matrix_from_vectors(np.asarray([-1, 0, 0]), axis)
joint.setPositions(joint.convertToPositions(matrix))

This makes the joint point from projectile to p.