symforce-org / symforce

Fast symbolic computation, code generation, and nonlinear optimization for robotics
https://symforce.org
Apache License 2.0
1.44k stars 147 forks source link

sym.Pose3.position() read/write data types inconsistency #246

Closed joansola closed 2 years ago

joansola commented 2 years ago

Describe the bug Since v0.6.0, the type of sym.Pose3.t has changed from a 1d array ndarray(1,3) to a vertical array, ndarray(3,1) Now in v0.6.0, reading Pose3.t produces such a vertical array, but writing into Pose3.t requires a single horizontal array.

To Reproduce

pose = sym.Pose3.identity()
position = pose.t
pose2 = sym.Pose3(t=position, R=pose.R)
display(pose2.to_storage())
>>>
[0.0, 0.0, 0.0, 1.0, array([0.]), array([0.]), array([0.])]

and any operation on pose2 now fails.

How am I supposed to deal with such positions without going into ugly code such as the one below?

pose = sym.Pose3.identity()
position = pose.t
pose2 = sym.Pose3(t=position.transpose()[0], R=pose.R)
display(pose2.to_storage())

Expected behavior

pose = sym.Pose3.identity()
position = pose.t
pose2 = sym.Pose3(t=position, R=pose.R)
display(pose2.to_storage())
>>>
[0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]

Screenshots

image

Environment (please complete the following information):

aaron-skydio commented 2 years ago

Agree this is quite confusing - we should definitely make these compatible. Probably what should happen is that the sym.Pose3 constructor should do the same thing the generated functions do and accept either T.Union[T.Sequence[float], np.ndarray] and allow the ndarray to either be flat or a column vector. No matter what though you should be able to take one pose.t and pass it to the constructor of sym.Pose3, we should add a unit test that checks this specifically (probably same for the rotation component).

Yes, the workaround for now would be something like that ugly transpose and indexing

joansola commented 2 years ago

Hi Aaron. I see! Well, what I did, I got rid of all instances of sym.Pose3 and use instead sf.Pose3. Things are a little easier this way and I have less conflicts. The reason why I used sym comes back to another old issue you probably remember about copying Pose3 objects, but this is now solved.

Thanks again! Keep up the good work!