Typically this is caused by your URDF having some self collisions at the zero configuration. I suggest building the RobotTest app, then opening your URDF. If you check the “Self Collisions” box it will highlight self colliding links in red. You can also print the self colliding links using the button next to that checkbox, and get some text to copy and paste into the tag under the URDF to ignore those false positive self collisions.

Hi,
I have build the klampt lib from source and used the RobotTest app as you mentioned and I do see the collisions. But I'm not able to get the self colliding links as you mentioned. I tried with a URDF file and with a ROB file but both don't give me anything. Am I doing something wrong?
here you can find screenshots with the commands I used:

../klampt/bin/RobotTest robots/irb2600.urdf

../klampt/bin/RobotTest robots/irb2600.urdf

That's strange. Can you try setting DO_SIMPLIFY=0 at the top and wrapping plan.setEndpoints(...) in a try/catch block as follows:

try:
    plan.setEndpoints(configs[i],configs[i+1])
 except RuntimeError:
    #one of the configurations must be infeasible
    failed_start = [name for (test,name) in zip(plan.space.feasibilityTests,plan.space.feasibilityTestNames) if not(test(configs[i]))]
    failed_goal = [name for (test,name) in zip(plan.space.feasibilityTests,plan.space.feasibilityTestNames) if not(test(configs[i+1]))]
    print("Start configuration failed tests:",','.join(failed_start))
    print("Goal configuration failed tests:",','.join(failed_goal))
    raise

Hi,
Is there any documentation that explains how the planning_test script works?
I can´t seen to figure out what the "editing plan config" windows do and what the point is of the "editing IK solved configs" windows...
I did edit the code as you asked but because I don't seem to understand how the script works, I always get different errors:

=============================================================================
Start configuration failed tests: closed loop constraint
Goal configuration failed tests: closed loop constraint
=============================================================================
RuntimeError: Start configuration is infeasible

=============================================================================
Start configuration failed tests: 
Goal configuration failed tests: closed loop constraint
=============================================================================
RuntimeError: Goal configuration is infeasible

Failed to generate a plan

Could you also please check my other github issue regarding the robotplanning wizard?

Kind regards,
Benoît

coming back to the RobotTest program showing collision lines on the robot arm, I tried the ur5.rob file from the library and here you can also see red lines.

• Are those red lines (first image) indicating that there are collisions or are there only collisions when the visual parts of the robot are red (second image) ?

Ah, the planning_test.py has a bunch of flags at the top of the file to test more complex version of the motion planning functions, and these were set to 1 in the committed version. They should be set to 0. If you git pull, the most recent planning_test.py will hopefully be more clear.

The red lines in RobotTest indicate the self collision tests that would be run when calling robot.selfCollides() or using the standard motion planning functions. In the second image the robot has a number of links that are already self colliding (red), so this suggests that you should Print Collisions and copy the results into the URDF.

I'll get back to the wizard at some point -- that code is definitely incomplete and should probably be removed for now.

Hi,
Thanks a lot for the clarification! I will pull the latest version and check that out! Regarding the wizard, it's good to know that it is not done yet, so I don't have to scratch my head about that problem anymore.

Kind regards,

Benoît

Hi,

Is the CLOSED_LOOP_TEST functionality of the planning_test.py script working correctly or is it also still being developed? If it is in fact not finished, is there any other way to setup a motion planner with multiple constraints?

The problem I'm facing right now:

Instead of setting a coordinate as a ik.objective as showed in the example script:

obj = ik.objective(robot.link(robot.numLinks()-1),local=[0,0,0],world=[0.5,0,0.5])

I would like to constraint the angle of the endeffector like this:

obj = ik.objective(robot.link(linkindex),R=so3.from_axis_angle(([0,1,0],(toolAngle*(math.pi/180)))),t=t)

But this doesn´t seem to be working (while it does work when just setting up IK).

Kind regards,

Benoît Lagasse