Compute the IK error in the target reference frame rather than its parent

[parkerowan]

This allows for relaxing different axis constraints in the target reference frame using existing error weights, e.g. ignoring rotation about the z vector of the target frame.

---

Before creating a pull request

• Document new methods and classes (modification to existing method)
• Format new code files using clang-format

Before merging a pull request

• Set version target by selecting a milestone on the right side
• Summarize this change in CHANGELOG.md
• Add unit test(s) for this change

[mxgrey]

My memory from writing this is a bit hazy, so I'd need to think more about it to be certain, but if I remember correctly I think I implemented it like this to accurately represent the formal definition of a task space region. I think the difference in the frame of the target and the frame of the bounds is intentional because you may want to represent the desired end effector orientation independently of the coordinates of the constraint limits.

A user could accomplish the effect of this PR by creating two SimpleFrame objects:

1. a SimpleFrame to represent the parent frame that defines the coordinates of the constraint, and

2. a SimpleFrame which is a child of frame (1) to represent the target. If desired, this could have an identity transform relative to frame (1), so that frame (1) effectively acts as both the target transform and the constraint coordinates.

I think this PR would make us lose this flexibility.

That being said, I'd be open to optionally allowing users to explicitly specify a reference frame for the bounds of their Task Space Region. It could be added as a property to the InverseKinematics::TaskSpaceRegion class which defaults to a nullptr, and if it's a nullptr then the parent frame of the target will be used. This would have the bonus effect of preserving existing behavior.

[codecov]

Codecov Report

Merging #1430 into master will increase coverage by 0.01%.
The diff coverage is 83.33%.

@@            Coverage Diff             @@
##           master    #1430      +/-   ##
==========================================
+ Coverage   57.99%   58.01%   +0.01%     
==========================================
  Files         412      412              
  Lines       29861    29860       -1     
==========================================
+ Hits        17319    17324       +5     
+ Misses      12542    12536       -6

Impacted Files	Coverage Δ
dart/dynamics/InverseKinematics.cpp	49.86% <83.33%> (+0.73%)	⬆️

[jslee02]

@mxgrey Thanks for the clarification. Reading the paper, I got why the parent of the target is used as the reference of bounds in the code; The parent of the target frame is the reference frame of TSR, denoted as T_w.

In addition to your suggestion (allowing to explicitly specifying the reference frame), I'd like to add a simpler error method that uses the target frame for the reference frame of bounds as well for the users who are not familiar with the TSR's frame conventions. Then the logic in this PR could be in that error method.

[mxgrey]

I have nothing against making it a separate error method, but one option to consider is to instead make a function that still returns a TaskSpaceRegion object, but have that object configured to behave the way that's being discussed. That would reduce some duplicate implementation, since the behavior that's being proposed is just a subset of what a TaskSpaceRegion can do.

[jslee02]

I have nothing against making it a separate error method, but one option to consider is to instead make a function that still returns a TaskSpaceRegion object, but have that object configured to behave the way that's being discussed. That would reduce some duplicate implementation, since the behavior that's being proposed is just a subset of what a TaskSpaceRegion can do.

All sounds good to me!

[parkerowan]

@mxgrey, @jslee02 Thanks for the feedback!

That being said, I'd be open to optionally allowing users to explicitly specify a reference frame for the bounds of their Task Space Region. It could be added as a property to the InverseKinematics::TaskSpaceRegion class which defaults to a nullptr, and if it's a nullptr then the parent frame of the target will be used. This would have the bonus effect of preserving existing behavior.

This sounds like a good way to go for backward compatibility and flexibility. I'll update the PR and add tests to capture the cases.

[jslee02]

Closing this PR in favor of #1548