Default planning time

[allicen]

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.

[marip8]

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]

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.