WIP: Do not assume all-zero/initial robot pose as non-self-colliding pose and make non-self-colliding pose configurable #1072

strmtk · 2022-03-31T04:03:01Z

This patch addresses the problem that a robot link pair which is in a collision when a robot is at its initial pose or a pose that all DOF values are zeros is excluded from self collision checking unintentionally (regardless of whether the pair is marked as adjacent).

How it happens

Self collision checking is performed only on pairs listed in robot.GetNonAdjacentLinks()
The list excludes robot link pairs which are in a self collision when links transformations are set to _vInitialLinkTransformations
_vInitialLinkTransformations are set to link transforms of when a robot is added to an environment in KinBody::_ComputeInternalInformation(),

Changes made in this patch

Drops the assumption that a robot pose that all DOF values are zeros or an initial robot pose are non-self-colliding poses
Adds a new data structure KinBody::PositionConfiguration which represents a pose of a robot with arbitrary connected body active states
Adds a new parameter nonSelfCollidingRobotConfigurations in KinBody, which type is List[PositionConfiguration]. It's an empty list by default.

Note that this patch includes a compatibility-breaking change (the first item in the list above). Some robot models may need additional adjacent link pair specifications or an entry in nonSelfCollidingRobotConfigurations.

…onfigurations

…nfigurations

…llidingPositionConfigurationsAndLinkTransformations

felixvd

Thanks! See comments inline.

felixvd · 2022-03-31T04:24:46Z

include/openrave/kinbody.h

+
+            std::string GetResolvedJointName() const;
+
+            std::string _id; ///< id of joint configuration state, for incremental update


Is this the same id as the parent PositionConfiguration? If not, is it also unique?

It is not. This field is used as a primary key when the data structure is stored in a database, thus is unique among JointConfigurationStates.

include/openrave/kinbody.h

felixvd · 2022-03-31T04:39:08Z

python/bindings/openravepy_kinbody.cpp

