Default planning time #402

allicen · 2024-05-01T21:21:23Z

How can I change the planning time? Set more than 5 seconds

Thank you!

==================
Optimization results:
status: OPT_PENALTY_ITERATION_LIMIT
cost values: (2.90515e-26, 4.2114e-23, 0, 0, 0, 0.00376514, 0.250181, 2.09396, 1.40071, 0.78795, 0, 0, 0.642342)
constraint violations: (2.90515e-26, 4.2114e-23, 0, 0, 0, 0.00376514, 0.250181, 2.09396, 1.40071, 0.78795, 0, 0)
n func evals: 20
n qp solves: 21
==================
Info:    TrajOptMotionPlannerTask motion planning failed (Failed to find valid solution: OPT_PENALTY_ITERATION_LIMIT) for process input: Tesseract Composite Instruction
Debug:   Error
Info:    Planning took 5.572277 seconds.

The text was updated successfully, but these errors were encountered:

marip8 · 2024-05-01T23:01:36Z

I don't think trajopt is timing out here. The specific error that you are seeing here is a "penalty iteration limit", which means that:

the constraints of your optimization problem could not be satisfied
trajopt increased the weight of their error (i.e., penalty) in the optimization by an order of magnitude
the optimization still did not converge, so trajopt repeated this process the maximum number of times before concluding that the optimzation could not be solved

This whole process coincidentally took about 5 seconds, but I don't think it technically timed out. You can change the penalty iteration limit in one of the trajopt profiles, but I doubt increasing it will solve your problem

Lots of things can cause constraints to not be satisfied, resulting in this error. If you could post the contents of the last iteration of the trajopt log with the list of costs/constraints and their values, we might be able to help you resolve this issue

allicen · 2024-05-05T20:16:00Z

Thanks for the answer! This allowed me to better understand the situation.

I am attaching the logs.
trajopt_constraints.log
trajopt_costs.log
trajopt_vars.log
trajopt_solver.log

Logs from the terminal:

Debug:   generating program
Program: Composite Instruction, Description: Tesseract Composite Instruction
Program: {
Program:   Move Instruction, Move Type: 1, State WP: Pos=     1.52647    -0.553581      1.68679     -2.68775     -1.46159 -0.000419442
, Description: Start Instruction
Program:   Move Instruction, Move Type: 1, State WP: Pos=     1.54235     -1.20335     0.634284     -1.13847      -1.5738 -0.000371281
, Description: freespace_finish_plan
Program: }
[ INFO] [1714939743.165975455]: Planner task name - TrajOptPipeline
Debug:   Profile 'FREESPACE' 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(ur5_wrist_2_link) = 1.016989, parent(ur5_wrist_2_link) = ur5_wrist_1_link
Debug:   distance(gripper_finger2_knuckle_link) = 1.177339, parent(gripper_finger2_knuckle_link) = gripper_base_link
Debug:   distance(ur5_ee_link) = 1.114483, parent(ur5_ee_link) = ur5_wrist_3_link
Debug:   distance(top_chassis_link) = 0.543339, parent(top_chassis_link) = base_link
Debug:   distance(ur5_upper_arm_link) = 0.089640, parent(ur5_upper_arm_link) = ur5_shoulder_link
Debug:   distance(ur5_wrist_3_link) = 1.099483, parent(ur5_wrist_3_link) = ur5_wrist_2_link
Debug:   distance(hand_eye_camera_link) = 1.156131, parent(hand_eye_camera_link) = ur5_tool0
Debug:   distance(top_plate_rear_link) = 0.716219, parent(top_plate_rear_link) = top_plate_link
Debug:   distance(rear_right_wheel) = 0.928133, parent(rear_right_wheel) = base_link
Debug:   distance(realsense_gripper_left_ir_frame) = 1.177637, parent(realsense_gripper_left_ir_frame) = realsense_gripper_depth_frame
Debug:   distance(front_bumper_link) = 1.031889, parent(front_bumper_link) = base_link
Debug:   distance(realsense_gripper_depth_optical_frame) = 1.177637, parent(realsense_gripper_depth_optical_frame) = realsense_gripper_depth_frame
Debug:   distance(top_plate_front_link) = 0.716219, parent(top_plate_front_link) = top_plate_link
Debug:   distance(ur5_wrist_1_link) = 0.922214, parent(ur5_wrist_1_link) = ur5_forearm_link
Debug:   distance(realsense_gripper_link) = 1.177637, parent(realsense_gripper_link) = realsense_gripper_bottom_screw_frame
Debug:   distance(front_left_wheel) = 0.928133, parent(front_left_wheel) = base_link
Debug:   distance(ur5_tool0) = 1.099483, parent(ur5_tool0) = ur5_wrist_3_link
Debug:   distance(realsense_gripper_right_ir_optical_frame) = 1.227637, parent(realsense_gripper_right_ir_optical_frame) = realsense_gripper_right_ir_frame
Debug:   distance(ur5_base) = 0.300000, parent(ur5_base) = ur5_base_link
Debug:   distance(front_right_wheel) = 0.928133, parent(front_right_wheel) = base_link
Debug:   distance(rear_bumper_link) = 1.031889, parent(rear_bumper_link) = base_link
Debug:   distance(sensor_arch_mount_link) = 0.905744, parent(sensor_arch_mount_link) = top_plate_link
Debug:   distance(gripper_finger1_knuckle_link) = 1.177339, parent(gripper_finger1_knuckle_link) = gripper_base_link
Debug:   distance(base_link) = 0.543339, parent(base_link) = top_plate_link
Debug:   distance(ur5_shoulder_link) = 0.089564, parent(ur5_shoulder_link) = ur5_base_link
Debug:   distance(realsense_gripper_color_optical_frame) = 1.192637, parent(realsense_gripper_color_optical_frame) = realsense_gripper_color_frame
Debug:   distance(gripper_base_link) = 1.114483, parent(gripper_base_link) = ur5_ee_link
Debug:   distance(gripper_finger2_finger_link) = 1.209089, parent(gripper_finger2_finger_link) = gripper_finger2_knuckle_link
Debug:   distance(bumper_link) = 0.543339, parent(bumper_link) = base_link
Debug:   distance(gripper_finger1_finger_link) = 1.209089, parent(gripper_finger1_finger_link) = gripper_finger1_knuckle_link
Debug:   distance(base_footprint) = 0.660619, parent(base_footprint) = base_link
Debug:   distance(gripper_finger1_inner_knuckle_link) = 1.177202, parent(gripper_finger1_inner_knuckle_link) = gripper_base_link
Debug:   distance(gripper_finger2_inner_knuckle_link) = 1.177202, parent(gripper_finger2_inner_knuckle_link) = gripper_base_link
Debug:   distance(realsense_gripper_color_frame) = 1.192637, parent(realsense_gripper_color_frame) = realsense_gripper_depth_frame
Debug:   distance(realsense_gripper_right_ir_frame) = 1.227637, parent(realsense_gripper_right_ir_frame) = realsense_gripper_depth_frame
Debug:   distance(realsense_gripper_left_ir_optical_frame) = 1.177637, parent(realsense_gripper_left_ir_optical_frame) = realsense_gripper_left_ir_frame
Debug:   distance(ur5_base_link) = 0.000000, parent(ur5_base_link) = ur5_base_link
Debug:   distance(ur5_forearm_link) = 0.514912, parent(ur5_forearm_link) = ur5_upper_arm_link
Debug:   distance(realsense_gripper_bottom_screw_frame) = 1.156131, parent(realsense_gripper_bottom_screw_frame) = hand_eye_camera_link
Debug:   distance(rear_left_wheel) = 0.928133, parent(rear_left_wheel) = base_link
Debug:   distance(gripper_finger2_finger_tip_link) = 1.234352, parent(gripper_finger2_finger_tip_link) = gripper_finger2_inner_knuckle_link
Debug:   distance(gripper_finger1_finger_tip_link) = 1.234352, parent(gripper_finger1_finger_tip_link) = gripper_finger1_inner_knuckle_link
Debug:   distance(top_plate_link) = 0.303665, parent(top_plate_link) = ur5_base_link
Debug:   distance(realsense_gripper_depth_frame) = 1.177637, parent(realsense_gripper_depth_frame) = realsense_gripper_link
Debug:   distance(inertial_link) = 0.543339, parent(inertial_link) = base_link
Debug:   distance(world) = 0.543339, parent(world) = base_link
Debug:   distance(box_merge_0) = 0.543339, parent(box_merge_0) = world
Debug:   distance(box_merge_1) = 0.543339, parent(box_merge_1) = world
Debug:   distance(box_merge_2) = 0.543339, parent(box_merge_2) = world
Debug:   distance(box_merge_3) = 0.543339, parent(box_merge_3) = world
Debug:   distance(box_merge_4) = 0.543339, parent(box_merge_4) = world
Debug:   distance(box_merge_5) = 0.543339, parent(box_merge_5) = world
Debug:   distance(box_merge_6) = 0.543339, parent(box_merge_6) = world
Debug:   distance(box_merge_7) = 0.543339, parent(box_merge_7) = world
Debug:   distance(box_merge_8) = 0.543339, parent(box_merge_8) = world
Debug:   distance(box_merge_9) = 0.543339, parent(box_merge_9) = world
Debug:   distance(box_merge_10) = 0.543339, parent(box_merge_10) = world
Debug:   distance(box_merge_11) = 0.543339, parent(box_merge_11) = world
Debug:   distance(box_merge_12) = 0.543339, parent(box_merge_12) = world
Debug:   distance(box_merge_13) = 0.543339, parent(box_merge_13) = world
Debug:   distance(box_merge_14) = 0.543339, parent(box_merge_14) = world
Debug:   distance(box_merge_15) = 0.543339, parent(box_merge_15) = world
Debug:   distance(box_merge_16) = 0.543339, parent(box_merge_16) = world
Debug:   distance(box_merge_17) = 0.543339, parent(box_merge_17) = world
Debug:   distance(box_merge_18) = 0.543339, parent(box_merge_18) = world
Debug:   distance(box_merge_19) = 0.543339, parent(box_merge_19) = world
Debug:   distance(box_merge_20) = 0.543339, parent(box_merge_20) = world
Debug:   distance(box_merge_21) = 0.543339, parent(box_merge_21) = world
Debug:   distance(box_merge_22) = 0.543339, parent(box_merge_22) = world
Debug:   distance(box_merge_23) = 0.543339, parent(box_merge_23) = world
Debug:   distance(box_merge_24) = 0.543339, parent(box_merge_24) = world
Debug:   distance(box_merge_25) = 0.543339, parent(box_merge_25) = world
Debug:   distance(box_merge_26) = 0.543339, parent(box_merge_26) = world
Debug:   distance(box_merge_27) = 0.543339, parent(box_merge_27) = world
Debug:   distance(box_merge_28) = 0.543339, parent(box_merge_28) = world
Debug:   distance(box_merge_29) = 0.543339, parent(box_merge_29) = world
Debug:   distance(box_merge_30) = 0.543339, parent(box_merge_30) = world
Debug:   distance(box_merge_31) = 0.543339, parent(box_merge_31) = world
Debug:   distance(box_merge_32) = 0.543339, parent(box_merge_32) = world
Debug:   distance(box_merge_33) = 0.543339, parent(box_merge_33) = world
Debug:   distance(box_merge_34) = 0.543339, parent(box_merge_34) = world
Debug:   distance(box_merge_35) = 0.543339, parent(box_merge_35) = world
Debug:   distance(box_merge_36) = 0.543339, parent(box_merge_36) = world
Debug:   distance(box_merge_37) = 0.543339, parent(box_merge_37) = world
Debug:   distance(box_merge_38) = 0.543339, parent(box_merge_38) = world
Debug:   distance(box_merge_39) = 0.543339, parent(box_merge_39) = world
Debug:   distance(box_merge_40) = 0.543339, parent(box_merge_40) = world
Debug:   distance(box_merge_41) = 0.543339, parent(box_merge_41) = world
Debug:   distance(box_merge_42) = 0.543339, parent(box_merge_42) = world
Debug:   distance(box_merge_43) = 0.543339, parent(box_merge_43) = world
Debug:   distance(box_merge_44) = 0.543339, parent(box_merge_44) = world
Debug:   distance(box_merge_45) = 0.543339, parent(box_merge_45) = world
Debug:   distance(box_merge_46) = 0.543339, parent(box_merge_46) = world
Debug:   distance(box_merge_47) = 0.543339, parent(box_merge_47) = world
Debug:   distance(box_merge_48) = 0.543339, parent(box_merge_48) = world
Debug:   distance(box_merge_49) = 0.543339, parent(box_merge_49) = world
Debug:   distance(box_merge_50) = 0.543339, parent(box_merge_50) = world
Debug:   distance(box_merge_51) = 0.543339, parent(box_merge_51) = world
Debug:   distance(box_merge_52) = 0.543339, parent(box_merge_52) = world
Debug:   distance(box_merge_53) = 0.543339, parent(box_merge_53) = world
Debug:   distance(box_merge_54) = 0.543339, parent(box_merge_54) = world
Debug:   distance(box_merge_55) = 0.543339, parent(box_merge_55) = world
Debug:   distance(box_merge_56) = 0.543339, parent(box_merge_56) = world
Debug:   distance(box_merge_57) = 0.543339, parent(box_merge_57) = world
Debug:   distance(box_merge_58) = 0.543339, parent(box_merge_58) = world
Debug:   distance(box_merge_59) = 0.543339, parent(box_merge_59) = world
Debug:   distance(box_merge_60) = 0.543339, parent(box_merge_60) = world
Debug:   distance(box_merge_61) = 0.543339, parent(box_merge_61) = world
Debug:   distance(box_merge_62) = 0.543339, parent(box_merge_62) = world
Debug:   distance(box_merge_63) = 0.543339, parent(box_merge_63) = world
Debug:   distance(box_merge_64) = 0.543339, parent(box_merge_64) = world
Debug:   distance(box_merge_65) = 0.543339, parent(box_merge_65) = world
Debug:   distance(box_merge_66) = 0.543339, parent(box_merge_66) = world
Debug:   distance(box_merge_67) = 0.543339, parent(box_merge_67) = world
Debug:   distance(box_merge_68) = 0.543339, parent(box_merge_68) = world
Debug:   distance(box_merge_69) = 0.543339, parent(box_merge_69) = world
Debug:   distance(box_merge_70) = 0.543339, parent(box_merge_70) = world
Debug:   distance(box_merge_71) = 0.543339, parent(box_merge_71) = world
Debug:   distance(box_merge_72) = 0.543339, parent(box_merge_72) = world
Debug:   distance(box_merge_73) = 0.543339, parent(box_merge_73) = world
Debug:   distance(box_merge_74) = 0.543339, parent(box_merge_74) = world
Debug:   distance(box_merge_75) = 0.543339, parent(box_merge_75) = world
Debug:   distance(box_merge_76) = 0.543339, parent(box_merge_76) = world
Debug:   distance(box_merge_77) = 0.543339, parent(box_merge_77) = world
Debug:   distance(box_merge_78) = 0.543339, parent(box_merge_78) = world
Debug:   distance(box_merge_79) = 0.543339, parent(box_merge_79) = world
Debug:   distance(box_merge_80) = 0.543339, parent(box_merge_80) = world
Debug:   distance(box_merge_81) = 0.543339, parent(box_merge_81) = world
Debug:   distance(box_merge_82) = 0.543339, parent(box_merge_82) = world
Debug:   distance(box_merge_83) = 0.543339, parent(box_merge_83) = world
Debug:   distance(box_merge_84) = 0.543339, parent(box_merge_84) = world
Debug:   distance(box_merge_85) = 0.543339, parent(box_merge_85) = world
Debug:   distance(box_merge_86) = 0.543339, parent(box_merge_86) = world
Debug:   distance(box_merge_87) = 0.543339, parent(box_merge_87) = world
Debug:   distance(box_merge_88) = 0.543339, parent(box_merge_88) = world
Debug:   distance(box_merge_89) = 0.543339, parent(box_merge_89) = world
Debug:   distance(box_merge_90) = 0.543339, parent(box_merge_90) = world
Debug:   distance(box_merge_91) = 0.543339, parent(box_merge_91) = world
Debug:   distance(box_merge_92) = 0.543339, parent(box_merge_92) = world
Debug:   distance(box_merge_93) = 0.543339, parent(box_merge_93) = world
Debug:   distance(box_merge_94) = 0.543339, parent(box_merge_94) = world
Debug:   distance(box_merge_95) = 0.543339, parent(box_merge_95) = world
Debug:   Environment, getJointGroup(manipulator) cache hit!
Debug:   Environment, getKinematicGroup(manipulator, ) cache miss!
Debug:   Environment, getGroupJointNames(manipulator) cache hit!
Debug:   Environment, getKinematicGroup(manipulator, ) cache hit!
Debug:   Seed Min Length Task Succeeded!
Debug:   Environment, getKinematicGroup(manipulator, ) cache hit!
Debug:   Profile 'FREESPACE' was not found in namespace 'TrajOptMotionPlannerTask' for type 'N18tesseract_planning20TrajOptSolverProfileE'. Using default if available. Available profiles:
Info:    1 JointVelTermInfo coeffs given. Applying to all 6 joints
Info:    1 JointVelTermInfo targets given. Applying to all 6 joints
[INFO] iteration 1
[INFO]  

