Trying to make wrench transformations in SE3