Add support for floating, planar joint types #281

EzraBrooks · 2024-02-16T19:41:05Z

Closes #8

WIP, need to add an example (and debug any issues said example ends up having)

javascript/src/URDFClasses.js

gkjohnson · 2024-02-17T01:29:43Z

javascript/src/URDFClasses.js

-                this.jointValue = new Array(2).fill(0);
+                // Planar joints rotate about their Z axis
+                this.axis = new Vector3(0, 0, 1);
+                this.jointValue = new Array(3).fill(0);


I didn't realize planar joints could rotate? Is there documentation on this? The specification is pretty sparse when it comes to the descriptions on joint behavior but it makes it sound like planar movement only? If the ROS implementation differs from the spec then it might be worth getting the wiki page updated.

planar — this joint allows motion in a plane perpendicular to the axis.

I'll need to dig into this more. I was pointed to some code in MoveIt and ROS Control that treats them as X,Y,Rz - I was also previously under the impression that it was only X,Y.

Info seems pretty scattered but from these urdfpy docs it looks like a "planar" joint is described as a joint that only supports planar movement on x and y in the plane described by the axis vector. It's not clear how you're supposed to derive the local x and y axes from the axis vector, though.

For context, here's the code in MoveIt that clearly treats it as having a rotational component.

But you're definitely right that that's not how it's implemented in urdfpy. I'm trying to dig into how this is implemented in RViz, because I assume whatever MoveIt is doing is visualized fine in RViz.

I'll discuss this more internally with the MoveIt & MoveIt Pro team and determine whether we actually think this is part of the standard or whether it's some incorrect assumption made long ago. git blame says that code is 12 years old!

If it's a MoveIt-specific thing then I'll have the implementation here be 2dof and we'll fix it on the MoveIt end.

The spec does not read so ambiguously to me and the URDF py project explicitly does not support a rotational axis. URDF is a defined specification and I'm generally not keen on adding new features outside of the spec (for this or other formats) just because it's convenient. It inevitably leads to situations like this where users will produce files that are designed with a single, spec-breaking implementation in mind but which will not work in applications that have followed it, ultimately diluting the purpose of the spec.

If this is something that's important or is more or less becoming spec-by-convention I recommend making suggestions to edit the URDF specification. I understand that ROS is already a bit of a wild west in terms of software but the least we can do is not continue to proliferate inconsistencies with the few formats that are in place to enable interoperability.

Unfortunately every attempt to update the URDF specification that I'm aware of has stalled for years at a time :(

My understanding is that URDFpy is the outlier here and other URDF parsers (i.e. urdf-rs) do support a rotational axis.

we'll reopen the conversation internally about removing the rotation from planar joints in MoveIt. If you're certain we don't want to support a rotation component here, I can remove it from this PR and move just that addition to another branch I'll use on my project for now.

I'm mostly asking for a good faith effort to be made to improve the specification if it seems that it's out of date or not clearly representative of the common interpretation, as you are suggesting, so this kind of confusion does not happen in the future. It is not difficult to change the wiki page - I made a change myself years ago at afford more geometry formats to be allowed in the specification. If all but urdf-py is interpreting planar joints as "two translation axes and one rotation axis" then the documentation should be updated to reflect that more clearly and at least an issue should be made for urdf-py to bring it in line with other tools. You can request edit access for your wiki account here.

Unfortunately every attempt to update the URDF specification that I'm aware of has stalled for years at a time :(

If you're referring to URDF 2.0, then yeah it's been stalled forever and I'll be surprised if it goes anywhere because of how large and breaking the the scope is. But making clarifying, non breaking changes to the URDF spec is not the same as trying to replace it completely.

thanks for the clarification. We'll start a conversation on the ROS wiki side!

This has been a huge can of worms across the board, honestly - we went to polish up mobile base support in our application and found issues all across the ROS stack (even to the point of straight-up segfaults when you pass a planar joint to things). As always, I appreciate your engagement with my issues and PRs here. That's more than can be said for a lot of the packages we use!

javascript/src/URDFClasses.js

gkjohnson

This is great, thanks! I know it's still WIP but I added a few comments. An example would be awesome, as well, and we'll want to update the README for the joint value function.

EzraBrooks · 2024-02-20T21:55:03Z

I'm headed out on vacation for a week but handing this branch over to my team for testing with mobile bases, so hopefully there'll be a good example case soon to iron out any bugs and showcase the capability here :)

gkjohnson · 2024-02-21T01:56:55Z

I'm headed out on vacation for a week but handing this branch over to my team for testing with mobile bases, so hopefully there'll be a good example case soon to iron out any bugs and showcase the capability here :)

Sounds good - once the PR is marked as "ready for review" I can provide comments!

