joschu / trajopt Goto Github PK

I am interested in upgrading to the latest version of bullet. Does anyone know what changes were made to the internal version of bullet?

When I attempt to build trajopt, I get the error message:
CMake Error: The following variables are used in this project, but they are set to NOTFOUND.
Please set them or make sure they are set and tested correctly in the CMake files:
GUROBI_LIBRARY (ADVANCED)
linked by target "sco" in directory /home/ams/trajopt/src/sco

-- Configuring incomplete, errors occurred!

I have Gurobi installed in /home/ams/gurobi651, and GUROBI_HOME set to: /home/ams/gurobi651/linux64

I tried adding "gurobi65" to the NAMES section of FindGUROBI.cmake, in the trajopt cmake/modules directory, but that didn't fix it.

Currently there are only tutorials for trajoptpy in the documentation. It will be very helpful if you could add a few examples based on the C++ api making use of sco and trajopt.

output of ctest is as follows
lakshmi@lakshmi-Nitro-AN515-51:~/trajopt/build$ ctest
Test project /home/lakshmi/trajopt/build
Start 1: arm_to_joint_target.py
1/10 Test #1: arm_to_joint_target.py .............***Failed 0.27 sec
Start 2: arm_to_cart_target.py
2/10 Test #2: arm_to_cart_target.py ..............***Failed 0.22 sec
Start 3: fullbody_plan.py
3/10 Test #3: fullbody_plan.py ...................***Failed 0.22 sec
Start 4: position_base.py
4/10 Test #4: position_base.py ...................***Failed 0.04 sec
Start 5: this_side_up.py
5/10 Test #5: this_side_up.py ....................***Failed 0.21 sec
Start 6: arm_to_cart_target_position_only
6/10 Test #6: arm_to_cart_target_position_only ...***Failed 0.21 sec
Start 7: sco-unit
7/10 Test #7: sco-unit ........................... Passed 0.11 sec
Start 8: collision-checker-unit
8/10 Test #8: collision-checker-unit .............***Failed 0.10 sec
Start 9: planning-unit
9/10 Test #9: planning-unit ......................***Failed 1.23 sec
Start 10: cast-cost-unit
10/10 Test #10: cast-cost-unit .....................***Exception: SegFault 0.11 sec

10% tests passed, 9 tests failed out of 10

Total Test time (real) = 2.79 sec

The following tests FAILED:
1 - arm_to_joint_target.py (Failed)
2 - arm_to_cart_target.py (Failed)
3 - fullbody_plan.py (Failed)
4 - position_base.py (Failed)
5 - this_side_up.py (Failed)
6 - arm_to_cart_target_position_only (Failed)
8 - collision-checker-unit (Failed)
9 - planning-unit (Failed)
10 - cast-cost-unit (SEGFAULT)
Errors while running CTest

output of ctest -V command
lakshmi@lakshmi-Nitro-AN515-51:~/trajopt/build$ ctest -V
UpdateCTestConfiguration from :/home/lakshmi/trajopt/build/DartConfiguration.tcl
UpdateCTestConfiguration from :/home/lakshmi/trajopt/build/DartConfiguration.tcl
Test project /home/lakshmi/trajopt/build
Constructing a list of tests
Done constructing a list of tests
Checking test dependency graph...
Checking test dependency graph end
test 1
Start 1: arm_to_joint_target.py

1: Test command: /usr/bin/python "/home/lakshmi/trajopt/python_examples/arm_to_joint_target.py"
1: Test timeout computed to be: 9.99988e+06
1: Traceback (most recent call last):
1: File "/home/lakshmi/trajopt/python_examples/arm_to_joint_target.py", line 7, in
1: import trajoptpy
1: File "/home/lakshmi/trajopt/trajoptpy/init.py", line 1, in
1: from ctrajoptpy import *
1: ImportError: /home/lakshmi/trajopt/build/lib/ctrajoptpy.so: undefined symbol: _ZTIN7trajopt11RobotAndDOFE
1/10 Test #1: arm_to_joint_target.py .............***Failed 0.22 sec
test 2
Start 2: arm_to_cart_target.py

2: Test command: /usr/bin/python "/home/lakshmi/trajopt/python_examples/arm_to_cart_target.py"
2: Test timeout computed to be: 9.99988e+06
2: Traceback (most recent call last):
2: File "/home/lakshmi/trajopt/python_examples/arm_to_cart_target.py", line 8, in
2: import trajoptpy
2: File "/home/lakshmi/trajopt/trajoptpy/init.py", line 1, in
2: from ctrajoptpy import *
2: ImportError: /home/lakshmi/trajopt/build/lib/ctrajoptpy.so: undefined symbol: _ZTIN7trajopt11RobotAndDOFE
2/10 Test #2: arm_to_cart_target.py ..............***Failed 0.20 sec
test 3
Start 3: fullbody_plan.py

3: Test command: /usr/bin/python "/home/lakshmi/trajopt/python_examples/fullbody_plan.py"
3: Test timeout computed to be: 9.99988e+06
3: Traceback (most recent call last):
3: File "/home/lakshmi/trajopt/python_examples/fullbody_plan.py", line 7, in
3: import openravepy, trajoptpy
3: File "/home/lakshmi/trajopt/trajoptpy/init.py", line 1, in
3: from ctrajoptpy import *
3: ImportError: /home/lakshmi/trajopt/build/lib/ctrajoptpy.so: undefined symbol: _ZTIN7trajopt11RobotAndDOFE
3/10 Test #3: fullbody_plan.py ...................***Failed 0.21 sec
test 4
Start 4: position_base.py

4: Test command: /usr/bin/python "/home/lakshmi/trajopt/python_examples/position_base.py"
4: Test timeout computed to be: 9.99988e+06
4: Traceback (most recent call last):
4: File "/home/lakshmi/trajopt/python_examples/position_base.py", line 7, in
4: import trajoptpy
4: File "/home/lakshmi/trajopt/trajoptpy/init.py", line 1, in
4: from ctrajoptpy import *
4: ImportError: /home/lakshmi/trajopt/build/lib/ctrajoptpy.so: undefined symbol: _ZTIN7trajopt11RobotAndDOFE
4/10 Test #4: position_base.py ...................***Failed 0.04 sec
test 5
Start 5: this_side_up.py

5: Test command: /usr/bin/python "/home/lakshmi/trajopt/python_examples/this_side_up.py"
5: Test timeout computed to be: 9.99988e+06
5: Traceback (most recent call last):
5: File "/home/lakshmi/trajopt/python_examples/this_side_up.py", line 9, in
5: import trajoptpy
5: File "/home/lakshmi/trajopt/trajoptpy/init.py", line 1, in
5: from ctrajoptpy import *
5: ImportError: /home/lakshmi/trajopt/build/lib/ctrajoptpy.so: undefined symbol: _ZTIN7trajopt11RobotAndDOFE
5/10 Test #5: this_side_up.py ....................***Failed 0.21 sec
test 6
Start 6: arm_to_cart_target_position_only

6: Test command: /usr/bin/python "/home/lakshmi/trajopt/python_examples/arm_to_cart_target.py" "--position_only"
6: Test timeout computed to be: 9.99988e+06
6: Traceback (most recent call last):
6: File "/home/lakshmi/trajopt/python_examples/arm_to_cart_target.py", line 8, in
6: import trajoptpy
6: File "/home/lakshmi/trajopt/trajoptpy/init.py", line 1, in
6: from ctrajoptpy import *
6: ImportError: /home/lakshmi/trajopt/build/lib/ctrajoptpy.so: undefined symbol: _ZTIN7trajopt11RobotAndDOFE
6/10 Test #6: arm_to_cart_target_position_only ...***Failed 0.21 sec
test 7
Start 7: sco-unit

