Feature: TSR and TSRs cleanup and helper functions #159
Conversation
…o feature/tsr_distance Conflicts: src/prpy/tsr/tsr.py
…otics/prpy into feature/tsr_distance
|
|
||
| xyzcheck = [(((x + EPSILON) >= Bw_xyz[i, 0]) and | ||
| ((x - EPSILON) <= Bw_xyz[i, 1])) | ||
| or (Bw_xyz[i, 1] - Bw_xyz[i, 0] < EPSILON) |
There was a problem hiding this comment.
Why is this extra check for a point TSR necessary?
| @@ -41,6 +48,8 @@ def __init__(self, T0_w=None, Tw_e=None, Bw=None, | |||
| Tw_e = numpy.eye(4) | |||
| if Bw is None: | |||
| Bw = numpy.zeros((6, 2)) | |||
| elif any((Bw[:, 1]-Bw[:, 0]) > 2*pi + EPSILON): | |||
| raise(ValueError('Bw range must be within 2*PI')) | |||
There was a problem hiding this comment.
Two quick questions about "what it means to be a TSR":
- Must all W transforms contain the identity pose (i.e. must the first column of Bw be all-nonpositive, and the second be all-nonnegative)? There's no fundamental reason why this must be so, but IIRC CBiRRT does not like bounds on W that exclude identity.
- Dmitry's TSR code [0] claims that Bw[:,0] can be greater than Bw[:,1], signifying "an axis flip (not an error)". Does this code support this? If so, this check should have an absolute value in it.
In any case, it might be a good idea to write up a small section on the prpy README describing what a TSR is.
[0] https://github.com/personalrobotics/comps/blob/master/cbirrt2/TaskSpaceRegion.cpp#L65
There was a problem hiding this comment.
This goes back to @mkoval's previous comment on the expressiveness of the TSR representation. I'm quite ambivalent to any of this, so please implement whatever seems reasonable.
There was a problem hiding this comment.
@psigen suggested a simple and elegant solution [which @mkoval might also have alluded to in his earlier post]: treat the bounds as signed, i.e. as from -> to instead of [min, max] and modify Bw[i,1] to Bw[i,1] + 2pi if Bw[i,1] < Bw[i,0]. This will shift it out of the pi wraparound. Of course, this means, Bw[i,1] will no longer be bounded [-pi,pi] but can be as high as pi+2pi=3pi for example if we want the almost great circle [pi, pi-eps]. But I'm willing to live with that.
In is_valid, the rpy_check array needed to be initialized to the correct size to be indexed appropriately. I also did some minor rearranging and commenting of code in there, nothing that changes functionality though.
| # 1. Bw[i,1] > Bw[i,0] which is necessary for LBFGS-B | ||
| # 2. signed rotations, necessary for expressiveness | ||
| Bw_cont = numpy.copy(Bw) | ||
| Bw_cont[3:6, :] = (Bw_cont[3:6, :] + pi) % (2*pi) - pi |
There was a problem hiding this comment.
Will this normalization always work? If Bw_cont[3:6, :] < -pi, then adding pi will not make the value positive. I think this should be:
k = max(int(-Bw_cont[3:6, :].min() / pi), 0) + 1
Bw_cont[3:6, :] = (Bw_cont[3:6, :] + k * pi) % (2*pi) - piI always screw up angle normalization, so I very well might be wrong here. 😓
There was a problem hiding this comment.
Err, what? Behold:
import matplotlib.pyplot as plt
import numpy
pi = numpy.pi
x = numpy.linspace(-10*pi, 10*pi, 1000)
y = [((i + pi) % (2*pi) - pi) for i in x]
plt.plot(x, y)
plt.show()
There was a problem hiding this comment.
I don't know why this works. But the graph looks right, so I'll just accept it as truth.
|
Overall, this looks good. I will merge as soon as we confirm that the angle normalization code is correct. I also made a few nitpicks about syntax, but they're all minor. |
|
WRT @cdellin's comment, I agree writing robust conversion code is hard. Also agree with @psigen that we should create a new PR to decide where some of the conversion code should go. My preference is |
|
👍 |
|
I created #165 to track the conversion function cleanup. There are no major complaints left, so I am merging this. |
This PR cleans up some of the existing TSR and TSRs functions, and adds new helper functions for computing distance to TSR and TSRs, among others.