Error when Using Custom URDF

[emfebert]

Hi,

I'm trying to make the "glass_upright_example.cpp" work with a custom URDF (using an Xarm7).

I created a SRDF using the tesseract tool, and rviz seems to load the robot model just fine, however when trying to perform planning I'm getting the following error:

[ERROR] [1692826887.315279945]: Hit Enter to solve for trajectory.

[ERROR] [1692826893.059655973]: Hit enter key to continue!

terminate called after throwing an instance of 'std::runtime_error'
  what():  TypeErasureBase, tried to cast 'Dn' to 'N18tesseract_planning20CompositeInstructionE'!
[tesseract_ros_examples_glass_upright_example_node-3] process has died [pid 578163, exit code -6, cmd /home/febert/tesseract_ws2/devel/lib/tesseract_ros_examples/tesseract_ros_examples_glass_upright_example_node __name:=tesseract_ros_examples_glass_upright_example_node __log:=/home/febert/.ros/log/cfed852e-41fd-11ee-927b-578fc9b16ba7/tesseract_ros_examples_glass_upright_example_node-3.log].
log file: /home/febert/.ros/log/cfed852e-41fd-11ee-927b-578fc9b16ba7/tesseract_ros_examples_glass_upright_example_node-3*.log

I tried to inspect /home/febert/.ros/log/cfed852e-41fd-11ee-927b-578fc9b16ba7/tesseract_ros_examples_glass_upright_example_node-3*.log but the logfile isn't there.

The urdf file I'm using is here: https://github.com/emancro/tesseract/blob/master/tesseract_support/urdf/xarm7.urdf

And the srdf file is here: https://github.com/emancro/tesseract/blob/master/tesseract_support/urdf/xarm7.srdf

Thanks a lot!

[emfebert]

When trying to insert the same xarm7 URDF into "pick_and_place_example.cpp", rviz seems to load everthing fine, but I'm getting the following error:

[ERROR] [1692830967.156034151]: Hit enter key to continue!

Program: Composite Instruction, Description: Tesseract Composite Instruction
Program: {
Program:   Move Instruction, Move Type: 1, State WP: Pos=   0    0    0 1.57    0    0    0
, Description: Start Instruction
Program:   Move Instruction, Move Type: 1, Cart WP: xyz=0.15, 0.4, 1.127
, Description: From start to pick Approach
Program:   Move Instruction, Move Type: 0, Cart WP: xyz=0.15, 0.4, 0.977
, Description: Pick Approach
Program: }
Info:    Pick plan
Debug:   Profile 'DEFAULT' was not found in namespace 'MinLengthTask' for type 'N18tesseract_planning16MinLengthProfileE'. Using default if available. Available profiles:
Debug:   Environment, getJointGroup(manipulator) cache miss!
Debug:   Error
Debug:   Aborted
[ INFO] [1692830981.889913576]: Stopped publishing maintained environment.
[tesseract_ros_examples_pick_and_place_example_node-2] process has finished cleanly
log file: /home/febert/.ros/log/4fb36bc6-4207-11ee-a284-ebfc1f01d17c/tesseract_ros_examples_pick_and_place_example_node-2*.log

[Levi-Armstrong]

The issue is that your URDF does not conform to the ROS Industrial standard because it is missing link base, base_link and tool0.

[Levi-Armstrong]

The second issue is that the examples expect the group name to be manipulator which is not the name in your srdf.

[emfebert]

Hi Levi,

Thank you so much! I made those updates and I'm now getting a different error:

[ERROR] [1692911511.343784825]: Hit enter key to continue!

Program: Composite Instruction, Description: Tesseract Composite Instruction
Program: {
Program:   Move Instruction, Move Type: 1, State WP: Pos=0 0 0 0 0 0 0
, Description: Start Instruction
Program:   Move Instruction, Move Type: 1, Cart WP: xyz=0.15, 0.4, 1.127
, Description: From start to pick Approach
Program:   Move Instruction, Move Type: 0, Cart WP: xyz=0.15, 0.4, 0.977
, Description: Pick Approach
Program: }
Info:    Pick plan
Debug:   Profile 'DEFAULT' was not found in namespace 'MinLengthTask' for type 'N18tesseract_planning16MinLengthProfileE'. Using default if available. Available profiles:
Debug:   Environment, getJointGroup(manipulator) cache miss!
Debug:   Environment, getGroupJointNames(manipulator) cache miss!
Debug:   distances and parents:
Debug:   distance(base) = 0.000000, parent(base) = base_link
Debug:   distance(link7) = 1.086886, parent(link7) = link6
Debug:   distance(link6) = 0.963659, parent(link6) = link5
Debug:   distance(tool0) = 1.086886, parent(tool0) = link7
Debug:   distance(link3) = 0.560000, parent(link3) = link2
Debug:   distance(link5) = 0.963659, parent(link5) = link4
Debug:   distance(link1) = 0.267000, parent(link1) = base_link
Debug:   distance(link4) = 0.612500, parent(link4) = link3
Debug:   distance(base_link) = 0.000000, parent(base_link) = base_link
Debug:   distance(link2) = 0.267000, parent(link2) = link1
Debug:   distance(world) = 0.000000, parent(world) = base_link
Debug:   distance(box) = 0.439915, parent(box) = base_link
Debug:   Environment, getJointGroup(manipulator) cache hit!
Debug:   Environment, getKinematicGroup(manipulator, ) cache miss!
Debug:   Environment, getGroupJointNames(manipulator) cache hit!
Error:   SimplePlanner failed to generate problem: KinematicsPluginFactory, tried to get default inv kin solver for a group 'manipulator' that does not exist!.
         at line 93 in /home/febert/tesseract_ws2/src/tesseract_planning/tesseract_motion_planners/simple/src/simple_motion_planner.cpp