| ======================================================================================== |
|                                      ROS Industrial                                      |
|                                  TrajOpt Motion Planning                                 |
| ======================================================================================== |
|      merit |   oldexact |  new_exact | new_approx |    dapprox |     dexact |      ratio |
| ---------------------------------------------------------------------------------------- | COSTS
| ---------- |  0.000e+00 |  7.731e-24 |  4.049e-15 | -4.049e-15 | -7.731e-24 | ---------- | joint_waypoint_0 
| ---------- |  0.000e+00 |  7.418e-22 | -4.691e-15 |  4.691e-15 | -7.418e-22 | ---------- | joint_waypoint_10 
| ---------- |  0.000e+00 |  0.000e+00 |  2.060e-11 | -2.060e-11 |  0.000e+00 | ---------- | collision_0     
| ---------- |  0.000e+00 |  0.000e+00 |  1.353e-09 | -1.353e-09 |  0.000e+00 | ---------- | collision_1     
| ---------- |  9.508e-02 |  0.000e+00 |  1.369e-09 |  9.508e-02 |  9.508e-02 |  1.000e+00 | collision_2     
| ---------- |  5.800e-01 |  1.009e-01 |  1.389e-01 |  4.411e-01 |  4.791e-01 |  1.086e+00 | collision_3     
| ---------- |  1.694e+00 |  1.043e+00 |  1.093e+00 |  6.010e-01 |  6.514e-01 |  1.084e+00 | collision_4     
| ---------- |  2.244e+00 |  2.335e+00 |  1.691e+00 |  5.528e-01 | -9.073e-02 | -1.641e-01 | collision_5     
| ---------- |  1.808e+00 |  1.585e+00 |  1.530e+00 |  2.779e-01 |  2.223e-01 |  8.000e-01 | collision_6     
| ---------- |  1.276e+00 |  1.315e+00 |  9.583e-01 |  3.180e-01 | -3.873e-02 | -1.218e-01 | collision_7     
| ---------- |  2.161e-01 |  0.000e+00 |  1.014e-01 |  1.147e-01 |  2.161e-01 |  1.884e+00 | collision_8     
| ---------- |  0.000e+00 |  0.000e+00 |  4.756e-11 | -4.756e-11 |  0.000e+00 | ---------- | collision_9     
| ---------- |  3.943e-01 |  5.199e-01 |  5.199e-01 | -1.256e-01 | -1.256e-01 |  1.000e+00 | joint_vel_cost  
| ======================================================================================== |
| ---------- |  8.307e+00 |  6.898e+00 |  6.032e+00 | ---------- | ---------- | ---------- | SUM COSTS
| ======================================================================================== |
| ---------------------------------------------------------------------------------------- | CONSTRAINTS
|  1.000e+01 |  0.000e+00 |  7.731e-23 |  1.004e-10 | -1.004e-10 | -7.731e-23 | ---------- | joint_waypoint_0 
|  1.000e+01 |  0.000e+00 |  7.418e-21 |  1.189e-09 | -1.189e-09 | -7.418e-21 | ---------- | joint_waypoint_10 
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_0     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_1     
|  1.000e+01 |  9.508e-01 |  0.000e+00 |  0.000e+00 |  9.508e-01 |  9.508e-01 |  1.000e+00 | collision_2     
|  1.000e+01 |  5.800e+00 |  1.009e+00 |  1.389e+00 |  4.411e+00 |  4.791e+00 |  1.086e+00 | collision_3     
|  1.000e+01 |  1.694e+01 |  1.043e+01 |  1.093e+01 |  6.010e+00 |  6.514e+00 |  1.084e+00 | collision_4     
|  1.000e+01 |  2.244e+01 |  2.335e+01 |  1.691e+01 |  5.528e+00 | -9.073e-01 | -1.641e-01 | collision_5     
|  1.000e+01 |  1.808e+01 |  1.585e+01 |  1.530e+01 |  2.779e+00 |  2.223e+00 |  8.000e-01 | collision_6     
|  1.000e+01 |  1.276e+01 |  1.315e+01 |  9.583e+00 |  3.180e+00 | -3.873e-01 | -1.218e-01 | collision_7     
|  1.000e+01 |  2.161e+00 |  0.000e+00 |  1.014e+00 |  1.147e+00 |  2.161e+00 |  1.884e+00 | collision_8     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_9     
| ======================================================================================== |
| ---------- |  7.913e+00 |  6.378e+00 |  5.512e+00 | ---------- | ---------- | ---------- | SUM CONSTRAINTS (WITHOUT MERIT) 
| ======================================================================================== |
| ---------- |  8.744e+01 |  7.068e+01 | ---------- |  2.628e+01 |  1.675e+01 |  6.375e-01 | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH MERIT)
| ======================================================================================== |
[INFO] expanded trust region. new box size: 0.1500
[INFO] iteration 2
[INFO]  

| ======================================================================================== |
|                                      ROS Industrial                                      |
|                                  TrajOpt Motion Planning                                 |
| ======================================================================================== |
|      merit |   oldexact |  new_exact | new_approx |    dapprox |     dexact |      ratio |
| ---------------------------------------------------------------------------------------- | COSTS
| ---------- |  7.731e-24 |  4.230e-23 |  1.647e-14 | -1.647e-14 | -3.456e-23 | ---------- | joint_waypoint_0 
| ---------- |  7.418e-22 |  2.032e-21 | -4.517e-15 |  4.517e-15 | -1.291e-21 | ---------- | joint_waypoint_10 
| ---------- |  0.000e+00 |  0.000e+00 |  1.774e-10 | -1.774e-10 |  0.000e+00 | ---------- | collision_0     
| ---------- |  0.000e+00 |  0.000e+00 |  8.030e-11 | -8.030e-11 |  0.000e+00 | ---------- | collision_1     
| ---------- |  0.000e+00 |  0.000e+00 | -1.025e-10 |  1.025e-10 |  0.000e+00 | ---------- | collision_2     
| ---------- |  1.009e-01 |  0.000e+00 |  1.732e-08 |  1.009e-01 |  1.009e-01 |  1.000e+00 | collision_3     
| ---------- |  1.043e+00 |  2.291e-01 |  3.738e-01 |  6.687e-01 |  8.134e-01 |  1.216e+00 | collision_4     
| ---------- |  2.335e+00 |  3.299e+00 |  2.033e+00 |  3.018e-01 | -9.645e-01 | -3.195e+00 | collision_5     
| ---------- |  1.585e+00 |  1.470e+00 |  1.365e+00 |  2.206e-01 |  1.156e-01 |  5.240e-01 | collision_6     
| ---------- |  1.315e+00 |  1.044e+00 |  1.099e+00 |  2.156e-01 |  2.709e-01 |  1.257e+00 | collision_7     
| ---------- |  0.000e+00 |  0.000e+00 |  9.344e-10 | -9.344e-10 |  0.000e+00 | ---------- | collision_8     
| ---------- |  0.000e+00 |  0.000e+00 | -2.636e-12 |  2.636e-12 |  0.000e+00 | ---------- | collision_9     
| ---------- |  5.199e-01 |  8.060e-01 |  8.060e-01 | -2.861e-01 | -2.861e-01 |  1.000e+00 | joint_vel_cost  
| ======================================================================================== |
| ---------- |  6.898e+00 |  6.848e+00 |  5.677e+00 | ---------- | ---------- | ---------- | SUM COSTS
| ======================================================================================== |
| ---------------------------------------------------------------------------------------- | CONSTRAINTS
|  1.000e+01 |  7.731e-23 |  4.230e-22 |  1.609e-10 | -1.609e-10 | -3.456e-22 | ---------- | joint_waypoint_0 
|  1.000e+01 |  7.418e-21 |  2.032e-20 |  1.835e-09 | -1.835e-09 | -1.291e-20 | ---------- | joint_waypoint_10 
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_0     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_1     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_2     
|  1.000e+01 |  1.009e+00 |  0.000e+00 |  0.000e+00 |  1.009e+00 |  1.009e+00 |  1.000e+00 | collision_3     
|  1.000e+01 |  1.043e+01 |  2.291e+00 |  3.738e+00 |  6.687e+00 |  8.134e+00 |  1.216e+00 | collision_4     
|  1.000e+01 |  2.335e+01 |  3.299e+01 |  2.033e+01 |  3.018e+00 | -9.645e+00 | -3.196e+00 | collision_5     
|  1.000e+01 |  1.585e+01 |  1.470e+01 |  1.365e+01 |  2.205e+00 |  1.156e+00 |  5.241e-01 | collision_6     
|  1.000e+01 |  1.315e+01 |  1.044e+01 |  1.099e+01 |  2.156e+00 |  2.709e+00 |  1.257e+00 | collision_7     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_8     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_9     
| ======================================================================================== |
| ---------- |  6.378e+00 |  6.042e+00 |  4.871e+00 | ---------- | ---------- | ---------- | SUM CONSTRAINTS (WITHOUT MERIT) 
| ======================================================================================== |
| ---------- |  7.068e+01 |  6.727e+01 | ---------- |  1.630e+01 |  3.413e+00 |  2.094e-01 | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH MERIT)
| ======================================================================================== |
[INFO] shrunk trust region. new box size: 0.0150
[INFO]  