EzraBrooks · 2024-03-25T16:06:30Z

sorry for the wait here; we're still working out some issues with the backend we were going to use this with! will get this finished up as soon as I have it working for us :)

Abishalini · 2024-03-28T16:37:47Z

@gkjohnson Would it be okay to add the rotational component for the planar joint?
Adding this here for more visibility - #281 (comment)

gkjohnson · 2024-03-29T10:29:15Z

javascript/src/URDFClasses.js

+                if (this.jointValue.every((value, index) => values[index] === value || values[index] === null)) return didUpdate;
+                // anonymous block scope to prevent variable name clobbering with other cases in the switch
+                {


Nit: can we follow the code formatting in the rest of the and wrap the whole case in braces:

case 'floating': { // ... }

yep. worth noting this is not just styling: this is creating an anonymous block scope.

gkjohnson · 2024-03-29T10:35:43Z

javascript/src/URDFClasses.js

+                    this.position.copy(this.origPosition);
+                    // Respect origin RPY when setting position
+                    if (posX !== this.jointValue[0] && posX !== null) {
+                        _tempAxis.set(1, 0, 0).applyEuler(this.rotation);
+                        this.position.addScaledVector(_tempAxis, posX);
+                        didUpdate = true;
+                    }
+                    if (posY !== this.jointValue[1] && posY !== null) {
+                        _tempAxis.set(0, 1, 0).applyEuler(this.rotation);
+                        this.position.addScaledVector(_tempAxis, posY);
+                        didUpdate = true;
+                    }
+                    if (posZ !== this.jointValue[2] && posZ !== null) {
+                        _tempAxis.set(0, 0, 1).applyEuler(this.rotation);
+                        this.position.addScaledVector(_tempAxis, posZ);
+                        didUpdate = true;
+                    }


I don't think we need the if conditions here. Right now you're always resetting the object position but then you're only shifting along one of the axes if it's changed. That means the position of the joint will only reflect the values that have changed in the recent function call.

At this point in the function we can really just return true indicating that the joint changed since we've already checked if the joint values have changed above.

yep, you're right - I totally blanked on the fact that the initial position set was outside the if clauses.

gkjohnson · 2024-03-29T10:36:34Z

javascript/src/URDFClasses.js

+                        didUpdate = true;
+                    }
+                    if (posY !== this.jointValue[1] && posY !== null) {
+                        _tempAxis.set(0, 1, 0).applyEuler(this.rotation);


We'll want to make sure we use the "origQuaternion" here so we're operating in the frame defined by the original joint state otherwise the rotation of these axes will be dependent on the previous state of the rotation based on prior joint angles.

That makes sense to me, but does that mean the identical code in the prismatic joint has the same issue?

The prismatic joint will never have the rotation changed so the orientation inthis.rotation will never be different from this.origQuaternion in that case.

gkjohnson · 2024-03-29T10:45:46Z

javascript/src/URDFClasses.js

+                    _tempAxis.set(1, 0, 0).applyEuler(this.rotation);
+                    this.position.addScaledVector(_tempAxis, posX);
+                    this.jointValue[0] = posX;


From the specification the plane that the joint is supposed to move on should be based on the axis specified in the joint - this is just using the local x axis:

planar — this joint allows motion in a plane perpendicular to the axis.

One additional issue with the spec is that it does not clearly define how to derive the X and Y plane axes from the single "axis" vector. It could just be from rotating the frame such that the z axis the shortest direction to the axis direction with no twist rotation - but this leaves us with an undefined case when the axis is 0 0 - 1. If there is a consistent and well-defined approach to this in MoveIt or other applications we should use that one. It would also be good to update the specification document to be more clear here.

Yeah, good point. This is something we waffled on internally due to the vagueness of the specification. @Abishalini @henningkayser how does MoveIt/OMPL/etc. handle this?

javascript/src/URDFClasses.js

Fix test that expects 2 dofs for planar which has 3 use isNaN on individual DoFs instead of `== null` on all of them together Go back to checking for null, parsing should be fixed as of gkjohnson#282 Return early if all values are identical to current state _or null_ note to self Avoid allocating new memory for vectors during joint setting Revert added Rz DoF for planar joint type as far as I can tell, the URDF spec does not allow multi-dof limits add first cut at floating joint support formatting re-add Rz component to planar joints update setJointValue type in typescript definition test update fix block scoping cleanup

EzraBrooks · 2024-08-21T16:08:51Z

Hi Garrett, we've decided to stay the course on 3DoF planar joints, which is what ROS expects (even though I agree with you that it's kind of a silly expectation). I'm closing this PR in favor of our fork. Thanks so much for your work on this library and for your help along the way!

gkjohnson · 2024-08-21T23:56:25Z

@EzraBrooks was there a blocker on updating the URDF spec? From some links you've sent before and other references I've seen since it does look like planar joints do generally (for some reason) include a third twist component and I think I've come around a bit since previous discussions. I'm just trying to reconcile the differences in the spec with how people are using things (which is the more important piece). It's unfortunate that parts of interchange documentation get left behind while software changes are made to get something working but it is looking like the documentation is the "wrong" thing here.

gkjohnson · 2024-08-28T03:03:07Z

I'm going to go ahead and merge this rather than contribute to the fragmentation I'm seeing in the community - I'm seeing that the spec is likely not the "source of truth" in these cases but rather an afterthought, unfortunately. Likewise the current implementation is structured so that the X, Y translation are the first arguments so the function will ignore the rotation component meaning it will work in either case.

Sorry for the delay in coming around on this.

gkjohnson · 2024-08-28T11:10:29Z

Just published in v0.12.2

EzraBrooks commented Feb 16, 2024

View reviewed changes

javascript/src/URDFClasses.js Outdated Show resolved Hide resolved

gkjohnson reviewed Feb 17, 2024

View reviewed changes

EzraBrooks mentioned this pull request Feb 19, 2024

Don't parseFloat null values in setJointValue #282

Merged

EzraBrooks changed the title ~~Add planar joint support~~ Add support for floating, planar joint types Feb 19, 2024

gkjohnson added this to the v0.12.2 milestone Feb 20, 2024

gkjohnson reviewed Mar 29, 2024

View reviewed changes

github-advanced-security bot found potential problems Apr 22, 2024

View reviewed changes

javascript/src/URDFClasses.js Fixed Show fixed Hide fixed

EzraBrooks force-pushed the add-planar-joint-support branch from 682a4f3 to 608b446 Compare August 21, 2024 16:07

Fix joint translations

b560407

EzraBrooks force-pushed the add-planar-joint-support branch from 608b446 to b560407 Compare August 21, 2024 16:07

EzraBrooks closed this Aug 21, 2024

gkjohnson reopened this Aug 28, 2024

gkjohnson marked this pull request as ready for review August 28, 2024 03:03

Spelling

2dc28e1

gkjohnson merged commit 0c442aa into gkjohnson:master Aug 28, 2024
5 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add support for floating, planar joint types #281

Add support for floating, planar joint types #281

EzraBrooks commented Feb 16, 2024 •

edited

Loading

gkjohnson Feb 17, 2024

EzraBrooks Feb 17, 2024

gkjohnson Feb 17, 2024

EzraBrooks Feb 19, 2024

EzraBrooks Feb 19, 2024

gkjohnson Mar 29, 2024 •

edited

Loading

EzraBrooks Mar 29, 2024

EzraBrooks Mar 29, 2024 •

edited

Loading

gkjohnson Mar 30, 2024

EzraBrooks Apr 1, 2024

gkjohnson left a comment

EzraBrooks commented Feb 20, 2024

gkjohnson commented Feb 21, 2024

EzraBrooks commented Mar 25, 2024

Abishalini commented Mar 28, 2024 •

edited

Loading

gkjohnson Mar 29, 2024

EzraBrooks Mar 29, 2024

gkjohnson Mar 29, 2024

EzraBrooks Mar 29, 2024

gkjohnson Mar 29, 2024

EzraBrooks Mar 29, 2024

gkjohnson Mar 30, 2024

gkjohnson Mar 29, 2024

EzraBrooks Mar 29, 2024

EzraBrooks commented Aug 21, 2024 •

edited

Loading

gkjohnson commented Aug 21, 2024 •

edited

Loading

gkjohnson commented Aug 28, 2024

gkjohnson commented Aug 28, 2024

Add support for floating, planar joint types #281

Add support for floating, planar joint types #281

Conversation

EzraBrooks commented Feb 16, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

gkjohnson Mar 29, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

EzraBrooks Mar 29, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

gkjohnson left a comment

Choose a reason for hiding this comment

EzraBrooks commented Feb 20, 2024

gkjohnson commented Feb 21, 2024

EzraBrooks commented Mar 25, 2024

Abishalini commented Mar 28, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

EzraBrooks commented Aug 21, 2024 • edited Loading

gkjohnson commented Aug 21, 2024 • edited Loading

gkjohnson commented Aug 28, 2024

gkjohnson commented Aug 28, 2024

EzraBrooks commented Feb 16, 2024 •

edited

Loading

gkjohnson Mar 29, 2024 •

edited

Loading

EzraBrooks Mar 29, 2024 •

edited

Loading

Abishalini commented Mar 28, 2024 •

edited

Loading

EzraBrooks commented Aug 21, 2024 •

edited

Loading

gkjohnson commented Aug 21, 2024 •

edited

Loading