improve RobotState::updateStateWithLinkAt() #765
Conversation
... to compute earliest parent link that is rigidly connected to a given link
|
@davetcoleman I accidentally pushed this as a PR branch to ros-planning first. The failed build only exists, because I immediately removed this branch again and Travis could find the branch anymore... |
| @@ -239,6 +239,9 @@ class RobotModel | |||
| /** \brief Get a link by its name. Output error and return NULL when the link is missing. */ | |||
| LinkModel* getLinkModel(const std::string& link); | |||
|
|
|||
| /** \brief Get earliest link model that is rigidly connected to the given link */ | |||
There was a problem hiding this comment.
define "earliest" - is this the parent link? the root link? i guess with the term rigidly it means:
joint1 - linkA - fixedJoint1 - linkB
getRigidlyConnectedParentLinkModel(linkB)
return linkA correct?
There was a problem hiding this comment.
OK, obviously comment was too brief. I added more explanation.
| return it->second; | ||
| logError("Link '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); | ||
| return NULL; | ||
| return const_cast<RobotModel*>(this)->getLinkModel(name); |
There was a problem hiding this comment.
i don't understand - wouldn't this be recursive and not work?
why remove the helpful logError message?
There was a problem hiding this comment.
This simply relays the call to the otherwise fully identical non-const version of the function.
Removing the constness via const_cast ensures that it is not recursive.
The log message is retained from the non-const version.
There was a problem hiding this comment.
For aesthetic reasons I prefer to cast the other way around (make the non-const function call the const version) so that you don't have to cast const-ness away.
Apart from that this method is perfectly valid.
There was a problem hiding this comment.
In this case you would have to cast constness away for the result type: const LinkModel* -> LinkModel*, which then requires two const_casts. In my opinion that's even harder to read, although it better matches the semantics. However, this is cosmetics.
There was a problem hiding this comment.
Ah, that explains why you chose to do it this way around.
I agree that this is cosmetics. No need to get out the eye liner :)
| The link transforms of link and all its descendants are updated, but not marked as dirty, | ||
| although they do not match the joint values anymore! | ||
| Collision body transforms are not yet updated, but marked dirty only. | ||
| Use update(false) or updateCollisionBodyTransforms() to update them as well. |
| // position_ + | ||
| // parent_link->getParentJointModel()->getFirstVariableIndex()); | ||
| // all collision body transforms are invalid now | ||
| dirty_collision_body_transforms_ = parent_link->getParentJointModel(); |
There was a problem hiding this comment.
i think this could adversely impact existing MoveIt! setups so we should not backport to Indigo
There was a problem hiding this comment.
This indeed changes behavior: It allows to update and subsequently use the warped collision transforms.
However, I don't see why this bug fix shouldn't be worth backporting.
There was a problem hiding this comment.
I agree that this causes more computational load when someone uses this (rather unusual) function with the backward parameter, but I don't see why we shouldn't backport it: without it the outdated transforms go unmarked.
There was a problem hiding this comment.
because it changes behavior on existing systems in ways we can't predict, and indigo is old and is suppose to be stable
There was a problem hiding this comment.
To clarify (please confirm @rhaschke) if you currently use updateStateWithLinkAt(... backward=true) in indigo, the code will use invalid transforms for collision checking (or it asserts and quits in debug build).
This is the only behavior these lines change by requiring an update of the invalid transforms.
There was a problem hiding this comment.
I don't use Indigo, and I didn't use updateStateWithLinkAt(... backward=true) either. I just discovered that the collision transforms are not (and cannot be) updated without this change.
| * what you went for. Instead, updateStateWithLinkAt(getRigidlyConnectedParentLinkModel(grasp_frame), ...) | ||
| * will actually warp wrist (and all its descendants). | ||
| */ | ||
| static const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const LinkModel* link); |
There was a problem hiding this comment.
My main question with this function is where to put it.
- Leave it here in
RobotModelas a static function? - Define it as a member of
LinkModel? - Put it (as a static function again) into
RobotState. i.e. the vicinity ofupdateStateWithLinkAt()?
The order resembles my preference. @davetcoleman @v4hn Any other opinions?
There was a problem hiding this comment.
Personally I think you picked the best spot already. I vote to keep it in RobotModel together with all the getLink functions.
There was a problem hiding this comment.
@davetcoleman If you agree too, I think this is ready for merging.
There was a problem hiding this comment.
+1 LinkModel. Def not RobotState but option 2 works also for me.
There was a problem hiding this comment.
So then we agree on this. Could anybody of you finally accept / merge this PR?
Co-authored-by: Tyler Weaver <tyler@picknik.ai>
Improve documentation for
RobotState::updateStateWithLinkAt(). For me it was unclear, which transforms were updated and which were marked dirty.If argument
backward = trueis passed, mark all collision body transforms as dirty.Provide utility function
RobotModel::getRigidlyConnectedParentLinkModel()to find the top-most parent link that is ridigly linked to the given link. Usually, when placing a link withRobotState::updateStateWithLinkAt(), this parent link should be used.