| ======================================================================================== |
|                                      ROS Industrial                                      |
|                                  TrajOpt Motion Planning                                 |
| ======================================================================================== |
|      merit |   oldexact |  new_exact | new_approx |    dapprox |     dexact |      ratio |
| ---------------------------------------------------------------------------------------- | COSTS
| ---------- |  7.731e-24 |  5.792e-23 |  1.644e-14 | -1.644e-14 | -5.019e-23 | ---------- | joint_waypoint_0 
| ---------- |  7.418e-22 |  7.903e-21 |  2.031e-15 | -2.031e-15 | -7.161e-21 | ---------- | joint_waypoint_10 
| ---------- |  0.000e+00 |  0.000e+00 | -4.126e-12 |  4.126e-12 |  0.000e+00 | ---------- | collision_0     
| ---------- |  0.000e+00 |  0.000e+00 | -7.200e-12 |  7.200e-12 |  0.000e+00 | ---------- | collision_1     
| ---------- |  0.000e+00 |  0.000e+00 | -3.209e-11 |  3.209e-11 |  0.000e+00 | ---------- | collision_2     
| ---------- |  1.009e-01 |  6.644e-02 |  6.683e-02 |  3.412e-02 |  3.451e-02 |  1.011e+00 | collision_3     
| ---------- |  1.043e+00 |  8.070e-01 |  8.359e-01 |  2.067e-01 |  2.355e-01 |  1.140e+00 | collision_4     
| ---------- |  2.335e+00 |  2.176e+00 |  2.214e+00 |  1.204e-01 |  1.590e-01 |  1.320e+00 | collision_5     
| ---------- |  1.585e+00 |  1.502e+00 |  1.510e+00 |  7.583e-02 |  8.375e-02 |  1.104e+00 | collision_6     
| ---------- |  1.315e+00 |  1.112e+00 |  1.161e+00 |  1.537e-01 |  2.035e-01 |  1.324e+00 | collision_7     
| ---------- |  0.000e+00 |  0.000e+00 |  7.830e-10 | -7.830e-10 |  0.000e+00 | ---------- | collision_8     
| ---------- |  0.000e+00 |  0.000e+00 |  2.004e-10 | -2.004e-10 |  0.000e+00 | ---------- | collision_9     
| ---------- |  5.199e-01 |  5.295e-01 |  5.295e-01 | -9.580e-03 | -9.580e-03 |  1.000e+00 | joint_vel_cost  
| ======================================================================================== |
| ---------- |  6.898e+00 |  6.192e+00 |  6.317e+00 | ---------- | ---------- | ---------- | SUM COSTS
| ======================================================================================== |
| ---------------------------------------------------------------------------------------- | CONSTRAINTS
|  1.000e+01 |  7.731e-23 |  5.792e-22 |  2.596e-10 | -2.596e-10 | -5.019e-22 | ---------- | joint_waypoint_0 
|  1.000e+01 |  7.418e-21 |  7.903e-20 |  2.639e-09 | -2.639e-09 | -7.161e-20 | ---------- | joint_waypoint_10 
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_0     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_1     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_2     
|  1.000e+01 |  1.009e+00 |  6.644e-01 |  6.683e-01 |  3.412e-01 |  3.451e-01 |  1.011e+00 | collision_3     
|  1.000e+01 |  1.043e+01 |  8.070e+00 |  8.359e+00 |  2.067e+00 |  2.355e+00 |  1.140e+00 | collision_4     
|  1.000e+01 |  2.335e+01 |  2.176e+01 |  2.214e+01 |  1.204e+00 |  1.590e+00 |  1.321e+00 | collision_5     
|  1.000e+01 |  1.585e+01 |  1.502e+01 |  1.510e+01 |  7.578e-01 |  8.375e-01 |  1.105e+00 | collision_6     
|  1.000e+01 |  1.315e+01 |  1.112e+01 |  1.161e+01 |  1.537e+00 |  2.035e+00 |  1.324e+00 | collision_7     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_8     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_9     
| ======================================================================================== |
| ---------- |  6.378e+00 |  5.662e+00 |  5.788e+00 | ---------- | ---------- | ---------- | SUM CONSTRAINTS (WITHOUT MERIT) 
| ======================================================================================== |
| ---------- |  7.068e+01 |  6.281e+01 | ---------- |  6.487e+00 |  7.870e+00 |  1.213e+00 | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH MERIT)
| ======================================================================================== |
[INFO] expanded trust region. new box size: 0.0225
[INFO] iteration 3
[INFO]  

| ======================================================================================== |
|                                      ROS Industrial                                      |
|                                  TrajOpt Motion Planning                                 |
| ======================================================================================== |
|      merit |   oldexact |  new_exact | new_approx |    dapprox |     dexact |      ratio |
| ---------------------------------------------------------------------------------------- | COSTS
| ---------- |  5.792e-23 |  5.439e-22 |  1.902e-14 | -1.902e-14 | -4.860e-22 | ---------- | joint_waypoint_0 
| ---------- |  7.903e-21 |  2.961e-21 | -2.877e-15 |  2.877e-15 |  4.942e-21 | ---------- | joint_waypoint_10 
| ---------- |  0.000e+00 |  0.000e+00 | -1.466e-11 |  1.466e-11 |  0.000e+00 | ---------- | collision_0     
| ---------- |  0.000e+00 |  0.000e+00 | -1.366e-11 |  1.366e-11 |  0.000e+00 | ---------- | collision_1     
| ---------- |  0.000e+00 |  0.000e+00 | -2.210e-12 |  2.210e-12 |  0.000e+00 | ---------- | collision_2     
| ---------- |  6.644e-02 |  3.290e-02 |  3.275e-02 |  3.369e-02 |  3.354e-02 |  9.956e-01 | collision_3     
| ---------- |  8.070e-01 |  5.753e-01 |  6.105e-01 |  1.965e-01 |  2.317e-01 |  1.179e+00 | collision_4     
| ---------- |  2.176e+00 |  1.974e+00 |  2.031e+00 |  1.444e-01 |  2.017e-01 |  1.397e+00 | collision_5     
| ---------- |  1.502e+00 |  1.458e+00 |  1.462e+00 |  4.006e-02 |  4.376e-02 |  1.092e+00 | collision_6     
| ---------- |  1.112e+00 |  1.019e+00 |  1.058e+00 |  5.351e-02 |  9.291e-02 |  1.736e+00 | collision_7     
| ---------- |  0.000e+00 |  0.000e+00 | -1.803e-10 |  1.803e-10 |  0.000e+00 | ---------- | collision_8     
| ---------- |  0.000e+00 |  0.000e+00 |  1.126e-10 | -1.126e-10 |  0.000e+00 | ---------- | collision_9     
| ---------- |  5.295e-01 |  5.535e-01 |  5.535e-01 | -2.401e-02 | -2.401e-02 |  1.000e+00 | joint_vel_cost  
| ======================================================================================== |
| ---------- |  6.192e+00 |  5.612e+00 |  5.747e+00 | ---------- | ---------- | ---------- | SUM COSTS
| ======================================================================================== |
| ---------------------------------------------------------------------------------------- | CONSTRAINTS
|  1.000e+01 |  5.792e-22 |  5.439e-21 |  5.593e-10 | -5.593e-10 | -4.860e-21 | ---------- | joint_waypoint_0 
|  1.000e+01 |  7.903e-20 |  2.961e-20 |  1.414e-09 | -1.414e-09 |  4.942e-20 | ---------- | joint_waypoint_10 
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_0     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_1     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_2     
|  1.000e+01 |  6.644e-01 |  3.290e-01 |  3.275e-01 |  3.369e-01 |  3.354e-01 |  9.956e-01 | collision_3     
|  1.000e+01 |  8.070e+00 |  5.753e+00 |  6.105e+00 |  1.965e+00 |  2.317e+00 |  1.180e+00 | collision_4     
|  1.000e+01 |  2.176e+01 |  1.974e+01 |  2.031e+01 |  1.444e+00 |  2.017e+00 |  1.397e+00 | collision_5     
|  1.000e+01 |  1.502e+01 |  1.458e+01 |  1.462e+01 |  4.004e-01 |  4.376e-01 |  1.093e+00 | collision_6     
|  1.000e+01 |  1.112e+01 |  1.019e+01 |  1.058e+01 |  5.351e-01 |  9.291e-01 |  1.736e+00 | collision_7     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_8     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_9     
| ======================================================================================== |
| ---------- |  5.662e+00 |  5.058e+00 |  5.194e+00 | ---------- | ---------- | ---------- | SUM CONSTRAINTS (WITHOUT MERIT) 
| ======================================================================================== |
| ---------- |  6.281e+01 |  5.620e+01 | ---------- |  5.125e+00 |  6.616e+00 |  1.291e+00 | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH MERIT)
| ======================================================================================== |
[INFO] expanded trust region. new box size: 0.0338
[INFO] iteration 4
[INFO]  

| ======================================================================================== |
|                                      ROS Industrial                                      |
|                                  TrajOpt Motion Planning                                 |
| ======================================================================================== |
|      merit |   oldexact |  new_exact | new_approx |    dapprox |     dexact |      ratio |
| ---------------------------------------------------------------------------------------- | COSTS
| ---------- |  5.439e-22 |  1.295e-21 |  9.918e-15 | -9.918e-15 | -7.511e-22 | ---------- | joint_waypoint_0 
| ---------- |  2.961e-21 |  1.356e-20 | -1.253e-14 |  1.253e-14 | -1.060e-20 | ---------- | joint_waypoint_10 
| ---------- |  0.000e+00 |  0.000e+00 | -1.892e-11 |  1.892e-11 |  0.000e+00 | ---------- | collision_0     
| ---------- |  0.000e+00 |  0.000e+00 |  6.381e-11 | -6.381e-11 |  0.000e+00 | ---------- | collision_1     
| ---------- |  0.000e+00 |  0.000e+00 |  1.321e-10 | -1.321e-10 |  0.000e+00 | ---------- | collision_2     
| ---------- |  3.290e-02 |  6.317e-03 |  5.474e-03 |  2.743e-02 |  2.659e-02 |  9.693e-01 | collision_3     
| ---------- |  5.753e-01 |  2.763e-01 |  3.281e-01 |  2.472e-01 |  2.990e-01 |  1.210e+00 | collision_4     
| ---------- |  1.974e+00 |  2.178e+00 |  1.799e+00 |  1.751e-01 | -2.042e-01 | -1.167e+00 | collision_5     
| ---------- |  1.458e+00 |  1.392e+00 |  1.411e+00 |  4.648e-02 |  6.572e-02 |  1.414e+00 | collision_6     
| ---------- |  1.019e+00 |  8.284e-01 |  8.921e-01 |  1.265e-01 |  1.902e-01 |  1.503e+00 | collision_7     
| ---------- |  0.000e+00 |  0.000e+00 | -1.225e-12 |  1.225e-12 |  0.000e+00 | ---------- | collision_8     
| ---------- |  0.000e+00 |  0.000e+00 | -1.724e-11 |  1.724e-11 |  0.000e+00 | ---------- | collision_9     
| ---------- |  5.535e-01 |  6.342e-01 |  6.342e-01 | -8.068e-02 | -8.068e-02 |  1.000e+00 | joint_vel_cost  
| ======================================================================================== |
| ---------- |  5.612e+00 |  5.315e+00 |  5.070e+00 | ---------- | ---------- | ---------- | SUM COSTS
| ======================================================================================== |
| ---------------------------------------------------------------------------------------- | CONSTRAINTS
|  1.000e+01 |  5.439e-21 |  1.295e-20 |  9.146e-10 | -9.146e-10 | -7.511e-21 | ---------- | joint_waypoint_0 
|  1.000e+01 |  2.961e-20 |  1.356e-19 |  3.572e-09 | -3.572e-09 | -1.060e-19 | ---------- | joint_waypoint_10 
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_0     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_1     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_2     
|  1.000e+01 |  3.290e-01 |  6.317e-02 |  5.475e-02 |  2.743e-01 |  2.659e-01 |  9.693e-01 | collision_3     
|  1.000e+01 |  5.753e+00 |  2.763e+00 |  3.281e+00 |  2.472e+00 |  2.990e+00 |  1.210e+00 | collision_4     
|  1.000e+01 |  1.974e+01 |  2.178e+01 |  1.799e+01 |  1.751e+00 | -2.042e+00 | -1.167e+00 | collision_5     
|  1.000e+01 |  1.458e+01 |  1.392e+01 |  1.411e+01 |  4.648e-01 |  6.572e-01 |  1.414e+00 | collision_6     
|  1.000e+01 |  1.019e+01 |  8.284e+00 |  8.921e+00 |  1.265e+00 |  1.902e+00 |  1.504e+00 | collision_7     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_8     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_9     
| ======================================================================================== |
| ---------- |  5.058e+00 |  4.681e+00 |  4.436e+00 | ---------- | ---------- | ---------- | SUM CONSTRAINTS (WITHOUT MERIT) 
| ======================================================================================== |
| ---------- |  5.620e+01 |  5.213e+01 | ---------- |  6.769e+00 |  4.070e+00 |  6.012e-01 | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH MERIT)
| ======================================================================================== |
[INFO] expanded trust region. new box size: 0.0506
[INFO] iteration 5
[INFO]  