Error:   MinLengthTask, failed to subdivid!
         at line 132 in /home/febert/tesseract_ws2/src/tesseract_planning/tesseract_task_composer/planning/src/nodes/min_length_task.cpp
Debug:   Error
Debug:   Aborted
[ INFO] [1692911514.285239977]: Stopped publishing maintained environment.

Here are the updates urdf and srdf files: https://github.com/emancro/tesseract/blob/master/tesseract_support/urdf/xarm7.urdf https://github.com/emancro/tesseract/blob/master/tesseract_support/urdf/xarm7.srdf

Thanks again!

[Levi-Armstrong]

Sorry not near a computer, but you are missing entries in the srdf for the kinematics plugin yaml and contact checker plugins yaml. Take a look at the srdf that the original example was using and you can copy those and update them.

[emfebert]

Hi Levi,

I made the modifications that you mentioned and it seems to be doing something now.

When setting ifopt=true in the roslaunch file it will find a path around the sphere-obstacle, but at the end it still has slight intersection with between the collision geometry of the robot and the sphere as shown in the picture below:

When having ifopt=False it will generate a plan that runs almost in a straight line right through the obstacle:

Below is the output when using ifopt=True:

ERROR] [1693342592.283178449]: Hit Enter to solve for trajectory.

[OsqpEigen::Solver::solve] The solution is unfeasible.
Error:   Solver Failure
         at line 325 in /home/febert/tesseract_ws2/src/trajopt/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp
[OsqpEigen::Solver::solve] The solution is unfeasible.
Error:   Solver Failure
         at line 325 in /home/febert/tesseract_ws2/src/trajopt/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp
Warning: Approximate merit function got worse (-3.115e+00). (convexification is probably wrong to zeroth order)
         at line 218 in /home/febert/tesseract_ws2/src/trajopt/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp
Warning: Approximate merit function got worse (-3.425e+01). (convexification is probably wrong to zeroth order)
         at line 218 in /home/febert/tesseract_ws2/src/trajopt/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp
Warning: Approximate merit function got worse (-3.456e+02). (convexification is probably wrong to zeroth order)
         at line 218 in /home/febert/tesseract_ws2/src/trajopt/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp
Warning: Approximate merit function got worse (-3.459e+03). (convexification is probably wrong to zeroth order)
         at line 218 in /home/febert/tesseract_ws2/src/trajopt/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp
[ERROR] [1693342601.086112506]: Hit enter key to continue!
[ INFO] [1693342601.121406121]: Stopped publishing maintained environment.
[tesseract_ros_examples_xarm7_trajopt_node-3] process has finished cleanly
log file: /home/febert/.ros/log/88047578-46ae-11ee-a284-ebfc1f01d17c/tesseract_ros_examples_xarm7_trajopt_node-3*.log

I also noticed that the cartesian coefficients seems to be set to zero for x,y,z (copied below). What is the reason for this? Does it mean it doesn't try to find solutions that make the endeffector go to the target x,y,z precisely?

   plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 5);
    plan_profile->cartesian_coeff(0) = 0;
    plan_profile->cartesian_coeff(1) = 0;
    plan_profile->cartesian_coeff(2) = 0;

[Levi-Armstrong]

The ifopt version is a work in progress so I would recommend setting that to false and see if it succeeds.

The reason xyz is set to zero is that we don't want to constraint the position along the path so it can avoid the sphere but we do constrain the orientation so it maintains the same orientation.

[emfebert]

I'm trying to import a custom workcell environment, is there a way to use VHACD for convex composition? I saw it's used somewhere in the code.

Here's the orginial workcell geometry:

And this is the result:

[Levi-Armstrong]

Okay, yea you must have a environment that is decomposed because under the hood it converts everything to convex hulls.

[Levi-Armstrong]

@emfebert Can this be closed?