OpenRAVE Users List

Robot CheckSelfCollision, why check link/link for grabbed bodies?

Classic

List

Threaded

4 messages Options

Bahram Maleki-Fard

Robot CheckSelfCollision, why check link/link for grabbed bodies?

Hi all,

the commit e5df33e8 from back in 2010 changed these lines of
RobotBase::CheckSelfCollision()

FOREACHC(itrobotlink,itbody->vNonCollidingLinks) {
- if(
GetEnv()->CheckCollision(*itrobotlink,KinBodyConstPtr(pbody),report) ) {
- bCollision = true;
- break;
+ // have to use link/link collision since link/body checks
attached bodies
+ FOREACHC(itbodylink,pbody->GetLinks()) {
+ if(
GetEnv()->CheckCollision(*itrobotlink,KinBody::LinkConstPtr(*itbodylink),report)
) {
+ bCollision = true;
+ break;
+ }
}
+ if( bCollision )
+ break;
}

What is the reason behind this?

We use OpenRAVE only for planning, not for controlling the arm. Which
means, we update the robot with the joints from the real arm, put
objects where we perceive them (e.g. by sensors), and plan.
The problem is, object shapes and robot model are not absolutely 100%
accurate. Now lets say we grab a cup, and close the fingers. The fingers
of the real arm wrap nicely around the real cup, but in the OpenRAVE
environment it is not that accurate. The fingers there are slightly
moving "into" the cylindrical object. Then we attach that object of
cours, for further planning.
Now the code from above comes into play. It will always fail, because
the finger links collide with the attached object. Whereas with the
previous version, that collision would be ignored, because the object is
attached to the parent-link of the fingers.

I can see that for other cases where the links are not initially in
collision with the grabbed object, we want to have the "new"
collision-check, otherwise there could be "real" collisions. But how can
I resolve the situation I am in? Do I have to disable the Link/object
with obj->Enable(false) ? Or is there a nicer way to do this?

Thanks a lot :)

Cheers,
Bahram

------------------------------------------------------------------------------
Meet PCI DSS 3.0 Compliance Requirements with EventLog Analyzer
Achieve PCI DSS 3.0 Compliant Status with Out-of-the-box PCI DSS Reports
Are you Audit-Ready for PCI DSS 3.0 Compliance? Download White paper
Comply to PCI DSS 3.0 Requirement 10 and 11.5 with EventLog Analyzer
http://pubads.g.doubleclick.net/gampad/clk?id=154622311&iu=/4140/ostg.clktrk
_______________________________________________
Openrave-users mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/openrave-users

Rosen Diankov

Re: Robot CheckSelfCollision, why check link/link for grabbed bodies?

Administrator

When calling RobotBase::Grab, it will record what links of the robot collide with the grabbed object, and ignore those collisions.

Most likely fingers are moving after you called Grab.
To fix this call RobotBase::RegrabAll() after you know the fingers stop moving.

hope this helps,

2014-10-08 4:01 GMT+09:00 Bahram Maleki-Fard <[hidden email]>:

Hi all,

the commit e5df33e8 from back in 2010 changed these lines of
RobotBase::CheckSelfCollision()

FOREACHC(itrobotlink,itbody->vNonCollidingLinks) {
- if(
GetEnv()->CheckCollision(*itrobotlink,KinBodyConstPtr(pbody),report) ) {
- bCollision = true;
- break;
+ // have to use link/link collision since link/body checks
attached bodies
+ FOREACHC(itbodylink,pbody->GetLinks()) {
+ if(
GetEnv()->CheckCollision(*itrobotlink,KinBody::LinkConstPtr(*itbodylink),report)
) {
+ bCollision = true;
+ break;
+ }
}
+ if( bCollision )
+ break;
}

What is the reason behind this?

We use OpenRAVE only for planning, not for controlling the arm. Which
means, we update the robot with the joints from the real arm, put
objects where we perceive them (e.g. by sensors), and plan.
The problem is, object shapes and robot model are not absolutely 100%
accurate. Now lets say we grab a cup, and close the fingers. The fingers
of the real arm wrap nicely around the real cup, but in the OpenRAVE
environment it is not that accurate. The fingers there are slightly
moving "into" the cylindrical object. Then we attach that object of
cours, for further planning.
Now the code from above comes into play. It will always fail, because
the finger links collide with the attached object. Whereas with the
previous version, that collision would be ignored, because the object is
attached to the parent-link of the fingers.

I can see that for other cases where the links are not initially in
collision with the grabbed object, we want to have the "new"
collision-check, otherwise there could be "real" collisions. But how can
I resolve the situation I am in? Do I have to disable the Link/object
with obj->Enable(false) ? Or is there a nicer way to do this?

Thanks a lot :)