| ======================================================================================== |
|                                      ROS Industrial                                      |
|                                  TrajOpt Motion Planning                                 |
| ======================================================================================== |
|      merit |   oldexact |  new_exact | new_approx |    dapprox |     dexact |      ratio |
| ---------------------------------------------------------------------------------------- | COSTS
| ---------- |  1.295e-21 |  6.261e-22 |  1.499e-14 | -1.499e-14 |  6.689e-22 | ---------- | joint_waypoint_0 
| ---------- |  1.356e-20 |  1.405e-23 | -4.680e-15 |  4.680e-15 |  1.355e-20 | ---------- | joint_waypoint_10 
| ---------- |  0.000e+00 |  0.000e+00 | -5.019e-11 |  5.019e-11 |  0.000e+00 | ---------- | collision_0     
| ---------- |  0.000e+00 |  0.000e+00 |  7.064e-11 | -7.064e-11 |  0.000e+00 | ---------- | collision_1     
| ---------- |  0.000e+00 |  0.000e+00 | -4.340e-11 |  4.340e-11 |  0.000e+00 | ---------- | collision_2     
| ---------- |  6.317e-03 |  0.000e+00 |  1.433e-09 |  6.317e-03 |  6.317e-03 |  1.000e+00 | collision_3     
| ---------- |  2.763e-01 |  1.307e-01 |  1.326e-01 |  1.437e-01 |  1.455e-01 |  1.013e+00 | collision_4     
| ---------- |  2.178e+00 |  2.213e+00 |  1.954e+00 |  2.245e-01 | -3.486e-02 | -1.553e-01 | collision_5     
| ---------- |  1.392e+00 |  1.401e+00 |  1.397e+00 | -5.234e-03 | -8.988e-03 |  1.717e+00 | collision_6     
| ---------- |  8.284e-01 |  8.073e-01 |  7.000e-01 |  1.284e-01 |  2.109e-02 |  1.642e-01 | collision_7     
| ---------- |  0.000e+00 |  0.000e+00 |  5.879e-12 | -5.879e-12 |  0.000e+00 | ---------- | collision_8     
| ---------- |  0.000e+00 |  0.000e+00 | -1.139e-11 |  1.139e-11 |  0.000e+00 | ---------- | collision_9     
| ---------- |  6.342e-01 |  7.877e-01 |  7.877e-01 | -1.535e-01 | -1.535e-01 |  1.000e+00 | joint_vel_cost  
| ======================================================================================== |
| ---------- |  5.315e+00 |  5.340e+00 |  4.971e+00 | ---------- | ---------- | ---------- | SUM COSTS
| ======================================================================================== |
| ---------------------------------------------------------------------------------------- | CONSTRAINTS
|  1.000e+01 |  1.295e-20 |  6.261e-21 |  6.218e-10 | -6.218e-10 |  6.689e-21 | ---------- | joint_waypoint_0 
|  1.000e+01 |  1.356e-19 |  1.405e-22 |  9.263e-11 | -9.263e-11 |  1.355e-19 | ---------- | joint_waypoint_10 
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_0     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_1     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_2     
|  1.000e+01 |  6.317e-02 |  0.000e+00 |  0.000e+00 |  6.317e-02 |  6.317e-02 |  1.000e+00 | collision_3     
|  1.000e+01 |  2.763e+00 |  1.307e+00 |  1.326e+00 |  1.437e+00 |  1.455e+00 |  1.013e+00 | collision_4     
|  1.000e+01 |  2.178e+01 |  2.213e+01 |  1.954e+01 |  2.244e+00 | -3.486e-01 | -1.553e-01 | collision_5     
|  1.000e+01 |  1.392e+01 |  1.401e+01 |  1.397e+01 | -5.265e-02 | -8.988e-02 |  1.707e+00 | collision_6     
|  1.000e+01 |  8.284e+00 |  8.073e+00 |  7.000e+00 |  1.284e+00 |  2.109e-01 |  1.642e-01 | collision_7     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_8     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_9     
| ======================================================================================== |
| ---------- |  4.681e+00 |  4.552e+00 |  4.184e+00 | ---------- | ---------- | ---------- | SUM CONSTRAINTS (WITHOUT MERIT) 
| ======================================================================================== |
| ---------- |  5.213e+01 |  5.086e+01 | ---------- |  5.319e+00 |  1.267e+00 |  2.381e-01 | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH MERIT)
| ======================================================================================== |
[INFO] shrunk trust region. new box size: 0.0051
[INFO]  

| ======================================================================================== |
|                                      ROS Industrial                                      |
|                                  TrajOpt Motion Planning                                 |
| ======================================================================================== |
|      merit |   oldexact |  new_exact | new_approx |    dapprox |     dexact |      ratio |
| ---------------------------------------------------------------------------------------- | COSTS
| ---------- |  1.295e-21 |  4.147e-27 |  2.179e-14 | -2.179e-14 |  1.295e-21 | ---------- | joint_waypoint_0 
| ---------- |  1.356e-20 |  5.974e-25 | -1.001e-14 |  1.001e-14 |  1.356e-20 | ---------- | joint_waypoint_10 
| ---------- |  0.000e+00 |  0.000e+00 | -1.568e-12 |  1.568e-12 |  0.000e+00 | ---------- | collision_0     
| ---------- |  0.000e+00 |  0.000e+00 | -7.309e-13 |  7.309e-13 |  0.000e+00 | ---------- | collision_1     
| ---------- |  0.000e+00 |  0.000e+00 |  7.479e-13 | -7.479e-13 |  0.000e+00 | ---------- | collision_2     
| ---------- |  6.317e-03 |  4.207e-03 |  4.188e-03 |  2.129e-03 |  2.110e-03 |  9.912e-01 | collision_3     
| ---------- |  2.763e-01 |  2.535e-01 |  2.537e-01 |  2.258e-02 |  2.280e-02 |  1.010e+00 | collision_4     
| ---------- |  2.178e+00 |  2.107e+00 |  2.117e+00 |  6.151e-02 |  7.147e-02 |  1.162e+00 | collision_5     
| ---------- |  1.392e+00 |  1.398e+00 |  1.403e+00 | -1.110e-02 | -6.262e-03 |  5.642e-01 | collision_6     
| ---------- |  8.284e-01 |  7.942e-01 |  7.945e-01 |  3.394e-02 |  3.424e-02 |  1.009e+00 | collision_7     
| ---------- |  0.000e+00 |  0.000e+00 | -8.968e-12 |  8.968e-12 |  0.000e+00 | ---------- | collision_8     
| ---------- |  0.000e+00 |  0.000e+00 |  5.661e-12 | -5.661e-12 |  0.000e+00 | ---------- | collision_9     
| ---------- |  6.342e-01 |  6.415e-01 |  6.415e-01 | -7.257e-03 | -7.257e-03 |  1.000e+00 | joint_vel_cost  
| ======================================================================================== |
| ---------- |  5.315e+00 |  5.198e+00 |  5.214e+00 | ---------- | ---------- | ---------- | SUM COSTS
| ======================================================================================== |
| ---------------------------------------------------------------------------------------- | CONSTRAINTS
|  1.000e+01 |  1.295e-20 |  4.147e-26 |  1.705e-12 | -1.705e-12 |  1.295e-20 | ---------- | joint_waypoint_0 
|  1.000e+01 |  1.356e-19 |  5.974e-24 |  2.829e-11 | -2.829e-11 |  1.356e-19 | ---------- | joint_waypoint_10 
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_0     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_1     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_2     
|  1.000e+01 |  6.317e-02 |  4.207e-02 |  4.188e-02 |  2.129e-02 |  2.110e-02 |  9.912e-01 | collision_3     
|  1.000e+01 |  2.763e+00 |  2.535e+00 |  2.537e+00 |  2.258e-01 |  2.280e-01 |  1.010e+00 | collision_4     
|  1.000e+01 |  2.178e+01 |  2.107e+01 |  2.117e+01 |  6.151e-01 |  7.147e-01 |  1.162e+00 | collision_5     
|  1.000e+01 |  1.392e+01 |  1.398e+01 |  1.403e+01 | -1.111e-01 | -6.262e-02 |  5.636e-01 | collision_6     
|  1.000e+01 |  8.284e+00 |  7.942e+00 |  7.945e+00 |  3.393e-01 |  3.424e-01 |  1.009e+00 | collision_7     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_8     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_9     
| ======================================================================================== |
| ---------- |  4.681e+00 |  4.557e+00 |  4.572e+00 | ---------- | ---------- | ---------- | SUM CONSTRAINTS (WITHOUT MERIT) 
| ======================================================================================== |
| ---------- |  5.213e+01 |  5.077e+01 | ---------- |  1.192e+00 |  1.361e+00 |  1.141e+00 | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH MERIT)
| ======================================================================================== |
[INFO] expanded trust region. new box size: 0.0076
[INFO] iteration 6
[INFO]  

| ======================================================================================== |
|                                      ROS Industrial                                      |
|                                  TrajOpt Motion Planning                                 |
| ======================================================================================== |
|      merit |   oldexact |  new_exact | new_approx |    dapprox |     dexact |      ratio |
| ---------------------------------------------------------------------------------------- | COSTS
| ---------- |  4.147e-27 |  1.745e-24 |  1.113e-14 | -1.113e-14 | -1.740e-24 | ---------- | joint_waypoint_0 
| ---------- |  5.974e-25 |  2.909e-25 | -1.182e-14 |  1.182e-14 |  3.065e-25 | ---------- | joint_waypoint_10 
| ---------- |  0.000e+00 |  0.000e+00 |  1.952e-11 | -1.952e-11 |  0.000e+00 | ---------- | collision_0     
| ---------- |  0.000e+00 |  0.000e+00 |  3.344e-11 | -3.344e-11 |  0.000e+00 | ---------- | collision_1     
| ---------- |  0.000e+00 |  0.000e+00 |  4.606e-11 | -4.606e-11 |  0.000e+00 | ---------- | collision_2     
| ---------- |  4.207e-03 |  1.035e-03 |  9.910e-04 |  3.216e-03 |  3.171e-03 |  9.862e-01 | collision_3     
| ---------- |  2.535e-01 |  2.239e-01 |  2.291e-01 |  2.432e-02 |  2.960e-02 |  1.217e+00 | collision_4     
| ---------- |  2.107e+00 |  2.496e+00 |  2.045e+00 |  6.192e-02 | -3.897e-01 | -6.294e+00 | collision_5     
| ---------- |  1.398e+00 |  1.395e+00 |  1.398e+00 |  1.955e-04 |  2.900e-03 |  1.483e+01 | collision_6     
| ---------- |  7.942e-01 |  7.681e-01 |  7.721e-01 |  2.207e-02 |  2.605e-02 |  1.180e+00 | collision_7     
| ---------- |  0.000e+00 |  0.000e+00 |  1.302e-11 | -1.302e-11 |  0.000e+00 | ---------- | collision_8     
| ---------- |  0.000e+00 |  0.000e+00 |  6.398e-12 | -6.398e-12 |  0.000e+00 | ---------- | collision_9     
| ---------- |  6.415e-01 |  6.635e-01 |  6.635e-01 | -2.198e-02 | -2.198e-02 |  1.000e+00 | joint_vel_cost  
| ======================================================================================== |
| ---------- |  5.198e+00 |  5.548e+00 |  5.109e+00 | ---------- | ---------- | ---------- | SUM COSTS
| ======================================================================================== |
| ---------------------------------------------------------------------------------------- | CONSTRAINTS
|  1.000e+01 |  4.147e-26 |  1.745e-23 |  3.065e-11 | -3.065e-11 | -1.740e-23 | ---------- | joint_waypoint_0 
|  1.000e+01 |  5.974e-24 |  2.909e-24 |  2.119e-11 | -2.119e-11 |  3.065e-24 | ---------- | joint_waypoint_10 
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_0     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_1     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_2     
|  1.000e+01 |  4.207e-02 |  1.035e-02 |  9.910e-03 |  3.216e-02 |  3.171e-02 |  9.862e-01 | collision_3     
|  1.000e+01 |  2.535e+00 |  2.239e+00 |  2.291e+00 |  2.432e-01 |  2.960e-01 |  1.217e+00 | collision_4     
|  1.000e+01 |  2.107e+01 |  2.496e+01 |  2.045e+01 |  6.191e-01 | -3.897e+00 | -6.295e+00 | collision_5     
|  1.000e+01 |  1.398e+01 |  1.395e+01 |  1.398e+01 |  1.796e-03 |  2.900e-02 |  1.614e+01 | collision_6     
|  1.000e+01 |  7.942e+00 |  7.681e+00 |  7.721e+00 |  2.207e-01 |  2.605e-01 |  1.180e+00 | collision_7     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_8     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_9     
| ======================================================================================== |
| ---------- |  4.557e+00 |  4.885e+00 |  4.445e+00 | ---------- | ---------- | ---------- | SUM CONSTRAINTS (WITHOUT MERIT) 
| ======================================================================================== |
| ---------- |  5.077e+01 |  5.440e+01 | ---------- |  1.207e+00 | -3.630e+00 | -3.008e+00 | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH MERIT)
| ======================================================================================== |
[INFO] shrunk trust region. new box size: 0.0008
[INFO]  

