├── .gitignore ├── .landscape.yml ├── .travis.yml ├── LICENSE.md ├── README.md ├── __init__.py ├── configs ├── kuka_lwr4.yaml ├── kuka_lwr4_static.yaml ├── threeLinks.yaml ├── walkman_cad.yaml └── walkman_static.yaml ├── data ├── KUKA │ ├── HW │ │ ├── measurements_1.npz │ │ ├── measurements_2.npz │ │ └── measurements_3.npz │ └── README.md ├── THREELINK │ └── SIM │ │ └── measurements_opt1_fb.npz └── WALKMAN │ └── README.md ├── documentation ├── TUTORIAL.md ├── identification_overview.svg ├── identification_overview.xml ├── kuka_vis.png └── overview slides.pdf ├── examples ├── excite_kuka_lwr4.sh ├── identify_kuka_lwr4.sh ├── identify_threeLink.sh ├── optimize_kuka_lwr4.sh └── parallel_optimize_kuka_lwr4.sh ├── excitation ├── README.md ├── __init__.py ├── optimizer.py ├── postureOptimizer.py ├── robotCommunication │ ├── __init__.py │ ├── ros_moveit.py │ ├── yarpGYM │ │ ├── CMakeLists.txt │ │ ├── app │ │ │ ├── CMakeLists.txt │ │ │ └── conf │ │ │ │ └── excitation_initial_config.ini │ │ └── src │ │ │ ├── excitation_constants.h │ │ │ ├── excitation_initial_config.ini │ │ │ ├── excitation_main.cpp │ │ │ ├── excitation_module.hpp │ │ │ ├── excitation_msg.h │ │ │ ├── excitation_thread.cpp │ │ │ ├── excitation_thread.h │ │ │ └── saveEigen.h │ └── yarp_gym.py ├── trajectoryGenerator.py └── trajectoryOptimizer.py ├── excite.py ├── identification ├── __init__.py ├── data.py ├── helpers.py ├── model.py ├── nlopt.py ├── output.py ├── quaternion.py ├── sdp.py └── sdp_helpers.py ├── identify.py ├── matplotlibrc ├── model ├── kuka_lwr4.urdf ├── kuka_lwr4.urdf.trajectory_opt_1.npz ├── kuka_lwr4_centroepiaggio.urdf ├── kuka_lwr4_cogimon.urdf ├── kuka_lwr4_gaz.urdf ├── kuka_lwr4_identified.urdf ├── kuka_lwr4_regressor.xml ├── meshes │ ├── kuka │ │ ├── COPYRIGHT │ │ ├── base.STL │ │ ├── link_1.STL │ │ ├── link_2.STL │ │ ├── link_3.STL │ │ ├── link_4.STL │ │ ├── link_5.STL │ │ ├── link_6.STL │ │ └── link_7.STL │ └── walkman │ │ ├── AnklePitch.STL │ │ ├── BottomUpperArm.STL │ │ ├── Chest.STL │ │ ├── Elbow.STL │ │ ├── Forearm.STL │ │ ├── ForearmPitch.STL │ │ ├── ForearmRoll.STL │ │ ├── Head.STL │ │ ├── HipPitch.STL │ │ ├── HipRoll.STL │ │ ├── HipYaw.STL │ │ ├── KneePitch.STL │ │ ├── KneePitchCalf.STL │ │ ├── NeckPitch.STL │ │ ├── NeckYaw.STL │ │ ├── NewChest.STL │ │ ├── NewFoot.STL │ │ ├── PelvisWeightBench.STL │ │ ├── ProtectionSystem.STL │ │ ├── RaisingSupport.STL │ │ ├── ShoulderPitch.STL │ │ ├── SoftHandOpen.STL │ │ ├── TopUpperArm.STL │ │ ├── TorsoRoll.STL │ │ ├── TorsoYawPitch.STL │ │ ├── Waist.STL │ │ ├── base.STL │ │ ├── hokuyo_link.STL │ │ ├── multisense.STL │ │ └── simple │ │ ├── AnklePitch.STL │ │ ├── AnklePitch.capsule │ │ ├── BottomUpperArm.STL │ │ ├── BottomUpperArm.capsule │ │ ├── Chest.STL │ │ ├── Elbow.STL │ │ ├── Elbow.capsule │ │ ├── Foot.STL │ │ ├── Forearm.STL │ │ ├── Forearm.capsule │ │ ├── ForearmPitch.STL │ │ ├── ForearmPitch.capsule │ │ ├── ForearmRoll.STL │ │ ├── ForearmRoll.capsule │ │ ├── Head.STL │ │ ├── Head.capsule │ │ ├── HipPitch.STL │ │ ├── HipPitch.capsule │ │ ├── HipRoll.STL │ │ ├── HipRoll.capsule │ │ ├── HipYaw.STL │ │ ├── HipYaw.capsule │ │ ├── KneePitch.STL │ │ ├── KneePitchCalf.STL │ │ ├── KneePitchCalf.capsule │ │ ├── NeckPitch.STL │ │ ├── NeckPitch.capsule │ │ ├── NeckYaw.STL │ │ ├── NeckYaw.capsule │ │ ├── NewChest.STL │ │ ├── NewChest.capsule │ │ ├── PelvisWeightBench.STL │ │ ├── PelvisWeightBench.capsule │ │ ├── ProtectionSystem.STL │ │ ├── ProtectionSystem.capsule │ │ ├── RaisingSupport.STL │ │ ├── ShoulderPitch.STL │ │ ├── ShoulderPitch.capsule │ │ ├── SoftHandOpen.STL │ │ ├── SoftHandOpen.capsule │ │ ├── TopUpperArm.STL │ │ ├── TopUpperArm.capsule │ │ ├── TorsoRoll.STL │ │ ├── TorsoRoll.capsule │ │ ├── TorsoYawPitch.STL │ │ ├── TorsoYawPitch.capsule │ │ ├── Waist.STL │ │ ├── Waist.capsule │ │ └── Waist_old.STL ├── threeLinks.urdf ├── threeLinks_regressor.xml ├── walkman.urdf ├── walkman_left_arm.urdf ├── walkman_left_arm_regressor.xml ├── walkman_left_leg.urdf ├── walkman_measured.urdf ├── walkman_only_legs.urdf ├── walkman_regressor.xml ├── walkman_regressor_fixed.xml ├── walkman_weights.ods ├── world_kuka.urdf └── world_walkman_fixed.urdf ├── output ├── css │ ├── main.css │ ├── normalize.css │ └── normalize.min.css ├── js │ └── ansi_up.js └── templates │ └── index.html ├── requirements.txt ├── tests ├── test_excite.py ├── test_identification_fixed.py ├── test_identification_floating.py └── test_regressors.py ├── tools ├── createNoisyURDF.py ├── csv2npz.py ├── getMaxInertia.py ├── inspectMeasurements.py ├── paramErrorPlot.py ├── scaleInertia.py ├── scaleInertia2.py ├── start_gazebo_kuka_lwr4.sh └── start_gazebo_kuka_lwr4_sim.sh ├── trajectory.py └── visualizer.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | 5 | # C extensions 6 | *.so 7 | 8 | # Distribution / packaging 9 | .Python 10 | env/ 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | *.egg-info/ 23 | .installed.cfg 24 | *.egg 25 | 26 | # PyInstaller 27 | # Usually these files are written by a python script from a template 28 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 29 | *.manifest 30 | *.spec 31 | 32 | # Installer logs 33 | pip-log.txt 34 | pip-delete-this-directory.txt 35 | 36 | # Unit test / coverage reports 37 | htmlcov/ 38 | .tox/ 39 | .coverage 40 | .coverage.* 41 | .cache 42 | nosetests.xml 43 | coverage.xml 44 | *,cover 45 | 46 | # Translations 47 | *.mo 48 | *.pot 49 | 50 | # Django stuff: 51 | *.log 52 | 53 | # Sphinx documentation 54 | docs/_build/ 55 | 56 | # PyBuilder 57 | target/ 58 | 59 | #Vim 60 | *.swp 61 | *.swp 62 | documentation/.ipynb_checkpoints 63 | model/kuka_lwr4.urdf.regressor.npz 64 | model/kuka_lwr4.urdf.tmp 65 | data/KUKA/SIM/old 66 | output/output_*.html 67 | model/*.urdf.regressor.npz 68 | model/*.urdf.trajectory.npz 69 | identification/sdpa_dat 70 | .swl 71 | .swm 72 | .swn 73 | data 74 | .mypy_cache 75 | -------------------------------------------------------------------------------- /.landscape.yml: -------------------------------------------------------------------------------- 1 | strictness: medium 2 | python-targets: 3 | - 2 4 | - 3 5 | pylint: 6 | disable: 7 | - redefined-builtin 8 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | dist: trusty 2 | sudo: required 3 | env: "TRAVIS_CMAKE_GENERATOR=\"Unix Makefiles\", TRAVIS_BUILD_TYPE=\"Release\"" 4 | language: python 5 | python: 6 | - "3.4" 7 | # - "2.7" 8 | cache: 9 | - pip 10 | - ccache 11 | before_cache: 12 | - rm -f $HOME/.cache/pip/log/debug.log 13 | addons: 14 | apt: 15 | sources: 16 | - sourceline: 'deb http://de.archive.ubuntu.com/ubuntu trusty-backports main restricted universe multiverse' 17 | packages: 18 | - python-dev 19 | - python3-dev 20 | - gfortran 21 | 22 | before_install: 23 | - sudo add-apt-repository ppa:george-edison55/cmake-3.x --yes 24 | - sudo apt-get -qq update 25 | - sudo apt-get install -y cmake cmake-data 26 | #- sudo apt-get remove -y swig 27 | #- sudo apt-get install -y -t trusty-backports swig3.0 28 | #- sudo ln -s /usr/bin/swig3.0 /usr/bin/swig 29 | #cvxopt dependencies 30 | - sudo apt-get install -y liblapack3 liblapack-dev 31 | - sudo apt-get install -y libblas3 libblas-dev 32 | #install dsdp solver 33 | - sudo apt-get install -y dsdp 34 | #mpi4py dependencies 35 | - sudo apt-get install -y libopenmpi-dev 36 | 37 | # command to install dependencies 38 | install: 39 | #- pip install --global-option=build_ext --global-option="-I/usr/include/suitesparse" cvxopt 40 | - wget http://faculty.cse.tamu.edu/davis/SuiteSparse/SuiteSparse-4.5.3.tar.gz 41 | - echo '2403007be38266e3607edfbf3833bee7f6bcb0f1 SuiteSparse-4.5.3.tar.gz' | shasum -c -; 42 | if [ $? -eq 0 ]; then 43 | tar -xf SuiteSparse-4.5.3.tar.gz; 44 | CVXOPT_SUITESPARSE_SRC_DIR=$(pwd)/SuiteSparse pip install cvxopt; 45 | fi 46 | - pip install -r requirements.txt 47 | - pip uninstall -y mpi4py 48 | #- pip install git+https://github.com/kjyv/pyOpt.git 49 | 50 | before_script: 51 | #install iDynTree 52 | - git clone https://github.com/robotology/idyntree-superbuild 53 | - cd idyntree-superbuild 54 | - chmod +x ./.ci/travis-deps.sh 55 | - sh .ci/travis-deps.sh 56 | 57 | # Use ccache also for clang and clang++ on linux 58 | - if [ "$TRAVIS_OS_NAME" == "linux" ]; then if [ "$CC" == "clang" ]; then sudo ln -s ../../bin/ccache /usr/lib/ccache/clang; fi; fi 59 | - if [ "$TRAVIS_OS_NAME" == "linux" ]; then if [ "$CXX" == "clang++" ]; then sudo ln -s ../../bin/ccache /usr/lib/ccache/clang++; fi; fi 60 | 61 | - mkdir build 62 | - cd build 63 | - export CMAKE_PREFIX_PATH=`pwd`/install 64 | 65 | - if [[$TRAVIS_PYTHON_VERSION == 2.7 ]]; then 66 | export PYTHON_INCLUDE_DIR=/usr/include/python2.7; 67 | export PYTHON_LIBRARY=/usr/lib/python2.7/config/libpython2.7.so; 68 | export PYTHONPATH=$PYTHONPATH:~/virtualenv/python2.7/lib/python2.7/site-packages; 69 | fi 70 | - if [[$TRAVIS_PYTHON_VERSION == 3.4 ]]; then 71 | export PYTHON_INCLUDE_DIR=/usr/include/python3.4; 72 | export PYTHON_LIBRARY=/usr/lib/python3.4/config/libpython3.4.so; 73 | fi 74 | # -DPYTHON_LIBRARY=$(python-config --prefix)/lib/libpython2.7.so -DPYTHON_INCLUDE_DIR=$(python-config --prefix)/include/python2.7 75 | # -DPYTHON_LIBRARY=$(python-config --prefix)/lib/libpython3.5.so -DPYTHON_INCLUDE_DIR=$(python-config --prefix)/include/python3.5 76 | - cmake -G"${TRAVIS_CMAKE_GENERATOR}" -DCMAKE_BUILD_TYPE=${TRAVIS_BUILD_TYPE} -DIDYNTREE_USES_YARP:BOOL=OFF -DIDYNTREE_USES_ICUB_MAIN:BOOL=OFF -DIDYNTREE_USES_PYTHON:BOOL=ON -DNON_INTERACTIVE_BUILD:BOOL=ON .. 77 | - cmake --build . --config ${TRAVIS_BUILD_TYPE} -- -j5 78 | - sudo cmake --build . --config ${TRAVIS_BUILD_TYPE} --target install 79 | 80 | #add python module dir variables to env 81 | - export PYTHONPATH=$PYTHONPATH:`pwd`/install/lib/python$TRAVIS_PYTHON_VERSION/site-packages 82 | 83 | # command to run tests 84 | script: 85 | - cd ../.. 86 | - export MPLBACKEND="agg" 87 | - travis_wait 30 pytest tests 88 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # FloBaRoID [![Build Status](https://travis-ci.org/kjyv/FloBaRoID.svg?branch=master)](https://travis-ci.org/kjyv/FloBaRoID) 2 | 3 | (FLOating BAse RObot dynamical IDentification) 4 | 5 | FloBaRoID is a python toolkit for parameter identification of floating-base rigid body tree-structures such as 6 | humanoid robots. It aims to provide a complete solution for obtaining physical consistent identified dynamics parameters. 7 | 8 |
9 | Overview diagram 10 | Visualization of Kuka LWR4+ 11 |
12 | 13 | Tools: 14 | 15 | * **trajectory.py**: generate optimized trajectories 16 | * **excite.py**: send trajectory to control the robot movement and record the resulting measurements 17 | * **identify.py**: identify dynamical parameters (mass, COM and rotational inertia) starting from an URDF description and from torque and force measurements 18 | * **visualize.py**: show 3D robot model of URDF, trajectory motion 19 | 20 | 21 | Features: 22 | 23 | * find optimized excitation trajectories with non-linear global optimization (as parameters of fourier-series for periodic soft trajectories) 24 | * data preprocessing 25 | * derive velocity and acceleration values from position readings 26 | * data is zero-phase low-pass filtered from supplied measurements 27 | * it is possible to only select a combination of data blocks to yield a better condition number (Venture, 2009) 28 | * validation with other measurement files 29 | * excitation of robots, using ROS/MoveIt! or Yarp 30 | * implemented estimation methods: 31 | * ordinary least squares, OLS 32 | * weighted least squares (Zak, 1994) 33 | * estimation of parameter error using previously known CAD values (Gautier, 2013) 34 | * essential standard parameters (Pham, Gautier, 2013), estimating only those that are most certain for the measurement data and leaving the others unchanged 35 | * identification problem formulation with constraints as linear convex SDP problem to get optimal physical consistent standard parameters (Sousa, 2014) 36 | * non-linear optimization within consistent parameter space (Traversaro, 2016) 37 | * visualization of trajectories 38 | * plotting of measured and estimated joint state and torques (interactive, HTML, PDF or Tikz) 39 | * output of the identified parameters directly into URDF 40 | 41 | requirements for identification module: 42 | 43 | * python 2.7 or >= 3.3 44 | * python modules 45 | * numpy (> 1.8), scipy, sympy (== 1.0), pyyaml, trimesh, cvxopt, pylmi-sdp, matplotlib (>= 1.4), colorama, palettable, humanize, tqdm 46 | * iDynTree, e.g. from [iDynTree superbuild](https://github.com/robotology/idyntree-superbuild/) (with enabled python binding) 47 | * when using Python 2.7: future 48 | * when using Python < 3.5: typing 49 | * dsdp5 (command line executable) 50 | 51 | optional: 52 | 53 | * pyglet, pyOpenGL (for visualizer) 54 | * symengine.py (to speedup SDP) 55 | * mpld3, jinja2 (for html plots) 56 | * matplotlib2tikz (for tikz plots) 57 | * rbdl (alternative for inverse dynamics) 58 | 59 | requirements for excitation module: 60 | 61 | * for ros, python modules: ros, moveit\_msg, moveit\_commander 62 | * for yarp: c compiler, installed [robotology-superbuild](https://github.com/robotology-playground/robotology-superbuild), python modules: yarp 63 | * for other robots, new modules might have to be written 64 | 65 | requirements for optimization module: 66 | 67 | * optimization: python modules: iDynTree, pyOpt (fork at https://github.com/kjyv/pyOpt is recommended) 68 | * pyipopt from https://github.com/xuy/pyipopt (plus cmd line ipopt/libipopt with libhsl/coin-hsl) 69 | * mpi4py / mpirun (for parallel trajectory optimization) 70 | * [fcl 0.5.0](https://github.com/flexible-collision-library/fcl/releases) and python-fcl (from https://github.com/jf---/python-fcl) (possibly disable octomap if there are errors) 71 | 72 | 73 | You can do `pip install -r requirements.txt` for most of them but you will need to check for the correct versions of each library. You might have to install some library dependencies if you get compile errors. If you're using on Ubuntu and also have ros installed, it is recommended to install with pip within a [virtualenv](https://virtualenv.pypa.io/en/stable/). 74 | 75 | Also see the [Tutorial](documentation/TUTORIAL.md). 76 | 77 | Known limitations: 78 | 79 | * trajectory optimization is limited to fixed-base robots (full simulation, balance criterion etc. not implemented) 80 | * YARP excitation module is not very generic (ROS should be) 81 | * using position control over YARP is not realtime safe and can expose timing issues (especially with python to C bridge) 82 | * Since preparing SDP matrices uses sympy expressions, most of the time for solving the identification problem is spent in symbolic manipulations rather than the actual convex optimization solver. Possibly the time demands can be reduced. 83 | 84 | SDP optimization code is based on or uses parts from [cdsousa/wam7\_dyn\_ident](https://github.com/cdsousa/wam7_dyn_ident) 85 | 86 | Usage is licensed under the LGPL 3.0, see License.md. Please quote the following publication if you're using this software for any project: 87 | `S. Bethge, J. Malzahn, N. Tsagarakis, D. Caldwell: "FloBaRoID — A Software Package for the Identification of Robot Dynamics Parameters", 26th International Conference on Robotics in Alpe-Adria-Danube Region (RAAD), 2017` 88 | -------------------------------------------------------------------------------- /__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/__init__.py -------------------------------------------------------------------------------- /data/KUKA/HW/measurements_1.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/data/KUKA/HW/measurements_1.npz -------------------------------------------------------------------------------- /data/KUKA/HW/measurements_2.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/data/KUKA/HW/measurements_2.npz -------------------------------------------------------------------------------- /data/KUKA/HW/measurements_3.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/data/KUKA/HW/measurements_3.npz -------------------------------------------------------------------------------- /data/KUKA/README.md: -------------------------------------------------------------------------------- 1 | data files for KUKA LWR4+ 2 | -------------------------------------------------------------------------------- /data/THREELINK/SIM/measurements_opt1_fb.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/data/THREELINK/SIM/measurements_opt1_fb.npz -------------------------------------------------------------------------------- /data/WALKMAN/README.md: -------------------------------------------------------------------------------- 1 | Some data files for IIT Walkman 2 | -------------------------------------------------------------------------------- /documentation/kuka_vis.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/documentation/kuka_vis.png -------------------------------------------------------------------------------- /documentation/overview slides.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/documentation/overview slides.pdf -------------------------------------------------------------------------------- /examples/excite_kuka_lwr4.sh: -------------------------------------------------------------------------------- 1 | ./excite.py --config configs/kuka_lwr4.yaml --model model/kuka_lwr4.urdf --dryrun --trajectory model/kuka_lwr4.urdf.trajectory_opt_1.npz 2 | -------------------------------------------------------------------------------- /examples/identify_kuka_lwr4.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | ./identify.py --config configs/kuka_lwr4.yaml --model model/kuka_lwr4.urdf --measurements data/KUKA/HW/measurements_2.npz --measurements data/KUKA/HW/measurements_3.npz --validation data/KUKA/HW/measurements_1.npz 4 | -------------------------------------------------------------------------------- /examples/identify_threeLink.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | ./identify.py --config configs/threeLinks.yaml --model model/threeLinks.urdf --measurements data/THREELINK/SIM/measurements_opt1_fb.npz 4 | -------------------------------------------------------------------------------- /examples/optimize_kuka_lwr4.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | ./trajectory.py --config configs/kuka_lwr4.yaml --model_real model/kuka_lwr4.urdf --model model/kuka_lwr4.urdf --world model/world_kuka.urdf 4 | -------------------------------------------------------------------------------- /examples/parallel_optimize_kuka_lwr4.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | mpirun -n 4 ./trajectory.py --config configs/kuka_lwr4.yaml --model_real model/kuka_lwr4.urdf --model model/kuka_lwr4.urdf --world model/world_kuka.urdf 4 | -------------------------------------------------------------------------------- /excitation/README.md: -------------------------------------------------------------------------------- 1 | 2 | # Excitation scripts for generating trajectories and recording measurements 3 | 4 | ## Trajectory optimization 5 | 6 | Using global non-linear optimization (particle swarm optimization), parameters for trajectories can 7 | be found that minimize the condition number of the regressor retrieved through the generated 8 | trajectory (in simulation of course). Set the option `optimizeTrajectory` in your .yaml file to 9 | enable this. It is important to set good bounds to narrow the search space and to have proper joint 10 | angle, velocity and torque limits in the URDF model file. See further comments in the sample 11 | configuration files. 12 | 13 | ## Show trajectory data 14 | 15 | In order to show curves of an optimized or previously recorded trajectory again, 16 | `excite.py --plot --dryrun --filename ` can be used. Using 17 | `visualizer.py --config --model --trajectory ` 18 | a 3D representation of the model and trajectory can be shown. 19 | 20 | ## Excitation modules 21 | 22 | Existing modules that will send commands to a robot and record measurements can be used with slight 23 | modifications or as template for other robot command interfaces. 24 | 25 | ## Generate excitation for Walk-Man 26 | 27 | A Yarp GYM module is included that has to be built and started: 28 | 29 | in robotCommunication/yarpGYM/: 30 | 31 | `mkdir build && cd build` 32 | `cmake ../` 33 | `make` 34 | 35 | run 'yarpserver --write' and then 'gazebo', load the robot 36 | 37 | run `./excitation` 38 | 39 | `$ yarp write ... /excitation/switch:i` 40 | `>> start` 41 | 42 | The control thread is now started and accepts commands. 43 | 44 | Using $ yarp write ... /excitation/command:i 45 | it is then possible to manually set positions, e.g. by writing 46 | `(set_legs_refs 0 0 0 0 0 0 0 0 0 0 0 0) 0` 47 | 48 | 49 | To generate excitation trajectories and send them to the robot, set 50 | the option exciteMethod to 'yarp' and run `./excite.py [...]`. 51 | This will also read the resulting joint torques measurements and write them to a file 52 | measurements.npz 53 | 54 | ## Generate excitation for Kuka LWR4+ 55 | 56 | (using ROS package from https://github.com/CentroEPiaggio/kuka-lwr) 57 | 58 | start controllers, simulator and moveit (to directly use the hardware add options: use\_lwr\_sim:=false lwr\_powered:=true) 59 | `$ roslaunch single_lwr_launch single_lwr.launch load_moveit:=true` 60 | 61 | (make sure that gazebo plugin gets loaded in world file and joint\_state\_publisher has high enough rate param of 100-200 Hz set in launch file) 62 | 63 | To generate excitation trajectories and send them to the robot, set 64 | the option exciteMethod to 'ros' and run `./excite.py [...]`. 65 | 66 | 67 | ## Measurements data file structure 68 | 69 | The measurements retrieved from excitation are saved in a numpy .npz binary file archive which 70 | includes multiple data streams. All data fields have the same amount of samples S relative to the 71 | time in field 'times'. The same structure is expected by identification.py. 72 | 73 | |field name|content| 74 | |---|---| 75 | |positions | joint positions in rad, SxN_DOF| 76 | |[positions_raw] | unfiltered joint positions, SxN_DOF (optional)| 77 | |velocities | joint angular velocity in rad/sec, SxN_DOF| 78 | |[velocities_raw] | unfiltered joint angular velocities in rad/sec, SxN_DOF (optional)| 79 | |accelerations | joint angular accelerations in rad/s2, SxN_DOF| 80 | |torques | measured torques of each joint in Nm, SxN_DOF| 81 | |[torques_raw] | unfiltered torques of each joint, SxN_DOF (optional)| 82 | |base_velocity* | linear (0-2) and angular (3-5) velocity of the base link expressed in the world reference frame in m/s and rad/s, Sx6| 83 | |base_acceleration* | proper linear (0-2) and angular (3-5) acceleration of the base link (without gravity) expressed in the world reference frame in m/s2 and rad/s2, Sx6| 84 | |base_rpy* | Orientation of the base link in roll-pitch-yaw order expressed relative to the world reference frame in rad Sx3| 85 | |contacts | measured external contact wrench for each sample, array of dictionaries {'urdf frame name': Sx6}| 86 | |times | time of each sample in sec, Sx1| 87 | |frequency | frequency of measured values in Hz, 1 value| 88 | 89 | Values in [] are optional. 90 | Values with * only need to be specified when using floating base dynamics. 91 | 92 | All data is expected by identify.py to already be cleaned and low-pass filtered and to not 93 | include any big measurement errors. The noise should ideally be gaussian and have zero mean. 94 | 95 | The sampling frequency should be sufficiently high (e.g. at least 100 Hz) to get reasonably good position and velocity derivatives. 96 | 97 | The amount of samples should also be high enough for it to contain sufficient information about the parameters. It depends on how many parameters are to be identified and on the motion range of the robot. At least 10 98 | times the amount of parameters is possibly a good rule of thumb, more is always better. The higher 99 | the sampling frequency, the less information there is in successive samples, so the number should be 100 | higher at e.g. 1000 Hz. At the same time, more or less redundant samples can be skipped by setting the skipSamples option to speed up identification. 101 | -------------------------------------------------------------------------------- /excitation/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/excitation/__init__.py -------------------------------------------------------------------------------- /excitation/robotCommunication/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/excitation/robotCommunication/__init__.py -------------------------------------------------------------------------------- /excitation/robotCommunication/ros_moveit.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | from __future__ import print_function 3 | from builtins import range 4 | from builtins import object 5 | import sys 6 | import numpy as np 7 | #import threading 8 | 9 | import rospy 10 | import moveit_commander 11 | #from moveit_msgs.msg import RobotTrajectory, DisplayTrajectory 12 | from trajectory_msgs.msg import JointTrajectoryPoint 13 | from sensor_msgs.msg import JointState 14 | 15 | # records the states obtained from joint_states messages 16 | class RecordJointStates(object): 17 | def __init__(self): 18 | #rospy.init_node('joint_states_listener') 19 | #self.lock = threading.Lock() 20 | self.name = [] 21 | self.positions = [] 22 | self.velocities = [] 23 | self.torques = [] 24 | self.times = [] 25 | self.next = True 26 | self.listen() 27 | 28 | def listen(self): 29 | rospy.Subscriber('joint_states', JointState, self.joint_states_callback) 30 | 31 | #callback function: when a joint_states message arrives, save the values 32 | def joint_states_callback(self, msg): 33 | self.name = msg.name 34 | self.positions.append(msg.position) 35 | self.velocities.append(msg.velocity) 36 | self.torques.append(msg.effort) #ros "effort" is force for linear or torque for rotational joints 37 | self.times.append(msg.header.stamp.secs + msg.header.stamp.nsecs / 1.0e9) 38 | 39 | def main(config, trajectory, data): 40 | moveit_commander.roscpp_initialize(sys.argv) 41 | rospy.init_node('excitation_move_group', anonymous=False) 42 | #robot = moveit_commander.RobotCommander() 43 | move_group = config['ros_move_group'] 44 | group = moveit_commander.MoveGroupCommander(move_group) 45 | 46 | # in case there are previous executions still running 47 | group.stop() 48 | 49 | group.allow_replanning(True) 50 | 51 | # set higher goal tolerance in case moveit aborts with GOAL_TOLERANCE_VIOLATED. 52 | # driver has its own tol as well? 53 | #group.set_goal_tolerance(0.1) 54 | #group.set_goal_joint_tolerance(0.1) 55 | #group.set_goal_position_tolerance(0.1) 56 | 57 | #config['num_dofs'] = len(group.get_current_joint_values()) 58 | 59 | # create some plan and clear its trajectory 60 | plan = group.plan() 61 | plan.joint_trajectory.points = [] 62 | 63 | # generate trajectory and send in one message to moveit 64 | duration = config['args'].periods*trajectory.getPeriodLength() 65 | sent_positions = list() 66 | sent_time = list() 67 | sent_velocities = list() 68 | sent_accelerations = list() 69 | 70 | step = 1.0/200 # data rate of 200 Hz 71 | start_t = 0 72 | while not trajectory.wait_for_zero_vel(start_t): 73 | start_t+=step 74 | t = start_t 75 | 76 | # add trajectory points to plan 77 | while t < start_t+duration: 78 | trajectory.setTime(t) 79 | point = JointTrajectoryPoint() 80 | for i in range(0, config['num_dofs']): 81 | q = trajectory.getAngle(i) 82 | point.positions.append(q) 83 | dq = trajectory.getVelocity(i) 84 | point.velocities.append(dq) 85 | ddq = trajectory.getAcceleration(i) 86 | point.accelerations.append(ddq) 87 | 88 | point.time_from_start = rospy.Duration(t) 89 | plan.joint_trajectory.points.append(point) 90 | #if t == start_t: 91 | # print np.rad2deg(point.positions) 92 | sent_positions.append(point.positions) 93 | sent_velocities.append(point.velocities) 94 | sent_accelerations.append(point.accelerations) 95 | sent_time.append(t) 96 | t+=step 97 | 98 | # move to start position 99 | group.set_joint_value_target(plan.joint_trajectory.points[0].positions) 100 | #group.set_named_target('full_lwr_home') 101 | group.go() 102 | 103 | # record measurements 104 | jSt = RecordJointStates() 105 | group.execute(plan, wait=False) 106 | num_sent = len(sent_positions) 107 | start_t = rospy.get_time() 108 | while len(jSt.positions) < num_sent: # and rospy.get_time() < start_t+duration 109 | # gets data in thread 110 | rospy.sleep(step) 111 | 112 | data['Q'] = np.array(jSt.positions[0:num_sent])[:,0::2] 113 | data['V'] = np.array(jSt.velocities[0:num_sent])[:,0::2] 114 | data['T'] = np.array(jSt.times[0:num_sent]) 115 | data['Tau'] = np.array(jSt.torques[0:num_sent])[:,0::2] 116 | data['measured_frequency'] = data['Q'].shape[0] / duration 117 | data['Qsent'] = np.array(sent_positions); 118 | data['QdotSent'] = np.array(sent_velocities); 119 | data['QddotSent'] = np.array(sent_accelerations); 120 | 121 | print("got {} samples in {}s.".format(data['Q'].shape[0], duration), end=' ') 122 | print("(about {} Hz)".format(data['measured_frequency'])) 123 | -------------------------------------------------------------------------------- /excitation/robotCommunication/yarpGYM/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (C) 2014 Walkman 3 | # Author: 4 | # email: 5 | # 6 | # This program is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU Lesser General Public License as published by 8 | # the Free Software Foundation, either version 3 of the License, or 9 | # (at your option) any later version. 10 | # 11 | # This program is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | # GNU Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public License 17 | # along with this program. If not, see 18 | # 19 | 20 | cmake_minimum_required(VERSION 2.8.12) 21 | if(COMMAND cmake_policy) 22 | cmake_policy(SET CMP0003 OLD) 23 | cmake_policy(SET CMP0005 NEW) 24 | cmake_policy(SET CMP0017 NEW) 25 | endif(COMMAND cmake_policy) 26 | include(ExternalProject) 27 | project(excitation) 28 | 29 | include(CheckCXXCompilerFlag) 30 | check_cxx_compiler_flag("-std=c++11" COMPILER_SUPPORTS_CXX11) 31 | check_cxx_compiler_flag("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 32 | if(COMPILER_SUPPORTS_CXX11) 33 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -g") 34 | elseif(COMPILER_SUPPORTS_CXX0X) 35 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 36 | else() 37 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 38 | endif() 39 | 40 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}") 41 | 42 | FIND_PACKAGE(YARP REQUIRED) 43 | FIND_PACKAGE(idynutils REQUIRED) 44 | FIND_PACKAGE(iDynTree REQUIRED) 45 | FIND_PACKAGE(Eigen3 REQUIRED) 46 | FIND_PACKAGE(GYM REQUIRED) 47 | FIND_PACKAGE(paramHelp REQUIRED) 48 | 49 | # add include directories 50 | INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR} 51 | ${YARP_INCLUDE_DIRS} 52 | ${idynutils_INCLUDE_DIRS} 53 | ${GYM_INCLUDE_DIRS} 54 | ) 55 | 56 | FILE(GLOB_RECURSE LibFiles "src/*.h") 57 | add_custom_target(headersH SOURCES ${LibFiles}) 58 | FILE(GLOB_RECURSE LibFiles "src/*.hpp") 59 | add_custom_target(headersHPP SOURCES ${LibFiles}) 60 | 61 | ADD_EXECUTABLE(${PROJECT_NAME} src/excitation_main.cpp 62 | src/excitation_thread.cpp 63 | ) 64 | 65 | TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${iDynTree_LIBRARIES} 66 | ${YARP_LIBRARIES} 67 | ${idynutils_LIBRARIES} 68 | ${GYM_LIBRARIES} 69 | ) 70 | 71 | add_subdirectory(app) 72 | 73 | install(TARGETS ${PROJECT_NAME} DESTINATION bin) 74 | -------------------------------------------------------------------------------- /excitation/robotCommunication/yarpGYM/app/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (C) 2014 Walkman 3 | # Author: 4 | # email: 5 | # 6 | # This program is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU Lesser General Public License as published by 8 | # the Free Software Foundation, either version 3 of the License, or 9 | # (at your option) any later version. 10 | # 11 | # This program is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | # GNU Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public License 17 | # along with this program. If not, see 18 | # 19 | 20 | find_package(YARP REQUIRED) 21 | 22 | # pick up yarp's cmake scripts 23 | list(APPEND CMAKE_MODULE_PATH ${YARP_MODULE_PATH}) 24 | include(YarpInstallationHelpers) 25 | 26 | file(GLOB conf conf/*.ini) 27 | 28 | # directory name matches what is provided as "default context" in code 29 | yarp_install(FILES ${conf} DESTINATION ${YARP_CONTEXTS_INSTALL_DIR}/excitation) 30 | 31 | add_custom_target( excitation_conf_files_to_build ALL 32 | COMMAND ${CMAKE_COMMAND} -E copy_directory 33 | "${PROJECT_SOURCE_DIR}/app/conf" 34 | "${CMAKE_BINARY_DIR}/" 35 | ) 36 | -------------------------------------------------------------------------------- /excitation/robotCommunication/yarpGYM/app/conf/excitation_initial_config.ini: -------------------------------------------------------------------------------- 1 | # period of the control thread in [millisec] 2 | thread_period 5 3 | # name of the robot 4 | robot_name bigman 5 | -------------------------------------------------------------------------------- /excitation/robotCommunication/yarpGYM/src/excitation_constants.h: -------------------------------------------------------------------------------- 1 | #ifndef excitation_CONSTANTS_H_ 2 | #define excitation_CONSTANTS_H_ 3 | 4 | // param helper constants 5 | // max_vel : ID and SIZE definition 6 | #define PARAM_ID_MAX_VEL 1 7 | #define PARAM_SIZE_MAX_VEL 1 8 | 9 | #endif 10 | -------------------------------------------------------------------------------- /excitation/robotCommunication/yarpGYM/src/excitation_initial_config.ini: -------------------------------------------------------------------------------- 1 | # period of the control thread in [millisec] 2 | thread_period 5 3 | # name of the robot 4 | robot_name bigman 5 | -------------------------------------------------------------------------------- /excitation/robotCommunication/yarpGYM/src/excitation_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "excitation_module.hpp" 4 | 5 | // default module period 6 | #define MODULE_PERIOD 5 //[millisec] 7 | 8 | int main(int argc, char* argv[]) 9 | { 10 | // yarp network declaration and check 11 | yarp::os::Network yarp; 12 | if(!yarp.checkNetwork()){ 13 | std::cerr <<"yarpserver not running - run yarpserver"<< std::endl; 14 | exit(EXIT_FAILURE); 15 | } 16 | // yarp network initialization 17 | yarp.init(); 18 | 19 | // create rf 20 | yarp::os::ResourceFinder rf; 21 | rf.setVerbose(true); 22 | // set excitation_initial_config.ini as default 23 | // to specify another config file, run with this arg: --from your_config_file.ini 24 | rf.setDefaultConfigFile( "excitation_initial_config.ini" ); 25 | rf.setDefaultContext( "excitation" ); 26 | rf.configure(argc, argv); 27 | // create my module 28 | excitation_module excitation_mod = excitation_module( argc, argv, "excitation", MODULE_PERIOD, rf ); 29 | 30 | // run the module 31 | excitation_mod.runModule( rf ); 32 | 33 | // yarp network deinitialization 34 | yarp.fini(); 35 | 36 | exit(EXIT_SUCCESS); 37 | } 38 | -------------------------------------------------------------------------------- /excitation/robotCommunication/yarpGYM/src/excitation_module.hpp: -------------------------------------------------------------------------------- 1 | #ifndef excitation_MODULE_HPP_ 2 | #define excitation_MODULE_HPP_ 3 | 4 | #include 5 | 6 | #include "excitation_thread.h" 7 | #include "excitation_constants.h" 8 | 9 | /** 10 | * @brief excitation module derived from generic_module 11 | * 12 | * @author 13 | */ 14 | class excitation_module : public generic_module { 15 | public: 16 | 17 | /** 18 | * @brief constructor: do nothing but construct the superclass 19 | * 20 | */ 21 | excitation_module(int argc, 22 | char* argv[], 23 | std::string module_prefix, 24 | int module_period, 25 | yarp::os::ResourceFinder rf ) : generic_module( argc, 26 | argv, 27 | module_prefix, 28 | module_period, 29 | rf ) 30 | { 31 | } 32 | 33 | /** 34 | * @brief custom_get_ph_parameters inherit from generic module, we reimplement it since we have more parameters in the 35 | * param_help (tutorial_configuration.ini file) than the default ones. 36 | * @return a vector of the custom parameters for the param helper 37 | */ 38 | virtual std::vector< paramHelp::ParamProxyInterface* > custom_get_ph_parameters() 39 | { 40 | // custom param helper parameters vector 41 | std::vector custom_params; 42 | // insert max_vel param 43 | /*custom_params.push_back( new paramHelp::ParamProxyBasic( "max_vel", 44 | PARAM_ID_MAX_VEL, 45 | PARAM_SIZE_MAX_VEL, 46 | paramHelp::PARAM_IN_OUT, 47 | NULL, 48 | "maximum velocity in [degree/second]" ) ); 49 | */ 50 | return custom_params; 51 | } 52 | }; 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /excitation/robotCommunication/yarpGYM/src/excitation_msg.h: -------------------------------------------------------------------------------- 1 | /* Copyright [2016] [Stefan Bethge] 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License.*/ 14 | 15 | #ifndef EXCITATION_MSG 16 | #define EXCITATION_MSG 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | class excitation_msg 23 | { 24 | public: 25 | excitation_msg() { 26 | command = ""; 27 | //right 28 | angle0 = 0.0; 29 | angle1 = 0.0; 30 | angle2 = 0.0; 31 | angle3 = 0.0; 32 | angle4 = 0.0; 33 | angle5 = 0.0; 34 | //left 35 | angle6 = 0.0; 36 | angle7 = 0.0; 37 | angle8 = 0.0; 38 | angle9 = 0.0; 39 | angle10 = 0.0; 40 | angle11 = 0.0; 41 | } 42 | 43 | std::string command; 44 | double angle0, angle1, angle2, angle3, angle4, angle5; 45 | double angle6, angle7, angle8, angle9, angle10, angle11; 46 | 47 | yarp::os::Bottle toBottle() { 48 | yarp::os::Bottle temp; 49 | yarp::os::Bottle& list = temp.addList(); 50 | 51 | list.addString(command); 52 | 53 | if(command == "set_legs_refs") 54 | { 55 | list.addDouble(angle0); 56 | list.addDouble(angle1); 57 | list.addDouble(angle2); 58 | list.addDouble(angle3); 59 | list.addDouble(angle4); 60 | list.addDouble(angle5); 61 | list.addDouble(angle6); 62 | list.addDouble(angle7); 63 | list.addDouble(angle8); 64 | list.addDouble(angle9); 65 | list.addDouble(angle10); 66 | list.addDouble(angle11); 67 | } 68 | 69 | return temp; 70 | } 71 | 72 | void fromBottle(yarp::os::Bottle* temp) 73 | { 74 | if (temp->get(0).isNull()) 75 | { 76 | command=""; 77 | return; 78 | } 79 | yarp::os::Bottle* list = temp->get(0).asList(); 80 | if (list==NULL) 81 | { 82 | command=""; 83 | return; 84 | } 85 | if (list->get(0).isNull()) 86 | { 87 | command=""; 88 | return; 89 | } 90 | 91 | //TODO: check that list has enough entries and otherwise give error message 92 | if(list->size() < 6*2) { 93 | std::cout << "warning: not enough parameters given in Bottle!" << std::endl; 94 | return; 95 | } 96 | 97 | command = list->get(0).asString(); 98 | int index = 1; 99 | if(command == "set_legs_refs") 100 | { 101 | angle0 = list->get(index++).asDouble(); 102 | angle1 = list->get(index++).asDouble(); 103 | angle2 = list->get(index++).asDouble(); 104 | angle3 = list->get(index++).asDouble(); 105 | angle4 = list->get(index++).asDouble(); 106 | angle5 = list->get(index++).asDouble(); 107 | angle6 = list->get(index++).asDouble(); 108 | angle7 = list->get(index++).asDouble(); 109 | angle8 = list->get(index++).asDouble(); 110 | angle9 = list->get(index++).asDouble(); 111 | angle10 = list->get(index++).asDouble(); 112 | angle11 = list->get(index++).asDouble(); 113 | } 114 | 115 | return; 116 | } 117 | 118 | }; 119 | 120 | #endif 121 | -------------------------------------------------------------------------------- /excitation/robotCommunication/yarpGYM/src/excitation_thread.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "excitation_thread.h" 4 | #include "excitation_constants.h" 5 | #include "saveEigen.h" 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | using namespace std; 13 | using namespace Eigen; 14 | list < VectorXd > tmpLog; 15 | excitation_thread::excitation_thread( std::string module_prefix, 16 | yarp::os::ResourceFinder rf, 17 | std::shared_ptr< paramHelp::ParamHelperServer > ph) : 18 | ref_speed_vector( num_joints ), 19 | command_interface( module_prefix ), 20 | generic_thread( module_prefix, rf, ph ) 21 | { 22 | right_leg_chain_if = new walkman::yarp_single_chain_interface("right_leg", module_prefix, get_robot_name()); 23 | left_leg_chain_if = new walkman::yarp_single_chain_interface("left_leg", module_prefix, get_robot_name()); 24 | num_joints = right_leg_chain_if->getNumberOfJoints() + left_leg_chain_if->getNumberOfJoints(); 25 | 26 | // set controller mode on chain interface 27 | right_leg_chain_if->setPositionDirectMode(); //sets setpoint directly, uses maximum speed all the time 28 | left_leg_chain_if->setPositionDirectMode(); 29 | //chain_interface->setPositionMode(); //generate trajectories, allows setting velocities 30 | //chain_interface->setVelocityMode(); 31 | 32 | //init velocity control with zero velocity 33 | /*yarp::sig::Vector vels; 34 | if(vels.size()!=num_joints) vels.resize(num_joints); 35 | vels.zero(); 36 | chain_interface->move(vels);*/ 37 | 38 | //open data output port 39 | outgoingPort.open("/" + module_prefix + "/state:o"); 40 | } 41 | 42 | excitation_thread::~excitation_thread() 43 | { 44 | //save(&tmpLog, "log.csv"); 45 | } 46 | 47 | bool excitation_thread::custom_init() 48 | { 49 | // get a shared pointer to param helper 50 | std::shared_ptr ph = get_param_helper(); 51 | // link parameters (single value linking) 52 | //ph->linkParam( PARAM_ID_N_DOFS, &n_dofs ); 53 | 54 | return true; 55 | } 56 | 57 | void excitation_thread::run() 58 | { 59 | static yarp::sig::Vector pos; pos.resize(num_joints); 60 | //static yarp::sig::Vector vels; 61 | //if(vels.size()!=num_joints) vels.resize(num_joints); 62 | 63 | static yarp::sig::Vector q_r, q_l, qdot_r, qdot_l, tau_r, tau_l; 64 | double t = yarp::os::Time::now(); 65 | 66 | /* 67 | double l=1, w_f=1, a=0.5, b=0.8, q0=-35, nf=1; 68 | double angle = (a/(w_f*l)*sin(w_f*l*t) + a/(w_f*l)*cos(w_f*l*t))/M_PI*180; 69 | 70 | if(pos.size()!=num_joints) pos.resize(num_joints); 71 | pos.zero(); 72 | /*pos[0] = excitation_cmd.angle0; 73 | pos[1] = excitation_cmd.angle1; 74 | pos[2] = angle; 75 | pos[3] = angle; 76 | pos[4] = excitation_cmd.angle4; 77 | pos[5] = excitation_cmd.angle5; 78 | pos[6] = excitation_cmd.angle6; 79 | chain_interface->move(pos); 80 | */ 81 | 82 | static int seq_num; 83 | excitation_cmd.command = ""; 84 | command_interface.getCommand(excitation_cmd, seq_num); 85 | 86 | if( excitation_cmd.command == "set_legs_refs") { 87 | //read right leg refs 88 | pos[0] = excitation_cmd.angle0; 89 | pos[1] = excitation_cmd.angle1; 90 | pos[2] = excitation_cmd.angle2; 91 | pos[3] = excitation_cmd.angle3; 92 | pos[4] = excitation_cmd.angle4; 93 | pos[5] = excitation_cmd.angle5; 94 | 95 | //read left leg refs 96 | pos[6] = excitation_cmd.angle6; 97 | pos[7] = excitation_cmd.angle7; 98 | pos[8] = excitation_cmd.angle8; 99 | pos[9] = excitation_cmd.angle9; 100 | pos[10] = excitation_cmd.angle10; 101 | pos[11] = excitation_cmd.angle11; 102 | 103 | //chain_interface->setReferenceSpeeds(vels); 104 | right_leg_chain_if->move(pos.subVector(0,5)); 105 | left_leg_chain_if->move(pos.subVector(6,11)); 106 | 107 | //chain_interface->move(vels); 108 | 109 | // get state data and send 110 | right_leg_chain_if->sensePosition(q_r); //get positions in deg 111 | left_leg_chain_if->sensePosition(q_l); 112 | right_leg_chain_if->senseVelocity(qdot_r); //get velocities in deg/s 113 | left_leg_chain_if->senseVelocity(qdot_l); 114 | right_leg_chain_if->senseTorque(tau_r); //get torques in Nm 115 | left_leg_chain_if->senseTorque(tau_l); 116 | 117 | yarp::os::Bottle out_bottle; 118 | yarp::os::Bottle &out0 = out_bottle.addList(); 119 | yarp::os::Bottle &out1 = out_bottle.addList(); 120 | yarp::os::Bottle &out2 = out_bottle.addList(); 121 | 122 | for(int i=0; isensePosition(q); //get positions in deg 139 | chain_interface->senseVelocity(qdot); //get velocities in deg/s 140 | chain_interface->senseTorque(ta); //get torques in Nm 141 | 142 | yarp::os::Bottle out_bottle; 143 | yarp::os::Bottle &out0 = out_bottle.addList(); 144 | yarp::os::Bottle &out1 = out_bottle.addList(); 145 | yarp::os::Bottle &out2 = out_bottle.addList(); 146 | 147 | for(int i=0; i command not valid" << std::endl; 173 | } 174 | } 175 | -------------------------------------------------------------------------------- /excitation/robotCommunication/yarpGYM/src/excitation_thread.h: -------------------------------------------------------------------------------- 1 | #ifndef excitation_THREAD_H_ 2 | #define excitation_THREAD_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include "excitation_msg.h" 13 | 14 | /** 15 | * @brief excitation control thread 16 | * 17 | **/ 18 | class excitation_thread : public generic_thread 19 | { 20 | private: 21 | // walkman chain interfaces 22 | walkman::yarp_single_chain_interface *right_leg_chain_if; 23 | walkman::yarp_single_chain_interface *left_leg_chain_if; 24 | // chain configuration vector 25 | yarp::sig::Vector chain_configuration; 26 | // joints number 27 | int num_joints; 28 | // ref speed vector 29 | yarp::sig::Vector ref_speed_vector; 30 | // command interface 31 | walkman::yarp_custom_command_interface command_interface; 32 | excitation_msg excitation_cmd; 33 | 34 | //data output 35 | yarp::os::Bottle outgoingData; 36 | yarp::os::Port outgoingPort; 37 | 38 | public: 39 | 40 | /** 41 | * @brief constructor 42 | * 43 | * @param module_prefix the prefix of the module 44 | * @param rf resource finderce 45 | * @param ph param helper 46 | */ 47 | excitation_thread( std::string module_prefix, yarp::os::ResourceFinder rf, std::shared_ptr ph ); 48 | ~excitation_thread(); 49 | 50 | /** 51 | * @brief excitation control thread initialization 52 | * 53 | * @return true on succes, false otherwise 54 | */ 55 | virtual bool custom_init(); 56 | 57 | /** 58 | * @brief excitation control thread main loop 59 | * 60 | */ 61 | virtual void run(); 62 | }; 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /excitation/robotCommunication/yarpGYM/src/saveEigen.h: -------------------------------------------------------------------------------- 1 | /* 2 | * All rights reserved. 3 | * Copyright (C) 2014 Przemyslaw Kryczka kryczka.p@gmail.com 4 | * 5 | */ 6 | 7 | 8 | 9 | //------------------------------------------------------------------------------------------------ 10 | // 11 | //! @file saveEigen.h 12 | //! 13 | //! @date 2012-12-27 created by P. KRYCZKA 14 | // 15 | //------------------------------------------------------------------------------------------------ 16 | #ifndef SAVEEIGEN_H 17 | #define SAVEEIGEN_H 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include "saveEigen.h" 24 | #include 25 | 26 | // using namespace Eigen; 27 | // using namespace std; 28 | 29 | /** 30 | * @brief Brief description 31 | */ 32 | 33 | inline void save(Eigen::MatrixXd *mat, std::string filename) 34 | { 35 | std::ofstream file; 36 | file.open(filename.c_str(), std::ios::out); 37 | 38 | for (int j = 0; j < mat->rows(); j++) 39 | { 40 | for(int i = 0; i < mat->cols()-1 ; i++) 41 | file << (*mat)(j,i) << ", " ; 42 | file << (*mat)(j,mat->cols()-1) << "\n"; 43 | } 44 | 45 | file.close(); 46 | } 47 | 48 | 49 | inline void save_app(Eigen::MatrixXd *mat, std::string filename) 50 | { 51 | std::ofstream file; 52 | file.open(filename.c_str(), std::ofstream::out | std::ofstream::app); 53 | 54 | for (int j = 0; j < mat->rows(); j++) 55 | { 56 | for(int i = 0; i < mat->cols()-1 ; i++) 57 | file << (*mat)(j,i) << ", " ; 58 | file << (*mat)(j,mat->cols()-1) << "\n"; 59 | } 60 | file.close(); 61 | } 62 | 63 | inline void save(std::list *listLog, std::string filename) 64 | { 65 | std::ofstream file; 66 | file.open(filename.c_str(), std::ofstream::out); 67 | 68 | Eigen::VectorXd tmpVect; 69 | while (!listLog->empty()) 70 | { 71 | tmpVect = listLog->front(); 72 | listLog->pop_front(); 73 | file << tmpVect.transpose() << std::endl; 74 | 75 | } 76 | 77 | file.close(); 78 | } 79 | 80 | inline void save(Eigen::VectorXd *tmpVect, std::string filename) 81 | { 82 | std::ofstream file; 83 | file.open(filename.c_str(), std::ofstream::out); 84 | 85 | file << *tmpVect << std::endl; 86 | 87 | file.close(); 88 | } 89 | 90 | inline void save(std::list listLog, std::string filename) 91 | { 92 | std::ofstream file; 93 | file.open(filename.c_str(), std::ofstream::out); 94 | 95 | Eigen::Vector3d tmpVect; 96 | while (!listLog.empty()) 97 | { 98 | tmpVect = listLog.front(); 99 | listLog.pop_front(); 100 | file << tmpVect.transpose() << std::endl; 101 | 102 | } 103 | 104 | file.close(); 105 | } 106 | 107 | inline void save(std::list *listLog, std::string filename) 108 | { 109 | std::ofstream file; 110 | file.open(filename.c_str(), std::ofstream::out); 111 | 112 | Eigen::Vector2d tmpVect; 113 | while (!listLog->empty()) 114 | { 115 | tmpVect = listLog->front(); 116 | listLog->pop_front(); 117 | file << tmpVect.transpose() << std::endl; 118 | 119 | } 120 | 121 | file.close(); 122 | } 123 | //-------------------------------------------------------------- 124 | // Non Eigen functions 125 | //-------------------------------------------------------------- 126 | 127 | inline void save(std::list > *listLog, std::string filename) 128 | { 129 | std::ofstream file; 130 | file.open(filename.c_str(), std::ofstream::out); 131 | 132 | std::vector tmpVect; 133 | while (!listLog->empty()) 134 | { 135 | tmpVect = listLog->front(); 136 | listLog->pop_front(); 137 | for (int i=0; i *listLog, std::string filename) 147 | { 148 | std::ofstream file; 149 | file.open(filename.c_str(), std::ofstream::out); 150 | 151 | double tmpValue; 152 | while (!listLog->empty()) 153 | { 154 | tmpValue = listLog->front(); 155 | listLog->pop_front(); 156 | file << tmpValue << std::endl; 157 | } 158 | 159 | file.close(); 160 | } 161 | 162 | inline void save(std::list *listLog, std::string filename) 163 | { 164 | std::ofstream file; 165 | file.open(filename.c_str(), std::ofstream::out); 166 | 167 | int tmpValue; 168 | while (!listLog->empty()) 169 | { 170 | tmpValue = listLog->front(); 171 | listLog->pop_front(); 172 | file << tmpValue << std::endl; 173 | } 174 | 175 | file.close(); 176 | } 177 | 178 | 179 | inline void save(std::vector *listLog, std::string filename) 180 | { 181 | std::ofstream file; 182 | file.open(filename.c_str(), std::ofstream::out); 183 | 184 | double tmpValue; 185 | for (int i=0; isize();i++) 186 | { 187 | file << listLog->at(i) << std::endl; 188 | } 189 | 190 | file.close(); 191 | } 192 | 193 | inline void save(std::vector *listLog, std::string filename) 194 | { 195 | std::ofstream file; 196 | file.open(filename.c_str(), std::ofstream::out); 197 | 198 | double tmpValue; 199 | for (int i=0; isize();i++) 200 | { 201 | file << listLog->at(i) << std::endl; 202 | } 203 | 204 | file.close(); 205 | } 206 | 207 | inline void save(int number, std::string filename) 208 | { 209 | std::ofstream file; 210 | file.open(filename.c_str(), std::ofstream::out); 211 | 212 | file << number; 213 | 214 | file.close(); 215 | } 216 | 217 | 218 | #endif -------------------------------------------------------------------------------- /excitation/robotCommunication/yarp_gym.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | from __future__ import print_function 3 | from builtins import map 4 | from builtins import range 5 | import sys 6 | import yarp 7 | import numpy as np 8 | 9 | def gen_position_msg(msg_port, angles): 10 | bottle = msg_port.prepare() 11 | bottle.clear() 12 | angles_right, angles_left = angles[0:6], angles[6:] 13 | bottle.fromString("(set_legs_refs {} {}) 0".format(' '.join(map(str, angles_right)), ' '.join(map(str, angles_left)) )) 14 | return bottle 15 | 16 | def gen_command(msg_port, command): 17 | bottle = msg_port.prepare() 18 | bottle.clear() 19 | bottle.fromString("({}) 0".format(command)) 20 | return bottle 21 | 22 | def main(config, trajectory, out): 23 | # connect to yarp and open output port 24 | yarp.Network.init() 25 | yarp.Time.useNetworkClock("/clock") 26 | yarp.Time.now() #use clock once to sync (?) 27 | while not yarp.Time.isValid(): 28 | continue 29 | 30 | portName = '/excitation/command:' 31 | command_port = yarp.BufferedPortBottle() 32 | command_port.open(portName+'o') 33 | yarp.Network.connect(portName+'o', portName+'i') 34 | 35 | portName = '/excitation/state:' 36 | data_port = yarp.BufferedPortBottle() 37 | data_port.open(portName+"i") 38 | yarp.Network.connect(portName+'o', portName+'i') 39 | 40 | t_init = yarp.Time.now() 41 | t_elapsed = 0.0 42 | duration = config['args'].periods*trajectory.getPeriodLength() #init overall run duration to a periodic length 43 | 44 | measured_positions = list() 45 | measured_velocities = list() 46 | measured_accelerations = list() 47 | measured_torques = list() 48 | measured_time = list() 49 | 50 | sent_positions = list() 51 | sent_time = list() 52 | sent_velocities = list() 53 | sent_accelerations = list() 54 | 55 | # try high level p correction when using velocity ctrl 56 | #e = [0] * config['num_dofs'] 57 | #velocity_correction = [0] * config['num_dofs'] 58 | 59 | waited_for_start = 0 60 | started = False 61 | while t_elapsed < duration: 62 | trajectory.setTime(t_elapsed) 63 | target_angles = [trajectory.getAngle(i) for i in range(0, config['num_dofs'])] 64 | target_velocities = [trajectory.getVelocity(i) for i in range(0, config['num_dofs'])] 65 | target_accelerations = [trajectory.getAcceleration(i) for i in range(0, config['num_dofs'])] 66 | #for i in range(0, config['num_dofs']): 67 | # target_velocities[i]+=velocity_correction[i] 68 | 69 | # make sure we start moving at a position with zero velocity 70 | if not started: 71 | started = trajectory.wait_for_zero_vel(t_elapsed) 72 | t_elapsed = yarp.Time.now() - t_init 73 | waited_for_start = t_elapsed 74 | 75 | if started: 76 | # set angles and wait one period to have settled at zero velocity position 77 | gen_position_msg(command_port, target_angles) 78 | command_port.write() 79 | 80 | print("waiting to arrive at an initial position...", end=' ') 81 | sys.stdout.flush() 82 | yarp.Time.delay(trajectory.getPeriodLength()) 83 | t_init+=trajectory.getPeriodLength() 84 | duration+=waited_for_start 85 | print("ok.") 86 | continue 87 | 88 | # set target angles 89 | gen_position_msg(command_port, target_angles) 90 | command_port.write() 91 | 92 | sent_positions.append(target_angles) 93 | sent_velocities.append(target_velocities) 94 | sent_accelerations.append(target_accelerations) 95 | sent_time.append(yarp.Time.now()) 96 | 97 | # get and wait for next value, so sync to GYM loop 98 | data_out = data_port.read(shouldWait=True) 99 | 100 | b_positions = data_out.get(0).asList() 101 | b_velocities = data_out.get(1).asList() 102 | b_torques = data_out.get(2).asList() 103 | d_time = data_out.get(3).asDouble() 104 | 105 | positions = np.zeros(config['num_dofs']) 106 | velocities = np.zeros(config['num_dofs']) 107 | accelerations = np.zeros(config['num_dofs']) 108 | torques = np.zeros(config['num_dofs']) 109 | 110 | if config['num_dofs'] == b_positions.size(): 111 | for i in range(0, config['num_dofs']): 112 | positions[i] = b_positions.get(i).asDouble() 113 | velocities[i] = b_velocities.get(i).asDouble() 114 | torques[i] = b_torques.get(i).asDouble() 115 | else: 116 | print("warning, wrong amount of values received! ({} DOFS vs. {})".format(config['num_dofs'], b_positions.size())) 117 | 118 | # test manual correction for position error 119 | #p = 0 120 | #for i in range(0, config['num_dofs']): 121 | # e[i] = (angles[i] - positions[i]) 122 | # velocity_correction[i] = e[i]*p 123 | 124 | # collect measurement data 125 | measured_positions.append(positions) 126 | measured_velocities.append(velocities) 127 | measured_torques.append(torques) 128 | measured_time.append(d_time) 129 | t_elapsed = d_time - t_init 130 | 131 | # clean up 132 | command_port.close() 133 | data_port.close() 134 | out['Q'] = np.array(measured_positions); del measured_positions 135 | out['Qsent'] = np.array(sent_positions); 136 | out['QdotSent'] = np.array(sent_velocities); 137 | out['QddotSent'] = np.array(sent_accelerations); 138 | out['V'] = np.array(measured_velocities); del measured_velocities 139 | out['Tau'] = np.array(measured_torques); del measured_torques 140 | out['T'] = np.array(measured_time); del measured_time 141 | 142 | out['measured_frequency'] = len(sent_positions)/duration 143 | 144 | # some stats 145 | print("got {} samples in {}s.".format(out['Q'].shape[0], duration), end=' ') 146 | print("(about {} Hz)".format(out['measured_frequency'])) 147 | -------------------------------------------------------------------------------- /excite.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #-*- coding: utf-8 -*- 3 | 4 | from __future__ import division 5 | from __future__ import print_function 6 | from builtins import range 7 | import sys 8 | from typing import Dict 9 | 10 | import numpy as np 11 | from colorama import Fore, Back, Style 12 | import iDynTree; iDynTree.init_helpers(); iDynTree.init_numpy_helpers() 13 | 14 | import argparse 15 | parser = argparse.ArgumentParser(description='Send an excitation trajectory and record measurements to .') 16 | parser.add_argument('--model', required=True, type=str, help='the file to load the robot model from') 17 | parser.add_argument('--filename', type=str, help='the filename to save the measurements to') 18 | parser.add_argument('--trajectory', type=str, help='the file to load the trajectory from') 19 | parser.add_argument('--config', required=True, type=str, help="use options from given config file") 20 | parser.add_argument('--dryrun', help="don't actually send the trajectory", action='store_true') 21 | 22 | parser.add_argument('--plot', help='plot measured data', action='store_true') 23 | parser.add_argument('--plot-targets', dest='plot_targets', help="plot targets instead of measurements", action='store_true') 24 | parser.set_defaults(plot=False, plot_targets=False, dryrun=False, filename='measurements.npz') 25 | args = parser.parse_args() 26 | 27 | import yaml 28 | with open(args.config, 'r') as stream: 29 | try: 30 | config = yaml.load(stream) 31 | except yaml.YAMLError as exc: 32 | print(exc) 33 | 34 | config['args'] = args 35 | config['urdf'] = args.model 36 | config['plot_targets'] = args.plot_targets 37 | config['jointNames'] = iDynTree.StringVector([]) 38 | if not iDynTree.dofsListFromURDF(config['urdf'], config['jointNames']): 39 | sys.exit() 40 | config['num_dofs'] = len(config['jointNames']) 41 | 42 | #append parent dir for relative import 43 | #import os 44 | #sys.path.insert(1, os.path.join(sys.path[0], '..')) 45 | 46 | from excitation.optimizer import plotter 47 | from excitation.trajectoryGenerator import PulsedTrajectory, FixedPositionTrajectory, simulateTrajectory 48 | 49 | traj_data = {} # type: Dict[str, np._ArrayLike] # hold some global data vars in here 50 | 51 | def main(): 52 | if args.trajectory: 53 | traj_file = args.trajectory 54 | else: 55 | traj_file = config['urdf'] + '.trajectory.npz' 56 | 57 | #load from file 58 | try: 59 | # replay optimized trajectory if found 60 | tf = np.load(traj_file, encoding='latin1') 61 | if 'static' in tf and tf['static']: 62 | # static posture file 63 | trajectory = FixedPositionTrajectory(config) 64 | trajectory.initWithAngles(tf['angles']) 65 | print("using static postures from file {}".format(traj_file)) 66 | else: 67 | # proper trajectory 68 | trajectory = PulsedTrajectory(config['num_dofs'], use_deg=tf['use_deg']) 69 | trajectory.initWithParams(tf['a'], tf['b'], tf['q'], tf['nf'], tf['wf']) 70 | print("using trajectory from file {}".format(traj_file)) 71 | except IOError: 72 | print("No trajectory file found, can't excite ({})!".format(traj_file)) 73 | sys.exit(1) 74 | 75 | # generating simulation of trajectory in any case 76 | traj_data, data = simulateTrajectory(config, trajectory) 77 | if config['excitationSimulate'] and config['exciteMethod']: 78 | print(Fore.RED + 'Using simulated torques!' + Fore.RESET) 79 | 80 | if args.dryrun: 81 | return 82 | 83 | # excite real robot 84 | if config['exciteMethod'] == 'yarp': 85 | from excitation.robotCommunication import yarp_gym 86 | yarp_gym.main(config, trajectory, traj_data) 87 | elif config['exciteMethod'] == 'ros': 88 | from excitation.robotCommunication import ros_moveit 89 | ros_moveit.main(config, trajectory, traj_data) 90 | else: 91 | # or just use simulation data 92 | print("No excitation method given! Only doing simulation") 93 | saveMeasurements(args.filename, traj_data) 94 | return 95 | 96 | #adapt measured array sizes to input array sizes 97 | traj_data['Q'] = np.resize(traj_data['Q'], data.samples['positions'].shape) 98 | traj_data['V'] = np.resize(traj_data['V'], data.samples['velocities'].shape) 99 | traj_data['Tau'] = np.resize(traj_data['Tau'], data.samples['torques'].shape) 100 | traj_data['T'] = np.resize(traj_data['T'], data.samples['times'].shape) 101 | 102 | # generate some empty arrays, will be calculated in preprocess() 103 | if 'Vdot' not in traj_data: 104 | traj_data['Vdot'] = np.zeros_like(traj_data['V']) 105 | traj_data['Vraw'] = np.zeros_like(traj_data['V']) 106 | traj_data['Qraw'] = np.zeros_like(traj_data['Q']) 107 | traj_data['TauRaw'] = np.zeros_like(traj_data['Tau']) 108 | 109 | # if simulating torques, prepare some arrays with proper length (needs to be same as input for 110 | # simulation) 111 | if config['excitationSimulate']: 112 | tau_len = traj_data['Tau'].shape[0] # get length of measured (zero) taus 113 | if tau_len < traj_data['torques'].shape[0]: 114 | #less measured samples than input samples 115 | traj_data['Tau'][:,:] = traj_data['torques'][0:tau_len,:] 116 | if config['exciteMethod'] == None: 117 | traj_data['V'][:,:] = traj_data['velocities'][0:tau_len,:] 118 | else: 119 | #less or equal input samples than measured samples 120 | torques_len = traj_data['torques'].shape[0] 121 | traj_data['Tau'][:torques_len, :] = traj_data['torques'][:,:] 122 | if config['exciteMethod'] == None: 123 | traj_data['V'] = traj_data['velocities'][:,:] 124 | 125 | # filter, differentiate, convert, etc. 126 | data.preprocess(Q=traj_data['Q'], Q_raw=traj_data['Qraw'], V=traj_data['V'], 127 | V_raw=traj_data['Vraw'], Vdot=traj_data['Vdot'], Tau=traj_data['Tau'], 128 | Tau_raw = traj_data['TauRaw'], T=traj_data['T'], Fs=traj_data['measured_frequency']) 129 | 130 | # use simulated torques as measured data (since e.g. Gazebo produces unusable torque values) 131 | # (simulate again with measured/filtered data) 132 | if config['excitationSimulate']: 133 | traj_data_sim, data = simulateTrajectory(config, trajectory, measurements=traj_data) 134 | traj_data['Tau'] = data.samples['torques'] 135 | 136 | saveMeasurements(args.filename, traj_data) 137 | 138 | def saveMeasurements(filename, data): 139 | # write sample arrays to data file 140 | if config['exciteMethod']: 141 | np.savez(filename, 142 | positions=data['Q'], positions_raw=data['Qraw'], 143 | velocities=data['V'], velocities_raw=data['Vraw'], 144 | accelerations=data['Vdot'], 145 | torques=data['Tau'], torques_raw=data['TauRaw'], 146 | target_positions=np.deg2rad(data['Qsent']), target_velocities=np.deg2rad(data['QdotSent']), 147 | target_accelerations=np.deg2rad(data['QddotSent']), 148 | base_velocity=data['base_velocity'], base_acceleration=data['base_acceleration'], 149 | base_rpy=data['base_rpy'], contacts=data['contacts'], 150 | times=data['T'], frequency=data['measured_frequency']) 151 | else: 152 | np.savez(filename, 153 | positions=data['positions'], positions_raw=data['positions'], 154 | velocities=data['velocities'], velocities_raw=data['velocities'], 155 | accelerations=data['accelerations'], 156 | torques=data['torques'], torques_raw=data['torques'], 157 | target_positions=np.deg2rad(data['target_positions']), 158 | target_velocities=np.deg2rad(data['target_velocities']), 159 | target_accelerations=np.deg2rad(data['target_accelerations']), 160 | base_velocity=data['base_velocity'], base_acceleration=data['base_acceleration'], 161 | base_rpy=data['base_rpy'], contacts=data['contacts'], 162 | times=data['times'], frequency=data['measured_frequency']) 163 | 164 | print("saved measurements to {}".format(args.filename)) 165 | 166 | if __name__ == '__main__': 167 | main() 168 | if(args.plot): 169 | plotter(config, filename=args.filename) 170 | -------------------------------------------------------------------------------- /identification/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/identification/__init__.py -------------------------------------------------------------------------------- /identification/quaternion.py: -------------------------------------------------------------------------------- 1 | 2 | import math 3 | import numpy as np 4 | 5 | class Quaternion(object): 6 | @classmethod 7 | def rotateVbyQ(cls, v, q): 8 | ''' rotate vector v (v0,v1,v2) b quaternion q (x,y,z,w) ''' 9 | qv = np.zeros(4) 10 | qv[:3] = v.copy() 11 | 12 | qconj = Quaternion.conjugate(q) 13 | q_prime = Quaternion.prod( Quaternion.prod(q, qv), qconj ) 14 | return q_prime[:3] 15 | 16 | def prod(q1, q2): 17 | """ Perform the Hamiltonian product of two quaternions. Note that this product 18 | is non-commutative -- this function returns q1 x q2. """ 19 | 20 | if (len(q1) != 4) or (len(q2) != 4): 21 | raise TypeError('Parameters cannot be interpreted as quaternions') 22 | 23 | qprod = np.zeros(4) 24 | 25 | qprod[0] = q1[3]*q2[0] + q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] 26 | qprod[1] = q1[3]*q2[1] - q1[0]*q2[2] + q1[1]*q2[3] + q1[2]*q2[0] 27 | qprod[2] = q1[3]*q2[2] + q1[0]*q2[1] - q1[1]*q2[0] + q1[2]*q2[3] 28 | qprod[3] = q1[3]*q2[3] - q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] 29 | 30 | return qprod 31 | 32 | @classmethod 33 | def conjugate(cls, q): 34 | """ Compute the quaternion conjugate of q. """ 35 | 36 | if len(q) != 4: 37 | raise TypeError('Parameter `q` cannot be interpreted as a quaternion') 38 | 39 | qconj = np.zeros(4) 40 | qconj[0] = -q[0] 41 | qconj[1] = -q[1] 42 | qconj[2] = -q[2] 43 | qconj[3] = q[3] 44 | 45 | return qconj 46 | 47 | 48 | @classmethod 49 | def fromRPY(cls, roll, pitch, yaw): 50 | t0 = np.cos(yaw * 0.5) 51 | t1 = np.sin(yaw * 0.5) 52 | t2 = np.cos(roll * 0.5) 53 | t3 = np.sin(roll * 0.5) 54 | t4 = np.cos(pitch * 0.5) 55 | t5 = np.sin(pitch * 0.5) 56 | 57 | q = np.zeros(4) 58 | q[0] = t0 * t3 * t4 - t1 * t2 * t5 #x 59 | q[1] = t0 * t2 * t5 + t1 * t3 * t4 #y 60 | q[2] = t1 * t2 * t4 - t0 * t3 * t5 #z 61 | q[3] = t0 * t2 * t4 + t1 * t3 * t5 #w 62 | return q 63 | 64 | @classmethod 65 | def fromSO3(cls, rotMat): 66 | """ Return quaternion from rotation matrix. """ 67 | 68 | """ 69 | # transforms3d 70 | M = np.array(rotMat, dtype=np.float64, copy=False)[:4, :4] 71 | q = np.empty((4, )) 72 | t = np.trace(M) 73 | i, j, k = 0, 1, 2 74 | if M[1, 1] > M[0, 0]: 75 | i, j, k = 1, 2, 0 76 | if M[2, 2] > M[i, i]: 77 | i, j, k = 2, 0, 1 78 | t = M[i, i] - (M[j, j] + M[k, k]) + 1 79 | q[i] = t 80 | q[j] = M[i, j] + M[j, i] 81 | q[k] = M[k, i] + M[i, k] 82 | q[3] = M[k, j] - M[j, k] 83 | q = q[[3, 0, 1, 2]] 84 | q *= 0.5 / math.sqrt(t * 1) 85 | if q[0] < 0.0: 86 | np.negative(q, q) 87 | return q 88 | """ 89 | 90 | ''' 91 | # siciliano 92 | def sign(x): 93 | if x < 0: 94 | return -1 95 | else: 96 | return 1 97 | r = rotMat 98 | x = 0.5 * (sign(r[2,1]-r[1,2]) * np.sqrt(r[0,0]-r[1,1]-r[2,2] + 1) ) 99 | y = 0.5 * (sign(r[0,2]-r[2,0]) * np.sqrt(r[1,1]-r[2,2]-r[0,0] + 1) ) 100 | z = 0.5 * (sign(r[1,0]-r[0,1]) * np.sqrt(r[2,2]-r[0,0]-r[1,1] + 1) ) 101 | 102 | w = 0.5 * np.sqrt(np.sum(np.diag(rotMat)) + 1) 103 | 104 | return [x,y,z,w] 105 | ''' 106 | 107 | # from "Converting a Rotation Matrix to a Quaternion" by Mike Day 108 | r = rotMat 109 | 110 | if r[2,2] < 0: 111 | if r[0,0] > r[1,1]: 112 | t = 1 + r[0,0] - r[1,1] - r[2,2] 113 | q = np.array([t, r[0,1]+r[1,0], r[2,0]+r[0,2], r[1,2]-r[2,1]], dtype=np.float64) 114 | else: 115 | t = 1 - r[0,0] + r[1,1] - r[2,2] 116 | q = np.array([r[0,1]+r[1,0], t, r[1,2]+r[2,1], r[2,0]-r[0,2]], dtype=np.float64) 117 | else: 118 | if r[0,0] < -r[1,1]: 119 | t = 1 - r[0,0] - r[1,1] + r[2,2] 120 | q = np.array([r[2,0]+r[0,2], r[1,2]+r[2,1], t, r[0,1]-r[1,0]], dtype=np.float64) 121 | else: 122 | t = 1 + r[0,0] + r[1,1] + r[2,2] 123 | q = np.array([r[1,2]-r[2,1], r[2,0]-r[0,2], r[0,1]-r[1,0], t], dtype=np.float64) 124 | q *= 0.5 / np.sqrt(t) 125 | 126 | return q 127 | 128 | @classmethod 129 | def toSO3(cls, quaternion): 130 | """return rotation matrix for quaternion q""" 131 | 132 | """ 133 | q = np.array(quaternion, dtype=np.float64, copy=True) 134 | n = np.dot(q, q) 135 | if n < np.finfo(float).eps * 4.0: #test if close to zero 136 | return np.identity(3) 137 | q *= np.sqrt(2.0 / n) 138 | q = np.outer(q, q) 139 | 140 | return np.array([ 141 | [1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0]], 142 | [ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0]], 143 | [ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2]], 144 | ]) 145 | """ 146 | 147 | x, y, z, w = quaternion 148 | xx = x ** 2 149 | yy = y ** 2 150 | zz = z ** 2 151 | ww = w ** 2 152 | xy = x * y 153 | wz = w * z 154 | xz = x * z 155 | wy = w * y 156 | yz = y * z 157 | wx = w * x 158 | 159 | return np.array([ 160 | [2*(ww + xx) - 1, 2*(xy - wz), 2*(xz + wy)], 161 | [2*(xy + wz), 2*(ww + yy) - 1, 2*(yz - wx)], 162 | [2*(xz - wy), 2*(yz + wx), 2*(ww + zz) - 1] 163 | ]) 164 | -------------------------------------------------------------------------------- /identification/sdp_helpers.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | from builtins import str 3 | from builtins import range 4 | 5 | from typing import Tuple, List 6 | 7 | import sympy 8 | from sympy import Basic, BlockDiagMatrix, Symbol, sympify 9 | from distutils.version import LooseVersion 10 | old_sympy = LooseVersion(sympy.__version__) < LooseVersion('0.7.4') 11 | 12 | import numpy as np 13 | 14 | import cvxopt 15 | from cvxopt import matrix 16 | import lmi_sdp 17 | 18 | epsilon_sdptol = 1e-6 19 | 20 | from colorama import Fore, Back 21 | 22 | #simplified LMI definitions (works with newer sympy, lmi_sdp variants do not) 23 | def LMI_PD(lhs, rhs=0): 24 | # type: (sympy.Eq, sympy.Eq) -> (sympy.Eq) 25 | if old_sympy: 26 | lmi = lmi_sdp.LMI_PD(lhs, rhs) 27 | else: 28 | lmi = lhs > sympify(rhs) 29 | 30 | return lmi 31 | 32 | def LMI_PSD(lhs, rhs=0): 33 | # type: (sympy.Eq, sympy.Eq) -> (sympy.Eq) 34 | if old_sympy: 35 | lmi = lmi_sdp.LMI_PSD(lhs, rhs) 36 | else: 37 | lmi = lhs >= sympify(rhs) 38 | return lmi 39 | 40 | ##copied some methods from lmi_sdp here for compatibility changes 41 | def lmi_to_coeffs(lmi, variables, split_blocks=False): 42 | # type: (List[sympy.Matrix], List[Symbol], bool) -> List[sympy.Matrix] 43 | """Transforms LMIs from symbolic to numerical. 44 | 45 | Parameters 46 | ---------- 47 | lmi: symbolic LMI or Matrix, or a list of them 48 | variables: list of symbols 49 | split_blocks: bool or string 50 | If set to True, function tries to subdivide each LMI into 51 | smaller diagonal blocks. If set to 'BlockDiagMatrix', 52 | BlockDiagMatrix's are split into their diagonal blocks but the 53 | funtion does not try to subdivide them any further. 54 | 55 | Returns 56 | ------- 57 | coeffs: list of numerical LMIs 58 | List of numerical LMIs where each one is a pair where the first 59 | element is a list of numpy arrays corresponding to the coefficients of 60 | each variable, and the second element is a numpy array with zero order 61 | coefficients (constants not multipling by any variable). The 62 | numerical coefficients are extracted from the matrix `M` of the 63 | canonical PSD (or PD) LMI form `M>=0` (or `M>0`). 64 | 65 | Example 66 | ------- 67 | >>> from sympy import Matrix 68 | >>> from sympy.abc import x, y, z 69 | >>> from lmi_sdp import LMI_PSD, lmi_to_coeffs 70 | >>> vars = [x, y, z] 71 | >>> m = Matrix([[x+3, y-2], [y-2, z]]) 72 | >>> lmi = LMI_PSD(m) 73 | >>> lmi_to_coeffs(lmi, vars) 74 | [([array([[ 1., 0.], 75 | [ 0., 0.]]), array([[ 0., 1.], 76 | [ 1., 0.]]), array([[ 0., 0.], 77 | [ 0., 1.]])], array([[ 3., -2.], 78 | [-2., 0.]]))] 79 | """ 80 | 81 | if old_sympy: 82 | return lmi_sdp.lmi_to_coeffs(lmi, variables, split_blocks) 83 | 84 | if isinstance(lmi, Basic): 85 | lmis = [lmi] 86 | else: 87 | lmis = list(lmi) 88 | 89 | slms = [] # SLM stands for 'Symmetric Linear Matrix' 90 | for lmi in lmis: 91 | if lmi.is_Matrix: 92 | lmi = LMI_PSD(lmi) 93 | lm = lmi.canonical.gts 94 | slms.append(lm) 95 | 96 | if split_blocks: 97 | orig_slms = slms 98 | slms = [] 99 | for slm in orig_slms: 100 | if isinstance(slm, BlockDiagMatrix): 101 | if split_blocks == 'BlockDiagMatrix': 102 | slms += slm.diag 103 | else: 104 | slms += sum([d.get_diag_blocks() for d in slm.diag], []) 105 | else: 106 | slms += slm.get_diag_blocks() 107 | 108 | coeffs = [lmi_sdp.lm_sym_to_coeffs(slm, variables) for slm in slms] 109 | 110 | return coeffs 111 | 112 | def to_cvxopt(objective_func, lmis, variables, objective_type='minimize', 113 | split_blocks=True): 114 | """Prepare objective and LMI to be used with cvxopt SDP solver. 115 | 116 | Parameters 117 | ---------- 118 | objective_func: symbolic linear expression 119 | lmi: symbolic LMI or Matrix, or a list of them 120 | variables: list of symbols 121 | The variable symbols which form the LMI/SDP space. 122 | objective_type: 'maximize' or 'minimize', defaults to 'minimize' 123 | split_blocks: bool 124 | If set to True, function tries to subdivide each LMI into 125 | smaller diagonal blocks 126 | 127 | Returns 128 | ------- 129 | c, Gs, hs: parameters ready to be input to cvxopt.solvers.sdp() 130 | """ 131 | if cvxopt is None: 132 | raise lmi_sdp.sdp.NotAvailableError(to_cvxopt.__name__) 133 | 134 | obj_coeffs = lmi_sdp.objective_to_coeffs(objective_func, variables, 135 | objective_type) 136 | lmi_coeffs = lmi_to_coeffs(lmis, variables, split_blocks) 137 | 138 | c = matrix(obj_coeffs) 139 | 140 | Gs = [] 141 | hs = [] 142 | 143 | for (LMis, LM0) in lmi_coeffs: 144 | Gs.append(matrix([(-LMi).flatten().astype(float).tolist() 145 | for LMi in LMis])) 146 | hs.append(matrix(LM0.astype(float).tolist())) 147 | 148 | return c, Gs, hs 149 | 150 | 151 | def to_sdpa_sparse(objective_func, lmis, variables, objective_type='minimize', 152 | split_blocks=True, comment=None): 153 | """Put problem (objective and LMIs) into SDPA sparse format.""" 154 | obj_coeffs = lmi_sdp.objective_to_coeffs(objective_func, variables, 155 | objective_type) 156 | lmi_coeffs = lmi_to_coeffs(lmis, variables, split_blocks) 157 | 158 | s = lmi_sdp.sdp._sdpa_header(obj_coeffs, lmi_coeffs, comment) 159 | 160 | def _print_sparse(x, b, m, sign=1): 161 | s = '' 162 | shape = m.shape 163 | for i in range(shape[0]): 164 | for j in range(i, shape[1]): 165 | e = sign*m[i, j] 166 | if e != 0: 167 | s += '%d %d %d %d %s\n' % (x, b, i+1, j+1, str(e)) 168 | return s 169 | 170 | for b in range(len(lmi_coeffs)): 171 | s += _print_sparse(0, b+1, lmi_coeffs[b][1], sign=-1) 172 | for x in range(len(obj_coeffs)): 173 | for b in range(len(lmi_coeffs)): 174 | s += _print_sparse(x+1, b+1, lmi_coeffs[b][0][x]) 175 | 176 | return s 177 | 178 | def cvxopt_conelp(objf, lmis, variables, primalstart=None): 179 | # type: (List[Symbol], List[sympy.Eq], List[Symbol], np._ArrayLike) -> Tuple[np.matrix, str] 180 | ''' using cvxopt conelp to solve SDP program 181 | 182 | a more exact but possibly less robust solver than dsdp5 183 | 184 | TODO: is currently not using primalstart argument (benefits?) 185 | primalstart['sl'] - initial value of u? 186 | primalsstart['x'] - initial values of x 187 | primalsstart['ss'] - value like hs for initial x values (lmis replaced with primal values 188 | and converted to cvxopt matrix format), must be within constraints 189 | 190 | Notes: 191 | - Errors of the form "Rank(A) < p or Rank([G; A]) < n" mean that there are linear 192 | dependencies in the problem matrix A, i.e. too many base parameters/columns are 193 | determined (depends on proper base regressor, dependent e.g. on minTol value. If data has 194 | too little dependencies, use structural regressor) or in the constraints G (one might 195 | need to add constraints). 196 | ''' 197 | 198 | import cvxopt.solvers 199 | c, Gs, hs = to_cvxopt(objf, lmis, variables) 200 | cvxopt.solvers.options['maxiters'] = 100 201 | cvxopt.solvers.options['show_progress'] = False 202 | #cvxopt.solvers.options['feastol'] = 1e-5 203 | 204 | sdpout = cvxopt.solvers.sdp(c, Gs=Gs, hs=hs) 205 | state = sdpout['status'] 206 | if sdpout['status'] == 'optimal': 207 | #print("(does not necessarily mean feasible)") 208 | pass 209 | elif primalstart is not None: 210 | # return primalstart if no solution was found 211 | print(Fore.RED + '{}'.format(sdpout['status']) + Fore.RESET) 212 | sdpout['x'] = np.reshape( np.concatenate(([0], primalstart)), (len(primalstart)+1, 1) ) 213 | return np.matrix(sdpout['x']), state 214 | 215 | 216 | def cvxopt_dsdp5(objf, lmis, variables, primalstart=None, wide_bounds=False): 217 | # type: (List[Symbol], List[sympy.Eq], List[Symbol], np._ArrayLike, bool) -> Tuple[np.matrix, str] 218 | # using cvxopt interface to dsdp5 219 | # (not using primal atm) 220 | import cvxopt.solvers 221 | c, Gs, hs = to_cvxopt(objf, lmis, variables) 222 | cvxopt.solvers.options['dsdp']= {'DSDP_GapTolerance': epsilon_sdptol, 'DSDP_Monitor': 10} 223 | if wide_bounds: 224 | sdpout = cvxopt.solvers.sdp(c, Gs=Gs, hs=hs, beta=10e15, gama=10e15, solver='dsdp') 225 | else: 226 | sdpout = cvxopt.solvers.sdp(c, Gs=Gs, hs=hs, solver='dsdp') 227 | state = sdpout['status'] 228 | if sdpout['status'] == 'optimal': 229 | print("{}".format(sdpout['status'])) 230 | #print("(does not necessarily mean feasible)") 231 | else: 232 | print(Fore.RED + '{}'.format(sdpout['status']) + Fore.RESET) 233 | return np.matrix(sdpout['x']), state 234 | 235 | 236 | def dsdp5(objf, lmis, variables, primalstart=None, wide_bounds=False): 237 | # type: (List[Symbol], List[sympy.Eq], List[Symbol], np._ArrayLike, bool) -> Tuple[np.matrix, str] 238 | ''' use dsdp5 directly (faster than cvxopt, can use starting points, more robust) ''' 239 | import subprocess 240 | import os 241 | 242 | sdpadat = to_sdpa_sparse(objf, lmis, variables) 243 | import uuid 244 | dir = os.path.join(os.path.dirname(os.path.abspath(__file__)), 'sdpa_dat_{}'.format(uuid.uuid4())) 245 | if not os.path.exists(dir): 246 | os.makedirs(dir) 247 | 248 | with open(os.path.join(dir, 'sdp.dat-s'), 'w') as f: 249 | f.write(sdpadat) 250 | 251 | if primalstart is None: 252 | primalstart = np.zeros(len(variables)-1) 253 | with open(os.path.join(dir, 'primal.dat'), 'wb') as f: 254 | np.savetxt(f, primalstart) 255 | 256 | # change options to allow for far away solutions 257 | if wide_bounds: 258 | bounds = ['-boundy', '1e15', '-penalty', '1e15'] 259 | else: 260 | bounds = [] 261 | 262 | try: 263 | result = subprocess.check_output(['dsdp5', 'sdp.dat-s', '-save', 'dsdp5.out', '-gaptol', 264 | '{}'.format(epsilon_sdptol)] + bounds + 265 | ['-y0', 'primal.dat'], 266 | cwd = dir).decode('utf-8') 267 | state = 'optimal' 268 | except subprocess.CalledProcessError as e: 269 | print("DSDP stopped early: {}".format(e.returncode)) 270 | state = 'stopped' 271 | result = e.output 272 | 273 | error = list() 274 | for s in result.split('\n'): 275 | if 'DSDP Terminated Due to' in s: 276 | error.append(s) 277 | state = 'infeasible' 278 | if 'DSDP Primal Unbounded, Dual Infeasible' in s: 279 | error.append(s) 280 | state = 'infeasible' 281 | if 'DSDP Converged' in s: 282 | # there can be converged and dual infeasible messages but errors come last 283 | state = 'optimal' 284 | 285 | #if state != 'optimal': 286 | #if there were errors, print all the output 287 | # print(result) 288 | 289 | if error: 290 | state = 'infeasible' 291 | print(Fore.RED + error[0] + Fore.RESET) 292 | else: 293 | print(state) 294 | outfile = open(os.path.join(dir, 'dsdp5.out'), 'r').readlines() 295 | sol = [float(v) for v in outfile[0].split()] 296 | 297 | # remove tmp dir again 298 | import shutil 299 | shutil.rmtree(dir) 300 | 301 | return np.matrix(sol).T, state 302 | 303 | 304 | #set a default solver 305 | solve_sdp = cvxopt_conelp # choose one from dsdp5, cvxopt_dsdp5, cvxopt_conelp 306 | 307 | # it seems cvxopt_conelp and cvxopt_dsdp5 are working well when the data is good whereas dsdp5 308 | # sometimes fails that situation completely. However, in some bad data situations dsdp5 performs very well 309 | # (with changed bounds) where the other two don't work. 310 | -------------------------------------------------------------------------------- /model/kuka_lwr4.urdf.trajectory_opt_1.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/kuka_lwr4.urdf.trajectory_opt_1.npz -------------------------------------------------------------------------------- /model/kuka_lwr4_centroepiaggio.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | true 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | -------------------------------------------------------------------------------- /model/kuka_lwr4_regressor.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lwr_0_joint 5 | lwr_1_joint 6 | lwr_2_joint 7 | lwr_3_joint 8 | lwr_4_joint 9 | lwr_5_joint 10 | lwr_6_joint 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /model/meshes/kuka/COPYRIGHT: -------------------------------------------------------------------------------- 1 | The meshes of the lightweight robot are based on a blender model 2 | which was re-designed from scratch. (It can be downloaded at 3 | http://toychest.in.tum.de/wiki/_media/projects:lwr-arm.blend) 4 | 5 | This blender model, as well as the meshes found in this folder 6 | are licensed under CC-BY-3.0: 7 | 8 | Copyright (c) 2010 by Ingo Kresse . 9 | This work is made available under the terms of the 10 | Creative Commons Attribution 3.0 license, 11 | http://creativecommons.org/licenses/by/3.0/. 12 | 13 | -------------------------------------------------------------------------------- /model/meshes/kuka/base.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/kuka/base.STL -------------------------------------------------------------------------------- /model/meshes/kuka/link_1.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/kuka/link_1.STL -------------------------------------------------------------------------------- /model/meshes/kuka/link_2.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/kuka/link_2.STL -------------------------------------------------------------------------------- /model/meshes/kuka/link_3.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/kuka/link_3.STL -------------------------------------------------------------------------------- /model/meshes/kuka/link_4.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/kuka/link_4.STL -------------------------------------------------------------------------------- /model/meshes/kuka/link_5.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/kuka/link_5.STL -------------------------------------------------------------------------------- /model/meshes/kuka/link_6.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/kuka/link_6.STL -------------------------------------------------------------------------------- /model/meshes/kuka/link_7.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/kuka/link_7.STL -------------------------------------------------------------------------------- /model/meshes/walkman/AnklePitch.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/AnklePitch.STL -------------------------------------------------------------------------------- /model/meshes/walkman/BottomUpperArm.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/BottomUpperArm.STL -------------------------------------------------------------------------------- /model/meshes/walkman/Chest.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/Chest.STL -------------------------------------------------------------------------------- /model/meshes/walkman/Elbow.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/Elbow.STL -------------------------------------------------------------------------------- /model/meshes/walkman/Forearm.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/Forearm.STL -------------------------------------------------------------------------------- /model/meshes/walkman/ForearmPitch.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/ForearmPitch.STL -------------------------------------------------------------------------------- /model/meshes/walkman/ForearmRoll.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/ForearmRoll.STL -------------------------------------------------------------------------------- /model/meshes/walkman/Head.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/Head.STL -------------------------------------------------------------------------------- /model/meshes/walkman/HipPitch.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/HipPitch.STL -------------------------------------------------------------------------------- /model/meshes/walkman/HipRoll.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/HipRoll.STL -------------------------------------------------------------------------------- /model/meshes/walkman/HipYaw.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/HipYaw.STL -------------------------------------------------------------------------------- /model/meshes/walkman/KneePitch.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/KneePitch.STL -------------------------------------------------------------------------------- /model/meshes/walkman/KneePitchCalf.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/KneePitchCalf.STL -------------------------------------------------------------------------------- /model/meshes/walkman/NeckPitch.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/NeckPitch.STL -------------------------------------------------------------------------------- /model/meshes/walkman/NeckYaw.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/NeckYaw.STL -------------------------------------------------------------------------------- /model/meshes/walkman/NewChest.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/NewChest.STL -------------------------------------------------------------------------------- /model/meshes/walkman/NewFoot.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/NewFoot.STL -------------------------------------------------------------------------------- /model/meshes/walkman/PelvisWeightBench.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/PelvisWeightBench.STL -------------------------------------------------------------------------------- /model/meshes/walkman/ProtectionSystem.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/ProtectionSystem.STL -------------------------------------------------------------------------------- /model/meshes/walkman/RaisingSupport.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/RaisingSupport.STL -------------------------------------------------------------------------------- /model/meshes/walkman/ShoulderPitch.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/ShoulderPitch.STL -------------------------------------------------------------------------------- /model/meshes/walkman/SoftHandOpen.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/SoftHandOpen.STL -------------------------------------------------------------------------------- /model/meshes/walkman/TopUpperArm.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/TopUpperArm.STL -------------------------------------------------------------------------------- /model/meshes/walkman/TorsoRoll.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/TorsoRoll.STL -------------------------------------------------------------------------------- /model/meshes/walkman/TorsoYawPitch.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/TorsoYawPitch.STL -------------------------------------------------------------------------------- /model/meshes/walkman/Waist.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/Waist.STL -------------------------------------------------------------------------------- /model/meshes/walkman/base.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/base.STL -------------------------------------------------------------------------------- /model/meshes/walkman/hokuyo_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/hokuyo_link.STL -------------------------------------------------------------------------------- /model/meshes/walkman/multisense.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/multisense.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/AnklePitch.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/AnklePitch.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/AnklePitch.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /model/meshes/walkman/simple/BottomUpperArm.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/BottomUpperArm.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/BottomUpperArm.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /model/meshes/walkman/simple/Chest.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/Chest.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/Elbow.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/Elbow.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/Elbow.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /model/meshes/walkman/simple/Foot.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/Foot.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/Forearm.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/Forearm.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/Forearm.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /model/meshes/walkman/simple/ForearmPitch.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/ForearmPitch.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/ForearmPitch.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /model/meshes/walkman/simple/ForearmRoll.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/ForearmRoll.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/ForearmRoll.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /model/meshes/walkman/simple/Head.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/Head.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/Head.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /model/meshes/walkman/simple/HipPitch.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/HipPitch.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/HipPitch.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /model/meshes/walkman/simple/HipRoll.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/HipRoll.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/HipRoll.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /model/meshes/walkman/simple/HipYaw.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/HipYaw.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/HipYaw.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /model/meshes/walkman/simple/KneePitch.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/KneePitch.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/KneePitchCalf.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/KneePitchCalf.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/KneePitchCalf.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /model/meshes/walkman/simple/NeckPitch.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/NeckPitch.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/NeckPitch.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /model/meshes/walkman/simple/NeckYaw.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/NeckYaw.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/NeckYaw.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /model/meshes/walkman/simple/NewChest.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/NewChest.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/NewChest.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /model/meshes/walkman/simple/PelvisWeightBench.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/PelvisWeightBench.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/PelvisWeightBench.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /model/meshes/walkman/simple/ProtectionSystem.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/ProtectionSystem.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/ProtectionSystem.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /model/meshes/walkman/simple/RaisingSupport.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/RaisingSupport.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/ShoulderPitch.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/ShoulderPitch.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/ShoulderPitch.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /model/meshes/walkman/simple/SoftHandOpen.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/SoftHandOpen.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/SoftHandOpen.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /model/meshes/walkman/simple/TopUpperArm.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/TopUpperArm.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/TopUpperArm.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /model/meshes/walkman/simple/TorsoRoll.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/TorsoRoll.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/TorsoRoll.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /model/meshes/walkman/simple/TorsoYawPitch.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/TorsoYawPitch.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/TorsoYawPitch.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /model/meshes/walkman/simple/Waist.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/Waist.STL -------------------------------------------------------------------------------- /model/meshes/walkman/simple/Waist.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /model/meshes/walkman/simple/Waist_old.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/meshes/walkman/simple/Waist_old.STL -------------------------------------------------------------------------------- /model/threeLinks.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | -------------------------------------------------------------------------------- /model/threeLinks_regressor.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | joint_1_2 6 | joint_2_3 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /model/walkman_left_arm_regressor.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | LShSag 5 | LShLat 6 | LShYaw 7 | LElbj 8 | LForearmPlate 9 | LWrj1 10 | LWrj2 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /model/walkman_regressor.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | LHipLat 8 | LHipYaw 9 | LHipSag 10 | LKneeSag 11 | LAnkSag 12 | LAnkLat 13 | 14 | 15 | RHipLat 16 | RHipYaw 17 | RHipSag 18 | RKneeSag 19 | RAnkSag 20 | RAnkLat 21 | 22 | 23 | WaistLat 24 | WaistSag 25 | WaistYaw 26 | 27 | 28 | LShSag 29 | LShLat 30 | LShYaw 31 | LElbj 32 | LForearmPlate 33 | LWrj1 34 | LWrj2 35 | 36 | 37 | 39 | 40 | 41 | RShSag 42 | RShLat 43 | RShYaw 44 | RElbj 45 | RForearmPlate 46 | RWrj1 47 | RWrj2 48 | 49 | 50 | 57 | 58 | -------------------------------------------------------------------------------- /model/walkman_regressor_fixed.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | LHipLat 6 | LHipYaw 7 | LHipSag 8 | LKneeSag 9 | LAnkSag 10 | LAnkLat 11 | 12 | 13 | RHipLat 14 | RHipYaw 15 | RHipSag 16 | RKneeSag 17 | RAnkSag 18 | RAnkLat 19 | 20 | 21 | WaistLat 22 | WaistSag 23 | WaistYaw 24 | 25 | 26 | LShSag 27 | LShLat 28 | LShYaw 29 | LElbj 30 | LForearmPlate 31 | LWrj1 32 | LWrj2 33 | 34 | 35 | 37 | 38 | 39 | RShSag 40 | RShLat 41 | RShYaw 42 | RElbj 43 | RForearmPlate 44 | RWrj1 45 | RWrj2 46 | 47 | 48 | 55 | 56 | -------------------------------------------------------------------------------- /model/walkman_weights.ods: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kjyv/FloBaRoID/67c89fd5b25dfd660f619ac3ba3b72f18ae6aee4/model/walkman_weights.ods -------------------------------------------------------------------------------- /model/world_kuka.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /model/world_walkman_fixed.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /output/css/main.css: -------------------------------------------------------------------------------- 1 | /*! HTML5 Boilerplate v5.0 | MIT License | http://h5bp.com/ */ 2 | 3 | html { 4 | color: #222; 5 | font-size: 1em; 6 | line-height: 1.4; 7 | } 8 | 9 | ::-moz-selection { 10 | background: #b3d4fc; 11 | text-shadow: none; 12 | } 13 | 14 | ::selection { 15 | background: #b3d4fc; 16 | text-shadow: none; 17 | } 18 | 19 | hr { 20 | display: block; 21 | height: 1px; 22 | border: 0; 23 | border-top: 1px solid #ccc; 24 | margin: 1em 0; 25 | padding: 0; 26 | } 27 | 28 | audio, 29 | canvas, 30 | iframe, 31 | img, 32 | svg, 33 | video { 34 | vertical-align: middle; 35 | } 36 | 37 | fieldset { 38 | border: 0; 39 | margin: 0; 40 | padding: 0; 41 | } 42 | 43 | textarea { 44 | resize: vertical; 45 | } 46 | 47 | .browserupgrade { 48 | margin: 0.2em 0; 49 | background: #ccc; 50 | color: #000; 51 | padding: 0.2em 0; 52 | } 53 | 54 | 55 | /* ========================================================================== 56 | Author's custom styles 57 | ========================================================================== */ 58 | 59 | 60 | div { 61 | float: left; 62 | } 63 | 64 | div#console { 65 | white-space: pre; 66 | font-size: 0.7em; 67 | font-family: monospace; 68 | } 69 | 70 | g.mpld3-baseaxes text.mpld3-text:first-of-type { 71 | font-size: 15px; 72 | } 73 | g.mpld3-baseaxes text.mpld3-text { 74 | font-size: 10px; 75 | } 76 | 77 | 78 | /* ========================================================================== 79 | Media Queries 80 | ========================================================================== */ 81 | 82 | @media only screen and (min-width: 35em) { 83 | 84 | } 85 | 86 | @media print, 87 | (-o-min-device-pixel-ratio: 5/4), 88 | (-webkit-min-device-pixel-ratio: 1.25), 89 | (min-resolution: 120dpi) { 90 | 91 | } 92 | 93 | /* ========================================================================== 94 | Helper classes 95 | ========================================================================== */ 96 | 97 | .hidden { 98 | display: none !important; 99 | visibility: hidden; 100 | } 101 | 102 | .visuallyhidden { 103 | border: 0; 104 | clip: rect(0 0 0 0); 105 | height: 1px; 106 | margin: -1px; 107 | overflow: hidden; 108 | padding: 0; 109 | position: absolute; 110 | width: 1px; 111 | } 112 | 113 | .visuallyhidden.focusable:active, 114 | .visuallyhidden.focusable:focus { 115 | clip: auto; 116 | height: auto; 117 | margin: 0; 118 | overflow: visible; 119 | position: static; 120 | width: auto; 121 | } 122 | 123 | .invisible { 124 | visibility: hidden; 125 | } 126 | 127 | .clearfix:before, 128 | .clearfix:after { 129 | content: " "; 130 | display: table; 131 | } 132 | 133 | .clearfix:after { 134 | clear: both; 135 | } 136 | 137 | .clearfix { 138 | *zoom: 1; 139 | } 140 | 141 | /* ========================================================================== 142 | Print styles 143 | ========================================================================== */ 144 | 145 | @media print { 146 | *, 147 | *:before, 148 | *:after { 149 | background: transparent !important; 150 | color: #000 !important; 151 | box-shadow: none !important; 152 | text-shadow: none !important; 153 | } 154 | 155 | a, 156 | a:visited { 157 | text-decoration: underline; 158 | } 159 | 160 | a[href]:after { 161 | content: " (" attr(href) ")"; 162 | } 163 | 164 | abbr[title]:after { 165 | content: " (" attr(title) ")"; 166 | } 167 | 168 | a[href^="#"]:after, 169 | a[href^="javascript:"]:after { 170 | content: ""; 171 | } 172 | 173 | pre, 174 | blockquote { 175 | border: 1px solid #999; 176 | page-break-inside: avoid; 177 | } 178 | 179 | thead { 180 | display: table-header-group; 181 | } 182 | 183 | tr, 184 | img { 185 | page-break-inside: avoid; 186 | } 187 | 188 | img { 189 | max-width: 100% !important; 190 | } 191 | 192 | p, 193 | h2, 194 | h3 { 195 | orphans: 3; 196 | widows: 3; 197 | } 198 | 199 | h2, 200 | h3 { 201 | page-break-after: avoid; 202 | } 203 | } 204 | -------------------------------------------------------------------------------- /output/css/normalize.css: -------------------------------------------------------------------------------- 1 | /*! normalize.css v3.0.2 | MIT License | git.io/normalize */ 2 | 3 | /** 4 | * 1. Set default font family to sans-serif. 5 | * 2. Prevent iOS text size adjust after orientation change, without disabling 6 | * user zoom. 7 | */ 8 | 9 | html { 10 | font-family: sans-serif; /* 1 */ 11 | -ms-text-size-adjust: 100%; /* 2 */ 12 | -webkit-text-size-adjust: 100%; /* 2 */ 13 | } 14 | 15 | /** 16 | * Remove default margin. 17 | */ 18 | 19 | body { 20 | margin: 0; 21 | } 22 | 23 | /* HTML5 display definitions 24 | ========================================================================== */ 25 | 26 | /** 27 | * Correct `block` display not defined for any HTML5 element in IE 8/9. 28 | * Correct `block` display not defined for `details` or `summary` in IE 10/11 29 | * and Firefox. 30 | * Correct `block` display not defined for `main` in IE 11. 31 | */ 32 | 33 | article, 34 | aside, 35 | details, 36 | figcaption, 37 | figure, 38 | footer, 39 | header, 40 | hgroup, 41 | main, 42 | menu, 43 | nav, 44 | section, 45 | summary { 46 | display: block; 47 | } 48 | 49 | /** 50 | * 1. Correct `inline-block` display not defined in IE 8/9. 51 | * 2. Normalize vertical alignment of `progress` in Chrome, Firefox, and Opera. 52 | */ 53 | 54 | audio, 55 | canvas, 56 | progress, 57 | video { 58 | display: inline-block; /* 1 */ 59 | vertical-align: baseline; /* 2 */ 60 | } 61 | 62 | /** 63 | * Prevent modern browsers from displaying `audio` without controls. 64 | * Remove excess height in iOS 5 devices. 65 | */ 66 | 67 | audio:not([controls]) { 68 | display: none; 69 | height: 0; 70 | } 71 | 72 | /** 73 | * Address `[hidden]` styling not present in IE 8/9/10. 74 | * Hide the `template` element in IE 8/9/11, Safari, and Firefox < 22. 75 | */ 76 | 77 | [hidden], 78 | template { 79 | display: none; 80 | } 81 | 82 | /* Links 83 | ========================================================================== */ 84 | 85 | /** 86 | * Remove the gray background color from active links in IE 10. 87 | */ 88 | 89 | a { 90 | background-color: transparent; 91 | } 92 | 93 | /** 94 | * Improve readability when focused and also mouse hovered in all browsers. 95 | */ 96 | 97 | a:active, 98 | a:hover { 99 | outline: 0; 100 | } 101 | 102 | /* Text-level semantics 103 | ========================================================================== */ 104 | 105 | /** 106 | * Address styling not present in IE 8/9/10/11, Safari, and Chrome. 107 | */ 108 | 109 | abbr[title] { 110 | border-bottom: 1px dotted; 111 | } 112 | 113 | /** 114 | * Address style set to `bolder` in Firefox 4+, Safari, and Chrome. 115 | */ 116 | 117 | b, 118 | strong { 119 | font-weight: bold; 120 | } 121 | 122 | /** 123 | * Address styling not present in Safari and Chrome. 124 | */ 125 | 126 | dfn { 127 | font-style: italic; 128 | } 129 | 130 | /** 131 | * Address variable `h1` font-size and margin within `section` and `article` 132 | * contexts in Firefox 4+, Safari, and Chrome. 133 | */ 134 | 135 | h1 { 136 | font-size: 2em; 137 | margin: 0.67em 0; 138 | } 139 | 140 | /** 141 | * Address styling not present in IE 8/9. 142 | */ 143 | 144 | mark { 145 | background: #ff0; 146 | color: #000; 147 | } 148 | 149 | /** 150 | * Address inconsistent and variable font size in all browsers. 151 | */ 152 | 153 | small { 154 | font-size: 80%; 155 | } 156 | 157 | /** 158 | * Prevent `sub` and `sup` affecting `line-height` in all browsers. 159 | */ 160 | 161 | sub, 162 | sup { 163 | font-size: 75%; 164 | line-height: 0; 165 | position: relative; 166 | vertical-align: baseline; 167 | } 168 | 169 | sup { 170 | top: -0.5em; 171 | } 172 | 173 | sub { 174 | bottom: -0.25em; 175 | } 176 | 177 | /* Embedded content 178 | ========================================================================== */ 179 | 180 | /** 181 | * Remove border when inside `a` element in IE 8/9/10. 182 | */ 183 | 184 | img { 185 | border: 0; 186 | } 187 | 188 | /** 189 | * Correct overflow not hidden in IE 9/10/11. 190 | */ 191 | 192 | svg:not(:root) { 193 | overflow: hidden; 194 | } 195 | 196 | /* Grouping content 197 | ========================================================================== */ 198 | 199 | /** 200 | * Address margin not present in IE 8/9 and Safari. 201 | */ 202 | 203 | figure { 204 | margin: 1em 40px; 205 | } 206 | 207 | /** 208 | * Address differences between Firefox and other browsers. 209 | */ 210 | 211 | hr { 212 | -moz-box-sizing: content-box; 213 | box-sizing: content-box; 214 | height: 0; 215 | } 216 | 217 | /** 218 | * Contain overflow in all browsers. 219 | */ 220 | 221 | pre { 222 | overflow: auto; 223 | } 224 | 225 | /** 226 | * Address odd `em`-unit font size rendering in all browsers. 227 | */ 228 | 229 | code, 230 | kbd, 231 | pre, 232 | samp { 233 | font-family: monospace, monospace; 234 | font-size: 1em; 235 | } 236 | 237 | /* Forms 238 | ========================================================================== */ 239 | 240 | /** 241 | * Known limitation: by default, Chrome and Safari on OS X allow very limited 242 | * styling of `select`, unless a `border` property is set. 243 | */ 244 | 245 | /** 246 | * 1. Correct color not being inherited. 247 | * Known issue: affects color of disabled elements. 248 | * 2. Correct font properties not being inherited. 249 | * 3. Address margins set differently in Firefox 4+, Safari, and Chrome. 250 | */ 251 | 252 | button, 253 | input, 254 | optgroup, 255 | select, 256 | textarea { 257 | color: inherit; /* 1 */ 258 | font: inherit; /* 2 */ 259 | margin: 0; /* 3 */ 260 | } 261 | 262 | /** 263 | * Address `overflow` set to `hidden` in IE 8/9/10/11. 264 | */ 265 | 266 | button { 267 | overflow: visible; 268 | } 269 | 270 | /** 271 | * Address inconsistent `text-transform` inheritance for `button` and `select`. 272 | * All other form control elements do not inherit `text-transform` values. 273 | * Correct `button` style inheritance in Firefox, IE 8/9/10/11, and Opera. 274 | * Correct `select` style inheritance in Firefox. 275 | */ 276 | 277 | button, 278 | select { 279 | text-transform: none; 280 | } 281 | 282 | /** 283 | * 1. Avoid the WebKit bug in Android 4.0.* where (2) destroys native `audio` 284 | * and `video` controls. 285 | * 2. Correct inability to style clickable `input` types in iOS. 286 | * 3. Improve usability and consistency of cursor style between image-type 287 | * `input` and others. 288 | */ 289 | 290 | button, 291 | html input[type="button"], /* 1 */ 292 | input[type="reset"], 293 | input[type="submit"] { 294 | -webkit-appearance: button; /* 2 */ 295 | cursor: pointer; /* 3 */ 296 | } 297 | 298 | /** 299 | * Re-set default cursor for disabled elements. 300 | */ 301 | 302 | button[disabled], 303 | html input[disabled] { 304 | cursor: default; 305 | } 306 | 307 | /** 308 | * Remove inner padding and border in Firefox 4+. 309 | */ 310 | 311 | button::-moz-focus-inner, 312 | input::-moz-focus-inner { 313 | border: 0; 314 | padding: 0; 315 | } 316 | 317 | /** 318 | * Address Firefox 4+ setting `line-height` on `input` using `!important` in 319 | * the UA stylesheet. 320 | */ 321 | 322 | input { 323 | line-height: normal; 324 | } 325 | 326 | /** 327 | * It's recommended that you don't attempt to style these elements. 328 | * Firefox's implementation doesn't respect box-sizing, padding, or width. 329 | * 330 | * 1. Address box sizing set to `content-box` in IE 8/9/10. 331 | * 2. Remove excess padding in IE 8/9/10. 332 | */ 333 | 334 | input[type="checkbox"], 335 | input[type="radio"] { 336 | box-sizing: border-box; /* 1 */ 337 | padding: 0; /* 2 */ 338 | } 339 | 340 | /** 341 | * Fix the cursor style for Chrome's increment/decrement buttons. For certain 342 | * `font-size` values of the `input`, it causes the cursor style of the 343 | * decrement button to change from `default` to `text`. 344 | */ 345 | 346 | input[type="number"]::-webkit-inner-spin-button, 347 | input[type="number"]::-webkit-outer-spin-button { 348 | height: auto; 349 | } 350 | 351 | /** 352 | * 1. Address `appearance` set to `searchfield` in Safari and Chrome. 353 | * 2. Address `box-sizing` set to `border-box` in Safari and Chrome 354 | * (include `-moz` to future-proof). 355 | */ 356 | 357 | input[type="search"] { 358 | -webkit-appearance: textfield; /* 1 */ 359 | -moz-box-sizing: content-box; 360 | -webkit-box-sizing: content-box; /* 2 */ 361 | box-sizing: content-box; 362 | } 363 | 364 | /** 365 | * Remove inner padding and search cancel button in Safari and Chrome on OS X. 366 | * Safari (but not Chrome) clips the cancel button when the search input has 367 | * padding (and `textfield` appearance). 368 | */ 369 | 370 | input[type="search"]::-webkit-search-cancel-button, 371 | input[type="search"]::-webkit-search-decoration { 372 | -webkit-appearance: none; 373 | } 374 | 375 | /** 376 | * Define consistent border, margin, and padding. 377 | */ 378 | 379 | fieldset { 380 | border: 1px solid #c0c0c0; 381 | margin: 0 2px; 382 | padding: 0.35em 0.625em 0.75em; 383 | } 384 | 385 | /** 386 | * 1. Correct `color` not being inherited in IE 8/9/10/11. 387 | * 2. Remove padding so people aren't caught out if they zero out fieldsets. 388 | */ 389 | 390 | legend { 391 | border: 0; /* 1 */ 392 | padding: 0; /* 2 */ 393 | } 394 | 395 | /** 396 | * Remove default vertical scrollbar in IE 8/9/10/11. 397 | */ 398 | 399 | textarea { 400 | overflow: auto; 401 | } 402 | 403 | /** 404 | * Don't inherit the `font-weight` (applied by a rule above). 405 | * NOTE: the default cannot safely be changed in Chrome and Safari on OS X. 406 | */ 407 | 408 | optgroup { 409 | font-weight: bold; 410 | } 411 | 412 | /* Tables 413 | ========================================================================== */ 414 | 415 | /** 416 | * Remove most spacing between table cells. 417 | */ 418 | 419 | table { 420 | border-collapse: collapse; 421 | border-spacing: 0; 422 | } 423 | 424 | td, 425 | th { 426 | padding: 0; 427 | } 428 | -------------------------------------------------------------------------------- /output/css/normalize.min.css: -------------------------------------------------------------------------------- 1 | /*! normalize.css v3.0.2 | MIT License | git.io/normalize */html{font-family:sans-serif;-ms-text-size-adjust:100%;-webkit-text-size-adjust:100%}body{margin:0}article,aside,details,figcaption,figure,footer,header,hgroup,main,menu,nav,section,summary{display:block}audio,canvas,progress,video{display:inline-block;vertical-align:baseline}audio:not([controls]){display:none;height:0}[hidden],template{display:none}a{background-color:transparent}a:active,a:hover{outline:0}abbr[title]{border-bottom:1px dotted}b,strong{font-weight:700}dfn{font-style:italic}h1{font-size:2em;margin:.67em 0}mark{background:#ff0;color:#000}small{font-size:80%}sub,sup{font-size:75%;line-height:0;position:relative;vertical-align:baseline}sup{top:-.5em}sub{bottom:-.25em}img{border:0}svg:not(:root){overflow:hidden}figure{margin:1em 40px}hr{-moz-box-sizing:content-box;box-sizing:content-box;height:0}pre{overflow:auto}code,kbd,pre,samp{font-family:monospace,monospace;font-size:1em}button,input,optgroup,select,textarea{color:inherit;font:inherit;margin:0}button{overflow:visible}button,select{text-transform:none}button,html input[type=button],input[type=reset],input[type=submit]{-webkit-appearance:button;cursor:pointer}button[disabled],html input[disabled]{cursor:default}button::-moz-focus-inner,input::-moz-focus-inner{border:0;padding:0}input{line-height:normal}input[type=checkbox],input[type=radio]{box-sizing:border-box;padding:0}input[type=number]::-webkit-inner-spin-button,input[type=number]::-webkit-outer-spin-button{height:auto}input[type=search]{-webkit-appearance:textfield;-moz-box-sizing:content-box;-webkit-box-sizing:content-box;box-sizing:content-box}input[type=search]::-webkit-search-cancel-button,input[type=search]::-webkit-search-decoration{-webkit-appearance:none}fieldset{border:1px solid silver;margin:0 2px;padding:.35em .625em .75em}legend{border:0;padding:0}textarea{overflow:auto}optgroup{font-weight:700}table{border-collapse:collapse;border-spacing:0}td,th{padding:0} -------------------------------------------------------------------------------- /output/js/ansi_up.js: -------------------------------------------------------------------------------- 1 | // ansi_up.js 2 | // version : 1.3.0 3 | // author : Dru Nelson 4 | // license : MIT 5 | // http://github.com/drudru/ansi_up 6 | 7 | (function (Date, undefined) { 8 | 9 | var ansi_up, 10 | VERSION = "1.3.0", 11 | 12 | // check for nodeJS 13 | hasModule = (typeof module !== 'undefined'), 14 | 15 | // Normal and then Bright 16 | ANSI_COLORS = [ 17 | [ 18 | { color: "0, 0, 0", 'class': "ansi-black" }, 19 | { color: "187, 0, 0", 'class': "ansi-red" }, 20 | { color: "0, 187, 0", 'class': "ansi-green" }, 21 | { color: "187, 187, 0", 'class': "ansi-yellow" }, 22 | { color: "0, 0, 187", 'class': "ansi-blue" }, 23 | { color: "187, 0, 187", 'class': "ansi-magenta" }, 24 | { color: "0, 187, 187", 'class': "ansi-cyan" }, 25 | { color: "255,255,255", 'class': "ansi-white" } 26 | ], 27 | [ 28 | { color: "85, 85, 85", 'class': "ansi-bright-black" }, 29 | { color: "255, 85, 85", 'class': "ansi-bright-red" }, 30 | { color: "0, 255, 0", 'class': "ansi-bright-green" }, 31 | { color: "255, 255, 85", 'class': "ansi-bright-yellow" }, 32 | { color: "85, 85, 255", 'class': "ansi-bright-blue" }, 33 | { color: "255, 85, 255", 'class': "ansi-bright-magenta" }, 34 | { color: "85, 255, 255", 'class': "ansi-bright-cyan" }, 35 | { color: "255, 255, 255", 'class': "ansi-bright-white" } 36 | ] 37 | ], 38 | 39 | // 256 Colors Palette 40 | PALETTE_COLORS; 41 | 42 | function Ansi_Up() { 43 | this.fg = this.bg = this.fg_truecolor = this.bg_truecolor = null; 44 | this.bright = 0; 45 | } 46 | 47 | Ansi_Up.prototype.setup_palette = function() { 48 | PALETTE_COLORS = []; 49 | // Index 0..15 : System color 50 | (function() { 51 | var i, j; 52 | for (i = 0; i < 2; ++i) { 53 | for (j = 0; j < 8; ++j) { 54 | PALETTE_COLORS.push(ANSI_COLORS[i][j]['color']); 55 | } 56 | } 57 | })(); 58 | 59 | // Index 16..231 : RGB 6x6x6 60 | // https://gist.github.com/jasonm23/2868981#file-xterm-256color-yaml 61 | (function() { 62 | var levels = [0, 95, 135, 175, 215, 255]; 63 | var format = function (r, g, b) { return levels[r] + ', ' + levels[g] + ', ' + levels[b] }; 64 | var r, g, b; 65 | for (r = 0; r < 6; ++r) { 66 | for (g = 0; g < 6; ++g) { 67 | for (b = 0; b < 6; ++b) { 68 | PALETTE_COLORS.push(format.call(this, r, g, b)); 69 | } 70 | } 71 | } 72 | })(); 73 | 74 | // Index 232..255 : Grayscale 75 | (function() { 76 | var level = 8; 77 | var format = function(level) { return level + ', ' + level + ', ' + level }; 78 | var i; 79 | for (i = 0; i < 24; ++i, level += 10) { 80 | PALETTE_COLORS.push(format.call(this, level)); 81 | } 82 | })(); 83 | }; 84 | 85 | Ansi_Up.prototype.escape_for_html = function (txt) { 86 | return txt.replace(/[&<>]/gm, function(str) { 87 | if (str == "&") return "&"; 88 | if (str == "<") return "<"; 89 | if (str == ">") return ">"; 90 | }); 91 | }; 92 | 93 | Ansi_Up.prototype.linkify = function (txt) { 94 | return txt.replace(/(https?:\/\/[^\s]+)/gm, function(str) { 95 | return "" + str + ""; 96 | }); 97 | }; 98 | 99 | Ansi_Up.prototype.ansi_to_html = function (txt, options) { 100 | return this.process(txt, options, true); 101 | }; 102 | 103 | Ansi_Up.prototype.ansi_to_text = function (txt) { 104 | var options = {}; 105 | return this.process(txt, options, false); 106 | }; 107 | 108 | Ansi_Up.prototype.process = function (txt, options, markup) { 109 | var self = this; 110 | var raw_text_chunks = txt.split(/\033\[/); 111 | var first_chunk = raw_text_chunks.shift(); // the first chunk is not the result of the split 112 | 113 | var color_chunks = raw_text_chunks.map(function (chunk) { 114 | return self.process_chunk(chunk, options, markup); 115 | }); 116 | 117 | color_chunks.unshift(first_chunk); 118 | 119 | return color_chunks.join(''); 120 | }; 121 | 122 | Ansi_Up.prototype.process_chunk = function (text, options, markup) { 123 | 124 | // Are we using classes or styles? 125 | options = typeof options == 'undefined' ? {} : options; 126 | var use_classes = typeof options.use_classes != 'undefined' && options.use_classes; 127 | var key = use_classes ? 'class' : 'color'; 128 | 129 | // Each 'chunk' is the text after the CSI (ESC + '[') and before the next CSI/EOF. 130 | // 131 | // This regex matches four groups within a chunk. 132 | // 133 | // The first and third groups match code type. 134 | // We supported only SGR command. It has empty first group and 'm' in third. 135 | // 136 | // The second group matches all of the number+semicolon command sequences 137 | // before the 'm' (or other trailing) character. 138 | // These are the graphics or SGR commands. 139 | // 140 | // The last group is the text (including newlines) that is colored by 141 | // the other group's commands. 142 | var matches = text.match(/^([!\x3c-\x3f]*)([\d;]*)([\x20-\x2c]*[\x40-\x7e])([\s\S]*)/m); 143 | 144 | if (!matches) return text; 145 | 146 | var orig_txt = matches[4]; 147 | var nums = matches[2].split(';'); 148 | 149 | // We currently support only "SGR" (Select Graphic Rendition) 150 | // Simply ignore if not a SGR command. 151 | if (matches[1] !== '' || matches[3] !== 'm') { 152 | return orig_txt; 153 | } 154 | 155 | if (!markup) { 156 | return orig_txt; 157 | } 158 | 159 | var self = this; 160 | 161 | while (nums.length > 0) { 162 | var num_str = nums.shift(); 163 | var num = parseInt(num_str); 164 | 165 | if (isNaN(num) || num === 0) { 166 | self.fg = self.bg = null; 167 | self.bright = 0; 168 | } else if (num === 1) { 169 | self.bright = 1; 170 | } else if (num == 39) { 171 | self.fg = null; 172 | } else if (num == 49) { 173 | self.bg = null; 174 | } else if ((num >= 30) && (num < 38)) { 175 | self.fg = ANSI_COLORS[self.bright][(num % 10)][key]; 176 | } else if ((num >= 90) && (num < 98)) { 177 | self.fg = ANSI_COLORS[1][(num % 10)][key]; 178 | } else if ((num >= 40) && (num < 48)) { 179 | self.bg = ANSI_COLORS[0][(num % 10)][key]; 180 | } else if ((num >= 100) && (num < 108)) { 181 | self.bg = ANSI_COLORS[1][(num % 10)][key]; 182 | } else if (num === 38 || num === 48) { // extend color (38=fg, 48=bg) 183 | (function() { 184 | var is_foreground = (num === 38); 185 | if (nums.length >= 1) { 186 | var mode = nums.shift(); 187 | if (mode === '5' && nums.length >= 1) { // palette color 188 | var palette_index = parseInt(nums.shift()); 189 | if (palette_index >= 0 && palette_index <= 255) { 190 | if (!use_classes) { 191 | if (!PALETTE_COLORS) { 192 | self.setup_palette.call(self); 193 | } 194 | if (is_foreground) { 195 | self.fg = PALETTE_COLORS[palette_index]; 196 | } else { 197 | self.bg = PALETTE_COLORS[palette_index]; 198 | } 199 | } else { 200 | var klass = (palette_index >= 16) 201 | ? ('ansi-palette-' + palette_index) 202 | : ANSI_COLORS[palette_index > 7 ? 1 : 0][palette_index % 8]['class']; 203 | if (is_foreground) { 204 | self.fg = klass; 205 | } else { 206 | self.bg = klass; 207 | } 208 | } 209 | } 210 | } else if(mode === '2' && nums.length >= 3) { // true color 211 | var r = parseInt(nums.shift()); 212 | var g = parseInt(nums.shift()); 213 | var b = parseInt(nums.shift()); 214 | if ((r >= 0 && r <= 255) && (g >= 0 && g <= 255) && (b >= 0 && b <= 255)) { 215 | var color = r + ', ' + g + ', ' + b; 216 | if (!use_classes) { 217 | if (is_foreground) { 218 | self.fg = color; 219 | } else { 220 | self.bg = color; 221 | } 222 | } else { 223 | if (is_foreground) { 224 | self.fg = 'ansi-truecolor'; 225 | self.fg_truecolor = color; 226 | } else { 227 | self.bg = 'ansi-truecolor'; 228 | self.bg_truecolor = color; 229 | } 230 | } 231 | } 232 | } 233 | } 234 | })(); 235 | } 236 | } 237 | 238 | if ((self.fg === null) && (self.bg === null)) { 239 | return orig_txt; 240 | } else { 241 | var styles = []; 242 | var classes = []; 243 | var data = {}; 244 | var render_data = function (data) { 245 | var fragments = []; 246 | var key; 247 | for (key in data) { 248 | if (data.hasOwnProperty(key)) { 249 | fragments.push('data-' + key + '="' + this.escape_for_html(data[key]) + '"'); 250 | } 251 | } 252 | return fragments.length > 0 ? ' ' + fragments.join(' ') : ''; 253 | }; 254 | 255 | if (self.fg) { 256 | if (use_classes) { 257 | classes.push(self.fg + "-fg"); 258 | if (self.fg_truecolor !== null) { 259 | data['ansi-truecolor-fg'] = self.fg_truecolor; 260 | self.fg_truecolor = null; 261 | } 262 | } else { 263 | styles.push("color:rgb(" + self.fg + ")"); 264 | } 265 | } 266 | if (self.bg) { 267 | if (use_classes) { 268 | classes.push(self.bg + "-bg"); 269 | if (self.bg_truecolor !== null) { 270 | data['ansi-truecolor-bg'] = self.bg_truecolor; 271 | self.bg_truecolor = null; 272 | } 273 | } else { 274 | styles.push("background-color:rgb(" + self.bg + ")"); 275 | } 276 | } 277 | if (use_classes) { 278 | return '' + orig_txt + ''; 279 | } else { 280 | return '' + orig_txt + ''; 281 | } 282 | } 283 | }; 284 | 285 | // Module exports 286 | ansi_up = { 287 | 288 | escape_for_html: function (txt) { 289 | var a2h = new Ansi_Up(); 290 | return a2h.escape_for_html(txt); 291 | }, 292 | 293 | linkify: function (txt) { 294 | var a2h = new Ansi_Up(); 295 | return a2h.linkify(txt); 296 | }, 297 | 298 | ansi_to_html: function (txt, options) { 299 | var a2h = new Ansi_Up(); 300 | return a2h.ansi_to_html(txt, options); 301 | }, 302 | 303 | ansi_to_text: function (txt) { 304 | var a2h = new Ansi_Up(); 305 | return a2h.ansi_to_text(txt); 306 | }, 307 | 308 | ansi_to_html_obj: function () { 309 | return new Ansi_Up(); 310 | } 311 | }; 312 | 313 | // CommonJS module is defined 314 | if (hasModule) { 315 | module.exports = ansi_up; 316 | } 317 | /*global ender:false */ 318 | if (typeof window !== 'undefined' && typeof ender === 'undefined') { 319 | window.ansi_up = ansi_up; 320 | } 321 | /*global define:false */ 322 | if (typeof define === "function" && define.amd) { 323 | define("ansi_up", [], function () { 324 | return ansi_up; 325 | }); 326 | } 327 | })(Date); 328 | -------------------------------------------------------------------------------- /output/templates/index.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Identification results 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | {% for f in figures %} 14 | {{ f }} 15 | {% endfor %} 16 |
17 | {{ text }} 18 |
19 | 20 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | scipy 3 | sympy 4 | pyyaml 5 | trimesh 6 | cvxopt 7 | pylmi-sdp 8 | matplotlib 9 | colorama 10 | palettable 11 | humanize 12 | future 13 | mpld3 14 | jinja2 15 | IPython 16 | tqdm 17 | typing 18 | PyOpenGL 19 | pyglet 20 | matplotlib2tikz 21 | mpi4py 22 | -------------------------------------------------------------------------------- /tests/test_excite.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | #-*- coding: utf-8 -*- 3 | 4 | import subprocess 5 | import os 6 | import sys 7 | 8 | def test_identification(): 9 | path = os.path.join(os.path.dirname(os.path.realpath(__file__)), '..') 10 | try: 11 | output = subprocess.check_output('./examples/excite_kuka_lwr4.sh', cwd=path, shell=True, env=os.environ.copy()) 12 | except subprocess.CalledProcessError as e: 13 | print (e.output) 14 | sys.exit(e.returncode) 15 | 16 | if __name__ == '__main__': 17 | test_identification() 18 | -------------------------------------------------------------------------------- /tests/test_identification_fixed.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | #-*- coding: utf-8 -*- 3 | 4 | import subprocess 5 | import os 6 | import sys 7 | 8 | def test_identification_fixed(): 9 | path = os.path.join(os.path.dirname(os.path.realpath(__file__)), '..') 10 | try: 11 | output = subprocess.check_output('./examples/identify_kuka_lwr4.sh', cwd=path, shell=True, env=os.environ.copy()) 12 | except subprocess.CalledProcessError as e: 13 | print (e.output) 14 | sys.exit(e.returncode) 15 | 16 | if __name__ == '__main__': 17 | test_identification_fixed() 18 | -------------------------------------------------------------------------------- /tests/test_identification_floating.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | #-*- coding: utf-8 -*- 3 | 4 | import subprocess 5 | import os 6 | import sys 7 | 8 | def test_identification_floating(): 9 | path = os.path.join(os.path.dirname(os.path.realpath(__file__)), '..') 10 | try: 11 | output = subprocess.check_output('./examples/identify_threeLink.sh', cwd=path, shell=True, env=os.environ.copy()) 12 | except subprocess.CalledProcessError as e: 13 | print (e.output) 14 | sys.exit(e.returncode) 15 | 16 | if __name__ == '__main__': 17 | test_identification_floating() 18 | -------------------------------------------------------------------------------- /tests/test_regressors.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | #-*- coding: utf-8 -*- 3 | 4 | import numpy as np 5 | import numpy.linalg as la 6 | 7 | # kinematics, dynamics and URDF reading 8 | import iDynTree; iDynTree.init_helpers(); iDynTree.init_numpy_helpers() 9 | from IPython import embed 10 | import matplotlib as mpl 11 | import matplotlib.pyplot as plt 12 | 13 | import os 14 | urdf_file = os.path.join(os.path.dirname(__file__), "../model/threeLinks.urdf") 15 | contactFrame = 'contact_ft' 16 | 17 | def test_regressors(): 18 | #get some random state values and compare inverse dynamics torques with torques 19 | #obtained with regressor and parameter vector 20 | 21 | idyn_model = iDynTree.Model() 22 | iDynTree.modelFromURDF(urdf_file, idyn_model) 23 | 24 | # create generator instance and load model 25 | generator = iDynTree.DynamicsRegressorGenerator() 26 | generator.loadRobotAndSensorsModelFromFile(urdf_file) 27 | regrXml = ''' 28 | 29 | 30 | 31 | 32 | 33 | ''' 34 | generator.loadRegressorStructureFromString(regrXml) 35 | num_model_params = generator.getNrOfParameters() 36 | num_out = generator.getNrOfOutputs() 37 | n_dofs = generator.getNrOfDegreesOfFreedom() 38 | num_samples = 100 39 | 40 | xStdModel = iDynTree.VectorDynSize(num_model_params) 41 | generator.getModelParameters(xStdModel) 42 | xStdModel = xStdModel.toNumPy() 43 | 44 | gravity_twist = iDynTree.Twist.fromList([0,0,-9.81,0,0,0]) 45 | gravity_acc = iDynTree.SpatialAcc.fromList([0, 0, -9.81, 0, 0, 0]) 46 | 47 | dynComp = iDynTree.DynamicsComputations() 48 | dynComp.loadRobotModelFromFile(urdf_file) 49 | 50 | regressor_stack = np.zeros(shape=((n_dofs+6)*num_samples, num_model_params)) 51 | idyn_torques = np.zeros(shape=((n_dofs+6)*num_samples)) 52 | contactForceSum = np.zeros(shape=((n_dofs+6)*num_samples)) 53 | 54 | for sample_index in range(0, num_samples): 55 | q = iDynTree.VectorDynSize.fromList(((np.random.ranf(n_dofs)*2-1)*np.pi).tolist()) 56 | dq = iDynTree.VectorDynSize.fromList(((np.random.ranf(n_dofs)*2-1)*np.pi).tolist()) 57 | ddq = iDynTree.VectorDynSize.fromList(((np.random.ranf(n_dofs)*2-1)*np.pi).tolist()) 58 | base_vel = np.pi*np.random.rand(6) 59 | base_acc = np.pi*np.random.rand(6) 60 | 61 | #rpy = [0,0,0] 62 | rpy = np.random.ranf(3)*0.1 63 | rot = iDynTree.Rotation.RPY(rpy[0], rpy[1], rpy[2]) 64 | pos = iDynTree.Position.Zero() 65 | world_T_base = iDynTree.Transform(rot, pos).inverse() 66 | 67 | # rotate base vel and acc to world frame 68 | to_world = world_T_base.getRotation().toNumPy() 69 | base_vel[0:3] = to_world.dot(base_vel[0:3]) 70 | base_vel[3:] = to_world.dot(base_vel[3:]) 71 | base_acc[0:3] = to_world.dot(base_acc[0:3]) 72 | base_acc[3:] = to_world.dot(base_acc[3:]) 73 | 74 | base_velocity = iDynTree.Twist.fromList(base_vel) 75 | base_acceleration = iDynTree.Twist.fromList(base_acc) 76 | base_acceleration_acc = iDynTree.ClassicalAcc.fromList(base_acc) 77 | 78 | # regressor 79 | generator.setRobotState(q, dq, ddq, world_T_base, base_velocity, base_acceleration, gravity_twist) 80 | 81 | regressor = iDynTree.MatrixDynSize(num_out, num_model_params) 82 | knownTerms = iDynTree.VectorDynSize(num_out) 83 | if not generator.computeRegressor(regressor, knownTerms): 84 | print("Error during numeric computation of regressor") 85 | 86 | #the base forces are expressed in the base frame for the regressor, so transform them 87 | to_world = np.fromstring(world_T_base.getRotation().toString(), sep=' ').reshape((3,3)) 88 | regressor = regressor.toNumPy() 89 | regressor[0:3, :] = to_world.dot(regressor[0:3, :]) 90 | regressor[3:6, :] = to_world.dot(regressor[3:6, :]) 91 | 92 | row_index = (n_dofs+6)*sample_index # index for current row in stacked regressor matrix 93 | np.copyto(regressor_stack[row_index:row_index+n_dofs+6], regressor) 94 | 95 | # inverse dynamics 96 | #dynComp.setFloatingBase('base_link') 97 | dynComp.setRobotState(q, dq, ddq, world_T_base, base_velocity, base_acceleration_acc, gravity_acc) 98 | torques = iDynTree.VectorDynSize(n_dofs) 99 | baseReactionForce = iDynTree.Wrench() 100 | dynComp.inverseDynamics(torques, baseReactionForce) 101 | 102 | torques = np.concatenate((baseReactionForce.toNumPy(), torques.toNumPy())) 103 | np.copyto(idyn_torques[row_index:row_index+n_dofs+6], torques) 104 | 105 | # contacts 106 | dim = n_dofs + 6 107 | contact = np.array([0, 0, 10, 0, 0, 0]) 108 | jacobian = iDynTree.MatrixDynSize(6, dim) 109 | dynComp.getFrameJacobian(contactFrame, jacobian) 110 | jacobian = jacobian.toNumPy() 111 | contactForceSum[sample_index*dim:(sample_index+1)*dim] = jacobian.T.dot(contact) 112 | 113 | regressor_torques = np.dot(regressor_stack, xStdModel) + contactForceSum 114 | idyn_torques += contactForceSum 115 | 116 | error = np.reshape(regressor_torques - idyn_torques, (num_samples, dim)) 117 | 118 | #plots = plt.plot(range(0, num_samples), error) 119 | #plt.legend(plots, ['f_x', 'f_y', 'f_z', 'm_x', 'm_y', 'm_z', 'j_0']) 120 | #plt.show() 121 | 122 | error_norm = la.norm(error) 123 | print(error_norm) 124 | assert error_norm <= 0.01 125 | 126 | if __name__ == '__main__': 127 | test_regressors() 128 | -------------------------------------------------------------------------------- /tools/createNoisyURDF.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from builtins import range 4 | import numpy as np 5 | import iDynTree; iDynTree.init_helpers(); iDynTree.init_numpy_helpers() 6 | import identificationHelpers 7 | import argparse 8 | 9 | def main(): 10 | ''' 11 | open a urdf file and add noise to each parameter. Can be used for testing, but noisy params are usually not consistent, so may be of limited use. 12 | ''' 13 | parser = argparse.ArgumentParser(description='Load measurements and URDF model to get inertial parameters.') 14 | parser.add_argument('--urdf_input', required=True, type=str, help='the file to load the robot model from') 15 | parser.add_argument('--urdf_output', required=True, type=str, help='the file to save the noisy robot model to') 16 | parser.add_argument('--noise', required=False, type=float, help='scale of noise (default 0.01)') 17 | parser.set_defaults(noise=0.01) 18 | args = parser.parse_args() 19 | 20 | model = iDynTree.Model() 21 | iDynTree.modelFromURDF(args.urdf_input, model) 22 | link_names = [] 23 | for i in range(0, model.getNrOfLinks()): 24 | link_names.append(model.getLinkName(i)) 25 | n_params = model.getNrOfLinks()*10 26 | 27 | dynComp = iDynTree.DynamicsComputations() 28 | dynComp.loadRobotModelFromFile(args.urdf_input) 29 | xStdModel = iDynTree.VectorDynSize(n_params) 30 | dynComp.getModelDynamicsParameters(xStdModel) 31 | xStdModel = xStdModel.toNumPy() 32 | # percentage noise 33 | #for p in range(0, len(xStdModel)): 34 | # xStdModel[p] += np.random.randn()*args.noise*xStdModel[p] 35 | # additive noise 36 | xStdModel += np.random.randn(n_params)*args.noise 37 | 38 | 39 | helpers = identificationHelpers.IdentificationHelpers(n_params) 40 | helpers.replaceParamsInURDF(input_urdf=args.urdf_input, output_urdf=args.urdf_output, \ 41 | new_params=xStdModel, link_names=link_names) 42 | 43 | if __name__ == '__main__': 44 | main() 45 | -------------------------------------------------------------------------------- /tools/getMaxInertia.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #-*- coding: utf-8 -*- 3 | 4 | ''' more or less specific to walk-man: get maximum inertia matrix values over the 5 | whole motion range ''' 6 | 7 | import sys 8 | import numpy as np 9 | import numpy.linalg as la 10 | from scipy import signal 11 | import scipy.linalg as sla 12 | import sympy 13 | from sympy import symbols 14 | import iDynTree; iDynTree.init_helpers(); iDynTree.init_numpy_helpers() 15 | from IPython import embed 16 | 17 | import os 18 | sys.path.insert(1, os.path.join(sys.path[0], '..')) 19 | from identification import helpers 20 | 21 | if __name__ == '__main__': 22 | #urdf_file = '../model/centauro.urdf' 23 | urdf_file = '../model/walkman_orig.urdf' 24 | dynamics = iDynTree.DynamicsComputations() 25 | dynamics.loadRobotModelFromFile(urdf_file) 26 | dynamics.setFloatingBase('LFoot') 27 | n_dofs = dynamics.getNrOfDegreesOfFreedom() 28 | jointNames = [] 29 | for i in range(n_dofs): 30 | jointNames.append(dynamics.getJointName(i)) 31 | limits = helpers.URDFHelpers.getJointLimits(urdf_file, use_deg=False) 32 | 33 | #for each joint, sweep through all possible joint angles and get mass matrix 34 | q = iDynTree.VectorDynSize(n_dofs) 35 | q.zero() 36 | dq = iDynTree.VectorDynSize(n_dofs) 37 | #dq.fromList([1.0]*n_dofs) 38 | dq.zero() 39 | ddq = iDynTree.VectorDynSize(n_dofs) 40 | ddq.zero() 41 | world_gravity = iDynTree.SpatialAcc.fromList([0, 0, -9.81, 0, 0, 0]) 42 | base_velocity = iDynTree.Twist() 43 | base_velocity.zero() 44 | base_acceleration = iDynTree.ClassicalAcc() 45 | base_acceleration.zero() 46 | rot = iDynTree.Rotation.RPY(0, 0, 0) 47 | pos = iDynTree.Position.Zero() 48 | world_T_base = iDynTree.Transform(rot, pos) 49 | torques = iDynTree.VectorDynSize(n_dofs) 50 | baseReactionForce = iDynTree.Wrench() 51 | 52 | m = iDynTree.MatrixDynSize(n_dofs, n_dofs) 53 | maxima = [0]*n_dofs 54 | minima = [99999]*n_dofs 55 | getMaxima = 1 56 | 57 | for i in range(n_dofs): 58 | for pos in np.arange(limits[jointNames[i]]['lower'], limits[jointNames[i]]['upper'], 0.01): 59 | q.zero() 60 | 61 | q[i] = pos 62 | if getMaxima: 63 | # saggital = pitch, transversal = yaw, lateral = roll 64 | if i == jointNames.index('LHipYaw'): 65 | # lift right leg up 90 deg 66 | q[jointNames.index('RHipYaw')] = np.deg2rad(-90) 67 | q[jointNames.index('RHipSag')] = np.deg2rad(-90) 68 | 69 | elif i == jointNames.index('RHipYaw'): 70 | # lift left leg up 90 deg, turn outside 71 | q[jointNames.index('LHipYaw')] = np.deg2rad(90) 72 | q[jointNames.index('LHipSag')] = np.deg2rad(-90) 73 | 74 | elif i == jointNames.index('WaistSag'): 75 | # lift both arms up 76 | q[jointNames.index('LShSag')] = np.deg2rad(-162) 77 | q[jointNames.index('LShLat')] = np.deg2rad(85) 78 | 79 | q[jointNames.index('RShSag')] = np.deg2rad(-162) 80 | q[jointNames.index('RShLat')] = np.deg2rad(-85) 81 | 82 | elif i == jointNames.index('WaistYaw'): 83 | # lift arms up to 90 deg 84 | q[jointNames.index('LShSag')] = np.deg2rad(25) 85 | q[jointNames.index('LShLat')] = np.deg2rad(85) 86 | 87 | q[jointNames.index('RShSag')] = np.deg2rad(25) 88 | q[jointNames.index('RShLat')] = np.deg2rad(-85) 89 | 90 | # print ("{} {}".format(i, jointNames[i])) 91 | 92 | dynamics.setRobotState(q, dq, ddq, world_T_base, base_velocity, base_acceleration, world_gravity) 93 | dynamics.getFreeFloatingMassMatrix(m) 94 | I = m.toNumPy() 95 | 96 | # subtract inertia that results from floating base link having zero acceleration (it 97 | # should actually react to acceleration of current joint) 98 | I_qq = I[6:, 6:] 99 | I_xx = I[:6, :6] 100 | I_qx = I[6:, :6] 101 | I_xq = I[:6, 6:] 102 | I_eff = I_qq - I_qx.dot(la.inv(I_xx).dot(I_xq)) 103 | i_j = np.diag(I_eff) 104 | 105 | maxima[i] = np.max((i_j[i], maxima[i])) 106 | minima[i] = np.min((i_j[i], minima[i])) 107 | 108 | ''' 109 | # get only gravity vector for q (qdot = qddot = 0) 110 | dynamics.setRobotState(q, dq, ddq, world_T_base, base_velocity, base_acceleration, world_gravity) 111 | dynamics.inverseDynamics(torques, baseReactionForce) 112 | gravity = torques.toNumPy() 113 | 114 | # get mass matrix for q, qddot 115 | ddq[i] = 1 116 | dynamics.setRobotState(q, dq, ddq, world_T_base, base_velocity, base_acceleration, world_gravity) 117 | dynamics.inverseDynamics(torques, baseReactionForce) 118 | massVector = torques.toNumPy() - gravity 119 | ddq.zero() 120 | 121 | maxima[i] = np.max((massVector[i], maxima[i])) 122 | ''' 123 | 124 | if getMaxima: 125 | print("maxima {}".format(dynamics.getFloatingBase())) 126 | for l in map(lambda j: "{}: {}".format(jointNames[j], maxima[j]), range(len(maxima))): 127 | print(l) 128 | else: 129 | print("minima {}".format(dynamics.getFloatingBase())) 130 | for l in map(lambda j: "{}: {}".format(jointNames[j], minima[j]), range(len(minima))): 131 | print(l) 132 | -------------------------------------------------------------------------------- /tools/inspectMeasurements.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #-*- coding: utf-8 -*- 3 | 4 | from __future__ import division 5 | from __future__ import print_function 6 | from builtins import range 7 | import sys 8 | import numpy as np 9 | import matplotlib.pyplot as plt 10 | 11 | import iDynTree; iDynTree.init_helpers(); iDynTree.init_numpy_helpers() 12 | 13 | from IPython import embed 14 | import re 15 | 16 | import argparse 17 | parser = argparse.ArgumentParser(description='Open a previously taken measurements.npz file and drop into ipython') 18 | parser.add_argument('--filename', required=True, type=str, help='the filename to load the measurements from') 19 | parser.add_argument('--model', required=False, type=str, help='the file to load the robot model from') 20 | parser.add_argument('--fb', required=False, type=bool, help='is the model floating base?') 21 | #parser.add_argument('--config', required=True, type=str, help="use options from given config file") 22 | 23 | #parser.add_argument('--plot', help='plot measured data', action='store_true') 24 | #parser.set_defaults(plot=False,) 25 | args = parser.parse_args() 26 | 27 | def mapToJointNames(matrix, row=None): 28 | generator = iDynTree.DynamicsRegressorGenerator() 29 | generator.loadRobotAndSensorsModelFromFile(args.model) 30 | if args.fb: 31 | regrXml = ''' 32 | 33 | 34 | 35 | 36 | 37 | ''' 38 | else: 39 | regrXml = ''' 40 | 41 | 42 | 43 | 44 | ''' 45 | generator.loadRegressorStructureFromString(regrXml) 46 | jointNames = re.sub(r"DOF Index: \d+ Name: ", "", generator.getDescriptionOfDegreesOfFreedom()).split() 47 | 48 | if args.fb: 49 | fb = 6 50 | else: 51 | fb = 0 52 | 53 | if row: 54 | return {jointNames[j-fb]:matrix[row,j] for j in range(matrix.shape[1])} 55 | else: 56 | return {jointNames[j-fb]:matrix[:,j] for j in range(matrix.shape[1])} 57 | 58 | def main(): 59 | data = np.load(args.filename) 60 | print("loaded file into 'data'") 61 | print("data.keys():") 62 | print(data.keys()) 63 | print("enter e.g. mapToJointNames(data['torques'], row=50)") 64 | print("") 65 | embed() 66 | 67 | if __name__ == '__main__': 68 | main() 69 | -------------------------------------------------------------------------------- /tools/paramErrorPlot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #-*- coding: utf-8 -*- 3 | 4 | from __future__ import division 5 | from __future__ import print_function 6 | from __future__ import absolute_import 7 | from builtins import range 8 | import sys 9 | from typing import AnyStr, List 10 | 11 | # math 12 | import numpy as np 13 | import numpy.linalg as la 14 | 15 | # plotting 16 | import matplotlib.pyplot as plt 17 | 18 | # kinematics, dynamics and URDF reading 19 | import iDynTree; iDynTree.init_helpers(); iDynTree.init_numpy_helpers() 20 | 21 | import argparse 22 | parser = argparse.ArgumentParser(description='Scale mass and inertia from .') 23 | parser.add_argument('--ref_model', required=True, type=str, help='the file to load the reference parameters from') 24 | parser.add_argument('--model', required=True, nargs='+', action='append', type=str, help='the file to load the parameters to compare from') 25 | args = parser.parse_args() 26 | 27 | def loadModelfromURDF(urdf_file): 28 | # type: (AnyStr) -> (iDynTree.DynamicsComputations) 29 | dynComp = iDynTree.DynamicsComputations() 30 | dynComp.loadRobotModelFromFile(urdf_file) 31 | 32 | return dynComp 33 | 34 | def matToNumPy(mat): 35 | return np.fromstring(mat.toString(), sep=' ').reshape(mat.rows(), mat.cols()) 36 | 37 | def vecToNumPy(vec): 38 | return np.fromstring(vec.toString(), sep=' ') 39 | 40 | def plotErrors(errors, labels): 41 | fig, ax = plt.subplots() 42 | 43 | num_vals = len(errors[0]) 44 | std_dev = np.zeros(num_vals) 45 | 46 | index = np.arange(num_vals) 47 | bar_width = 1/len(errors) - 0.1 48 | 49 | opacity = 0.4 50 | error_config = {'ecolor': '0.3'} 51 | 52 | colors = ['g', 'r', 'b', 'y'] 53 | 54 | for i in range(len(errors)): 55 | plt.bar(index+bar_width*i, errors[i], bar_width, 56 | alpha=opacity, 57 | color=colors[i], 58 | yerr=std_dev, 59 | error_kw=error_config, 60 | label=labels[i]) 61 | 62 | plt.xlabel('Link Index') 63 | plt.ylabel('Error Norm') 64 | plt.title('Param error by method') 65 | plt.xticks(index + bar_width / 2, linkNames, rotation='vertical') 66 | plt.legend() 67 | 68 | plt.tight_layout() 69 | plt.show() 70 | 71 | def getParamErrors(ref_model, p_model, num_links, group="COM"): 72 | # type: (iDynTree.DynamicsComputations, iDynTree.DynamicsComputations, int, AnyStr) -> (List[float]) 73 | """ give error for groups of params """ 74 | 75 | errors = [] 76 | 77 | for link_id in range(num_links): 78 | p_link_name = linkNames[link_id] 79 | p_link_id = p_model.getLinkIndex(p_link_name) 80 | p_spat_inertia = p_model.getLinkInertia(p_link_id) 81 | p_inertia = matToNumPy(p_spat_inertia.getRotationalInertiaWrtCenterOfMass()) 82 | p_mass = p_spat_inertia.getMass() 83 | p_com = vecToNumPy(p_spat_inertia.getCenterOfMass()) 84 | 85 | ref_link_name = linkNames[link_id] 86 | ref_link_id = ref_model.getLinkIndex(ref_link_name) 87 | ref_spat_inertia = ref_model.getLinkInertia(ref_link_id) 88 | ref_inertia = matToNumPy(ref_spat_inertia.getRotationalInertiaWrtCenterOfMass()) 89 | ref_mass = ref_spat_inertia.getMass() 90 | ref_com = vecToNumPy(ref_spat_inertia.getCenterOfMass()) 91 | 92 | if group == 'COM': 93 | errors.append(ref_com - p_com) 94 | elif group == 'mass': 95 | errors.append(ref_mass - p_mass) 96 | elif group == 'inertia': 97 | errors.append(ref_inertia - p_inertia) 98 | 99 | return errors 100 | 101 | 102 | if __name__ == '__main__': 103 | 104 | ref_model = loadModelfromURDF(args.ref_model) 105 | generator = iDynTree.DynamicsRegressorGenerator() 106 | if not generator.loadRobotAndSensorsModelFromFile(args.ref_model): 107 | sys.exit() 108 | 109 | regrXml = ''' 110 | 111 | 112 | 113 | 114 | ''' 115 | generator.loadRegressorStructureFromString(regrXml) 116 | num_links = generator.getNrOfLinks() - generator.getNrOfFakeLinks() 117 | 118 | linkNames = [] # type: List[AnyStr] 119 | import re 120 | for d in generator.getDescriptionOfParameters().strip().split("\n"): 121 | link = re.findall(r"of link (.*)", d)[0] 122 | if link not in linkNames: 123 | linkNames.append(link) 124 | 125 | methods_com_errors = [] 126 | methods_inertia_errors = [] 127 | 128 | # check input order 129 | print(args.model) 130 | 131 | for filename in args.model: 132 | p_model = loadModelfromURDF(filename[0]) 133 | 134 | # mass error norm 135 | mass_errors = la.norm(getParamErrors(ref_model, p_model, num_links, group='mass')) 136 | 137 | # com error norm 138 | com_errors = la.norm(getParamErrors(ref_model, p_model, num_links, group='COM'), axis=1) 139 | 140 | # inertia error norm 141 | inertia_error_tensors = getParamErrors(ref_model, p_model, num_links, group='inertia') 142 | inertia_errors = [] 143 | for i in range(len(inertia_error_tensors)): 144 | inertia_errors.append(la.norm(inertia_error_tensors[i])) 145 | 146 | #methods_mass_errors.append(mass_errors) 147 | methods_com_errors.append(com_errors) 148 | methods_inertia_errors.append(inertia_errors) 149 | 150 | #labels = ['ID COM (Kown Mass)', "(3) ID Inertia (Known Mass + ID'd COM)", '(2) ID Inertia + COM (Known Mass)', '(1) ID all (20% wrong masses)'] 151 | labels = ['ID Inertia + COM (Known Mass)', 'ID all parameters (20% wrong masses)'] 152 | plotErrors(methods_com_errors, labels=labels) 153 | plotErrors(methods_inertia_errors, labels=labels) 154 | 155 | -------------------------------------------------------------------------------- /tools/scaleInertia.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #-*- coding: utf-8 -*- 3 | 4 | import numpy as np 5 | import re 6 | 7 | if __name__ == '__main__': 8 | line = '' 9 | 10 | scaling = 1.95833333333333 11 | nums = np.array(re.findall(r'[-+]?\d*\.\d+|\d+', line)).astype(float) 12 | nums *= scaling 13 | nums = nums.astype(str) 14 | 15 | pieces = [] 16 | old_end = 0 17 | for m in re.finditer(r'[-+]?\d*\.\d+|\d+', line): 18 | pieces.append(line[old_end:m.start()]) 19 | old_end = m.end() 20 | 21 | new_line = "" 22 | i = 0 23 | for p in pieces: 24 | new_line += p + nums[i] 25 | i += 1 26 | new_line += line[old_end:] 27 | print(new_line) 28 | -------------------------------------------------------------------------------- /tools/scaleInertia2.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #-*- coding: utf-8 -*- 3 | 4 | ''' 5 | open urdf, scale all masses and inertia with given value, save to new file 6 | ''' 7 | 8 | import re 9 | from typing import AnyStr 10 | import numpy as np 11 | 12 | import iDynTree; iDynTree.init_helpers(); iDynTree.init_numpy_helpers() 13 | 14 | import argparse 15 | parser = argparse.ArgumentParser(description='Scale mass and inertia from .') 16 | parser.add_argument('--model', required=True, type=str, help='the file to load the robot model from') 17 | parser.add_argument('--output', required=True, type=str, help='the file to write the changed model to') 18 | parser.add_argument('--scale', required=True, type=float, help='the value to scale with') 19 | args = parser.parse_args() 20 | 21 | def loadModel(urdf_file): 22 | # type: (AnyStr) -> (iDynTree.DynamicsComputations) 23 | dynComp = iDynTree.DynamicsComputations() 24 | dynComp.loadRobotModelFromFile(urdf_file) 25 | 26 | return dynComp 27 | 28 | def toNumPy(mat): 29 | return np.fromstring(mat.toString(), sep=' ').reshape(mat.rows(), mat.cols()) 30 | 31 | if __name__ == '__main__': 32 | 33 | scaling = args.scale 34 | 35 | dynComp = loadModel(args.model) 36 | 37 | import xml.etree.ElementTree as ET 38 | # preserve comments 39 | class PCBuilder(ET.TreeBuilder): 40 | def comment(self, data): 41 | self.start(ET.Comment, {}) 42 | self.data(data) 43 | self.end(ET.Comment) 44 | tree = ET.parse(args.model, parser=ET.XMLParser(target=PCBuilder())) 45 | 46 | for link_id in range(dynComp.getNrOfLinks()): 47 | link_name = dynComp.getFrameName(link_id) # not really clean (when are there other frames than link frames, sensors?) 48 | link_id = dynComp.getLinkIndex(link_name) # so, make double sure 49 | spat_inertia = dynComp.getLinkInertia(link_id) 50 | 51 | inertia = toNumPy(spat_inertia.getRotationalInertiaWrtCenterOfMass()) 52 | mass = spat_inertia.getMass() 53 | 54 | mass_line = '' 55 | inertia_line = '' 56 | 57 | if mass > 0: 58 | print("link {} {}".format(link_id, link_name)) 59 | print(mass_line.format(mass)) 60 | print(inertia_line.format(inertia[0,0], inertia[0,1], inertia[0,2], 61 | inertia[1,1], inertia[1,2], 62 | inertia[2,2])) 63 | print("") 64 | 65 | inertia *= scaling 66 | mass *= scaling 67 | 68 | if mass > 0: 69 | print(mass_line.format(mass)) 70 | print(inertia_line.format(inertia[0,0], inertia[0,1], inertia[0,2], 71 | inertia[1,1], inertia[1,2], 72 | inertia[2,2])) 73 | 74 | for l in tree.findall('link'): 75 | if l.attrib['name'] == link_name: 76 | try: 77 | l.find('inertial/mass').attrib['value'] = '{}'.format(mass) 78 | except AttributeError: 79 | continue 80 | inert = l.find('inertial/inertia') 81 | inert.attrib['ixx'] = '{}'.format(inertia[0,0]) 82 | inert.attrib['ixy'] = '{}'.format(inertia[0,1]) 83 | inert.attrib['ixz'] = '{}'.format(inertia[0,2]) 84 | inert.attrib['iyy'] = '{}'.format(inertia[1,1]) 85 | inert.attrib['iyz'] = '{}'.format(inertia[1,2]) 86 | inert.attrib['izz'] = '{}'.format(inertia[2,2]) 87 | 88 | tree.write(args.output, xml_declaration=True) 89 | -------------------------------------------------------------------------------- /tools/start_gazebo_kuka_lwr4.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | #roslaunch single_lwr_launch single_lwr.launch load_moveit:=true 4 | roslaunch single_lwr_launch single_lwr.launch load_moveit:=true use_lwr_sim:=false lwr_powered:=true 5 | -------------------------------------------------------------------------------- /tools/start_gazebo_kuka_lwr4_sim.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | roslaunch single_lwr_launch single_lwr.launch load_moveit:=true 4 | -------------------------------------------------------------------------------- /trajectory.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #-*- coding: utf-8 -*- 3 | 4 | from __future__ import division 5 | from __future__ import print_function 6 | from builtins import range 7 | import sys 8 | 9 | import numpy as np 10 | import iDynTree; iDynTree.init_helpers(); iDynTree.init_numpy_helpers() 11 | 12 | from identify import Identification 13 | from identification.model import Model 14 | from excitation.trajectoryGenerator import PulsedTrajectory 15 | from excitation.trajectoryOptimizer import TrajectoryOptimizer, simulateTrajectory 16 | from excitation.postureOptimizer import PostureOptimizer 17 | 18 | import argparse 19 | parser = argparse.ArgumentParser(description='Generate excitation trajectories, save to .') 20 | parser.add_argument('--filename', type=str, help='the filename to save the trajectory to, otherwise .trajectory.npz') 21 | parser.add_argument('--config', required=True, type=str, help="use options from given config file") 22 | parser.add_argument('--model', required=True, type=str, help='the file to load the robot model from') 23 | parser.add_argument('--model_real', required=False, type=str, help='the file to load the "real" robot model from') 24 | parser.add_argument('--world', required=False, type=str, help='the file to load world links from') 25 | args = parser.parse_args() 26 | 27 | import yaml 28 | with open(args.config, 'r') as stream: 29 | try: 30 | config = yaml.load(stream) 31 | except yaml.YAMLError as exc: 32 | print(exc) 33 | 34 | config['urdf'] = args.model 35 | config['urdf_real'] = args.model_real 36 | if config['useStaticTrajectories'] and not config['urdf_real']: 37 | print('When optimizing static postures, need model_real argument!') 38 | sys.exit() 39 | config['jointNames'] = iDynTree.StringVector([]) 40 | if not iDynTree.dofsListFromURDF(config['urdf'], config['jointNames']): 41 | sys.exit() 42 | config['num_dofs'] = len(config['jointNames']) 43 | config['skipSamples'] = 0 44 | 45 | def main(): 46 | # save either optimized or random trajectory parameters to filename 47 | if args.filename: 48 | traj_file = args.filename 49 | else: 50 | traj_file = config['urdf'] + '.trajectory.npz' 51 | 52 | if config['optimizeTrajectory']: 53 | # find trajectory params by optimization 54 | old_sim = config['simulateTorques'] 55 | config['simulateTorques'] = True 56 | model = Model(config, config['urdf']) 57 | if config['useStaticTrajectories']: 58 | old_gravity = config['identifyGravityParamsOnly'] 59 | idf = Identification(config, config['urdf'], config['urdf_real'], measurements_files=None, 60 | regressor_file=None, validation_file=None) 61 | trajectoryOptimizer = PostureOptimizer(config, idf, model, simulation_func=simulateTrajectory, world=args.world) 62 | config['identifyGravityParamsOnly'] = old_gravity 63 | else: 64 | idf = Identification(config, config['urdf'], urdf_file_real=None, measurements_files=None, 65 | regressor_file=None, validation_file=None) 66 | trajectoryOptimizer = TrajectoryOptimizer(config, idf, model, simulation_func=simulateTrajectory, world=args.world) 67 | 68 | trajectory = trajectoryOptimizer.optimizeTrajectory() 69 | config['simulateTorques'] = old_sim 70 | else: 71 | # use some random params 72 | print("no optimized trajectory found, generating random one") 73 | trajectory = PulsedTrajectory(config['num_dofs'], use_deg=config['useDeg']).initWithRandomParams() 74 | print("a {}".format([t_a.tolist() for t_a in trajectory.a])) 75 | print("b {}".format([t_b.tolist() for t_b in trajectory.b])) 76 | print("q {}".format(trajectory.q.tolist())) 77 | print("nf {}".format(trajectory.nf.tolist())) 78 | print("wf {}".format(trajectory.w_f_global)) 79 | 80 | print("Saving found trajectory to {}".format(traj_file)) 81 | 82 | if config['useStaticTrajectories']: 83 | # always saved with rad angles 84 | np.savez(traj_file, static=True, angles=trajectory.angles) 85 | else: 86 | # TODO: remove degrees option 87 | np.savez(traj_file, use_deg=trajectory.use_deg, static=False, a=trajectory.a, b=trajectory.b, 88 | q=trajectory.q, nf=trajectory.nf, wf=trajectory.w_f_global) 89 | 90 | if __name__ == '__main__': 91 | main() 92 | --------------------------------------------------------------------------------