├── .gitignore ├── CMakeLists.txt ├── COPYING ├── LICENSE ├── Makefile ├── README.md ├── catkin.cmake ├── doc ├── CMakeLists.txt ├── Doxyfile.in ├── RPY.png ├── extract_API_EG.sh ├── generate_doxyfile.sh ├── getRPY.rst └── push_doxyfile.sh ├── examples ├── CMakeLists.txt ├── conversions_example.cpp ├── debug1.cpp ├── expressiontree_cached_named.cpp ├── expressiontree_cb.cpp ├── expressiontree_conditional.cpp ├── expressiontree_distance_to_line.cpp ├── expressiontree_example.cpp ├── expressiontree_example10.cpp ├── expressiontree_example11.cpp ├── expressiontree_example12.cpp ├── expressiontree_example2.cpp ├── expressiontree_example3.cpp ├── expressiontree_example4.cpp ├── expressiontree_example5.cpp ├── expressiontree_example6.cpp ├── expressiontree_example7.cpp ├── expressiontree_example8.cpp ├── expressiontree_example9.cpp ├── expressiontree_mimo_ex.cpp ├── expressiontree_motion_profile.cpp ├── expressiontree_motion_profile2.cpp ├── expressiontree_optimizer.cpp ├── expressiontree_perpendicular.cpp ├── expressiontree_sensor.cpp ├── expressiontree_variabletype.cpp ├── geometry.cpp ├── initial_value.cpp ├── matrix_traits.cpp ├── mptrap_tst.cpp ├── plotframe.m ├── resolved_motion_rate_control.cpp ├── saturate.cpp ├── solving_and_cloning.cpp ├── tutorial1.cpp ├── tutorial2.cpp └── tutorial3.cpp ├── include └── kdl │ ├── conversions.hpp │ ├── expressiontree.hpp │ ├── expressiontree_chain.hpp │ ├── expressiontree_double.hpp │ ├── expressiontree_euler.hpp │ ├── expressiontree_expressions.hpp │ ├── expressiontree_frame.hpp │ ├── expressiontree_matrix.hpp │ ├── expressiontree_mimo.hpp │ ├── expressiontree_motionprofiles.hpp │ ├── expressiontree_n_ary.hpp │ ├── expressiontree_rotation.hpp │ ├── expressiontree_traits.hpp │ ├── expressiontree_twist.hpp │ ├── expressiontree_var.hpp │ ├── expressiontree_vector.hpp │ ├── expressiontree_wrench.hpp │ └── mptrap.hpp ├── license_EUPL_v1_1_en.pdf ├── mainpage.dox ├── manifest.old ├── package.xml ├── preamble.txt ├── rosdoc.yaml ├── src ├── expressiontree_chain.cpp ├── expressiontree_double.cpp ├── expressiontree_expressions.cpp ├── expressiontree_frame.cpp ├── expressiontree_mimo.cpp ├── expressiontree_motionprofiles.cpp ├── expressiontree_motprof.cpp.bak ├── expressiontree_rotation.cpp ├── expressiontree_twist.cpp ├── expressiontree_vector.cpp ├── expressiontree_wrench.cpp ├── mptrap.cpp └── util.hpp └── tests ├── expressiongraph_test.cpp └── expressiongraph_test.hpp /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | install_dir 3 | bin 4 | my_experiments 5 | 6 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # USE THE CATKIN CMAKELISTS FOR BUILDING WITH CATKIN 2 | # ELSE PROCEED WITH ROSBUILD BELOW 3 | if(NOT USE_ROSBUILD) 4 | include(catkin.cmake) 5 | return() 6 | endif() 7 | 8 | cmake_minimum_required(VERSION 2.4.6) 9 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 10 | 11 | # Set the build type. Options are: 12 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 13 | # Debug : w/ debug symbols, w/o optimization 14 | # Release : w/o debug symbols, w/ optimization 15 | # RelWithDebInfo : w/ debug symbols, w/ optimization 16 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 17 | #set(ROS_BUILD_TYPE RelWithDebInfo) 18 | set(ROS_BUILD_TYPE Release) 19 | #set(ROS_BUILD_TYPE Debug) 20 | rosbuild_init() 21 | 22 | #set the default path for built executables to the "bin" directory 23 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 24 | #set the default path for built libraries to the "lib" directory 25 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 26 | 27 | #uncomment if you have defined messages 28 | #rosbuild_genmsg() 29 | #uncomment if you have defined services 30 | #rosbuild_gensrv() 31 | #SET(ENABLE_EXAMPLES ON CACHE BOOL "enable compilation of a series of examples") 32 | OPTION(ENABLE_EXAMPLES "enable compilation of a series of examples" ON) 33 | INCLUDE(${PROJECT_SOURCE_DIR}/examples/CMakeLists.txt) 34 | 35 | FILE( GLOB EXPRESSIONTREE_SRCS src/[^.]*.cpp src/[^.]*.cxx) 36 | 37 | 38 | 39 | 40 | #common commands for building c++ executables and libraries 41 | rosbuild_add_library(${PROJECT_NAME} ${EXPRESSIONTREE_SRCS}) 42 | #target_link_libraries(${PROJECT_NAME} another_library) 43 | #rosbuild_add_boost_directories() 44 | #rosbuild_link_boost(${PROJECT_NAME} thread) 45 | #rosbuild_add_executable(example examples/example.cpp) 46 | #target_link_libraries(example ${PROJECT_NAME}) 47 | 48 | rosbuild_add_gtest( expressiongraph_test ./tests/expressiongraph_test.cpp ) 49 | target_link_libraries(expressiongraph_test ${PROJECT_NAME} orocos-kdl) 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /COPYING: -------------------------------------------------------------------------------- 1 | This software is licensed under EUPL v1.1 (http://ec.europa.eu/idabc/eupl) 2 | 3 | The license text can be found in license_EUPL_v1_1_en.pdf 4 | 5 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | This software is licensed under EUPL v1.1 (http://ec.europa.eu/idabc/eupl) 2 | 3 | The license text can be found in license_EUPL_v1_1_en.pdf 4 | 5 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | /*! \mainpage Expression graph library 2 | 3 | # Expression graph library. 4 | 5 | 6 | This library provides expression trees for representation of geometric expressions and automatic differentiation 7 | of these expressions. This enables to write down geometric expressions at the position level, and automatically 8 | compute Jacobians and higher order derivatives efficiently and without loss of precision. 9 | The library is built upon the KDL library and uses the same geometric primitives. Most operators and functions for 10 | the basic primitives of KDL, have an equivalent in the expression tree library. 11 | 12 | 13 | author: 14 | 15 | 16 | ``` 17 | #!c++ 18 | 19 | Erwin Aertbelien 20 | KU Leuven - Dep. of Mechanical Engineering 21 | Celestijnenlaan 300B - bus 2480 22 | 3001 HEVERLEE 23 | Belgium 24 | 25 | E-mail: dot at kuleuven.be 26 | 27 | ``` 28 | 29 | ### EUPL License ### 30 | 31 | Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 32 | 33 | Licensed under the EUPL, Version 1.1 only (the "Licence"); You may not use this work except in compliance with the Licence. You may obtain a copy of the Licence at: 34 | 35 | [EUPL license](http://ec.europa.eu/idabc/eupl) 36 | 37 | Unless required by applicable law or agreed to in writing, software distributed under the Licence is distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or im plied. 38 | See the Licence for the specific language governing permissions and limitations under the Licence. 39 | 40 | 41 | You can submit patches to this repository to the above original author. 42 | To allow for dual licensing, we will only apply patches for which the authors 43 | have given a release. 44 | 45 | This library is first release to the public at November,25th, 2014, 46 | 47 | ### Libraries used: ### 48 | This library links dynamically with the KDL library (http://wiki.ros.org/kdl). The KDL library is distributed under LGPL. 49 | 50 | ### Contributing authors: ### 51 | 52 | * Erwin Aertbeliën (eaertbel) 53 | * Enea Scioni (haianos) 54 | -------------------------------------------------------------------------------- /catkin.cmake: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(expressiongraph) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | find_package(orocos_kdl REQUIRED) 7 | find_package(cmake_modules REQUIRED) 8 | find_package(Eigen3 REQUIRED) 9 | find_package(Boost REQUIRED COMPONENTS random) 10 | catkin_package( 11 | INCLUDE_DIRS include 12 | LIBRARIES ${PROJECT_NAME} 13 | DEPENDS orocos_kdl EIGEN3 14 | ) 15 | 16 | 17 | include_directories( 18 | include 19 | ${catkin_INCLUDE_DIRS} 20 | ${EIGEN3_INCLUDE_DIRS} 21 | ${orocos_kdl_INCLUDE_DIRS} 22 | ${Boost_INCLUDE_DIRS}) 23 | 24 | # BUILDING AND LINKING LIBRARY 25 | FILE( GLOB EXPRESSIONTREE_SRCS src/[^.]*.cpp src/[^.]*.cxx) 26 | 27 | set(EXPRESSIONTREE_SRCS 28 | src/expressiontree_expressions.cpp 29 | src/expressiontree_motionprofiles.cpp 30 | src/expressiontree_rotation.cpp 31 | src/expressiontree_wrench.cpp 32 | src/expressiontree_chain.cpp 33 | src/expressiontree_frame.cpp 34 | src/expressiontree_twist.cpp 35 | src/mptrap.cpp 36 | src/expressiontree_double.cpp 37 | src/expressiontree_mimo.cpp 38 | src/expressiontree_vector.cpp 39 | ) 40 | 41 | add_library(${PROJECT_NAME} ${EXPRESSIONTREE_SRCS}) 42 | target_link_libraries(${PROJECT_NAME} 43 | ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES} ${EIGEN3_LIBRARIES} ${Boost_LIBRARIES}) 44 | 45 | # BUILDING AND LINKING TESTS 46 | catkin_add_gtest(${PROJECT_NAME}_test tests/expressiongraph_test.cpp) 47 | target_link_libraries(${PROJECT_NAME}_test 48 | ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES} ${PROJECT_NAME} ${EIGEN3_LIBRARIES} ${Boost_LIBRARIES}) 49 | 50 | # POTENTIALLY, BUILDING AND LINKING EXAMPLES 51 | OPTION(ENABLE_EXAMPLES "enable compilation of a series of examples" ON) 52 | INCLUDE(${PROJECT_SOURCE_DIR}/examples/CMakeLists.txt) 53 | 54 | # INSTALLING LIBRARY AND HEADER-FILES 55 | install(TARGETS ${PROJECT_NAME} ${EXAMPLES} 56 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 57 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 58 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 59 | 60 | install(DIRECTORY include/ 61 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 62 | FILES_MATCHING PATTERN "*.hpp" 63 | ) 64 | 65 | -------------------------------------------------------------------------------- /doc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | CONFIGURE_FILE("${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile.in" "${CMAKE_CURRENT_BINARY_DIR}/Doxyfile" IMMEDIATE @ONLY) 3 | ADD_CUSTOM_TARGET(docs "doxygen" "Doxyfile") 4 | INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/kdl.tag DESTINATION share/doc/liborocos-kdl/ OPTIONAL) # only installs if found. 5 | -------------------------------------------------------------------------------- /doc/RPY.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/eaertbel/expressiongraph/46f210e48bf56186a0cdf1fd198ccc20812041a8/doc/RPY.png -------------------------------------------------------------------------------- /doc/extract_API_EG.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | pwd 3 | cd ~/My\ Webspace/expressiontrees 4 | rm -r -f ./API_EG 5 | tar xzf ~/API_EG.tar.gz 6 | mv html API_EG 7 | 8 | -------------------------------------------------------------------------------- /doc/generate_doxyfile.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | rm Doxyfile 3 | sed "s/__PROJECT_NUMBER__/\"`git log --pretty=format:"%ai" -1`\"/g" Doxyfile.in > Doxyfile 4 | doxygen 5 | 6 | 7 | -------------------------------------------------------------------------------- /doc/getRPY.rst: -------------------------------------------------------------------------------- 1 | getRPY 2 | ------ 3 | 4 | Function and expressiongraph node to get the roll, pitch, yaw angles and their derivatives. 5 | 6 | In eTaSL, the expression is constructed as: 7 | 8 | .. code-block:: lua 9 | 10 | roll,pitch, yaw = getRPY( R) 11 | 12 | 13 | where R is an expression for a rotation matrix and roll, pitch and yaw are scalar expressions. 14 | 15 | In the C++ API, you have: 16 | 17 | .. code-block:: c++ 18 | 19 | Expression::Ptr getRPY( Expression::Ptr R ) 20 | 21 | 22 | The roll pitch yaw convention of a rigid boy is defined as follows: you first rotate the rigid body 23 | around the x-axis using the roll angle phi, then around 24 | the y-axis using the pitch angle and then around 25 | the z-axis using the yaw angle: 26 | 27 | .. code-block:: lua 28 | RPY(phi,theta,psi) = Rot(Z,psi) * Rot(Y, theta) * Rot(X, phi) 29 | 30 | 31 | .. figure:: RPY.png 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /doc/push_doxyfile.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | rm Doxyfile 3 | sed "s/__PROJECT_NUMBER__/\"`git log --pretty=format:"%ai" -1`\"/g" Doxyfile.in > Doxyfile 4 | doxygen 5 | echo "... compressing" 6 | cd doxygen 7 | tar czf ../API_EG.tar.gz html 8 | cd .. 9 | echo "... copying to ftp site" 10 | scp API_EG.tar.gz ftp.mech.kuleuven.be:. 11 | echo "... executing server side script" 12 | ssh ftp.mech.kuleuven.be 'bash -s' < extract_API_EG.sh 13 | 14 | 15 | -------------------------------------------------------------------------------- /examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | IF(ENABLE_EXAMPLES) 2 | 3 | set(EXAMPLES 4 | geometry 5 | expressiontree_example 6 | expressiontree_example2 7 | expressiontree_example3 8 | expressiontree_example4 9 | expressiontree_example5 10 | expressiontree_example6 11 | expressiontree_example7 12 | expressiontree_example8 13 | expressiontree_example9 14 | expressiontree_example10 15 | expressiontree_example11 16 | expressiontree_example12 17 | resolved_motion_rate_control 18 | expressiontree_perpendicular 19 | conversions_example 20 | expressiontree_distance_to_line 21 | expressiontree_conditional 22 | expressiontree_cached_named 23 | expressiontree_optimizer 24 | expressiontree_mimo_ex 25 | expressiontree_sensor 26 | expressiontree_motion_profile 27 | expressiontree_motion_profile2 28 | expressiontree_variabletype 29 | tutorial1 30 | tutorial2 31 | tutorial3 32 | saturate 33 | debug1 34 | matrix_traits 35 | initial_value 36 | mptrap_tst 37 | solving_and_cloning 38 | ) 39 | 40 | add_executable(solving_and_cloning examples/solving_and_cloning.cpp ) 41 | TARGET_LINK_LIBRARIES(solving_and_cloning ${PROJECT_NAME} ${Eigen_LIBRARIES}) 42 | 43 | 44 | add_executable(mptrap_tst examples/mptrap_tst.cpp ) 45 | TARGET_LINK_LIBRARIES(mptrap_tst ${PROJECT_NAME} ${Eigen_LIBRARIES}) 46 | 47 | 48 | add_executable(matrix_traits examples/matrix_traits.cpp ) 49 | TARGET_LINK_LIBRARIES(matrix_traits ${PROJECT_NAME} ${Eigen_LIBRARIES}) 50 | 51 | add_executable(geometry examples/geometry.cpp ) 52 | TARGET_LINK_LIBRARIES(geometry ${PROJECT_NAME} ${Eigen_LIBRARIES}) 53 | 54 | add_executable(expressiontree_variabletype examples/expressiontree_variabletype.cpp ) 55 | TARGET_LINK_LIBRARIES(expressiontree_variabletype ${PROJECT_NAME} ${Eigen_LIBRARIES}) 56 | 57 | 58 | add_executable(expressiontree_example examples/expressiontree_example.cpp ) 59 | TARGET_LINK_LIBRARIES(expressiontree_example ${PROJECT_NAME} ${Eigen_LIBRARIES}) 60 | 61 | add_executable(expressiontree_example2 examples/expressiontree_example2.cpp ) 62 | TARGET_LINK_LIBRARIES(expressiontree_example2 ${PROJECT_NAME} ${Eigen_LIBRARIES}) 63 | 64 | add_executable(expressiontree_example3 examples/expressiontree_example3.cpp ) 65 | TARGET_LINK_LIBRARIES(expressiontree_example3 ${PROJECT_NAME} ${Eigen_LIBRARIES}) 66 | 67 | add_executable(expressiontree_example4 examples/expressiontree_example4.cpp ) 68 | TARGET_LINK_LIBRARIES(expressiontree_example4 ${PROJECT_NAME} ${Eigen_LIBRARIES}) 69 | 70 | add_executable(expressiontree_example5 examples/expressiontree_example5.cpp ) 71 | TARGET_LINK_LIBRARIES(expressiontree_example5 ${PROJECT_NAME} ${Eigen_LIBRARIES}) 72 | 73 | add_executable(expressiontree_example6 examples/expressiontree_example6.cpp ) 74 | TARGET_LINK_LIBRARIES(expressiontree_example6 ${PROJECT_NAME} ${Eigen_LIBRARIES}) 75 | 76 | add_executable(expressiontree_example7 examples/expressiontree_example7.cpp ) 77 | TARGET_LINK_LIBRARIES(expressiontree_example7 ${PROJECT_NAME} ${Eigen_LIBRARIES}) 78 | 79 | add_executable(expressiontree_example8 examples/expressiontree_example8.cpp ) 80 | TARGET_LINK_LIBRARIES(expressiontree_example8 ${PROJECT_NAME} ${Eigen_LIBRARIES}) 81 | 82 | add_executable(expressiontree_example9 examples/expressiontree_example9.cpp ) 83 | TARGET_LINK_LIBRARIES(expressiontree_example9 ${PROJECT_NAME} ${Eigen_LIBRARIES}) 84 | add_executable(expressiontree_example10 examples/expressiontree_example10.cpp ) 85 | TARGET_LINK_LIBRARIES(expressiontree_example10 ${PROJECT_NAME} ${Eigen_LIBRARIES}) 86 | 87 | add_executable(expressiontree_example11 examples/expressiontree_example11.cpp ) 88 | TARGET_LINK_LIBRARIES(expressiontree_example11 ${PROJECT_NAME} ${Eigen_LIBRARIES}) 89 | 90 | add_executable(expressiontree_example12 examples/expressiontree_example12.cpp ) 91 | TARGET_LINK_LIBRARIES(expressiontree_example12 ${PROJECT_NAME} ${Eigen_LIBRARIES}) 92 | 93 | #add_executable(expressiontree_example13 examples/expressiontree_example13.cpp ) 94 | #TARGET_LINK_LIBRARIES(expressiontree_example13 ${PROJECT_NAME} ${Eigen_LIBRARIES}) 95 | 96 | add_executable(resolved_motion_rate_control examples/resolved_motion_rate_control.cpp ) 97 | TARGET_LINK_LIBRARIES(resolved_motion_rate_control ${PROJECT_NAME} ${Eigen_LIBRARIES}) 98 | 99 | add_executable(expressiontree_perpendicular examples/expressiontree_perpendicular.cpp ) 100 | TARGET_LINK_LIBRARIES(expressiontree_perpendicular ${PROJECT_NAME} ${Eigen_LIBRARIES}) 101 | 102 | add_executable(conversions_example examples/conversions_example.cpp ) 103 | TARGET_LINK_LIBRARIES(conversions_example ${PROJECT_NAME} ${Eigen_LIBRARIES}) 104 | 105 | add_executable(expressiontree_distance_to_line examples/expressiontree_distance_to_line.cpp) 106 | TARGET_LINK_LIBRARIES(expressiontree_distance_to_line ${PROJECT_NAME} ${Eigen_LIBRARIES}) 107 | 108 | add_executable(expressiontree_conditional examples/expressiontree_conditional.cpp) 109 | TARGET_LINK_LIBRARIES(expressiontree_conditional ${PROJECT_NAME} ${Eigen_LIBRARIES}) 110 | 111 | add_executable(expressiontree_cached_named examples/expressiontree_cached_named.cpp) 112 | TARGET_LINK_LIBRARIES(expressiontree_cached_named ${PROJECT_NAME} ${Eigen_LIBRARIES}) 113 | 114 | add_executable(expressiontree_optimizer examples/expressiontree_optimizer.cpp) 115 | TARGET_LINK_LIBRARIES(expressiontree_optimizer ${PROJECT_NAME} ${Eigen_LIBRARIES}) 116 | 117 | add_executable(expressiontree_mimo_ex examples/expressiontree_mimo_ex.cpp) 118 | TARGET_LINK_LIBRARIES(expressiontree_mimo_ex ${PROJECT_NAME} ${Eigen_LIBRARIES}) 119 | 120 | add_executable(expressiontree_sensor examples/expressiontree_sensor.cpp) 121 | TARGET_LINK_LIBRARIES(expressiontree_sensor ${PROJECT_NAME} ${Eigen_LIBRARIES}) 122 | 123 | add_executable(expressiontree_motion_profile examples/expressiontree_motion_profile.cpp) 124 | TARGET_LINK_LIBRARIES(expressiontree_motion_profile ${PROJECT_NAME} ${Eigen_LIBRARIES}) 125 | 126 | add_executable(expressiontree_motion_profile2 examples/expressiontree_motion_profile2.cpp) 127 | TARGET_LINK_LIBRARIES(expressiontree_motion_profile2 ${PROJECT_NAME} ${Eigen_LIBRARIES}) 128 | 129 | 130 | add_executable(initial_value examples/initial_value.cpp) 131 | TARGET_LINK_LIBRARIES(initial_value ${PROJECT_NAME} ${Eigen_LIBRARIES}) 132 | 133 | 134 | 135 | # TUTORIAL FILES: 136 | add_executable(tutorial1 examples/tutorial1.cpp) 137 | TARGET_LINK_LIBRARIES(tutorial1 ${PROJECT_NAME} ${Eigen_LIBRARIES}) 138 | 139 | add_executable(tutorial2 examples/tutorial2.cpp) 140 | TARGET_LINK_LIBRARIES(tutorial2 ${PROJECT_NAME} ${Eigen_LIBRARIES}) 141 | 142 | add_executable(tutorial3 examples/tutorial3.cpp) 143 | TARGET_LINK_LIBRARIES(tutorial3 ${PROJECT_NAME} ${Eigen_LIBRARIES}) 144 | 145 | add_executable(saturate examples/saturate.cpp) 146 | TARGET_LINK_LIBRARIES(saturate ${PROJECT_NAME} ${Eigen_LIBRARIES}) 147 | 148 | add_executable(debug1 examples/debug1.cpp) 149 | TARGET_LINK_LIBRARIES(debug1 ${PROJECT_NAME} ${Eigen_LIBRARIES}) 150 | 151 | 152 | ENDIF(ENABLE_EXAMPLES) 153 | 154 | -------------------------------------------------------------------------------- /examples/conversions_example.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiontree_example.cpp 3 | * 4 | * Created on: Sept., 2012 5 | * Author: Erwin Aertbelien 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | /* 30 | * This small example show the conversion between Eigen types and KDL types 31 | * 32 | * The toKDLxxx() function specify the KDL type in the name because it is impossible 33 | * to determine the KDL type at compile time when using a dynamic eigen matrix. 34 | */ 35 | int main(int argc, char* argv[]) { 36 | using namespace KDL; 37 | using namespace std; 38 | cout << "Demonstration of simple conversion routines between KDL and Eigen types \n"; 39 | Vector a(1.0,2.0,3.0); 40 | cout << "KDL Vector " << a << "\n"; 41 | Eigen::Vector3d a_eigen; 42 | a_eigen = toEigen(a); 43 | cout << "Eigen representation " << a_eigen.transpose() << "\n"; 44 | cout << "converted back to KDL " << toKDLVector(a_eigen) << "\n"; 45 | cout << endl; 46 | 47 | Twist t(Vector(1.0,2.0,3.0),Vector(4.0,5.0,6.0)); 48 | cout << "KDL Vector " << t << "\n"; 49 | Eigen::Matrix t_eigen; 50 | t_eigen = toEigen(t); 51 | cout << "Eigen representation " << t_eigen.transpose() << "\n"; 52 | cout << "converted back to KDL " << toKDLTwist(t_eigen) << "\n"; 53 | cout << endl; 54 | 55 | 56 | Wrench w(Vector(1.0,2.0,3.0),Vector(4.0,5.0,6.0)); 57 | cout << "KDL Wrench " << t << "\n"; 58 | Eigen::Matrix w_eigen; 59 | w_eigen = toEigen(w); 60 | cout << "Eigen representation " << w_eigen.transpose() << "\n"; 61 | cout << "converted back to KDL " << toKDLTwist(w_eigen) << "\n"; 62 | 63 | Rotation R( Rotation::EulerZYX(30*deg2rad,45*deg2rad,0*deg2rad) ); 64 | cout << "KDL Rotation " << R << "\n"; 65 | Eigen::Matrix R_eigen; 66 | R_eigen = toEigen( R ); 67 | cout << "Eigen 3x3 matrix " << R_eigen << "\n"; 68 | cout << "converted back to KDL " << toKDLRotation( R_eigen ) << "\n"; 69 | Eigen::Quaternion q; 70 | q = toEigenQuaternion( R ); 71 | cout << "Eigen Quaternion " << q.coeffs() << "\n"; 72 | cout << "converted back to KDL " << toKDLRotation( q ) << "\n"; 73 | cout << endl; 74 | return 0; 75 | } 76 | -------------------------------------------------------------------------------- /examples/debug1.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file expressiontree_example6.cpp 3 | * \brief example of using ExpressionTree in combination with KDL::Chain. 4 | * 5 | * \Author: 2014, Erwin Aertbelien 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | /** 31 | * numerically comparing to ways to compute a Jacobian of a Chain. 32 | * (combining expressiontree_example5.cpp and expressiontree_example6.cpp) 33 | */ 34 | int main(int argc, char* argv[]) { 35 | using namespace KDL; 36 | using namespace std; 37 | 38 | Expression::Ptr a = input(1); 39 | Expression::Ptr b = input(2); 40 | 41 | Expression::Ptr r = acos( cos( a ) )-a; 42 | 43 | cout << "\nExpression 2 in prefix notation : \n"; 44 | cout << "\n\n"; 45 | 46 | a->setInputValue(1,0.1); 47 | b->setInputValue(1,0.2); 48 | 49 | cout << "value r " << r->value() << endl; 50 | cout << "derivative towards 1 " << r->derivative(1) << endl; 51 | cout << "derivative towards 2 " << r->derivative(2) << endl; 52 | return 0; 53 | } 54 | -------------------------------------------------------------------------------- /examples/expressiontree_cached_named.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file expressiontree_named_cached.cpp 3 | * \brief example of Expression<> classes, and more specifically cached and named nodes. 4 | * 5 | * The cached node remembers the evaluation of a node, such that it can be reused later on. 6 | * The cached value remains available until setInputValue(s) is called. 7 | * 8 | * One can give a name to a cached node. In an expression containing this cached node, 9 | * one can recover the subexpression using a subexpression(complete_expr, name) call. 10 | * 11 | * \Author: Dec 2012, Erwin Aertbelien 12 | * expressiongraph library 13 | * 14 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 15 | * 16 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 17 | * You may not use this work except in compliance with the Licence. 18 | * You may obtain a copy of the Licence at: 19 | * 20 | * http://ec.europa.eu/idabc/eupl 21 | * 22 | * Unless required by applicable law or agreed to in writing, software 23 | * distributed under the Licence is distributed on an "AS IS" basis, 24 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 25 | * See the Licence for the specific language governing permissions and 26 | * limitations under the Licence. 27 | */ 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | int main(int argc, char* argv[]) { 34 | using namespace KDL; 35 | using namespace std; 36 | 37 | // the following example can be useful to understand the source code: 38 | cout << type_comparison::return_if_equal(Frame::Identity()) << endl; 39 | cout << type_comparison::return_if_equal(1.0) << endl; 40 | 41 | // and now for the real example: 42 | double L1=0.310; 43 | double L2=0.400; 44 | double L3=0.390; 45 | double L4=0.078; 46 | Expression::Ptr robot = cached( 47 | "ee", 48 | cached("wrist", 49 | cached("elbow", 50 | cached("shoulder", 51 | frame( rot_z(input(0)), Constant(Vector(0,0,L1))) * 52 | frame( rot_x(input(1)) ) * 53 | frame( rot_z(input(2) ) ) 54 | ) * 55 | frame( rot_x(input(3) ), Constant(Vector(0,0,L2))) 56 | ) * 57 | frame( rot_z(input(4) ), Constant(Vector(0,0,L3))) 58 | ) * 59 | frame( rot_x(input(5) ) ) * 60 | frame( rot_z(input(6) ), Constant(Vector(0,0,L4)) ) 61 | ); 62 | 63 | // write to a .dot file for visualization: 64 | std::ofstream of("expressiontree_named_cached1.dot"); 65 | robot->write_dotfile(of); 66 | Expression::Ptr elbow = robot->subExpression_Frame("elbow"); 67 | if (elbow) { 68 | std::ofstream of("expressiontree_named_cached2.dot"); 69 | elbow->write_dotfile(of); 70 | } else { 71 | cerr << "did not find a subexpression with the name 'elbow' " << endl; 72 | } 73 | Expression::Ptr wrist = robot->subExpression_Frame("wrist"); 74 | if (wrist) { 75 | std::ofstream of("expressiontree_named_cached3.dot"); 76 | wrist->write_dotfile(of); 77 | } else { 78 | cerr << "did not find a subexpression with the name 'wrist' " << endl; 79 | } 80 | 81 | return 0; 82 | } 83 | -------------------------------------------------------------------------------- /examples/expressiontree_cb.cpp: -------------------------------------------------------------------------------- 1 | // example of a MIMO expressiongraph node: 2 | // a multiplication by a 2x2 matrix. 3 | #include 4 | #include 5 | 6 | using namespace KDL; 7 | 8 | 9 | class MMult : public MIMO { 10 | public: 11 | double a,b,c,d; 12 | double y[2]; 13 | double dydx[2][2]; 14 | 15 | typedef boost::shared_ptr Ptr; 16 | 17 | MMult(double _a, double _b, double _c, double _d, Expression::Ptr x1, Expression::Ptr x2): 18 | MIMO("MMult"), 19 | a(_a),b(_b),c(_c),d(_d) { 20 | inputDouble.push_back(x1); 21 | inputDouble.push_back(x2); 22 | } 23 | 24 | void compute() { 25 | if (cached) return; 26 | y[0] = a*inputDouble[0]->value() + b*inputDouble[1]->value(); 27 | y[1] = c*inputDouble[0]->value() + d*inputDouble[1]->value(); 28 | dydx[0][0] = a; 29 | dydx[0][1] = b; 30 | dydx[1][0] = c; 31 | dydx[1][1] = d; 32 | cached=true; 33 | } 34 | 35 | virtual MIMO::Ptr clone() { 36 | MIMO::Ptr tmp( new MMult(a,b,c,d,inputDouble[0]->clone(),inputDouble[1]->clone())); 37 | return tmp; 38 | } 39 | }; 40 | 41 | MMult::Ptr create_MMult(double a, double b, double c, double d, Expression::Ptr x1, Expression::Ptr x2) { 42 | MMult::Ptr tmp( new MMult(a,b,c,d,x1,x2)); 43 | return tmp; 44 | } 45 | 46 | 47 | class MMultOut : public MIMO_Output { 48 | public: 49 | typedef boost::shared_ptr Ptr; 50 | int outputnr; 51 | 52 | MMultOut(MIMO::Ptr m, int _outputnr):MIMO_Output("MMult_Out",m), outputnr(_outputnr) {} 53 | 54 | double value() { 55 | MMult::Ptr p = boost::static_pointer_cast(mimo); 56 | p->compute(); 57 | return p->y[outputnr]; 58 | } 59 | 60 | double derivative(int i){ 61 | MMult::Ptr p = boost::static_pointer_cast(mimo); 62 | p->compute(); 63 | if ((0<=i)&&(i<=1)) { 64 | return p->dydx[outputnr][i]; 65 | } else { 66 | return 0.0; 67 | } 68 | } 69 | MIMO_Output::Ptr clone() { 70 | MMultOut::Ptr tmp( new MMultOut(getMIMOClone(),outputnr)); 71 | return tmp; 72 | } 73 | }; 74 | 75 | MMultOut::Ptr create_MMult_Output( MMult::Ptr m, int i) { 76 | MMultOut::Ptr tmp( new MMultOut(m, i)); 77 | return tmp; 78 | } 79 | 80 | 81 | 82 | int main(int argc,char* argv[]) { 83 | using namespace std; 84 | Expression::Ptr x1 = input(0); 85 | Expression::Ptr x2 = input(1); 86 | MMult::Ptr m = create_MMult(1,2,3,4,x1,x2); 87 | Expression::Ptr y1 = create_MMult_Output(m,0); 88 | Expression::Ptr y2 = create_MMult_Output(m,1); 89 | 90 | // display the graph: 91 | ofstream of("mimo.dot"); 92 | write_dotfile_start(of); 93 | y1->write_dotfile_init(); 94 | y2->write_dotfile_init(); 95 | y1->write_dotfile_update(of); 96 | y2->write_dotfile_update(of); 97 | write_dotfile_end(of); 98 | of.close(); 99 | 100 | // evaluate the graph, method 1 for two sets of values: 101 | y1->setInputValue(0,1.0); 102 | y2->setInputValue(0,1.0); 103 | y1->setInputValue(1,2.0); 104 | y2->setInputValue(1,2.0); 105 | cout << "method 1 with (x1=1,x2=2)" << endl; 106 | cout << "y1="<< y1->value() << endl; 107 | cout << "y2="<< y2->value() << endl; 108 | cout << "y1 derivatives = " << y1->derivative(0) << " " << y1->derivative(1) << endl; 109 | cout << "y2 derivatives = " << y2->derivative(0) << " " << y2->derivative(1) << endl; 110 | y1->setInputValue(0,0.0); 111 | y2->setInputValue(0,0.0); 112 | y1->setInputValue(1,1.0); 113 | y2->setInputValue(1,1.0); 114 | cout << "method 1 with (x1=0,x2=1)" << endl; 115 | cout << "y1="<< y1->value() << endl; 116 | cout << "y2="<< y2->value() << endl; 117 | cout << "y1 derivatives = " << y1->derivative(0) << " " << y1->derivative(1) << endl; 118 | cout << "y2 derivatives = " << y2->derivative(0) << " " << y2->derivative(1) << endl; 119 | 120 | // evaluate the graph, method 2, using expression optimizer: 121 | ExpressionOptimizer optimizer; 122 | std::vector variablelist; 123 | variablelist.push_back(0); 124 | variablelist.push_back(1); 125 | optimizer.prepare( variablelist ); 126 | y1->addToOptimizer(optimizer); 127 | y2->addToOptimizer(optimizer); 128 | std::vector values(2); 129 | values[0] = 1.0; 130 | values[1] = 2.0; 131 | optimizer.setInputValues( values ); 132 | cout << "method 2 with (x1=1,x2=2)" << endl; 133 | cout << "y1="<< y1->value() << endl; 134 | cout << "y2="<< y2->value() << endl; 135 | cout << "y1 derivatives = " << y1->derivative(0) << " " << y1->derivative(1) << endl; 136 | cout << "y2 derivatives = " << y2->derivative(0) << " " << y2->derivative(1) << endl; 137 | values[0] = 0.0; 138 | values[1] = 1.0; 139 | optimizer.setInputValues( values ); 140 | cout << "method 2 with (x1=0,x2=1)" << endl; 141 | cout << "y1="<< y1->value() << endl; 142 | cout << "y2="<< y2->value() << endl; 143 | cout << "y1 derivatives = " << y1->derivative(0) << " " << y1->derivative(1) << endl; 144 | cout << "y2 derivatives = " << y2->derivative(0) << " " << y2->derivative(1) << endl; 145 | 146 | // cloning: 147 | Expression::Ptr y1b = y1->clone(); 148 | Expression::Ptr y2b = y2->clone(); 149 | 150 | // display the graph after cloning: 151 | ofstream of2("mimo_cloning.dot"); 152 | write_dotfile_start(of2); 153 | y1->write_dotfile_init(); 154 | y2->write_dotfile_init(); 155 | y1b->write_dotfile_init(); 156 | y2b->write_dotfile_init(); 157 | y1->write_dotfile_update(of2); 158 | y2->write_dotfile_update(of2); 159 | y1b->write_dotfile_update(of2); 160 | y2b->write_dotfile_update(of2); 161 | write_dotfile_end(of2); 162 | of2.close(); 163 | 164 | 165 | // evaluation after cloning 166 | ExpressionOptimizer optimizer2; 167 | optimizer2.prepare( variablelist ); 168 | y1b->addToOptimizer(optimizer2); 169 | y2b->addToOptimizer(optimizer2); 170 | values[0] = 1.0; 171 | values[1] = 2.0; 172 | optimizer2.setInputValues( values ); 173 | cout << "method 2(cloned) with (x1=1,x2=2)" << endl; 174 | cout << "y1b="<< y1b->value() << endl; 175 | cout << "y2b="<< y2b->value() << endl; 176 | cout << "y1 derivatives = " << y1b->derivative(0) << " " << y1b->derivative(1) << endl; 177 | cout << "y2 derivatives = " << y2b->derivative(0) << " " << y2b->derivative(1) << endl; 178 | values[0] = 0.0; 179 | values[1] = 1.0; 180 | optimizer2.setInputValues( values ); 181 | cout << "method 2(cloned) with (x1=0,x2=1)" << endl; 182 | cout << "y1b="<< y1b->value() << endl; 183 | cout << "y2b="<< y2b->value() << endl; 184 | cout << "y1 derivatives = " << y1b->derivative(0) << " " << y1b->derivative(1) << endl; 185 | cout << "y2 derivatives = " << y2b->derivative(0) << " " << y2b->derivative(1) << endl; 186 | 187 | return 0; 188 | } 189 | -------------------------------------------------------------------------------- /examples/expressiontree_conditional.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiontree_conditional.cpp 3 | * 4 | * Created on: Dec., 2012 5 | * Author: Erwin Aertbelien 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #include 25 | #include 26 | 27 | /* 28 | * Example that demonstrates the use of the "conditional" function. 29 | */ 30 | int main(int argc, char* argv[]) { 31 | using namespace KDL; 32 | using namespace std; 33 | 34 | Expression::Ptr f = 35 | KDL::conditional( 36 | input(0)-Constant(5.0), 37 | (input(0)-Constant(5.0))*(input(0)-Constant(5.0)), 38 | Constant(0.0) 39 | ); 40 | Expression::Ptr fd = f->derivativeExpression(0); 41 | for (double t=0.0;t<=10;t+=0.01) { 42 | f->setInputValue(t); 43 | fd->setInputValue(t); 44 | cout << t << "\t" 45 | << f->value() << "\t" 46 | << f->derivative(0) << "\t" 47 | << fd->value() << "\t" 48 | << fd->derivative(0) << "\n"; 49 | } 50 | return 0; 51 | } 52 | -------------------------------------------------------------------------------- /examples/expressiontree_distance_to_line.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiontree_example.cpp 3 | * 4 | * Created on: Sept., 2012 5 | * Author: Erwin Aertbelien 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #include 25 | #include 26 | 27 | /* 28 | * Example that demonstrates the evaluation of an expression in KDL Types ( Frames, Rotation, Vector) for 29 | * both value() and derivative(). 30 | * 31 | * This example also shows that ExpressionTree can be used as an alternative to compute the Jacobian for 32 | * a kinematic chain. 33 | * @TODO: provide adaptor for Chain 34 | * @TODO: name collision between std::vector and KDL::vector (expressiontrees) can be confusing 35 | * for unexperienced users. 36 | */ 37 | int main(int argc, char* argv[]) { 38 | using namespace KDL; 39 | using namespace std; 40 | double L1=0.310; 41 | double L2=0.400; 42 | double L3=0.390; 43 | double L4=0.078; 44 | 45 | Chain chain; 46 | chain.addSegment(Segment("Segment 0", Joint("Joint 0", Joint::RotZ),Frame(Vector(0,0,L1)))); 47 | chain.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotX),Frame(Vector(0,0,0)))); 48 | chain.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotZ),Frame(Vector(0,0,L2)))); 49 | chain.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotX),Frame(Vector(0,0,0)))); 50 | chain.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotZ),Frame(Vector(0,0,L3)))); 51 | chain.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::RotX),Frame(Vector(0,0,L4)))); 52 | chain.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotZ),Frame(Vector(0,0,0)))); 53 | 54 | 55 | Expression::Ptr kinchain = kinematic_chain(chain, 0 ); 56 | Frame F_w_plane( Rotation::Identity(), Vector(0,0,0.5) ); 57 | 58 | 59 | // specify input values: 60 | std::vector joints(7); 61 | joints[0] = 0; 62 | joints[1] = M_PI/6; 63 | joints[2] = 0; 64 | joints[3] = 0.3; 65 | joints[4] = 0; 66 | joints[5] = 0.5; 67 | joints[6] = 0; 68 | kinchain->setInputValues( joints ); 69 | 70 | Frame line( Rotation::Identity(), Vector(0.2,0.2,0.0)); // z axis of frame corresponds to line. 71 | 72 | Expression::Ptr tmp = cached( Constant(line.Inverse()) * origin(kinchain) ); 73 | Expression::Ptr distance_to_line = sqrt( coord_x(tmp)*coord_x(tmp) + coord_z(tmp)*coord_z(tmp) ); 74 | cout << "value " << distance_to_line->value() << "\n"; 75 | for (int i=0;i<7;++i) { 76 | cout << "jac towards joint " << i << " : " << distance_to_line->derivative(i) << endl; 77 | } 78 | 79 | cout << " visualize with 'dot -Tpdf expressiontree_distance_to_line.dot > expressiontree_distance_to_line.pdf'" << endl; 80 | std::ofstream of("expressiontree_distance_to_line.dot"); 81 | distance_to_line->write_dotfile(of); 82 | return 0; 83 | } 84 | -------------------------------------------------------------------------------- /examples/expressiontree_example.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiontree_example.cpp 3 | * 4 | * Created on: Aug 7, 2012 5 | * Author: Wouter Bancken 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #include 25 | 26 | /* 27 | * Example on how to use VariableType 28 | */ 29 | int main(int argc, char* argv[]) { 30 | using namespace KDL; 31 | 32 | VariableType::Ptr v = Variable(0,2); 33 | v->setValue( Rotation::EulerZYX(0.1, 0.2, 0.3) ); 34 | v->setJacobian(0, KDL::Vector(0,0,1) ); // be carefull: "0" refers to the 0-th column of the Jacobian, not to the 0-th variable. 35 | v->setJacobian(1, KDL::Vector(1,0,0) ); 36 | std::cout << std::endl; 37 | std::cout << v->ndx.size() << std::endl; 38 | Expression::Ptr result; 39 | result = Constant(Rotation::EulerZYX(0.01, 0.02, 0.03))*v; 40 | 41 | std::cout << "expression tree:\n"; 42 | result->debug_printtree(); 43 | std::cout << std::endl; 44 | 45 | std::cout << "\n\nvalue \n"; 46 | std::cout << result->value(); 47 | std::cout << "\n\nderivative 0 \n"; 48 | std::cout << result->derivative(0); 49 | std::cout << "\n\nderivative 1 \n"; 50 | std::cout << result->derivative(1); 51 | std::cout << "\n\nderivative 2 \n"; 52 | std::cout << result->derivative(2); 53 | std::cout << " \n\nnumber of derivatives "; 54 | std::cout << result->number_of_derivatives(); 55 | std::cout << "\n\n"; 56 | 57 | /* second example: */ 58 | VariableType::Ptr vec = Variable(1,1); 59 | vec->setValue( KDL::Vector(4,5,6) ); 60 | vec->setJacobian(0, KDL::Vector(0,1,0) ); 61 | Expression::Ptr result2 = norm(vec); 62 | 63 | std::cout << "expression tree:\n"; 64 | result2->debug_printtree(); 65 | std::cout << std::endl; 66 | 67 | std::cout << "\n\nvalue \n"; 68 | std::cout << result2->value(); 69 | std::cout << "\n\nderivative 0 \n"; 70 | std::cout << result2->derivative(0); 71 | std::cout << "\n\nderivative 1 \n"; 72 | std::cout << result2->derivative(1); 73 | std::cout << "\n\n"; 74 | return 0; 75 | } 76 | -------------------------------------------------------------------------------- /examples/expressiontree_example10.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file expressiontree_example9.cpp 3 | * \brief example of using ExpressionTree in combination with KDL::Chain. 4 | * 5 | * \Author: Sept. 2012, Erwin Aertbelien 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | /** 31 | * computing derivatives of a Jacobian: dJ/dq 32 | */ 33 | int main(int argc, char* argv[]) { 34 | using namespace KDL; 35 | using namespace std; 36 | double L1=0.310; 37 | double L2=0.400; 38 | double L3=0.390; 39 | double L4=0.078; 40 | 41 | Chain chain; 42 | chain.addSegment(Segment("Segment 0", Joint("Joint 0", Joint::RotZ),Frame(Vector(0,0,L1)))); 43 | chain.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotX),Frame(Vector(0,0,0)))); 44 | chain.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotZ),Frame(Vector(0,0,L2)))); 45 | chain.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotX),Frame(Vector(0,0,0)))); 46 | chain.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotZ),Frame(Vector(0,0,L3)))); 47 | chain.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::RotX),Frame(Vector(0,0,L4)))); 48 | chain.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotZ),Frame(Vector(0,0,0)))); 49 | 50 | Expression::Ptr F_w_robot1 = kinematic_chain(chain, 0 ); 51 | Frame F_w_plane( Rotation::Identity(), Vector(0,0,0.5) ); 52 | 53 | cout << "--------------------------------------" << endl; 54 | cout << " A constraint for the distance to a plane " << endl; 55 | cout << " along a beam along the z-axis at the end effector " << endl; 56 | cout << "--------------------------------------" << endl; 57 | 58 | cout << "------ Constraint, directly computed " << endl; 59 | // signed distance to plane divided by cos(angle), 60 | // problematic if beam // to plane. 61 | Expression::Ptr distance = dot( Constant(F_w_plane.M.UnitZ()), Constant( F_w_plane.p) - origin(F_w_robot1) ) / 62 | dot( Constant(F_w_plane.M.UnitZ()), unit_z(rotation(F_w_robot1) ) ); 63 | display(cout,distance); 64 | 65 | cout << "------ Constraint, iTasc style " << endl; 66 | // follows iTasc paper, figure 6 67 | // introduces 6 chi_f joints (7->12). 68 | Expression::Ptr o1 = Constant(F_w_plane); 69 | Expression::Ptr o2 = F_w_robot1; 70 | Expression::Ptr f1 = o1 * frame( KDL::vector(input(7),input(8),Constant(0.0))); 71 | Expression::Ptr f2 = o2 * frame( KDL::vector(Constant(0.0),Constant(0.0),input(12)) ); 72 | 73 | Expression::Ptr loop_closure = f1*frame(rot_x(input(9)))*frame(rot_y(input(10)))*frame(rot_z(input(11)))*inv(f2); 74 | 75 | Expression::Ptr distance_2 = input(12); 76 | cout << "loop closure " << endl; 77 | display(cout,loop_closure); 78 | cout << "distance " << endl; 79 | display(cout,distance_2); 80 | 81 | return 0; 82 | } 83 | -------------------------------------------------------------------------------- /examples/expressiontree_example11.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiontree_example11.cpp 3 | * 4 | * Created on: Sept., 2012 5 | * Author: Erwin Aertbelien 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #include 25 | 26 | /* 27 | * Example that demonstrates the evaluation of a simple expression in doubles for 28 | * both value() and derivative() 29 | * and the use of derivativeExpression() to compute a derivative to an arbitrary order. 30 | */ 31 | int main(int argc, char* argv[]) { 32 | using namespace KDL; 33 | 34 | // note: 35 | // input(0) represent variable 0, it gives back the input values given by 36 | // the setInputValue.. type methods. It always has type 37 | // Expression::Ptr. 38 | // Constant(xx) represent a constant value (i.e. with derivatives zero). 39 | // It has the type of xx. If you specify Constant(1), you'll 40 | // have the type Expression::Ptr, which is probably not what 41 | // you want. (There are no implicit type conversions) 42 | Expression::Ptr result = Constant(2.0)*input(0)*sin(Constant(2*M_PI)*input(0)); 43 | Expression::Ptr resultd = result->derivativeExpression(0); 44 | Expression::Ptr resultdd = resultd->derivativeExpression(0); 45 | 46 | // The columns printer are: 47 | // 48 | // time / derivative using result / derivative using resultd / 2nd derivative using resultd / 3th derivative using resultdd 49 | // 50 | for (double t=0.0;t<10.0;t+=0.01) { 51 | // be sure to call the methods in this order: 52 | // it is important to first call setInputValue(), then value() and then derivative() 53 | // (since value() can cache computations to be used by derivative(), for efficiency reasons ) 54 | result->setInputValue(t); 55 | resultd->setInputValue(t); 56 | double val = result->value(); 57 | double deriv = result->derivative(0); // normally you would not use this but resultd->value() 58 | double vald = resultd->value(); 59 | double deriv2 = resultd->derivative(0); 60 | double deriv3 = resultdd->derivative(0); 61 | std::cout << t << "\t" << val << "\t" << deriv << "\t" << vald << "\t" << deriv2 << "\t" << deriv3 <(std::cout, result); 67 | // std::cout << std::endl; 68 | return 0; 69 | } 70 | -------------------------------------------------------------------------------- /examples/expressiontree_example12.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiontree_example11.cpp 3 | * 4 | * Created on: Sept., 2012 5 | * Author: Erwin Aertbelien 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #include 25 | 26 | /* 27 | * Example that demonstrates the evaluation of a simple expression in doubles for 28 | * both value() and derivative() 29 | * and the use of derivativeExpression() to compute a derivative to an arbitrary order. 30 | */ 31 | int main(int argc, char* argv[]) { 32 | using namespace KDL; 33 | using namespace std; 34 | cout << " -----------------------------------------------\n"; 35 | cout << "Specifying a kinematic chain in two different ways\n"; 36 | cout << " -----------------------------------------------\n"; 37 | 38 | double L1=0.310; 39 | double L2=0.400; 40 | double L3=0.390; 41 | double L4=0.078; 42 | Chain chain; 43 | chain.addSegment(Segment("Segment 0", Joint("Joint 0", Joint::RotZ),Frame(Vector(0,0,L1)))); 44 | chain.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotX),Frame(Vector(0,0,0)))); 45 | chain.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotZ),Frame(Vector(0,0,L2)))); 46 | chain.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotX),Frame(Vector(0,0,0)))); 47 | chain.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotZ),Frame(Vector(0,0,L3)))); 48 | chain.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::RotX),Frame(Vector(0,0,L4)))); 49 | chain.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotZ),Frame(Vector(0,0,0)))); 50 | 51 | Expression::Ptr kinchain1 = kinematic_chain( chain, 0 ); 52 | Expression::Ptr kinchain2 = cached( 53 | frame( rot_z(input(0)), Constant(Vector(0,0,L1))) * 54 | frame( rot_x(input(1)) ) * 55 | frame( rot_z(input(2) ) ) * 56 | frame( rot_x(input(3) ), Constant(Vector(0,0,L2))) * 57 | frame( rot_z(input(4) ), Constant(Vector(0,0,L3))) * 58 | frame( rot_x(input(5) ) ) * 59 | frame( rot_z(input(6) ), Constant(Vector(0,0,L4)) ) 60 | ); 61 | 62 | cout << " -----------------------------------------------\n"; 63 | cout << "Specifying joint values\n"; 64 | cout << " -----------------------------------------------\n"; 65 | std::vector joints(7); 66 | int N=10; 67 | joints[0] = 0*M_PI*0.8/N; 68 | joints[1] = 1*M_PI*0.8/N; 69 | joints[2] = 2*M_PI*0.8/N; 70 | joints[3] = 3*M_PI*0.8/N; 71 | joints[4] = 4*M_PI*0.8/N; 72 | joints[5] = 5*M_PI*0.8/N; 73 | joints[6] = 6*M_PI*0.8/N; 74 | 75 | 76 | cout << " -----------------------------------------------\n"; 77 | cout << "evaluating:\n"; 78 | cout << " -----------------------------------------------\n"; 79 | kinchain1->setInputValues(joints); 80 | kinchain2->setInputValues(joints); 81 | display(cout,kinchain2); 82 | for (int i=0;i<7;++i) { 83 | Expression::Ptr deriv = kinchain2->derivativeExpression(i); 84 | cout << "-"<< endl; 85 | kinchain2->debug_printtree(); 86 | deriv->setInputValues(joints); 87 | cout << "Derivative towards " << i << endl; 88 | display(cout,deriv); 89 | } 90 | return 0; 91 | } 92 | -------------------------------------------------------------------------------- /examples/expressiontree_example2.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiontree_example.cpp 3 | * 4 | * Created on: Sept., 2012 5 | * Author: Erwin Aertbelien 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #include 25 | 26 | /* 27 | * Example that demonstrates the evaluation of a simple expression in doubles for 28 | * both value() and derivative() 29 | */ 30 | int main(int argc, char* argv[]) { 31 | using namespace KDL; 32 | 33 | // note: 34 | // input(0) represent variable 0, it gives back the input values given by 35 | // the setInputValue.. type methods. It always has type 36 | // Expression::Ptr. 37 | // Constant(xx) represent a constant value (i.e. with derivatives zero). 38 | // It has the type of xx. If you specify Constant(1), you'll 39 | // have the type Expression::Ptr, which is probably not what 40 | // you want. (There are no implicit type conversions) 41 | Expression::Ptr result = Constant(2.0)*input(0)*sin(Constant(2*M_PI)*input(0)); 42 | 43 | std::cout << "Expression in prefix notation : \n"; 44 | result->debug_printtree(); 45 | std::cout << "\n\n"; 46 | for (double t=0.0;t<10.0;t+=0.01) { 47 | // be sure to call the methods in this order: 48 | // it is important to first call setInputValue(), then value() and then derivative() 49 | // (since value() can cache computations to be used by derivative(), for efficiency reasons ) 50 | result->setInputValue(t); 51 | double val = result->value(); 52 | double deriv = result->derivative(0); 53 | std::cout << t << "\t" << val << "\t" << deriv << std::endl; 54 | } 55 | // you can also use the display() function : 56 | // you will have to specify the template parameter explicitly, because 57 | // C++ cannot deduce the type with display() : 58 | // display(std::cout, result); 59 | // std::cout << std::endl; 60 | return 0; 61 | } 62 | -------------------------------------------------------------------------------- /examples/expressiontree_example3.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiontree_example3.cpp 3 | * 4 | * Created on: Sept., 2012 5 | * Author: Erwin Aertbelien 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #include 25 | 26 | /** 27 | * An example of a expression in multiple variables. 28 | */ 29 | int main(int argc, char* argv[]) { 30 | using namespace KDL; 31 | using namespace std; 32 | Expression::Ptr expr = sin(input(0)) + cos(input(1)); 33 | std::vector inp(2); 34 | inp[0] = 1; 35 | inp[1] = 2; 36 | expr->setInputValues(inp); 37 | cout << "Expression Tree in prefix notation : \n"; 38 | expr->debug_printtree(); 39 | cout << "\n\nValue " << expr->value() << endl; 40 | cout << "Derivative towards 0 " << expr->derivative(0) << endl; 41 | cout << "Derivative towards 1 " << expr->derivative(1) << endl; 42 | Expression::Ptr exprd0 = expr->derivativeExpression(0); 43 | cout << "derivative towards 0 " << endl; 44 | exprd0->print(cout); 45 | Expression::Ptr exprd1 = expr->derivativeExpression(1); 46 | cout << endl << "derivative towards 1 " << endl; 47 | exprd1->print(cout); 48 | cout << endl; 49 | return 0; 50 | } 51 | -------------------------------------------------------------------------------- /examples/expressiontree_example4.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiontree_example.cpp 3 | * 4 | * Created on: Sept., 2012 5 | * Author: Erwin Aertbelien 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #include 25 | #include 26 | 27 | /* 28 | * Example that demonstrates the evaluation of an expression in KDL Types ( Frames, Rotation, Vector) for 29 | * both value() and derivative(). 30 | * 31 | * This example also shows that ExpressionTree can be used as an alternative to compute the Jacobian for 32 | * a kinematic chain. 33 | * @TODO: provide adaptor for Chain 34 | * @TODO: name collision between std::vector and KDL::vector (expressiontrees) can be confusing 35 | * for unexperienced users. 36 | */ 37 | int main(int argc, char* argv[]) { 38 | using namespace KDL; 39 | using namespace std; 40 | double L1=0.310; 41 | double L2=0.400; 42 | double L3=0.390; 43 | double L4=0.078; 44 | Expression::Ptr kinchain = cached( 45 | frame( rot_z(input(0)), Constant(Vector(0,0,L1))) * 46 | frame( rot_x(-input(1)) ) * 47 | frame( rot_z(input(2) ) ) * 48 | frame( rot_x(input(3) ), Constant(Vector(0,0,L2))) * 49 | frame( rot_z(input(4) ), Constant(Vector(0,0,L3))) * 50 | frame( rot_x(-input(5) ) ) * 51 | frame( rot_z(input(6) ), Constant(Vector(0,0,L4)) ) 52 | ); 53 | 54 | std::cout << "Expression in prefix notation : \n"; 55 | kinchain->debug_printtree(); 56 | cout << "\n\n"; 57 | 58 | // specify input values: 59 | std::vector joints(7); 60 | joints[0] = 0; 61 | joints[1] = M_PI/6; 62 | joints[2] = 0; 63 | joints[3] = 0; 64 | joints[4] = 0; 65 | joints[5] = 0; 66 | joints[6] = 0; 67 | kinchain->setInputValues( joints ); 68 | 69 | cout << "--- ex. 1 : Pose of the end effector of the kin. chain ---" << endl; 70 | // remember: always call setInputValues() before value() 71 | cout << "Position of end effector : " << kinchain->value() << endl; 72 | // remember: always call value() before derivative() 73 | for (int i=0;i<7;++i) { 74 | cout << "jac towards joint " << i << kinchain->derivative(i) << endl; 75 | } 76 | 77 | cout << "--- ex. 2 : Distance to point [0.2,0.2,0.8] ---" << endl; 78 | Vector refpoint(0.2,0.2,0.8); 79 | 80 | Expression::Ptr distance = norm( origin(kinchain) - Constant(refpoint) ); 81 | 82 | cout << "value " << distance->value() << "\n"; 83 | for (int i=0;i<7;++i) { 84 | cout << "jac towards joint " << i << " : " << distance->derivative(i) << endl; 85 | } 86 | 87 | 88 | cout << "--- ex. 3 : Distance to Plane given by x-y axis and point in plane [0,0,0.8] ---" << endl; 89 | Frame plane( Rotation::Identity(), Vector(0,0,0.8) ); // x-y axis of frame corresponds to plane. 90 | 91 | Expression::Ptr distance_to_plane = 92 | coord_z(Constant(plane.Inverse()) * origin(kinchain) ); 93 | 94 | cout << "value " << distance_to_plane->value() << "\n"; 95 | for (int i=0;i<7;++i) { 96 | cout << "jac towards joint " << i << " : " << distance_to_plane->derivative(i) << endl; 97 | } 98 | cout << "--- ex. 4 : Distance to a vertical line through [0.2,0.2,0] ---" << endl; 99 | Frame line( Rotation::Identity(), Vector(0.2,0.2,0.0)); // z axis of frame corresponds to line. 100 | 101 | Expression::Ptr tmp = cached( Constant(line.Inverse()) * origin(kinchain) ); 102 | Expression::Ptr distance_to_line = sqrt( coord_x(tmp)*coord_x(tmp) + coord_z(tmp)*coord_z(tmp) ); 103 | cout << "value " << distance_to_line->value() << "\n"; 104 | for (int i=0;i<7;++i) { 105 | cout << "jac towards joint " << i << " : " << distance_to_line->derivative(i) << endl; 106 | } 107 | 108 | cout << "--- ex. 5 : write an expressiontree to a graphviz .dot file " << endl; 109 | cout << " visualize with 'dot -Tpdf expressiontree_example4.dot > t.pdf'" << endl; 110 | std::ofstream of("expressiontree_example4.dot"); 111 | distance_to_line->write_dotfile(of); 112 | return 0; 113 | } 114 | -------------------------------------------------------------------------------- /examples/expressiontree_example5.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiontree_example.cpp 3 | * 4 | * Created on: Sept., 2012 5 | * Author: Erwin Aertbelien 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | /* 30 | * An example that investigates the computation time of expression tree's. 31 | * takes a 30 dof kin chain and some additional expressions and evaluates 32 | * the computation time. 33 | * 34 | * computation for this kin. chain using expression trees is 10us 35 | * on a 2011 Dell Latitude Laptop. 36 | * @TODO : cloning on cached works unexpectedly and will result into multiple instances 37 | * of the same. 38 | * @TODO : cached is not represented well in the graphviz plots. One instance is represented 39 | * as multiple instances. 40 | * @TODO: we are dealing with DAG not with tree's ! 41 | * @TODO: handle/forbid copy constructors and assignment operators for all ExpressionTree classes. 42 | * @TODO: kinfam_io.hpp does not work for input. 43 | * @TODO: split up into .hpp and .cpp for all classes where this is possible. 44 | */ 45 | int main(int argc, char* argv[]) { 46 | using namespace KDL; 47 | using namespace std; 48 | double L1=0.310; 49 | double L2=0.400; 50 | double L3=0.390; 51 | double L4=0.078; 52 | Expression::Ptr kinchain = cached( 53 | frame( rot_z(input(0)), Constant(Vector(0,0,L1))) * 54 | frame( rot_x(-input(1)) ) * 55 | frame( rot_z(input(2) ) ) * 56 | frame( rot_x(input(3) ), Constant(Vector(0,0,L2))) * 57 | frame( rot_z(input(4) ), Constant(Vector(0,0,L3))) * 58 | frame( rot_x(-input(5) ) ) * 59 | frame( rot_y(input(6) ), Constant(Vector(0,0,L4)) )* 60 | frame( rot_y(input(7) ), Constant(Vector(0,0,L4)) )* 61 | frame( rot_z(input(8) ), Constant(Vector(0,0,L4)) )* 62 | frame( rot_y(input(9) ), Constant(Vector(0,0,L4)) )* 63 | frame( rot_z(input(10) ), Constant(Vector(0,0,L4)) )* 64 | frame( rot_y(input(11) ), Constant(Vector(0,0,L4)) )* 65 | frame( rot_z(input(12) ), Constant(Vector(0,0,L4)) )* 66 | frame( rot_y(input(13) ), Constant(Vector(0,0,L4)) )* 67 | frame( rot_z(input(14) ), Constant(Vector(0,0,L4)) )* 68 | frame( rot_y(input(15) ), Constant(Vector(0,0,L4)) )* 69 | frame( rot_z(input(16) ), Constant(Vector(0,0,L4)) )* 70 | frame( rot_y(input(17) ), Constant(Vector(0,0,L4)) )* 71 | frame( rot_z(input(18) ), Constant(Vector(0,0,L4)) )* 72 | frame( rot_y(input(19) ), Constant(Vector(0,0,L4)) )* 73 | frame( rot_z(input(20) ), Constant(Vector(0,0,L4)) )* 74 | frame( rot_y(input(21) ), Constant(Vector(0,0,L4)) )* 75 | frame( rot_z(input(22) ), Constant(Vector(0,0,L4)) )* 76 | frame( rot_y(input(23) ), Constant(Vector(0,0,L4)) )* 77 | frame( rot_z(input(24) ), Constant(Vector(0,0,L4)) )* 78 | frame( rot_y(input(25) ), Constant(Vector(0,0,L4)) )* 79 | frame( rot_z(input(26) ), Constant(Vector(0,0,L4)) )* 80 | frame( rot_y(input(27) ), Constant(Vector(0,0,L4)) )* 81 | frame( rot_z(input(28) ), Constant(Vector(0,0,L4)) )* 82 | frame( rot_y(input(29) ), Constant(Vector(0,0,L4)) ) 83 | ); 84 | 85 | std::cout << "Expression in prefix notation : \n"; 86 | kinchain->debug_printtree(); 87 | cout << "\n\n"; 88 | 89 | Frame line( Rotation::Identity(), Vector(0.2,0.2,0.0)); // z axis of frame corresponds to line. 90 | 91 | Expression::Ptr tmp = cached( Constant(line.Inverse()) * origin(kinchain) ); 92 | Expression::Ptr distance_to_line = sqrt( coord_x(tmp)*coord_x(tmp) + coord_z(tmp)*coord_z(tmp) ); 93 | std::cout << "numer of derivatives " << distance_to_line->number_of_derivatives() << std::endl; 94 | boost::timer timer; 95 | int N = 100000; 96 | std::vector joints(distance_to_line->number_of_derivatives()); 97 | for (int n=0;nsetInputValues( joints ); 110 | distance_to_line->value(); 111 | for (int i=0;iderivative(i); 113 | } 114 | } 115 | cout << "time per evaluation in microseconds : " 116 | << timer.elapsed()*1000000.0/N << " us " << endl; 117 | std::ofstream of("expressiontree_example5.dot"); 118 | distance_to_line->write_dotfile(of); 119 | 120 | return 0; 121 | } 122 | -------------------------------------------------------------------------------- /examples/expressiontree_example6.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file expressiontree_example6.cpp 3 | * \brief example of using ExpressionTree in combination with KDL::Chain. 4 | * 5 | * \Author: Sept. 2012, Erwin Aertbelien 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | /* 29 | * An example that investigates the computation time of expression tree's. 30 | * takes a 30 dof kin chain and some additional expressions and evaluates 31 | * the computation time. 32 | * 33 | * computation for this kin. chain using expression trees is 10us 34 | * on a 2011 Dell Latitude Laptop. 35 | * @TODO : cloning on cached works unexpectedly and will result into multiple instances 36 | * of the same. 37 | * @TODO : cached is not represented well in the graphviz plots. One instance is represented 38 | * as multiple instances. 39 | * @TODO: we are dealing with DAG not with tree's ! 40 | * @TODO: handle/forbid copy constructors and assignment operators for all ExpressionTree classes. 41 | * @TODO: adapt license to 42 | */ 43 | int main(int argc, char* argv[]) { 44 | using namespace KDL; 45 | using namespace std; 46 | double L1=0.310; 47 | double L2=0.400; 48 | double L3=0.390; 49 | double L4=0.078; 50 | Chain chain; 51 | chain.addSegment(Segment("Segment 0", Joint("Joint 0", Joint::RotZ),Frame(Vector(0.0,0.0,L1)))); 52 | chain.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotX),Frame(Vector(0.0,0.0,0.0)))); 53 | chain.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotZ),Frame(Vector(0.0,0.0,0.0)))); 54 | chain.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotX),Frame(Vector(0.0,0.0,L2)))); 55 | chain.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotZ),Frame(Vector(0.0,0.0,L3)))); 56 | chain.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::RotX),Frame(Vector(0.0,0.0,L4)))); 57 | chain.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotY),Frame(Vector(0.0,0.0,L4)))); 58 | chain.addSegment(Segment("Segment 7", Joint("Joint 7", Joint::RotY),Frame(Vector(0.0,0.0,L4)))); 59 | chain.addSegment(Segment("Segment 8", Joint("Joint 8", Joint::RotZ),Frame(Vector(0.0,0.0,L4)))); 60 | chain.addSegment(Segment("Segment 9", Joint("Joint 9", Joint::RotY),Frame(Vector(0.0,0.0,L4)))); 61 | chain.addSegment(Segment("Segment 10", Joint("Joint 10", Joint::RotZ),Frame(Vector(0.0,0.0,L4)))); 62 | chain.addSegment(Segment("Segment 11", Joint("Joint 11", Joint::RotY),Frame(Vector(0.0,0.0,L4)))); 63 | chain.addSegment(Segment("Segment 12", Joint("Joint 12", Joint::RotZ),Frame(Vector(0.0,0.0,L4)))); 64 | chain.addSegment(Segment("Segment 13", Joint("Joint 13", Joint::RotY),Frame(Vector(0.0,0.0,L4)))); 65 | chain.addSegment(Segment("Segment 14", Joint("Joint 14", Joint::RotZ),Frame(Vector(0.0,0.0,L4)))); 66 | chain.addSegment(Segment("Segment 15", Joint("Joint 15", Joint::RotY),Frame(Vector(0.0,0.0,L4)))); 67 | chain.addSegment(Segment("Segment 16", Joint("Joint 16", Joint::RotZ),Frame(Vector(0.0,0.0,L4)))); 68 | chain.addSegment(Segment("Segment 17", Joint("Joint 17", Joint::RotY),Frame(Vector(0.0,0.0,L4)))); 69 | chain.addSegment(Segment("Segment 18", Joint("Joint 18", Joint::RotZ),Frame(Vector(0.0,0.0,L4)))); 70 | chain.addSegment(Segment("Segment 19", Joint("Joint 19", Joint::RotY),Frame(Vector(0.0,0.0,L4)))); 71 | chain.addSegment(Segment("Segment 20", Joint("Joint 20", Joint::RotZ),Frame(Vector(0.0,0.0,L4)))); 72 | chain.addSegment(Segment("Segment 21", Joint("Joint 21", Joint::RotY),Frame(Vector(0.0,0.0,L4)))); 73 | chain.addSegment(Segment("Segment 22", Joint("Joint 22", Joint::RotZ),Frame(Vector(0.0,0.0,L4)))); 74 | chain.addSegment(Segment("Segment 23", Joint("Joint 23", Joint::RotY),Frame(Vector(0.0,0.0,L4)))); 75 | chain.addSegment(Segment("Segment 24", Joint("Joint 24", Joint::RotZ),Frame(Vector(0.0,0.0,L4)))); 76 | chain.addSegment(Segment("Segment 25", Joint("Joint 25", Joint::RotY),Frame(Vector(0.0,0.0,L4)))); 77 | chain.addSegment(Segment("Segment 26", Joint("Joint 26", Joint::RotZ),Frame(Vector(0.0,0.0,L4)))); 78 | chain.addSegment(Segment("Segment 27", Joint("Joint 27", Joint::RotY),Frame(Vector(0.0,0.0,L4)))); 79 | chain.addSegment(Segment("Segment 28", Joint("Joint 28", Joint::RotZ),Frame(Vector(0.0,0.0,L4)))); 80 | chain.addSegment(Segment("Segment 29", Joint("Joint 29", Joint::RotY),Frame(Vector(0.0,0.0,L4)))); 81 | 82 | Expression::Ptr kinchain = Constant( Frame(Vector(0,0,0.1)))*kinematic_chain( chain, 0 ); 83 | std::cout << "Expression in prefix notation : \n"; 84 | kinchain->debug_printtree(); 85 | cout << "\n\n"; 86 | 87 | Frame line( Rotation::Identity(), Vector(0.2,0.2,0.0)); // z axis of frame corresponds to line. 88 | 89 | Expression::Ptr tmp = cached( Constant(line.Inverse()) * origin(kinchain) ); 90 | Expression::Ptr distance_to_line = coord_x(tmp)*coord_x(tmp) + coord_z(tmp)*coord_z(tmp); 91 | std::cout << "numer of derivatives " << distance_to_line->number_of_derivatives() << std::endl; 92 | boost::timer timer; 93 | int N = 100000; 94 | std::vector joints(distance_to_line->number_of_derivatives()); 95 | for (int n=0;nsetInputValues( joints ); 108 | distance_to_line->value(); 109 | for (int i=0;iderivative(i); 111 | } 112 | } 113 | cout << "time per evaluation in microseconds : " 114 | << timer.elapsed()*1000000.0/N << " us " << endl; 115 | std::ofstream of("expressiontree_example6.dot"); 116 | distance_to_line->write_dotfile(of); 117 | 118 | return 0; 119 | } 120 | -------------------------------------------------------------------------------- /examples/expressiontree_example7.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file expressiontree_example6.cpp 3 | * \brief example of using ExpressionTree in combination with KDL::Chain. 4 | * 5 | * \Author: Sept. 2012, Erwin Aertbelien 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | /** 31 | * numerically comparing to ways to compute a Jacobian of a Chain. 32 | * (combining expressiontree_example5.cpp and expressiontree_example6.cpp) 33 | */ 34 | int main(int argc, char* argv[]) { 35 | using namespace KDL; 36 | using namespace std; 37 | double L1=0.310; 38 | double L2=0.400; 39 | double L3=0.390; 40 | double L4=0.078; 41 | 42 | /** 43 | * The same chain is defined twice. Note that the definitions of chain 44 | * are different: 45 | * chain: first a joint rotation and then the frame transformation. 46 | * frame in the expression tree: first translation and then rotation4. 47 | */ 48 | Chain chain; 49 | chain.addSegment(Segment("Segment 0", Joint("Joint 0", Joint::RotZ),Frame(Vector(0,0,L1)))); 50 | chain.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotX),Frame(Vector(0,0,0)))); 51 | chain.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotZ),Frame(Vector(0,0,L2)))); 52 | chain.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotX),Frame(Vector(0,0,0)))); 53 | chain.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotZ),Frame(Vector(0,0,L3)))); 54 | chain.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::RotX),Frame(Vector(0,0,L4)))); 55 | chain.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotZ),Frame(Vector(0,0,0)))); 56 | 57 | Expression::Ptr kinchain1 = kinematic_chain( chain, 0 ); 58 | Expression::Ptr kinchain2 = cached( 59 | frame( rot_z(input(0)), Constant(Vector(0,0,L1))) * 60 | frame( rot_x(input(1)) ) * 61 | frame( rot_z(input(2) ) ) * 62 | frame( rot_x(input(3) ), Constant(Vector(0,0,L2))) * 63 | frame( rot_z(input(4) ), Constant(Vector(0,0,L3))) * 64 | frame( rot_x(input(5) ) ) * 65 | frame( rot_z(input(6) ), Constant(Vector(0,0,L4)) ) 66 | ); 67 | 68 | 69 | 70 | std::cout << "\nExpression 1 in prefix notation : \n"; 71 | kinchain1->debug_printtree(); 72 | std::cout << "\nExpression 2 in prefix notation : \n"; 73 | kinchain2->debug_printtree(); 74 | cout << "\n\n"; 75 | 76 | Frame line( Rotation::Identity(), Vector(0.2,0.2,0.0)); // z axis of frame corresponds to line. 77 | 78 | Expression::Ptr tmp1 = cached( Constant(line.Inverse()) * origin(kinchain1) ); 79 | Expression::Ptr tmp2 = cached( Constant(line.Inverse()) * origin(kinchain2) ); 80 | Expression::Ptr distance_to_line1 = sqrt( coord_x(tmp1)*coord_x(tmp1) + coord_z(tmp1)*coord_z(tmp1) ); 81 | Expression::Ptr distance_to_line2 = sqrt( coord_x(tmp2)*coord_x(tmp2) + coord_z(tmp2)*coord_z(tmp2) ); 82 | std::cout << "numer of derivatives 1 " << distance_to_line1->number_of_derivatives() << std::endl; 83 | std::cout << "numer of derivatives 2 " << distance_to_line2->number_of_derivatives() << std::endl; 84 | boost::timer timer; 85 | int N = 10; 86 | std::vector joints(distance_to_line1->number_of_derivatives()); 87 | // specify input values: 88 | joints[0] = 0*M_PI*0.8/N; 89 | joints[1] = 1*M_PI*0.8/N; 90 | joints[2] = 2*M_PI*0.8/N; 91 | joints[3] = 3*M_PI*0.8/N; 92 | joints[4] = 4*M_PI*0.8/N; 93 | joints[5] = 5*M_PI*0.8/N; 94 | joints[6] = 6*M_PI*0.8/N; 95 | distance_to_line1->setInputValues( joints ); 96 | distance_to_line2->setInputValues( joints ); 97 | cout << "Value : \n";\ 98 | cout << "chain \t,\t pure expr \n"; 99 | cout << distance_to_line1->value() << "\t,\t" << distance_to_line2->value() << "\n"; 100 | cout << "Derivatives : \n"; 101 | cout << "chain \t,\t num.deriv.\t,\tpure expr \t,\tnum. deriv \n"; 102 | for (int i=0;iderivative(i) << "\t,\t" 104 | << setw(10) << numerical_derivative(distance_to_line1,i,joints[i],1E-10)<< "\t,\t" 105 | << setw(10) << distance_to_line2->derivative(i) <<"\t,\t" 106 | << setw(10) << numerical_derivative(distance_to_line2,i,joints[i],1E-10)<< "\n"; 107 | } 108 | return 0; 109 | } 110 | -------------------------------------------------------------------------------- /examples/expressiontree_example8.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file expressiontree_example6.cpp 3 | * \brief example of using ExpressionTree in combination with KDL::Chain. 4 | * 5 | * \Author: Sept. 2012, Erwin Aertbelien 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | KDL::Twist numerical_derivative_jacobian( boost::shared_ptr& expr, std::vector values, int first,int second, double dt=1E-7) { 31 | KDL::Twist a,b; 32 | double val = values[second]; 33 | 34 | values[second] = val-dt; 35 | expr->setInputValues(values); 36 | expr->value(); // always call value() before derivative() 37 | a = expr->derivative(first); 38 | 39 | values[second] = val+dt; 40 | expr->setInputValues(values); 41 | expr->value(); 42 | b = expr->derivative(first); 43 | 44 | values[second] = val; 45 | return diff(a,b,2*dt); 46 | } 47 | 48 | KDL::Twist numerical_timederivative_jacobian( boost::shared_ptr& expr, std::vector values, std::vector values_dot,int column, double dt=1E-7) { 49 | std::vector val(values.size()); 50 | KDL::Twist a,b; 51 | for (int i=0;isetInputValues(val); 56 | expr->value(); // always call value() before derivative() 57 | a = expr->derivative(column); 58 | 59 | 60 | for (int i=0;isetInputValues(val); 64 | expr->value(); 65 | b = expr->derivative(column); 66 | 67 | return diff(a,b,2*dt); 68 | } 69 | 70 | /** 71 | * computing derivatives of a Jacobian: dJ/dq 72 | */ 73 | int main(int argc, char* argv[]) { 74 | using namespace KDL; 75 | using namespace std; 76 | double L1=0.310; 77 | double L2=0.400; 78 | double L3=0.390; 79 | double L4=0.078; 80 | 81 | /** 82 | * The same chain is defined twice. Note that the definitions of chain 83 | * are different: 84 | * chain: first a joint rotation and then the frame transformation. 85 | * frame in the expression tree: first translation and then rotation4. 86 | */ 87 | Chain chain; 88 | chain.addSegment(Segment("Segment 0", Joint("Joint 0", Joint::RotZ),Frame(Vector(0,0,L1)))); 89 | chain.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotX),Frame(Vector(0,0,0)))); 90 | chain.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotZ),Frame(Vector(0,0,L2)))); 91 | chain.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotX),Frame(Vector(0,0,0)))); 92 | chain.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotZ),Frame(Vector(0,0,L3)))); 93 | chain.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::RotX),Frame(Vector(0,0,L4)))); 94 | chain.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotZ),Frame(Vector(0,0,0)))); 95 | 96 | // you cannot use the kinematic_chain helper function because you need the specific type 97 | // of Expression_chain instead of the base class Expression<> 98 | boost::shared_ptr kinchain( new Expression_Chain(chain, 0 )); 99 | std::vector joints(kinchain->number_of_derivatives()); 100 | // specify input values: 101 | int N=10; 102 | joints[0] = 0*M_PI*0.8/N; 103 | joints[1] = 1*M_PI*0.8/N; 104 | joints[2] = 2*M_PI*0.8/N; 105 | joints[3] = 3*M_PI*0.8/N; 106 | joints[4] = 4*M_PI*0.8/N; 107 | joints[5] = 5*M_PI*0.8/N; 108 | joints[6] = 6*M_PI*0.8/N; 109 | kinchain->setInputValues( joints ); 110 | cout << "Value : \n"; 111 | Frame tf = kinchain->value(); 112 | cout << tf << "\n"; 113 | cout << "Columns of the Jacobian \n"; 114 | for (int i=0;inumber_of_derivatives();++i) { 115 | cout << "Derivative " << i << " : " << kinchain->derivative(i) << endl; 116 | } 117 | 118 | cout << "\n\n\nComputing the partial derivative of the Jacobian\n"; 119 | for (int i=0;inumber_of_derivatives();++i) { 120 | for (int j=0;jnumber_of_derivatives();++j) { 121 | cout << "Derivative towards " << i << " and then towards " << j << " : " << kinchain->derivative(i,j) << endl; 122 | cout << "Numerically " << i << " towards " << j << " : " 123 | << numerical_derivative_jacobian(kinchain,joints,i,j,1E-7) << endl; 124 | } 125 | std::cout << endl; 126 | } 127 | 128 | 129 | cout << "\n\n\nComputing the time derivative of the Jacobian\n"; 130 | std::vector joints_dot(kinchain->number_of_derivatives()); 131 | joints_dot[0] = 0*M_PI*0.1; 132 | joints_dot[1] = 1*M_PI*0.1; 133 | joints_dot[2] = 2*M_PI*0.1; 134 | joints_dot[3] = 3*M_PI*0.1; 135 | joints_dot[4] = 4*M_PI*0.1; 136 | joints_dot[5] = 5*M_PI*0.1; 137 | joints_dot[6] = 6*M_PI*0.1; 138 | kinchain->value(); // for all ExpressionTree objects, always call value(), even if you do not need it. 139 | for (int i=0;inumber_of_derivatives();++i) { 140 | cout << "J_dot column " << i << " : " << kinchain->derivative_dot(i,joints_dot) << "\n"; 141 | cout << "numerical " << i << " : " << numerical_timederivative_jacobian(kinchain,joints,joints_dot,i) << "\n"; 142 | } 143 | return 0; 144 | } 145 | -------------------------------------------------------------------------------- /examples/expressiontree_example9.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file expressiontree_example9.cpp 3 | * \brief example of using ExpressionTree in combination with KDL::Chain. 4 | * 5 | * \Author: Sept. 2012, Erwin Aertbelien 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | /** 31 | * computing derivatives of a Jacobian: dJ/dq 32 | */ 33 | int main(int argc, char* argv[]) { 34 | using namespace KDL; 35 | using namespace std; 36 | double L1=0.310; 37 | double L2=0.400; 38 | double L3=0.390; 39 | double L4=0.078; 40 | 41 | /** 42 | * The same chain is defined twice. Note that the definitions of chain 43 | * are different: 44 | * chain: first a joint rotation and then the frame transformation. 45 | * frame in the expression tree: first translation and then rotation4. 46 | */ 47 | Chain chain; 48 | chain.addSegment(Segment("Segment 0", Joint("Joint 0", Joint::RotZ),Frame(Vector(0,0,L1)))); 49 | chain.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotX),Frame(Vector(0,0,0)))); 50 | chain.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotZ),Frame(Vector(0,0,L2)))); 51 | chain.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotX),Frame(Vector(0,0,0)))); 52 | chain.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotZ),Frame(Vector(0,0,L3)))); 53 | chain.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::RotX),Frame(Vector(0,0,L4)))); 54 | chain.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotZ),Frame(Vector(0,0,0)))); 55 | 56 | // two robots, 1 m apart 57 | // note that chain is copied, so you can reuse it freely. 58 | Expression::Ptr F_w_robot1 = cached( Constant(Frame(Vector(0,0,0))) * kinematic_chain(chain, 0 )); 59 | Expression::Ptr F_w_robot2 = cached( Constant(Frame(Vector(1.0,0,0))) * kinematic_chain(chain, 7 )); 60 | 61 | 62 | // ** constraint 1: distance between 2 robot ee is L, 63 | double L=1.0; 64 | Expression::Ptr constraint1 = norm( origin(F_w_robot2) - origin(F_w_robot1) ) - Constant(L); 65 | 66 | // ** constraint 2&3: robot1 ee is on a cylinder, ee xy-plane lies on surface of cylinder. 67 | Frame F_w_cylinder( Rotation::Identity(), Vector(0.1,-0.95,0.0)); // center point at the base of the cylinder. 68 | double radius = 0.1; // radius of cylinder 69 | Expression::Ptr tmpF_cylinder_robot1 = cached( Constant(F_w_cylinder.Inverse()) * F_w_robot1 ); 70 | // ee point on the cylinder: 71 | Expression::Ptr constraint2 = coord_x(origin(tmpF_cylinder_robot1))*coord_x(origin(tmpF_cylinder_robot1)) + 72 | coord_y(origin(tmpF_cylinder_robot1))*coord_y(origin(tmpF_cylinder_robot1)) 73 | - Constant(radius*radius); 74 | // ee on surface of cylinder 75 | Expression::Ptr constraint3 = dot( unit_z( rotation(F_w_robot1) ), Constant(F_w_cylinder.M.UnitZ())); 76 | 77 | std::vector joints(constraint1->number_of_derivatives()); 78 | // specify input values: 79 | joints[0] = 0; 80 | joints[1] = 20*deg2rad; 81 | joints[2] = 10*deg2rad; 82 | joints[3] = 30*deg2rad; 83 | joints[4] = 0; 84 | joints[5] = 30*deg2rad; 85 | joints[6] = 0; 86 | joints[7] = 0; 87 | joints[8] = 20*deg2rad; 88 | joints[9] = 0; 89 | joints[10] = 30*deg2rad; 90 | joints[11] = 0; 91 | joints[12] = 30*deg2rad; 92 | joints[13] = 0; 93 | 94 | cout << "--- Distance constraint ---\n"; 95 | constraint1->setInputValues( joints ); 96 | display(cout,constraint1); 97 | cout << "\n"; 98 | cout << "--- Cylinder constraint 2 ---\n"; 99 | constraint2->setInputValues( joints ); 100 | display(cout,constraint2); 101 | cout << "--- Cylinder constraint 3 ---\n"; 102 | constraint3->setInputValues( joints ); 103 | display(cout,constraint3); 104 | 105 | cout << "--- dependencies ---- \n"; 106 | std::set varset; 107 | constraint3->getDependencies(varset); 108 | for (std::set::iterator it=varset.begin(); it!= varset.end(); ++it) { 109 | cout << *it << "\t"; 110 | } 111 | cout << "\n"; 112 | std::set varset2; 113 | constraint1->getDependencies(varset2); 114 | for (std::set::iterator it=varset2.begin(); it!= varset2.end(); ++it) { 115 | cout << *it << "\t"; 116 | } 117 | cout << "\n"; 118 | 119 | return 0; 120 | } 121 | -------------------------------------------------------------------------------- /examples/expressiontree_mimo_ex.cpp: -------------------------------------------------------------------------------- 1 | // example of a MIMO expressiongraph node: 2 | // a multiplication by a 2x2 matrix. 3 | #include 4 | #include 5 | 6 | using namespace KDL; 7 | 8 | class MMult : public MIMO { 9 | public: 10 | double a,b,c,d; 11 | double y[2]; 12 | double dydx[2][2]; 13 | 14 | typedef boost::shared_ptr Ptr; 15 | 16 | MMult(double _a, double _b, double _c, double _d, Expression::Ptr x1, Expression::Ptr x2): 17 | MIMO("MMult"), 18 | a(_a),b(_b),c(_c),d(_d) { 19 | inputDouble.push_back(x1); 20 | inputDouble.push_back(x2); 21 | } 22 | 23 | void compute() { 24 | if (cached) return; 25 | y[0] = a*inputDouble[0]->value() + b*inputDouble[1]->value(); 26 | y[1] = c*inputDouble[0]->value() + d*inputDouble[1]->value(); 27 | dydx[0][0] = a; 28 | dydx[0][1] = b; 29 | dydx[1][0] = c; 30 | dydx[1][1] = d; 31 | cached=true; 32 | } 33 | 34 | virtual MIMO::Ptr clone() { 35 | MIMO::Ptr tmp( new MMult(a,b,c,d,inputDouble[0]->clone(),inputDouble[1]->clone())); 36 | return tmp; 37 | } 38 | }; 39 | 40 | MMult::Ptr create_MMult(double a, double b, double c, double d, Expression::Ptr x1, Expression::Ptr x2) { 41 | MMult::Ptr tmp( new MMult(a,b,c,d,x1,x2)); 42 | return tmp; 43 | } 44 | 45 | 46 | class MMultOut : public MIMO_Output { 47 | public: 48 | typedef boost::shared_ptr Ptr; 49 | int outputnr; 50 | 51 | MMultOut(MIMO::Ptr m, int _outputnr):MIMO_Output("MMult_Out",m), outputnr(_outputnr) {} 52 | 53 | double value() { 54 | MMult::Ptr p = boost::static_pointer_cast(mimo); 55 | p->compute(); 56 | return p->y[outputnr]; 57 | } 58 | 59 | double derivative(int i){ 60 | MMult::Ptr p = boost::static_pointer_cast(mimo); 61 | p->compute(); 62 | if ((0<=i)&&(i<=1)) { 63 | return p->dydx[outputnr][i]; 64 | } else { 65 | return 0.0; 66 | } 67 | } 68 | MIMO_Output::Ptr clone() { 69 | MMultOut::Ptr tmp( new MMultOut(getMIMOClone(),outputnr)); 70 | return tmp; 71 | } 72 | }; 73 | 74 | MMultOut::Ptr create_MMult_Output( MMult::Ptr m, int i) { 75 | MMultOut::Ptr tmp( new MMultOut(m, i)); 76 | return tmp; 77 | } 78 | 79 | 80 | int main(int argc,char* argv[]) { 81 | using namespace std; 82 | Expression::Ptr x1 = input(0); 83 | Expression::Ptr x2 = input(1); 84 | MMult::Ptr m = create_MMult(1,2,3,4,x1,x2); 85 | Expression::Ptr y1 = create_MMult_Output(m,0); 86 | Expression::Ptr y2 = create_MMult_Output(m,1); 87 | 88 | // display the graph: 89 | ofstream of("mimo.dot"); 90 | write_dotfile_start(of); 91 | y1->write_dotfile_init(); 92 | y2->write_dotfile_init(); 93 | y1->write_dotfile_update(of); 94 | y2->write_dotfile_update(of); 95 | write_dotfile_end(of); 96 | of.close(); 97 | 98 | // evaluate the graph, method 1 for two sets of values: 99 | y1->setInputValue(0,1.0); 100 | y2->setInputValue(0,1.0); 101 | y1->setInputValue(1,2.0); 102 | y2->setInputValue(1,2.0); 103 | cout << "method 1 with (x1=1,x2=2)" << endl; 104 | cout << "y1="<< y1->value() << endl; 105 | cout << "y2="<< y2->value() << endl; 106 | cout << "y1 derivatives = " << y1->derivative(0) << " " << y1->derivative(1) << endl; 107 | cout << "y2 derivatives = " << y2->derivative(0) << " " << y2->derivative(1) << endl; 108 | y1->setInputValue(0,0.0); 109 | y2->setInputValue(0,0.0); 110 | y1->setInputValue(1,1.0); 111 | y2->setInputValue(1,1.0); 112 | cout << "method 1 with (x1=0,x2=1)" << endl; 113 | cout << "y1="<< y1->value() << endl; 114 | cout << "y2="<< y2->value() << endl; 115 | cout << "y1 derivatives = " << y1->derivative(0) << " " << y1->derivative(1) << endl; 116 | cout << "y2 derivatives = " << y2->derivative(0) << " " << y2->derivative(1) << endl; 117 | 118 | // evaluate the graph, method 2, using expression optimizer: 119 | ExpressionOptimizer optimizer; 120 | std::vector variablelist; 121 | variablelist.push_back(0); 122 | variablelist.push_back(1); 123 | optimizer.prepare( variablelist ); 124 | y1->addToOptimizer(optimizer); 125 | y2->addToOptimizer(optimizer); 126 | std::vector values(2); 127 | values[0] = 1.0; 128 | values[1] = 2.0; 129 | optimizer.setInputValues( values ); 130 | cout << "method 2 with (x1=1,x2=2)" << endl; 131 | cout << "y1="<< y1->value() << endl; 132 | cout << "y2="<< y2->value() << endl; 133 | cout << "y1 derivatives = " << y1->derivative(0) << " " << y1->derivative(1) << endl; 134 | cout << "y2 derivatives = " << y2->derivative(0) << " " << y2->derivative(1) << endl; 135 | values[0] = 0.0; 136 | values[1] = 1.0; 137 | optimizer.setInputValues( values ); 138 | cout << "method 2 with (x1=0,x2=1)" << endl; 139 | cout << "y1="<< y1->value() << endl; 140 | cout << "y2="<< y2->value() << endl; 141 | cout << "y1 derivatives = " << y1->derivative(0) << " " << y1->derivative(1) << endl; 142 | cout << "y2 derivatives = " << y2->derivative(0) << " " << y2->derivative(1) << endl; 143 | 144 | // cloning: 145 | Expression::Ptr y1b = y1->clone(); 146 | Expression::Ptr y2b = y2->clone(); 147 | 148 | // display the graph after cloning: 149 | ofstream of2("mimo_cloning.dot"); 150 | write_dotfile_start(of2); 151 | y1->write_dotfile_init(); 152 | y2->write_dotfile_init(); 153 | y1b->write_dotfile_init(); 154 | y2b->write_dotfile_init(); 155 | y1->write_dotfile_update(of2); 156 | y2->write_dotfile_update(of2); 157 | y1b->write_dotfile_update(of2); 158 | y2b->write_dotfile_update(of2); 159 | write_dotfile_end(of2); 160 | of2.close(); 161 | 162 | 163 | // evaluation after cloning 164 | ExpressionOptimizer optimizer2; 165 | optimizer2.prepare( variablelist ); 166 | y1b->addToOptimizer(optimizer2); 167 | y2b->addToOptimizer(optimizer2); 168 | values[0] = 1.0; 169 | values[1] = 2.0; 170 | optimizer2.setInputValues( values ); 171 | cout << "method 2(cloned) with (x1=1,x2=2)" << endl; 172 | cout << "y1b="<< y1b->value() << endl; 173 | cout << "y2b="<< y2b->value() << endl; 174 | cout << "y1 derivatives = " << y1b->derivative(0) << " " << y1b->derivative(1) << endl; 175 | cout << "y2 derivatives = " << y2b->derivative(0) << " " << y2b->derivative(1) << endl; 176 | values[0] = 0.0; 177 | values[1] = 1.0; 178 | optimizer2.setInputValues( values ); 179 | cout << "method 2(cloned) with (x1=0,x2=1)" << endl; 180 | cout << "y1b="<< y1b->value() << endl; 181 | cout << "y2b="<< y2b->value() << endl; 182 | cout << "y1 derivatives = " << y1b->derivative(0) << " " << y1b->derivative(1) << endl; 183 | cout << "y2 derivatives = " << y2b->derivative(0) << " " << y2b->derivative(1) << endl; 184 | 185 | return 0; 186 | } 187 | -------------------------------------------------------------------------------- /examples/expressiontree_motion_profile.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * OBSOLETE: 3 | #include 4 | 5 | int main(int argc,char* argv[]) { 6 | using namespace KDL; 7 | using namespace std; 8 | 9 | Expression::Ptr time = input(1); 10 | 11 | double startpos = 1.0; 12 | double endpos = -4.0; 13 | double maxvel = 2.0; 14 | double maxacc = 3.0; 15 | Expression::Ptr s = trapezoidal_mp( time, startpos,endpos, maxvel, maxacc ); 16 | double duration = trapezoidal_mp_duration( startpos,endpos, maxvel, maxacc ); 17 | cerr << "duration = " << duration << endl; 18 | for (double t=0.0;t<4.0;t+=0.01) { 19 | s->setInputValue(1,t); 20 | double sval = s->value(); 21 | double sder = s->derivative(1); 22 | cout << t << "\t" << sval << "\t" << sder << "\n"; 23 | } 24 | 25 | return 0; 26 | } 27 | **/ 28 | 29 | #include 30 | 31 | int main(int argc, char* argv[]) { 32 | using namespace KDL; 33 | using namespace std; 34 | 35 | MotionProfileTrapezoidal::Ptr mp = create_motionprofile_trapezoidal(); 36 | Expression::Ptr time = input(1); 37 | mp->setProgressExpression(time); 38 | 39 | Expression::Ptr start1 = input(2); 40 | Expression::Ptr end1 = input(3); 41 | Expression::Ptr maxvel1 = input(4); 42 | Expression::Ptr maxacc1 = input(5); 43 | mp->addOutput(start1,end1,maxvel1,maxacc1); 44 | 45 | Expression::Ptr start2 = input(6); 46 | Expression::Ptr end2 = input(7); 47 | Expression::Ptr maxvel2 = input(8); 48 | Expression::Ptr maxacc2 = input(9); 49 | mp->addOutput(start2,end2,maxvel2,maxacc2); 50 | 51 | Expression::Ptr output1 = get_output_profile( mp, 0); 52 | Expression::Ptr output2 = get_output_profile( mp, 1); 53 | Expression::Ptr duration = get_duration( mp); 54 | 55 | // setInputValue for one of the outputs is enough... 56 | output1->setInputValue(2,0.0); 57 | output1->setInputValue(3,4.0); 58 | output1->setInputValue(4,1.0); 59 | output1->setInputValue(5,0.25); 60 | 61 | output1->setInputValue(6,3.0); 62 | output1->setInputValue(7,0.0); 63 | output1->setInputValue(8,0.8); 64 | output1->setInputValue(9,0.8); 65 | double dt = 0.01; 66 | for (double time = 0; time < duration->value(); time+=dt) { 67 | output1->setInputValue(1,time); 68 | cout << time << "\t" << output1->value() << "\t" << output1->derivative(1) << "\t" 69 | << output2->value() << "\t" << output2->derivative(1) << endl; 70 | } 71 | cerr << mp->mp[0].s << endl; 72 | cerr << mp->mp[1].s << endl; 73 | 74 | 75 | 76 | return 0; 77 | } 78 | 79 | -------------------------------------------------------------------------------- /examples/expressiontree_motion_profile2.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * OBSOLETE: 3 | #include 4 | 5 | int main(int argc,char* argv[]) { 6 | using namespace KDL; 7 | using namespace std; 8 | 9 | Expression::Ptr time = input(1); 10 | 11 | double startpos = 1.0; 12 | double endpos = -4.0; 13 | double maxvel = 2.0; 14 | double maxacc = 3.0; 15 | Expression::Ptr s = trapezoidal_mp( time, startpos,endpos, maxvel, maxacc ); 16 | double duration = trapezoidal_mp_duration( startpos,endpos, maxvel, maxacc ); 17 | cerr << "duration = " << duration << endl; 18 | for (double t=0.0;t<4.0;t+=0.01) { 19 | s->setInputValue(1,t); 20 | double sval = s->value(); 21 | double sder = s->derivative(1); 22 | cout << t << "\t" << sval << "\t" << sder << "\n"; 23 | } 24 | 25 | return 0; 26 | } 27 | **/ 28 | 29 | #include 30 | 31 | void compare_autodiff_numeric( const std::string& msg,KDL::Expression::Ptr e, double value, int idx, double dx) { 32 | e->setInputValue(idx,value); 33 | double v1 = e->value(); 34 | e->setInputValue(idx,value+dx); 35 | double v2 = e->value(); 36 | double numeric = (v2-v1)/dx; 37 | double autodiff= e->derivative(idx); 38 | std::cerr << msg << " : numeric="<< numeric << " and autodiff="<::Ptr time = input(1); 49 | mp->setProgressExpression(time); 50 | 51 | Expression::Ptr start1 = input(2); 52 | Expression::Ptr end1 = input(3); 53 | Expression::Ptr maxvel1 = input(4); 54 | Expression::Ptr maxacc1 = input(5); 55 | mp->addOutput(start1,end1,maxvel1,maxacc1); 56 | 57 | Expression::Ptr start2 = input(6); 58 | Expression::Ptr end2 = input(7); 59 | Expression::Ptr maxvel2 = input(8); 60 | Expression::Ptr maxacc2 = input(9); 61 | mp->addOutput(start2,end2,maxvel2,maxacc2); 62 | 63 | Expression::Ptr output1 = get_output_profile( mp, 0); 64 | Expression::Ptr output2 = get_output_profile( mp, 1); 65 | Expression::Ptr duration = get_duration( mp); 66 | 67 | // setInputValue for one of the outputs is enough... 68 | output1->setInputValue(2,0.0); 69 | output1->setInputValue(3,4.0); 70 | output1->setInputValue(4,1.0); 71 | output1->setInputValue(5,0.25); 72 | 73 | output1->setInputValue(6,3.0); 74 | output1->setInputValue(7,0.0); 75 | output1->setInputValue(8,0.8); 76 | output1->setInputValue(9,0.8); 77 | /* 78 | cerr << "outputs the derivative towards the end position of 1 " << endl; 79 | int idx=3; 80 | double dt = 0.01; 81 | for (double time = 0; time < duration->value(); time+=dt) { 82 | output1->setInputValue(1,time); 83 | cout << time << "\t" << output1->value() << "\t" << output1->derivative(idx) << "\t" 84 | << output2->value() << "\t" << output2->derivative(idx) << endl; 85 | } 86 | */ 87 | output1->setInputValue(1,4.2); 88 | compare_autodiff_numeric("end value ",output1, 4.0, 3, 1E-8); 89 | return 0; 90 | } 91 | -------------------------------------------------------------------------------- /examples/expressiontree_optimizer.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file expressiontree_example6.cpp 3 | * \brief example of using ExpressionTree in combination with KDL::Chain. 4 | * 5 | * \Author: Sept. 2012, Erwin Aertbelien 6 | * 7 | ***************************************************************************/ 8 | 9 | 10 | #include 11 | #include 12 | 13 | int main(int argc, char* argv[]) { 14 | using namespace KDL; 15 | using namespace std; 16 | double L1=0.310; 17 | double L2=0.400; 18 | double L3=0.390; 19 | double L4=0.078; 20 | 21 | Chain chain; 22 | chain.addSegment(Segment("Segment 0", Joint("Joint 0", Joint::RotZ),Frame(Vector(0,0,L1)))); 23 | chain.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotX),Frame(Vector(0,0,0)))); 24 | chain.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotZ),Frame(Vector(0,0,L2)))); 25 | chain.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotX),Frame(Vector(0,0,0)))); 26 | chain.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotZ),Frame(Vector(0,0,L3)))); 27 | chain.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::RotX),Frame(Vector(0,0,L4)))); 28 | chain.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotZ),Frame(Vector(0,0,0)))); 29 | 30 | int robotvar; 31 | robotvar=0; 32 | std::string base_name="robot1"; 33 | Expression::Ptr robot1 = cached( 34 | base_name +":ee", 35 | cached(base_name+":wrist", 36 | cached(base_name+":elbow", 37 | cached(base_name+":shoulder", 38 | frame( rot_z(input(robotvar)), Constant(Vector(0,0,L1))) * 39 | frame( rot_y(-input(robotvar+1)) ) * 40 | frame( rot_z(input(robotvar+2) ) ) 41 | ) * 42 | frame( rot_y(input(robotvar+3) ), Constant(Vector(0,0,L2))) 43 | ) * 44 | frame( rot_z(input(robotvar+4) ), Constant(Vector(0,0,L3))) 45 | ) * 46 | frame( rot_y(-input(robotvar+5) ) ) * 47 | frame( rot_z(input(robotvar+6) ), Constant(Vector(0,0,L4)) ) 48 | ); 49 | robotvar=7; 50 | base_name="robot2"; 51 | Expression::Ptr robot2 = cached( 52 | base_name +":ee", 53 | cached(base_name+":wrist", 54 | cached(base_name+":elbow", 55 | cached(base_name+":shoulder", 56 | frame( rot_z(input(robotvar)), Constant(Vector(0,0,L1))) * 57 | frame( rot_y(-input(robotvar+1)) ) * 58 | frame( rot_z(input(robotvar+2) ) ) 59 | ) * 60 | frame( rot_y(input(robotvar+3) ), Constant(Vector(0,0,L2))) 61 | ) * 62 | frame( rot_z(input(robotvar+4) ), Constant(Vector(0,0,L3))) 63 | ) * 64 | frame( rot_y(-input(robotvar+5) ) ) * 65 | frame( rot_z(input(robotvar+6) ), Constant(Vector(0,0,L4)) ) 66 | ); 67 | 68 | 69 | // base at [0 0 1], tool at [0 0 0.3] wrt mounting plate: 70 | // chain is copied when using kinematic_chain(...) 71 | Expression::Ptr kinchain1 = cached( 72 | Constant(Frame(Vector(0,0,1))) * robot1 * Constant(Frame(Vector(0,0,0.3))) 73 | ); 74 | // base at [0 0 0], tool at [0 0 0.3] wrt mounting plate: 75 | Expression::Ptr kinchain2 = cached( 76 | Constant(Frame(Vector(0,0,0))) * robot2 * Constant(Frame(Vector(0,0,0.3))) 77 | ); 78 | 79 | 80 | // z-axes of the end effector of the 2 robots are perpendicular to each other: 81 | Expression::Ptr perpendicular = cached(dot(unit_z( rotation( kinchain1 )), unit_z(rotation( kinchain2 ) ))); 82 | 83 | std::vector jval(14); 84 | for (int i=0;i(perpendicular, 3,jval[3]) << "\n"; 86 | 87 | 88 | for (int i=0;isetInputValues(jval); 90 | perpendicular->derivative(3); 91 | 92 | perpendicular->setInputValue(3,0.05); 93 | cout << perpendicular->value() << "\t"; 94 | cout << perpendicular->derivative(3) << "\n"; 95 | perpendicular->setInputValue(3,1.0); 96 | cout << perpendicular->value() << "\t"; 97 | cout << perpendicular->derivative(3) << "\n"; 98 | 99 | ExpressionOptimizer optimizer; 100 | std::vector variablelist; 101 | variablelist.push_back(3); 102 | optimizer.prepare( variablelist ); 103 | perpendicular->addToOptimizer(optimizer); 104 | std::vector values; 105 | values.resize(1); 106 | values[0] = 0.05; 107 | optimizer.setInputValues( values ); 108 | cout << perpendicular->value() << "\t"; 109 | cout << perpendicular->derivative(3) << "\n"; 110 | values[0] = 1.0; 111 | optimizer.setInputValues( values ); 112 | cout << perpendicular->value() << "\t"; 113 | cout << perpendicular->derivative(3) << "\n"; 114 | 115 | std::ofstream of("expressiontree_optimizer.dot"); 116 | perpendicular->write_dotfile(of); 117 | 118 | return 0; 119 | } 120 | -------------------------------------------------------------------------------- /examples/expressiontree_perpendicular.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file expressiontree_example6.cpp 3 | * \brief example of using ExpressionTree in combination with KDL::Chain. 4 | * 5 | * \Author: Sept. 2012, Erwin Aertbelien 6 | ***************************************************************************/ 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | /* 13 | * An example that investigates the computation time of expression tree's. 14 | * takes a 30 dof kin chain and some additional expressions and evaluates 15 | * the computation time. 16 | * 17 | */ 18 | int main(int argc, char* argv[]) { 19 | using namespace KDL; 20 | using namespace std; 21 | double L1=0.310; 22 | double L2=0.400; 23 | double L3=0.390; 24 | double L4=0.078; 25 | 26 | Chain chain; 27 | chain.addSegment(Segment("Segment 0", Joint("Joint 0", Joint::RotZ),Frame(Vector(0,0,L1)))); 28 | chain.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotX),Frame(Vector(0,0,0)))); 29 | chain.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotZ),Frame(Vector(0,0,L2)))); 30 | chain.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotX),Frame(Vector(0,0,0)))); 31 | chain.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotZ),Frame(Vector(0,0,L3)))); 32 | chain.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::RotX),Frame(Vector(0,0,L4)))); 33 | chain.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotZ),Frame(Vector(0,0,0)))); 34 | 35 | // base at [0 0 1], tool at [0 0 0.3] wrt mounting plate: 36 | // chain is copied when using kinematic_chain(...) 37 | Expression::Ptr kinchain1 = cached( 38 | Constant(Frame(Vector(0,0,1))) * kinematic_chain( chain, 0 ) * Constant(Frame(Vector(0,0,0.3))) 39 | ); 40 | // base at [0 0 0], tool at [0 0 0.3] wrt mounting plate: 41 | Expression::Ptr kinchain2 = cached( 42 | Constant(Frame(Vector(0,0,0))) * kinematic_chain( chain, 7 ) * Constant(Frame(Vector(0,0,0.3))) 43 | ); 44 | 45 | 46 | // z-axes of the end effector of the 2 robots are perpendicular to each other: 47 | Expression::Ptr perpendicular = dot(unit_z( rotation( kinchain1 )), unit_z(rotation( kinchain2 ) )); 48 | Expression::Ptr angle = acos(dot(unit_z( rotation( kinchain1 )), unit_z(rotation( kinchain2 ) ))); 49 | 50 | // write to a .dot file for visualization: 51 | std::ofstream of("expressiontree_perpendicular.dot"); 52 | std::ofstream of2("expressiontree_angle.dot"); 53 | perpendicular->write_dotfile(of); 54 | angle->write_dotfile(of2); 55 | 56 | 57 | return 0; 58 | } 59 | -------------------------------------------------------------------------------- /examples/expressiontree_sensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * \file expressiontree_sensor.cpp 3 | * 4 | * Created on: Dec., 2012 5 | * Author: Erwin Aertbelien 6 | ***************************************************************************/ 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | using namespace KDL; 15 | 16 | 17 | 18 | 19 | class NoiseGen { 20 | boost::mt19937 rng; 21 | boost::normal_distribution<> nd; 22 | boost::variate_generator > var_nor; 23 | public: 24 | NoiseGen():nd(0,0.03), var_nor(rng,nd) {} 25 | double operator()() { 26 | return var_nor(); 27 | } 28 | }; 29 | 30 | 31 | /** 32 | * You can construct a expression graph node for a sensor 33 | * by definining a unary eg node and caching it and making 34 | * it depended on time. Then, each time the time variable changes 35 | * (and only then) the expression is read out again... 36 | */ 37 | class Sensor: 38 | public UnaryExpression 39 | { 40 | public: 41 | typedef UnaryExpression UnExpr; 42 | double val; 43 | NoiseGen noise; 44 | public: 45 | Sensor() {} 46 | 47 | Sensor(const UnExpr::ArgumentExpr::Ptr& arg): 48 | UnExpr("sensor",arg) 49 | {} 50 | 51 | virtual double value() { 52 | val = 12.3+noise(); // read out sensor 53 | return val; 54 | } 55 | 56 | virtual double derivative(int i) { 57 | return 0; 58 | } 59 | 60 | virtual Expression::Ptr derivativeExpression(int i) { 61 | return Constant(0.0); 62 | } 63 | 64 | virtual UnExpr::Ptr clone() { 65 | Expression::Ptr expr( 66 | new Sensor( argument->clone()) 67 | ); 68 | return expr; 69 | } 70 | }; 71 | 72 | inline Expression::Ptr getSensor() { 73 | // this assumes that the time variable has variable index 0. 74 | Expression::Ptr expr( 75 | new Sensor( input(0) ) 76 | ); 77 | return cached(expr); 78 | } 79 | 80 | 81 | /* 82 | * Example that demonstrates how to encapsulate a sensor. 83 | */ 84 | int main(int argc, char* argv[]) { 85 | using namespace KDL; 86 | using namespace std; 87 | 88 | Expression::Ptr e = getSensor(); 89 | std::cout << e->value() << std::endl; 90 | std::cout << e->value() << std::endl; 91 | e->setInputValue(0,0.1); 92 | std::cout << e->value() << std::endl; 93 | std::cout << e->value() << std::endl; 94 | e->setInputValue(0,0.2); 95 | std::cout << e->value() << std::endl; 96 | } 97 | 98 | -------------------------------------------------------------------------------- /examples/expressiontree_variabletype.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiontree_variabletype.cpp 3 | * 4 | * Created on: Aug 7, 2012 5 | * Author: Wouter Bancken 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #include 25 | 26 | /* 27 | * Tutorial on the use of VariableType 28 | * It shows: 29 | * - How does it behave w.r.t. cloning 30 | * - How you change the value of expressions. 31 | * - It is important to always depend on some variable, even when the VariableType 32 | * really expresses a constant value. Otherwise you cannot indicate a change of the 33 | * VariableType 34 | */ 35 | int main(int argc, char* argv[]) { 36 | using namespace KDL; 37 | 38 | //--------------------------------------------------------------------- 39 | //1. The use of VariableType: 40 | 41 | // create a VariableType dependend on two variables starting from index 0 42 | // (i.e. input(0) and input(1) variables 43 | VariableType::Ptr v = Variable(0,2); 44 | v->setValue( Rotation::EulerZYX(0.0, 0.0, 0.0) ); 45 | v->setJacobian(0, KDL::Vector(0,0,1) ); // be carefull: "0" refers to the 0-th column of the Jacobian, not to the 0-th variable. 46 | v->setJacobian(1, KDL::Vector(1,0,0) ); 47 | std::cout << std::endl; 48 | // display the amount of dependend variables 49 | std::cout << v->ndx.size() << std::endl; 50 | // list the dependencies (can be done for any expression): 51 | std::cout << "Dependencies "; 52 | std::set s; 53 | v->getDependencies(s); 54 | for (std::set::iterator it = s.begin();it!=s.end();++it) { 55 | std::cout << *it << " "; 56 | } 57 | std::cout << std::endl; 58 | // use it in an expression (where there is a cached node): 59 | Expression::Ptr result; 60 | result = cached(Constant(Rotation::EulerZYX(0.0, -M_PI/2, 0.0))*v); 61 | std::cout << "The result before change the variable " << std::endl; 62 | std::cout << result->value() << std::endl; 63 | // change the value of the VariableType 64 | v->setValue( Rotation::EulerZYX(0.3,0.3,0.2)); 65 | std::cout << "VariableType value : " << std::endl; 66 | std::cout << v->value() << std::endl; 67 | std::cout << "The next result is wrong :" << std::endl; 68 | std::cout << result->value() << std::endl; // WRONG RESULT ! The expression is not warned about the change in VariableType! 69 | // and the cached variable is therefore not updated. 70 | std::cout << "The next result is correct :" << std::endl; 71 | result->setInputValue(0,0.0); 72 | std::cout << result->value() << std::endl; // by touching one of the dependant variables, the cached variables are updated and now 73 | // the result is correct. 74 | std::cout << "expression tree:\n"; 75 | result->print(std::cout); 76 | std::cout << std::endl; 77 | // In conclusion, the dependant variables of a VariableType serve two purposes: 78 | // a) indicated dependencies and when to update values 79 | // b) indicate variables for which there is/can be non-zero derivatives 80 | 81 | 82 | //--------------------------------------------------------------------- 83 | 84 | std::cout << "\n\nvalue \n"; 85 | std::cout << result->value(); 86 | std::cout << "\n\nderivative 0 \n"; 87 | std::cout << result->derivative(0); 88 | std::cout << "\n\nderivative 1 \n"; 89 | std::cout << result->derivative(1); 90 | std::cout << "\n\nderivative 2 \n"; 91 | std::cout << result->derivative(2); 92 | std::cout << " \n\nnumber of derivatives "; 93 | std::cout << result->number_of_derivatives(); 94 | std::cout << "\n\n"; 95 | 96 | // cloning the expression: 97 | Expression::Ptr cloned_result = result->clone(); 98 | std::cout << "Value of result " << std::endl; 99 | std::cout << result->value() << std::endl; 100 | std::cout << "Value of cloned result "<< std::endl; 101 | std::cout << cloned_result->value() << std::endl; 102 | std::cout << "Change the original VariableType " << std::endl; 103 | v->setValue( Rotation::EulerZYX(0.5, 1.0, -0.2) ); 104 | result->setInputValue(0,0); 105 | cloned_result->setInputValue(0,0); 106 | std::cout << "Value of result after clone and change of variable value" << std::endl; 107 | std::cout << result->value() << std::endl; 108 | std::cout << "Value of cloned result after clone and change of variable value"<< std::endl; 109 | std::cout << "(the value is now decoupled because the clone) " << std::endl; 110 | std::cout << cloned_result->value() << std::endl; 111 | std::cout << "But a clone of a VariableType can be updated with the state of the " << std::endl; 112 | std::cout << "original where it is cloned from" << std::endl; 113 | cloned_result->update_variabletype_from_original(); 114 | std::cout << cloned_result->value() << std::endl; 115 | /* second example: */ 116 | VariableType::Ptr vec = Variable(1,1); 117 | vec->setValue( KDL::Vector(4,5,6) ); 118 | vec->setJacobian(0, KDL::Vector(0,1,0) ); 119 | Expression::Ptr result2 = norm(vec); 120 | 121 | std::cout << "expression tree:\n"; 122 | result2->debug_printtree(); 123 | std::cout << std::endl; 124 | 125 | std::cout << "\n\nvalue \n"; 126 | std::cout << result2->value(); 127 | std::cout << "\n\nderivative 0 \n"; 128 | std::cout << result2->derivative(0); 129 | std::cout << "\n\nderivative 1 \n"; 130 | std::cout << result2->derivative(1); 131 | std::cout << "\n\n"; 132 | return 0; 133 | } 134 | -------------------------------------------------------------------------------- /examples/geometry.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | int main() 5 | { 6 | 7 | //Creating Vectors 8 | KDL::Vector v1;//Default constructor 9 | KDL::Vector v2(1.0,2.0,3.0);//Most used constructor 10 | KDL::Vector v3(v2);//Copy constructor 11 | KDL::Vector v4 = KDL::Vector::Zero();//Static member 12 | 13 | //Use operator << to print the values of your vector 14 | std::cout<<"v1 ="< 2 | 3 | /* 4 | * Example on how to use the initial_value function. 5 | * The initial_value functions takes time and another argument x. 6 | * When time <= 0, the argument x is cached. 7 | * When time >= 0, the cached argument is used instead of the current value of x. 8 | * 9 | * So, to have the correct semantics (i.e. an initial value of an expression), time has 10 | * to monotoneously increase and start from zero, in your program. 11 | */ 12 | int main(int argc, char* argv[]) { 13 | using namespace KDL; 14 | 15 | Expression::Ptr time = input(0); 16 | 17 | Expression::Ptr x = cached( cos(time) ); 18 | 19 | Expression::Ptr y = x-initial_value(time,x); 20 | 21 | for (double t=0;t < 2.0; t+= 0.1) { 22 | x->setInputValue(0,t); 23 | y->setInputValue(0,t); 24 | double val = y->value(); 25 | double valx = x->value(); 26 | double dval = y->derivative(0); 27 | std::cout << t << "\t" << val << "\t" << valx << "\t" << dval << std::endl; 28 | } 29 | 30 | return 0; 31 | } 32 | -------------------------------------------------------------------------------- /examples/matrix_traits.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | int main() { 6 | 7 | using namespace KDL; 8 | using namespace std; 9 | using namespace Eigen; 10 | 11 | 12 | typedef Matrix Mat; 13 | typedef Matrix MatA; 14 | typedef Matrix MatB; 15 | 16 | cout << demangle(typeid( AutoDiffTrait::ValueType ).name()) << endl; 17 | cout << demangle(typeid( AutoDiffTrait::DerivType ).name()) << endl; 18 | cout << AutoDiffTrait::zeroDerivative() << endl; 19 | cout << AutoDiffTrait::size << endl; 20 | cout << "A="<< endl; 21 | MatA A; 22 | A << 1,2,3, 23 | 4,5,2, 24 | -1,3,2; 25 | cout << A << endl; 26 | MatB B; 27 | cout << "B="<< endl; 28 | B << 3,4, 29 | 2,1, 30 | 5,2; 31 | cout << B << endl; 32 | Expression< MatA>::Ptr a = Constant< MatA >( A ); 33 | Expression< MatB>::Ptr b = Constant< MatB >( B ); 34 | cout << "A*B+B expression " << endl; 35 | Expression< MatB>::Ptr res = cached< MatB>( addition<3,2>( multiply<3,3,2>(a,b), b) ); 36 | res->print(cout); 37 | cout << endl; 38 | cout << "value = " << endl; 39 | cout << res->value(); 40 | cout << endl; 41 | cout << "element out of result " << endl; 42 | Expression::Ptr e = get_element<3,2>(1,0, res); 43 | e->print(cout); 44 | cout << "value: " << endl; 45 | cout << e->value() << endl; 46 | }; 47 | -------------------------------------------------------------------------------- /examples/mptrap_tst.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // small program to verify functionality of MPTrap 3 | // 4 | // Typically this class is not used directly, but the expressiongraph node 5 | // MotionProfileTrapezoidal is used. 6 | // 7 | #include 8 | 9 | using namespace KDL; 10 | using namespace std; 11 | 12 | int main() { 13 | MPTrap mp; 14 | double vmax = 1.0; 15 | double amax = 0.5; 16 | mp.setPlan(0.0, 4.0, vmax, amax); 17 | double duration = mp.planMinDuration(); 18 | mp.compute_derivs(); 19 | //mp.adaptDuration(8); 20 | /*for (double time=0.;time < mp.duration; time+=0.01) { 21 | cout << time << "\t" << mp.pos(time) << "\t" << mp.d_pos_d_time(time) << "\n"; 22 | }*/ 23 | 24 | cout << " ==== derivatives towards end position :" << endl; 25 | double dx = 1E-8; 26 | MPTrap mp2; 27 | mp2.setPlan(0.0, 4.0+dx, vmax, amax); 28 | mp2.planMinDuration(); 29 | cout << "at time=1 : " << mp.pos(1) << endl; 30 | cout << "at time=1+dt : " << mp2.pos(1) << endl; 31 | cout << "num. der. : " << (mp2.pos(1)-mp.pos(1))/dx << endl; 32 | cout << "autodiff. : " << mp.d_pos_d_epos(1) << endl; 33 | 34 | cout << "at time=3 : " << mp.pos(3) << endl; 35 | cout << "at time=3+dt : " << mp2.pos(3) << endl; 36 | cout << "num. der. : " << (mp2.pos(3)-mp.pos(3))/dx << endl; 37 | cout << "autodiff. : " << mp.d_pos_d_epos(3) << endl; 38 | 39 | cout << "num. der. duration " << (mp2.duration - mp.duration)/dx << endl; 40 | cout << "autodiff duration " << mp.d_duration_d_dpos << endl; 41 | cout << "at time=5 : " << mp.pos(5) << endl; 42 | cout << "at time=5+dt : " << mp2.pos(5) << endl; 43 | cout << "num. der. : " << (mp2.pos(5)-mp.pos(5))/dx << endl; 44 | cout << "autodiff. : " << mp.d_pos_d_epos(5) << endl; 45 | cout << " ==== derivatives towards start position :" << endl; 46 | dx = 1E-8; 47 | mp2.setPlan(0.0+dx, 4.0, vmax, amax); 48 | mp2.planMinDuration(); 49 | cout << "at time=1 : " << mp.pos(1) << endl; 50 | cout << "at time=1+dt : " << mp2.pos(1) << endl; 51 | cout << "num. der. : " << (mp2.pos(1)-mp.pos(1))/dx << endl; 52 | cout << "autodiff. : " << mp.d_pos_d_spos(1) << endl; 53 | 54 | cout << "at time=3 : " << mp.pos(3) << endl; 55 | cout << "at time=3+dt : " << mp2.pos(3) << endl; 56 | cout << "num. der. : " << (mp2.pos(3)-mp.pos(3))/dx << endl; 57 | cout << "autodiff. : " << mp.d_pos_d_spos(3) << endl; 58 | 59 | cout << "num. der. duration " << (mp2.duration - mp.duration)/dx << endl; 60 | cout << "autodiff duration " << -mp.d_duration_d_dpos << endl; 61 | cout << "at time=5 : " << mp.pos(5) << endl; 62 | cout << "at time=5+dt : " << mp2.pos(5) << endl; 63 | cout << "num. der. : " << (mp2.pos(5)-mp.pos(5))/dx << endl; 64 | cout << "autodiff. : " << mp.d_pos_d_spos(5) << endl; 65 | 66 | return 0; 67 | } 68 | -------------------------------------------------------------------------------- /examples/plotframe.m: -------------------------------------------------------------------------------- 1 | function plotframe(T,scale) 2 | if nargin <2 3 | scale=0.2; 4 | end 5 | 6 | 7 | origin=T*[0;0;0;1]; 8 | x = T*[scale;0;0;1]; 9 | y = T*[0;scale;0;1]; 10 | z = T*[0;0;scale;1]; 11 | 12 | line([origin(1) x(1)],[origin(2) x(2)],[origin(3) x(3)],'color','red','linewidth',2); 13 | line([origin(1) y(1)],[origin(2) y(2)],[origin(3) y(3)],'color','green','linewidth',2); 14 | line([origin(1) z(1)],[origin(2) z(2)],[origin(3) z(3)],'color','blue','linewidth',2); -------------------------------------------------------------------------------- /examples/resolved_motion_rate_control.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | using namespace KDL; 7 | 8 | /** 9 | // for now: only a version with scalar K 10 | void fill(Expression::Ptr arg, int row, const Frame& desired, const Twist& desired_dot, double K, Eigen::MatrixXd& J, Eigen::VectorXd& rhs) 11 | { 12 | arg->value(); // obligatory 13 | rhs.block(row,1,6,1) = toEigen( K*diff(arg->value(),desired) + desired_dot ); 14 | int N = min(J.cols(),arg->number_of_derivatives()); 15 | for (int i=0;iderivative(i)); 17 | } 18 | } 19 | */ 20 | 21 | /** 22 | * @TODO: Bring EulerAngles to Expression Trees 23 | * @TODO: Inequality solver 24 | * @TODO: Multiple priority solver 25 | */ 26 | class SimpleSolver { 27 | Eigen::MatrixXd JTJ; 28 | Eigen::LLT llt; // for Cholesky decomposition 29 | Eigen::MatrixXd J; 30 | Eigen::VectorXd rhs; 31 | int row; 32 | public: 33 | SimpleSolver(int constraints, int nvar) : JTJ(nvar,nvar), llt(nvar), J(constraints,nvar), rhs(constraints) { 34 | row = 0; 35 | } 36 | 37 | void reset() { 38 | row = 0; 39 | } 40 | 41 | void add(Expression::Ptr arg, double K,double desired=0.0, double desired_dot=0.0 ) { 42 | rhs(row) = K*(desired - arg->value()) + desired_dot; 43 | int N = min(J.cols(),arg->number_of_derivatives()); 44 | for (int i=0;iderivative(i); 46 | } 47 | row+=1; 48 | } 49 | 50 | void solve(Eigen::VectorXd& qdot) { 51 | // a somewhat naieve DLS-solver: 52 | JTJ = J.transpose()*J; 53 | double lambda=0.001; 54 | for (int j=0;j::Ptr kinchain = cached( 85 | Constant(Frame(Vector(0,0,0.5))) * kinematic_chain( chain, 0 ) * Constant(Frame(Vector(0,0,0.3))) 86 | ); 87 | Expression::Ptr line = Constant( Frame(Rotation::Identity(), Vector(0.2,0.2,0.0))); // z axis of frame corresponds to line. 88 | // constraint 1: 89 | Expression::Ptr tmp = cached( inv(line) * origin(kinchain) ); 90 | Expression::Ptr distance_to_line = coord_x(tmp)*coord_x(tmp) + coord_y(tmp)*coord_y(tmp) - Constant(0.1*0.1); 91 | // constraint 2: 92 | Expression::Ptr perpendicular_to_line = dot( unit_z(rotation(line)), unit_z(rotation(kinchain))); 93 | // constraint 3: 94 | Expression::Ptr fixed_joint = input(2) - Constant(0.5); 95 | 96 | // solving: 97 | int nvar = 7; 98 | std::vector joints(nvar); 99 | joints[0] = 0*M_PI*0.08; 100 | joints[1] = 1*M_PI*0.08; 101 | joints[2] = 0.5; 102 | joints[3] = 3*M_PI*0.08; 103 | joints[4] = 4*M_PI*0.08; 104 | joints[5] = 5*M_PI*0.08; 105 | joints[6] = 6*M_PI*0.08; 106 | Eigen::VectorXd qdot(nvar); 107 | SimpleSolver solver(3,nvar); 108 | double dt=0.01; 109 | double K = 4; 110 | for (double t=0;t<10;t+=dt) { 111 | solver.reset(); 112 | distance_to_line->setInputValues(joints); 113 | solver.add(distance_to_line, K); 114 | perpendicular_to_line->setInputValues(joints); 115 | solver.add(perpendicular_to_line, K); 116 | fixed_joint->setInputValues(joints); 117 | solver.add(fixed_joint, K); 118 | 119 | solver.solve(qdot); 120 | 121 | // integration and print-out 122 | cout << distance_to_line->value() << "\t" 123 | << perpendicular_to_line->value() << "\t" 124 | << fixed_joint->value() << "\t"; 125 | for (int i=0;i 2 | #include 3 | #include 4 | 5 | int main(int argc, char* argv[]) { 6 | using namespace KDL; 7 | using namespace std; 8 | 9 | Expression::Ptr v = saturate(input(1),0.5,1.5); 10 | 11 | Expression::Ptr dv = v->derivativeExpression(1); 12 | for (double t=0.0; t < 2.0; t+=0.05 ) { 13 | // remember: the order of these method calls matter: 14 | v->setInputValue(1,t); 15 | double val = v->value(); 16 | double dval = v->derivative(1); 17 | // also evaluate the derivativeExpression, to compare it with the previous: 18 | dv->setInputValue(1,t); 19 | double dval2= dv->value(); 20 | double ddval= dv->derivative(1); 21 | cout << t << "\t" << val << "\t" << dval << "\t" << dval2 << "\t" << ddval <<"\n"; 22 | } 23 | return 0; 24 | } 25 | -------------------------------------------------------------------------------- /examples/solving_and_cloning.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file cloning.cpp 3 | * \brief example of cloning and Variable expressions as parameters 4 | * 5 | * \Author: Sept. 2016, Erwin Aertbelien 6 | * 7 | * 8 | * expressiongraph library 9 | * 10 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 11 | * 12 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 13 | * You may not use this work except in compliance with the Licence. 14 | * You may obtain a copy of the Licence at: 15 | * 16 | * http://ec.europa.eu/idabc/eupl 17 | * 18 | * Unless required by applicable law or agreed to in writing, software 19 | * distributed under the Licence is distributed on an "AS IS" basis, 20 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 21 | * See the Licence for the specific language governing permissions and 22 | * limitations under the Licence. 23 | */ 24 | 25 | 26 | 27 | /** 28 | * 29 | * This file demonstrates a technique that combines the use of Variable expressions and 30 | * cloning. The Variable expressions are cloned and are de facto constants after the clone 31 | * (because there is no access any more the change their value). This behaviour can be exploited 32 | * for e.g. in fitting applications. 33 | * 34 | **/ 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #include 44 | 45 | using namespace KDL; 46 | 47 | void solve( std::vector::Ptr> list, int time_ndx, const std::vector& ndx, Eigen::VectorXd& solution) { 48 | using namespace std; 49 | int m = list.size(); 50 | int n = ndx.size(); 51 | Eigen::VectorXd err(m); 52 | Eigen::MatrixXd Jac(m, n ); 53 | Eigen::JacobiSVD svd(m, n ); 54 | Eigen::VectorXd d_sol(n); 55 | 56 | for (int it=0;it<10;++it) { 57 | for (int i=0;isetInputValues(ndx,solution); 59 | list[i]->setInputValue(time_ndx,0); // to force the variable expr to be read (that depend on time_ndx). 60 | err(i) = list[i]->value(); 61 | for (int j=0;jderivative(ndx[j]); 63 | } 64 | } 65 | //cout << "error " << err.transpose() << endl; 66 | //cout << "Jacobian " << endl << Jac << endl; 67 | svd.compute(Jac,Eigen::ComputeThinU| Eigen::ComputeThinV ); 68 | d_sol = svd.solve(err); 69 | solution = solution - d_sol; 70 | cout << "solution at it="<::Ptr addMeasurement( 76 | Expression::Ptr model, VariableType::Ptr& x, VariableType::Ptr& y, 77 | double x_val, double y_val ) { 78 | x->val = x_val; 79 | y->val = y_val; 80 | return model->clone(); 81 | } 82 | 83 | 84 | int main() { 85 | using namespace std; 86 | // Building up the measurement model in function of a set of unknowns and some parameters x and y: 87 | VariableType::Ptr x = Variable(1,1); 88 | VariableType::Ptr y = Variable(1,1); 89 | Expression::Ptr u1 = input(2); 90 | Expression::Ptr u2 = input(3); 91 | Expression::Ptr model = u1*x+u2 - y; 92 | 93 | // Enter measurements: 94 | std::vector::Ptr> list; 95 | list.push_back( addMeasurement(model,x,y, 1, 2)); 96 | list.push_back( addMeasurement(model,x,y, 1.5, 2.2)); 97 | list.push_back( addMeasurement(model,x,y, 1.8, 2.3)); 98 | list.push_back( addMeasurement(model,x,y, 2, 2.5)); 99 | 100 | 101 | // Solve: 102 | std::vector ndx(2); 103 | ndx[0]=2;ndx[1]=3; 104 | Eigen::VectorXd solution(2); 105 | solution(0) = 1.4; 106 | solution(1) = 0; 107 | solve( list, 1, ndx, solution); 108 | 109 | return 0; 110 | } 111 | 112 | -------------------------------------------------------------------------------- /examples/tutorial1.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiontree_example3.cpp 3 | * 4 | * Created on: Sept., 2012 5 | * Author: Erwin Aertbelien 6 | * 7 | */ 8 | #include 9 | 10 | /** 11 | * An example of a expression in multiple variables. 12 | */ 13 | int main(int argc, char* argv[]) { 14 | using namespace KDL; 15 | using namespace std; 16 | 17 | // define expression: 18 | Expression::Ptr expr = Constant(2.0)*cos(input(0)) + Constant(3.0)*sin(input(1)); 19 | std::vector inp(2); 20 | inp[0] = 1; 21 | inp[1] = 2; 22 | 23 | // compute value and derivatives: 24 | expr->setInputValues(inp); 25 | cout << "Expression Tree in prefix notation : \n"; 26 | expr->print(std::cout); 27 | cout << "\n\nValue " << expr->value() << endl; 28 | cout << "Derivative towards var. 0 " << expr->derivative(0) << endl; 29 | cout << "Derivative towards var. 1 " << expr->derivative(1) << endl; 30 | 31 | // manually compute the derivative: 32 | double deriv1 = -2*sin(inp[0]); 33 | double deriv2 = 3*cos(inp[1]); 34 | cout << "Symbolically computed derivative towards var. 0 " << deriv1 << endl; 35 | cout << "Symbolically computed derivative towards var. 1 " << deriv2 << endl; 36 | 37 | // visualize as a tree: 38 | std::ofstream of("tutorial1.dot"); 39 | expr->write_dotfile(of); 40 | of.close(); 41 | 42 | return 0; 43 | } 44 | -------------------------------------------------------------------------------- /examples/tutorial2.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiontree_example3.cpp 3 | * 4 | * Created on: Sept., 2012 5 | * Author: Erwin Aertbelien 6 | * 7 | */ 8 | #include 9 | 10 | /** 11 | * An example of a expression in multiple variables. 12 | */ 13 | int main(int argc, char* argv[]) { 14 | using namespace KDL; 15 | using namespace std; 16 | 17 | double L1=0.310; 18 | double L2=0.400; 19 | double L3=0.390; 20 | double L4=0.078; 21 | 22 | // identical to the kinematic chain of taskfunctions3, but using 23 | // expression trees instead of kinematic chain, 24 | // and it exposes the intermediate elbow. 25 | Expression::Ptr elbow = cached( 26 | frame( rot_z(input(0)), Constant(Vector(0,0,L1))) * 27 | frame( rot_x(input(1)) ) * 28 | frame( rot_z(input(2) ) ) * 29 | frame( rot_x(input(3) ), Constant(Vector(0,0,L2)))); 30 | Expression::Ptr wrist = cached( 31 | elbow * 32 | frame( rot_z(input(4) ), Constant(Vector(0,0,L3)))); 33 | Expression::Ptr kinchain = cached( 34 | wrist * 35 | frame( rot_x(input(5) ) ) * 36 | frame( rot_z(input(6) ), Constant(Vector(0,0,L4)) ) 37 | ); 38 | 39 | std::vector joints(7); 40 | joints[0] = 0*M_PI*0.08; 41 | joints[1] = 30*M_PI/180.0; 42 | joints[2] = 0*M_PI/180.0; 43 | joints[3] = 120*M_PI/180.0; 44 | joints[4] = 0*M_PI/180.0; 45 | joints[5] = 30*M_PI/180.0; 46 | joints[6] = 0*M_PI/180.0; 47 | 48 | kinchain->setInputValues(joints); 49 | cout << "pose and Jacobian at the given joint position " << endl; 50 | display(cout, kinchain ); 51 | // visualize as a tree: 52 | ofstream of("tutorial2.dot"); 53 | kinchain->write_dotfile(of); 54 | of.close(); 55 | 56 | 57 | return 0; 58 | } 59 | -------------------------------------------------------------------------------- /examples/tutorial3.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiontree_example3.cpp 3 | * 4 | * Created on: Sept., 2012 5 | * Author: Erwin Aertbelien 6 | * 7 | ***************************************************************************/ 8 | 9 | #include 10 | 11 | /** 12 | * This tutorial is not finished. Correctness is not verified ! 13 | */ 14 | int main(int argc, char* argv[]) { 15 | using namespace KDL; 16 | using namespace std; 17 | 18 | double L1=0.310; 19 | double L2=0.400; 20 | double L3=0.390; 21 | double L4=0.078; 22 | 23 | // identical to the kinematic chain of taskfunctions3, but using 24 | // expression trees instead of kinematic chain, 25 | // and it exposes the intermediate elbow. 26 | Expression::Ptr elbow = cached( 27 | frame( rot_z(input(0)), Constant(Vector(0,0,L1))) * 28 | frame( rot_x(input(1)) ) * 29 | frame( rot_z(input(2) ) ) * 30 | frame( rot_x(input(3) ), Constant(Vector(0,0,L2)))); 31 | Expression::Ptr wrist = cached( 32 | elbow * 33 | frame( rot_z(input(4) ), Constant(Vector(0,0,L3)))); 34 | Expression::Ptr kinchain = cached( 35 | wrist * 36 | frame( rot_x(input(5) ) ) * 37 | frame( rot_z(input(6) ), Constant(Vector(0,0,L4)) ) 38 | ); 39 | 40 | std::vector joints(7); 41 | joints[0] = 0*M_PI*0.08; 42 | joints[1] = 30*M_PI/180.0; 43 | joints[2] = 0*M_PI/180.0; 44 | joints[3] = 120*M_PI/180.0; 45 | joints[4] = 0*M_PI/180.0; 46 | joints[5] = 30*M_PI/180.0; 47 | joints[6] = 0*M_PI/180.0; 48 | 49 | kinchain->setInputValues(joints); 50 | 51 | 52 | // supose there is a camera mounted at the mounting plate of the end effector 53 | Expression::Ptr camera = kinchain*Constant(Frame::Identity()); 54 | // with its main viewing axis along Z of the mounting plate: 55 | // the X-axis of the mounting plate corresponds to a horizontal line in the image. 56 | 57 | // horizon should be horizontal: 58 | Expression::Ptr e_1 = dot(unit_x(rotation(camera)), Constant(Vector(0,0,1))); 59 | 60 | // pointing towards a given point p, distance point p to center-axis camera: 61 | Expression::Ptr p = Constant(Vector(0.9,0.9,0)); 62 | Expression::Ptr d = cached( unit_z(rotation(camera))); 63 | Expression::Ptr e_2 = norm(d*(d*(p-origin(camera))) ); 64 | 65 | 66 | // camera image: 67 | Expression::Ptr p_cam = cached(inv(camera)*p); 68 | Expression::Ptr x_n = coord_x(p_cam)/coord_z(p_cam); 69 | Expression::Ptr y_n = coord_y(p_cam)/coord_y(p_cam); 70 | 71 | // camera parameters: 72 | double W = 640; 73 | double H = 480; 74 | double alpha_x = 20/180.0*M_PI; // openingsangle for x 75 | // camera model (http://en.wikipedia.org/wiki/Camera_resectioning): 76 | Expression::Ptr fc1 = Constant( W / tan(alpha_x) ); 77 | Expression::Ptr fc2 = fc1; 78 | Expression::Ptr alpha = Constant( 0.0 ); 79 | Expression::Ptr u0 = Constant( W/2 ); 80 | Expression::Ptr v0 = Constant( H/2 ); 81 | 82 | Expression::Ptr u = fc1*x_n + alpha*fc1*y_n + u0; 83 | Expression::Ptr v = fc2*y_n + v0; 84 | 85 | cout << "point in image, x-coordinate " << u->value() << endl; 86 | cout << "point in image, y-coordinate " << v->value() << endl; 87 | 88 | cout << "pose and Jacobian at the given joint position " << endl; 89 | display(cout, kinchain ); 90 | // visualize as a tree: 91 | ofstream of("tutorial3.dot"); 92 | kinchain->write_dotfile(of); 93 | of.close(); 94 | 95 | 96 | return 0; 97 | } 98 | -------------------------------------------------------------------------------- /include/kdl/conversions.hpp: -------------------------------------------------------------------------------- 1 | #ifndef KDL_CONVERSIONS_HPP 2 | #define KDL_CONVERSIONS_HPP 3 | /* 4 | * expressiongraph library 5 | * 6 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 7 | * 8 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 9 | * You may not use this work except in compliance with the Licence. 10 | * You may obtain a copy of the Licence at: 11 | * 12 | * http://ec.europa.eu/idabc/eupl 13 | * 14 | * Unless required by applicable law or agreed to in writing, software 15 | * distributed under the Licence is distributed on an "AS IS" basis, 16 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | * See the Licence for the specific language governing permissions and 18 | * limitations under the Licence. 19 | */ 20 | 21 | 22 | 23 | #include 24 | #include 25 | /** @file conversions.hpp 26 | * @brief Conversions to/from KDL types 27 | */ 28 | 29 | 30 | namespace KDL { 31 | /** \addtogroup conversions Conversion to/from KDL Types. 32 | * @{ 33 | */ 34 | 35 | /** 36 | * converts an Eigen vector to KDL::Vector 37 | */ 38 | inline Vector toKDLVector( const Eigen::Vector3d& arg) { 39 | Vector tmp( arg(0), arg(1), arg(2) ); 40 | return tmp; 41 | } 42 | 43 | 44 | /** 45 | * converts an Eigen vector to KDL::Vector 46 | * The Eigen vector should have a length of 3. 47 | */ 48 | inline Vector toKDLVector( const Eigen::VectorXd& arg) { 49 | assert( arg.rows()==3 ); 50 | Vector tmp( arg(0), arg(1), arg(2) ); 51 | return tmp; 52 | } 53 | 54 | /** 55 | * converts a KDL::Vector to Eigen vector (fixed size, allocated on stack, real-time) 56 | */ 57 | inline Eigen::Vector3d toEigen( const KDL::Vector& arg ) { 58 | Eigen::Vector3d tmp; 59 | tmp(0)=arg.x(); 60 | tmp(1)=arg.y(); 61 | tmp(2)=arg.z(); 62 | return tmp; 63 | } 64 | 65 | 66 | /** 67 | * converts an Eigen vector to KDL::Twist 68 | */ 69 | inline KDL::Twist toKDLTwist( const Eigen::Matrix& arg) { 70 | Twist tmp( Vector(arg(0), arg(1), arg(2)), Vector( arg(3),arg(4),arg(5)) ); 71 | return tmp; 72 | } 73 | 74 | 75 | /** 76 | * converts an Eigen vector to KDL::Twist 77 | * The Eigen vector should have a length of 6. 78 | */ 79 | inline KDL::Twist toKDLTwist( const Eigen::VectorXd& arg) { 80 | assert( arg.rows()==6 ); 81 | Twist tmp( Vector(arg(0), arg(1), arg(2)), Vector( arg(3),arg(4),arg(5)) ); 82 | return tmp; 83 | } 84 | 85 | /** 86 | * converts a KDL::Twist to Eigen vector (fixed size, allocated on stack, real-time) 87 | */ 88 | inline Eigen::Matrix toEigen( const KDL::Twist& arg ) { 89 | Eigen::Matrix tmp; 90 | tmp(0)=arg.vel.x(); 91 | tmp(1)=arg.vel.y(); 92 | tmp(2)=arg.vel.z(); 93 | tmp(3)=arg.rot.x(); 94 | tmp(4)=arg.rot.y(); 95 | tmp(5)=arg.rot.z(); 96 | return tmp; 97 | } 98 | 99 | /** 100 | * converts an Eigen vector to KDL::Wrench 101 | */ 102 | inline KDL::Wrench toKDLWrench( const Eigen::Matrix& arg) { 103 | Wrench tmp( Vector(arg(0), arg(1), arg(2)), Vector( arg(3),arg(4),arg(5)) ); 104 | return tmp; 105 | } 106 | 107 | /** 108 | * converts an Eigen vector to KDL::Wrench 109 | * The Eigen vector should have a length of 6. 110 | */ 111 | inline KDL::Wrench toKDLWrench( const Eigen::VectorXd& arg) { 112 | assert( arg.rows()==6 ); 113 | Wrench tmp( Vector(arg(0), arg(1), arg(2)), Vector( arg(3),arg(4),arg(5)) ); 114 | return tmp; 115 | } 116 | 117 | /** 118 | * converts a KDL::Wrench to Eigen vector (fixed size, allocated on stack, real-time) 119 | */ 120 | inline Eigen::Matrix toEigen( const KDL::Wrench& arg ) { 121 | Eigen::Matrix tmp; 122 | tmp(0)=arg.force.x(); 123 | tmp(1)=arg.force.y(); 124 | tmp(2)=arg.force.z(); 125 | tmp(3)=arg.torque.x(); 126 | tmp(4)=arg.torque.y(); 127 | tmp(5)=arg.torque.z(); 128 | return tmp; 129 | } 130 | 131 | /** 132 | * converts Eigen matrix 3x3 to a KDL::Rotation 133 | */ 134 | inline KDL::Rotation toKDLRotation(const Eigen::Matrix& arg) { 135 | KDL::Rotation R( 136 | arg(0,0), arg(0,1), arg(0,2), 137 | arg(1,0), arg(1,1), arg(1,2), 138 | arg(2,0), arg(2,1), arg(2,2) 139 | ); 140 | return R; 141 | } 142 | 143 | /** 144 | * converts Eigen matrix to a KDL::Rotation 145 | * size should be 3 x 3 146 | */ 147 | inline KDL::Rotation toKDLRotation(const Eigen::MatrixXd& arg) { 148 | assert(arg.rows()==3); 149 | assert(arg.cols()==3); 150 | KDL::Rotation R( 151 | arg(0,0), arg(0,1), arg(0,2), 152 | arg(1,0), arg(1,1), arg(1,2), 153 | arg(2,0), arg(2,1), arg(2,2) 154 | ); 155 | return R; 156 | } 157 | 158 | /** 159 | * converts a KDL::Rotation to an Eigen matrix 160 | */ 161 | inline Eigen::Matrix toEigen(const KDL::Rotation& arg) { 162 | Eigen::Matrix m; 163 | m.col(0) = toEigen( arg.UnitX() ); 164 | m.col(1) = toEigen( arg.UnitY() ); 165 | m.col(2) = toEigen( arg.UnitZ() ); 166 | return m; 167 | } 168 | 169 | /** 170 | * converts an Eigen Quaternion to a KDL::Rotation 171 | */ 172 | inline KDL::Rotation toKDLRotation(const Eigen::Quaternion& arg) { 173 | return toKDLRotation( arg.toRotationMatrix() ); 174 | } 175 | 176 | /** 177 | * converts a KDL::Rotation to an Eigen Quaternion 178 | */ 179 | inline Eigen::Quaternion toEigenQuaternion(const KDL::Rotation& arg) { 180 | return Eigen::Quaternion( toEigen(arg) ); 181 | } 182 | 183 | /** 184 | * @} 185 | * end of group conversions 186 | */ 187 | }; 188 | #endif 189 | -------------------------------------------------------------------------------- /include/kdl/expressiontree.hpp: -------------------------------------------------------------------------------- 1 | #ifndef KDL_EXPRESSIONTREE_HPP 2 | #define KDL_EXPRESSIONTREE_HPP 3 | 4 | /* 5 | * expressiongraph library 6 | * 7 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 8 | * 9 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 10 | * You may not use this work except in compliance with the Licence. 11 | * You may obtain a copy of the Licence at: 12 | * 13 | * http://ec.europa.eu/idabc/eupl 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the Licence is distributed on an "AS IS" basis, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the Licence for the specific language governing permissions and 19 | * limitations under the Licence. 20 | */ 21 | 22 | #include "expressiontree_expressions.hpp" 23 | #include "expressiontree_double.hpp" 24 | #include "expressiontree_frame.hpp" 25 | #include "expressiontree_rotation.hpp" 26 | #include "expressiontree_twist.hpp" 27 | #include "expressiontree_vector.hpp" 28 | #include "expressiontree_wrench.hpp" 29 | #include "expressiontree_chain.hpp" 30 | #include "expressiontree_var.hpp" 31 | #include "expressiontree_mimo.hpp" 32 | 33 | #endif 34 | 35 | -------------------------------------------------------------------------------- /include/kdl/expressiontree_chain.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiontree_chain.hpp 3 | * 4 | * Created on: Sept, 2012 5 | * Author: Erwin Aertbelien 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #ifndef KDL_EXPRESSIONTREE_CHAIN_HPP 25 | #define KDL_EXPRESSIONTREE_CHAIN_HPP 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | namespace KDL { 32 | 33 | 34 | /** 35 | * provides a mapping of a KDL::Chain to an ExpressionTree. 36 | */ 37 | class Expression_Chain: 38 | public FunctionType 39 | { 40 | Chain chain; 41 | std::vector jointndx_to_segmentndx; 42 | std::vector jval; 43 | std::vector T_base_jointroot; 44 | std::vector T_base_jointtip; 45 | Frame T_base_head; 46 | std::vector jacobian; 47 | std::vector cached_deriv; 48 | bool cached; 49 | int _number_of_derivatives; 50 | int index_of_first_joint; 51 | void initialize(); 52 | public: 53 | /** 54 | * - provide a chain, This chain will be copied and used within this Expression chain object. 55 | * - provide a mapping of the joints of the kinematic chain to the Expression tree variable number. 56 | * - also provides the default value of a joint, it is not filled in using jointndx_to_varndx, this 57 | * allows to keep certain joints constant of a kinematic chain. 58 | * - This class caches its results until a new call to setInputValue(s) (no need to use CachedType for 59 | * this functionality). 60 | * - The jointndx_to_varndx mechanism is parallel to the mechanism already built-in into chain, but more 61 | * powerfull, and is necessary to combine different chains in an expression. Additional constructors 62 | * are/will be provided for ease of use. 63 | * - The constructors are not real-time. 64 | * - This classes caches its result, no need to add a CachedType node. Can efficiently be called 65 | * multiple times. 66 | * 67 | * See documentation of other constructor. 68 | * uses a jointndx_to_varndx mapping that maps the joints of a chain 69 | * in a sequence starting at varndx_of_first_joint. 70 | */ 71 | Expression_Chain( const Chain& _chain, int varndx_of_first_joint ); 72 | 73 | virtual void setInputValues(const std::vector& values); 74 | 75 | virtual void setInputValue(int var, double value); 76 | virtual void setInputValue(int var, const Rotation& value) {} 77 | 78 | virtual Frame value(); 79 | 80 | virtual void addToOptimizer(ExpressionOptimizer& opt) { 81 | assert( 0 /* for now chain cannot be used together with optimization */ ); 82 | } 83 | 84 | virtual void getDependencies(std::set& varset) { 85 | for (size_t i=0;i& varset) { 90 | getDependencies(varset); 91 | } 92 | virtual void getRotDependencies(std::set& varset) { 93 | } 94 | 95 | virtual void update_variabletype_from_original() {} 96 | 97 | /** 98 | * This class caches the computation of the derivative 99 | */ 100 | virtual Twist derivative(int var_ndx); 101 | 102 | /** 103 | * compute 2nd derivative, first deriving towards first_var, then towards second_var 104 | * An additional method specific for Expression_Chain. 105 | * The second derivative is not cached. 106 | */ 107 | virtual Twist derivative(int first_var,int second_var); 108 | 109 | virtual Expression::Ptr derivativeExpression(int i); 110 | 111 | /** 112 | * @param column: column to gives back. 113 | * @param jval_dot: time derivative of the joint variables 114 | * not specified joint variable velocities are 115 | * zero. 116 | */ 117 | virtual Twist derivative_dot(int column, const std::vector& jval_dot); 118 | 119 | virtual int number_of_derivatives() { 120 | return _number_of_derivatives; 121 | } 122 | 123 | virtual Expression_Chain::Ptr clone(); 124 | 125 | //virtual void write_dotfile_helper(std::ostream& of, size_t& thisnode, size_t& counter); 126 | 127 | 128 | }; 129 | 130 | inline Expression::Ptr kinematic_chain(const Chain& chain, int index_of_first_joint ) { 131 | Expression::Ptr expr( new Expression_Chain( chain, index_of_first_joint ) ); 132 | return expr; 133 | } 134 | 135 | class Expression_Chain_Derivative: 136 | public FunctionType 137 | { 138 | boost::shared_ptr argument; 139 | int var_ndx; 140 | protected: 141 | Expression_Chain_Derivative(boost::shared_ptr arg, int i); 142 | public: 143 | 144 | virtual void setInputValues(const std::vector& values); 145 | virtual void setInputValue(int var, double value); 146 | virtual void setInputValue(int var, const Rotation& value) {} 147 | virtual Twist value(); 148 | 149 | virtual Twist derivative(int var_ndx); 150 | 151 | virtual Expression::Ptr derivativeExpression(int i); 152 | 153 | virtual int number_of_derivatives() { 154 | return argument->number_of_derivatives(); 155 | } 156 | virtual void update_variabletype_from_original() {} 157 | 158 | virtual Expression::Ptr clone(); 159 | 160 | friend class Expression_Chain; 161 | }; 162 | 163 | 164 | 165 | 166 | #if 0 167 | class Expression_Manipulability: 168 | public Expression 169 | { 170 | Expression_Manipulability(Expression_Chain::Ptr chainexpr); 171 | 172 | Expression_Chain( const Chain& _chain, int varndx_of_first_joint ); 173 | 174 | virtual void setInputValues(const std::vector& values); 175 | 176 | virtual Frame value(); 177 | 178 | virtual Twist derivative(int var_ndx); 179 | 180 | virtual Twist derivative(int first_var,int second_var); 181 | 182 | virtual Twist derivative_dot(int column, const std::vector& jval_dot); 183 | 184 | virtual int number_of_derivatives() { 185 | return _number_of_derivatives; 186 | } 187 | 188 | virtual typename Expression_Chain::Ptr clone(); 189 | 190 | 191 | }; 192 | #endif 193 | 194 | }; // namespace KDL 195 | #endif 196 | -------------------------------------------------------------------------------- /include/kdl/expressiontree_euler.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiontree_euler.cpp 3 | * 4 | * Created on: Aug 7, 2012 5 | * Author: Erwin Aertbelien 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #ifndef KDL_EXPRESSIONTREE_EULER_HPP 25 | #define KDL_EXPRESSIONTREE_EULER_HPP 26 | 27 | #include 28 | #include 29 | 30 | 31 | using namespace KDL { 32 | 33 | class EulerZYX_double_double_double: 34 | public TernaryExpression 35 | { 36 | double ca,sa,cb,sb,cc,sc; 37 | public: 38 | typedef TernaryExpression TernExpr; 39 | public: 40 | EulerZYX() {} 41 | EulerZYX( const typename Expression::Ptr& arg1, 42 | const typename Expression::Ptr& arg2, 43 | const typename Expression::Ptr& arg3): 44 | TernExpr("eulerzyx",arg1,arg2,arg3) 45 | {} 46 | 47 | virtual KDL::Rotation value() { 48 | double arg; 49 | arg = argument1->value(); 50 | ca = cos(arg);sa=sin(arg); 51 | arg = argument2->value(); 52 | cb = cos(arg);sb=sin(arg); 53 | arg = argument3->value(); 54 | cc = cos(arg);sc=sin(arg); 55 | return KDL::Rotation( ca*cb, ca*sb*sc-cc*sa, sa*sc+ca*cc*sb, 56 | cb*sa, ca*cc+sa*sb*sc, cc*sa*sb-ca*sc, 57 | -sb , cb*sc, cb*cc ); 58 | } 59 | 60 | virtual KDL::Vector derivative(int i) { 61 | double da = argument1->derivative(i); 62 | double db = argument2->derivative(i); 63 | double dc = argument3->derivative(i); 64 | return KDL::Vector( -sa*db + ca*cb*dc , 65 | ca*db + cb*sa*dc , 66 | da - sb*dc ); 67 | // vector(0,0,dc) 68 | // + Rot_Z(a)*vector(0,db,0) 69 | // + Rot_Z(a)*Rot_Y(b)*vector(dc,0,0) 70 | } 71 | 72 | virtual Expression::Ptr derivativeExpression(int i); 73 | 74 | virtual typename TernExpr::Ptr clone() { 75 | Expression::Ptr expr( 76 | new EulerZYX(argument1->clone(),argument2->clone(),argument3->clone()) 77 | ); 78 | return expr; 79 | } 80 | }; 81 | 82 | 83 | 84 | } // namespace KDL 85 | #endif 86 | -------------------------------------------------------------------------------- /include/kdl/expressiontree_matrix.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiontree_matrix.cpp 3 | * 4 | * 5 | * expressiongraph library 6 | * 7 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 8 | * 9 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 10 | * You may not use this work except in compliance with the Licence. 11 | * You may obtain a copy of the Licence at: 12 | * 13 | * http://ec.europa.eu/idabc/eupl 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the Licence is distributed on an "AS IS" basis, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the Licence for the specific language governing permissions and 19 | * limitations under the Licence. 20 | */ 21 | 22 | #ifndef KDL_EXPRESSIONTREE_MATRIX_HPP 23 | #define KDL_EXPRESSIONTREE_MATRIX_HPP 24 | 25 | #include 26 | #include 27 | 28 | 29 | namespace KDL { 30 | 31 | template< int n, int m> 32 | typename Expression >::Ptr 33 | addition( typename Expression< Eigen::Matrix >::Ptr a1, 34 | typename Expression< Eigen::Matrix >::Ptr a2 ); 35 | 36 | template< int n, int m, int k> 37 | typename Expression >::Ptr 38 | multiply ( typename Expression< Eigen::Matrix >::Ptr a1, 39 | typename Expression< Eigen::Matrix >::Ptr a2 ); 40 | 41 | 42 | template 43 | Expression::Ptr get_element( int i,int j, Expression::Ptr a); 44 | 45 | 46 | 47 | 48 | 49 | template< int n, int m, int k> 50 | class Matrix_Multiplication: 51 | public BinaryExpression, typename Eigen::Matrix, typename Eigen::Matrix > 52 | { 53 | typedef BinaryExpression, typename Eigen::Matrix,typename Eigen::Matrix > BinExpr; 54 | typename Eigen::Matrix arg1value; 55 | typename Eigen::Matrix arg2value; 56 | typename Eigen::Matrix result; 57 | public: 58 | Matrix_Multiplication( 59 | const typename BinExpr::Argument1Expr::Ptr& arg1, 60 | const typename BinExpr::Argument2Expr::Ptr& arg2): 61 | BinExpr("multiplication",arg1,arg2) 62 | {} 63 | virtual typename Eigen::Matrix value() { 64 | arg1value = this->argument1->value(); 65 | arg2value = this->argument2->value(); 66 | result = arg1value * arg2value; 67 | return result; 68 | } 69 | 70 | virtual typename Eigen::Matrix derivative(int i){ 71 | result = this->argument1->derivative(i) * arg2value; 72 | result += arg1value*this->argument2->derivative(i); 73 | return result; 74 | } 75 | 76 | virtual typename Expression >::Ptr derivativeExpression(int i) { 77 | return addition( multiply( this->argument1, this->argument2->derivativeExpression(i) ), 78 | multiply( this->argument1->derivativeExpression(i), this->argument2)); 79 | } 80 | 81 | virtual typename BinExpr::Ptr clone() { 82 | return boost::make_shared< Matrix_Multiplication >( 83 | this->argument1->clone(), 84 | this->argument2->clone() 85 | ); 86 | } 87 | }; 88 | 89 | 90 | /** 91 | * multiply two matrices, you will need to call this with 92 | * explicit template parameters. 93 | */ 94 | template< int n, int m, int k> 95 | inline typename Expression >::Ptr 96 | multiply ( typename Expression< Eigen::Matrix >::Ptr a1, 97 | typename Expression< Eigen::Matrix >::Ptr a2 ) { 98 | return boost::make_shared< Matrix_Multiplication >(a1,a2); 99 | } 100 | 101 | template< int n, int m> 102 | class Matrix_Addition: 103 | public BinaryExpression, typename Eigen::Matrix, typename Eigen::Matrix > 104 | { 105 | typedef BinaryExpression, typename Eigen::Matrix,typename Eigen::Matrix > BinExpr; 106 | typename Eigen::Matrix arg1value; 107 | typename Eigen::Matrix arg2value; 108 | typename Eigen::Matrix result; 109 | public: 110 | Matrix_Addition( 111 | const typename BinExpr::Argument1Expr::Ptr& arg1, 112 | const typename BinExpr::Argument2Expr::Ptr& arg2): 113 | BinExpr("multiplication",arg1,arg2) 114 | {} 115 | virtual typename Eigen::Matrix value() { 116 | arg1value = this->argument1->value(); 117 | arg2value = this->argument2->value(); 118 | result = arg1value + arg2value; 119 | return result; 120 | } 121 | 122 | virtual typename Eigen::Matrix derivative(int i){ 123 | result = this->argument1->derivative(i) + this->argument2->derivative(i); 124 | return result; 125 | } 126 | 127 | virtual typename Expression >::Ptr derivativeExpression(int i) { 128 | return addition( this->argument1->derivativeExpression(i), this->argument2->derivativeExpression(i) ); 129 | } 130 | 131 | virtual typename BinExpr::Ptr clone() { 132 | return boost::make_shared< Matrix_Addition >( 133 | this->argument1->clone(), 134 | this->argument2->clone() 135 | ); 136 | } 137 | }; 138 | 139 | 140 | /** 141 | * multiply two matrices, you will need to call this with 142 | * explicit template parameters. 143 | */ 144 | template< int n, int m> 145 | inline typename Expression >::Ptr 146 | addition( typename Expression< Eigen::Matrix >::Ptr a1, 147 | typename Expression< Eigen::Matrix >::Ptr a2 ) { 148 | return boost::make_shared< Matrix_Addition >(a1,a2); 149 | } 150 | 151 | 152 | 153 | //CoordX Vector 154 | template 155 | class MatrixElement: 156 | public UnaryExpression > 157 | { 158 | public: 159 | typedef UnaryExpression > UnExpr; 160 | int i; 161 | int j; 162 | public: 163 | MatrixElement(){} 164 | MatrixElement(const typename UnExpr::ArgumentExpr::Ptr& arg, int _i, int _j): 165 | UnExpr("get_element",arg), 166 | i(_i), 167 | j(_j) 168 | {} 169 | 170 | virtual double value() { 171 | return this->argument->value()(i,j); 172 | } 173 | 174 | virtual double derivative(int i) { 175 | return this->argument->derivative(i)(i,j); 176 | } 177 | 178 | virtual typename Expression::Ptr derivativeExpression(int c) { 179 | return get_element(i,j,this->argument->derivativeExpression(c)); 180 | } 181 | 182 | virtual typename UnExpr::Ptr clone() { 183 | return boost::make_shared< MatrixElement >(this->argument->clone(), i, j); 184 | } 185 | }; 186 | 187 | template 188 | inline Expression::Ptr get_element( int i,int j, typename Expression >::Ptr a) { 189 | if ((0<=i)&&(i >(a,i,j); 191 | } else { 192 | return Expression::Ptr(); 193 | } 194 | } 195 | 196 | } // end of namespace KDL 197 | #endif 198 | -------------------------------------------------------------------------------- /include/kdl/expressiontree_motionprofiles.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiontree_motprofiles.hpp 3 | * 4 | * Created on: Jan, 2016 5 | * Author: Erwin Aertbelien 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2016 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #ifndef KDL_EXPRESSIONTREE_MOTPROFILES_HPP 25 | #define KDL_EXPRESSIONTREE_MOTPROFILES_HPP 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | namespace KDL { 33 | 34 | class MotionProfileTrapezoidal : 35 | public MIMO { 36 | public: 37 | int critical_output; // index of the output that determines the duration 38 | std::vector< MPTrap > mp; 39 | double progrvar_value; 40 | typedef boost::shared_ptr Ptr; 41 | MotionProfileTrapezoidal(); 42 | 43 | /** 44 | * \brief declares an expression for the input variable for this motionprofile 45 | * 46 | * i.e. the progression variable, typically time or a time 47 | * related parameter 48 | */ 49 | void setProgressExpression(const Expression::Ptr& s); 50 | 51 | /** 52 | * \brief gets an expression for the progression variable 53 | */ 54 | Expression::Ptr getProgressExpression(); 55 | 56 | /** 57 | * \brief declares a given output motionprofile 58 | * All output motionprofiles will be synchronized, i.e. will start and end 59 | * at the same time. 60 | * \param startv : starting value expression 61 | * \param endv: ending value expression 62 | * \param maxvel: maximum velocity for this output 63 | * \param maxacc: maximum acceleration for this output. 64 | */ 65 | void addOutput( const Expression::Ptr& startv, 66 | const Expression::Ptr& endv, 67 | const Expression::Ptr& maxvel, 68 | const Expression::Ptr& maxacc); 69 | /** 70 | * \brief returns the number of declared outputs 71 | */ 72 | int nrOfOutputs(); 73 | 74 | /** 75 | * \brief gets the starting value expression for output idx 76 | * \param idx output (from 0 to nrOfOutputs()-1 ) 77 | */ 78 | Expression::Ptr getStartValue(int idx); 79 | 80 | /** 81 | * \brief gets the ending value expression for output idx 82 | * \param idx output (from 0 to nrOfOutputs()-1 ) 83 | */ 84 | Expression::Ptr getEndValue(int idx); 85 | 86 | /** 87 | * \brief gets the maximum velocity expression for output idx 88 | * \param idx output (from 0 to nrOfOutputs()-1 ) 89 | */ 90 | Expression::Ptr getMaxVelocity(int idx); 91 | 92 | /** 93 | * \brief gets the maximum acceleration expression for output idx 94 | * \param idx output (from 0 to nrOfOutputs()-1 ) 95 | */ 96 | Expression::Ptr getMaxAcceleration(int idx); 97 | 98 | void compute(); 99 | 100 | virtual MIMO::Ptr clone(); 101 | }; 102 | 103 | 104 | class MotionProfileTrapezoidalOutput : public MIMO_Output { 105 | public: 106 | typedef boost::shared_ptr Ptr; 107 | int outputnr; 108 | int idx_base; 109 | 110 | MotionProfileTrapezoidalOutput(MIMO::Ptr m, int _outputnr); 111 | double value(); 112 | double derivative(int i); 113 | MIMO_Output::Ptr clone(); 114 | }; 115 | 116 | /** 117 | * \brief creates a MotionProfileTrapezoidal object 118 | * 119 | * You will need to call a set of operations on this object: 120 | * - setProgressExpression(...) to set the "time"-like variable in the motionprofile 121 | * - addOutput( startval, endval, maxvel, maxacc) : all inputs are expressions. The influence of changing maxvel 122 | * and maxacc will not be propagated into the computations of the derivative. 123 | * 124 | * After this, you can call get_output_profile(...) to get an expression for the different outputs you had 125 | * defined. 126 | */ 127 | MotionProfileTrapezoidal::Ptr create_motionprofile_trapezoidal() { 128 | return boost::make_shared< MotionProfileTrapezoidal>(); 129 | } 130 | 131 | /** 132 | * \brief gets an expression representing the motion profile for a given output 133 | * \param idx index of the output for which the expression is returned. 134 | */ 135 | Expression::Ptr get_output_profile(MotionProfileTrapezoidal::Ptr& m,int output) { 136 | return boost::make_shared( m,output); 137 | } 138 | 139 | /** 140 | * \brief gets an expression representing the duration 141 | */ 142 | Expression::Ptr get_duration(MotionProfileTrapezoidal::Ptr& m) { 143 | return boost::make_shared( m,-1); 144 | } 145 | 146 | 147 | 148 | 149 | 150 | } // end of namespace KDL 151 | #endif 152 | -------------------------------------------------------------------------------- /include/kdl/expressiontree_traits.hpp: -------------------------------------------------------------------------------- 1 | #ifndef KDL_EXPRESSIONTREE_TRAITS_HPP 2 | #define KDL_EXPRESSIONTREE_TRAITS_HPP 3 | 4 | /* 5 | * expressiongraph library 6 | * 7 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 8 | * 9 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 10 | * You may not use this work except in compliance with the Licence. 11 | * You may obtain a copy of the Licence at: 12 | * 13 | * http://ec.europa.eu/idabc/eupl 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the Licence is distributed on an "AS IS" basis, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the Licence for the specific language governing permissions and 19 | * limitations under the Licence. 20 | */ 21 | #ifdef __GNUC__ 22 | #include 23 | #include 24 | #endif 25 | 26 | #include 27 | 28 | namespace KDL { 29 | 30 | /** 31 | * Trait classes 32 | * Defines relationships between types and some utility methods 33 | * that can be used to write code about types in a more general way. 34 | */ 35 | template 36 | struct AutoDiffTrait { 37 | typedef T ValueType; 38 | typedef T DerivType; 39 | static DerivType zeroDerivative() { 40 | assert(0 /* should never be called, only specialized versions should be called. */); 41 | return 0; 42 | } 43 | const static int size=0; 44 | }; 45 | 46 | template<> 47 | struct AutoDiffTrait { 48 | typedef double ValueType; 49 | typedef double DerivType; 50 | static DerivType zeroDerivative() { 51 | return 0; 52 | } 53 | const static int size=1; 54 | }; 55 | 56 | template<> 57 | struct AutoDiffTrait 58 | { 59 | typedef KDL::Vector ValueType; 60 | typedef KDL::Vector DerivType; 61 | static DerivType zeroDerivative() { 62 | return KDL::Vector::Zero(); 63 | } 64 | const static int size=3; 65 | 66 | }; 67 | 68 | template<> 69 | struct AutoDiffTrait 70 | { 71 | typedef KDL::Rotation ValueType; 72 | typedef KDL::Vector DerivType; 73 | static DerivType zeroDerivative() { 74 | return KDL::Vector::Zero(); 75 | } 76 | const static int size=3; 77 | }; 78 | 79 | //Frame 80 | template<> 81 | struct AutoDiffTrait 82 | { 83 | typedef KDL::Frame ValueType; 84 | typedef KDL::Twist DerivType; 85 | static DerivType zeroDerivative() { 86 | return KDL::Twist::Zero(); 87 | } 88 | const static int size=6; 89 | 90 | }; 91 | 92 | //Wrench 93 | template<> 94 | struct AutoDiffTrait 95 | { 96 | typedef KDL::Wrench ValueType; 97 | typedef KDL::Wrench DerivType; 98 | static DerivType zeroDerivative() { 99 | return KDL::Wrench::Zero(); 100 | } 101 | const static int size=6; 102 | 103 | }; 104 | 105 | //Twist 106 | template<> 107 | struct AutoDiffTrait 108 | { 109 | typedef KDL::Twist ValueType; 110 | typedef KDL::Twist DerivType; 111 | static DerivType zeroDerivative() { 112 | return KDL::Twist::Zero(); 113 | } 114 | const static int size=6; 115 | 116 | }; 117 | 118 | //Matrix 119 | template 120 | struct AutoDiffTrait > 121 | { 122 | typedef Eigen::Matrix ValueType; 123 | typedef Eigen::Matrix DerivType; 124 | static DerivType zeroDerivative() { 125 | return Eigen::Matrix::Zero(); 126 | } 127 | const static int size=n*m; 128 | 129 | }; 130 | 131 | 132 | 133 | template 134 | struct type_comparison { 135 | static T2 return_if_equal(const T1& arg) { 136 | return T2(); 137 | } 138 | }; 139 | 140 | template 141 | struct type_comparison { 142 | static T return_if_equal(const T& arg) { 143 | return arg; 144 | } 145 | }; 146 | 147 | #ifdef __GNUC__ 148 | inline std::string demangle( const std::string& name) { 149 | int status; 150 | char* buf=abi::__cxa_demangle(name.c_str(), 0,0, &status); 151 | std::string fullname; 152 | switch(status) { 153 | case 0: 154 | fullname=buf; 155 | break; 156 | case -1: 157 | fullname="MEMORY ALLOC FAILED"; 158 | break; 159 | case -2: 160 | fullname="INVALID NAME"; 161 | break; 162 | case -3: 163 | fullname="INVALID NAME"; 164 | break; 165 | } 166 | free(buf); 167 | return fullname; 168 | } 169 | #else 170 | inline std::string demangle( const std::string& name) { 171 | return name; 172 | } 173 | #endif 174 | 175 | 176 | } // namespace KDL; 177 | #endif 178 | -------------------------------------------------------------------------------- /include/kdl/mptrap.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * mptrap.hpp 3 | * 4 | * Created on: Jan, 2016 5 | * Author: Erwin Aertbelien 6 | * 7 | * Implementation of trapezoidal motion profile including derivatives. 8 | * 9 | * Copyright 2016 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #ifndef KDL_MPTRAP_HPP 25 | #define KDL_MPTRAP_HPP 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | namespace KDL { 32 | 33 | /** 34 | * MPTrap implements a trapezoidal motionprofile 35 | * ( implementation class, not be used externally ) 36 | */ 37 | 38 | struct MPTrap { 39 | double amax; ///< maximum acceleration 40 | double vmax; ///< maximum velocity 41 | double spos; ///< start position 42 | double epos; ///< end position 43 | double duration; 44 | double t1; 45 | double t2; 46 | double s; 47 | double dT; 48 | double d_t1_d_dpos; 49 | double d_t2_d_dpos; 50 | double d_duration_d_dpos; 51 | 52 | MPTrap(); 53 | 54 | void setPlan(double spos, double epos, double vmax, double amax); 55 | 56 | double planMinDuration(); 57 | void adaptDuration(double new_duration); 58 | /** 59 | * derivatives only to be used when this is the critical motion profile 60 | */ 61 | void compute_derivs(); 62 | double pos(double time); 63 | double d_pos_d_time(double time); 64 | double d_pos_d_spos(double time); 65 | double d_pos_d_epos(double time); 66 | double d_pos_d_t(double time); 67 | }; 68 | 69 | 70 | } // end of namespace KDL 71 | #endif 72 | -------------------------------------------------------------------------------- /license_EUPL_v1_1_en.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/eaertbel/expressiongraph/46f210e48bf56186a0cdf1fd198ccc20812041a8/license_EUPL_v1_1_en.pdf -------------------------------------------------------------------------------- /mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b expressiontree 6 | 7 | 30 | 31 | --> 32 | 33 | 34 | */ 35 | -------------------------------------------------------------------------------- /manifest.old: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | This package provides an expression tree framework for the Kinematics and Dynamics 5 | Library (KDL). 6 | 7 | 8 | Erwin Aertbelien 9 | Proprietary 10 | 11 | http://www.orocos.org/XXXX 12 | 13 | 15 | 16 | 17 | 18 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | expressiongraph 4 | 5 | This package provides an expression tree framework for the Kinematics 6 | and Dynamics Library (KDL). 7 | 8 | 0.1.0 9 | Erwin Aertbelien 10 | Erwin Aertbelien 11 | EUPL v1.1 12 | 13 | catkin 14 | orocos_kdl 15 | orocos_kdl 16 | gtest 17 | cmake_modules 18 | cmake_modules 19 | 20 | -------------------------------------------------------------------------------- /preamble.txt: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiongraph library 3 | * 4 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 5 | * 6 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 7 | * You may not use this work except in compliance with the Licence. 8 | * You may obtain a copy of the Licence at: 9 | * 10 | * http://ec.europa.eu/idabc/eupl 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the Licence is distributed on an "AS IS" basis, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the Licence for the specific language governing permissions and 16 | * limitations under the Licence. 17 | */ 18 | 19 | -------------------------------------------------------------------------------- /rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | homepage: http://people.mech.kuleuven.be/~eaertbel/expressiontrees 3 | -------------------------------------------------------------------------------- /src/expressiontree_frame.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiontree_frame.cpp 3 | * 4 | * Created on: September 5 | * Author: Wouter Bancken - Erwin Aertbelien 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include "util.hpp" 31 | 32 | namespace KDL { 33 | /* 34 | Expression::Ptr ChangeCoordinateFrame_FrameRotation::derivativeExpression(int i) { 35 | } 36 | */ 37 | Expression::Ptr Frame_RotationVector::derivativeExpression(int i) { 38 | int nr = getDep2(i,argument1,argument2); 39 | if (nr==1) { 40 | return Constant(Twist::Zero()); 41 | } if (nr==2) { 42 | return twist( 43 | argument2->derivativeExpression(i), 44 | Constant(Vector::Zero()) 45 | ); 46 | } if (nr==3) { 47 | return twist( 48 | Constant(Vector::Zero()), 49 | argument1->derivativeExpression(i) 50 | ); 51 | } else { 52 | return twist( 53 | argument2->derivativeExpression(i), 54 | argument1->derivativeExpression(i) 55 | ); 56 | } 57 | } 58 | 59 | 60 | Expression::Ptr Inverse_Frame::derivativeExpression(int i) 61 | { 62 | int nr = getDep(i,argument); 63 | if (nr==1) { 64 | return Constant(Twist::Zero()); 65 | } else { 66 | Expression::Ptr da = cached( argument->derivativeExpression(i) ); 67 | Expression::Ptr val = cached( argument ); 68 | Expression::Ptr rotvelda = cached(rotvel(da)); 69 | return inv(rotation(val))*twist( rotvelda*origin(val)-transvel(da), -rotvelda); 70 | } 71 | } 72 | 73 | 74 | Expression::Ptr Composition_FrameFrame::derivativeExpression(int i) 75 | { 76 | Expression::Ptr a = cached(argument1 ); 77 | Expression::Ptr b = cached(argument2 ); 78 | Expression::Ptr da = cached(argument1->derivativeExpression(i)); 79 | Expression::Ptr db = cached(argument2->derivativeExpression(i)); 80 | Expression::Ptr rotvelda = cached(rotvel(da)); 81 | Expression::Ptr rotationa = cached(rotation(a)); 82 | int nr = getDep(i,argument1,argument2); 83 | if (nr==1) { 84 | return Constant(Twist::Zero()); 85 | } if (nr==2) { 86 | return twist( rotationa*transvel(db) , 87 | rotationa*rotvel(db) 88 | ); 89 | } if (nr==3) { 90 | return twist( rotvelda*(rotationa*origin(b)) + transvel(da), 91 | rotvelda 92 | ); 93 | } else { 94 | return twist( rotvelda*(rotationa*origin(b)) + rotationa*transvel(db) + transvel(da), 95 | rotvelda + rotationa*rotvel(db) 96 | ); 97 | } 98 | } 99 | 100 | Expression::Ptr Composition_FrameVector::derivativeExpression(int i) 101 | { 102 | Expression::Ptr aRot = cached( rotation( argument1 ) ); 103 | Expression::Ptr da = cached( argument1->derivativeExpression(i) ); 104 | 105 | int nr = getDep2(i,argument1,argument2); 106 | if (nr==1) { 107 | return Constant(Vector::Zero()); 108 | } if (nr==2) { 109 | return aRot*argument2->derivativeExpression(i); 110 | } if (nr==3) { 111 | return rotvel(da)*( aRot* argument2 ) + transvel(da); 112 | } else { 113 | return rotvel(da)*( aRot* argument2 ) + 114 | aRot*argument2->derivativeExpression(i) + 115 | transvel(da); 116 | } 117 | } 118 | 119 | 120 | Expression::Ptr Origin_Frame::derivativeExpression(int i) 121 | { 122 | int nr = getDep(i,argument); 123 | if (nr==1) { 124 | return Constant(Vector::Zero()); 125 | } else { 126 | return transvel( argument->derivativeExpression(i) ); 127 | } 128 | } 129 | 130 | 131 | Expression::Ptr Rotation_Frame::derivativeExpression(int i) 132 | { 133 | int nr = getDep(i,argument); 134 | if (nr==1) { 135 | return Constant(Vector::Zero()); 136 | } else { 137 | return rotvel( argument->derivativeExpression(i) ); 138 | } 139 | } 140 | 141 | 142 | 143 | 144 | 145 | 146 | }; // namespace KDL 147 | -------------------------------------------------------------------------------- /src/expressiontree_rotation.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiontree_rotation.cpp 3 | * 4 | * Created on: Sept., 2012 5 | * Author: Erwin Aertbelien 6 | * expressiongraph library 7 | * 8 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 9 | * 10 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 11 | * You may not use this work except in compliance with the Licence. 12 | * You may obtain a copy of the Licence at: 13 | * 14 | * http://ec.europa.eu/idabc/eupl 15 | * 16 | * Unless required by applicable law or agreed to in writing, software 17 | * distributed under the Licence is distributed on an "AS IS" basis, 18 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 19 | * See the Licence for the specific language governing permissions and 20 | * limitations under the Licence. 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | #include "util.hpp" 27 | 28 | namespace KDL { 29 | 30 | Expression::Ptr RotVec_Double::derivativeExpression(int i) { 31 | Expression::Ptr arg1 = cached( argument1 ); 32 | Expression::Ptr arg2 = cached( argument2 ); 33 | int nr = getDep2(i,argument1, argument2); 34 | if (nr==1) { 35 | return Constant(Vector::Zero()); 36 | } 37 | if (nr==2) { 38 | return argument1*argument2->derivativeExpression(i); 39 | } 40 | if (nr==3) { 41 | return argument1->derivativeExpression(i)*argument2; 42 | } else { 43 | return argument1*argument2->derivativeExpression(i) + argument1->derivativeExpression(i)*argument2; 44 | } 45 | } 46 | 47 | 48 | 49 | Expression::Ptr Rot_Double::derivativeExpression(int i) { 50 | int nr = getDep(i,argument); 51 | if (nr==1) { 52 | return Constant(Vector::Zero()); 53 | } else { 54 | return Constant( axis ) * argument->derivativeExpression(i); 55 | } 56 | } 57 | 58 | Expression::Ptr RotX_Double::derivativeExpression(int i) { 59 | int nr = getDep(i,argument); 60 | if (nr==1) { 61 | return Constant(Vector::Zero()); 62 | } else { 63 | return KDL::vector( argument->derivativeExpression(i), Constant(0.0), Constant(0.0) ); 64 | } 65 | } 66 | 67 | Expression::Ptr RotY_Double::derivativeExpression(int i) { 68 | int nr = getDep(i,argument); 69 | if (nr==1) { 70 | return Constant(Vector::Zero()); 71 | } else { 72 | return KDL::vector( Constant(0.0), argument->derivativeExpression(i), Constant(0.0) ); 73 | } 74 | } 75 | 76 | Expression::Ptr RotZ_Double::derivativeExpression(int i) { 77 | int nr = getDep(i,argument); 78 | if (nr==1) { 79 | return Constant(Vector::Zero()); 80 | } else { 81 | return KDL::vector( Constant(0.0), Constant(0.0), argument->derivativeExpression(i) ); 82 | } 83 | } 84 | 85 | Expression::Ptr Inverse_Rotation::derivativeExpression(int i) { 86 | int nr = getDep(i,argument); 87 | if (nr==1) { 88 | return Constant(Vector::Zero()); 89 | } else { 90 | return -( inv( argument ) * argument->derivativeExpression(i) ); 91 | } 92 | } 93 | 94 | Expression::Ptr Composition_RotationRotation::derivativeExpression(int i) { 95 | int nr = getDep(i,argument1,argument2); 96 | if (nr==1) { 97 | return Constant(Vector::Zero()); 98 | } if (nr==2) { 99 | return argument1*argument2->derivativeExpression(i); 100 | } if (nr==3) { 101 | return argument1->derivativeExpression(i); 102 | } else { 103 | return argument1*argument2->derivativeExpression(i) + argument1->derivativeExpression(i); 104 | } 105 | } 106 | 107 | Expression::Ptr Composition_RotationVector::derivativeExpression(int i) { 108 | Expression::Ptr arg1 = cached( argument1 ); 109 | Expression::Ptr arg2 = cached( argument2 ); 110 | int nr = getDep2(i,argument1,argument2); 111 | if (nr==1) { 112 | return Constant(Vector::Zero()); 113 | } if (nr==2) { 114 | return arg1*argument2->derivativeExpression(i); 115 | } if (nr==3) { 116 | return argument1->derivativeExpression(i) * (arg1 * arg2); 117 | } else { 118 | return argument1->derivativeExpression(i) * (arg1 * arg2) + arg1*argument2->derivativeExpression(i); 119 | } 120 | } 121 | 122 | Expression::Ptr UnitX_Rotation::derivativeExpression(int i) { 123 | int nr = getDep(i,argument); 124 | if (nr==1) { 125 | return Constant(Vector::Zero()); 126 | } else { 127 | return argument->derivativeExpression(i) * unit_x( argument ); 128 | } 129 | } 130 | Expression::Ptr UnitY_Rotation::derivativeExpression(int i) { 131 | int nr = getDep(i,argument); 132 | if (nr==1) { 133 | return Constant(Vector::Zero()); 134 | } else { 135 | return argument->derivativeExpression(i) * unit_y( argument ); 136 | } 137 | } 138 | Expression::Ptr UnitZ_Rotation::derivativeExpression(int i) { 139 | int nr = getDep(i,argument); 140 | if (nr==1) { 141 | return Constant(Vector::Zero()); 142 | } else { 143 | return argument->derivativeExpression(i) * unit_z( argument ); 144 | } 145 | } 146 | 147 | Expression::Ptr Get_Rotation_Vector::derivativeExpression(int i) { 148 | int nr = getDep(i,argument); 149 | if (nr==1) { 150 | return Constant(Vector::Zero()); 151 | } else { 152 | return argument->derivativeExpression(i); 153 | } 154 | } 155 | 156 | Expression::Ptr Get_RPY_Rotation::derivativeExpression(int i) { 157 | int nr = getDep(i,argument); 158 | if (nr==1) { 159 | return Constant(Vector::Zero()); 160 | } else { 161 | Expression::Ptr omega = argument->derivativeExpression(i); 162 | Expression::Ptr rpy = getRPY( argument); 163 | Expression::Ptr ca = cos( coord_z(rpy) ); 164 | Expression::Ptr sa = sin( coord_z(rpy) ); 165 | Expression::Ptr cb = cos( coord_y(rpy) ); 166 | Expression::Ptr sb = sin( coord_y(rpy) ); 167 | return vector( 168 | ca/cb*coord_x(omega) + sa/cb*coord_y(omega), 169 | -sa*coord_x(omega) + ca*coord_y(omega), 170 | ca*sb/cb*coord_x(omega) + sa*sb/cb*coord_y(omega) + coord_z(omega) 171 | ); 172 | } 173 | } 174 | 175 | 176 | 177 | 178 | 179 | } // end of namespace KDL 180 | -------------------------------------------------------------------------------- /src/expressiontree_twist.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiontree_twist.cpp 3 | * 4 | * Created on: Sept., 2012 5 | * Author: Erwin Aertbelien 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | namespace KDL { 30 | 31 | Expression::Ptr Twist_VectorVector::derivativeExpression(int i) 32 | { 33 | return twist( argument1, argument2 ); 34 | } 35 | 36 | 37 | Expression::Ptr Negate_Twist::derivativeExpression(int i) { 38 | return -argument->derivativeExpression(i); 39 | } 40 | Expression::Ptr Velocity_Twist::derivativeExpression(int i) { 41 | return transvel(argument->derivativeExpression(i)); 42 | } 43 | Expression::Ptr RotVelocity_Twist::derivativeExpression(int i) { 44 | return rotvel(argument->derivativeExpression(i)); 45 | } 46 | 47 | Expression::Ptr Addition_TwistTwist::derivativeExpression(int i) { 48 | return argument1->derivativeExpression(i) + argument2->derivativeExpression(i); 49 | } 50 | 51 | Expression::Ptr Subtraction_TwistTwist::derivativeExpression(int i) { 52 | return argument1->derivativeExpression(i) - argument2->derivativeExpression(i); 53 | } 54 | 55 | Expression::Ptr Composition_RotationTwist::derivativeExpression(int i) { 56 | Expression::Ptr a = cached( argument1 ); 57 | Expression::Ptr b = cached( argument2 ); 58 | Expression::Ptr da = cached(argument1->derivativeExpression(i)); 59 | Expression::Ptr db = cached(argument2->derivativeExpression(i)); 60 | return twist( a*transvel(db) + da*(a*transvel(b)), 61 | a*rotvel(db) + da*(a*rotvel(b)) 62 | ); 63 | } 64 | Expression::Ptr Multiplication_TwistDouble::derivativeExpression(int i) { 65 | return argument1 * argument2->derivativeExpression(i) + 66 | argument1->derivativeExpression(i) * argument2; 67 | } 68 | 69 | Expression::Ptr RefPoint_TwistVector::derivativeExpression(int i) { 70 | Expression::Ptr da = cached( argument1->derivativeExpression(i) ); 71 | return twist( transvel(da) + rotvel(da)*argument2 + 72 | rotvel(argument1)*argument2->derivativeExpression(i), 73 | rotvel(da) ); 74 | } 75 | 76 | } // end of namespace KDL 77 | 78 | -------------------------------------------------------------------------------- /src/expressiontree_vector.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiontree_vector.cpp 3 | * 4 | * Created on: Aug 7, 2012 5 | * Author: Erwin Aertbelien 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #include 25 | #include 26 | #include "util.hpp" 27 | 28 | namespace KDL { 29 | 30 | Expression::Ptr Vector_DoubleDoubleDouble::derivativeExpression(int i) { 31 | bool d1 = isDepOn(i,argument1); 32 | bool d2 = isDepOn(i,argument2); 33 | bool d3 = isDepOn(i,argument3); 34 | if (!d1 && !d2 && !d3) { 35 | return Constant(Vector::Zero()); 36 | } else if (!d1 && !d2) { 37 | return vector(Constant(0.0), Constant(0.0), argument3->derivativeExpression(i) ); 38 | } else if (!d1 && !d3) { 39 | return vector(Constant(0.0), argument2->derivativeExpression(i), Constant(0.0) ); 40 | } else if (!d2 && !d3) { 41 | return vector(argument1->derivativeExpression(i), Constant(0.0), Constant(0.0) ); 42 | } else if (!d1) { 43 | return vector(Constant(0.0), argument2->derivativeExpression(i), argument3->derivativeExpression(i) ); 44 | } else if (!d2) { 45 | return vector(argument1->derivativeExpression(i), Constant(0.0), argument3->derivativeExpression(i) ); 46 | } else if (!d3) { 47 | return vector(argument1->derivativeExpression(i), argument2->derivativeExpression(i), Constant(0.0) ); 48 | } else { 49 | return vector(argument1->derivativeExpression(i), argument2->derivativeExpression(i), argument3->derivativeExpression(i) ); 50 | } 51 | } 52 | 53 | Expression::Ptr Dot_VectorVector::derivativeExpression(int i) { 54 | int nr = getDep(i,argument1,argument2); 55 | if (nr==1) { 56 | return Constant(0.0); 57 | } if (nr==2) { 58 | return dot(argument1,argument2->derivativeExpression(i)); 59 | } if (nr==3) { 60 | return dot(argument1->derivativeExpression(i),argument2); 61 | } else { 62 | return dot(argument1,argument2->derivativeExpression(i)) + 63 | dot(argument1->derivativeExpression(i),argument2); 64 | } 65 | } 66 | 67 | Expression::Ptr CrossProduct_VectorVector::derivativeExpression(int i) { 68 | int nr = getDep(i,argument1,argument2); 69 | if (nr==1) { 70 | return Constant(Vector::Zero()); 71 | } if (nr==2) { 72 | return argument1 * argument2->derivativeExpression(i); 73 | } if (nr==3) { 74 | return argument1->derivativeExpression(i) * argument2; 75 | } else { 76 | return argument1 * argument2->derivativeExpression(i) + 77 | argument1->derivativeExpression(i) * argument2; 78 | } 79 | } 80 | 81 | Expression::Ptr Addition_VectorVector::derivativeExpression(int i) { 82 | int nr = getDep(i,argument1,argument2); 83 | if (nr==1) { 84 | return Constant(Vector::Zero()); 85 | } if (nr==2) { 86 | return argument2->derivativeExpression(i); 87 | } if (nr==3) { 88 | return argument1->derivativeExpression(i); 89 | } else { 90 | return argument1->derivativeExpression(i) + argument2->derivativeExpression(i); 91 | } 92 | } 93 | 94 | Expression::Ptr Subtraction_VectorVector::derivativeExpression(int i) { 95 | return argument1->derivativeExpression(i) - argument2->derivativeExpression(i); 96 | } 97 | 98 | Expression::Ptr Negate_Vector::derivativeExpression(int i) { 99 | int nr = getDep(i,argument); 100 | if (nr==1) { 101 | return Constant(Vector::Zero()); 102 | } else { 103 | return -argument->derivativeExpression(i); 104 | } 105 | } 106 | 107 | Expression::Ptr Norm_Vector::derivativeExpression(int i) { 108 | int nr = getDep(i,argument); 109 | if (nr==1) { 110 | return Constant(0.0); 111 | } else { 112 | Expression::Ptr val = cached( argument ); 113 | return dot( val, argument->derivativeExpression(i) ) / norm(val) ; 114 | } 115 | } 116 | 117 | Expression::Ptr SquaredNorm_Vector::derivativeExpression(int i) { 118 | int nr = getDep(i,argument); 119 | if (nr==1) { 120 | return Constant(0.0); 121 | } else { 122 | Expression::Ptr val = cached( argument ); 123 | return dot( val, argument->derivativeExpression(i) )*Constant(2.0); 124 | } 125 | } 126 | 127 | Expression::Ptr Multiplication_VectorDouble::derivativeExpression(int i) { 128 | int nr = getDep2(i,argument1,argument2); 129 | if (nr==1) { 130 | return Constant(Vector::Zero()); 131 | } if (nr==2) { 132 | return argument1 * argument2->derivativeExpression(i); 133 | } if (nr==3) { 134 | return argument1->derivativeExpression(i) * argument2; 135 | } else { 136 | return argument1 * argument2->derivativeExpression(i) + argument1->derivativeExpression(i) * argument2; 137 | } 138 | } 139 | 140 | Expression::Ptr CoordX_Vector::derivativeExpression(int i) { 141 | int nr = getDep(i,argument); 142 | if (nr==1) { 143 | return Constant(0.0); 144 | } else { 145 | return coord_x(argument->derivativeExpression(i)); 146 | } 147 | } 148 | 149 | Expression::Ptr CoordY_Vector::derivativeExpression(int i) { 150 | int nr = getDep(i,argument); 151 | if (nr==1) { 152 | return Constant(0.0); 153 | } else { 154 | return coord_y(argument->derivativeExpression(i)); 155 | } 156 | } 157 | 158 | Expression::Ptr CoordZ_Vector::derivativeExpression(int i) { 159 | int nr = getDep(i,argument); 160 | if (nr==1) { 161 | return Constant(0.0); 162 | } else { 163 | return coord_z(argument->derivativeExpression(i)); 164 | } 165 | } 166 | 167 | Expression::Ptr Diff_VectorVector::derivativeExpression(int i) { 168 | int nr = getDep(i,argument1,argument2); 169 | if (nr==1) { 170 | return Constant(Vector::Zero()); 171 | } if (nr==2) { 172 | return argument2->derivativeExpression(i); 173 | } if (nr==3) { 174 | return - argument1->derivativeExpression(i); 175 | } else { 176 | return argument2->derivativeExpression(i) - argument1->derivativeExpression(i); 177 | } 178 | } 179 | 180 | 181 | } // end of namespace KDL 182 | -------------------------------------------------------------------------------- /src/expressiontree_wrench.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiontree_wrench.cpp 3 | * 4 | * Created on: Sept. 2012 5 | * Author: Erwin Aertbelien - Wouter Bancken 6 | * 7 | * expressiongraph library 8 | * 9 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 10 | * 11 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 12 | * You may not use this work except in compliance with the Licence. 13 | * You may obtain a copy of the Licence at: 14 | * 15 | * http://ec.europa.eu/idabc/eupl 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the Licence is distributed on an "AS IS" basis, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the Licence for the specific language governing permissions and 21 | * limitations under the Licence. 22 | */ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | namespace KDL { 30 | Expression::Ptr Wrench_VectorVector::derivativeExpression(int i) 31 | { 32 | return wrench( argument1, argument2 ); 33 | } 34 | Expression::Ptr Force_Wrench::derivativeExpression(int i) { 35 | return force(argument->derivativeExpression(i)); 36 | } 37 | Expression::Ptr Torque_Wrench::derivativeExpression(int i) { 38 | return torque(argument->derivativeExpression(i)); 39 | } 40 | 41 | 42 | 43 | Expression::Ptr Negate_Wrench::derivativeExpression(int i) { 44 | return -argument->derivativeExpression(i); 45 | } 46 | Expression::Ptr Addition_WrenchWrench::derivativeExpression(int i) { 47 | return argument1->derivativeExpression(i) + argument2->derivativeExpression(i); 48 | } 49 | Expression::Ptr Subtraction_WrenchWrench::derivativeExpression(int i) { 50 | return argument1->derivativeExpression(i) + argument2->derivativeExpression(i); 51 | } 52 | 53 | Expression::Ptr Composition_RotationWrench::derivativeExpression(int i) { 54 | Expression::Ptr a = cached(argument1); 55 | Expression::Ptr da = cached(argument1->derivativeExpression(i)); 56 | Expression::Ptr b = cached(argument2); 57 | Expression::Ptr db = cached(argument2->derivativeExpression(i)); 58 | return wrench( a*force(db) + da*(a*force(b)), 59 | a*torque(db) + da*(a*torque(b)) ); 60 | } 61 | 62 | Expression::Ptr Multiplication_WrenchDouble::derivativeExpression(int i) { 63 | return argument1*argument2->derivativeExpression(i) + 64 | argument1->derivativeExpression(i)*argument2; 65 | } 66 | 67 | Expression::Ptr RefPoint_WrenchVector::derivativeExpression(int i) { 68 | Expression::Ptr a = cached(argument1); 69 | Expression::Ptr da = cached(argument1->derivativeExpression(i)); 70 | Expression::Ptr b = cached(argument2); 71 | Expression::Ptr db = cached(argument2->derivativeExpression(i)); 72 | return wrench( force(da), 73 | torque(da) + force(da)*b + force(a)*db ); 74 | } 75 | 76 | 77 | 78 | } // end of namespace KDL 79 | 80 | -------------------------------------------------------------------------------- /src/mptrap.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | namespace KDL { 5 | 6 | MPTrap::MPTrap(): 7 | amax(1),vmax(1),spos(0),epos(0),duration(0),t1(0),t2(0), 8 | s(1),dT(0), d_t1_d_dpos(0),d_t2_d_dpos(0), d_duration_d_dpos(0) 9 | {} 10 | 11 | void MPTrap::setPlan(double _spos, double _epos, double _vmax, double _amax) { 12 | spos = _spos; 13 | epos = _epos; 14 | vmax = _vmax; 15 | amax = _amax; 16 | } 17 | 18 | double MPTrap::planMinDuration() { 19 | double dpos = epos - spos; 20 | s = dpos >= 0 ? 1 : -1; 21 | if (fabs(dpos) < 1E-7) { 22 | dpos = s*1E-7; 23 | } 24 | t1 = vmax/amax; 25 | double dx = s*amax*t1*t1/2.0; 26 | dT = (dpos -2*dx)*s/vmax; 27 | if (dT>0) { 28 | duration = 2*t1 + dT; 29 | t2 = duration - t1; 30 | } else { 31 | t1 = sqrt( dpos*s/amax); 32 | t2 = t1; 33 | duration = 2*t1; 34 | } 35 | return duration; 36 | } 37 | 38 | void MPTrap::adaptDuration(double new_duration) { 39 | double f = duration/new_duration; 40 | t1 = t1 / f; 41 | t2 = t2 / f; 42 | duration = new_duration; 43 | vmax = vmax * f; 44 | amax = amax * f*f; 45 | } 46 | 47 | /** 48 | * derivatives only to be used when this is the critical motion profile 49 | */ 50 | void MPTrap::compute_derivs() { 51 | if (dT>0) { 52 | d_duration_d_dpos = s/vmax; 53 | d_t1_d_dpos = 0.; 54 | d_t2_d_dpos = d_duration_d_dpos; 55 | } else { 56 | d_t1_d_dpos = -0.5/t1*s/amax; 57 | d_t2_d_dpos = d_t1_d_dpos; 58 | d_duration_d_dpos = 2*d_t1_d_dpos; 59 | } 60 | 61 | } 62 | double MPTrap::pos(double time) { 63 | if (time <= 0) { 64 | return spos; 65 | } else if (time < t1) { 66 | return spos + s*amax*time*time/2.; 67 | } else if (time < t2) { 68 | return spos + s*amax*t1*t1/2. + s*vmax*(time-t1); 69 | } else if (time < duration) { 70 | return epos - s*amax*(duration-time)*(duration-time)/2.; 71 | } else { 72 | return epos; 73 | } 74 | } 75 | 76 | double MPTrap::d_pos_d_time(double time) { 77 | if (time <= 0.) { 78 | return 0.; 79 | } else if (time < t1) { 80 | return s*amax*time; 81 | } else if (time < t2) { 82 | return s*vmax;; 83 | } else if (time < duration) { 84 | return s*amax*(duration-time); 85 | } else { 86 | return 0.; 87 | } 88 | } 89 | 90 | double MPTrap::d_pos_d_spos(double time) { 91 | if (time <= 0) { 92 | return 1.; 93 | } else if (time < t1) { 94 | return 1.; 95 | } else if (time < t2) { 96 | return 1. - s*amax*t1*d_t1_d_dpos + s*vmax*d_t1_d_dpos; 97 | } else if (time < duration) { 98 | return s*amax*(duration-time)*d_duration_d_dpos; 99 | } else { 100 | return 0; 101 | } 102 | 103 | } 104 | 105 | double MPTrap::d_pos_d_epos(double time) { 106 | if (time <= 0) { 107 | return 0; 108 | } else if (time < t1) { 109 | return 0; 110 | } else if (time < t2) { 111 | return s*amax*t1*d_t1_d_dpos - s*vmax*d_t1_d_dpos; 112 | } else if (time < duration) { 113 | return 1. - s*amax*(duration-time)*d_duration_d_dpos; 114 | } else { 115 | return 1; 116 | } 117 | } 118 | 119 | 120 | } // end of namespace KDL 121 | -------------------------------------------------------------------------------- /src/util.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * expressiongraph library 3 | * 4 | * Copyright 2014 Erwin Aertbelien - KU Leuven - Dep. of Mechanical Engineering 5 | * 6 | * Licensed under the EUPL, Version 1.1 only (the "Licence"); 7 | * You may not use this work except in compliance with the Licence. 8 | * You may obtain a copy of the Licence at: 9 | * 10 | * http://ec.europa.eu/idabc/eupl 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the Licence is distributed on an "AS IS" basis, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the Licence for the specific language governing permissions and 16 | * limitations under the Licence. 17 | */ 18 | 19 | #ifndef KDL_EXPRESSIONGRAPH_UTIL_HPP 20 | #define KDL_EXPRESSIONGRAPH_UTIL_HPP 21 | 22 | // private utilities for constant collapsing: 23 | 24 | /* 25 | int nr = getDep(i,argument1,argument2); 26 | if (nr==1) { 27 | return Constant(0.0); 28 | } if (nr==2) { 29 | } if (nr==3) { 30 | } else { 31 | } 32 | 33 | int nr = getDep(i,argument); 34 | if (nr==1) { 35 | return Constant(0.0); 36 | } else { 37 | } 38 | 39 | 40 | */ 41 | 42 | namespace KDL { 43 | 44 | template 45 | inline int getDep( int i, typename Expression::Ptr argument1, typename Expression::Ptr argument2) { 46 | //std::cout << "multiplication between " << std::endl; 47 | //argument1->print(std::cout);std::cout << std::endl; 48 | //argument2->print(std::cout);std::cout << std::endl; 49 | std::set vset1; 50 | argument1->getDependencies(vset1); 51 | bool depend1 = vset1.count(i)>0; 52 | //std::cout << depend1 << std::endl; 53 | std::set vset2; 54 | argument2->getDependencies(vset2); 55 | bool depend2 = vset2.count(i)>0; 56 | //std::cout << depend2 << std::endl; 57 | if (!depend1 && !depend2) { 58 | return 1; 59 | } else if (!depend1) { 60 | return 2; 61 | } else if (!depend2) { 62 | return 3; 63 | } else { 64 | return 4; 65 | } 66 | } 67 | template 68 | inline int getDep2( int i, typename Expression::Ptr argument1, typename Expression::Ptr argument2) { 69 | //std::cout << "multiplication between " << std::endl; 70 | //argument1->print(std::cout);std::cout << std::endl; 71 | //argument2->print(std::cout);std::cout << std::endl; 72 | std::set vset1; 73 | argument1->getDependencies(vset1); 74 | bool depend1 = vset1.count(i)>0; 75 | //std::cout << depend1 << std::endl; 76 | std::set vset2; 77 | argument2->getDependencies(vset2); 78 | bool depend2 = vset2.count(i)>0; 79 | //std::cout << depend2 << std::endl; 80 | if (!depend1 && !depend2) { 81 | return 1; 82 | } else if (!depend1) { 83 | return 2; 84 | } else if (!depend2) { 85 | return 3; 86 | } else { 87 | return 4; 88 | } 89 | } 90 | template 91 | bool isDepOn(int i, typename Expression::Ptr a) { 92 | std::set vset; 93 | a->getDependencies(vset); 94 | return vset.count(i)>0; 95 | } 96 | 97 | template 98 | inline int getDep( int i, typename Expression::Ptr argument1) { 99 | std::set vset1; 100 | argument1->getDependencies(vset1); 101 | bool depend1 = vset1.count(i)>0; 102 | if (!depend1) { 103 | return 1; 104 | } else { 105 | return 2; 106 | } 107 | } 108 | 109 | 110 | } // namespace KDL 111 | 112 | #endif 113 | --------------------------------------------------------------------------------