| ======================================================================================== |
|                                      ROS Industrial                                      |
|                                  TrajOpt Motion Planning                                 |
| ======================================================================================== |
|      merit |   oldexact |  new_exact | new_approx |    dapprox |     dexact |      ratio |
| ---------------------------------------------------------------------------------------- | COSTS
| ---------- |  4.147e-27 |  4.179e-25 |  5.811e-15 | -5.811e-15 | -4.138e-25 | ---------- | joint_waypoint_0 
| ---------- |  5.974e-25 |  1.610e-27 | -2.899e-15 |  2.899e-15 |  5.958e-25 | ---------- | joint_waypoint_10 
| ---------- |  0.000e+00 |  0.000e+00 |  1.558e-11 | -1.558e-11 |  0.000e+00 | ---------- | collision_0     
| ---------- |  0.000e+00 |  0.000e+00 | -5.265e-12 |  5.265e-12 |  0.000e+00 | ---------- | collision_1     
| ---------- |  0.000e+00 |  0.000e+00 | -1.115e-10 |  1.115e-10 |  0.000e+00 | ---------- | collision_2     
| ---------- |  4.207e-03 |  3.892e-03 |  3.892e-03 |  3.150e-04 |  3.146e-04 |  9.986e-01 | collision_3     
| ---------- |  2.535e-01 |  2.509e-01 |  2.509e-01 |  2.598e-03 |  2.588e-03 |  9.962e-01 | collision_4     
| ---------- |  2.107e+00 |  2.098e+00 |  2.098e+00 |  8.811e-03 |  8.984e-03 |  1.020e+00 | collision_5     
| ---------- |  1.398e+00 |  1.400e+00 |  1.400e+00 | -1.833e-03 | -1.745e-03 |  9.519e-01 | collision_6     
| ---------- |  7.942e-01 |  7.897e-01 |  7.899e-01 |  4.309e-03 |  4.505e-03 |  1.046e+00 | collision_7     
| ---------- |  0.000e+00 |  0.000e+00 |  1.870e-11 | -1.870e-11 |  0.000e+00 | ---------- | collision_8     
| ---------- |  0.000e+00 |  0.000e+00 |  6.821e-12 | -6.821e-12 |  0.000e+00 | ---------- | collision_9     
| ---------- |  6.415e-01 |  6.424e-01 |  6.424e-01 | -9.355e-04 | -9.355e-04 |  1.000e+00 | joint_vel_cost  
| ======================================================================================== |
| ---------- |  5.198e+00 |  5.185e+00 |  5.185e+00 | ---------- | ---------- | ---------- | SUM COSTS
| ======================================================================================== |
| ---------------------------------------------------------------------------------------- | CONSTRAINTS
|  1.000e+01 |  4.147e-26 |  4.179e-24 |  1.473e-11 | -1.473e-11 | -4.138e-24 | ---------- | joint_waypoint_0 
|  1.000e+01 |  5.974e-24 |  1.610e-26 |  1.589e-12 | -1.589e-12 |  5.958e-24 | ---------- | joint_waypoint_10 
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_0     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_1     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_2     
|  1.000e+01 |  4.207e-02 |  3.892e-02 |  3.892e-02 |  3.150e-03 |  3.146e-03 |  9.986e-01 | collision_3     
|  1.000e+01 |  2.535e+00 |  2.509e+00 |  2.509e+00 |  2.597e-02 |  2.588e-02 |  9.965e-01 | collision_4     
|  1.000e+01 |  2.107e+01 |  2.098e+01 |  2.098e+01 |  8.809e-02 |  8.984e-02 |  1.020e+00 | collision_5     
|  1.000e+01 |  1.398e+01 |  1.400e+01 |  1.400e+01 | -1.836e-02 | -1.745e-02 |  9.504e-01 | collision_6     
|  1.000e+01 |  7.942e+00 |  7.897e+00 |  7.899e+00 |  4.306e-02 |  4.505e-02 |  1.046e+00 | collision_7     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_8     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_9     
| ======================================================================================== |
| ---------- |  4.557e+00 |  4.542e+00 |  4.543e+00 | ---------- | ---------- | ---------- | SUM CONSTRAINTS (WITHOUT MERIT) 
| ======================================================================================== |
| ---------- |  5.077e+01 |  5.061e+01 | ---------- |  1.552e-01 |  1.602e-01 |  1.032e+00 | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH MERIT)
| ======================================================================================== |
[INFO] expanded trust region. new box size: 0.0011
[INFO] iteration 7
[INFO]  

| ======================================================================================== |
|                                      ROS Industrial                                      |
|                                  TrajOpt Motion Planning                                 |
| ======================================================================================== |
|      merit |   oldexact |  new_exact | new_approx |    dapprox |     dexact |      ratio |
| ---------------------------------------------------------------------------------------- | COSTS
| ---------- |  4.179e-25 |  8.083e-28 |  9.361e-15 | -9.361e-15 |  4.171e-25 | ---------- | joint_waypoint_0 
| ---------- |  1.610e-27 |  2.611e-29 | -6.459e-15 |  6.459e-15 |  1.584e-27 | ---------- | joint_waypoint_10 
| ---------- |  0.000e+00 |  0.000e+00 |  1.160e-13 | -1.160e-13 |  0.000e+00 | ---------- | collision_0     
| ---------- |  0.000e+00 |  0.000e+00 | -5.129e-12 |  5.129e-12 |  0.000e+00 | ---------- | collision_1     
| ---------- |  0.000e+00 |  0.000e+00 | -1.857e-11 |  1.857e-11 |  0.000e+00 | ---------- | collision_2     
| ---------- |  3.892e-03 |  3.419e-03 |  3.418e-03 |  4.742e-04 |  4.733e-04 |  9.979e-01 | collision_3     
| ---------- |  2.509e-01 |  2.975e-01 |  2.478e-01 |  3.088e-03 | -4.662e-02 | -1.510e+01 | collision_4     
| ---------- |  2.098e+00 |  2.085e+00 |  2.086e+00 |  1.153e-02 |  1.245e-02 |  1.080e+00 | collision_5     
| ---------- |  1.400e+00 |  1.401e+00 |  1.402e+00 | -1.400e-03 | -1.266e-03 |  9.042e-01 | collision_6     
| ---------- |  7.897e-01 |  7.842e-01 |  7.844e-01 |  5.226e-03 |  5.462e-03 |  1.045e+00 | collision_7     
| ---------- |  0.000e+00 |  0.000e+00 |  9.582e-13 | -9.582e-13 |  0.000e+00 | ---------- | collision_8     
| ---------- |  0.000e+00 |  0.000e+00 |  6.004e-13 | -6.004e-13 |  0.000e+00 | ---------- | collision_9     
| ---------- |  6.424e-01 |  6.454e-01 |  6.454e-01 | -2.951e-03 | -2.951e-03 |  1.000e+00 | joint_vel_cost  
| ======================================================================================== |
| ---------- |  5.185e+00 |  5.217e+00 |  5.169e+00 | ---------- | ---------- | ---------- | SUM COSTS
| ======================================================================================== |
| ---------------------------------------------------------------------------------------- | CONSTRAINTS
|  1.000e+01 |  4.179e-24 |  8.083e-27 |  7.111e-13 | -7.111e-13 |  4.171e-24 | ---------- | joint_waypoint_0 
|  1.000e+01 |  1.610e-26 |  2.611e-28 |  2.485e-13 | -2.485e-13 |  1.584e-26 | ---------- | joint_waypoint_10 
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_0     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_1     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_2     
|  1.000e+01 |  3.892e-02 |  3.419e-02 |  3.418e-02 |  4.742e-03 |  4.733e-03 |  9.979e-01 | collision_3     
|  1.000e+01 |  2.509e+00 |  2.975e+00 |  2.478e+00 |  3.088e-02 | -4.662e-01 | -1.510e+01 | collision_4     
|  1.000e+01 |  2.098e+01 |  2.085e+01 |  2.086e+01 |  1.150e-01 |  1.245e-01 |  1.083e+00 | collision_5     
|  1.000e+01 |  1.400e+01 |  1.401e+01 |  1.402e+01 | -1.405e-02 | -1.266e-02 |  9.011e-01 | collision_6     
|  1.000e+01 |  7.897e+00 |  7.842e+00 |  7.844e+00 |  5.221e-02 |  5.462e-02 |  1.046e+00 | collision_7     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_8     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_9     
| ======================================================================================== |
| ---------- |  4.542e+00 |  4.572e+00 |  4.523e+00 | ---------- | ---------- | ---------- | SUM CONSTRAINTS (WITHOUT MERIT) 
| ======================================================================================== |
| ---------- |  5.061e+01 |  5.093e+01 | ---------- |  2.047e-01 | -3.275e-01 | -1.600e+00 | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH MERIT)
| ======================================================================================== |
[INFO] shrunk trust region. new box size: 0.0001
[INFO]  

| ======================================================================================== |
|                                      ROS Industrial                                      |
|                                  TrajOpt Motion Planning                                 |
| ======================================================================================== |
|      merit |   oldexact |  new_exact | new_approx |    dapprox |     dexact |      ratio |
| ---------------------------------------------------------------------------------------- | COSTS
| ---------- |  4.179e-25 |  1.331e-29 |  1.647e-14 | -1.647e-14 |  4.179e-25 | ---------- | joint_waypoint_0 
| ---------- |  1.610e-27 |  2.182e-27 | -6.457e-15 |  6.457e-15 | -5.717e-28 | ---------- | joint_waypoint_10 
| ---------- |  0.000e+00 |  0.000e+00 |  1.863e-14 | -1.863e-14 |  0.000e+00 | ---------- | collision_0     
| ---------- |  0.000e+00 |  0.000e+00 |  2.021e-13 | -2.021e-13 |  0.000e+00 | ---------- | collision_1     
| ---------- |  0.000e+00 |  0.000e+00 | -1.427e-12 |  1.427e-12 |  0.000e+00 | ---------- | collision_2     
| ---------- |  3.892e-03 |  3.841e-03 |  3.841e-03 |  5.081e-05 |  5.080e-05 |  9.999e-01 | collision_3     
| ---------- |  2.509e-01 |  2.506e-01 |  2.506e-01 |  2.718e-04 |  2.714e-04 |  9.984e-01 | collision_4     
| ---------- |  2.098e+00 |  2.096e+00 |  2.096e+00 |  1.335e-03 |  1.354e-03 |  1.015e+00 | collision_5     
| ---------- |  1.400e+00 |  1.400e+00 |  1.400e+00 | -3.109e-04 | -3.045e-04 |  9.795e-01 | collision_6     
| ---------- |  7.897e-01 |  7.890e-01 |  7.890e-01 |  6.611e-04 |  6.842e-04 |  1.035e+00 | collision_7     
| ---------- |  0.000e+00 |  0.000e+00 | -5.186e-11 |  5.186e-11 |  0.000e+00 | ---------- | collision_8     
| ---------- |  0.000e+00 |  0.000e+00 |  1.854e-12 | -1.854e-12 |  0.000e+00 | ---------- | collision_9     
| ---------- |  6.424e-01 |  6.425e-01 |  6.425e-01 | -8.731e-05 | -8.731e-05 |  1.000e+00 | joint_vel_cost  
| ======================================================================================== |
| ---------- |  5.185e+00 |  5.183e+00 |  5.183e+00 | ---------- | ---------- | ---------- | SUM COSTS
| ======================================================================================== |
| ---------------------------------------------------------------------------------------- | CONSTRAINTS
|  1.000e+01 |  4.179e-24 |  1.331e-28 |  1.069e-13 | -1.069e-13 |  4.179e-24 | ---------- | joint_waypoint_0 
|  1.000e+01 |  1.610e-26 |  2.182e-26 |  1.684e-12 | -1.684e-12 | -5.717e-27 | ---------- | joint_waypoint_10 
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_0     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_1     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_2     
|  1.000e+01 |  3.892e-02 |  3.841e-02 |  3.841e-02 |  5.081e-04 |  5.080e-04 |  9.999e-01 | collision_3     
|  1.000e+01 |  2.509e+00 |  2.506e+00 |  2.506e+00 |  2.716e-03 |  2.714e-03 |  9.992e-01 | collision_4     
|  1.000e+01 |  2.098e+01 |  2.096e+01 |  2.096e+01 |  1.334e-02 |  1.354e-02 |  1.015e+00 | collision_5     
|  1.000e+01 |  1.400e+01 |  1.400e+01 |  1.400e+01 | -3.099e-03 | -3.045e-03 |  9.827e-01 | collision_6     
|  1.000e+01 |  7.897e+00 |  7.890e+00 |  7.890e+00 |  6.614e-03 |  6.842e-03 |  1.034e+00 | collision_7     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_8     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_9     
| ======================================================================================== |
| ---------- |  4.542e+00 |  4.540e+00 |  4.540e+00 | ---------- | ---------- | ---------- | SUM CONSTRAINTS (WITHOUT MERIT) 
| ======================================================================================== |
| ---------- |  5.061e+01 |  5.058e+01 | ---------- |  2.200e-02 |  2.253e-02 |  1.024e+00 | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH MERIT)
| ======================================================================================== |
[INFO] expanded trust region. new box size: 0.0002
[INFO] iteration 8
[INFO]  