Cheers,
Bahram

------------------------------------------------------------------------------
Meet PCI DSS 3.0 Compliance Requirements with EventLog Analyzer
Achieve PCI DSS 3.0 Compliant Status with Out-of-the-box PCI DSS Reports
Are you Audit-Ready for PCI DSS 3.0 Compliance? Download White paper
Comply to PCI DSS 3.0 Requirement 10 and 11.5 with EventLog Analyzer
http://pubads.g.doubleclick.net/gampad/clk?id=154622311&iu=/4140/ostg.clktrk
_______________________________________________
Openrave-users mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/openrave-users

------------------------------------------------------------------------------
Meet PCI DSS 3.0 Compliance Requirements with EventLog Analyzer
Achieve PCI DSS 3.0 Compliant Status with Out-of-the-box PCI DSS Reports
Are you Audit-Ready for PCI DSS 3.0 Compliance? Download White paper
Comply to PCI DSS 3.0 Requirement 10 and 11.5 with EventLog Analyzer
http://pubads.g.doubleclick.net/gampad/clk?id=154622311&iu=/4140/ostg.clktrk
_______________________________________________
Openrave-users mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/openrave-users

Bahram Maleki-Fard

Re: Robot CheckSelfCollision, why check link/link for grabbed bodies?

Thanks, that makes sense.

However, I just tested it and still have problems. I put a lot of debug-messages in Grabbed::_ProcessCollidingLinks and saw the following:
When Grab'ing a KinBody, collisions are correctly detected and ignored. So this is as intended and exactly what you described.
I use a second environment though for IK-checks and planning only (let's call it "planner-env", cloned once the robot has been added to first environment (let's call this one "viewer-env", as the viewer is attached to it). Before doing IK check, in the planner-env I do the following to update it:

1) ReleaseAllGrabbed()
2 delete all non-robot kinbodies
3) SetDOFValues(dofs), where "dofs" is the DOFS of the robot from viewer-environment
4) For each non-robot body in viewer-env, clone it to the planner-env with this code:

    KinBodyPtr new_body, empty;
    new_body = env->ReadKinBodyData(empty, "<KinBody></KinBody>");
    // is that the only way to create a new KinBody?
    new_body->Clone(old_body, 0); // "old_body" is the body in viewer_env
    env->Add(new_body);
    KinBody::KinBodyStateSaver save(old_body, 0xffffffff);
    saver.Restore(new_body);

5) For each manipulator and body in planner-env, grab the body if it is also grabbed by the same manipulator in viewer-environment

When executing the Grab in (5), I can see from the debug messages I put in _ProcessCollidingLinks that it fails to detect a collision between the finger-links and the grabbed object though.This was not the case when Grab-ing in viewer-environment.
I'm still looking into this, but do you have any idea why?

Thanks again.

Cheers,
Bahram

2014/10/08 08:19 , Rosen Diankov:

When calling RobotBase::Grab, it will record what links of the robot collide with the grabbed object, and ignore those collisions.

Most likely fingers are moving after you called Grab.
To fix this call RobotBase::RegrabAll() after you know the fingers stop moving.

hope this helps,

2014-10-08 4:01 GMT+09:00 Bahram Maleki-Fard <[hidden email]>:

Hi all,

the commit e5df33e8 from back in 2010 changed these lines of
RobotBase::CheckSelfCollision()