7: Test command: /home/lakshmi/trajopt/build/bin/sco-unit
7: Test timeout computed to be: 9.99988e+06
7: You can set logging level with TRAJOPT_LOG_THRESH. Valid values: FATAL ERROR WARN INFO DEBUG TRACE. Defaulting to INFO
7: [==========] Running 7 tests from 2 test cases.
7: [----------] Global test environment set-up.
7: [----------] 6 tests from SQP
7: [ RUN ] SQP.QuadraticSeparable
7: [INFO] iteration 1
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 2.700e+01 | 2.700e+01 | 2.700e+01 | 1.000e+00
7: TOTAL | 2.700e+01 | 2.700e+01 | 2.700e+01 | 1.000e+00
7: [INFO] expanded trust region. new box size: 150.0000
7: [INFO] iteration 2
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 8.599e-09 | 8.599e-09 | 8.599e-09 | ------
7: TOTAL | 8.599e-09 | 8.599e-09 | 8.599e-09 | 1.000e+00
7: [INFO] converged because improvement was small (8.599e-09 < 1.000e-04)
7: [INFO]
7: ==================
7: Optimization results:
7: status: CONVERGED
7: cost values: (8.59892e-09)
7: constraint violations: ()
7: n func evals: 3
7: n qp solves: 2
7: ==================
7: [INFO] checking (5.35379e-05, 1.00005, 2.00005) ?= (0, 1, 2)
7: [ OK ] SQP.QuadraticSeparable (3 ms)
7: [ RUN ] SQP.QuadraticNonseparable
7: [INFO] iteration 1
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 2.090e+02 | 2.090e+02 | 2.090e+02 | 1.000e+00
7: TOTAL | 2.090e+02 | 2.090e+02 | 2.090e+02 | 1.000e+00
7: [INFO] expanded trust region. new box size: 150.0000
7: [INFO] iteration 2
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 1.655e-06 | 1.822e-06 | 1.650e-06 | 9.056e-01
7: TOTAL | 1.655e-06 | 1.822e-06 | 1.650e-06 | 9.056e-01
7: [INFO] expanded trust region. new box size: 225.0000
7: [INFO] iteration 3
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 4.475e-09 | 9.495e-15 | -6.077e-16 | ------
7: TOTAL | 4.475e-09 | 9.495e-15 | -6.077e-16 | -6.399e-02
7: [INFO] converged because improvement was small (9.495e-15 < 1.000e-06)
7: [INFO]
7: ==================
7: Optimization results:
7: status: CONVERGED
7: cost values: (4.475e-09)
7: constraint violations: ()
7: n func evals: 4
7: n qp solves: 3
7: ==================
7: [INFO] checking (0.999985, 6.99978, 1.99993) ?= (1, 7, 2)
7: [ OK ] SQP.QuadraticNonseparable (2 ms)
7: [ RUN ] SQP.TP1
7: [INFO] iteration 1
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 1.800e+01 | 3.280e+00 | 3.290e+00 | 1.003e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 0.000e+00 | 0.000e+00 | 0.000e+00 | ------
7: TOTAL | 1.800e+01 | 3.280e+00 | 3.290e+00 | 1.003e+00
7: [INFO] expanded trust region. new box size: 0.1500
7: [INFO] iteration 2
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 1.471e+01 | 3.831e+00 | 3.862e+00 | 1.008e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 0.000e+00 | 0.000e+00 | 0.000e+00 | ------
7: TOTAL | 1.471e+01 | 3.831e+00 | 3.862e+00 | 1.008e+00
7: [INFO] expanded trust region. new box size: 0.2250
7: [INFO] iteration 3
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 1.085e+01 | 3.648e+00 | 3.748e+00 | 1.027e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 0.000e+00 | 0.000e+00 | 0.000e+00 | ------
7: TOTAL | 1.085e+01 | 3.648e+00 | 3.748e+00 | 1.027e+00
7: [INFO] expanded trust region. new box size: 0.3375
7: [INFO] iteration 4
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 7.099e+00 | 2.120e+00 | 2.301e+00 | 1.085e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 0.000e+00 | 0.000e+00 | 0.000e+00 | ------
7: TOTAL | 7.099e+00 | 2.120e+00 | 2.301e+00 | 1.085e+00
7: [INFO] expanded trust region. new box size: 0.5063
7: [INFO] iteration 5
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 4.798e+00 | 1.574e+00 | 1.865e+00 | 1.185e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 0.000e+00 | 0.000e+00 | 0.000e+00 | ------
7: TOTAL | 4.798e+00 | 1.574e+00 | 1.865e+00 | 1.185e+00
7: [INFO] expanded trust region. new box size: 0.7594
7: [INFO] iteration 6
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 2.933e+00 | 2.097e+00 | 2.082e+00 | 9.928e-01
7: CONSTRAINTS | -------------------------------------------------
7: g | 0.000e+00 | 0.000e+00 | 0.000e+00 | ------
7: TOTAL | 2.933e+00 | 2.097e+00 | 2.082e+00 | 9.928e-01
7: [INFO] expanded trust region. new box size: 1.1391
7: [INFO] iteration 7
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 8.505e-01 | 8.943e-01 | -3.491e-02 | -3.904e-02
7: CONSTRAINTS | -------------------------------------------------
7: g | 0.000e+00 | 0.000e+00 | 0.000e+00 | ------
7: TOTAL | 8.505e-01 | 8.943e-01 | -3.491e-02 | -3.904e-02
7: [INFO] shrunk trust region. new box size: 0.1139
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 8.505e-01 | 1.983e-01 | 1.975e-01 | 9.959e-01
7: CONSTRAINTS | -------------------------------------------------
7: g | 0.000e+00 | 0.000e+00 | 0.000e+00 | ------
7: TOTAL | 8.505e-01 | 1.983e-01 | 1.975e-01 | 9.959e-01
7: [INFO] expanded trust region. new box size: 0.1709
7: [INFO] iteration 8
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 6.530e-01 | 2.463e-01 | 2.462e-01 | 9.996e-01
7: CONSTRAINTS | -------------------------------------------------
7: g | 0.000e+00 | 0.000e+00 | 0.000e+00 | ------
7: TOTAL | 6.530e-01 | 2.463e-01 | 2.462e-01 | 9.996e-01
7: [INFO] expanded trust region. new box size: 0.2563
7: [INFO] iteration 9
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 4.068e-01 | 2.579e-01 | 2.574e-01 | 9.982e-01
7: CONSTRAINTS | -------------------------------------------------
7: g | 0.000e+00 | 0.000e+00 | 0.000e+00 | ------
7: TOTAL | 4.068e-01 | 2.579e-01 | 2.574e-01 | 9.982e-01
7: [INFO] expanded trust region. new box size: 0.3844
7: [INFO] iteration 10
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 1.493e-01 | 1.284e-01 | 1.253e-01 | 9.760e-01
7: CONSTRAINTS | -------------------------------------------------
7: g | 0.000e+00 | 0.000e+00 | 0.000e+00 | ------
7: TOTAL | 1.493e-01 | 1.284e-01 | 1.253e-01 | 9.760e-01
7: [INFO] expanded trust region. new box size: 0.5767
7: [INFO] iteration 11
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 2.402e-02 | 2.239e-02 | 2.367e-02 | 1.057e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 0.000e+00 | 0.000e+00 | 0.000e+00 | ------
7: TOTAL | 2.402e-02 | 2.239e-02 | 2.367e-02 | 1.057e+00
7: [INFO] expanded trust region. new box size: 0.8650
7: [INFO] iteration 12
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 3.555e-04 | 3.509e-04 | 3.554e-04 | 1.013e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 0.000e+00 | 0.000e+00 | 0.000e+00 | ------
7: TOTAL | 3.555e-04 | 3.509e-04 | 3.554e-04 | 1.013e+00
7: [INFO] expanded trust region. new box size: 1.2975
7: [INFO] iteration 13
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 1.546e-07 | 1.372e-07 | 1.533e-07 | 1.118e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 0.000e+00 | 0.000e+00 | 0.000e+00 | ------
7: TOTAL | 1.546e-07 | 1.372e-07 | 1.533e-07 | 1.118e+00
7: [INFO] expanded trust region. new box size: 1.9462
7: [INFO] iteration 14
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 1.259e-09 | 1.666e-14 | 9.153e-12 | ------
7: CONSTRAINTS | -------------------------------------------------
7: g | 0.000e+00 | 0.000e+00 | 0.000e+00 | ------
7: TOTAL | 1.259e-09 | 1.666e-14 | 9.153e-12 | 5.496e+02
7: [INFO] converged because improvement was small (1.666e-14 < 1.000e-10)
7: [INFO] woo-hoo! constraints are satisfied (to tolerance 1.00e-04)
7: [INFO]
7: ==================
7: Optimization results:
7: status: CONVERGED
7: cost values: (1.25905e-09)
7: constraint violations: (0)
7: n func evals: 16
7: n qp solves: 15
7: ==================
7: [INFO] checking (0.999965, 0.999925) ?= (1, 1)
7: [ OK ] SQP.TP1 (6 ms)
7: [ RUN ] SQP.TP3
7: [INFO] iteration 1
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 1.001e+00 | 1.000e-01 | 1.000e-01 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 0.000e+00 | 0.000e+00 | 0.000e+00 | ------
7: TOTAL | 1.001e+00 | 1.000e-01 | 1.000e-01 | 1.000e+00
7: [INFO] expanded trust region. new box size: 0.1500
7: [INFO] iteration 2
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 9.008e-01 | 1.500e-01 | 1.500e-01 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 0.000e+00 | 0.000e+00 | 0.000e+00 | ------
7: TOTAL | 9.008e-01 | 1.500e-01 | 1.500e-01 | 1.000e+00
7: [INFO] expanded trust region. new box size: 0.2250
7: [INFO] iteration 3
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 7.508e-01 | 2.250e-01 | 2.250e-01 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 0.000e+00 | 0.000e+00 | 0.000e+00 | ------
7: TOTAL | 7.508e-01 | 2.250e-01 | 2.250e-01 | 1.000e+00
7: [INFO] expanded trust region. new box size: 0.3375
7: [INFO] iteration 4
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 5.258e-01 | 3.375e-01 | 3.375e-01 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 0.000e+00 | 0.000e+00 | 0.000e+00 | ------
7: TOTAL | 5.258e-01 | 3.375e-01 | 3.375e-01 | 1.000e+00
7: [INFO] expanded trust region. new box size: 0.5063
7: [INFO] iteration 5
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 1.883e-01 | 1.876e-01 | 1.876e-01 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 0.000e+00 | -5.024e-08 | -5.023e-08 | 1.000e+00
7: TOTAL | 1.883e-01 | 1.876e-01 | 1.876e-01 | 1.000e+00
7: [INFO] expanded trust region. new box size: 0.7594
7: [INFO] iteration 6
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 7.536e-04 | 1.261e-04 | 1.261e-04 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 5.023e-08 | -4.513e-08 | -4.513e-08 | 1.000e+00
7: TOTAL | 7.537e-04 | 1.261e-04 | 1.261e-04 | 1.000e+00
7: [INFO] expanded trust region. new box size: 1.1391
7: [INFO] iteration 7
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 6.275e-04 | 1.679e-04 | 1.679e-04 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 9.537e-08 | -3.714e-07 | -3.714e-07 | 1.000e+00
7: TOTAL | 6.276e-04 | 1.675e-04 | 1.675e-04 | 1.000e+00
7: [INFO] expanded trust region. new box size: 1.7086
7: [INFO] iteration 8
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 4.596e-04 | 2.070e-04 | 2.070e-04 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 4.668e-07 | -4.437e-06 | -4.437e-06 | 1.000e+00
7: TOTAL | 4.601e-04 | 2.026e-04 | 2.026e-04 | 1.000e+00
7: [INFO] expanded trust region. new box size: 2.5629
7: [INFO] iteration 9
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 2.526e-04 | 5.609e-04 | 5.609e-04 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 4.903e-06 | -3.666e-04 | -3.666e-04 | 1.000e+00
7: TOTAL | 2.575e-04 | 1.944e-04 | 1.944e-04 | 1.000e+00
7: [INFO] expanded trust region. new box size: 3.8443
7: [INFO] iteration 10
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | -3.083e-04 | 3.333e-01 | 3.333e-01 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 3.715e-04 | -3.332e-01 | -3.332e-01 | 1.000e+00
7: TOTAL | 6.314e-05 | 6.314e-05 | 6.314e-05 | 1.000e+00
7: [INFO] expanded trust region. new box size: 5.7665
7: [INFO] iteration 11
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | -3.336e-01 | 1.084e+00 | 1.084e+00 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 3.336e-01 | -1.084e+00 | -1.084e+00 | 1.000e+00
7: TOTAL | 4.996e-16 | 2.264e-10 | -2.174e-09 | -9.601e+00
7: [INFO] shrunk trust region. new box size: 0.5767
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | -3.336e-01 | 4.823e-02 | 4.823e-02 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 3.336e-01 | -4.823e-02 | -4.823e-02 | 1.000e+00
7: TOTAL | 4.996e-16 | -2.416e-11 | -1.429e-11 | 5.916e-01
7: [INFO] converged because improvement was small (-2.416e-11 < 1.000e-10)
7: [INFO] not all constraints are satisfied. increasing penalties
7: [INFO] iteration 1
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | -3.336e-01 | -3.336e-01 | -3.336e-01 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 3.336e+00 | 3.336e+00 | 3.336e+00 | 1.000e+00
7: TOTAL | 3.002e+00 | 3.002e+00 | 3.002e+00 | 1.000e+00
7: [INFO] expanded trust region. new box size: 0.8650
7: [INFO] iteration 2
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 1.603e-10 | -2.792e-08 | -2.792e-08 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 0.000e+00 | 0.000e+00 | 0.000e+00 | ------
7: TOTAL | 1.603e-10 | -2.792e-08 | -2.792e-08 | 1.000e+00
7: [INFO] converged because improvement was small (-2.792e-08 < 1.000e-10)
7: [INFO] woo-hoo! constraints are satisfied (to tolerance 1.00e-04)
7: [INFO]
7: ==================
7: Optimization results:
7: status: CONVERGED
7: cost values: (1.60254e-10)
7: constraint violations: (0)
7: n func evals: 15
7: n qp solves: 14
7: ==================
7: [INFO] checking (0.00258134, 9.36204e-11) ?= (0, 0)
7: [ OK ] SQP.TP3 (5 ms)
7: [ RUN ] SQP.TP6
7: [INFO] iteration 1
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 8.100e+01 | 1.790e+00 | 1.790e+00 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 9.900e+02 | 2.100e+01 | 2.090e+01 | 9.952e-01
7: TOTAL | 1.071e+03 | 2.279e+01 | 2.269e+01 | 9.956e-01
7: [INFO] expanded trust region. new box size: 0.1500
7: [INFO] iteration 2
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 7.921e+01 | 2.648e+00 | 2.647e+00 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 9.691e+02 | 3.120e+01 | 3.097e+01 | 9.928e-01
7: TOTAL | 1.048e+03 | 3.385e+01 | 3.362e+01 | 9.934e-01
7: [INFO] expanded trust region. new box size: 0.2250
7: [INFO] iteration 3
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 7.656e+01 | 3.887e+00 | 3.887e+00 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 9.381e+02 | 4.613e+01 | 4.562e+01 | 9.890e-01
7: TOTAL | 1.015e+03 | 5.001e+01 | 4.951e+01 | 9.899e-01
7: [INFO] expanded trust region. new box size: 0.3375
7: [INFO] iteration 4
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 7.268e+01 | 5.640e+00 | 5.640e+00 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 8.925e+02 | 6.767e+01 | 6.653e+01 | 9.832e-01
7: TOTAL | 9.652e+02 | 7.331e+01 | 7.217e+01 | 9.845e-01
7: [INFO] expanded trust region. new box size: 0.5063
7: [INFO] iteration 5
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 6.704e+01 | 8.034e+00 | 8.034e+00 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 8.260e+02 | 9.809e+01 | 9.552e+01 | 9.739e-01
7: TOTAL | 8.930e+02 | 1.061e+02 | 1.036e+02 | 9.758e-01
7: [INFO] expanded trust region. new box size: 0.7594
7: [INFO] iteration 6
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 5.900e+01 | 1.109e+01 | 1.109e+01 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 7.305e+02 | 1.394e+02 | 1.337e+02 | 9.586e-01
7: TOTAL | 7.895e+02 | 1.505e+02 | 1.448e+02 | 9.617e-01
7: [INFO] expanded trust region. new box size: 1.1391
7: [INFO] iteration 7
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 4.791e+01 | 1.447e+01 | 1.447e+01 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 5.968e+02 | 1.919e+02 | 1.789e+02 | 9.324e-01
7: TOTAL | 6.447e+02 | 2.063e+02 | 1.934e+02 | 9.371e-01
7: [INFO] expanded trust region. new box size: 1.7086
7: [INFO] iteration 8
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 3.344e+01 | 1.684e+01 | 1.684e+01 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 4.179e+02 | 2.489e+02 | 2.197e+02 | 8.827e-01
7: TOTAL | 4.513e+02 | 2.657e+02 | 2.365e+02 | 8.901e-01
7: [INFO] expanded trust region. new box size: 2.5629
7: [INFO] iteration 9
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 1.660e+01 | 1.311e+01 | 1.311e+01 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 1.982e+02 | 1.982e+02 | 1.496e+02 | 7.545e-01
7: TOTAL | 2.148e+02 | 2.113e+02 | 1.627e+02 | 7.698e-01
7: [INFO] expanded trust region. new box size: 3.8443
7: [INFO] iteration 10
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 3.491e+00 | 3.369e+00 | 3.368e+00 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 4.865e+01 | 4.865e+01 | 2.560e+01 | 5.263e-01
7: TOTAL | 5.214e+01 | 5.202e+01 | 2.897e+01 | 5.569e-01
7: [INFO] expanded trust region. new box size: 5.7665
7: [INFO] iteration 11
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 1.227e-01 | 1.227e-01 | 1.227e-01 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 2.305e+01 | 2.305e+01 | 2.182e+01 | 9.467e-01
7: TOTAL | 2.317e+01 | 2.317e+01 | 2.194e+01 | 9.470e-01
7: [INFO] expanded trust region. new box size: 8.6498
7: [INFO] iteration 12
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 2.495e-11 | 1.743e-17 | -4.640e-14 | ------
7: CONSTRAINTS | -------------------------------------------------
7: g | 1.227e+00 | 1.227e+00 | 1.227e+00 | 1.000e+00
7: TOTAL | 1.227e+00 | 1.227e+00 | 1.227e+00 | 1.000e+00
7: [INFO] expanded trust region. new box size: 12.9746
7: [INFO] iteration 13
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | 2.500e-11 | 5.311e-18 | -2.191e-17 | ------
7: CONSTRAINTS | -------------------------------------------------
7: g | 3.152e-11 | -2.404e-11 | -2.404e-11 | ------
7: TOTAL | 5.652e-11 | -2.404e-11 | -2.404e-11 | 1.000e+00
7: [INFO] converged because improvement was small (-2.404e-11 < 1.000e-10)
7: [INFO] woo-hoo! constraints are satisfied (to tolerance 1.00e-04)
7: [INFO]
7: ==================
7: Optimization results:
7: status: CONVERGED
7: cost values: (2.50001e-11)
7: constraint violations: (3.15248e-11)
7: n func evals: 14
7: n qp solves: 13
7: ==================
7: [INFO] checking (0.999995, 0.99999) ?= (1, 1)
7: [ OK ] SQP.TP6 (5 ms)
7: [ RUN ] SQP.TP7
7: [INFO] iteration 1
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | -3.906e-01 | -2.000e-02 | -1.879e-02 | 9.395e-01
7: CONSTRAINTS | -------------------------------------------------
7: g | 2.500e+01 | 4.400e+00 | 4.138e+00 | 9.404e-01
7: TOTAL | 2.461e+01 | 4.380e+00 | 4.119e+00 | 9.404e-01
7: [INFO] expanded trust region. new box size: 0.1500
7: [INFO] iteration 2
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | -3.718e-01 | -2.636e-02 | -2.357e-02 | 8.943e-01
7: CONSTRAINTS | -------------------------------------------------
7: g | 2.086e+01 | 5.825e+00 | 5.296e+00 | 9.091e-01
7: TOTAL | 2.049e+01 | 5.799e+00 | 5.272e+00 | 9.091e-01
7: [INFO] expanded trust region. new box size: 0.2250
7: [INFO] iteration 3
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | -3.482e-01 | -3.115e-02 | -2.486e-02 | 7.979e-01
7: CONSTRAINTS | -------------------------------------------------
7: g | 1.557e+01 | 7.186e+00 | 6.181e+00 | 8.601e-01
7: TOTAL | 1.522e+01 | 7.155e+00 | 6.156e+00 | 8.604e-01
7: [INFO] expanded trust region. new box size: 0.3375
7: [INFO] iteration 4
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | -3.233e-01 | -2.797e-02 | -1.553e-02 | 5.553e-01
7: CONSTRAINTS | -------------------------------------------------
7: g | 9.385e+00 | 7.876e+00 | 6.166e+00 | 7.829e-01
7: TOTAL | 9.062e+00 | 7.848e+00 | 6.151e+00 | 7.837e-01
7: [INFO] expanded trust region. new box size: 0.5063
7: [INFO] iteration 5
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | -3.078e-01 | 8.868e-01 | 8.900e-01 | 1.004e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 3.219e+00 | 3.219e+00 | 1.654e+00 | 5.138e-01
7: TOTAL | 2.911e+00 | 4.106e+00 | 2.544e+00 | 6.196e-01
7: [INFO] expanded trust region. new box size: 0.7594
7: [INFO] iteration 6
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | -1.198e+00 | 1.382e+00 | 1.212e+00 | 8.768e-01
7: CONSTRAINTS | -------------------------------------------------
7: g | 1.565e+00 | 1.565e+00 | -1.254e+00 | -8.011e-01
7: TOTAL | 3.674e-01 | 2.948e+00 | -4.188e-02 | -1.421e-02
7: [INFO] shrunk trust region. new box size: 0.0759
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | -1.198e+00 | -2.591e-03 | -2.721e-03 | 1.050e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 1.565e+00 | 6.569e-01 | 6.188e-01 | 9.419e-01
7: TOTAL | 3.674e-01 | 6.543e-01 | 6.160e-01 | 9.415e-01
7: [INFO] expanded trust region. new box size: 0.1139
7: [INFO] iteration 7
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | -1.195e+00 | -8.269e-03 | -8.796e-03 | 1.064e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 9.465e-01 | 8.729e-01 | 7.972e-01 | 9.132e-01
7: TOTAL | -2.486e-01 | 8.647e-01 | 7.884e-01 | 9.118e-01
7: [INFO] expanded trust region. new box size: 0.1709
7: [INFO] iteration 8
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | -1.186e+00 | 2.837e-01 | 2.815e-01 | 9.923e-01
7: CONSTRAINTS | -------------------------------------------------
7: g | 1.493e-01 | 1.493e-01 | 1.679e-02 | 1.125e-01
7: TOTAL | -1.037e+00 | 4.329e-01 | 2.983e-01 | 6.890e-01
7: [INFO] expanded trust region. new box size: 0.2563
7: [INFO] iteration 9
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | -1.468e+00 | 2.757e-01 | 2.677e-01 | 9.710e-01
7: CONSTRAINTS | -------------------------------------------------
7: g | 1.325e-01 | 1.325e-01 | -6.533e-02 | -4.931e-01
7: TOTAL | -1.335e+00 | 4.082e-01 | 2.023e-01 | 4.957e-01
7: [INFO] expanded trust region. new box size: 0.3844
7: [INFO] iteration 10
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | -1.735e+00 | 3.460e-02 | 2.836e-02 | 8.196e-01
7: CONSTRAINTS | -------------------------------------------------
7: g | 1.978e-01 | 1.978e-01 | -9.776e-03 | -4.942e-02
7: TOTAL | -1.538e+00 | 2.324e-01 | 1.859e-02 | 7.997e-02
7: [INFO] shrunk trust region. new box size: 0.0384
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | -1.735e+00 | -2.607e-02 | -2.609e-02 | 1.001e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 1.978e-01 | 1.653e-01 | 1.606e-01 | 9.716e-01
7: TOTAL | -1.538e+00 | 1.392e-01 | 1.345e-01 | 9.661e-01
7: [INFO] expanded trust region. new box size: 0.0577
7: [INFO] iteration 11
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | -1.709e+00 | 1.253e-02 | 1.248e-02 | 9.962e-01
7: CONSTRAINTS | -------------------------------------------------
7: g | 3.721e-02 | 3.721e-02 | 3.023e-02 | 8.125e-01
7: TOTAL | -1.672e+00 | 4.974e-02 | 4.272e-02 | 8.588e-01
7: [INFO] expanded trust region. new box size: 0.0865
7: [INFO] iteration 12
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | -1.722e+00 | 1.471e-02 | 1.462e-02 | 9.942e-01
7: CONSTRAINTS | -------------------------------------------------
7: g | 6.976e-03 | 6.976e-03 | -8.213e-03 | -1.177e+00
7: TOTAL | -1.715e+00 | 2.168e-02 | 6.407e-03 | 2.955e-01
7: [INFO] expanded trust region. new box size: 0.1297
7: [INFO] iteration 13
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | -1.736e+00 | -4.367e-03 | -4.367e-03 | 1.000e+00
7: CONSTRAINTS | -------------------------------------------------
7: g | 1.519e-02 | 1.519e-02 | 1.516e-02 | 9.978e-01
7: TOTAL | -1.721e+00 | 1.082e-02 | 1.079e-02 | 9.969e-01
7: [INFO] expanded trust region. new box size: 0.1946
7: [INFO] iteration 14
7: [INFO]
7: | oldexact | dapprox | dexact | ratio
7: COSTS | -------------------------------------------------
7: f | -1.732e+00 | -7.308e-06 | -7.292e-06 | 9.979e-01
7: CONSTRAINTS | -------------------------------------------------
7: g | 3.365e-05 | 3.364e-05 | 2.887e-05 | 8.580e-01
7: TOTAL | -1.732e+00 | 2.634e-05 | 2.157e-05 | 8.192e-01
7: [INF

I am running ./arm_to_cart_pose.py on ubuntu 16.04 with openrave.0.9. sympy0.7.1
I am able to see the optimization process and it results converged but unble to visualize it with openrave logging error. Kindly please help me how I can execute the trajectory in openravepy by solving the error.

I ran the executable
./arm_to_cart_target.py

but I keep getting the following

=================
Optimization results:
status: CONVERGED
cost values: (4.38951, 0, 0, 0, 0, 0, 0, 0, 0, 0)
constraint violations: (5.32029e-10)
n func evals: 8
n qp solves: 7
==================
[('joint_vel', 4.389512672128216), ('cont_coll_0', 0.0), ('cont_coll_1', 0.0), ('cont_coll_2', 0.0), ('cont_coll_3', 0.0), ('cont_coll_4', 0.0), ('cont_coll_5', 0.0), ('cont_coll_6', 0.0), ('cont_coll_7', 0.0), ('cont_coll_8', 0.0)][('pose', 5.32028718373695e-10)]
result is printed
No handlers could be found for logger "openravepy"
2019-03-12 17:22:00,263 openrave [WARN] [kinbody.cpp:1299 KinBody::GetLinkEnableStatesMask] pr2 has too many links and will only return enable mask for first 64
result is printed after collision
checking final constraint
done

please help me how I can execute on the robot?

I am trying to move to a cartesian pose using trajopt optimization but I am giving with respect to a base_link frame but my robot moves to someother position . I compared the position with the optimized trajectory end point with joint values is the position where robot reaches but the pose given for optimization is different. Kindly please help me how to rectify this problem.

Hello,

I am able to compute trajectories without collision checking with my robot model. However, when I add collision cost to json file it is throwing seg fault. I'm on Ubuntu 16.04 with Openrave 0.9 (compiled from master).

This is the json file:

{
  "basic_info" : {
    "n_steps" : 9,
    "manip" : "whole_body",
    "start_fixed" : true
  },
  "costs" : [
  {
    "name" : "disc_coll",
    "type" : "collision",
    "params" :
    {
      "coeffs" : [ 50 ],
      "continuous" : false,
      "dist_pen" : [ 0.040 ],
      "first_step" : 0,
      "last_step" : 7
    }
    }
  ],
  "constraints" : [
  {
    "type" : "pose",
    "params" : {
      "xyz" : [2.0, 0.5, 0.8],
      "wxyz": [0.966, 0.0, 0.0, 0.259],
      "link": "hand_palm_link",
      "timestep" : 7
                }

  }
  ],

  "init_info" : {
      "type" : "stationary"
  }
}

And this is my function

void FRASIEROpenRAVE::computeTrajectory(){

  OpenRAVE::EnvironmentMutex::scoped_lock lockenv(env_->GetMutex());
  std::string json_path;
  json_path = config_path_ + "whole_body_traj_collision.json";

  Json::Value whole_body_traj_json;
  Json::Reader reader;
  std::ifstream i(json_path);

  bool json_parsing_success = reader.parse(i, whole_body_traj_json, true);

  if (!json_parsing_success) {
    std::cout << "can't read json!"  << std::endl;
    std::cout << reader.getFormatedErrorMessages() << std::endl;
  }
  std::vector<OpenRAVE::KinBodyPtr> bodies;

  env_->GetBodies(bodies);
   for(auto i:bodies){
     std::cout<<i->GetName()<<std::endl;
   }



   std::vector<double> q, q0;
   q0 = {0, 0, 0, 0, 0, 0, 0, 0};
   std::vector<int> q_idx;
   //manip_->GetArmIndices(q_idx);

   manip_ = hsr_->GetManipulator("whole_body");
   hsr_->SetActiveDOFs(manip_->GetArmIndices());

   hsr_->SetDOFValues(q0,1,manip_->GetArmIndices());
   hsr_->GetActiveDOFValues(q);
   for (auto k:q) {
     std::cout << k << '\n';
   }

   ros::Duration(1).sleep();
   // CollisionCheckerPtr colll = CreateCollisionChecker(env_);
   trajopt::ProblemConstructionInfo pci(env_);
   pci.fromJson(whole_body_traj_json);
   trajopt::TrajOptProbPtr traj_prob = trajopt::ConstructProblem(pci);

  trajopt::TrajOptResultPtr traj_result = trajopt::OptimizeProblem(traj_prob, true);
  std::cout << "traj : " << std::endl << traj_result->traj << std::endl;

}

And finally this is gdb output:

Thread 1 "frasier_openrav" received signal SIGSEGV, Segmentation fault.
0x00007fffed1e70e4 in convexHullSupport(btVector3 const&, btVector3 const*, int, btVector3 const&) [clone .constprop.751] () from /home/tarik/develop/robotics/trajopt/build/lib/libtrajopt.so

It is working without any problems if I don't add collision cost.

Hi,
I am trying to install TrajOpt on Ubuntu 18.04 but I get the following compilation error

[ 55%] Built target osgviewer
Scanning dependencies of target trajopt
[ 57%] Building CXX object src/trajopt/CMakeFiles/trajopt.dir/traj_plotter.cpp.o
[ 58%] Building CXX object src/trajopt/CMakeFiles/trajopt.dir/trajectory_costs.cpp.o
[ 60%] Building CXX object src/trajopt/CMakeFiles/trajopt.dir/kinematic_terms.cpp.o
[ 62%] Building CXX object src/trajopt/CMakeFiles/trajopt.dir/collision_terms.cpp.o
[ 64%] Linking CXX executable ../../../bin/sco-unit
[ 64%] Built target sco-unit
[ 66%] Building CXX object src/trajopt/CMakeFiles/trajopt.dir/json_marshal.cpp.o
[ 67%] Building CXX object src/trajopt/CMakeFiles/trajopt.dir/problem_description.cpp.o
[ 69%] Building CXX object src/trajopt/CMakeFiles/trajopt.dir/bullet_collision_checker.cpp.o
[ 71%] Building CXX object src/trajopt/CMakeFiles/trajopt.dir/configuration_space.cpp.o
[ 73%] Building CXX object src/trajopt/CMakeFiles/trajopt.dir/utils.cpp.o
[ 75%] Building CXX object src/trajopt/CMakeFiles/trajopt.dir/rave_utils.cpp.o
/home/driesdirckx/repos/trajopt/src/trajopt/bullet_collision_checker.cpp: In member function ‘void {anonymous}::BulletCollisionChecker::AddKinBody(const KinBodyPtr&)’:
/home/driesdirckx/repos/trajopt/src/trajopt/bullet_collision_checker.cpp:543:65: error: cannot convert ‘OpenRAVE::UserDataPtr {aka boost::shared_ptrOpenRAVE::UserData}’ to ‘bool’ in initialization
bool useTrimesh = trajopt::GetUserData(*body, "bt_use_trimesh");
^
src/trajopt/CMakeFiles/trajopt.dir/build.make:206: recipe for target 'src/trajopt/CMakeFiles/trajopt.dir/bullet_collision_checker.cpp.o' failed
make[2]: *** [src/trajopt/CMakeFiles/trajopt.dir/bullet_collision_checker.cpp.o] Error 1
make[2]: *** Waiting for unfinished jobs....
CMakeFiles/Makefile2:443: recipe for target 'src/trajopt/CMakeFiles/trajopt.dir/all' failed
make[1]: *** [src/trajopt/CMakeFiles/trajopt.dir/all] Error 2
Makefile:94: recipe for target 'all' failed
make: *** [all] Error 2

I first build OpenRave after which I build OpenSceneGraph and then I try to build TrajOpt from this repository. I already tried out the different solutions offered in issue #20 .I used the following versions and installation procedures for OpenRave and OSG, ..:

• OpenRAVE 0.9.0: http://robots.uc3m.es/gitbook-installation-guides/install-openrave.html
• OpenSceneGraph 3.4.1: http://robots.uc3m.es/gitbook-installation-guides/install-openrave.html OR https://scaron.info/teaching/installing-openrave-on-ubuntu-16.04.html?fbclid=IwAR1t8Rd0i58b_F4uEddnrbqAZYG7NJOQEktZhOMo53EKiJVotBWDTULrKng
• Boost: version 1.69.0 (also gave the error with version 1.58.0)

Both procedure for installing OpenRAVE and OSG give the same error if I try to compile TrajOpt. Installing the flexible collision library FCL has no effect on the error that I get.

Do you have any idea how I can fix this and successfully build TrajOpt ? Thanks in advance!

I installed trajoptpy, but when I try to run the program it results in the above error.
File "/home/lakshmi/git/trajopt/trajoptpy/init.py", line 1, in
from ctrajoptpy import *
ImportError: No module named ctrajoptpy

Test project /home/lakshmi/tes_traj/trajopt/build
Start 1: arm_to_joint_target.py
1/10 Test #1: arm_to_joint_target.py ............. Passed 2.87 sec
Start 2: arm_to_cart_target.py
2/10 Test #2: arm_to_cart_target.py ..............***Failed 11.83 sec
Start 3: fullbody_plan.py
3/10 Test #3: fullbody_plan.py ................... Passed 3.71 sec
Start 4: position_base.py
4/10 Test #4: position_base.py ................... Passed 1.94 sec
Start 5: this_side_up.py
5/10 Test #5: this_side_up.py ....................***Failed 11.36 sec
Start 6: arm_to_cart_target_position_only
6/10 Test #6: arm_to_cart_target_position_only ...***Failed 11.55 sec
Start 7: sco-unit
7/10 Test #7: sco-unit ........................... Passed 0.04 sec
Start 8: collision-checker-unit
8/10 Test #8: collision-checker-unit ............. Passed 0.06 sec
Start 9: planning-unit
9/10 Test #9: planning-unit ...................... Passed 1.70 sec
Start 10: cast-cost-unit
10/10 Test #10: cast-cost-unit ..................... Passed 0.06 sec

70% tests passed, 3 tests failed out of 10

Total Test time (real) = 45.11 sec

The following tests FAILED:
2 - arm_to_cart_target.py (Failed)
5 - this_side_up.py (Failed)
6 - arm_to_cart_target_position_only (Failed)
Errors while running CTest

I noticed that the repository has a BSD license but what to make sure that also applies to the this library also. If not what is the license of the library?

Error:
cpp:543:65: error: cannot convert ‘OpenRAVE::UserDataPtr {aka boost::shared_ptrOpenRAVE::UserData}’ to ‘bool’ in initialization

Line:

Fix:
bool useTrimesh = bool(trajopt::GetUserData(*body, "bt_use_trimesh"));

I am trying to run the python_examples on ubuntu 16.04 with openrave 0.9 and trajopt but my ctests failed for trajopt due to import error which is caused by undefined symbol in ctrajoptpy.so. Kindly please help me how to solve this error and the python_examples

Error:
/usr/bin/ld: ../../../3rdpartylib/bpmpd_linux64.a(supnode.o): relocation R_X86_64_32 against `.rodata.str1.8' can not be used when making a PIE object; recompile with -fPIC

Line to change:

Fix is to make static:
target_link_libraries(bpmpd_caller ${BPMPD_LIBRARY} -static)

When I attempt to build trajopt with CLOUDPROC , I get the error message:

[ 86%] Building CXX object src/cloudproc/CMakeFiles/cloudproc.dir/mesh_simplification.cpp.o
In file included from /home/zxy/learning-code/trajopt-lfd/src/cloudproc/mesh_simplification.cpp:8:0:
/home/zxy/learning-code/trajopt-lfd/src/cloudproc/vtkQuadricDecimation2.h:64:49: error: expected initializer before ‘:’ token
 class VTK_GRAPHICS_EXPORT vtkQuadricDecimation2 : public vtkPolyDataAlgorithm
                                                 ^
In file included from /usr/include/boost/type_traits/has_operator.hpp:30:0,
                 from /usr/include/boost/type_traits.hpp:35,
                 from /usr/include/pcl-1.7/pcl/register_point_struct.h:65,
                 from /usr/include/pcl-1.7/pcl/point_types.h:44,
                 from /home/zxy/learning-code/trajopt-lfd/src/cloudproc/mesh_simplification.cpp:10:
/usr/include/boost/type_traits/has_logical_not.hpp:13:28: error: expected declaration before end of line
src/cloudproc/CMakeFiles/cloudproc.dir/build.make:110: recipe for target 'src/cloudproc/CMakeFiles/cloudproc.dir/mesh_simplification.cpp.o' failed
make[2]: *** [src/cloudproc/CMakeFiles/cloudproc.dir/mesh_simplification.cpp.o] Error 1
CMakeFiles/Makefile2:793: recipe for target 'src/cloudproc/CMakeFiles/cloudproc.dir/all' failed
make[1]: *** [src/cloudproc/CMakeFiles/cloudproc.dir/all] Error 2
Makefile:94: recipe for target 'all' failed
make: *** [all] Error 2

HI,

thanks for the job you've made on this.

I've tried to used this solution to found gurobi libraries in my project, but the compiler doesn't find the header "gurobi_c++.h".

I used "gurobi_c++.h" and <gurobi_c++.h>

Has you one example?

when I try importing ctrajopt, I get the following error:
from ctrajoptpy import *
ImportError: /home/lakshmi/trajopt/build/lib/ctrajoptpy.so: undefined symbol: _ZTIN7trajopt11RobotAndDOFE
Kindly please help me, how to solve this error.

I am moving to a cartesian position using trajopt which results in an error of 7cm along y direction and 1cm along x-direction. How can I achieve moving to exact cartesian position.

I was playing around with the drc_walk.py example, and I noticed that it will only run properly if BUILD_HUMANOIDS is set at compile time. It might be a good idea to put a note in the file about this to save future confusion. It's particularly confusing because drc_walk.py imports humanoidspy but never explicitly calls it, even though it will segfault if the import is not present.

I'm following the instalation instructions on http://rll.berkeley.edu/trajopt/doc/sphinx_build/html/install.html#instructions
When I make, I get the following error:
fatal error: osg/CameraNode: No such file or directory
compilation terminated.
src/osgviewer/CMakeFiles/osgviewer.dir/build.make:62: recipe for target 'src/osgviewer/CMakeFiles/osgviewer.dir/osgviewer.cpp.o' failed
make[2]: *** [src/osgviewer/CMakeFiles/osgviewer.dir/osgviewer.cpp.o] Error 1
CMakeFiles/Makefile2:788: recipe for target 'src/osgviewer/CMakeFiles/osgviewer.dir/all' failed
make[1]: *** [src/osgviewer/CMakeFiles/osgviewer.dir/all] Error 2

I'm using trajopt git commit 9c634c0. Does anyone know how to solve this error?

I want to use trajopt for getting trapezoidal velocity profile. I would like to know how to use constrain and what are the variables to constraint to obtain trapezoidal cartesian velocity. And how to set variables or from moving one point to another in a linear motion with trapezoidal constant velocity. Kindly please help me with an example program or explanation

import: not authorized argparse' @ error/constitute.c/WriteImage/1028. ./this_side_up.py: line 2: syntax error near unexpected token ('
./this_side_up.py: line 2: `parser = argparse.ArgumentParser()'

When I executed trajopt optimization on URSim the robot moves with a displacement of 10cm and 7cm along x-axis and y-axis? I am defining pose as a constraint itself but still it displaces by 40cm.
Constraints are given as follows

void traj_prob::addLinearMotion(trajopt::ProblemConstructionInfo& pci,
Isometry3d start_pose,
Isometry3d end_pose,
int num_steps,
int first_time_step)
{
Vector3d xyz_delta = (end_pose.translation() - start_pose.translation()) / (num_steps - 1);

Quaterniond approach_rotation(start_pose.linear());
Matrix3d rotation_diff = (start_pose.linear().inverse() * end_pose.linear());
AngleAxisd aa_rotation_diff(rotation_diff);
double angle_delta = aa_rotation_diff.angle() / (num_steps - 1);
Vector3d delta_axis = aa_rotation_diff.axis();

for(int i=0; i<num_steps; i++)
{
    std::shared_ptr<CartPoseTermInfo> pose_constraint = std::shared_ptr<CartPoseTermInfo>(new CartPoseTermInfo);
        pose_constraint->term_type = TT_CNT;
        pose_constraint->link = ee_link_;
        pose_constraint->timestep = i + first_time_step;
        pose_constraint->xyz = start_pose.translation() + xyz_delta * i;

        Quaterniond rotation_delta(cos(0.5 * angle_delta * i),
                                   delta_axis.x() * sin(0.5 * angle_delta * i),
                                   delta_axis.y() * sin(0.5 * angle_delta * i),
                                   delta_axis.z() * sin(0.5 * angle_delta * i));
        Quaterniond rotation = rotation_delta * approach_rotation;

        /* Fill Code:
             . Set the pose rotation
             . Set pos_coeffs to all 10s
             . Set rot_coeffs to all 10s
             . Define the pose name as pose_[timestep]
             . pushback the constraint to cnt_infos
        */
        /* ========  ENTER CODE HERE ======== */
        pose_constraint->wxyz = Vector4d(rotation.w(), rotation.x(), rotation.y(), rotation.z());
        pose_constraint->pos_coeffs = Vector3d(10.0, 10.0, 10.0);
        pose_constraint->rot_coeffs = Vector3d(10.0, 10.0, 10.0);
        pose_constraint->name = "pose_" + std::to_string(i + first_time_step);
        pci.cnt_infos.push_back(pose_constraint);
}

}

TrajOptProbPtr traj_prob::generatePickProblem(Isometry3d& approach_pose,
Isometry3d& final_pose,
int steps_per_phase)
{
trajopt::ProblemConstructionInfo pci(env_);
// Add kinematics
pci.kin = kin_;

  /* Fill Code: Define the basic info
       . Set the pci number of steps
       . Set the start_fixed to false
       . Set the manipulator name (see class members)
       . Set dt lower limit
       . Set dt upper limit
  */
  /* ========  ENTER CODE HERE ======== */
  pci.basic_info.n_steps = steps_per_phase * 2;
  pci.basic_info.manip = manipulator_;
  pci.basic_info.dt_lower_lim = 0.2;
  pci.basic_info.dt_upper_lim = .5;
  pci.basic_info.start_fixed = true;
  pci.basic_info.use_time = false;

  //---------------------------------------------------------
  // ---------------- Fill Init Info ------------------------
  //---------------------------------------------------------

  // To use JOINT_INTERPOLATED - a linear interpolation of joint values
  //  pci.init_info.type = InitInfo::JOINT_INTERPOLATED;
  //  pci.init_info.data = numericalIK(final_pose);  // Note, take the last value off if using time (just want jnt
  //  values)

  // To use STATIONARY - all jnts initialized to the starting value
  pci.init_info.type = InitInfo::STATIONARY;
  pci.init_info.dt = 0.5;

  //---------------------------------------------------------
  // ---------------- Fill Term Infos -----------------------
  //---------------------------------------------------------

  // ================= Collision cost =======================
  std::shared_ptr<CollisionTermInfo> collision(new CollisionTermInfo);
  /* Fill Code:
       . Define the cost name
       . Define the term type (This is a cost)
       . Define this cost as continuous
       . Define the first time step
       . Define the last time step
       . Set the cost gap to be 1
       . Define the penalty type as sco::squared
  */
  /* ========  ENTER CODE HERE ======== */
  collision->name = "collision";
  collision->term_type = TT_COST;
  collision->continuous = true;
  collision->first_step = 0;
  collision->last_step = pci.basic_info.n_steps - 1;
  collision->gap = 1;
  collision->info = createSafetyMarginDataVector(pci.basic_info.n_steps, 0.025, 40);

  pci.cost_infos.push_back(collision);

  // ================= Velocity cost =======================
  std::shared_ptr<JointVelTermInfo> jv(new JointVelTermInfo);

  // Taken from iiwa documentation (radians/s) and scaled by 0.8
  std::vector<double> vel_lower_lim{ 1.71*-0.8, 1.71*-0.8, 1.75*-0.8, 2.27*-0.8, 2.44*-0.8, 2.14*-0.8 };
  std::vector<double> vel_upper_lim{ 1.71*0.8, 1.71*0.8, 1.75*0.8, 2.27*0.8, 2.44*0.8, 2.14*0.8 };

  /* Fill Code:
       . Define the term time (This is a cost)
       . Define the first time step
       . Define the last time step
       . Define the term name
  */
  /* ========  ENTER CODE HERE ======== */
  jv->targets = std::vector<double>(6, 0.0);
  jv->coeffs = std::vector<double>(6, 5.0);
//  jv->lower_tols = vel_lower_lim;
//  jv->upper_tols = vel_upper_lim;
  jv->term_type = TT_COST;
  jv->first_step = 0;
  jv->last_step = pci.basic_info.n_steps - 1;
  jv->name = "joint_velocity_cost";

  pci.cost_infos.push_back(jv);

  std::shared_ptr<CartVelTermInfo> cv(new CartVelTermInfo);
  cv->term_type = TT_COST;
  cv->first_step = 0;
  cv->last_step = pci.basic_info.n_steps-1;
  cv->max_displacement = 0.05;
  cv->link = "tool0";

  pci.cost_infos.push_back(cv);

  // ================= Path waypoints =======================
  this->addLinearMotion(pci, approach_pose, final_pose, steps_per_phase, steps_per_phase);

  //  this->addTotalTimeCost(pci, 50.0);

TrajOptProbPtr result = ConstructProblem(pci);
return result;

}
Kindly please suggest me if I am doing something wrong

Hello everyone,

I'm using ubuntu 18.04. After succesful compile, I exported the pythonpath and tried to run the python example, I got the following error

:~/Documents/openrave-installation/trajopt/src/trajopt/test$ python test_initializations.py

You can set logging level with TRAJOPT_LOG_THRESH. Valid values: FATAL ERROR WARN INFO DEBUG TRACE. Defaulting to INFO

Traceback (most recent call last):
File "test_initializations.py", line 5, in
from trajoptpy.cart_traj_init import shortest_paths
File "/home/lab/Documents/openrave-installation/trajopt/trajoptpy/init.py", line 1, in
from ctrajoptpy import *
ImportError: dynamic module does not define init function (initctrajoptpy)

I'm able to run the last 4 ctest as shown below

Test project /home/lab/Documents/openrave-installation/trajopt/build
Start 1: arm_to_joint_target.py
1/10 Test #1: arm_to_joint_target.py .............***Failed 0.20 sec
Start 2: arm_to_cart_target.py
2/10 Test #2: arm_to_cart_target.py ..............***Failed 0.19 sec
Start 3: fullbody_plan.py
3/10 Test #3: fullbody_plan.py ...................***Failed 0.19 sec
Start 4: position_base.py
4/10 Test #4: position_base.py ...................***Failed 0.03 sec
Start 5: this_side_up.py
5/10 Test #5: this_side_up.py ....................***Failed 0.19 sec
Start 6: arm_to_cart_target_position_only
6/10 Test #6: arm_to_cart_target_position_only ...***Failed 0.19 sec
Start 7: sco-unit
7/10 Test #7: sco-unit ........................... Passed 0.03 sec
Start 8: collision-checker-unit
8/10 Test #8: collision-checker-unit ............. Passed 0.03 sec
Start 9: planning-unit
9/10 Test #9: planning-unit ...................... Passed 0.93 sec
Start 10: cast-cost-unit
10/10 Test #10: cast-cost-unit ..................... Passed 0.03 sec

40% tests passed, 6 tests failed out of 10

Total Test time (real) = 2.02 sec

Please kindly help, thank you.

I would like to plan the whole body motion of a humanoid robot (Atlas) using trajopt. I follow the example in drc_walk, and it works well.

However, I realize that when I use press_button_request() to plan the motion, only the upper body is moving (the leg remains exactly at the same position). This is strange to me because the constraints only specify that the poses of the feet has to remain the same, but not the joint angles. The joint angles of the leg should be able to move, as long as it still keeps the feet at the same locations. The robot should be able to lower its knee if it tries to grab something below, as long as this still statisfies the ZMP constraint. Instead. all the planning show that the legs remain at the same position.

This is the same when I try to do step-forward: only the leg is moving but all the upper body part remains stationary.

Does anyone have any idea how I can make the whole body move together? Thank you!

OS: Ubuntu 18.04
Trajopt: Build from the latest commit of trajopt from source
Dependencies:

• python 2.7.17
• openrave 0.9, commit 7c5f5e27eec2b2ef10aa63fbc519a998c276f908 (built via scripts from https://github.com/crigroup/openrave-installation)
• boost 1.5.8
• OpenSceneGraph commit 1f89e6eb1087add6cd9c743ab07a5bce53b2f480

Output of python arm_to_cart_target.py:

You can set logging level with TRAJOPT_LOG_THRESH. Valid values: FATAL ERROR WARN INFO DEBUG TRACE. Defaulting to INFO
2020-05-06 17:42:51,857 openrave [WARN] [kinbody.cpp:1576 KinBody::SetDOFValues] dof 12 value is not in limits 0.000000e+00<1.150000e-02
2020-05-06 17:42:51,857 openrave [WARN] [kinbody.cpp:1587 KinBody::SetDOFValues] dof 18 value is not in limits 0.000000e+00<-1.500001e-01
2020-05-06 17:42:51,857 openrave [WARN] [kinbody.cpp:1587 KinBody::SetDOFValues] dof 20 value is not in limits 0.000000e+00<-1.000000e-01
2020-05-06 17:42:51,857 openrave [WARN] [kinbody.cpp:1587 KinBody::SetDOFValues] dof 30 value is not in limits 0.000000e+00<-1.500001e-01
2020-05-06 17:42:51,857 openrave [WARN] [kinbody.cpp:1587 KinBody::SetDOFValues] dof 32 value is not in limits 0.000000e+00<-1.000000e-01
2020-05-06 17:42:51,869 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/base_footprint with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,898 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/head_plate_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,898 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/projector_wg6802418_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,898 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/projector_wg6802418_child_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,898 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/sensor_mount_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,898 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/double_stereo_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,898 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/narrow_stereo_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,898 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/narrow_stereo_gazebo_l_stereo_camera_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,898 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/narrow_stereo_gazebo_l_stereo_camera_optical_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,898 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/narrow_stereo_gazebo_r_stereo_camera_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,898 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/narrow_stereo_gazebo_r_stereo_camera_optical_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,898 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/narrow_stereo_optical_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,898 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/wide_stereo_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,898 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/wide_stereo_gazebo_l_stereo_camera_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,898 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/wide_stereo_gazebo_l_stereo_camera_optical_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,898 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/wide_stereo_gazebo_r_stereo_camera_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,898 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/wide_stereo_gazebo_r_stereo_camera_optical_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,899 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/wide_stereo_optical_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,899 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/high_def_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,899 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/high_def_optical_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,910 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/l_forearm_cam_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,910 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/l_forearm_cam_optical_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,925 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/l_gripper_motor_slider_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,925 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/l_gripper_motor_screw_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,925 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/l_gripper_led_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,925 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/l_gripper_motor_accelerometer_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,930 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/l_gripper_l_finger_tip_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,930 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/l_gripper_tool_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,932 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/laser_tilt_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,943 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/r_forearm_cam_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,944 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/r_forearm_cam_optical_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,958 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/r_gripper_motor_slider_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,958 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/r_gripper_motor_screw_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,958 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/r_gripper_led_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,958 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/r_gripper_motor_accelerometer_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,964 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/r_gripper_l_finger_tip_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,964 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/r_gripper_tool_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,964 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/imu_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,964 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/l_torso_lift_side_plate_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,964 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/r_torso_lift_side_plate_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,964 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/torso_lift_motor_screw_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,964 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/base_bellow_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,964 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/base_laser_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,964 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/base_footprint with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,991 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/head_plate_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,991 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/projector_wg6802418_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,991 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/projector_wg6802418_child_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,991 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/sensor_mount_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,991 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/double_stereo_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,992 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/narrow_stereo_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,992 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/narrow_stereo_gazebo_l_stereo_camera_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,992 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/narrow_stereo_gazebo_l_stereo_camera_optical_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,992 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/narrow_stereo_gazebo_r_stereo_camera_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,992 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/narrow_stereo_gazebo_r_stereo_camera_optical_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,992 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/narrow_stereo_optical_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,992 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/wide_stereo_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,992 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/wide_stereo_gazebo_l_stereo_camera_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,992 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/wide_stereo_gazebo_l_stereo_camera_optical_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,992 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/wide_stereo_gazebo_r_stereo_camera_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,992 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/wide_stereo_gazebo_r_stereo_camera_optical_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,992 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/wide_stereo_optical_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,992 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/high_def_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:51,992 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/high_def_optical_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,003 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/l_forearm_cam_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,003 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/l_forearm_cam_optical_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,016 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/l_gripper_motor_slider_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,016 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/l_gripper_motor_screw_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,016 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/l_gripper_led_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,016 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/l_gripper_motor_accelerometer_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,022 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/l_gripper_l_finger_tip_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,022 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/l_gripper_tool_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,024 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/laser_tilt_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,034 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/r_forearm_cam_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,034 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/r_forearm_cam_optical_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,048 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/r_gripper_motor_slider_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,048 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/r_gripper_motor_screw_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,048 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/r_gripper_led_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,048 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/r_gripper_motor_accelerometer_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,054 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/r_gripper_l_finger_tip_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,054 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/r_gripper_tool_frame with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,054 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/imu_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,054 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/l_torso_lift_side_plate_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,054 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/r_torso_lift_side_plate_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,054 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/torso_lift_motor_screw_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,054 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/base_bellow_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,054 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link pr2/base_laser_link with 0 geometries (env 1) (userdatakey fclcollision0x55e46d67c1b0)
2020-05-06 17:42:52,197 openrave [WARN] [kinbody.cpp:1348 KinBody::GetLinkEnableStatesMask] pr2 has too many links and will only return enable mask for first 64
Academic license - for non-commercial use only
[INFO] creating bullet collision checker for environment
2020-05-06 17:42:52,248 openrave [WARN] [kinbody.cpp:1576 KinBody::SetDOFValues] dof 12 value is not in limits 0.000000e+00<1.150000e-02
2020-05-06 17:42:52,248 openrave [WARN] [kinbody.cpp:1587 KinBody::SetDOFValues] dof 18 value is not in limits 0.000000e+00<-1.500001e-01
2020-05-06 17:42:52,248 openrave [WARN] [kinbody.cpp:1587 KinBody::SetDOFValues] dof 20 value is not in limits 0.000000e+00<-1.000000e-01
2020-05-06 17:42:52,248 openrave [WARN] [kinbody.cpp:1587 KinBody::SetDOFValues] dof 30 value is not in limits 0.000000e+00<-1.500001e-01
2020-05-06 17:42:52,248 openrave [WARN] [kinbody.cpp:1587 KinBody::SetDOFValues] dof 32 value is not in limits 0.000000e+00<-1.000000e-01
[INFO] iteration 1
[INFO]  
                |   oldexact |    dapprox |     dexact |      ratio
          COSTS | -------------------------------------------------
      joint_vel |  4.610e+00 | -3.433e-02 | -3.433e-02 |  1.000e+00
    cont_coll_0 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_1 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_2 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_3 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_4 |  1.526e+00 |  1.526e+00 |  1.526e+00 |  1.000e+00
    cont_coll_5 |  5.432e+00 |  2.057e+00 |  2.768e+00 |  1.345e+00
    cont_coll_6 |  1.022e+01 |  4.606e+00 |  4.921e+00 |  1.068e+00
    cont_coll_7 |  1.461e+01 | -2.302e+00 | -2.338e+00 |  1.015e+00
    cont_coll_8 |  1.791e+00 |  1.613e+00 |  1.621e+00 |  1.005e+00
    CONSTRAINTS | -------------------------------------------------
           pose |  1.317e-13 |  8.158e-14 | -9.494e-02 |   ------  
          TOTAL |  3.818e+01 |  7.465e+00 |  8.368e+00 |  1.121e+00
[INFO] expanded trust region. new box size: 0.1500
[INFO] iteration 2
[INFO]  
                |   oldexact |    dapprox |     dexact |      ratio
          COSTS | -------------------------------------------------
      joint_vel |  4.644e+00 |  1.497e-02 |  1.497e-02 |  1.000e+00
    cont_coll_0 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_1 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_2 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_3 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_4 |  0.000e+00 | -7.750e-12 |  0.000e+00 |   ------  
    cont_coll_5 |  2.665e+00 |  2.665e+00 |  2.371e+00 |  8.900e-01
    cont_coll_6 |  5.296e+00 |  1.287e+00 |  1.570e+00 |  1.220e+00
    cont_coll_7 |  1.695e+01 | -2.895e-01 |  5.781e-01 | -1.997e+00
    cont_coll_8 |  1.700e-01 | -6.986e-01 | -7.549e-01 |  1.081e+00
    CONSTRAINTS | -------------------------------------------------
           pose |  9.494e-02 |  9.494e-02 | -1.406e-01 | -1.481e+00
          TOTAL |  2.982e+01 |  3.074e+00 |  3.639e+00 |  1.184e+00
[INFO] expanded trust region. new box size: 0.2250
[INFO] iteration 3
[INFO]  
                |   oldexact |    dapprox |     dexact |      ratio
          COSTS | -------------------------------------------------
      joint_vel |  4.629e+00 |  1.246e-02 |  1.246e-02 |  1.000e+00
    cont_coll_0 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_1 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_2 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_3 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_4 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_5 |  2.932e-01 |  2.932e-01 |  2.932e-01 |  1.000e+00
    cont_coll_6 |  3.726e+00 |  2.654e+00 |  3.726e+00 |  1.404e+00
    cont_coll_7 |  1.637e+01 |  7.483e+00 |  1.579e+01 |  2.110e+00
    cont_coll_8 |  9.249e-01 |  9.249e-01 |  9.249e-01 |  1.000e+00
    CONSTRAINTS | -------------------------------------------------
           pose |  2.356e-01 |  2.356e-01 |  2.343e-01 |  9.947e-01
          TOTAL |  2.618e+01 |  1.160e+01 |  2.098e+01 |  1.808e+00
[INFO] expanded trust region. new box size: 0.3375
[INFO] iteration 4
[INFO]  
                |   oldexact |    dapprox |     dexact |      ratio
          COSTS | -------------------------------------------------
      joint_vel |  4.617e+00 |  1.297e-01 |  1.297e-01 |  1.000e+00
    cont_coll_0 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_1 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_2 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_3 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_4 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_5 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_6 |  0.000e+00 | -3.595e-13 |  0.000e+00 |   ------  
    cont_coll_7 |  5.797e-01 |  5.797e-01 |  5.797e-01 |  1.000e+00
    cont_coll_8 |  0.000e+00 | -3.449e-12 |  0.000e+00 |   ------  
    CONSTRAINTS | -------------------------------------------------
           pose |  1.242e-03 |  1.242e-03 |  1.242e-03 |  1.000e+00
          TOTAL |  5.197e+00 |  7.107e-01 |  7.107e-01 |  1.000e+00
[INFO] expanded trust region. new box size: 0.5063
[INFO] iteration 5
[INFO]  
                |   oldexact |    dapprox |     dexact |      ratio
          COSTS | -------------------------------------------------
      joint_vel |  4.487e+00 |  6.383e-02 |  6.383e-02 |  1.000e+00
    cont_coll_0 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_1 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_2 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_3 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_4 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_5 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_6 |  0.000e+00 | -1.209e-10 |  0.000e+00 |   ------  
    cont_coll_7 |  0.000e+00 | -3.515e-10 |  0.000e+00 |   ------  
    cont_coll_8 |  0.000e+00 | -2.303e-10 |  0.000e+00 |   ------  
    CONSTRAINTS | -------------------------------------------------
           pose |  3.061e-08 |  3.022e-08 |  2.020e-08 |   ------  
          TOTAL |  4.487e+00 |  6.383e-02 |  6.383e-02 |  1.000e+00
[INFO] expanded trust region. new box size: 0.7594
[INFO] iteration 6
[INFO]  
                |   oldexact |    dapprox |     dexact |      ratio
          COSTS | -------------------------------------------------
      joint_vel |  4.423e+00 |  3.319e-02 |  3.319e-02 |  1.000e+00
    cont_coll_0 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_1 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_2 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_3 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_4 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_5 |  0.000e+00 | -1.684e-12 |  0.000e+00 |   ------  
    cont_coll_6 |  0.000e+00 | -5.123e-12 |  0.000e+00 |   ------  
    cont_coll_7 |  0.000e+00 | -8.433e-12 |  0.000e+00 |   ------  
    cont_coll_8 |  0.000e+00 | -3.357e-12 | -1.371e-06 |   ------  
    CONSTRAINTS | -------------------------------------------------
           pose |  1.041e-08 |  1.040e-08 |  7.119e-10 |   ------  
          TOTAL |  4.423e+00 |  3.319e-02 |  3.319e-02 |  1.000e+00
[INFO] expanded trust region. new box size: 1.1391
[INFO] iteration 7
[INFO]  
                |   oldexact |    dapprox |     dexact |      ratio
          COSTS | -------------------------------------------------
      joint_vel |  4.390e+00 |  1.231e-03 |  1.231e-03 |  1.000e+00
    cont_coll_0 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_1 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_2 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_3 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_4 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------  
    cont_coll_5 |  0.000e+00 | -6.042e-11 |  0.000e+00 |   ------  
    cont_coll_6 |  0.000e+00 | -2.467e-10 | -1.204e-02 |   ------  
    cont_coll_7 |  0.000e+00 | -4.198e-10 |  0.000e+00 |   ------  
    cont_coll_8 |  1.371e-06 |  1.371e-06 |  1.371e-06 |  1.000e+00
    CONSTRAINTS | -------------------------------------------------
           pose |  9.694e-09 |  9.507e-09 |  1.472e-10 |   ------  
          TOTAL |  4.390e+00 |  1.233e-03 | -1.080e-02 | -8.764e+00
[INFO] converged because improvement ratio was small (2.808e-04 < 1.000e-03)
[INFO] woo-hoo! constraints are satisfied (to tolerance 1.00e-04)
[INFO] 
==================
Optimization results:
status: CONVERGED
cost values: (4.38979, 0, 0, 0, 0, 0, 0, 0, 0, 1.37091e-06)
constraint violations: (4.84697e-10)
n func evals: 8
n qp solves: 7
==================
[('joint_vel', 4.389793480064708), ('cont_coll_0', 0.0), ('cont_coll_1', 0.0), ('cont_coll_2', 0.0), ('cont_coll_3', 0.0), ('cont_coll_4', 0.0), ('cont_coll_5', 0.0), ('cont_coll_6', 0.0), ('cont_coll_7', 0.0), ('cont_coll_8', 1.37090682986174e-06)][('pose', 4.846970399258299e-10)]
No handlers could be found for logger "openravepy"
2020-05-06 17:42:52,339 openrave [WARN] [kinbody.cpp:1348 KinBody::GetLinkEnableStatesMask] pr2 has too many links and will only return enable mask for first 64
Segmentation fault (core dumped)

I get trajopt result as PENALTY_ITERATION_LIMIT for this_side_up.py. I am new to trajopt and it will be great help if u can help me how I can solve this error and run this_side_up.py

Hello
It appears that the website with the documentation is down.
Does anyone know if this is temporary or permanent? Is there another way of accessing the documentation in the meantime?

Many thanks

'em running a python code, but in python code 'em facing problem with these lines of code

import trajoptpy
import trajoptpy.kin_utils as ku
from trajoptpy.check_traj import traj_is_safe, traj_collisions
import trajoptpy.math_utils as mu

actually the main error is importing trajopt as
ImportError: No module named trajoptpy

I have downloaded the github package of trajopt but after compiling I don't know PYTHONPATH is making problem or other dependency in my PC I have set the path as

mudasser@mudasser-PC:~$ export PYTHONPATH=$PYTHONPATH:/home/mudasser/trajopt
mudasser@mudasser-PC:~$ export PYTHONPATH=$PYTHONPATH:/home/mudasser/trajopt/build/lib

now while ctest and running the python code it is making problem for me. ctest error is given as

Test project /home/mudasser/trajopt/build
      Start  1: arm_to_joint_target.py
 1/10 Test  #1: arm_to_joint_target.py .............   Passed    5.10 sec
      Start  2: arm_to_cart_target.py
 2/10 Test  #2: arm_to_cart_target.py ..............***Failed    2.46 sec
      Start  3: fullbody_plan.py
 3/10 Test  #3: fullbody_plan.py ...................***Failed    2.44 sec
      Start  4: position_base.py
 4/10 Test  #4: position_base.py ...................***Failed    0.03 sec
      Start  5: this_side_up.py
 5/10 Test  #5: this_side_up.py ....................***Failed    2.25 sec
      Start  6: arm_to_cart_target_position_only
 6/10 Test  #6: arm_to_cart_target_position_only ...***Failed    2.99 sec
      Start  7: sco-unit
 7/10 Test  #7: sco-unit ...........................   Passed    0.08 sec
      Start  8: collision-checker-unit
 8/10 Test  #8: collision-checker-unit .............***Failed    0.00 sec
      Start  9: planning-unit
 9/10 Test  #9: planning-unit ......................***Failed    0.00 sec
      Start 10: cast-cost-unit
10/10 Test #10: cast-cost-unit .....................***Failed    0.00 sec

20% tests passed, 8 tests failed out of 10

Total Test time (real) =  15.36 sec

The following tests FAILED:
      2 - arm_to_cart_target.py (Failed)
      3 - fullbody_plan.py (Failed)
      4 - position_base.py (Failed)
      5 - this_side_up.py (Failed)
      6 - arm_to_cart_target_position_only (Failed)
      8 - collision-checker-unit (Failed)
      9 - planning-unit (Failed)
     10 - cast-cost-unit (Failed)
Errors while running CTest

can someone help me to fix trajopt problem?

Hi,

When I add the command line option to build cloudprocpy as well, it builds the cloudprocpy, but when I import the library from python, I got following error:

File "", line 1, in
ImportError: /home/cvgl_ros/trajopt/build/lib/cloudprocpy.so: undefined symbol: _ZN9cloudproc12medianFilterIN3pcl8PointXYZEEENS1_10PointCloudIT_E3PtrENS5_8ConstPtrEif

I'm not sure which other lib.so file it is looking for that has wrong version or something. Any one has idea on this? Thanks!

I couldn't build the package successfully. I get the following error,
../../../lib/libosgviewer.so: undefined reference to osgGA::EventHandler::event(osg::NodeVisitor*, osg::Drawable*)' ../../../lib/libosgviewer.so: undefined reference to typeinfo for osgGA::EventHandler'
../../../lib/libosgviewer.so: undefined reference to virtual thunk to osg::Drawable::EventCallback::run(osg::Object*, osg::Object*)' ../../../lib/libosgviewer.so: undefined reference to osgGA::EventHandler::handle(osgGA::Event*, osg::Object*, osg::NodeVisitor*)'
../../../lib/libosgviewer.so: undefined reference to osg::Callback::traverse(osg::Object*, osg::Object*)' ../../../lib/libosgviewer.so: undefined reference to osg::NodeVisitor::NodeVisitor(osg::NodeVisitor const&, osg::CopyOp const&)'
../../../lib/libosgviewer.so: undefined reference to osg::Drawable::EventCallback::run(osg::Object*, osg::Object*)' ../../../lib/libosgviewer.so: undefined reference to osgGA::GUIEventHandler::handle(osgGA::Event*, osg::Object*, osg::NodeVisitor*)'
../../../lib/libosgviewer.so: undefined reference to virtual thunk to osg::NodeCallback::run(osg::Object*, osg::Object*)' ../../../lib/libosgviewer.so: undefined reference to osg::Group::removeChild(osg::Node*)'
../../../lib/libosgviewer.so: undefined reference to osg::NodeVisitor::apply(osg::Geometry&)' ../../../lib/libosgviewer.so: undefined reference to VTT for osgGA::EventHandler'
../../../lib/libosgviewer.so: undefined reference to non-virtual thunk to osgGA::EventHandler::event(osg::NodeVisitor*, osg::Drawable*)' ../../../lib/libosgviewer.so: undefined reference to vtable for osgGA::EventHandler'
../../../lib/libosgviewer.so: undefined reference to osg::Group::getNumChildren() const' ../../../lib/libosgviewer.so: undefined reference to osg::NodeVisitor::apply(osg::Drawable&)'
../../../lib/libosgviewer.so: undefined reference to osg::NodeCallback::run(osg::Object*, osg::Object*)' ../../../lib/libosgviewer.so: undefined reference to osgGA::EventHandler::operator()(osg::Node*, osg::NodeVisitor*)'
collect2: error: ld returned 1 exit status
make[2]: *** [bin/cast-cost-unit] Error 1
make[1]: *** [src/trajopt/test/CMakeFiles/cast-cost-unit.dir/all] Error 2
make: *** [all] Error 2
18:35:30: The process "/usr/bin/cmake" exited with code 2.
Error while building/deploying project trajopt (kit: Desktop)
When executing step "Make"

i cloned the package and created build directory. So
cd build && cmake .. -DGUROBI_LIBRARY=/opt/gurobi751/linux64/lib/libgurobi75.so && make
gave me above error. Please help me to build it.

I am trying to run arm_to_cart_pose.py on real robot . How can I achieve it ?