| ======================================================================================== |
|                                      ROS Industrial                                      |
|                                  TrajOpt Motion Planning                                 |
| ======================================================================================== |
|      merit |   oldexact |  new_exact | new_approx |    dapprox |     dexact |      ratio |
| ---------------------------------------------------------------------------------------- | COSTS
| ---------- |  1.331e-29 |  5.650e-29 |  1.292e-14 | -1.292e-14 | -4.319e-29 | ---------- | joint_waypoint_0 
| ---------- |  2.182e-27 |  2.548e-26 | -6.456e-15 |  6.456e-15 | -2.330e-26 | ---------- | joint_waypoint_10 
| ---------- |  0.000e+00 |  0.000e+00 |  1.652e-13 | -1.652e-13 |  0.000e+00 | ---------- | collision_0     
| ---------- |  0.000e+00 |  0.000e+00 |  3.212e-13 | -3.212e-13 |  0.000e+00 | ---------- | collision_1     
| ---------- |  0.000e+00 |  0.000e+00 |  1.449e-12 | -1.449e-12 |  0.000e+00 | ---------- | collision_2     
| ---------- |  3.841e-03 |  3.765e-03 |  3.765e-03 |  7.610e-05 |  7.609e-05 |  9.998e-01 | collision_3     
| ---------- |  2.506e-01 |  2.502e-01 |  2.502e-01 |  4.203e-04 |  4.200e-04 |  9.995e-01 | collision_4     
| ---------- |  2.096e+00 |  2.094e+00 |  2.094e+00 |  2.101e-03 |  1.980e-03 |  9.424e-01 | collision_5     
| ---------- |  1.400e+00 |  1.401e+00 |  1.401e+00 | -3.898e-04 | -4.589e-04 |  1.177e+00 | collision_6     
| ---------- |  7.890e-01 |  7.880e-01 |  7.880e-01 |  9.835e-04 |  1.020e-03 |  1.037e+00 | collision_7     
| ---------- |  0.000e+00 |  0.000e+00 | -7.757e-11 |  7.757e-11 |  0.000e+00 | ---------- | collision_8     
| ---------- |  0.000e+00 |  0.000e+00 | -5.260e-11 |  5.260e-11 |  0.000e+00 | ---------- | collision_9     
| ---------- |  6.425e-01 |  6.427e-01 |  6.427e-01 | -1.614e-04 | -1.614e-04 |  1.000e+00 | joint_vel_cost  
| ======================================================================================== |
| ---------- |  5.183e+00 |  5.180e+00 |  5.180e+00 | ---------- | ---------- | ---------- | SUM COSTS
| ======================================================================================== |
| ---------------------------------------------------------------------------------------- | CONSTRAINTS
|  1.000e+01 |  1.331e-28 |  5.650e-28 |  3.732e-13 | -3.732e-13 | -4.319e-28 | ---------- | joint_waypoint_0 
|  1.000e+01 |  2.182e-26 |  2.548e-25 |  5.948e-12 | -5.948e-12 | -2.330e-25 | ---------- | joint_waypoint_10 
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_0     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_1     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_2     
|  1.000e+01 |  3.841e-02 |  3.765e-02 |  3.765e-02 |  7.610e-04 |  7.609e-04 |  9.998e-01 | collision_3     
|  1.000e+01 |  2.506e+00 |  2.502e+00 |  2.502e+00 |  4.203e-03 |  4.200e-03 |  9.994e-01 | collision_4     
|  1.000e+01 |  2.096e+01 |  2.094e+01 |  2.094e+01 |  1.950e-02 |  1.980e-02 |  1.015e+00 | collision_5     
|  1.000e+01 |  1.400e+01 |  1.401e+01 |  1.401e+01 | -4.699e-03 | -4.589e-03 |  9.766e-01 | collision_6     
|  1.000e+01 |  7.890e+00 |  7.880e+00 |  7.880e+00 |  9.842e-03 |  1.020e-02 |  1.036e+00 | collision_7     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_8     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_9     
| ======================================================================================== |
| ---------- |  4.540e+00 |  4.537e+00 |  4.537e+00 | ---------- | ---------- | ---------- | SUM CONSTRAINTS (WITHOUT MERIT) 
| ======================================================================================== |
| ---------- |  5.058e+01 |  5.055e+01 | ---------- |  3.263e-02 |  3.324e-02 |  1.019e+00 | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH MERIT)
| ======================================================================================== |
[INFO] expanded trust region. new box size: 0.0003
[INFO] iteration 9
[INFO]  

| ======================================================================================== |
|                                      ROS Industrial                                      |
|                                  TrajOpt Motion Planning                                 |
| ======================================================================================== |
|      merit |   oldexact |  new_exact | new_approx |    dapprox |     dexact |      ratio |
| ---------------------------------------------------------------------------------------- | COSTS
| ---------- |  5.650e-29 |  7.149e-30 |  1.469e-14 | -1.469e-14 |  4.935e-29 | ---------- | joint_waypoint_0 
| ---------- |  2.548e-26 |  4.369e-26 | -1.129e-15 |  1.129e-15 | -1.821e-26 | ---------- | joint_waypoint_10 
| ---------- |  0.000e+00 |  0.000e+00 |  9.036e-15 | -9.036e-15 |  0.000e+00 | ---------- | collision_0     
| ---------- |  0.000e+00 |  0.000e+00 |  1.462e-13 | -1.462e-13 |  0.000e+00 | ---------- | collision_1     
| ---------- |  0.000e+00 |  0.000e+00 |  4.872e-13 | -4.872e-13 |  0.000e+00 | ---------- | collision_2     
| ---------- |  3.765e-03 |  3.661e-03 |  3.661e-03 |  1.037e-04 |  1.037e-04 |  9.996e-01 | collision_3     
| ---------- |  2.502e-01 |  3.005e-01 |  2.495e-01 |  7.174e-04 | -5.036e-02 | -7.021e+01 | collision_4     
| ---------- |  2.094e+00 |  2.092e+00 |  2.092e+00 |  2.700e-03 |  2.718e-03 |  1.007e+00 | collision_5     
| ---------- |  1.401e+00 |  1.401e+00 |  1.401e+00 | -5.452e-04 | -5.257e-04 |  9.642e-01 | collision_6     
| ---------- |  7.880e-01 |  7.864e-01 |  7.865e-01 |  1.465e-03 |  1.530e-03 |  1.045e+00 | collision_7     
| ---------- |  0.000e+00 |  0.000e+00 | -6.932e-11 |  6.932e-11 |  0.000e+00 | ---------- | collision_8     
| ---------- |  0.000e+00 |  0.000e+00 | -2.144e-11 |  2.144e-11 |  0.000e+00 | ---------- | collision_9     
| ---------- |  6.427e-01 |  6.432e-01 |  6.432e-01 | -5.112e-04 | -5.112e-04 |  1.000e+00 | joint_vel_cost  
| ======================================================================================== |
| ---------- |  5.180e+00 |  5.227e+00 |  5.176e+00 | ---------- | ---------- | ---------- | SUM COSTS
| ======================================================================================== |
| ---------------------------------------------------------------------------------------- | CONSTRAINTS
|  1.000e+01 |  5.650e-28 |  7.149e-29 |  1.112e-13 | -1.112e-13 |  4.935e-28 | ---------- | joint_waypoint_0 
|  1.000e+01 |  2.548e-25 |  4.369e-25 |  7.894e-12 | -7.894e-12 | -1.821e-25 | ---------- | joint_waypoint_10 
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_0     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_1     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_2     
|  1.000e+01 |  3.765e-02 |  3.661e-02 |  3.661e-02 |  1.037e-03 |  1.037e-03 |  9.996e-01 | collision_3     
|  1.000e+01 |  2.502e+00 |  3.005e+00 |  2.495e+00 |  7.174e-03 | -5.036e-01 | -7.021e+01 | collision_4     
|  1.000e+01 |  2.094e+01 |  2.092e+01 |  2.092e+01 |  2.686e-02 |  2.718e-02 |  1.012e+00 | collision_5     
|  1.000e+01 |  1.401e+01 |  1.401e+01 |  1.401e+01 | -5.486e-03 | -5.257e-03 |  9.582e-01 | collision_6     
|  1.000e+01 |  7.880e+00 |  7.864e+00 |  7.865e+00 |  1.461e-02 |  1.530e-02 |  1.047e+00 | collision_7     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_8     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_9     
| ======================================================================================== |
| ---------- |  4.537e+00 |  4.584e+00 |  4.533e+00 | ---------- | ---------- | ---------- | SUM CONSTRAINTS (WITHOUT MERIT) 
| ======================================================================================== |
| ---------- |  5.055e+01 |  5.106e+01 | ---------- |  4.813e-02 | -5.124e-01 | -1.065e+01 | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH MERIT)
| ======================================================================================== |
[INFO] shrunk trust region. new box size: 0.0000
[INFO] converged because trust region is tiny
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_3
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_4
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_5
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_6
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_7
[INFO] New merit_error_coeffs: (10, 10, 10, 10, 10, 100, 100, 100, 100, 100, 10, 10)
[INFO] iteration 1
[INFO]  

| ======================================================================================== |
|                                      ROS Industrial                                      |
|                                  TrajOpt Motion Planning                                 |
| ======================================================================================== |
|      merit |   oldexact |  new_exact | new_approx |    dapprox |     dexact |      ratio |
| ---------------------------------------------------------------------------------------- | COSTS
| ---------- |  5.650e-29 |  2.940e-29 | -1.018e-14 |  1.018e-14 |  2.710e-29 | ---------- | joint_waypoint_0 
| ---------- |  2.548e-26 |  2.176e-29 |  6.485e-16 | -6.485e-16 |  2.546e-26 | ---------- | joint_waypoint_10 
| ---------- |  0.000e+00 |  0.000e+00 | -1.235e-13 |  1.235e-13 |  0.000e+00 | ---------- | collision_0     
| ---------- |  0.000e+00 |  0.000e+00 |  5.225e-13 | -5.225e-13 |  0.000e+00 | ---------- | collision_1     
| ---------- |  0.000e+00 |  0.000e+00 |  1.428e-11 | -1.428e-11 |  0.000e+00 | ---------- | collision_2     
| ---------- |  3.765e-03 |  3.152e-03 |  3.150e-03 |  6.151e-04 |  6.135e-04 |  9.973e-01 | collision_3     
| ---------- |  2.502e-01 |  2.944e-01 |  2.461e-01 |  4.106e-03 | -4.427e-02 | -1.078e+01 | collision_4     
| ---------- |  2.094e+00 |  2.077e+00 |  2.080e+00 |  1.473e-02 |  1.687e-02 |  1.145e+00 | collision_5     
| ---------- |  1.401e+00 |  1.401e+00 |  1.401e+00 | -1.974e-04 | -1.068e-04 |  5.409e-01 | collision_6     
| ---------- |  7.880e-01 |  7.822e-01 |  7.829e-01 |  5.069e-03 |  5.721e-03 |  1.129e+00 | collision_7     
| ---------- |  0.000e+00 |  0.000e+00 |  5.708e-12 | -5.708e-12 |  0.000e+00 | ---------- | collision_8     
| ---------- |  0.000e+00 |  0.000e+00 |  2.622e-13 | -2.622e-13 |  0.000e+00 | ---------- | collision_9     
| ---------- |  6.427e-01 |  6.478e-01 |  6.478e-01 | -5.134e-03 | -5.134e-03 |  1.000e+00 | joint_vel_cost  
| ======================================================================================== |
| ---------- |  5.180e+00 |  5.206e+00 |  5.161e+00 | ---------- | ---------- | ---------- | SUM COSTS
| ======================================================================================== |
| ---------------------------------------------------------------------------------------- | CONSTRAINTS
|  1.000e+01 |  5.650e-28 |  2.940e-28 |  2.135e-13 | -2.135e-13 |  2.710e-28 | ---------- | joint_waypoint_0 
|  1.000e+01 |  2.548e-25 |  2.176e-28 |  2.132e-13 | -2.132e-13 |  2.546e-25 | ---------- | joint_waypoint_10 
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_0     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_1     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_2     
|  1.000e+02 |  3.765e-01 |  3.152e-01 |  3.150e-01 |  6.151e-02 |  6.135e-02 |  9.973e-01 | collision_3     
|  1.000e+02 |  2.502e+01 |  2.944e+01 |  2.461e+01 |  4.106e-01 | -4.427e+00 | -1.078e+01 | collision_4     
|  1.000e+02 |  2.094e+02 |  2.077e+02 |  2.080e+02 |  1.473e+00 |  1.687e+00 |  1.145e+00 | collision_5     
|  1.000e+02 |  1.401e+02 |  1.401e+02 |  1.401e+02 | -1.978e-02 | -1.068e-02 |  5.397e-01 | collision_6     
|  1.000e+02 |  7.880e+01 |  7.822e+01 |  7.829e+01 |  5.069e-01 |  5.721e-01 |  1.129e+00 | collision_7     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_8     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_9     
| ======================================================================================== |
| ---------- |  4.537e+00 |  4.558e+00 |  4.513e+00 | ---------- | ---------- | ---------- | SUM CONSTRAINTS (WITHOUT MERIT) 
| ======================================================================================== |
| ---------- |  4.589e+02 |  4.610e+02 | ---------- |  2.451e+00 | -2.144e+00 | -8.748e-01 | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH MERIT)
| ======================================================================================== |
[INFO] shrunk trust region. new box size: 0.0002
[INFO]  

