Search code examples
constraintsroboticsdrakeinverse-kinematics

Slightly Different Angle Between Vectors Constraint


I'm working on constraining an IK program so that the normal vector in the end-effector frame is parallel to a known vector in the world-frame, when both are projected to the xy-plane. My initial thought was to use an AddAnglesBetweenVectorsConstraint, but that only allows me to specify the total angle between vectors, and no difference between the different axes. Is there currently a way in Drake to do this?

Edit: it turns out that this was not exactly the problem I needed to solve. In my answer below, I describe the real problem I was solving.


Solution

  • Turns out, you can do what I wanted to do using an AddAnglesBetweenVectorsConstraint, I just misdescribed my problem. Specifically, what I really wanted was to do was enforce that the y-axis in the end effector frame (where z-axis is the direction of the end-effector, and the x-axis is straight up in said frame) was perpdinicular both to the z-axis in the world frame and the known vector in the world-frame. This is exactly what AddAngleBetweenVectorsConstraint, and it worked very nicely.