+    if (!IS_PYTHONOBJECT_NONE(_vNonSelfCollidingPositionConfigurations)) {
+        for (int positionConfigurationIndex = 0; positionConfigurationIndex < len(_vNonSelfCollidingPositionConfigurations); ++positionConfigurationIndex) {
+            PyPositionConfigurationPtr pyPositionConfiguration = py::extract<PyPositionConfigurationPtr>(_vNonSelfCollidingPositionConfigurations[positionConfigurationIndex]);
+            if (!!pyPositionConfiguration) {


I had to look this up. It looks like I'm not alone in thinking that it's not very readable, but I see that it's used a lot in the code base already, so I won't fight wind mills.

It is a part of the project coding style rather than my preference. Please talk to @rdiankov about this.

felixvd · 2022-03-31T05:03:55Z

src/libopenrave/kinbody.cpp

@@ -113,6 +114,7 @@ bool KinBody::KinBodyInfo::operator==(const KinBodyInfo& other) const {
           && AreVectorsDeepEqual(_vLinkInfos, other._vLinkInfos)
           && AreVectorsDeepEqual(_vJointInfos, other._vJointInfos)
           && AreVectorsDeepEqual(_vGrabbedInfos, other._vGrabbedInfos)
+           && _vNonSelfCollidingPositionConfigurations == other._vNonSelfCollidingPositionConfigurations


When do we use AreVectorsDeepEqual here and when do we not?

Good point. AreVectorsDeepEqual checks value equality of each vector element, while == checks reference equality. Value equality needs to be checked here, so I updated the code.

felixvd · 2022-03-31T05:22:06Z

src/libopenrave/kinbody.cpp

@@ -4824,29 +4866,27 @@ void KinBody::_ComputeInternalInformation()
    }

    _nHierarchyComputed = 2;
-    // because of mimic joints, need to call SetDOFValues at least once, also use this to check for links that are off


This comment got lost. Was that intentional?

It is an intended removal. Previously, SetDOFValues() was called here while DOF values were not supposed to be changed by the call. In the new version, DOF values are supposed to be updated by a SetDOFValues() call, thus the comment is no longer valid.

felixvd · 2022-03-31T05:52:05Z

src/libopenrave/kinbody.cpp

+        std::vector<bool> adjacentLinkFlags;
+        _CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurations(adjacentLinkFlags);
+
+        // Constructs _vNonAdjacentLinks[0]


What does that mean? I can't even find why _vNonAdjacentLinks has 4 elements. I guess [0] is the list of adjacent links without a modification via adjacentoptions.

Could you add some more comments about the logic anyway?

felixvd · 2022-03-31T05:55:01Z

src/libopenrave/kinbody.cpp

+    return _vNonAdjacentLinks.at(adjacentoptions);
+}
+
+void KinBody::_CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurations(std::vector<bool>& adjacentLinkFlags) const


Did we profile this function? It looks pretty heavy.

Please add a description to the header, too.

Though it takes some computation cost, it should not cause a problem practically thanks to the cache. I added an annotation to the function.

I'm not saying this feature is too heavy or not worth it - I would just like to know if we have benchmarking set up anywhere. Do we track OpenRAVE's performance like that?

felixvd · 2022-03-31T06:33:31Z

src/libopenrave/kinbody.cpp

+        _vNonAdjacentLinks[0].clear();
+        for( size_t ind0 = 0; ind0 < _veclinks.size(); ++ind0 ) {
+            for( size_t ind1 = ind0 + 1; ind1 < _veclinks.size(); ++ind1 ) {
+                if( !adjacentLinkFlags.at(_GetIndex1d(ind0, ind1)) && !AreAdjacentLinks(ind0, ind1) ) {


The second check happens inside _CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurations already.

It is needed in a case no non-self-colliding position is given. The one in the calculation function is only for optimisation.

felixvd · 2022-03-31T06:37:49Z

src/libopenrave/kinbody.cpp

+                    continue;  // Already marked as adjacent, no need to check again
+                }
+                if( AreAdjacentLinks(ind0, ind1) ) {
+                    adjacentLinkFlags.at(adjacentLinkFlagIndex) = true;  // Optimisation; allows skipping the check next time


If I understand correctly, adjacentLinkFlags in this function has the same size and format as _vAdjacentLinks which you check against here via AreAdjacentLinks. In that case, why don't we initialize adjacentLinkFlags to _vAdjacentLinks in line 5455 and skip this comparison inside the loop?

Those are not equivalent. _vAdjacentLinks is calculated from actual link adjacencies and user-specified KinBodyInfo::_vForcedAdjacentLinks, while this local variable is calculated from user-specified non-self-colliding configurations. Please see the implementation of KinBody::_ComputeInternalInformation().

I had a quick look at it and will read in detail tomorrow, but I don't think the calculation of _vAdjacentLink is central to this question: Since you perform the check in this line, doesn't adjacentLinkFlags become a superset of _vAdjacentLink (which is a superset of _vForcedAdjacentLinks)? If yes, shouldn't we just initialize it to that value?

Sorry, I was misunderstanding your point. I updated the code to do so.

felixvd · 2022-03-31T06:43:48Z

src/libopenrave/kinbody.cpp

-                const bool bAdjacent = AreAdjacentLinks(ind0, ind1);
-                if(!bAdjacent && !collisionchecker->CheckCollision(LinkConstPtr(_veclinks[ind0]), LinkConstPtr(_veclinks[ind1])) ) {
-                    _vNonAdjacentLinks[0].push_back(ind0|(ind1<<16));
+        // Constructs initial dofValueIsDeterminableFlags


Could you add the reason we need to do this to these comments?

Added more comments.

Co-authored-by: Felix von Drigalski <[email protected]>

… equality

… into nonSelfColConfig

strmtk · 2022-03-31T07:32:00Z

@felixvd Can we focus on the changes made in the patch, and spare readability issues derived from the original branch for now? It would be a good idea to create another PR for those. Thank you.

…lidingPositionConfigurations()

…guration()

felixvd

Can we focus on the changes made in the patch, and spare readability issues derived from the original branch for now? It would be a good idea to create another PR for those.

As mentioned above, I didn't see that some of the lines weren't new, and there might have been a misunderstanding about some comments. That's my bad. As long as new code is clear and documented I'm happy.

(edited after a discussion with @yoshikikanemoto )

I'd like to consider one more point: when two non-self-colliding robot poses give different results for the adjacent links, is this intentional, or an error? I cannot think of a situation where it would be intended, but I might be missing something - can you come up with a counter example?

I imagine this flow:

The user sets up multiple non-self-colliding poses - a very intuitive way to define "my robot can go here".
The user adds cables/attachments as links to the robot to restrict unintended motion. They will be looking at the robot in one of the poses. If the new links collide in another pose, the links would be set to adjacent, and not restrict robot motion anymore. The behavior might be surprising, and similar to what this PR is trying to fix.
When settings are copied from another robot cell and the robot is replaced with another, links may similar be marked adjacent unintentionally.

It seems that using the intersection instead of the union here would add a layer of redundancy and make the system more robust to erroneous settings.

Adding this behavior isn't necessary to solve the bug that this PR is meant to fix, and this may not be the best place to implement it. But if you agree that this behavior would be useful, let's think about where it should live, before we have to make more or bigger changes later on.

Apart from that the PR looks good to me by the way.

felixvd · 2022-03-31T12:51:35Z

src/libopenrave/kinbody.cpp

+    return _vNonAdjacentLinks.at(adjacentoptions);
+}
+
+void KinBody::_CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurations(std::vector<bool>& adjacentLinkFlags) const
 {
    class TransformsSaver


Suggested change

class TransformsSaver

// Saves the link transformations and joint values of the body and applies them when this object is destroyed (at the end of the function)

class TransformsSaver

Could you confirm that I didn't misread this? Thanks!

That is a correct statement. However, one important aspect is missed: this is kind of a hackish replacement for KinBodyStateSaver(_, Save_LinkTransformation) to allow making a change on a const KinBody. This limitation comes from the function signature of GetNonAdjacentLinks() const.

felixvd · 2022-03-31T13:12:44Z

src/libopenrave/kinbody.cpp

@@ -5331,10 +5389,50 @@ bool CompareNonAdjacentFarthest(int pair0, int pair1)
 }

 const std::vector<int>& KinBody::GetNonAdjacentLinks(int adjacentoptions) const
+{
+    CHECK_INTERNAL_COMPUTATION;
+    if( _nNonAdjacentLinkCache & 0x80000000 ) {


Thanks. I didn't see that this was part of the original code.

include/openrave/kinbody.h

Co-authored-by: Felix von Drigalski <[email protected]>

Shintaro Matsuoka added 11 commits March 28, 2022 14:00

Adds PositionConfiguration

e96520b

Skip serialising JointConfigurationState::ConnectedBodyName when empty

b0417b6

Adds JointConfigurationState.jointAxis

812186d

Adds JSON serialisation/deserialisation for nonSelfCollidingPositionC…

c31b8d6

…onfigurations

Calculate non-adjacent link pairs based on nonSelfCollidingPositionCo…

9853c40

…nfigurations

Gets rid of const cast in KinBody::GetNonAdjacentLinks()

93faf2c

Adds {KinBody,RobotBase}::GetPositionConfiguration()

3e4ed28

Reimplements KinBody::SetNonCollidingConfiguration() with _vNonSelfCo…

84cee62

…llidingPositionConfigurationsAndLinkTransformations

Clean up

5f311cc

Adds Python binding for KinBody::AreAdjacentLinks()

d8c8ef7

kinbody.h: Better annotations

bd16f55

felixvd reviewed Mar 31, 2022

View reviewed changes

strmtk and others added 3 commits March 31, 2022 15:56

Annotate JointConfigurationState

7cd1f1b

Co-authored-by: Felix von Drigalski <[email protected]>

Need to check value equality of PositionConfigurations, not reference…

e0a4edb

… equality

Merge branch 'nonSelfColConfig' of ssh://github.com/rdiankov/openrave…

aa1d3da

… into nonSelfColConfig

Shintaro Matsuoka added 2 commits March 31, 2022 16:34

Replace 0x80000000 for KinBody::_nNonAdjacentLinkCache with a constant

2a9e9b5

Add annotations to KinBody::_CalculateAdjacentLinkFlagsFromNonSelfCol…

ddc5424

…lidingPositionConfigurations()

strmtk requested a review from rdiankov March 31, 2022 08:19

strmtk marked this pull request as draft March 31, 2022 08:20

Shintaro Matsuoka added 2 commits March 31, 2022 17:38

Need to skip inactive connected bodies in RobotBase::GetPositionConfi…

5709ac7

…guration()

Optimisation in KinBody::GetNonAdjacentLinks()

d2b5f46

felixvd reviewed Mar 31, 2022

View reviewed changes

Annotates JointConfigurationState::_id

aef1465

Co-authored-by: Felix von Drigalski <[email protected]>

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

WIP: Do not assume all-zero/initial robot pose as non-self-colliding pose and make non-self-colliding pose configurable #1072

WIP: Do not assume all-zero/initial robot pose as non-self-colliding pose and make non-self-colliding pose configurable #1072

strmtk commented Mar 31, 2022

felixvd left a comment

felixvd Mar 31, 2022

strmtk Mar 31, 2022

felixvd Mar 31, 2022

strmtk Mar 31, 2022

felixvd Mar 31, 2022

strmtk Mar 31, 2022

felixvd Mar 31, 2022

strmtk Mar 31, 2022 •

edited

Loading

felixvd Mar 31, 2022

felixvd Mar 31, 2022

strmtk Mar 31, 2022

felixvd Mar 31, 2022

felixvd Mar 31, 2022

strmtk Mar 31, 2022

felixvd Mar 31, 2022

felixvd Mar 31, 2022

strmtk Mar 31, 2022

felixvd Mar 31, 2022

strmtk Mar 31, 2022

felixvd Mar 31, 2022

strmtk Mar 31, 2022

strmtk commented Mar 31, 2022

felixvd left a comment •

edited

Loading

felixvd Mar 31, 2022

strmtk Apr 1, 2022

felixvd Mar 31, 2022


		std::string GetResolvedJointName() const;

		std::string _id; ///< id of joint configuration state, for incremental update

	class TransformsSaver
	// Saves the link transformations and joint values of the body and applies them when this object is destroyed (at the end of the function)
	class TransformsSaver

WIP: Do not assume all-zero/initial robot pose as non-self-colliding pose and make non-self-colliding pose configurable #1072

Are you sure you want to change the base?

WIP: Do not assume all-zero/initial robot pose as non-self-colliding pose and make non-self-colliding pose configurable #1072

Conversation

strmtk commented Mar 31, 2022

How it happens

Changes made in this patch

felixvd left a 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

strmtk Mar 31, 2022 • 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

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

strmtk commented Mar 31, 2022

felixvd left a comment • 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

strmtk Mar 31, 2022 •

edited

Loading

felixvd left a comment •

edited

Loading