| ======================================================================================== |
|                                      ROS Industrial                                      |
|                                  TrajOpt Motion Planning                                 |
| ======================================================================================== |
|      merit |   oldexact |  new_exact | new_approx |    dapprox |     dexact |      ratio |
| ---------------------------------------------------------------------------------------- | COSTS
| ---------- |  5.650e-29 |  6.021e-29 |  7.585e-15 | -7.585e-15 | -3.713e-30 | ---------- | joint_waypoint_0 
| ---------- |  2.548e-26 |  7.328e-29 | -2.904e-15 |  2.904e-15 |  2.541e-26 | ---------- | joint_waypoint_10 
| ---------- |  0.000e+00 |  0.000e+00 | -2.246e-14 |  2.246e-14 |  0.000e+00 | ---------- | collision_0     
| ---------- |  0.000e+00 |  0.000e+00 | -1.798e-14 |  1.798e-14 |  0.000e+00 | ---------- | collision_1     
| ---------- |  0.000e+00 |  0.000e+00 | -2.604e-14 |  2.604e-14 |  0.000e+00 | ---------- | collision_2     
| ---------- |  3.765e-03 |  3.705e-03 |  3.705e-03 |  6.059e-05 |  6.058e-05 |  9.998e-01 | collision_3     
| ---------- |  2.502e-01 |  3.010e-01 |  2.498e-01 |  4.181e-04 | -5.080e-02 | -1.215e+02 | collision_4     
| ---------- |  2.094e+00 |  2.093e+00 |  2.093e+00 |  1.648e-03 |  1.656e-03 |  1.005e+00 | collision_5     
| ---------- |  1.401e+00 |  1.401e+00 |  1.401e+00 | -2.428e-04 | -2.292e-04 |  9.439e-01 | collision_6     
| ---------- |  7.880e-01 |  7.872e-01 |  7.872e-01 |  7.688e-04 |  7.860e-04 |  1.022e+00 | collision_7     
| ---------- |  0.000e+00 |  0.000e+00 | -1.599e-11 |  1.599e-11 |  0.000e+00 | ---------- | collision_8     
| ---------- |  0.000e+00 |  0.000e+00 |  1.785e-13 | -1.785e-13 |  0.000e+00 | ---------- | collision_9     
| ---------- |  6.427e-01 |  6.431e-01 |  6.431e-01 | -4.377e-04 | -4.377e-04 |  1.000e+00 | joint_vel_cost  
| ======================================================================================== |
| ---------- |  5.180e+00 |  5.229e+00 |  5.178e+00 | ---------- | ---------- | ---------- | SUM COSTS
| ======================================================================================== |
| ---------------------------------------------------------------------------------------- | CONSTRAINTS
|  1.000e+01 |  5.650e-28 |  6.021e-28 |  2.842e-13 | -2.842e-13 | -3.713e-29 | ---------- | joint_waypoint_0 
|  1.000e+01 |  2.548e-25 |  7.328e-28 |  3.246e-13 | -3.246e-13 |  2.541e-25 | ---------- | joint_waypoint_10 
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_0     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_1     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_2     
|  1.000e+02 |  3.765e-01 |  3.705e-01 |  3.705e-01 |  6.059e-03 |  6.058e-03 |  9.998e-01 | collision_3     
|  1.000e+02 |  2.502e+01 |  3.010e+01 |  2.498e+01 |  4.181e-02 | -5.080e+00 | -1.215e+02 | collision_4     
|  1.000e+02 |  2.094e+02 |  2.093e+02 |  2.093e+02 |  1.639e-01 |  1.656e-01 |  1.010e+00 | collision_5     
|  1.000e+02 |  1.401e+02 |  1.401e+02 |  1.401e+02 | -2.425e-02 | -2.292e-02 |  9.452e-01 | collision_6     
|  1.000e+02 |  7.880e+01 |  7.872e+01 |  7.872e+01 |  7.689e-02 |  7.860e-02 |  1.022e+00 | collision_7     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_8     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_9     
| ======================================================================================== |
| ---------- |  4.537e+00 |  4.586e+00 |  4.534e+00 | ---------- | ---------- | ---------- | SUM CONSTRAINTS (WITHOUT MERIT) 
| ======================================================================================== |
| ---------- |  4.589e+02 |  4.638e+02 | ---------- |  2.666e-01 | -4.901e+00 | -1.838e+01 | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH MERIT)
| ======================================================================================== |
[INFO] shrunk trust region. new box size: 0.0000
[INFO] converged because trust region is tiny
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_3
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_4
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_5
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_6
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_7
[INFO] New merit_error_coeffs: (10, 10, 10, 10, 10, 1000, 1000, 1000, 1000, 1000, 10, 10)
[INFO] iteration 1
[INFO]  

| ======================================================================================== |
|                                      ROS Industrial                                      |
|                                  TrajOpt Motion Planning                                 |
| ======================================================================================== |
|      merit |   oldexact |  new_exact | new_approx |    dapprox |     dexact |      ratio |
| ---------------------------------------------------------------------------------------- | COSTS
| ---------- |  5.650e-29 |  7.614e-27 |  5.834e-15 | -5.834e-15 | -7.557e-27 | ---------- | joint_waypoint_0 
| ---------- |  2.548e-26 |  1.732e-29 |  6.485e-16 | -6.485e-16 |  2.546e-26 | ---------- | joint_waypoint_10 
| ---------- |  0.000e+00 |  0.000e+00 |  1.217e-12 | -1.217e-12 |  0.000e+00 | ---------- | collision_0     
| ---------- |  0.000e+00 |  0.000e+00 |  3.196e-12 | -3.196e-12 |  0.000e+00 | ---------- | collision_1     
| ---------- |  0.000e+00 |  0.000e+00 |  5.129e-12 | -5.129e-12 |  0.000e+00 | ---------- | collision_2     
| ---------- |  3.765e-03 |  3.152e-03 |  3.150e-03 |  6.151e-04 |  6.134e-04 |  9.973e-01 | collision_3     
| ---------- |  2.502e-01 |  2.944e-01 |  2.461e-01 |  4.106e-03 | -4.427e-02 | -1.078e+01 | collision_4     
| ---------- |  2.094e+00 |  2.077e+00 |  2.080e+00 |  1.472e-02 |  1.686e-02 |  1.145e+00 | collision_5     
| ---------- |  1.401e+00 |  1.401e+00 |  1.401e+00 | -1.971e-04 | -1.066e-04 |  5.411e-01 | collision_6     
| ---------- |  7.880e-01 |  7.822e-01 |  7.829e-01 |  5.069e-03 |  5.721e-03 |  1.129e+00 | collision_7     
| ---------- |  0.000e+00 |  0.000e+00 |  3.675e-12 | -3.675e-12 |  0.000e+00 | ---------- | collision_8     
| ---------- |  0.000e+00 |  0.000e+00 |  7.277e-14 | -7.277e-14 |  0.000e+00 | ---------- | collision_9     
| ---------- |  6.427e-01 |  6.478e-01 |  6.478e-01 | -5.134e-03 | -5.134e-03 |  1.000e+00 | joint_vel_cost  
| ======================================================================================== |
| ---------- |  5.180e+00 |  5.206e+00 |  5.161e+00 | ---------- | ---------- | ---------- | SUM COSTS
| ======================================================================================== |
| ---------------------------------------------------------------------------------------- | CONSTRAINTS
|  1.000e+01 |  5.650e-28 |  7.614e-26 |  3.124e-12 | -3.124e-12 | -7.557e-26 | ---------- | joint_waypoint_0 
|  1.000e+01 |  2.548e-25 |  1.732e-28 |  1.910e-13 | -1.910e-13 |  2.546e-25 | ---------- | joint_waypoint_10 
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_0     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_1     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_2     
|  1.000e+03 |  3.765e+00 |  3.152e+00 |  3.150e+00 |  6.151e-01 |  6.134e-01 |  9.973e-01 | collision_3     
|  1.000e+03 |  2.502e+02 |  2.944e+02 |  2.461e+02 |  4.106e+00 | -4.427e+01 | -1.078e+01 | collision_4     
|  1.000e+03 |  2.094e+03 |  2.077e+03 |  2.080e+03 |  1.472e+01 |  1.686e+01 |  1.145e+00 | collision_5     
|  1.000e+03 |  1.401e+03 |  1.401e+03 |  1.401e+03 | -1.976e-01 | -1.066e-01 |  5.396e-01 | collision_6     
|  1.000e+03 |  7.880e+02 |  7.822e+02 |  7.829e+02 |  5.069e+00 |  5.721e+00 |  1.129e+00 | collision_7     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_8     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_9     
| ======================================================================================== |
| ---------- |  4.537e+00 |  4.558e+00 |  4.513e+00 | ---------- | ---------- | ---------- | SUM CONSTRAINTS (WITHOUT MERIT) 
| ======================================================================================== |
| ---------- |  4.542e+03 |  4.563e+03 | ---------- |  2.433e+01 | -2.121e+01 | -8.716e-01 | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH MERIT)
| ======================================================================================== |
[INFO] shrunk trust region. new box size: 0.0002
[INFO]  

| ======================================================================================== |
|                                      ROS Industrial                                      |
|                                  TrajOpt Motion Planning                                 |
| ======================================================================================== |
|      merit |   oldexact |  new_exact | new_approx |    dapprox |     dexact |      ratio |
| ---------------------------------------------------------------------------------------- | COSTS
| ---------- |  5.650e-29 |  3.606e-29 |  1.291e-14 | -1.291e-14 |  2.044e-29 | ---------- | joint_waypoint_0 
| ---------- |  2.548e-26 |  2.958e-30 | -2.904e-15 |  2.904e-15 |  2.548e-26 | ---------- | joint_waypoint_10 
| ---------- |  0.000e+00 |  0.000e+00 |  5.378e-16 | -5.378e-16 |  0.000e+00 | ---------- | collision_0     
| ---------- |  0.000e+00 |  0.000e+00 | -6.336e-15 |  6.336e-15 |  0.000e+00 | ---------- | collision_1     
| ---------- |  0.000e+00 |  0.000e+00 | -1.066e-14 |  1.066e-14 |  0.000e+00 | ---------- | collision_2     
| ---------- |  3.765e-03 |  3.705e-03 |  3.705e-03 |  6.059e-05 |  6.058e-05 |  9.998e-01 | collision_3     
| ---------- |  2.502e-01 |  3.010e-01 |  2.498e-01 |  4.203e-04 | -5.079e-02 | -1.208e+02 | collision_4     
| ---------- |  2.094e+00 |  2.093e+00 |  2.093e+00 |  1.635e-03 |  1.652e-03 |  1.010e+00 | collision_5     
| ---------- |  1.401e+00 |  1.401e+00 |  1.401e+00 | -2.422e-04 | -2.288e-04 |  9.450e-01 | collision_6     
| ---------- |  7.880e-01 |  7.872e-01 |  7.872e-01 |  7.675e-04 |  7.848e-04 |  1.023e+00 | collision_7     
| ---------- |  0.000e+00 |  0.000e+00 | -3.600e-12 |  3.600e-12 |  0.000e+00 | ---------- | collision_8     
| ---------- |  0.000e+00 |  0.000e+00 |  4.918e-15 | -4.918e-15 |  0.000e+00 | ---------- | collision_9     
| ---------- |  6.427e-01 |  6.431e-01 |  6.431e-01 | -4.371e-04 | -4.371e-04 |  1.000e+00 | joint_vel_cost  
| ======================================================================================== |
| ---------- |  5.180e+00 |  5.229e+00 |  5.178e+00 | ---------- | ---------- | ---------- | SUM COSTS
| ======================================================================================== |
| ---------------------------------------------------------------------------------------- | CONSTRAINTS
|  1.000e+01 |  5.650e-28 |  3.606e-28 |  2.549e-13 | -2.549e-13 |  2.044e-28 | ---------- | joint_waypoint_0 
|  1.000e+01 |  2.548e-25 |  2.958e-29 |  6.218e-14 | -6.218e-14 |  2.548e-25 | ---------- | joint_waypoint_10 
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_0     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_1     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_2     
|  1.000e+03 |  3.765e+00 |  3.705e+00 |  3.705e+00 |  6.059e-02 |  6.058e-02 |  9.998e-01 | collision_3     
|  1.000e+03 |  2.502e+02 |  3.010e+02 |  2.498e+02 |  4.203e-01 | -5.079e+01 | -1.208e+02 | collision_4     
|  1.000e+03 |  2.094e+03 |  2.093e+03 |  2.093e+03 |  1.635e+00 |  1.652e+00 |  1.010e+00 | collision_5     
|  1.000e+03 |  1.401e+03 |  1.401e+03 |  1.401e+03 | -2.422e-01 | -2.288e-01 |  9.449e-01 | collision_6     
|  1.000e+03 |  7.880e+02 |  7.872e+02 |  7.872e+02 |  7.675e-01 |  7.848e-01 |  1.023e+00 | collision_7     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_8     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_9     
| ======================================================================================== |
| ---------- |  4.537e+00 |  4.586e+00 |  4.534e+00 | ---------- | ---------- | ---------- | SUM CONSTRAINTS (WITHOUT MERIT) 
| ======================================================================================== |
| ---------- |  4.542e+03 |  4.591e+03 | ---------- |  2.643e+00 | -4.857e+01 | -1.838e+01 | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH MERIT)
| ======================================================================================== |
[INFO] shrunk trust region. new box size: 0.0000
[INFO] converged because trust region is tiny
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_3
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_4
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_5
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_6
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_7
[INFO] New merit_error_coeffs: (10, 10, 10, 10, 10, 10000, 10000, 10000, 10000, 10000, 10, 10)
[INFO] iteration 1
[INFO]  