FOREACHC(itrobotlink,itbody->vNonCollidingLinks) {
- if(
GetEnv()->CheckCollision(*itrobotlink,KinBodyConstPtr(pbody),report) ) {
- bCollision = true;
- break;
+ // have to use link/link collision since link/body checks
attached bodies
+ FOREACHC(itbodylink,pbody->GetLinks()) {
+ if(
GetEnv()->CheckCollision(*itrobotlink,KinBody::LinkConstPtr(*itbodylink),report)
) {
+ bCollision = true;
+ break;
+ }
}
+ if( bCollision )
+ break;
}

What is the reason behind this?

We use OpenRAVE only for planning, not for controlling the arm. Which
means, we update the robot with the joints from the real arm, put
objects where we perceive them (e.g. by sensors), and plan.
The problem is, object shapes and robot model are not absolutely 100%
accurate. Now lets say we grab a cup, and close the fingers. The fingers
of the real arm wrap nicely around the real cup, but in the OpenRAVE
environment it is not that accurate. The fingers there are slightly
moving "into" the cylindrical object. Then we attach that object of
cours, for further planning.
Now the code from above comes into play. It will always fail, because
the finger links collide with the attached object. Whereas with the
previous version, that collision would be ignored, because the object is
attached to the parent-link of the fingers.

I can see that for other cases where the links are not initially in
collision with the grabbed object, we want to have the "new"
collision-check, otherwise there could be "real" collisions. But how can
I resolve the situation I am in? Do I have to disable the Link/object
with obj->Enable(false) ? Or is there a nicer way to do this?

Thanks a lot :)

Cheers,
Bahram

------------------------------------------------------------------------------
Meet PCI DSS 3.0 Compliance Requirements with EventLog Analyzer
Achieve PCI DSS 3.0 Compliant Status with Out-of-the-box PCI DSS Reports
Are you Audit-Ready for PCI DSS 3.0 Compliance? Download White paper
Comply to PCI DSS 3.0 Requirement 10 and 11.5 with EventLog Analyzer
http://pubads.g.doubleclick.net/gampad/clk?id=154622311&iu=/4140/ostg.clktrk
_______________________________________________
Openrave-users mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/openrave-users

Bahram Maleki-Fard

Re: Robot CheckSelfCollision, why check link/link for grabbed bodies?

Ok, tracked down the problem. When grabbing the object in the viewer-env, the internal _listAttachedBodies of that object is updated. This is done *after* _ProcessCollidingLinks (the _listAttachedBodies is empty at that time).
Now I clone that object to the planner-env, and that list is copied as well. When I call "Grab" in the planner-env now and _ProcessCollidingLinks is called, the _listAttachedBodies of the object is already filled with the links of the robot though (because of the cloning) => object->IsAttached(any_robot_link) is TRUE => CheckCollision returns FALSE => this robot link is not ignored for collision.

I can't find a method to "clear" this _listAttachedBodies though. Any idea how to resolve this?

I see that the RobotStateSaver::Restore might resolve this as it copies grabbed bodies, but I ran into some problems and therefore didn't go this route (I remember there was a bug, too, some Null-Pointer Exception regarding an "empty" KinBodyPtr...I'll check again and report this).

Thanks!

Cheers,
Bahram

2014/10/08 18:55 , Bahram Maleki-Fard:

Thanks, that makes sense.

However, I just tested it and still have problems. I put a lot of debug-messages in Grabbed::_ProcessCollidingLinks and saw the following:
When Grab'ing a KinBody, collisions are correctly detected and ignored. So this is as intended and exactly what you described.
I use a second environment though for IK-checks and planning only (let's call it "planner-env", cloned once the robot has been added to first environment (let's call this one "viewer-env", as the viewer is attached to it). Before doing IK check, in the planner-env I do the following to update it:

1) ReleaseAllGrabbed()
2 delete all non-robot kinbodies
3) SetDOFValues(dofs), where "dofs" is the DOFS of the robot from viewer-environment
4) For each non-robot body in viewer-env, clone it to the planner-env with this code:

    KinBodyPtr new_body, empty;
    new_body = env->ReadKinBodyData(empty, "<KinBody></KinBody>");
    // is that the only way to create a new KinBody?
    new_body->Clone(old_body, 0); // "old_body" is the body in viewer_env
    env->Add(new_body);
    KinBody::KinBodyStateSaver save(old_body, 0xffffffff);
    saver.Restore(new_body);

5) For each manipulator and body in planner-env, grab the body if it is also grabbed by the same manipulator in viewer-environment

When executing the Grab in (5), I can see from the debug messages I put in _ProcessCollidingLinks that it fails to detect a collision between the finger-links and the grabbed object though.This was not the case when Grab-ing in viewer-environment.
I'm still looking into this, but do you have any idea why?

Thanks again.

Cheers,
Bahram

2014/10/08 08:19 , Rosen Diankov:

When calling RobotBase::Grab, it will record what links of the robot collide with the grabbed object, and ignore those collisions.

Most likely fingers are moving after you called Grab.
To fix this call RobotBase::RegrabAll() after you know the fingers stop moving.

hope this helps,

2014-10-08 4:01 GMT+09:00 Bahram Maleki-Fard <[hidden email]>:

Hi all,

the commit e5df33e8 from back in 2010 changed these lines of
RobotBase::CheckSelfCollision()

FOREACHC(itrobotlink,itbody->vNonCollidingLinks) {
- if(
GetEnv()->CheckCollision(*itrobotlink,KinBodyConstPtr(pbody),report) ) {
- bCollision = true;
- break;
+ // have to use link/link collision since link/body checks
attached bodies
+ FOREACHC(itbodylink,pbody->GetLinks()) {
+ if(
GetEnv()->CheckCollision(*itrobotlink,KinBody::LinkConstPtr(*itbodylink),report)
) {
+ bCollision = true;
+ break;
+ }
}
+ if( bCollision )
+ break;
}

What is the reason behind this?

We use OpenRAVE only for planning, not for controlling the arm. Which
means, we update the robot with the joints from the real arm, put
objects where we perceive them (e.g. by sensors), and plan.
The problem is, object shapes and robot model are not absolutely 100%
accurate. Now lets say we grab a cup, and close the fingers. The fingers
of the real arm wrap nicely around the real cup, but in the OpenRAVE
environment it is not that accurate. The fingers there are slightly
moving "into" the cylindrical object. Then we attach that object of
cours, for further planning.
Now the code from above comes into play. It will always fail, because
the finger links collide with the attached object. Whereas with the
previous version, that collision would be ignored, because the object is
attached to the parent-link of the fingers.

I can see that for other cases where the links are not initially in
collision with the grabbed object, we want to have the "new"
collision-check, otherwise there could be "real" collisions. But how can
I resolve the situation I am in? Do I have to disable the Link/object
with obj->Enable(false) ? Or is there a nicer way to do this?

Thanks a lot :)

Cheers,
Bahram

------------------------------------------------------------------------------
Meet PCI DSS 3.0 Compliance Requirements with EventLog Analyzer
Achieve PCI DSS 3.0 Compliant Status with Out-of-the-box PCI DSS Reports
Are you Audit-Ready for PCI DSS 3.0 Compliance? Download White paper
Comply to PCI DSS 3.0 Requirement 10 and 11.5 with EventLog Analyzer
http://pubads.g.doubleclick.net/gampad/clk?id=154622311&iu=/4140/ostg.clktrk
_______________________________________________
Openrave-users mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/openrave-users
------------------------------------------------------------------------------
Meet PCI DSS 3.0 Compliance Requirements with EventLog Analyzer
Achieve PCI DSS 3.0 Compliant Status with Out-of-the-box PCI DSS Reports
Are you Audit-Ready for PCI DSS 3.0 Compliance? Download White paper
Comply to PCI DSS 3.0 Requirement 10 and 11.5 with EventLog Analyzer
http://pubads.g.doubleclick.net/gampad/clk?id=154622311&iu=/4140/ostg.clktrk
_______________________________________________
Openrave-users mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/openrave-users