| ======================================================================================== |
|                                      ROS Industrial                                      |
|                                  TrajOpt Motion Planning                                 |
| ======================================================================================== |
|      merit |   oldexact |  new_exact | new_approx |    dapprox |     dexact |      ratio |
| ---------------------------------------------------------------------------------------- | COSTS
| ---------- |  5.650e-29 |  9.166e-29 |  1.647e-14 | -1.647e-14 | -3.516e-29 | ---------- | joint_waypoint_0 
| ---------- |  2.548e-26 |  1.997e-29 |  6.486e-16 | -6.486e-16 |  2.546e-26 | ---------- | joint_waypoint_10 
| ---------- |  0.000e+00 |  0.000e+00 | -3.761e-14 |  3.761e-14 |  0.000e+00 | ---------- | collision_0     
| ---------- |  0.000e+00 |  0.000e+00 | -3.590e-14 |  3.590e-14 |  0.000e+00 | ---------- | collision_1     
| ---------- |  0.000e+00 |  0.000e+00 | -1.273e-11 |  1.273e-11 |  0.000e+00 | ---------- | collision_2     
| ---------- |  3.765e-03 |  3.151e-03 |  3.150e-03 |  6.152e-04 |  6.137e-04 |  9.975e-01 | collision_3     
| ---------- |  2.502e-01 |  2.944e-01 |  2.461e-01 |  4.107e-03 | -4.427e-02 | -1.078e+01 | collision_4     
| ---------- |  2.094e+00 |  2.077e+00 |  2.080e+00 |  1.473e-02 |  1.687e-02 |  1.145e+00 | collision_5     
| ---------- |  1.401e+00 |  1.401e+00 |  1.401e+00 | -1.969e-04 | -1.071e-04 |  5.436e-01 | collision_6     
| ---------- |  7.880e-01 |  7.822e-01 |  7.829e-01 |  5.067e-03 |  5.718e-03 |  1.129e+00 | collision_7     
| ---------- |  0.000e+00 |  0.000e+00 |  1.431e-11 | -1.431e-11 |  0.000e+00 | ---------- | collision_8     
| ---------- |  0.000e+00 |  0.000e+00 | -2.989e-15 |  2.989e-15 |  0.000e+00 | ---------- | collision_9     
| ---------- |  6.427e-01 |  6.478e-01 |  6.478e-01 | -5.133e-03 | -5.133e-03 |  1.000e+00 | joint_vel_cost  
| ======================================================================================== |
| ---------- |  5.180e+00 |  5.206e+00 |  5.161e+00 | ---------- | ---------- | ---------- | SUM COSTS
| ======================================================================================== |
| ---------------------------------------------------------------------------------------- | CONSTRAINTS
|  1.000e+01 |  5.650e-28 |  9.166e-28 |  3.686e-13 | -3.686e-13 | -3.516e-28 | ---------- | joint_waypoint_0 
|  1.000e+01 |  2.548e-25 |  1.997e-28 |  1.917e-13 | -1.917e-13 |  2.546e-25 | ---------- | joint_waypoint_10 
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_0     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_1     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_2     
|  1.000e+04 |  3.765e+01 |  3.151e+01 |  3.150e+01 |  6.153e+00 |  6.137e+00 |  9.973e-01 | collision_3     
|  1.000e+04 |  2.502e+03 |  2.944e+03 |  2.461e+03 |  4.106e+01 | -4.427e+02 | -1.078e+01 | collision_4     
|  1.000e+04 |  2.094e+04 |  2.077e+04 |  2.080e+04 |  1.473e+02 |  1.687e+02 |  1.145e+00 | collision_5     
|  1.000e+04 |  1.401e+04 |  1.401e+04 |  1.401e+04 | -1.981e+00 | -1.071e+00 |  5.404e-01 | collision_6     
|  1.000e+04 |  7.880e+03 |  7.822e+03 |  7.829e+03 |  5.066e+01 |  5.718e+01 |  1.129e+00 | collision_7     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_8     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_9     
| ======================================================================================== |
| ---------- |  4.537e+00 |  4.558e+00 |  4.513e+00 | ---------- | ---------- | ---------- | SUM CONSTRAINTS (WITHOUT MERIT) 
| ======================================================================================== |
| ---------- |  4.538e+04 |  4.559e+04 | ---------- |  2.432e+02 | -2.117e+02 | -8.706e-01 | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH MERIT)
| ======================================================================================== |
[INFO] shrunk trust region. new box size: 0.0002
[INFO]  

| ======================================================================================== |
|                                      ROS Industrial                                      |
|                                  TrajOpt Motion Planning                                 |
| ======================================================================================== |
|      merit |   oldexact |  new_exact | new_approx |    dapprox |     dexact |      ratio |
| ---------------------------------------------------------------------------------------- | COSTS
| ---------- |  5.650e-29 |  7.716e-29 |  1.114e-14 | -1.114e-14 | -2.066e-29 | ---------- | joint_waypoint_0 
| ---------- |  2.548e-26 |  1.800e-29 | -4.681e-15 |  4.681e-15 |  2.546e-26 | ---------- | joint_waypoint_10 
| ---------- |  0.000e+00 |  0.000e+00 |  8.388e-16 | -8.388e-16 |  0.000e+00 | ---------- | collision_0     
| ---------- |  0.000e+00 |  0.000e+00 |  9.895e-15 | -9.895e-15 |  0.000e+00 | ---------- | collision_1     
| ---------- |  0.000e+00 |  0.000e+00 | -2.349e-14 |  2.349e-14 |  0.000e+00 | ---------- | collision_2     
| ---------- |  3.765e-03 |  3.705e-03 |  3.705e-03 |  6.059e-05 |  6.058e-05 |  9.998e-01 | collision_3     
| ---------- |  2.502e-01 |  3.010e-01 |  2.498e-01 |  4.202e-04 | -5.079e-02 | -1.209e+02 | collision_4     
| ---------- |  2.094e+00 |  2.093e+00 |  2.093e+00 |  1.634e-03 |  1.651e-03 |  1.010e+00 | collision_5     
| ---------- |  1.401e+00 |  1.401e+00 |  1.401e+00 | -2.424e-04 | -2.289e-04 |  9.444e-01 | collision_6     
| ---------- |  7.880e-01 |  7.872e-01 |  7.872e-01 |  7.681e-04 |  7.854e-04 |  1.022e+00 | collision_7     
| ---------- |  0.000e+00 |  0.000e+00 | -9.072e-12 |  9.072e-12 |  0.000e+00 | ---------- | collision_8     
| ---------- |  0.000e+00 |  0.000e+00 | -2.367e-15 |  2.367e-15 |  0.000e+00 | ---------- | collision_9     
| ---------- |  6.427e-01 |  6.431e-01 |  6.431e-01 | -4.373e-04 | -4.373e-04 |  1.000e+00 | joint_vel_cost  
| ======================================================================================== |
| ---------- |  5.180e+00 |  5.229e+00 |  5.178e+00 | ---------- | ---------- | ---------- | SUM COSTS
| ======================================================================================== |
| ---------------------------------------------------------------------------------------- | CONSTRAINTS
|  1.000e+01 |  5.650e-28 |  7.716e-28 |  3.199e-13 | -3.199e-13 | -2.066e-28 | ---------- | joint_waypoint_0 
|  1.000e+01 |  2.548e-25 |  1.800e-28 |  1.377e-13 | -1.377e-13 |  2.546e-25 | ---------- | joint_waypoint_10 
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_0     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_1     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_2     
|  1.000e+04 |  3.765e+01 |  3.705e+01 |  3.705e+01 |  6.059e-01 |  6.058e-01 |  9.998e-01 | collision_3     
|  1.000e+04 |  2.502e+03 |  3.010e+03 |  2.498e+03 |  4.202e+00 | -5.079e+02 | -1.209e+02 | collision_4     
|  1.000e+04 |  2.094e+04 |  2.093e+04 |  2.093e+04 |  1.634e+01 |  1.651e+01 |  1.010e+00 | collision_5     
|  1.000e+04 |  1.401e+04 |  1.401e+04 |  1.401e+04 | -2.422e+00 | -2.289e+00 |  9.451e-01 | collision_6     
|  1.000e+04 |  7.880e+03 |  7.872e+03 |  7.872e+03 |  7.682e+00 |  7.854e+00 |  1.022e+00 | collision_7     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_8     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 | ---------- | collision_9     
| ======================================================================================== |
| ---------- |  4.537e+00 |  4.586e+00 |  4.534e+00 | ---------- | ---------- | ---------- | SUM CONSTRAINTS (WITHOUT MERIT) 
| ======================================================================================== |
| ---------- |  4.538e+04 |  4.586e+04 | ---------- |  2.641e+01 | -4.853e+02 | -1.838e+01 | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH MERIT)
| ======================================================================================== |
[INFO] shrunk trust region. new box size: 0.0000
[INFO] converged because trust region is tiny
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_3
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_4
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_5
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_6
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_7
[INFO] New merit_error_coeffs: (10, 10, 10, 10, 10, 100000, 100000, 100000, 100000, 100000, 10, 10)
[INFO] iteration 1
[WARN] convex solver failed! set TRAJOPT_LOG_THRESH=DEBUG to see solver output. saving model to /tmp/fail.lp and IIS to /tmp/fail.ilp
[INFO] shrunk trust region. new box size: 0.0002
[WARN] convex solver failed! set TRAJOPT_LOG_THRESH=DEBUG to see solver output. saving model to /tmp/fail.lp and IIS to /tmp/fail.ilp
[INFO] shrunk trust region. new box size: 0.0000
[INFO] converged because trust region is tiny
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_3
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_4
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_5
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_6
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_7
[INFO] New merit_error_coeffs: (10, 10, 10, 10, 10, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 10, 10)
[INFO] optimization couldn't satisfy all constraints
[INFO] 
==================
Optimization results:
status: OPT_PENALTY_ITERATION_LIMIT
cost values: (5.64995e-29, 2.54817e-26, 0, 0, 0, 0.00376516, 0.250176, 2.09428, 1.40089, 0.78796, 0, 0, 0.64266)
constraint violations: (5.64995e-29, 2.54817e-26, 0, 0, 0, 0.00376516, 0.250176, 2.09428, 1.40089, 0.78796, 0, 0)
n func evals: 20
n qp solves: 21
==================
Info:    TrajOptMotionPlannerTask motion planning failed (Failed to find valid solution: OPT_PENALTY_ITERATION_LIMIT) for process input: Tesseract Composite Instruction
Debug:   Error
Info:    Planning took 5.508835 seconds.

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Default planning time #402

Default planning time #402

allicen commented May 1, 2024

marip8 commented May 1, 2024

allicen commented May 5, 2024 •

edited

Loading

Default planning time #402

Default planning time #402

Comments

allicen commented May 1, 2024

marip8 commented May 1, 2024

allicen commented May 5, 2024 • edited Loading

allicen commented May 5, 2024 •

edited

Loading