├── hiqp_core ├── README.md ├── CHANGELOG.rst ├── rosdoc.yaml ├── package.xml ├── FindCASADI.cmake ├── include │ └── hiqp │ │ ├── solvers │ │ ├── casadi_solver.h │ │ ├── gurobi_solver__original.h │ │ └── gurobi_solver.h │ │ ├── hiqp_time_point.h │ │ ├── tasks │ │ ├── tdef_full_pose.h │ │ ├── tdef_jnt_config.h │ │ ├── tdyn_cubic.h │ │ ├── tdyn_linear.h │ │ ├── tdef_jnt_limits.h │ │ ├── tdyn_jnt_limits.h │ │ ├── tdyn_hyper_sin.h │ │ ├── tdyn_minimal_jerk.h │ │ ├── tdef_geometric_alignment.h │ │ └── tdef_geometric_projection.h │ │ ├── geometric_primitives │ │ ├── geometric_primitive_visitor.h │ │ ├── geometric_primitive_couter.h │ │ ├── geometric_primitive.h │ │ ├── geometric_point.h │ │ ├── geometric_sphere.h │ │ ├── geometric_plane.h │ │ ├── geometric_primitive_map.h │ │ ├── geometric_line.h │ │ ├── geometric_primitive_visualizer.h │ │ ├── geometric_cylinder.h │ │ └── geometric_frame.h │ │ ├── robot_state.h │ │ ├── visualizer.h │ │ ├── task_dynamics.h │ │ ├── task_definition.h │ │ ├── utilities.h │ │ ├── task.h │ │ ├── task_manager.h │ │ └── hiqp_solver.h ├── src │ ├── tasks │ │ ├── tdyn_linear.cpp │ │ ├── tdyn_jnt_limits.cpp │ │ ├── tdyn_hyper_sin.cpp │ │ ├── tdyn_cubic.cpp │ │ ├── tdef_jnt_config.cpp │ │ ├── tdyn_minimal_jerk.cpp │ │ ├── tdef_full_pose.cpp │ │ ├── tdef_jnt_limits.cpp │ │ └── tdef_geometric_alignment.cpp │ ├── hiqp_time_point.cpp │ └── solvers │ │ └── casadi_solver.cpp └── CMakeLists.txt ├── hiqp_ros ├── README.md ├── CHANGELOG.rst ├── rosdoc.yaml ├── plugins.xml ├── package.xml ├── include │ └── hiqp_ros │ │ ├── utilities.h │ │ ├── ros_topic_subscriber.h │ │ ├── hiqp_joint_effort_controller.h │ │ ├── hiqp_joint_velocity_controller.h │ │ ├── ros_visualizer.h │ │ └── hiqp_service_handler.h ├── CMakeLists.txt └── src │ └── utilities.cpp ├── hiqp_msgs ├── README.md ├── CHANGELOG.rst ├── msg │ ├── StringArray.msg │ ├── Vector3d.msg │ ├── TaskMeasures.msg │ └── TaskMeasure.msg ├── package.xml ├── srv │ ├── ListAllTasks.srv │ ├── ListAllPrimitives.srv │ ├── RemoveAllTasks.srv │ ├── RemoveAllPrimitives.srv │ ├── RemoveTask.srv │ ├── RemovePrimitive.srv │ ├── ActivateTask.srv │ ├── ActivatePriorityLevel.srv │ ├── DeactivateTask.srv │ ├── DeactivatePriorityLevel.srv │ ├── MonitorTask.srv │ ├── RemovePriorityLevel.srv │ ├── DemonitorTask.srv │ ├── MonitorPriorityLevel.srv │ ├── DemonitorPriorityLevel.srv │ ├── SetPrimitive.srv │ └── SetTask.srv └── CMakeLists.txt ├── hiqp ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml ├── .gitignore └── README.md /hiqp_core/README.md: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework 2 | Copyright (C) 2016 Marcus A Johansson -------------------------------------------------------------------------------- /hiqp_ros/README.md: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework 2 | Copyright (C) 2016 Marcus A Johansson 3 | -------------------------------------------------------------------------------- /hiqp_msgs/README.md: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework 2 | Copyright (C) 2016 Marcus A Johansson 3 | -------------------------------------------------------------------------------- /hiqp/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hiqp 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | -------------------------------------------------------------------------------- /hiqp_ros/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hiqp_ros 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | -------------------------------------------------------------------------------- /hiqp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(hiqp) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /hiqp_core/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hiqp_core 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | -------------------------------------------------------------------------------- /hiqp_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hiqp_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | -------------------------------------------------------------------------------- /hiqp_ros/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | name: C++ API 3 | output_dir: c++ 4 | file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox' 5 | -------------------------------------------------------------------------------- /hiqp_core/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | name: C++ API 3 | output_dir: c++ 4 | file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox' 5 | -------------------------------------------------------------------------------- /hiqp_ros/plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 8 | The HiQPJointVelocityController generates velocity commands for a set of joints. It expects a VelocityJointInterface type of hardware interface. 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /hiqp/package.xml: -------------------------------------------------------------------------------- 1 | 2 | hiqp 3 | 0.0.1 4 | 5 | 6 | The HiQP Control Framework (including ROS support). 7 | 8 | 9 | Marcus A Johansson 10 | Marcus A Johansson 11 | 12 | GPLv3 13 | 14 | https://github.com/neckutrek/hiqp 15 | 16 | catkin 17 | 18 | hiqp_core 19 | hiqp_msgs 20 | hiqp_ros 21 | hiqp_examples 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /hiqp_msgs/msg/StringArray.msg: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | string[] params -------------------------------------------------------------------------------- /hiqp_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hiqp_msgs 4 | 0.0.1 5 | Messages and services for the HiQP Control Framework 6 | 7 | Marcus A Johansson 8 | Marcus A Johansson 9 | 10 | GPLv3 11 | 12 | catkin 13 | message_generation 14 | geometry_msgs 15 | std_msgs 16 | visualization_msgs 17 | 18 | visualization_msgs 19 | std_msgs 20 | geometry_msgs 21 | message_runtime 22 | 23 | -------------------------------------------------------------------------------- /hiqp_msgs/srv/ListAllTasks.srv: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | --- 18 | bool success # is always true -------------------------------------------------------------------------------- /hiqp_msgs/srv/ListAllPrimitives.srv: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | --- 18 | bool success # is always true -------------------------------------------------------------------------------- /hiqp_msgs/msg/Vector3d.msg: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | float64 val1 18 | float64 val2 19 | float64 val3 -------------------------------------------------------------------------------- /hiqp_msgs/srv/RemoveAllTasks.srv: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | --- 18 | bool success # true upon successful removal of all tasks -------------------------------------------------------------------------------- /hiqp_msgs/srv/RemoveAllPrimitives.srv: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | --- 18 | bool success # true upon successful removal of all primitives -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.sublime-* 2 | .Rhistory 3 | 4 | # Backup and data files 5 | eg/eg.* 6 | *~ 7 | *.autosave 8 | *.bak 9 | *.off 10 | *.x 11 | x.* 12 | 13 | */doxygen 14 | 15 | */doc 16 | 17 | # patch results 18 | *.rej 19 | 20 | # Build products 21 | tmp/* 22 | build/Makefile* 23 | build/CMakeFiles 24 | build/CMakeCache.txt 25 | build/*/Makefile* 26 | build/*/object_script* 27 | build/*/Release/* 28 | build/*/Debug/* 29 | build/*/MinSizeRel/* 30 | build/*/RelWithDebInfo/* 31 | build 32 | 33 | *# 34 | *.# 35 | *.zip 36 | *.a 37 | *.dll 38 | *.lib 39 | *.exe 40 | *.bag 41 | *.o 42 | *.md5sum 43 | 44 | # Qt files 45 | *.pro.user 46 | 47 | # DevStudio files 48 | *.bsc 49 | *.ncb 50 | *.pdb 51 | *.suo 52 | *.user 53 | *.ilk 54 | 55 | # CVS files 56 | CVS/* 57 | */CVS/* 58 | */*/CVS/* 59 | */*/*/CVS/* 60 | *.cvsignore 61 | 62 | # Other files 63 | working/ 64 | Status API Training Shop Blog About 65 | © 2014 GitHub, Inc. Terms Privacy Security Contact 66 | -------------------------------------------------------------------------------- /hiqp_msgs/srv/RemoveTask.srv: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | string name # unique task identifier 18 | --- 19 | bool success # true upon successful removal -------------------------------------------------------------------------------- /hiqp_msgs/srv/RemovePrimitive.srv: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | string name # unique primitive identifier 18 | --- 19 | bool success # true upon successful removal -------------------------------------------------------------------------------- /hiqp_msgs/msg/TaskMeasures.msg: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | time stamp # time stamp of this monitoring data set 18 | TaskMeasure[] task_measures # array of task measures -------------------------------------------------------------------------------- /hiqp_msgs/srv/ActivateTask.srv: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | string name # name of the task to be activated 18 | --- 19 | bool success # true if activation was successful 20 | -------------------------------------------------------------------------------- /hiqp_msgs/srv/ActivatePriorityLevel.srv: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | uint16 priority # priority level to activate 18 | --- 19 | bool success # true if activation was successful 20 | -------------------------------------------------------------------------------- /hiqp_msgs/srv/DeactivateTask.srv: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | string name # name of the task to be deactivated 18 | --- 19 | bool success # true if deactivation was successful 20 | -------------------------------------------------------------------------------- /hiqp_msgs/srv/DeactivatePriorityLevel.srv: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | uint16 priority # priority level to deactivate 18 | --- 19 | bool success # true if deactivation was successful 20 | -------------------------------------------------------------------------------- /hiqp_msgs/srv/MonitorTask.srv: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | string name # name of the task to be monitored 18 | --- 19 | bool success # true if monitor activation was successful 20 | -------------------------------------------------------------------------------- /hiqp_msgs/srv/RemovePriorityLevel.srv: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | uint16 priority # priority level of the tasks to be removed 18 | --- 19 | bool success # true upon successful removal of all primitives -------------------------------------------------------------------------------- /hiqp_msgs/srv/DemonitorTask.srv: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | string name # name of the task to be demonitored 18 | --- 19 | bool success # true if monitoring deactivation was successful 20 | -------------------------------------------------------------------------------- /hiqp_msgs/srv/MonitorPriorityLevel.srv: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | uint16 priority # priority level to activate monitoring for 18 | --- 19 | bool success # true if activation of monitoring was successful 20 | -------------------------------------------------------------------------------- /hiqp_msgs/srv/DemonitorPriorityLevel.srv: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | uint16 priority # priority level to deactivate monitoring for 18 | --- 19 | bool success # true if deactivation of monitoring was successful 20 | -------------------------------------------------------------------------------- /hiqp_msgs/srv/SetPrimitive.srv: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | string name 18 | string type 19 | string frame_id 20 | bool visible 21 | float64[] color 22 | float64[] parameters 23 | --- 24 | bool success -------------------------------------------------------------------------------- /hiqp_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hiqp_core 4 | 0.0.1 5 | The HiQP Control Framework 6 | 7 | Marcus A Johansson 8 | Marcus A Johansson 9 | 10 | GPLv3 11 | 12 | catkin 13 | 14 | roscpp 15 | cmake_modules 16 | controller_interface 17 | visualization_msgs 18 | 19 | orocos_kdl 20 | kdl_parser 21 | hiqp_msgs 22 | 23 | roscpp 24 | controller_interface 25 | visualization_msgs 26 | orocos_kdl 27 | kdl_parser 28 | hiqp_msgs 29 | 30 | -------------------------------------------------------------------------------- /hiqp_msgs/msg/TaskMeasure.msg: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | string task_name # unique string identifier for the task 18 | float64[] e # the task performance value at this time step 19 | float64[] de # the task dynamics value at this time step 20 | float64[] pm # the user-defined custom performance monitoring values for thsi task at this time step 21 | -------------------------------------------------------------------------------- /hiqp_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hiqp_ros 4 | 0.0.1 5 | The HiQP_Controllers package 6 | 7 | Marcus A Johansson 8 | Marcus A Johansson 9 | 10 | GPLv3 11 | 12 | catkin 13 | 14 | roscpp 15 | cmake_modules 16 | controller_interface 17 | visualization_msgs 18 | orocos_kdl 19 | 20 | hiqp_core 21 | hiqp_msgs 22 | 23 | roscpp 24 | controller_interface 25 | visualization_msgs 26 | orocos_kdl 27 | 28 | hiqp_core 29 | hiqp_msgs 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /hiqp_core/FindCASADI.cmake: -------------------------------------------------------------------------------- 1 | find_path(CASADI_INCLUDE_DIR 2 | casadi/core/sx/sx_elem.hpp 3 | HINTS $ENV{CASADI_PREFIX}/include 4 | ) 5 | 6 | if(CASADI_INCLUDE_DIR) 7 | set(CASADI_INCLUDE_DIR ${CASADI_INCLUDE_DIR} ${CASADI_INCLUDE_DIR}/casadi) 8 | set(CASADI_FOUND_INCLUDE TRUE) 9 | message(STATUS "Found CasADi include dir: ${CASADI_INCLUDE_DIR}") 10 | else() 11 | message(STATUS "Could not find CasADi include dir") 12 | endif() 13 | 14 | set(CASADI_LIBS_LIST 15 | casadi_cplex_interface 16 | casadi_nlpsol_ipopt 17 | casadi_lapack_interface 18 | casadi_sundials_interface 19 | casadi_csparse_interface 20 | casadi_knitro_interface 21 | casadi_optimal_control 22 | casadi_integration 23 | casadi_nonlinear_programming 24 | casadi_csparse 25 | casadi_tinyxml 26 | casadi) 27 | 28 | foreach(LIB in ${CASADI_LIBS_LIST}) 29 | find_library(CASADI_LIB_${LIB} 30 | NAMES ${LIB} 31 | HINTS ${CASADI_INCLUDE_DIR}/build/lib $ENV{CASADI_PREFIX}/lib) 32 | if(CASADI_LIB_${LIB}) 33 | #message(STATUS "Found ${LIB}: ${CASADI_LIB_${LIB}}") 34 | set(CASADI_LIBRARIES ${CASADI_LIBRARIES} ${CASADI_LIB_${LIB}}) 35 | else() 36 | #message(STATUS "Could not find lib${LIB}") 37 | endif() 38 | endforeach() 39 | 40 | if(CASADI_LIBRARIES) 41 | message(STATUS "Found CasADi libs: ${CASADI_LIBRARIES}") 42 | else() 43 | message(STATUS "Could not find CasADi libs") 44 | endif() 45 | 46 | if(CASADI_FOUND_INCLUDE AND CASADI_LIBRARIES) 47 | set(CASADI_FOUND TRUE) 48 | endif() 49 | -------------------------------------------------------------------------------- /hiqp_msgs/srv/SetTask.srv: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | string name # the name to be associated with this task instantiation 18 | uint16 priority # the task priority (1 is highest) 19 | bool visible # whether or not the task should be visible initially 20 | bool active # whether or not the task should be active initially 21 | bool monitored # whether or not the task should be monitored initially 22 | string[] def_params # the task definition name and a list of strings that is passed to the init function of the task class 23 | string[] dyn_params # the name of the task dynamics along with its parameters 24 | --- 25 | bool success # true if the task creation was successful 26 | -------------------------------------------------------------------------------- /hiqp_ros/include/hiqp_ros/utilities.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_ROS_UTILITIES_H 18 | #define HIQP_ROS_UTILITIES_H 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | //#include 26 | #include 27 | //#include 28 | 29 | std::ostream& operator<<(std::ostream& os, const KDL::Vector& kdl_vector); 30 | std::ostream& operator<<(std::ostream& os, const KDL::Tree& kdl_tree); 31 | std::ostream& operator<<(std::ostream& os, const KDL::FrameVel& kdl_frame_vel); 32 | std::ostream& operator<<(std::ostream& os, const KDL::JntArrayVel& kdl_joints_vel); 33 | std::ostream& operator<<(std::ostream& os, const KDL::Chain& kdl_chain); 34 | 35 | #endif -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/solvers/casadi_solver.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_CASADI_SOLVER_H 18 | #define HIQP_CASADI_SOLVER_H 19 | 20 | #include 21 | 22 | namespace hiqp 23 | { 24 | 25 | /*! \brief An optimization based solver for a set of stages implemented in CasADi. 26 | * \author Marcus A Johansson */ 27 | class CasADiSolver : public HiQPSolver { 28 | public: 29 | CasADiSolver() {} 30 | ~CasADiSolver() noexcept {} 31 | 32 | bool solve(std::vector& solution); 33 | 34 | private: 35 | CasADiSolver(const CasADiSolver& other) = delete; 36 | CasADiSolver(CasADiSolver&& other) = delete; 37 | CasADiSolver& operator=(const CasADiSolver& other) = delete; 38 | CasADiSolver& operator=(CasADiSolver&& other) noexcept = delete; 39 | }; 40 | 41 | } // namespace hiqp 42 | 43 | #endif // include guard 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/hiqp_time_point.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_TIME_POINT_H 18 | #define HIQP_TIME_POINT_H 19 | 20 | namespace hiqp { 21 | 22 | /*! \brief Represents a time duration in seconds and nanoseconds. 23 | * \author Marcus A Johansson */ 24 | class HiQPTimePoint 25 | { 26 | public: 27 | HiQPTimePoint(); 28 | HiQPTimePoint(unsigned int sec, unsigned int nsec); 29 | HiQPTimePoint(const HiQPTimePoint& other); 30 | ~HiQPTimePoint(); 31 | 32 | double toSec() const; 33 | 34 | inline void setTimePoint(unsigned int sec, unsigned int nsec) 35 | { this->sec_ = sec; this->nsec_ = nsec; } 36 | 37 | inline unsigned int getSec() const { return sec_; } 38 | inline unsigned int getNSec() const { return nsec_; } 39 | 40 | HiQPTimePoint& operator=(const HiQPTimePoint& other); 41 | HiQPTimePoint operator+(const HiQPTimePoint& other) const; 42 | HiQPTimePoint operator-(const HiQPTimePoint& other) const; 43 | HiQPTimePoint& operator+=(const HiQPTimePoint& other); 44 | HiQPTimePoint& operator-=(const HiQPTimePoint& other); 45 | 46 | private: 47 | unsigned int sec_; 48 | unsigned int nsec_; 49 | }; 50 | 51 | } // namespace hiqp 52 | 53 | #endif // include guard -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/solvers/gurobi_solver__original.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_GUROBI_SOLVER_H 18 | #define HIQP_GUROBI_SOLVER_H 19 | 20 | #include 21 | #include 22 | 23 | namespace hiqp 24 | { 25 | /*! \brief An optimization based solver for a set of stages implemented in CasADi. 26 | * \author Robert Krug, Marcus A Johansson */ 27 | class GurobiSolver : public HiQPSolver { 28 | public: 29 | GurobiSolver(); 30 | ~GurobiSolver() noexcept {} 31 | 32 | bool solve(std::vector& solution); 33 | 34 | private: 35 | GurobiSolver(const GurobiSolver& other) = delete; 36 | GurobiSolver(GurobiSolver&& other) = delete; 37 | GurobiSolver& operator=(const GurobiSolver& other) = delete; 38 | GurobiSolver& operator=(GurobiSolver&& other) noexcept = delete; 39 | 40 | void reset(); 41 | 42 | GRBEnv env_; 43 | Eigen::VectorXd x_; // HQP solution (updated sequentially when solving) 44 | Eigen::VectorXd w_; // slack variables (updated sequentially when solving) 45 | Eigen::VectorXd b_; 46 | Eigen::MatrixXd A_; 47 | std::vector senses_; 48 | }; 49 | 50 | } // namespace hiqp 51 | 52 | #endif // include guard 53 | -------------------------------------------------------------------------------- /hiqp_core/src/tasks/tdyn_linear.cpp: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | 19 | #include 20 | 21 | #include 22 | 23 | namespace hiqp 24 | { 25 | namespace tasks 26 | { 27 | 28 | int TDynLinear::init(const std::vector& parameters, 29 | RobotStatePtr robot_state, 30 | const Eigen::VectorXd& e_initial, 31 | const Eigen::VectorXd& e_final) { 32 | int size = parameters.size(); 33 | if (size != 2) { 34 | printHiqpWarning("TDynFirstOrder requires 2 parameters, got " 35 | + std::to_string(size) + "! Initialization failed!"); 36 | return -1; 37 | } 38 | 39 | lambda_ = std::stod( parameters.at(1) ); 40 | 41 | e_dot_star_.resize(e_initial.rows()); 42 | performance_measures_.resize(e_initial.rows()); 43 | 44 | return 0; 45 | } 46 | 47 | int TDynLinear::update(RobotStatePtr robot_state, 48 | const Eigen::VectorXd& e, 49 | const Eigen::MatrixXd& J) { 50 | //e_dot_star_.resize(e.size()); 51 | e_dot_star_ = -lambda_ * e; 52 | return 0; 53 | } 54 | 55 | int TDynLinear::monitor() { 56 | return 0; 57 | } 58 | 59 | } // namespace tasks 60 | 61 | } // namespace hiqp -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework 2 | Copyright (C) 2016-2017 Marcus A. Johansson 3 | 4 | HiQP is a whole-body control framework for computing inverse kinematics (and dynamics in future releases) involving multiple objectives which can be incompatible. It uses the inequality task function approach in [1] and is based on hierarchical least-squares optimization. Lower-ranked tasks are fulfilled as good as possible (in the least-square sense) in the null-space of higher-ranked tasks. More details are given in [2] - available at http://www.diva-portal.org/smash/get/diva2:1056500/FULLTEXT01.pdf. 5 | 6 | ## Acknowledgements 7 | Robert Krug and Todor Stoyanov at the AASS Research Institute at Örebro University, Sweden, have been important contributors to this project. Thank you. 8 | 9 | ## Upcoming features/changes 10 | - A Wiki/user guide will be created as a tutorial on how to use HiQP with ROS. In the meantime, we refer to the hiqp_demos package for examples of how to use HiQP: git@github.com:OrebroUniversity/hiqp_demos.git 11 | - The list of available service calls will be revised and extended. 12 | - A capsule primitive will be added to the set of geometric primitives. 13 | - The framework will be extended to compute controls on a joint acceleration level in order to enable inverse dynamics control. 14 | 15 | ## Installation note 16 | 17 | HiQP relies on Gurobi to solve Quadratic Programs (QPs). For academic use, a free license can be obtained at http://www.gurobi.com/. Make sure to set the corresponding environment variables as described in http://www.gurobi.com/documentation/6.5/quickstart_linux/software_installation_guid.html. 18 | 19 | ## How to cite HiQP 20 | 21 | A publication presenting HiQP is in preparation. In the meantime, please refer to [2]. 22 | 23 |
24 | [1] ... O. Kanoun, F. Lamiraux and P.-B. Wieber, Kinematic control of redundant manipulators: Generalizing the task-priority framework to inequality task. IEEE T-RO, 27(4):785-792, 2011. 25 |
26 | [2] ... M. A. Johansson, Online whole-body control using hierarchical quadratic programming: implementation and evaluation of the HiQP control framework. MSc Thesis, 2016. 27 | -------------------------------------------------------------------------------- /hiqp_core/src/tasks/tdyn_jnt_limits.cpp: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | 19 | #include 20 | 21 | namespace hiqp 22 | { 23 | namespace tasks 24 | { 25 | 26 | int TDynJntLimits::init(const std::vector& parameters, 27 | RobotStatePtr robot_state, 28 | const Eigen::VectorXd& e_initial, 29 | const Eigen::VectorXd& e_final) { 30 | int size = parameters.size(); 31 | if (size != 3) { 32 | printHiqpWarning("TDynJntLimits requires 3 parameters, got " 33 | + std::to_string(size) + "! Initialization failed!"); 34 | return -1; 35 | } 36 | 37 | e_dot_star_.resize(4); 38 | dq_max_ = std::stod( parameters.at(1) ); 39 | gain_ = std::stod( parameters.at(2) ); 40 | performance_measures_.resize(0); 41 | return 0; 42 | } 43 | 44 | int TDynJntLimits::update(RobotStatePtr robot_state, 45 | const Eigen::VectorXd& e, 46 | const Eigen::MatrixXd& J) { 47 | e_dot_star_(0) = -dq_max_; 48 | e_dot_star_(1) = dq_max_; 49 | e_dot_star_(2) = -gain_ * e(2); 50 | e_dot_star_(3) = -gain_ * e(3); 51 | return 0; 52 | } 53 | 54 | int TDynJntLimits::monitor() { 55 | return 0; 56 | } 57 | 58 | } // namespace tasks 59 | 60 | } // namespace hiqp -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/tasks/tdef_full_pose.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_TDEF_FULL_POSE_H 18 | #define HIQP_TDEF_FULL_POSE_H 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | namespace hiqp 26 | { 27 | namespace tasks 28 | { 29 | 30 | /*! \brief Represents a task definition that sets a specific joint configuration. This task definition does not leave any redundancy available to other tasks! 31 | * \author Marcus A Johansson */ 32 | class TDefFullPose : public TaskDefinition { 33 | public: 34 | TDefFullPose(std::shared_ptr geom_prim_map, 35 | std::shared_ptr visualizer) 36 | : TaskDefinition(geom_prim_map, visualizer) {} 37 | 38 | ~TDefFullPose() noexcept {} 39 | 40 | int init(const std::vector& parameters, 41 | RobotStatePtr robot_state); 42 | 43 | int update(RobotStatePtr robot_state); 44 | 45 | int monitor(); 46 | 47 | private: 48 | TDefFullPose(const TDefFullPose& other) = delete; 49 | TDefFullPose(TDefFullPose&& other) = delete; 50 | TDefFullPose& operator=(const TDefFullPose& other) = delete; 51 | TDefFullPose& operator=(TDefFullPose&& other) noexcept = delete; 52 | 53 | std::vector desired_configuration_; 54 | }; 55 | 56 | } // namespace tasks 57 | 58 | } // namespace hiqp 59 | 60 | #endif // include guard -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/tasks/tdef_jnt_config.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_TDEF_JNT_CONFIG_H 18 | #define HIQP_TDEF_JNT_CONFIG_H 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | namespace hiqp 26 | { 27 | namespace tasks 28 | { 29 | 30 | /*! \brief A task definition that sets a specific joints position. 31 | * \author Marcus A Johansson */ 32 | class TDefJntConfig : public TaskDefinition { 33 | public: 34 | TDefJntConfig(std::shared_ptr geom_prim_map, 35 | std::shared_ptr visualizer) 36 | : TaskDefinition(geom_prim_map, visualizer) {} 37 | ~TDefJntConfig() noexcept {} 38 | 39 | int init(const std::vector& parameters, 40 | RobotStatePtr robot_state); 41 | 42 | int update(RobotStatePtr robot_state); 43 | 44 | int monitor(); 45 | 46 | private: 47 | TDefJntConfig(const TDefJntConfig& other) = delete; 48 | TDefJntConfig(TDefJntConfig&& other) = delete; 49 | TDefJntConfig& operator=(const TDefJntConfig& other) = delete; 50 | TDefJntConfig& operator=(TDefJntConfig&& other) noexcept = delete; 51 | 52 | std::string link_name_; 53 | int joint_q_nr_; 54 | double desired_configuration_; 55 | }; 56 | 57 | } // namespace tasks 58 | 59 | } // namespace hiqp 60 | 61 | #endif // include guard -------------------------------------------------------------------------------- /hiqp_core/src/tasks/tdyn_hyper_sin.cpp: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | #include 23 | 24 | namespace hiqp 25 | { 26 | namespace tasks 27 | { 28 | 29 | int TDynHyperSin::init(const std::vector& parameters, 30 | RobotStatePtr robot_state, 31 | const Eigen::VectorXd& e_initial, 32 | const Eigen::VectorXd& e_final) { 33 | int size = parameters.size(); 34 | if (size != 2) { 35 | printHiqpWarning("TDynFirstOrder requires 2 parameters, got " 36 | + std::to_string(size) + "! Initialization failed!"); 37 | return -1; 38 | } 39 | 40 | lambda_ = std::stod( parameters.at(1) ); 41 | 42 | e_dot_star_.resize(e_initial.rows()); 43 | performance_measures_.resize(e_initial.rows()); 44 | 45 | return 0; 46 | } 47 | 48 | int TDynHyperSin::update(RobotStatePtr robot_state, 49 | const Eigen::VectorXd& e, 50 | const Eigen::MatrixXd& J) { 51 | e_dot_star_.resize(e.size()); 52 | for (unsigned int i=0; i. 16 | 17 | #include 18 | 19 | #include 20 | 21 | namespace hiqp 22 | { 23 | namespace tasks 24 | { 25 | 26 | int TDynCubic::init(const std::vector& parameters, 27 | RobotStatePtr robot_state, 28 | const Eigen::VectorXd& e_initial, 29 | const Eigen::VectorXd& e_final) { 30 | int size = parameters.size(); 31 | if (size != 2) { 32 | printHiqpWarning("TDynFirstOrderCubic requires 2 parameters, got " 33 | + std::to_string(size) + "! Initialization failed!"); 34 | return -1; 35 | } 36 | 37 | lambda_ = std::stod( parameters.at(1) ); 38 | e_dot_star_.resize(e_initial.rows()); 39 | performance_measures_.resize(e_initial.rows()); 40 | return 0; 41 | } 42 | 43 | int TDynCubic::update(RobotStatePtr robot_state, 44 | const Eigen::VectorXd& e, 45 | const Eigen::MatrixXd& J) { 46 | e_dot_star_.resize(e.size()); 47 | std::cout << "i'mhere\n"; 48 | for (unsigned int i=0; i. 16 | 17 | #ifndef HIQP_TDYN_CUBIC_H 18 | #define HIQP_TDYN_CUBIC_H 19 | 20 | #include 21 | #include 22 | 23 | namespace hiqp 24 | { 25 | namespace tasks 26 | { 27 | 28 | /*! \brief A cubic dynamics similar to first-order dynamics but with slower convergence around e=0. 29 | * \author Marcus A Johansson */ 30 | class TDynCubic : public TaskDynamics { 31 | public: 32 | TDynCubic(std::shared_ptr geom_prim_map, 33 | std::shared_ptr visualizer) 34 | : TaskDynamics(geom_prim_map, visualizer) {} 35 | 36 | ~TDynCubic() noexcept {} 37 | 38 | int init(const std::vector& parameters, 39 | RobotStatePtr robot_state, 40 | const Eigen::VectorXd& e_initial, 41 | const Eigen::VectorXd& e_final); 42 | 43 | int update(RobotStatePtr robot_state, 44 | const Eigen::VectorXd& e, 45 | const Eigen::MatrixXd& J); 46 | 47 | int monitor(); 48 | 49 | private: 50 | TDynCubic(const TDynCubic& other) = delete; 51 | TDynCubic(TDynCubic&& other) = delete; 52 | TDynCubic& operator=(const TDynCubic& other) = delete; 53 | TDynCubic& operator=(TDynCubic&& other) noexcept = delete; 54 | 55 | double lambda_; 56 | }; 57 | 58 | } // namespace tasks 59 | 60 | } // namespace hiqp 61 | 62 | #endif // include guard 63 | -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/tasks/tdyn_linear.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_TDYN_LINEAR_H 18 | #define HIQP_TDYN_LINEAR_H 19 | 20 | #include 21 | #include 22 | 23 | namespace hiqp 24 | { 25 | namespace tasks 26 | { 27 | 28 | /*! \brief A general first-order task dynamics implementation that enforces an exponential decay of the task performance value. 29 | * \author Marcus A Johansson */ 30 | class TDynLinear : public TaskDynamics { 31 | public: 32 | TDynLinear(std::shared_ptr geom_prim_map, 33 | std::shared_ptr visualizer) 34 | : TaskDynamics(geom_prim_map, visualizer) {} 35 | 36 | ~TDynLinear() noexcept {} 37 | 38 | int init(const std::vector& parameters, 39 | RobotStatePtr robot_state, 40 | const Eigen::VectorXd& e_initial, 41 | const Eigen::VectorXd& e_final); 42 | 43 | int update(RobotStatePtr robot_state, 44 | const Eigen::VectorXd& e, 45 | const Eigen::MatrixXd& J); 46 | 47 | int monitor(); 48 | 49 | private: 50 | TDynLinear(const TDynLinear& other) = delete; 51 | TDynLinear(TDynLinear&& other) = delete; 52 | TDynLinear& operator=(const TDynLinear& other) = delete; 53 | TDynLinear& operator=(TDynLinear&& other) noexcept = delete; 54 | 55 | double lambda_; 56 | }; 57 | 58 | } // namespace tasks 59 | 60 | } // namespace hiqp 61 | 62 | #endif // include guard -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/tasks/tdef_jnt_limits.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_TDEF_JNT_LIMITS_H 18 | #define HIQP_TDEF_JNT_LIMITS_H 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | namespace hiqp 26 | { 27 | namespace tasks 28 | { 29 | 30 | /*! \brief A task definition that sets velocity and position limitations of a specific joint. 31 | * \author Marcus A Johansson */ 32 | class TDefJntLimits : public TaskDefinition { 33 | public: 34 | TDefJntLimits(std::shared_ptr geom_prim_map, 35 | std::shared_ptr visualizer) 36 | : TaskDefinition(geom_prim_map, visualizer) {} 37 | 38 | ~TDefJntLimits() noexcept = default; 39 | 40 | int init(const std::vector& parameters, 41 | RobotStatePtr robot_state); 42 | 43 | int update(RobotStatePtr robot_state); 44 | 45 | int monitor(); 46 | 47 | private: 48 | TDefJntLimits(const TDefJntLimits& other) = delete; 49 | TDefJntLimits(TDefJntLimits&& other) = delete; 50 | TDefJntLimits& operator=(const TDefJntLimits& other) = delete; 51 | TDefJntLimits& operator=(TDefJntLimits&& other) noexcept = delete; 52 | 53 | std::string link_frame_name_; 54 | std::size_t link_frame_q_nr_; 55 | double jnt_upper_bound_; 56 | double jnt_lower_bound_; 57 | 58 | }; 59 | 60 | } // namespace tasks 61 | 62 | } // namespace hiqp 63 | 64 | #endif // include guard -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/tasks/tdyn_jnt_limits.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_TDYN_JNT_LIMITS_H 18 | #define HIQP_TDYN_JNT_LIMITS_H 19 | 20 | #include 21 | #include 22 | 23 | namespace hiqp 24 | { 25 | namespace tasks 26 | { 27 | 28 | /*! \brief A special task dynamics to be used only with the TDefJntLimits task definition. 29 | * \author Marcus A Johansson */ 30 | class TDynJntLimits : public TaskDynamics { 31 | public: 32 | TDynJntLimits(std::shared_ptr geom_prim_map, 33 | std::shared_ptr visualizer) 34 | : TaskDynamics(geom_prim_map, visualizer) {} 35 | 36 | ~TDynJntLimits() noexcept = default; 37 | 38 | int init(const std::vector& parameters, 39 | RobotStatePtr robot_state, 40 | const Eigen::VectorXd& e_initial, 41 | const Eigen::VectorXd& e_final); 42 | 43 | int update(RobotStatePtr robot_state, 44 | const Eigen::VectorXd& e, 45 | const Eigen::MatrixXd& J); 46 | 47 | int monitor(); 48 | 49 | private: 50 | TDynJntLimits(const TDynJntLimits& other) = delete; 51 | TDynJntLimits(TDynJntLimits&& other) = delete; 52 | TDynJntLimits& operator=(const TDynJntLimits& other) = delete; 53 | TDynJntLimits& operator=(TDynJntLimits&& other) noexcept = delete; 54 | 55 | double dq_max_; 56 | double gain_; 57 | 58 | }; 59 | 60 | } // namespace tasks 61 | 62 | } // namespace hiqp 63 | 64 | #endif // include guard -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/tasks/tdyn_hyper_sin.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_TDYN_HYPER_SIN_H 18 | #define HIQP_TDYN_HYPER_SIN_H 19 | 20 | #include 21 | #include 22 | 23 | namespace hiqp 24 | { 25 | namespace tasks 26 | { 27 | 28 | /*! \brief A general hyperbolic sin task dynamics. Useful for only influencing the manipulator behaviour for task function values within a certain span. 29 | * \author Marcus A Johansson */ 30 | class TDynHyperSin : public TaskDynamics 31 | { 32 | public: 33 | 34 | TDynHyperSin(std::shared_ptr geom_prim_map, 35 | std::shared_ptr visualizer) 36 | : TaskDynamics(geom_prim_map, visualizer) {} 37 | 38 | ~TDynHyperSin() noexcept {} 39 | 40 | int init(const std::vector& parameters, 41 | RobotStatePtr robot_state, 42 | const Eigen::VectorXd& e_initial, 43 | const Eigen::VectorXd& e_final); 44 | 45 | int update(RobotStatePtr robot_state, 46 | const Eigen::VectorXd& e, 47 | const Eigen::MatrixXd& J); 48 | 49 | int monitor(); 50 | 51 | private: 52 | TDynHyperSin(const TDynHyperSin& other) = delete; 53 | TDynHyperSin(TDynHyperSin&& other) = delete; 54 | TDynHyperSin& operator=(const TDynHyperSin& other) = delete; 55 | TDynHyperSin& operator=(TDynHyperSin&& other) noexcept = delete; 56 | 57 | double lambda_; 58 | }; 59 | 60 | } // namespace tasks 61 | 62 | } // namespace hiqp 63 | 64 | #endif // include guard -------------------------------------------------------------------------------- /hiqp_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | cmake_minimum_required(VERSION 2.8.3) 18 | project(hiqp_msgs) 19 | 20 | find_package(catkin REQUIRED COMPONENTS message_generation 21 | std_msgs 22 | visualization_msgs 23 | geometry_msgs) 24 | 25 | add_message_files(FILES TaskMeasure.msg 26 | TaskMeasures.msg 27 | StringArray.msg 28 | Vector3d.msg) 29 | 30 | add_service_files(FILES SetTask.srv 31 | RemoveTask.srv 32 | RemoveAllTasks.srv 33 | ListAllTasks.srv 34 | ActivateTask.srv 35 | DeactivateTask.srv 36 | MonitorTask.srv 37 | DemonitorTask.srv 38 | SetPrimitive.srv 39 | RemovePrimitive.srv 40 | RemoveAllPrimitives.srv 41 | ListAllPrimitives.srv 42 | RemovePriorityLevel.srv 43 | ActivatePriorityLevel.srv 44 | DeactivatePriorityLevel.srv 45 | MonitorPriorityLevel.srv 46 | DemonitorPriorityLevel.srv) 47 | 48 | generate_messages(DEPENDENCIES std_msgs 49 | geometry_msgs 50 | visualization_msgs) 51 | 52 | catkin_package(CATKIN_DEPENDS message_runtime 53 | std_msgs 54 | visualization_msgs 55 | geometry_msgs) -------------------------------------------------------------------------------- /hiqp_core/src/tasks/tdef_jnt_config.cpp: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | namespace hiqp 23 | { 24 | namespace tasks 25 | { 26 | 27 | 28 | int TDefJntConfig::init(const std::vector& parameters, 29 | RobotStatePtr robot_state) { 30 | int size = parameters.size(); 31 | if (size != 3) { 32 | printHiqpWarning("TDefJntConfig requires 3 parameters, got " 33 | + std::to_string(size) + "! Initialization failed!"); 34 | return -1; 35 | } 36 | 37 | link_name_ = parameters.at(1); 38 | 39 | joint_q_nr_ = kdl_getQNrFromLinkName(robot_state->kdl_tree_, link_name_); 40 | 41 | if (joint_q_nr_ < 0) { 42 | printHiqpWarning("TDefJntConfig::init, couldn't find joint '" + link_name_ + "'! Initialization failed."); 43 | return -2; 44 | } 45 | 46 | if (!robot_state->isQNrWritable(joint_q_nr_)) { 47 | printHiqpWarning("TDefJntConfig::init, the joint '" + link_name_ + "' is not writable! Initialization failed."); 48 | return -3; 49 | } 50 | 51 | desired_configuration_ = std::stod( parameters.at(2) ); 52 | 53 | unsigned int n_joints = robot_state->getNumJoints(); 54 | e_.resize(1); 55 | J_.resize(1, n_joints); 56 | performance_measures_.resize(0); 57 | task_types_.insert(task_types_.begin(), 1, 0); 58 | 59 | for (int i=0; ikdl_jnt_array_vel_.q; 69 | e_(0) = desired_configuration_ - q(joint_q_nr_); 70 | return 0; 71 | } 72 | 73 | int TDefJntConfig::monitor() { 74 | return 0; 75 | } 76 | 77 | 78 | } // namespace tasks 79 | 80 | } // namespace hiqp -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/tasks/tdyn_minimal_jerk.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_TDYN_MINIMAL_JERK_H 18 | #define HIQP_TDYN_MINIMAL_JERK_H 19 | 20 | #include 21 | #include 22 | 23 | namespace hiqp 24 | { 25 | namespace tasks 26 | { 27 | 28 | /*! \brief A task dynamics that enforces minimal jerk throughout the whole motion. 29 | * \author Marcus A Johansson */ 30 | class TDynMinimalJerk : public TaskDynamics { 31 | public: 32 | TDynMinimalJerk(std::shared_ptr geom_prim_map, 33 | std::shared_ptr visualizer) 34 | : TaskDynamics(geom_prim_map, visualizer) {} 35 | 36 | ~TDynMinimalJerk() noexcept = default; 37 | 38 | int init(const std::vector& parameters, 39 | RobotStatePtr robot_state, 40 | const Eigen::VectorXd& e_initial, 41 | const Eigen::VectorXd& e_final); 42 | 43 | int update(RobotStatePtr robot_state, 44 | const Eigen::VectorXd& e, 45 | const Eigen::MatrixXd& J); 46 | 47 | int monitor(); 48 | 49 | private: 50 | TDynMinimalJerk(const TDynMinimalJerk& other) = delete; 51 | TDynMinimalJerk(TDynMinimalJerk&& other) = delete; 52 | TDynMinimalJerk& operator=(const TDynMinimalJerk& other) = delete; 53 | TDynMinimalJerk& operator=(TDynMinimalJerk&& other) noexcept = delete; 54 | 55 | HiQPTimePoint time_start_; 56 | double total_duration_; 57 | double gain_; 58 | Eigen::VectorXd e_initial_; 59 | Eigen::VectorXd e_final_; 60 | Eigen::VectorXd e_diff_; 61 | double f_; 62 | }; 63 | 64 | } // namespace tasks 65 | 66 | } // namespace hiqp 67 | 68 | #endif // include guard -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/geometric_primitives/geometric_primitive_visitor.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_GEOMETRIC_PRIMITIVE_VISITOR_H 18 | #define HIQP_GEOMETRIC_PRIMITIVE_VISITOR_H 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | namespace hiqp { 31 | 32 | namespace geometric_primitives { 33 | 34 | /*! \brief 35 | * {author Marcus A Johansson */ 36 | class GeometricPrimitiveVisitor { 37 | public: 38 | GeometricPrimitiveVisitor() {} 39 | ~GeometricPrimitiveVisitor() noexcept {} 40 | 41 | virtual void visit(std::shared_ptr point) = 0; 42 | virtual void visit(std::shared_ptr line) = 0; 43 | virtual void visit(std::shared_ptr plane) = 0; 44 | virtual void visit(std::shared_ptr box) = 0; 45 | virtual void visit(std::shared_ptr cylinder) = 0; 46 | virtual void visit(std::shared_ptr sphere) = 0; 47 | virtual void visit(std::shared_ptr frame) = 0; 48 | 49 | private: 50 | GeometricPrimitiveVisitor(const GeometricPrimitiveVisitor& other) = delete; 51 | GeometricPrimitiveVisitor(GeometricPrimitiveVisitor&& other) = delete; 52 | GeometricPrimitiveVisitor& operator=(const GeometricPrimitiveVisitor& other) = delete; 53 | GeometricPrimitiveVisitor& operator=(GeometricPrimitiveVisitor&& other) noexcept = delete; 54 | }; 55 | 56 | } // namespace geometric_primitives 57 | 58 | } // namespace hiqp 59 | 60 | #endif // include guard -------------------------------------------------------------------------------- /hiqp_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | cmake_minimum_required(VERSION 2.8.6) 18 | project(hiqp_ros) 19 | 20 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}) 21 | set(CMAKE_CXX_FLAGS "-O3 -std=c++11 -Wl,-z,defs") 22 | 23 | set(GUROBI_INCLUDE_DIR "$ENV{GUROBI_HOME}/include") 24 | set(GUROBI_LIB_DIR "$ENV{GUROBI_HOME}/lib") 25 | set(GUROBI_LIBS gurobi_c++ gurobi65) 26 | 27 | find_package(catkin REQUIRED COMPONENTS roscpp 28 | controller_interface 29 | visualization_msgs 30 | hiqp_core 31 | hiqp_msgs) 32 | 33 | find_package(orocos_kdl REQUIRED) 34 | find_package(Eigen3 REQUIRED) 35 | 36 | catkin_package(CATKIN_DEPENDS roscpp controller_interface visualization_msgs hiqp_core hiqp_msgs 37 | INCLUDE_DIRS include 38 | LIBRARIES ${PROJECT_NAME} 39 | DEPENDS orocos_kdl) 40 | 41 | include_directories(include ${catkin_INCLUDE_DIRS}) 42 | include_directories(${GUROBI_INCLUDE_DIR}) 43 | 44 | link_directories(${GUROBI_LIB_DIR}) 45 | 46 | add_library(${PROJECT_NAME} src/hiqp_joint_velocity_controller.cpp 47 | src/hiqp_service_handler.cpp 48 | src/ros_topic_subscriber.cpp 49 | src/ros_visualizer.cpp 50 | src/utilities.cpp) 51 | 52 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES} ${GUROBI_LIBS}) 53 | 54 | install(DIRECTORY include/${PROJECT_NAME}/ 55 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 56 | FILES_MATCHING PATTERN "*.h") 57 | 58 | install(TARGETS ${PROJECT_NAME} 59 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 60 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 61 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 62 | 63 | install(FILES plugins.xml 64 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 65 | -------------------------------------------------------------------------------- /hiqp_core/src/tasks/tdyn_minimal_jerk.cpp: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | 19 | #include 20 | 21 | namespace hiqp 22 | { 23 | namespace tasks 24 | { 25 | 26 | int TDynMinimalJerk::init(const std::vector& parameters, 27 | RobotStatePtr robot_state, 28 | const Eigen::VectorXd& e_initial, 29 | const Eigen::VectorXd& e_final) { 30 | int size = parameters.size(); 31 | if (size != 3) { 32 | printHiqpWarning("TDynMinimalJerk requires 3 parameters, got " 33 | + std::to_string(size) + "! Initialization failed!"); 34 | return -1; 35 | } 36 | 37 | performance_measures_.resize(e_initial.rows()); 38 | e_dot_star_.resize(e_initial.rows()); 39 | 40 | time_start_ = robot_state->sampling_time_point_; 41 | 42 | total_duration_ = std::stod( parameters.at(1) ); 43 | gain_ = std::stod( parameters.at(2) ); 44 | 45 | f_ = 30 / total_duration_; 46 | 47 | e_initial_ = e_initial; 48 | e_final_ = e_final; 49 | e_diff_ = e_final - e_initial; 50 | 51 | return 0; 52 | } 53 | 54 | int TDynMinimalJerk::update(RobotStatePtr robot_state, 55 | const Eigen::VectorXd& e, 56 | const Eigen::MatrixXd& J) { 57 | e_dot_star_.resize(e.size()); 58 | double tau = (robot_state->sampling_time_point_ - time_start_).toSec() / total_duration_; 59 | 60 | if (tau > 1) { 61 | e_dot_star_ = 0*e; 62 | } else { 63 | double T = 10*tau*tau*tau - 15*tau*tau*tau*tau + 6*tau*tau*tau*tau*tau; 64 | double t = f_ * (tau*tau - 2*tau*tau*tau + tau*tau*tau*tau); 65 | 66 | Eigen::VectorXd e_star = e_initial_ + e_diff_ * T; 67 | 68 | e_dot_star_ = e_diff_ * t - gain_ * (e - e_star); // minimal jerk + first order 69 | } 70 | 71 | return 0; 72 | } 73 | 74 | int TDynMinimalJerk::monitor() { 75 | return 0; 76 | } 77 | 78 | } // namespace tasks 79 | 80 | } // namespace hiqp -------------------------------------------------------------------------------- /hiqp_ros/include/hiqp_ros/ros_topic_subscriber.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_ROS_TOPIC_SUBSCRIBER_H 18 | #define HIQP_ROS_TOPIC_SUBSCRIBER_H 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | #include "ros/ros.h" 27 | 28 | using hiqp::geometric_primitives::GeometricPrimitiveMap; 29 | using hiqp::geometric_primitives::GeometricPoint; 30 | using hiqp::geometric_primitives::GeometricLine; 31 | using hiqp::geometric_primitives::GeometricPlane; 32 | using hiqp::geometric_primitives::GeometricBox; 33 | using hiqp::geometric_primitives::GeometricCylinder; 34 | using hiqp::geometric_primitives::GeometricSphere; 35 | using hiqp::geometric_primitives::GeometricFrame; 36 | 37 | namespace hiqp_ros 38 | { 39 | 40 | class ROSTopicSubscriber { 41 | public: 42 | 43 | ROSTopicSubscriber() {} 44 | ~ROSTopicSubscriber() {} 45 | 46 | int init(hiqp::TaskManager* task_manager) { 47 | task_manager_ = task_manager; 48 | } 49 | 50 | template 51 | int addSubscription(ros::NodeHandle &controller_nh, 52 | const std::string& topic_name, 53 | unsigned int buffer_size) { 54 | sub = controller_nh.subscribe( 55 | topic_name, buffer_size, &ROSTopicSubscriber::topicCallback, this); 56 | ROS_INFO_STREAM("Subsribed to topic '" << topic_name << "'"); 57 | } 58 | 59 | /*! \brief Implement this function for your own message! */ 60 | template 61 | void topicCallback(const ROSMessageType& msg); 62 | 63 | private: 64 | ROSTopicSubscriber(const ROSTopicSubscriber& other) = delete; 65 | ROSTopicSubscriber(ROSTopicSubscriber&& other) = delete; 66 | ROSTopicSubscriber& operator=(const ROSTopicSubscriber& other) = delete; 67 | ROSTopicSubscriber& operator=(ROSTopicSubscriber&& other) noexcept = delete; 68 | 69 | ros::Subscriber sub; 70 | 71 | hiqp::TaskManager* task_manager_; 72 | 73 | }; 74 | 75 | } // namespace hiqp 76 | 77 | #endif // Include guard -------------------------------------------------------------------------------- /hiqp_core/src/tasks/tdef_full_pose.cpp: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | 19 | #include 20 | 21 | #include 22 | 23 | namespace hiqp 24 | { 25 | namespace tasks 26 | { 27 | 28 | int TDefFullPose::init(const std::vector& parameters, 29 | RobotStatePtr robot_state) { 30 | int size = parameters.size(); 31 | unsigned int n_controls = robot_state->getNumControls(); 32 | unsigned int n_joints = robot_state->getNumJoints(); 33 | if (size != 1 && size != n_controls + 1) { 34 | printHiqpWarning("TDefFullPose requires 1 or " 35 | + std::to_string(n_controls+1) + " parameters, got " 36 | + std::to_string(size) + "! Initialization failed!"); 37 | return -1; 38 | } 39 | 40 | if (size == 1) { 41 | desired_configuration_ = std::vector(n_controls, 0); 42 | } else { 43 | desired_configuration_.resize(0); 44 | for (int i=1; i < n_controls+1; ++i) { 45 | desired_configuration_.push_back( std::stod( parameters.at(i) ) ); 46 | 47 | } 48 | } 49 | 50 | e_ = Eigen::VectorXd::Zero(n_controls); 51 | J_ = Eigen::MatrixXd::Zero(n_controls, n_joints); 52 | performance_measures_.resize(0); 53 | task_types_.insert(task_types_.begin(), n_controls, 0); 54 | 55 | // The jacobian has zero-columns for non-writable joints 56 | // -1 0 0 0 0 57 | // 0 -1 0 0 0 58 | // 0 0 0 -1 0 59 | for (int c=0, r=0; cisQNrWritable(c)) { 61 | J_(r, c) = -1; 62 | r++; 63 | } 64 | } 65 | 66 | return 0; 67 | } 68 | 69 | int TDefFullPose::update(RobotStatePtr robot_state) { 70 | const KDL::JntArray &q = robot_state->kdl_jnt_array_vel_.q; 71 | int j=0; 72 | for (int i=0; iisQNrWritable(i)) { 74 | e_(j) = desired_configuration_.at(j) - q(i); 75 | j++; 76 | } 77 | } 78 | return 0; 79 | } 80 | 81 | int TDefFullPose::monitor() { 82 | return 0; 83 | } 84 | 85 | } // namespace tasks 86 | 87 | } // namespace hiqp 88 | -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/geometric_primitives/geometric_primitive_couter.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_GEOMETRIC_PRIMITIVE_COUTER_H 18 | #define HIQP_GEOMETRIC_PRIMITIVE_COUTER_H 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | namespace hiqp { 26 | 27 | namespace geometric_primitives { 28 | 29 | /*! \brief A Geometric Primitive Visitor that prints info about the primitive it visits to std::cout. 30 | * \author Marcus A Johansson */ 31 | class GeometricPrimitiveCouter : public GeometricPrimitiveVisitor { 32 | public: 33 | GeometricPrimitiveCouter() = default; 34 | 35 | ~GeometricPrimitiveCouter() noexcept = default; 36 | 37 | void visit(std::shared_ptr point) { print(point); std::cout << "point\n"; } 38 | void visit(std::shared_ptr line) { print(line); std::cout << "line\n"; } 39 | void visit(std::shared_ptr plane) { print(plane); std::cout << "plane\n"; } 40 | void visit(std::shared_ptr box) { print(box); std::cout << "box\n"; } 41 | void visit(std::shared_ptr cylinder) { print(cylinder); std::cout << "cylinder\n"; } 42 | void visit(std::shared_ptr sphere) { print(sphere); std::cout << "sphere\n"; } 43 | void visit(std::shared_ptr frame) { print(frame); std::cout << "frame\n"; } 44 | 45 | 46 | private: 47 | GeometricPrimitiveCouter(const GeometricPrimitiveCouter& other) = delete; 48 | GeometricPrimitiveCouter(GeometricPrimitiveCouter&& other) = delete; 49 | GeometricPrimitiveCouter& operator=(const GeometricPrimitiveCouter& other) = delete; 50 | GeometricPrimitiveCouter& operator=(GeometricPrimitiveCouter&& other) noexcept = delete; 51 | 52 | template 53 | void print(std::shared_ptr primitive); 54 | 55 | }; 56 | 57 | template 58 | void GeometricPrimitiveCouter::print(std::shared_ptr primitive) { 59 | std::cout << primitive->getName() << ", " 60 | << primitive->getFrameId() << ", " 61 | << primitive->isVisible() << ", " 62 | << primitive->getVisualId() << ", "; 63 | } 64 | 65 | } // namespace geometric_primitives 66 | 67 | } // namespace hiqp 68 | 69 | #endif // include guard -------------------------------------------------------------------------------- /hiqp_core/src/hiqp_time_point.cpp: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | 19 | namespace hiqp { 20 | 21 | HiQPTimePoint::HiQPTimePoint() : sec_(0), nsec_(0) {} 22 | 23 | HiQPTimePoint::HiQPTimePoint(unsigned int sec, unsigned int nsec) 24 | : sec_(sec), nsec_(nsec) {} 25 | 26 | HiQPTimePoint::HiQPTimePoint(const HiQPTimePoint& other) 27 | : sec_(other.getSec()), nsec_(other.getNSec()) {} 28 | 29 | HiQPTimePoint::~HiQPTimePoint() {} 30 | 31 | double HiQPTimePoint::toSec() const { 32 | return static_cast(sec_) + static_cast(nsec_) * 1e-9; 33 | } 34 | 35 | HiQPTimePoint& HiQPTimePoint::operator=(const HiQPTimePoint& other) { 36 | this->sec_ = other.getSec(); 37 | this->nsec_ = other.getNSec(); 38 | return *this; 39 | } 40 | 41 | HiQPTimePoint HiQPTimePoint::operator+(const HiQPTimePoint& other) const { 42 | unsigned int sec = sec_ + other.getSec(); 43 | unsigned int nsec = nsec_ + other.getNSec(); 44 | unsigned int overlap = nsec * 1e-9; 45 | return HiQPTimePoint( sec + overlap, nsec - overlap * 1e9 ); 46 | } 47 | 48 | HiQPTimePoint HiQPTimePoint::operator-(const HiQPTimePoint& other) const { 49 | unsigned int sec = sec_ - other.getSec(); 50 | unsigned int nsec = 0; 51 | unsigned int underlap = 0; 52 | 53 | if (nsec_ >= other.getNSec()) { 54 | nsec = nsec_ - other.getNSec(); 55 | } else { 56 | nsec = 1e9 + nsec_ - other.getNSec(); 57 | sec -= 1; 58 | } 59 | 60 | return HiQPTimePoint(sec, nsec); 61 | } 62 | 63 | HiQPTimePoint& HiQPTimePoint::operator+=(const HiQPTimePoint& other) { 64 | unsigned int sec = sec_ + other.getSec(); 65 | unsigned int nsec = nsec_ + other.getNSec(); 66 | unsigned int overlap = nsec * 1e-9; 67 | 68 | this->sec_ = sec + overlap; 69 | this->nsec_ = nsec - overlap * 1e9; 70 | 71 | return *this; 72 | } 73 | 74 | HiQPTimePoint& HiQPTimePoint::operator-=(const HiQPTimePoint& other) { 75 | unsigned int sec = sec_ - other.getSec(); 76 | unsigned int nsec = 0; 77 | unsigned int underlap = 0; 78 | 79 | if (nsec_ >= other.getNSec()) { 80 | nsec = nsec_ - other.getNSec(); 81 | } else { 82 | nsec = 1e9 + nsec_ - other.getNSec(); 83 | sec -= 1; 84 | } 85 | 86 | this->sec_ = sec; 87 | this->nsec_ = nsec; 88 | 89 | return *this; 90 | } 91 | 92 | } // namespace hiqp -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/geometric_primitives/geometric_primitive.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_GEOMETRIC_PRIMITIVE_H 18 | #define HIQP_GEOMETRIC_PRIMITIVE_H 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | namespace hiqp 25 | { 26 | namespace geometric_primitives 27 | { 28 | 29 | /*! 30 | * \class GeometricPrimitive 31 | * \brief Abstract base class for all geometric primitives. 32 | */ 33 | class GeometricPrimitive 34 | { 35 | public: 36 | GeometricPrimitive(const std::string& name, 37 | const std::string& frame_id, 38 | bool visible, 39 | const std::vector& color) 40 | : name_(name), frame_id_(frame_id), visible_(visible), visual_id_(-1) { 41 | r_ = color.at(0); 42 | g_ = color.at(1); 43 | b_ = color.at(2); 44 | a_ = color.at(3); 45 | } 46 | 47 | ~GeometricPrimitive() noexcept = default; 48 | 49 | /*! \brief Must be specified by the inheriting class. */ 50 | virtual int init(const std::vector& parameters) = 0; 51 | 52 | inline int setVisualId(int visual_id) { visual_id_ = visual_id; } 53 | inline int getVisualId() { return visual_id_; } 54 | 55 | inline std::string getName() { return name_; } 56 | inline std::string getFrameId() { return frame_id_; } 57 | inline bool isVisible() { return visible_; } 58 | 59 | inline double getRedComponent() { return r_; } 60 | inline double getGreenComponent() { return g_; } 61 | inline double getBlueComponent() { return b_; } 62 | inline double getAlphaComponent() { return a_; } 63 | 64 | protected: 65 | std::string name_; 66 | std::string frame_id_; 67 | bool visible_; 68 | int visual_id_; 69 | double r_, g_, b_, a_; 70 | 71 | private: 72 | GeometricPrimitive(const GeometricPrimitive& other) = delete; 73 | GeometricPrimitive(GeometricPrimitive&& other) = delete; 74 | GeometricPrimitive& operator=(const GeometricPrimitive& other) = delete; 75 | GeometricPrimitive& operator=(GeometricPrimitive&& other) noexcept = delete; 76 | }; 77 | 78 | } // namespace geometric_primitives 79 | 80 | } // namespace hiqp 81 | 82 | #endif // include guard -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/geometric_primitives/geometric_point.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_GEOMETRIC_POINT_H 18 | #define HIQP_GEOMETRIC_POINT_H 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | 25 | #include 26 | 27 | namespace hiqp 28 | { 29 | namespace geometric_primitives 30 | { 31 | 32 | /*! \brief Parameters: [x, y, z] 33 | * \author Marcus A Johansson */ 34 | class GeometricPoint : public GeometricPrimitive { 35 | public: 36 | GeometricPoint(const std::string& name, 37 | const std::string& frame_id, 38 | bool visible, 39 | const std::vector& color) 40 | : GeometricPrimitive(name, frame_id, visible, color) {} 41 | 42 | ~GeometricPoint() noexcept {} 43 | 44 | /*! \brief Parses a set of parameters and initializes the point. 45 | * \param parameters : Should be of size 3. Indices 0-2 (required) defines the position of the point. 46 | * \return 0 on success, -1 if the wrong number of parameters was sent */ 47 | int init(const std::vector& parameters) { 48 | int size = parameters.size(); 49 | if (size != 3) { 50 | printHiqpWarning("GeometricPoint requires 3 parameters, got " 51 | + std::to_string(size) + "! Initialization failed!"); 52 | return -1; 53 | } 54 | 55 | kdl_p_(0) = parameters.at(0); 56 | kdl_p_(1) = parameters.at(1); 57 | kdl_p_(2) = parameters.at(2); 58 | 59 | eigen_p_ << kdl_p_(0), kdl_p_(1), kdl_p_(2); 60 | return 0; 61 | } 62 | 63 | inline const KDL::Vector& getPointKDL() { return kdl_p_; } 64 | 65 | inline const Eigen::Vector3d& getPointEigen() { return eigen_p_; } 66 | 67 | inline double getX() { return kdl_p_(0); } 68 | inline double getY() { return kdl_p_(1); } 69 | inline double getZ() { return kdl_p_(2); } 70 | 71 | protected: 72 | KDL::Vector kdl_p_; 73 | Eigen::Vector3d eigen_p_; 74 | 75 | private: 76 | GeometricPoint(const GeometricPoint& other) = delete; 77 | GeometricPoint(GeometricPoint&& other) = delete; 78 | GeometricPoint& operator=(const GeometricPoint& other) = delete; 79 | GeometricPoint& operator=(GeometricPoint&& other) noexcept = delete; 80 | }; 81 | 82 | } // namespace geometric_primitives 83 | 84 | } // namespace hiqp 85 | 86 | #endif // include guard -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/tasks/tdef_geometric_alignment.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_TDEF_GEOMETRIC_ALIGNMENT_H 18 | #define HIQP_TDEF_GEOMETRIC_ALIGNMENT_H 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | namespace hiqp 30 | { 31 | namespace tasks 32 | { 33 | 34 | /*! \brief A task definition that rotates primitives to align with each other. 35 | * \author Marcus A Johansson */ 36 | template 37 | class TDefGeometricAlignment : public TaskDefinition { 38 | public: 39 | TDefGeometricAlignment(std::shared_ptr geom_prim_map, 40 | std::shared_ptr visualizer); 41 | ~TDefGeometricAlignment() noexcept = default; 42 | 43 | int init(const std::vector& parameters, 44 | RobotStatePtr robot_state); 45 | 46 | int update(RobotStatePtr robot_state); 47 | 48 | int monitor(); 49 | 50 | private: 51 | TDefGeometricAlignment(const TDefGeometricAlignment& other) = delete; 52 | TDefGeometricAlignment(TDefGeometricAlignment&& other) = delete; 53 | TDefGeometricAlignment& operator=(const TDefGeometricAlignment& other) = delete; 54 | TDefGeometricAlignment& operator=(TDefGeometricAlignment&& other) noexcept = delete; 55 | 56 | int align(std::shared_ptr first, std::shared_ptr second); 57 | int alignVectors(const KDL::Vector& v1, const KDL::Vector v2); 58 | 59 | /// \brief This sets jacobian columns corresponding to non-writable joints to 0 60 | void maskJacobian(RobotStatePtr robot_state); 61 | 62 | std::shared_ptr fk_solver_pos_; 63 | std::shared_ptr fk_solver_jac_; 64 | 65 | std::shared_ptr primitive_a_; 66 | KDL::Frame pose_a_; 67 | KDL::Jacobian jacobian_a_; 68 | 69 | std::shared_ptr primitive_b_; 70 | KDL::Frame pose_b_; 71 | KDL::Jacobian jacobian_b_; 72 | 73 | double delta_; // the angular error margin 74 | 75 | }; 76 | 77 | } // namespace tasks 78 | 79 | } // namespace hiqp 80 | 81 | #include 82 | 83 | #endif // include guard 84 | -------------------------------------------------------------------------------- /hiqp_core/src/tasks/tdef_jnt_limits.cpp: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | namespace hiqp 23 | { 24 | namespace tasks 25 | { 26 | 27 | int TDefJntLimits::init(const std::vector& parameters, 28 | RobotStatePtr robot_state) { 29 | int size = parameters.size(); 30 | if (size != 4) 31 | { 32 | printHiqpWarning("TDefJntLimits requires 4 parameter, got " 33 | + std::to_string(size) + "! Initialization failed!"); 34 | return -1; 35 | } 36 | 37 | link_frame_name_ = parameters.at(1); 38 | link_frame_q_nr_ = kdl_getQNrFromLinkName(robot_state->kdl_tree_, link_frame_name_); 39 | 40 | if (link_frame_q_nr_ < 0) { 41 | printHiqpWarning("TDefJntLimits::init, couldn't find joint '" + link_frame_name_ + "'! Initialization failed."); 42 | return -2; 43 | } 44 | 45 | if (!robot_state->isQNrWritable(link_frame_q_nr_)) { 46 | printHiqpWarning("TDefJntLimits::init, the joint '" + link_frame_name_ + "' is not writable! Initialization failed."); 47 | return -3; 48 | } 49 | 50 | unsigned int n_joints = robot_state->getNumJoints(); 51 | e_.resize(4); 52 | J_.resize(4, n_joints); 53 | performance_measures_.resize(0); 54 | 55 | task_types_.resize(4); 56 | task_types_.at(0) = 1; // > -dq_max 57 | task_types_.at(1) = -1; // < dq_max 58 | task_types_.at(2) = 1; // > lower bound 59 | task_types_.at(3) = -1; // < upper bound 60 | 61 | 62 | jnt_lower_bound_ = std::stod( parameters.at(2) ); 63 | jnt_upper_bound_ = std::stod( parameters.at(3) ); 64 | 65 | //Jacobian entries are 1 at column link_frame_q_nr_ and 0 otherwise 66 | for (int i=0; i<4; ++i) { 67 | for (int j=0; jkdl_jnt_array_vel_.q(link_frame_q_nr_); 77 | 78 | e_(0) = q; //e_(0), and e_(1) do not actually matter, since they're ignored in DynamicsJntLimits where de_*=+/- dq_max 79 | e_(1) = q; 80 | e_(2) = q - jnt_lower_bound_; 81 | e_(3) = q - jnt_upper_bound_; 82 | 83 | return 0; 84 | } 85 | 86 | int TDefJntLimits::monitor() { 87 | return 0; 88 | } 89 | 90 | } // namespace tasks 91 | 92 | } // namespace hiqp 93 | -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/robot_state.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_ROBOT_STATE_H 18 | #define HIQP_ROBOT_STATE_H 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace hiqp { 26 | 27 | /*! \brief Holds information on what q_nr (KDL) is associated with what joint name, and whether that joint's resource is readable/writable. 28 | * \author Marcus A Johansson */ 29 | struct JointHandleInfo { 30 | JointHandleInfo(unsigned int q_nr, const std::string& joint_name, bool readable, bool writable) 31 | : q_nr_(q_nr), joint_name_(joint_name), readable_(readable), writable_(writable) {} 32 | unsigned int q_nr_; 33 | std::string joint_name_; 34 | bool readable_; 35 | bool writable_; 36 | }; 37 | 38 | /*! \brief Holds the state of the robot (sampling time, kdl tree, joint positions and velocities) 39 | * \author Marcus A Johansson */ 40 | struct RobotState { 41 | HiQPTimePoint sampling_time_point_; 42 | double sampling_time_; 43 | KDL::Tree kdl_tree_; 44 | KDL::JntArrayVel kdl_jnt_array_vel_; 45 | KDL::JntArray kdl_effort_; 46 | std::vector joint_handle_info_; 47 | 48 | /// \brief Returns whether the joint with qnr is writable or not 49 | inline bool isQNrWritable(unsigned int qnr) const { 50 | for (auto&& jhi : joint_handle_info_) { 51 | if (jhi.q_nr_ == qnr) 52 | return jhi.writable_; 53 | } 54 | return false; 55 | } 56 | 57 | /// \brief Returns the total number of joints (including read-only joint resources) 58 | inline unsigned int getNumJoints() const { 59 | return kdl_tree_.getNrOfJoints(); 60 | } 61 | 62 | /// \brief Returns the number of controllable joints (writable joint resources) 63 | inline unsigned int getNumControls() const { 64 | unsigned int n = 0; 65 | for (auto&& jhi : joint_handle_info_) { 66 | if (jhi.writable_) n++; 67 | } 68 | return n; 69 | } 70 | }; 71 | 72 | /// \todo Rename to RobotStateConstPtr 73 | /*! \brief A const pointer type to a robot state. Used to reference to the current robot state throught the framework. 74 | * \author Marcus A Johansson */ 75 | typedef std::shared_ptr RobotStatePtr; 76 | 77 | } // namespace hiqp 78 | 79 | #endif // include guard -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/visualizer.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_VISUALIZER_H 18 | #define HIQP_VISUALIZER_H 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | namespace hiqp 31 | { 32 | using geometric_primitives::GeometricPoint; 33 | using geometric_primitives::GeometricLine; 34 | using geometric_primitives::GeometricPlane; 35 | using geometric_primitives::GeometricBox; 36 | using geometric_primitives::GeometricCylinder; 37 | using geometric_primitives::GeometricSphere; 38 | using geometric_primitives::GeometricFrame; 39 | 40 | /*! \brief An interface for visualizing geometric primitives. Derive from this class to implement your own visualizer and provide it to TaskManager to get visualization of HiQP. 41 | * \author Marcus A Johansson */ 42 | class Visualizer { 43 | public: 44 | Visualizer() {} 45 | ~Visualizer() noexcept {} 46 | 47 | virtual int add(std::shared_ptr point) = 0; 48 | virtual int add(std::shared_ptr line) = 0; 49 | virtual int add(std::shared_ptr plane) = 0; 50 | virtual int add(std::shared_ptr box) = 0; 51 | virtual int add(std::shared_ptr cylinder) = 0; 52 | virtual int add(std::shared_ptr sphere) = 0; 53 | virtual int add(std::shared_ptr frame) = 0; 54 | 55 | virtual void update(int id, std::shared_ptr point) = 0; 56 | virtual void update(int id, std::shared_ptr line) = 0; 57 | virtual void update(int id, std::shared_ptr plane) = 0; 58 | virtual void update(int id, std::shared_ptr box) = 0; 59 | virtual void update(int id, std::shared_ptr cylinder) = 0; 60 | virtual void update(int id, std::shared_ptr sphere) = 0; 61 | virtual void update(int id, std::shared_ptr frame) = 0; 62 | 63 | virtual void remove(int id) = 0; 64 | 65 | virtual void removeMany(const std::vector& ids) = 0; 66 | 67 | private: 68 | Visualizer(const Visualizer& other) = delete; 69 | Visualizer(Visualizer&& other) = delete; 70 | Visualizer& operator=(const Visualizer& other) = delete; 71 | Visualizer& operator=(Visualizer&& other) noexcept = delete; 72 | }; 73 | 74 | } // namespace hiqp 75 | 76 | #endif // include guard -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/geometric_primitives/geometric_sphere.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_GEOMETRIC_SPHERE_H 18 | #define HIQP_GEOMETRIC_SPHERE_H 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | 25 | #include 26 | 27 | namespace hiqp 28 | { 29 | namespace geometric_primitives 30 | { 31 | 32 | /*! \brief Parameters: [x, y, z, radius] 33 | * \author Marcus A Johansson */ 34 | class GeometricSphere : public GeometricPrimitive { 35 | public: 36 | GeometricSphere(const std::string& name, 37 | const std::string& frame_id, 38 | bool visible, 39 | const std::vector& color) 40 | : GeometricPrimitive(name, frame_id, visible, color) {} 41 | 42 | ~GeometricSphere() noexcept = default; 43 | 44 | /*! \brief Parses a set of parameters and initializes the sphere. 45 | * \param parameters : Should be of size 4.
    46 | *
  1. Indices 0-2 (required) defines the position of the center of the sphere.
    2. 47 | *
  2. Index 3 (required) defines the radius of the sphere.
    3. 48 | *
49 | * \return 0 on success, -1 if the wrong number of parameters was sent */ 50 | int init(const std::vector& parameters) { 51 | int size = parameters.size(); 52 | if (size != 4) { 53 | printHiqpWarning("GeometricSphere requires 4 parameters, got " 54 | + std::to_string(size) + "! Initialization failed!"); 55 | return -1; 56 | } 57 | 58 | kdl_p_(0) = parameters.at(0); 59 | kdl_p_(1) = parameters.at(1); 60 | kdl_p_(2) = parameters.at(2); 61 | radius_ = parameters.at(3); 62 | 63 | eigen_p_ << kdl_p_(0), kdl_p_(1), kdl_p_(2); 64 | return 0; 65 | } 66 | 67 | inline const KDL::Vector& getCenterKDL() { return kdl_p_; } 68 | inline const Eigen::Vector3d& getCenterEigen() { return eigen_p_; } 69 | 70 | inline double getRadius() { return radius_; } 71 | inline double getX() { return kdl_p_(0); } 72 | inline double getY() { return kdl_p_(1); } 73 | inline double getZ() { return kdl_p_(2); } 74 | 75 | protected: 76 | KDL::Vector kdl_p_; // the offset of the sphere 77 | Eigen::Vector3d eigen_p_; 78 | double radius_; // the radius of the sphere 79 | 80 | private: 81 | GeometricSphere(const GeometricSphere& other) = delete; 82 | GeometricSphere(GeometricSphere&& other) = delete; 83 | GeometricSphere& operator=(const GeometricSphere& other) = delete; 84 | GeometricSphere& operator=(GeometricSphere&& other) noexcept = delete; 85 | }; 86 | 87 | } // namespace geometric_primitives 88 | 89 | } // namespace hiqp 90 | 91 | #endif // include guard -------------------------------------------------------------------------------- /hiqp_ros/include/hiqp_ros/hiqp_joint_effort_controller.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_JOINT_EFFORT_CONTROLLER_H 18 | #define HIQP_JOINT_EFFORT_CONTROLLER_H 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | namespace hiqp_ros { 31 | typedef hardware_interface::EffortJointInterface JointEffortInterface; 32 | 33 | /*! \brief A joint effort controller that provides full access to the HiQP control framework 34 | * \author Marcus A Johansson */ 35 | class HiQPJointEffortController : public BaseController { 36 | public: 37 | HiQPJointEffortController(); 38 | ~HiQPJointEffortController() noexcept; 39 | 40 | void initialize(); 41 | void computeControls(Eigen::VectorXd& u); 42 | 43 | private: 44 | HiQPJointEffortController(const HiQPJointEffortController& other) = delete; 45 | HiQPJointEffortController(HiQPJointEffortController&& other) = delete; 46 | HiQPJointEffortController& operator=(const HiQPJointEffortController& other) = delete; 47 | HiQPJointEffortController& operator=(HiQPJointEffortController&& other) noexcept = delete; 48 | 49 | void monitorTasks(); 50 | void renderPrimitives(); 51 | 52 | void loadRenderingParameters(); 53 | int loadAndSetupTaskMonitoring(); 54 | // void addAllTopicSubscriptions(); 55 | void loadJointLimitsFromParamServer(); 56 | void loadGeometricPrimitivesFromParamServer(); 57 | void loadTasksFromParamServer(); 58 | 59 | bool is_active_; 60 | 61 | bool monitoring_active_; 62 | double monitoring_publish_rate_; 63 | ros::Time last_monitoring_update_; 64 | 65 | double rendering_publish_rate_; 66 | ros::Time last_rendering_update_; 67 | 68 | ros::Publisher monitoring_pub_; 69 | 70 | ROSTopicSubscriber topic_subscriber_; 71 | 72 | HiQPServiceHandler service_handler_; // takes care of all ros service calls 73 | 74 | ROSVisualizer ros_visualizer_; 75 | std::shared_ptr visualizer_; 76 | 77 | hiqp::TaskManager task_manager_; 78 | std::shared_ptr task_manager_ptr_; 79 | 80 | }; 81 | 82 | } // namespace hiqp_ros 83 | 84 | #endif // include guard 85 | -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/tasks/tdef_geometric_projection.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_TDEF_GEOMETRIC_PROJECTION_H 18 | #define HIQP_TDEF_GEOMETRIC_PROJECTION_H 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | namespace hiqp 30 | { 31 | namespace tasks 32 | { 33 | 34 | /*! \brief A task definition that positions geometric primitives relative to each other through mutual geometric projection. 35 | * \author Marcus A Johansson */ 36 | template 37 | class TDefGeometricProjection : public TaskDefinition { 38 | public: 39 | TDefGeometricProjection(std::shared_ptr geom_prim_map, 40 | std::shared_ptr visualizer); 41 | ~TDefGeometricProjection() noexcept = default; 42 | 43 | int init(const std::vector& parameters, 44 | RobotStatePtr robot_state); 45 | 46 | int update(RobotStatePtr robot_state); 47 | 48 | int monitor(); 49 | 50 | private: 51 | TDefGeometricProjection(const TDefGeometricProjection& other) = delete; 52 | TDefGeometricProjection(TDefGeometricProjection&& other) = delete; 53 | TDefGeometricProjection& operator=(const TDefGeometricProjection& other) = delete; 54 | TDefGeometricProjection& operator=(TDefGeometricProjection&& other) noexcept = delete; 55 | 56 | int project(std::shared_ptr first, std::shared_ptr second); 57 | 58 | /// \brief This sets jacobian columns corresponding to non-writable joints to 0 59 | void maskJacobian(RobotStatePtr robot_state); 60 | 61 | /*! \brief Computes column number q_nr of the resulting jacobian for the 62 | * vector (p2-p1), NOTE! p1 must be related to pose_a_ and p2 to 63 | * pose_b_ ! 64 | */ 65 | KDL::Vector getVelocityJacobianForTwoPoints 66 | ( 67 | const KDL::Vector& p1, 68 | const KDL::Vector& p2, 69 | int q_nr 70 | ); 71 | 72 | std::shared_ptr fk_solver_pos_; 73 | std::shared_ptr fk_solver_jac_; 74 | 75 | std::shared_ptr primitive_a_; 76 | KDL::Frame pose_a_; 77 | KDL::Jacobian jacobian_a_; 78 | 79 | std::shared_ptr primitive_b_; 80 | KDL::Frame pose_b_; 81 | KDL::Jacobian jacobian_b_; 82 | }; 83 | 84 | } // namespace tasks 85 | 86 | } // namespace hiqp 87 | 88 | #include 89 | 90 | #endif // include guard 91 | -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/geometric_primitives/geometric_plane.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_GEOMETRIC_PLANE_H 18 | #define HIQP_GEOMETRIC_PLANE_H 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | 25 | namespace hiqp 26 | { 27 | namespace geometric_primitives 28 | { 29 | 30 | /*! \brief Parameters: [n.x, n.y, n.z, offset]. The plane equation corresponds to \f$n^T*x-offset=0\f$. 31 | * \author Marcus A Johansson */ 32 | class GeometricPlane : public GeometricPrimitive { 33 | public: 34 | GeometricPlane(const std::string& name, 35 | const std::string& frame_id, 36 | bool visible, 37 | const std::vector& color) 38 | : GeometricPrimitive(name, frame_id, visible, color) {} 39 | 40 | ~GeometricPlane() noexcept = default; 41 | 42 | /*! \brief Parses a set of parameters and initializes the plane. 43 | * \param parameters : Should be of size 4.
    44 | *
  1. Indices 0-2 (required) defines the normal vector of the plane.
    2. 45 | *
  2. Index 3 (required) defines the offset of the plane in the normal direction.
    3. 46 | *
47 | * \return 0 on success, -1 if the wrong number of parameters was sent. */ 48 | int init(const std::vector& parameters) { 49 | int size = parameters.size(); 50 | if (size != 4) { 51 | printHiqpWarning("GeometricPlane requires 4 parameters, got " 52 | + std::to_string(size) + "! Initialization failed!"); 53 | return -1; 54 | } 55 | 56 | kdl_n_(0) = parameters.at(0); 57 | kdl_n_(1) = parameters.at(1); 58 | kdl_n_(2) = parameters.at(2); 59 | 60 | kdl_n_.Normalize(); 61 | eigen_n_ << kdl_n_(0), kdl_n_(1), kdl_n_(2); 62 | d_ = parameters.at(3); 63 | return 0; 64 | } 65 | 66 | inline const KDL::Vector& getNormalKDL() { return kdl_n_; } 67 | inline const Eigen::Vector3d& getNormalEigen() { return eigen_n_; } 68 | 69 | inline double getOffset() { return d_; } 70 | inline double getNormalX() { return kdl_n_(0); } 71 | inline double getNormalY() { return kdl_n_(1); } 72 | inline double getNormalZ() { return kdl_n_(2); } 73 | 74 | protected: 75 | KDL::Vector kdl_n_; // the normal vector or the plane 76 | Eigen::Vector3d eigen_n_; 77 | double d_; // the offset in the normal direction 78 | 79 | private: 80 | GeometricPlane(const GeometricPlane& other) = delete; 81 | GeometricPlane(GeometricPlane&& other) = delete; 82 | GeometricPlane& operator=(const GeometricPlane& other) = delete; 83 | GeometricPlane& operator=(GeometricPlane&& other) noexcept = delete; 84 | }; 85 | 86 | } // namespace geometric_primitives 87 | 88 | } // namespace hiqp 89 | 90 | #endif // include guard 91 | -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/task_dynamics.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_TASK_DYNAMICS_H 18 | #define HIQP_TASK_DYNAMICS_H 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | namespace hiqp 30 | { 31 | 32 | using geometric_primitives::GeometricPrimitiveMap; 33 | 34 | class Task; 35 | 36 | /*! \brief A task dynamics enforces the optimizer to produce controls that results is a certain velocity of the task performance value. 37 | * \author Marcus A Johansson */ 38 | class TaskDynamics 39 | { 40 | public: 41 | TaskDynamics(std::shared_ptr geom_prim_map, 42 | std::shared_ptr visualizer) 43 | : geometric_primitive_map_(geom_prim_map), visualizer_(visualizer) 44 | {} 45 | 46 | ~TaskDynamics() noexcept {} 47 | 48 | virtual int init(const std::vector& parameters, 49 | RobotStatePtr robot_state, 50 | const Eigen::VectorXd& e_initial, 51 | const Eigen::VectorXd& e_final) = 0; 52 | 53 | virtual int update(RobotStatePtr robot_state, 54 | const Eigen::VectorXd& e, 55 | const Eigen::MatrixXd& J) = 0; 56 | 57 | virtual int monitor() = 0; 58 | 59 | protected: 60 | Eigen::VectorXd e_dot_star_; 61 | Eigen::VectorXd performance_measures_; 62 | 63 | inline std::string getTaskName() { return task_name_; } 64 | inline unsigned int getPriority() { return priority_; } 65 | inline bool getActive() { return active_; } 66 | inline bool getVisible() { return visible_; } 67 | inline std::shared_ptr 68 | getVisualizer() { return visualizer_; } 69 | inline std::shared_ptr 70 | getGeometricPrimitiveMap() { return geometric_primitive_map_; } 71 | 72 | private: 73 | friend Task; 74 | 75 | std::shared_ptr geometric_primitive_map_; 76 | std::shared_ptr visualizer_; 77 | 78 | std::string task_name_; 79 | unsigned int priority_; 80 | bool visible_; 81 | bool active_; 82 | 83 | TaskDynamics(const TaskDynamics& other) = delete; 84 | TaskDynamics(TaskDynamics&& other) = delete; 85 | TaskDynamics& operator=(const TaskDynamics& other) = delete; 86 | TaskDynamics& operator=(TaskDynamics&& other) noexcept = delete; 87 | }; 88 | 89 | } // namespace hiqp 90 | 91 | #endif // include guard -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/solvers/gurobi_solver.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_GUROBI_SOLVER_H 18 | #define HIQP_GUROBI_SOLVER_H 19 | 20 | #include 21 | #include 22 | 23 | namespace hiqp 24 | { 25 | /*! \brief An optimization based solver for a set of stages based on Gurobi. 26 | * \author Robert Krug, Marcus A Johansson */ 27 | class GurobiSolver : public HiQPSolver { 28 | public: 29 | GurobiSolver(); 30 | ~GurobiSolver() noexcept {} 31 | 32 | /*! \brief Builds and solves the QP: 33 | * min 0.5x^2 + 0.5w^2 34 | * where J*dq + w = de* 35 | */ 36 | bool solve(std::vector& solution); 37 | 38 | private: 39 | GurobiSolver(const GurobiSolver& other) = delete; 40 | GurobiSolver(GurobiSolver&& other) = delete; 41 | GurobiSolver& operator=(const GurobiSolver& other) = delete; 42 | GurobiSolver& operator=(GurobiSolver&& other) noexcept = delete; 43 | 44 | struct HQPConstraints { 45 | HQPConstraints() : n_acc_stage_dims_(0) {} 46 | 47 | void reset(unsigned int n_solution_dims); 48 | void appendConstraints(const HiQPStage& current_stage); 49 | 50 | unsigned int n_acc_stage_dims_; // number of accumulated dimensions of all the previously solved stages 51 | unsigned int n_stage_dims_; // number of dimensions of the current stage 52 | Eigen::VectorXd w_; 53 | Eigen::VectorXd de_; 54 | Eigen::MatrixXd J_; 55 | std::vector constraint_signs_; 56 | }; 57 | 58 | struct QPProblem { 59 | QPProblem(const GRBEnv& env, 60 | HQPConstraints& hqp_constraints, 61 | unsigned int solution_dims); 62 | 63 | ~QPProblem(); 64 | 65 | void setup(); 66 | void solve(); 67 | void getSolution(std::vector& solution); 68 | 69 | GRBModel model_; // Gurobi model (one per each QP problem is used) 70 | HQPConstraints& hqp_constraints_; 71 | unsigned int solution_dims_; 72 | 73 | GRBVar* dq_; // objective variables for joint velocities 74 | double* lb_dq_; // lower bounds for dq 75 | double* ub_dq_; // upper bounds for dq 76 | 77 | GRBVar* w_; // slack variables 78 | double* lb_w_; // lower bounds for w 79 | double* ub_w_; // upper bounds for w 80 | 81 | double* rhsides_; // Right-hand-side constants 82 | GRBLinExpr* lhsides_; // Left-hand-side expressions 83 | double* coeff_dq_; // Coeffs of dq in LHS expression 84 | double* coeff_w_; // Coeffs of w in LHS expression 85 | 86 | GRBConstr* constraints_; // 87 | }; 88 | 89 | GRBEnv env_; 90 | unsigned int n_solution_dims_; // number of solution dimensions 91 | HQPConstraints hqp_constraints_; 92 | }; 93 | 94 | } // namespace hiqp 95 | 96 | #endif // include guard 97 | -------------------------------------------------------------------------------- /hiqp_ros/include/hiqp_ros/hiqp_joint_velocity_controller.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_JOINT_VELOCITY_CONTROLLER_H 18 | #define HIQP_JOINT_VELOCITY_CONTROLLER_H 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | 41 | namespace hiqp_ros { 42 | typedef hardware_interface::VelocityJointInterface JointVelocityInterface; 43 | 44 | /*! \brief A joint velocity controller that provides full access to the HiQP control framework 45 | * \author Marcus A Johansson */ 46 | class HiQPJointVelocityController : public BaseController { 47 | public: 48 | HiQPJointVelocityController(); 49 | ~HiQPJointVelocityController() noexcept; 50 | 51 | void initialize(); 52 | void computeControls(Eigen::VectorXd& u); 53 | 54 | private: 55 | HiQPJointVelocityController(const HiQPJointVelocityController& other) = delete; 56 | HiQPJointVelocityController(HiQPJointVelocityController&& other) = delete; 57 | HiQPJointVelocityController& operator=(const HiQPJointVelocityController& other) = delete; 58 | HiQPJointVelocityController& operator=(HiQPJointVelocityController&& other) noexcept = delete; 59 | 60 | void monitorTasks(); 61 | void renderPrimitives(); 62 | 63 | void loadRenderingParameters(); 64 | int loadAndSetupTaskMonitoring(); 65 | // void addAllTopicSubscriptions(); 66 | void loadJointLimitsFromParamServer(); 67 | void loadGeometricPrimitivesFromParamServer(); 68 | void loadTasksFromParamServer(); 69 | 70 | bool is_active_; 71 | 72 | bool monitoring_active_; 73 | double monitoring_publish_rate_; 74 | ros::Time last_monitoring_update_; 75 | 76 | double rendering_publish_rate_; 77 | ros::Time last_rendering_update_; 78 | 79 | ros::Publisher monitoring_pub_; 80 | 81 | ROSTopicSubscriber topic_subscriber_; 82 | 83 | HiQPServiceHandler service_handler_; // takes care of all ros service calls 84 | 85 | ROSVisualizer ros_visualizer_; 86 | std::shared_ptr visualizer_; 87 | 88 | hiqp::TaskManager task_manager_; 89 | std::shared_ptr task_manager_ptr_; 90 | }; 91 | 92 | } // namespace hiqp 93 | 94 | #endif // include guard 95 | -------------------------------------------------------------------------------- /hiqp_ros/src/utilities.cpp: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | #include 19 | 20 | // Auxiliary function to 'ostream& << KDL::Tree&' 21 | void printChildrenToOstream(std::ostream& os, 22 | const std::vector& children, 23 | std::vector& is_last_child, 24 | unsigned int level = 0) { 25 | is_last_child.push_back(false); 26 | for (auto&& child : children) { 27 | for (int i=0; ifirst 31 | << " : " << child->second.segment.getJoint().getName() 32 | << "(" << child->second.q_nr << ")" 33 | << "\n"; 34 | 35 | is_last_child.at(level) = (child == children.back()); 36 | printChildrenToOstream(os, child->second.children, is_last_child, level+1); 37 | } 38 | return; 39 | } 40 | 41 | std::ostream& operator<<(std::ostream& os, const KDL::Tree& kdl_tree) { 42 | KDL::SegmentMap::const_iterator root_segment = kdl_tree.getRootSegment(); 43 | os << "nr of joints: " << kdl_tree.getNrOfJoints() << "\n"; 44 | os << "nr of segments: " << kdl_tree.getNrOfSegments() << "\n"; 45 | os << root_segment->first << "\n"; 46 | std::vector is_last_child; 47 | printChildrenToOstream(os, root_segment->second.children, is_last_child); 48 | return os; 49 | } 50 | 51 | std::ostream& operator<<(std::ostream& os, const KDL::Vector& kdl_vector) { 52 | os << "[" << kdl_vector(0) << ", " 53 | << kdl_vector(1) << ", " 54 | << kdl_vector(2) << "]"; 55 | return os; 56 | } 57 | 58 | std::ostream& operator<<(std::ostream& os, const KDL::FrameVel& kdl_frame_vel) { 59 | const KDL::Rotation &rot = kdl_frame_vel.value().M; 60 | const KDL::Vector &pos = kdl_frame_vel.value().p; 61 | const KDL::Vector &rotvel = kdl_frame_vel.deriv().rot; 62 | const KDL::Vector &vel = kdl_frame_vel.deriv().vel; 63 | os << std::setprecision(4) 64 | << rot.data[0] << " " << rot.data[1] << " " << rot.data[2] << " " 65 | << pos.data[0] << " " << rotvel.data[0] << " " << vel.data[0] << "\n" 66 | 67 | << rot.data[3] << " " << rot.data[4] << " " << rot.data[5] << " " 68 | << pos.data[1] << " " << rotvel.data[1] << " " << vel.data[1] << "\n" 69 | 70 | << rot.data[6] << " " << rot.data[7] << " " << rot.data[8] << " " 71 | << pos.data[2] << " " << rotvel.data[2] << " " << vel.data[2]; 72 | return os; 73 | } 74 | 75 | std::ostream& operator<<(std::ostream& os, const KDL::JntArrayVel& kdl_joints_vel) { 76 | const KDL::JntArray& q = kdl_joints_vel.q; 77 | const KDL::JntArray& qdot = kdl_joints_vel.qdot; 78 | unsigned int rows = q.rows(); 79 | for (int i=0; i" : "'"); 91 | } 92 | return os; 93 | } -------------------------------------------------------------------------------- /hiqp_ros/include/hiqp_ros/ros_visualizer.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_ROS_VISUALIZER_H 18 | #define HIQP_ROS_VISUALIZER_H 19 | 20 | #include 21 | 22 | #include 23 | 24 | using hiqp::Visualizer; 25 | using hiqp::geometric_primitives::GeometricPoint; 26 | using hiqp::geometric_primitives::GeometricLine; 27 | using hiqp::geometric_primitives::GeometricPlane; 28 | using hiqp::geometric_primitives::GeometricBox; 29 | using hiqp::geometric_primitives::GeometricCylinder; 30 | using hiqp::geometric_primitives::GeometricSphere; 31 | using hiqp::geometric_primitives::GeometricFrame; 32 | 33 | namespace hiqp_ros 34 | { 35 | 36 | /// \todo Make node handle pointer a shared pointer 37 | /*! \brief 38 | * \author Marcus A Johansson */ 39 | class ROSVisualizer : public hiqp::Visualizer { 40 | public: 41 | ROSVisualizer(); 42 | ~ROSVisualizer() noexcept {} 43 | 44 | int init(ros::NodeHandle* controller_nh); 45 | 46 | int add(std::shared_ptr point); 47 | int add(std::shared_ptr line); 48 | int add(std::shared_ptr plane); 49 | int add(std::shared_ptr box); 50 | int add(std::shared_ptr cylinder); 51 | int add(std::shared_ptr sphere); 52 | int add(std::shared_ptr frame); 53 | 54 | void update(int id, std::shared_ptr point); 55 | void update(int id, std::shared_ptr line); 56 | void update(int id, std::shared_ptr plane); 57 | void update(int id, std::shared_ptr box); 58 | void update(int id, std::shared_ptr cylinder); 59 | void update(int id, std::shared_ptr sphere); 60 | void update(int id, std::shared_ptr frame); 61 | 62 | void remove(int id); 63 | 64 | void removeMany(const std::vector& ids); 65 | 66 | private: 67 | ROSVisualizer(const ROSVisualizer& other) = delete; 68 | ROSVisualizer(ROSVisualizer&& other) = delete; 69 | ROSVisualizer& operator=(const ROSVisualizer& other) = delete; 70 | ROSVisualizer& operator=(ROSVisualizer&& other) noexcept = delete; 71 | 72 | int apply(int id, std::shared_ptr point, int action); 73 | int apply(int id, std::shared_ptr line, int action); 74 | int apply(int id, std::shared_ptr plane, int action); 75 | int apply(int id, std::shared_ptr box, int action); 76 | int apply(int id, std::shared_ptr cylinder, int action); 77 | int apply(int id, std::shared_ptr sphere, int action); 78 | int apply(int id, std::shared_ptr frame, int action); 79 | 80 | enum {ACTION_ADD = 0, ACTION_MODIFY = 1}; 81 | 82 | const std::string kNamespace = "/yumi"; 83 | const double kInfiniteLength = 12; 84 | const double kPointRadius = 0.002; 85 | const double kLineRadius = 0.0005; 86 | const double kPlaneThickness = 0.001; 87 | const double kFrameArrowRadius = 0.0017; 88 | const double kFrameArrowLength = 0.04; 89 | 90 | ros::NodeHandle* controller_nh_; 91 | 92 | ros::Publisher marker_array_pub_; 93 | 94 | std::size_t next_id_; 95 | }; 96 | 97 | } // namespace hiqp 98 | 99 | #endif // include guard -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/geometric_primitives/geometric_primitive_map.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_GEOMETRIC_PRIMITIVE_MAP_H 18 | #define HIQP_GEOMETRIC_PRIMITIVE_MAP_H 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | namespace hiqp 33 | { 34 | namespace geometric_primitives 35 | { 36 | 37 | /*! \brief A common map data structure for all geometric primitive types. 38 | * \author Marcus A Johansson */ 39 | class GeometricPrimitiveMap { 40 | public: 41 | GeometricPrimitiveMap() {} 42 | ~GeometricPrimitiveMap() noexcept {} 43 | 44 | int setGeometricPrimitive(const std::string& name, 45 | const std::string& type, 46 | const std::string& frame_id, 47 | bool visible, 48 | const std::vector& color, 49 | const std::vector& parameters); 50 | 51 | int removeGeometricPrimitive(std::string name); 52 | 53 | int clear(); 54 | 55 | template 56 | std::shared_ptr getGeometricPrimitive(const std::string& name); 57 | 58 | template 59 | void updateGeometricPrimitive(const std::string& name, 60 | const std::vector& parameters); 61 | 62 | void addDependencyToPrimitive(const std::string& name, const std::string& id); 63 | void removeDependency(const std::string& id); 64 | void acceptVisitor(GeometricPrimitiveVisitor& visitor, const std::string& primitive_name = ""); 65 | 66 | private: 67 | GeometricPrimitiveMap(const GeometricPrimitiveMap& other) = delete; 68 | GeometricPrimitiveMap(GeometricPrimitiveMap&& other) = delete; 69 | GeometricPrimitiveMap& operator=(const GeometricPrimitiveMap& other) = delete; 70 | GeometricPrimitiveMap& operator=(GeometricPrimitiveMap&& other) noexcept = delete; 71 | 72 | std::string getDependenciesAsString(std::string name); 73 | 74 | typedef std::map< std::string, std::vector< std::string> > DependencyMap; 75 | typedef std::map< std::string, std::shared_ptr > PointMap; 76 | typedef std::map< std::string, std::shared_ptr > LineMap; 77 | typedef std::map< std::string, std::shared_ptr > PlaneMap; 78 | typedef std::map< std::string, std::shared_ptr > BoxMap; 79 | typedef std::map< std::string, std::shared_ptr > CylinderMap; 80 | typedef std::map< std::string, std::shared_ptr > SphereMap; 81 | typedef std::map< std::string, std::shared_ptr > FrameMap; 82 | 83 | DependencyMap dependency_map_; 84 | PointMap point_map_; 85 | LineMap line_map_; 86 | PlaneMap plane_map_; 87 | BoxMap box_map_; 88 | CylinderMap cylinder_map_; 89 | SphereMap sphere_map_; 90 | FrameMap frame_map_; 91 | 92 | std::vector all_primitive_names_; 93 | 94 | }; 95 | 96 | } // namespace geometric_primitives 97 | 98 | } // namespace hiqp 99 | 100 | #endif // include guard -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/task_definition.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_TASK_DEFINITION_H 18 | #define HIQP_TASK_DEFINITION_H 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | 30 | namespace hiqp 31 | { 32 | 33 | using geometric_primitives::GeometricPrimitiveMap; 34 | 35 | class Task; 36 | 37 | /*! \brief Defines the task space and the performance value of a task. 38 | * \author Marcus A Johansson */ 39 | class TaskDefinition 40 | { 41 | public: 42 | TaskDefinition(std::shared_ptr geom_prim_map, 43 | std::shared_ptr visualizer) 44 | : geometric_primitive_map_(geom_prim_map), visualizer_(visualizer) {} 45 | 46 | ~TaskDefinition() noexcept {} 47 | 48 | virtual int init(const std::vector& parameters, 49 | RobotStatePtr robot_state) = 0; 50 | 51 | virtual int update(RobotStatePtr robot_state) = 0; 52 | 53 | virtual int monitor() = 0; 54 | 55 | unsigned int getDimensions() { return n_dimensions_; } 56 | Eigen::VectorXd getInitialValue() { return e_initial_; } 57 | Eigen::MatrixXd getInitialJacobian() { return J_initial_; } 58 | 59 | virtual Eigen::VectorXd getFinalValue(RobotStatePtr robot_state) 60 | { return Eigen::VectorXd::Zero(e_.rows()); } 61 | 62 | protected: 63 | Eigen::VectorXd e_; // the performance value of the task 64 | Eigen::MatrixXd J_; // the task jacobian 65 | std::vector task_types_; // -1 leq, 0 eq, 1 geq 66 | Eigen::VectorXd performance_measures_; 67 | unsigned int n_dimensions_; 68 | 69 | inline std::string getTaskName() { return task_name_; } 70 | inline unsigned int getPriority() { return priority_; } 71 | inline bool getActive() { return active_; } 72 | inline bool getVisible() { return visible_; } 73 | inline std::shared_ptr 74 | getVisualizer() { return visualizer_; } 75 | inline std::shared_ptr 76 | getGeometricPrimitiveMap() { return geometric_primitive_map_; } 77 | 78 | private: 79 | friend Task; 80 | 81 | std::shared_ptr geometric_primitive_map_; 82 | std::shared_ptr visualizer_; 83 | 84 | Eigen::VectorXd e_initial_; 85 | Eigen::MatrixXd J_initial_; 86 | 87 | std::string task_name_; 88 | unsigned int priority_; 89 | bool visible_; 90 | bool active_; 91 | 92 | TaskDefinition(const TaskDefinition& other) = delete; 93 | TaskDefinition(TaskDefinition&& other) = delete; 94 | TaskDefinition& operator=(const TaskDefinition& other) = delete; 95 | TaskDefinition& operator=(TaskDefinition&& other) noexcept = delete; 96 | 97 | /*! \brief Calls init() of the child class, and properly sets up the TaskDefinition object. */ 98 | int initialize(const std::vector& parameters, 99 | RobotStatePtr robot_state) { 100 | if (init(parameters, robot_state) != 0) 101 | return -1; 102 | update(robot_state); 103 | e_initial_ = e_; 104 | J_initial_ = J_; 105 | return 0; 106 | } 107 | }; 108 | 109 | } // namespace hiqp 110 | 111 | #endif // include guard 112 | -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/geometric_primitives/geometric_line.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_GEOMETRIC_LINE_H 18 | #define HIQP_GEOMETRIC_LINE_H 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | 25 | #include 26 | 27 | namespace hiqp 28 | { 29 | namespace geometric_primitives 30 | { 31 | 32 | /*! \brief Implements a line defined by an arbitrary point and a directional vector. Parameters: [dir.x, dir.y, dir.z, offset.x, offset.y, offset.z] 33 | * The offset variables define the point on the line, the direction variables the direction (not necessarily unitary) 34 | * \author Marcus A Johansson */ 35 | class GeometricLine : public GeometricPrimitive { 36 | public: 37 | 38 | GeometricLine(const std::string& name, 39 | const std::string& frame_id, 40 | bool visible, 41 | const std::vector& color) 42 | : GeometricPrimitive(name, frame_id, visible, color) {} 43 | 44 | ~GeometricLine() noexcept = default; 45 | 46 | 47 | /*! \brief Parses a set of parameters and initializes the line. 48 | * 49 | * \param parameters : Should be of size 6.
    50 | *
  1. Indices 0-2 (required) defines the directional vector of the cylinder.
    2. 51 | *
  2. Indices 3-5 (required) defines the position of a point on the cylinder's coaxial line.
    3. 52 | *
53 | * \return 0 on success, -1 if the wrong number of parameters was sent */ 54 | int init(const std::vector& parameters) { 55 | int size = parameters.size(); 56 | if (size != 6) 57 | { 58 | printHiqpWarning("GeometricLine requires 6 parameters, got " 59 | + std::to_string(size) + "! Initialization failed!"); 60 | return -1; 61 | } 62 | 63 | kdl_v_(0) = parameters.at(0); 64 | kdl_v_(1) = parameters.at(1); 65 | kdl_v_(2) = parameters.at(2); 66 | 67 | kdl_v_.Normalize(); 68 | 69 | kdl_p_(0) = parameters.at(3); 70 | kdl_p_(1) = parameters.at(4); 71 | kdl_p_(2) = parameters.at(5); 72 | 73 | l_ = -1; // infinitely long line 74 | 75 | eigen_v_ << kdl_v_(0), kdl_v_(1), kdl_v_(2); 76 | eigen_p_ << kdl_p_(0), kdl_p_(1), kdl_p_(2); 77 | 78 | return 0; 79 | } 80 | 81 | inline const KDL::Vector& getDirectionKDL() { return kdl_v_; } 82 | inline const Eigen::Vector3d& getDirectionEigen() { return eigen_v_; } 83 | inline const KDL::Vector& getOffsetKDL() { return kdl_p_; } 84 | inline const Eigen::Vector3d& getOffsetEigen() { return eigen_p_; } 85 | inline bool isInfinite() { return (l_ < 0); } 86 | 87 | inline double getDirectionX() { return kdl_v_(0); } 88 | inline double getDirectionY() { return kdl_v_(1); } 89 | inline double getDirectionZ() { return kdl_v_(2); } 90 | 91 | inline double getOffsetX() { return kdl_p_(0); } 92 | inline double getOffsetY() { return kdl_p_(1); } 93 | inline double getOffsetZ() { return kdl_p_(2); } 94 | 95 | protected: 96 | KDL::Vector kdl_v_; // the directional vector of the line 97 | Eigen::Vector3d eigen_v_; 98 | 99 | KDL::Vector kdl_p_; // the offset 100 | Eigen::Vector3d eigen_p_; 101 | 102 | double l_; // the length of the line segment, 103 | // negative if the line is infinite 104 | private: 105 | GeometricLine(const GeometricLine& other) = delete; 106 | GeometricLine(GeometricLine&& other) = delete; 107 | GeometricLine& operator=(const GeometricLine& other) = delete; 108 | GeometricLine& operator=(GeometricLine&& other) noexcept = delete; 109 | }; 110 | 111 | } // namespace geometric_primitives 112 | 113 | } // namespace hiqp 114 | 115 | #endif // include guard 116 | -------------------------------------------------------------------------------- /hiqp_core/src/tasks/tdef_geometric_alignment.cpp: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | namespace hiqp 33 | { 34 | namespace tasks 35 | { 36 | 37 | template<> 38 | int TDefGeometricAlignment::align 39 | ( 40 | std::shared_ptr line1, 41 | std::shared_ptr line2 42 | ) 43 | { 44 | KDL::Vector v1 = pose_a_.M * line1->getDirectionKDL(); 45 | KDL::Vector v2 = pose_b_.M * line2->getDirectionKDL(); 46 | 47 | return alignVectors(v1, v2); 48 | } 49 | 50 | 51 | 52 | 53 | 54 | template<> 55 | int TDefGeometricAlignment::align 56 | ( 57 | std::shared_ptr line, 58 | std::shared_ptr plane 59 | ) 60 | { 61 | KDL::Vector v1 = pose_a_.M * line->getDirectionKDL(); 62 | KDL::Vector v2 = - ( pose_b_.M * plane->getNormalKDL() ); 63 | 64 | return alignVectors(v1, v2); 65 | } 66 | 67 | 68 | 69 | 70 | 71 | template<> 72 | int TDefGeometricAlignment::align 73 | ( 74 | std::shared_ptr line, 75 | std::shared_ptr cylinder 76 | ) 77 | { 78 | KDL::Vector v1 = - (pose_a_.M * line->getDirectionKDL()); 79 | 80 | KDL::Vector p = pose_a_.p + pose_a_.M * line->getOffsetKDL(); 81 | KDL::Vector d = pose_b_.p + pose_b_.M * cylinder->getOffsetKDL(); 82 | KDL::Vector v = pose_b_.M * cylinder->getDirectionKDL(); 83 | 84 | KDL::Vector x = KDL::dot( (p-d), v) * v; 85 | 86 | KDL::Vector v2 = (p-d) - x; 87 | 88 | v2.Normalize(); 89 | 90 | return alignVectors(v1, v2); 91 | } 92 | 93 | 94 | 95 | 96 | 97 | template<> 98 | int TDefGeometricAlignment::align 99 | ( 100 | std::shared_ptr line, 101 | std::shared_ptr sphere 102 | ) 103 | { 104 | KDL::Vector v1 = pose_a_.M * line->getDirectionKDL(); 105 | 106 | KDL::Vector p = pose_a_.p + pose_a_.M * line->getOffsetKDL(); 107 | KDL::Vector d = pose_b_.p + pose_b_.M * sphere->getCenterKDL(); 108 | 109 | KDL::Vector v2 = d - p; 110 | 111 | v2.Normalize(); 112 | 113 | return alignVectors(v1, v2); 114 | } 115 | 116 | 117 | 118 | /// \bug Frame alignment seems not to consider the axis sense (e.g., alignment along x and -x seems the same) 119 | template<> 120 | int TDefGeometricAlignment::align 121 | ( 122 | std::shared_ptr frame1, 123 | std::shared_ptr frame2 124 | ) 125 | { 126 | KDL::Vector ax1 = pose_a_.M * frame1->getAxisXKDL(); 127 | KDL::Vector ax2 = pose_b_.M * frame2->getAxisXKDL(); 128 | KDL::Vector ay1 = pose_a_.M * frame1->getAxisYKDL(); 129 | KDL::Vector ay2 = pose_b_.M * frame2->getAxisYKDL(); 130 | 131 | double d1 = KDL::dot(ax1, ax2); 132 | double d2 = KDL::dot(ay1, ay2); 133 | 134 | e_(0) = d1 - std::cos(delta_); 135 | e_(1) = d2 - std::cos(delta_); 136 | 137 | KDL::Vector v1 = ax1 * ax2; 138 | KDL::Vector v2 = ay1 * ay2; 139 | 140 | for (int q_nr = 0; q_nr < jacobian_a_.columns(); ++q_nr) { 141 | KDL::Vector Ja = jacobian_a_.getColumn(q_nr).rot; 142 | KDL::Vector Jb = jacobian_b_.getColumn(q_nr).rot; 143 | J_(0, q_nr) = KDL::dot( v1, (Ja - Jb) ); 144 | J_(1, q_nr) = KDL::dot( v2, (Ja - Jb) ); 145 | } 146 | 147 | return 0; 148 | } 149 | 150 | 151 | 152 | 153 | 154 | } // namespace tasks 155 | 156 | } // namespace hiqp 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/geometric_primitives/geometric_primitive_visualizer.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_GEOMETRIC_PRIMITIVE_VISUALIZER_H 18 | #define HIQP_GEOMETRIC_PRIMITIVE_VISUALIZER_H 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | namespace hiqp { 26 | 27 | namespace geometric_primitives { 28 | 29 | /*! \brief A Geometric Primitive Visitor that sends the primitive it visits to a visualizer. 30 | * \author Marcus A Johansson */ 31 | class GeometricPrimitiveVisualizer : public GeometricPrimitiveVisitor { 32 | public: 33 | /*! \brief Adds to, updates or removes a primitive from the visualizer given. If action=1, removeAllVisitedPrimitives has to be called after finishing visiting to perform the actual removal. 34 | * \param action : 0 for adding/updating the visited geometric primitive to the visualizer, 1 for removing it. */ 35 | GeometricPrimitiveVisualizer(std::shared_ptr visualizer, int action) 36 | : visualizer_(visualizer), action_(action) 37 | {} 38 | 39 | ~GeometricPrimitiveVisualizer() noexcept {} 40 | 41 | void visit(std::shared_ptr point) { visit__(point); } 42 | void visit(std::shared_ptr line) { visit__(line); } 43 | void visit(std::shared_ptr plane) { visit__(plane); } 44 | void visit(std::shared_ptr box) { visit__(box); } 45 | void visit(std::shared_ptr cylinder) { visit__(cylinder); } 46 | void visit(std::shared_ptr sphere) { visit__(sphere); } 47 | void visit(std::shared_ptr frame); 48 | 49 | /// \brief Effectively removes all visited geometric primitives. 50 | void removeAllVisitedPrimitives() { 51 | visualizer_->removeMany(visited_visual_ids_); 52 | } 53 | 54 | private: 55 | GeometricPrimitiveVisualizer(const GeometricPrimitiveVisualizer& other) = delete; 56 | GeometricPrimitiveVisualizer(GeometricPrimitiveVisualizer&& other) = delete; 57 | GeometricPrimitiveVisualizer& operator=(const GeometricPrimitiveVisualizer& other) = delete; 58 | GeometricPrimitiveVisualizer& operator=(GeometricPrimitiveVisualizer&& other) noexcept = delete; 59 | 60 | template 61 | void visit__(std::shared_ptr primitive); 62 | 63 | std::shared_ptr visualizer_; 64 | int action_; 65 | std::vector visited_visual_ids_; 66 | }; 67 | 68 | template 69 | void GeometricPrimitiveVisualizer::visit__(std::shared_ptr primitive) { 70 | int id = primitive->getVisualId(); 71 | switch (action_) { 72 | case 0: 73 | if (id < 0) primitive->setVisualId(visualizer_->add(primitive)); 74 | else visualizer_->update(id, primitive); 75 | break; 76 | case 1: 77 | if (id >= 0) visited_visual_ids_.push_back(id); 78 | break; 79 | default: 80 | break; 81 | } 82 | } 83 | 84 | template <> 85 | void GeometricPrimitiveVisualizer::visit__(std::shared_ptr primitive) { 86 | int id = primitive->getVisualId(); 87 | switch (action_) { 88 | case 0: 89 | if (id < 0) primitive->setVisualId(visualizer_->add(primitive)); 90 | else visualizer_->update(id, primitive); 91 | break; 92 | case 1: 93 | if (id >= 0) { 94 | // the frame primitive consists of three successive visual markers 95 | visited_visual_ids_.push_back(id); 96 | visited_visual_ids_.push_back(id+1); 97 | visited_visual_ids_.push_back(id+2); 98 | } 99 | break; 100 | default: 101 | break; 102 | } 103 | } 104 | 105 | void GeometricPrimitiveVisualizer::visit(std::shared_ptr frame) { 106 | visit__(frame); 107 | } 108 | 109 | } // namespace geometric_primitives 110 | 111 | } // namespace hiqp 112 | 113 | #endif // include guard -------------------------------------------------------------------------------- /hiqp_core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # The HiQP Control Framework, an optimal control framework targeted at robotics 2 | # Copyright (C) 2016 Marcus A Johansson 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License 15 | # along with this program. If not, see . 16 | 17 | cmake_minimum_required(VERSION 2.8.6) 18 | project(hiqp_core) 19 | 20 | # Available backends: qpoases, gurobi, casadi 21 | set(HIQP_QPSOLVER_BACKEND "gurobi") 22 | 23 | ### 24 | ### --- DONT EDIT BELOW THIS LINE --- 25 | ### 26 | 27 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}) 28 | message(${CMAKE_MODULE_PATH}) 29 | set(CMAKE_CXX_FLAGS "-O3 -std=c++11 -Wl,-z,defs") 30 | set(CMAKE_VERBOSE_MAKEFILE off) 31 | 32 | find_package(catkin REQUIRED COMPONENTS roscpp 33 | controller_interface 34 | visualization_msgs 35 | kdl_parser 36 | hiqp_msgs) 37 | find_package(orocos_kdl REQUIRED) 38 | find_package(Eigen3 REQUIRED) 39 | 40 | catkin_package(CATKIN_DEPENDS roscpp controller_interface visualization_msgs kdl_parser hiqp_msgs 41 | INCLUDE_DIRS include 42 | LIBRARIES ${PROJECT_NAME} 43 | DEPENDS orocos_kdl) 44 | 45 | include_directories(include ${catkin_INCLUDE_DIRS}) 46 | 47 | set(SOLVER_SOURCE_FILE "") 48 | if(${HIQP_QPSOLVER_BACKEND} STREQUAL "qpoases") 49 | set(SOLVER_SOURCE_FILE "src/solvers/qpoases_solver.cpp") 50 | add_definitions(-DHIQP_QPOASES) 51 | elseif(${HIQP_QPSOLVER_BACKEND} STREQUAL "gurobi") 52 | set(GUROBI_INCLUDE_DIR "$ENV{GUROBI_HOME}/include") 53 | set(GUROBI_LIB_DIR "$ENV{GUROBI_HOME}/lib") 54 | set(GUROBI_LIBS gurobi_c++ gurobi65) 55 | include_directories(${GUROBI_INCLUDE_DIR}) 56 | link_directories(${GUROBI_LIB_DIR}) 57 | set(SOLVER_SOURCE_FILE "src/solvers/gurobi_solver.cpp") 58 | add_definitions(-DHIQP_GUROBI) 59 | elseif(${HIQP_QPSOLVER_BACKEND} STREQUAL "casadi") 60 | set(GUROBI_INCLUDE_DIR "$ENV{GUROBI_HOME}/include") 61 | set(GUROBI_LIB_DIR "$ENV{GUROBI_HOME}/lib") 62 | set(GUROBI_LIBS gurobi_c++ gurobi65) 63 | find_package(CASADI REQUIRED) 64 | catkin_package(LIBRARIES casadi) 65 | include_directories(${GUROBI_INCLUDE_DIR} ${CASADI_INCLUDE_DIR}) 66 | link_directories(${GUROBI_LIB_DIR}) 67 | set(SOLVER_SOURCE_FILE "src/solvers/casadi_solver.cpp") 68 | add_definitions(-DHIQP_CASADI) 69 | endif() 70 | 71 | add_library(${PROJECT_NAME} src/utilities.cpp 72 | src/hiqp_time_point.cpp 73 | src/task_manager.cpp 74 | src/task.cpp 75 | 76 | src/geometric_primitives/geometric_primitive_map.cpp 77 | 78 | ${SOLVER_SOURCE_FILE} 79 | 80 | src/tasks/tdyn_linear.cpp 81 | src/tasks/tdyn_cubic.cpp 82 | src/tasks/tdyn_hyper_sin.cpp 83 | src/tasks/tdyn_jnt_limits.cpp 84 | src/tasks/tdyn_minimal_jerk.cpp 85 | 86 | src/tasks/tdef_full_pose.cpp 87 | src/tasks/tdef_geometric_alignment.cpp 88 | src/tasks/tdef_geometric_projection.cpp 89 | src/tasks/tdef_jnt_config.cpp 90 | src/tasks/tdef_jnt_limits.cpp) 91 | 92 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} 93 | ${orocos_kdl_LIBRARIES}) 94 | if(${HIQP_QPSOLVER_BACKEND} STREQUAL "gurobi") 95 | target_link_libraries(${PROJECT_NAME} ${GUROBI_LIBS}) 96 | elseif(${HIQP_QPSOLVER_BACKEND} STREQUAL "casadi") 97 | target_link_libraries(${PROJECT_NAME} ${CASADI_LIBRARIES} ${GUROBI_LIBS}) 98 | endif() 99 | 100 | install(DIRECTORY include/${PROJECT_NAME}/ 101 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 102 | FILES_MATCHING PATTERN "*.h") 103 | 104 | install(TARGETS ${PROJECT_NAME} 105 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 106 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 107 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 108 | -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/utilities.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_UTILS_H 18 | #define HIQP_UTILS_H 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | namespace hiqp 32 | { 33 | 34 | std::ostream& operator<<(std::ostream& os, const KDL::Vector& kdl_vector); 35 | std::ostream& operator<<(std::ostream& os, const KDL::Tree& kdl_tree); 36 | std::ostream& operator<<(std::ostream& os, const KDL::FrameVel& kdl_frame_vel); 37 | std::ostream& operator<<(std::ostream& os, const KDL::JntArrayVel& kdl_joints_vel); 38 | std::ostream& operator<<(std::ostream& os, const KDL::Chain& kdl_chain); 39 | 40 | int kdl_getAllQNrFromTree(const KDL::Tree& kdl_tree, std::vector& qnrs); 41 | 42 | std::string kdl_getJointNameFromQNr(const KDL::Tree& kdl_tree, unsigned int q_nr); 43 | 44 | int kdl_getQNrFromJointName(const KDL::Tree& kdl_tree, 45 | const std::string& joint_name); 46 | 47 | int kdl_getQNrFromLinkName(const KDL::Tree& kdl_tree, 48 | const std::string& link_name); 49 | 50 | int kdl_JntToJac(const KDL::Tree& tree, 51 | const KDL::JntArrayVel& qqdot, 52 | KDL::Jacobian& jac, 53 | const std::string& segmentname); 54 | 55 | void printHiqpInfo(const std::string& msg); 56 | void printHiqpWarning(const std::string& msg); 57 | 58 | /// \brief Returns the largest absolute value of all entries in v 59 | double absMax(std::vector v); 60 | 61 | /*! \brief Calculates the Moore-Penrose Pseudoinverse for any sized matrices. The original source code is got from http://eigendobetter.com/, I edited it to be compilable in this form. 62 | * \author Marcus A Johansson 63 | * \param a : the matrix to be inverted 64 | * \return the inverted matrix */ 65 | template 66 | Derived pinv(const Eigen::MatrixBase& a) { 67 | typedef typename Eigen::MatrixBase::RealScalar RealScalar; 68 | if (a.rows() < a.cols()) 69 | { 70 | auto svd = a.derived().transpose().jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV); 71 | RealScalar tolerance = (RealScalar)std::numeric_limits::epsilon() * std::max((RealScalar)a.cols(), (RealScalar)a.rows()) * svd.singularValues().array().abs().maxCoeff(); 72 | return (svd.matrixV() * Derived((svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0)).asDiagonal() * svd.matrixU().adjoint()).transpose(); 73 | } 74 | Eigen::JacobiSVD svd = a.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV); 75 | RealScalar tolerance = (RealScalar)std::numeric_limits::epsilon() * std::max((RealScalar)a.cols(), (RealScalar)a.rows()) * svd.singularValues().array().abs().maxCoeff(); 76 | return svd.matrixV() * Derived((svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0)).asDiagonal() * svd.matrixU().adjoint(); 77 | } 78 | 79 | /*! \brief calculates the Damped-Least-Square matrix. 80 | * \author Marcus A Johansson 81 | * \param a : the matrix to be inverted 82 | * \return the damped-least-square matrix on success, 83 | * the given matrix is returned otherwise. */ 84 | template 85 | Derived dls(const Eigen::MatrixBase& a, double eta = 0.01) { 86 | unsigned int r = a.rows(); 87 | unsigned int c = a.cols(); 88 | 89 | if (r > c) return a; 90 | if (r == c) return pinv(a); 91 | 92 | Derived a_ext(r+c, c); 93 | a_ext.block(0, 0, r, c) = a; 94 | a_ext.block(r, 0, c, c) = eta * Eigen::MatrixXd::Identity(c, c); 95 | Derived b = Eigen::MatrixXd::Zero(r+c, r); 96 | b.block(0, 0, r, r) = Eigen::MatrixXd::Identity(r, r); 97 | 98 | return pinv(a_ext) * b; 99 | } 100 | 101 | } // namespace hiqp 102 | 103 | #endif -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/geometric_primitives/geometric_cylinder.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_GEOMETRIC_CYLINDER_H 18 | #define HIQP_GEOMETRIC_CYLINDER_H 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | 25 | #include 26 | 27 | namespace hiqp 28 | { 29 | namespace geometric_primitives 30 | { 31 | 32 | /*! \brief Parameters: [dir.x, dir.y, dir.z, offset.x, offset.y, offset.z, radius, height] 33 | * \author Marcus A Johansson */ 34 | class GeometricCylinder : public GeometricPrimitive 35 | { 36 | public: 37 | GeometricCylinder(const std::string& name, 38 | const std::string& frame_id, 39 | bool visible, 40 | const std::vector& color) 41 | : GeometricPrimitive(name, frame_id, visible, color) {} 42 | 43 | ~GeometricCylinder() noexcept = default; 44 | 45 | /*! \brief Parses a set of parameters and initializes the cylinder. 46 | * 47 | * \param parameters : Should be of size 8.
    48 | *
  1. Indices 0-2 (required) defines the directional vector of the cylinder.
    2. 49 | *
  2. Indices 3-5 (required) defines the position of a point on the cylinder's coaxial line.
    3. 50 | *
  3. Index 6 (required) defines the radius of the cylinder.
    4. 51 | *
  4. Index 7 (required) defines the height of the cylinder.
    5. 52 | *
53 | * \return 0 on success, -1 if the wrong number of parameters was sent */ 54 | int init(const std::vector& parameters) { 55 | int size = parameters.size(); 56 | if (size != 8) 57 | { 58 | printHiqpWarning("GeometricCylinder requires 8 parameters, got " 59 | + std::to_string(size) + "! Initialization failed!"); 60 | return -1; 61 | } 62 | 63 | kdl_v_(0) = parameters.at(0); 64 | kdl_v_(1) = parameters.at(1); 65 | kdl_v_(2) = parameters.at(2); 66 | kdl_v_.Normalize(); 67 | 68 | kdl_p_(0) = parameters.at(3); 69 | kdl_p_(1) = parameters.at(4); 70 | kdl_p_(2) = parameters.at(5); 71 | 72 | radius_ = parameters.at(6); 73 | 74 | h_ = parameters.at(7); 75 | 76 | eigen_v_ << kdl_v_(0), kdl_v_(1), kdl_v_(2); 77 | eigen_p_ << kdl_p_(0), kdl_p_(1), kdl_p_(2); 78 | 79 | return 0; 80 | } 81 | 82 | inline const KDL::Vector& getDirectionKDL() { return kdl_v_; } 83 | 84 | inline const Eigen::Vector3d& getDirectionEigen() { return eigen_v_; } 85 | 86 | inline const KDL::Vector& getOffsetKDL() { return kdl_p_; } 87 | 88 | inline const Eigen::Vector3d& getOffsetEigen() { return eigen_p_; } 89 | 90 | inline double getHeight() { return h_; } 91 | 92 | inline double getRadius() { return radius_; } 93 | 94 | inline bool isInfinite() { return (h_ < 0); } 95 | 96 | inline double getDirectionX() { return kdl_v_(0); } 97 | 98 | inline double getDirectionY() { return kdl_v_(1); } 99 | 100 | inline double getDirectionZ() { return kdl_v_(2); } 101 | 102 | inline double getOffsetX() { return kdl_p_(0); } 103 | 104 | inline double getOffsetY() { return kdl_p_(1); } 105 | 106 | inline double getOffsetZ() { return kdl_p_(2); } 107 | 108 | protected: 109 | KDL::Vector kdl_v_; // the directional vector of the cylinder 110 | Eigen::Vector3d eigen_v_; 111 | 112 | KDL::Vector kdl_p_; // the offset of the cylinder base 113 | Eigen::Vector3d eigen_p_; 114 | 115 | double h_; // the height of the cylinder 116 | 117 | double radius_; // the radius of the cylinder 118 | 119 | private: 120 | GeometricCylinder(const GeometricCylinder& other) = delete; 121 | GeometricCylinder(GeometricCylinder&& other) = delete; 122 | GeometricCylinder& operator=(const GeometricCylinder& other) = delete; 123 | GeometricCylinder& operator=(GeometricCylinder&& other) noexcept = delete; 124 | 125 | }; 126 | 127 | } // namespace geometric_primitives 128 | 129 | } // namespace hiqp 130 | 131 | #endif // include guard -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/geometric_primitives/geometric_frame.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_GEOMETRIC_FRAME_H 18 | #define HIQP_GEOMETRIC_FRAME_H 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | 25 | #include 26 | 27 | namespace hiqp 28 | { 29 | namespace geometric_primitives 30 | { 31 | 32 | /*! \brief Parameters: [x, y, z] 33 | * [x, y, z, ax, ay, az] 34 | * [x, y, z, qw, qx, qy, qz] 35 | * \author Marcus A Johansson */ 36 | class GeometricFrame : public GeometricPrimitive { 37 | public: 38 | GeometricFrame(const std::string& name, 39 | const std::string& frame_id, 40 | bool visible, 41 | const std::vector& color) 42 | : GeometricPrimitive(name, frame_id, visible, color) {} 43 | 44 | ~GeometricFrame() noexcept {} 45 | 46 | int init(const std::vector& parameters) { 47 | int size = parameters.size(); 48 | if (!(size == 3 || size == 6 || size == 7)) { 49 | printHiqpWarning("GeometricFrame requires 3, 6 or 7 parameters, got " 50 | + std::to_string(size) + "! Initialization failed!"); 51 | return -1; 52 | } 53 | 54 | kdl_c_(0) = parameters.at(0); 55 | kdl_c_(1) = parameters.at(1); 56 | kdl_c_(2) = parameters.at(2); 57 | 58 | eigen_c_ << kdl_c_(0), kdl_c_(1), kdl_c_(2); 59 | 60 | if (size == 6) { 61 | double angle1 = parameters.at(3); 62 | double angle2 = parameters.at(4); 63 | double angle3 = parameters.at(5); 64 | 65 | Eigen::Matrix3d m; 66 | m = Eigen::AngleAxisd(angle1, Eigen::Vector3d::UnitX()) 67 | * Eigen::AngleAxisd(angle2, Eigen::Vector3d::UnitY()) 68 | * Eigen::AngleAxisd(angle3, Eigen::Vector3d::UnitZ()); 69 | q_ = Eigen::Quaternion(m); 70 | 71 | } else if (size == 7) { 72 | double w = parameters.at(3); 73 | double x = parameters.at(4); 74 | double y = parameters.at(5); 75 | double z = parameters.at(6); 76 | q_ = Eigen::Quaternion(w, x, y, z); 77 | 78 | } else { 79 | q_ = Eigen::Quaternion(1.0, 0.0, 0.0, 0.0); 80 | } 81 | 82 | Eigen::Vector3d ax = q_._transformVector(Eigen::Vector3d(1,0,0)); 83 | axis_x_ = KDL::Vector(ax(0), ax(1), ax(2)); 84 | 85 | Eigen::Vector3d ay = q_._transformVector(Eigen::Vector3d(0,1,0)); 86 | axis_y_ = KDL::Vector(ay(0), ay(1), ay(2)); 87 | 88 | Eigen::Vector3d az = q_._transformVector(Eigen::Vector3d(0,1,0)); 89 | axis_z_ = KDL::Vector(az(0), az(1), az(2)); 90 | 91 | return 0; 92 | } 93 | 94 | inline const KDL::Vector& getCenterKDL() { return kdl_c_; } 95 | inline const Eigen::Vector3d& getCenterEigen() { return eigen_c_; } 96 | inline const Eigen::Quaternion& getQuaternionEigen() { return q_; } 97 | 98 | inline const KDL::Vector& getAxisXKDL() { return axis_x_; } 99 | inline const KDL::Vector& getAxisYKDL() { return axis_y_; } 100 | inline const KDL::Vector& getAxisZKDL() { return axis_z_; } 101 | 102 | inline double getX() { return kdl_c_(0); } 103 | inline double getY() { return kdl_c_(1); } 104 | inline double getZ() { return kdl_c_(2); } 105 | 106 | inline double getQW() { return q_.w(); } 107 | inline double getQX() { return q_.x(); } 108 | inline double getQY() { return q_.y(); } 109 | inline double getQZ() { return q_.z(); } 110 | 111 | protected: 112 | KDL::Vector kdl_c_; 113 | Eigen::Vector3d eigen_c_; 114 | 115 | KDL::Vector axis_x_; 116 | KDL::Vector axis_y_; 117 | KDL::Vector axis_z_; 118 | 119 | Eigen::Quaternion q_; 120 | 121 | private: 122 | GeometricFrame(const GeometricFrame& other) = delete; 123 | GeometricFrame(GeometricFrame&& other) = delete; 124 | GeometricFrame& operator=(const GeometricFrame& other) = delete; 125 | GeometricFrame& operator=(GeometricFrame&& other) noexcept = delete; 126 | 127 | }; // class Geometric Point 128 | 129 | } // namespace geometric_primitives 130 | 131 | } // namespace hiqp 132 | 133 | #endif // include guard -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/task.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef TASK_H 18 | #define TASK_H 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | namespace hiqp { 32 | 33 | /*! \brief A Task has a TaskDefinition and a TaskDynamics. 34 | * \author Marcus A Johansson */ 35 | class Task { 36 | public: 37 | Task(std::shared_ptr geom_prim_map, 38 | std::shared_ptr visualizer, 39 | int n_controls); 40 | 41 | ~Task() noexcept {} 42 | 43 | /*! \brief This should be called after all fields of Task are set properly. */ 44 | int init(const std::vector& def_params, 45 | const std::vector& dyn_params, 46 | RobotStatePtr robot_state); 47 | 48 | inline void setTaskName(const std::string& name) { task_name_ = name; } 49 | inline std::string getTaskName() { return task_name_; } 50 | inline void setPriority(unsigned int priority) { priority_ = priority; } 51 | inline unsigned int getPriority() { return priority_; } 52 | inline void setActive(bool active) { active_ = active; } 53 | inline bool getActive() { return active_; } 54 | inline void setVisible(bool visible) { visible_ = visible; } 55 | inline bool getVisible() { return visible_; } 56 | inline void setMonitored(bool monitored) { monitored_ = monitored; } 57 | inline bool getMonitored() { return monitored_; } 58 | inline unsigned int getDimensions() { if (def_) return def_->getDimensions(); else return 0; } 59 | 60 | /*! \brief Recomputes the task performance value, jacobian and its dynamics. */ 61 | int update(RobotStatePtr robot_state); 62 | 63 | void monitor() {if (def_) def_->monitor(); if (dyn_) dyn_->monitor();} 64 | 65 | /*! \brief Returns the task function performance values as a vector. */ 66 | Eigen::VectorXd getValue() const 67 | { if (def_) return def_->e_; else return Eigen::VectorXd(); } 68 | 69 | /*! \brief Returns the task jacobian as a matrix. */ 70 | Eigen::MatrixXd getJacobian() const 71 | { if (def_) return def_->J_; else return Eigen::MatrixXd(); } 72 | 73 | /*! \brief Returns the task dynamics as a vector. */ 74 | Eigen::VectorXd getDynamics() const 75 | { if (dyn_) return dyn_->e_dot_star_; else return Eigen::VectorXd(); } 76 | 77 | /*! \brief Returns the task types (leq/eq/geq task) for each dimension of the task space. Returns a vector or -1, 0 or 1 for leq, eq and geq tasks respectively. */ 78 | std::vector getTaskTypes() const 79 | { if (def_) return def_->task_types_; else return std::vector(); } 80 | 81 | /*! \brief Returns the user-defined custom performance measures defined in the monitor() member function in a child class of TaskDefinition. */ 82 | Eigen::VectorXd getPerformanceMeasures() const 83 | { if (def_) return def_->performance_measures_; else return Eigen::VectorXd(); } 84 | 85 | private: 86 | Task(const Task& other) = delete; 87 | Task(Task&& other) = delete; 88 | Task& operator=(const Task& other) = delete; 89 | Task& operator=(Task&& other) noexcept = delete; 90 | 91 | int constructDefinition(const std::vector& def_params); 92 | int constructDynamics(const std::vector& dyn_params); 93 | 94 | /// \brief Checks the consistency of the task definition and dynamics, i.e. the sizes of e, J, de*, task types, and number of joints of the robot 95 | bool checkConsistency(RobotStatePtr robot_state); 96 | 97 | std::shared_ptr def_; 98 | std::shared_ptr dyn_; 99 | 100 | std::shared_ptr geom_prim_map_; 101 | std::shared_ptr visualizer_; 102 | 103 | unsigned int n_controls_; 104 | std::string task_name_; 105 | unsigned int priority_; 106 | bool active_; 107 | bool visible_; 108 | bool monitored_; 109 | }; 110 | 111 | } // namespace hiqp 112 | 113 | #endif // include guard 114 | -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/task_manager.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_TASK_MANAGER_H 18 | #define HIQP_TASK_MANAGER_H 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | namespace hiqp { 33 | 34 | /*! \brief A structure used to get data out of tasks that is to be monitored. 35 | * \author Marcus A Johansson */ 36 | class TaskMeasure { 37 | public: 38 | TaskMeasure(const std::string& task_name, 39 | const Eigen::VectorXd& e, 40 | const Eigen::VectorXd& de, 41 | const Eigen::VectorXd& pm) 42 | : task_name_(task_name), e_(e), de_(de), pm_(pm) {} 43 | 44 | std::string task_name_; 45 | Eigen::VectorXd e_; 46 | Eigen::VectorXd de_; 47 | Eigen::VectorXd pm_; 48 | }; 49 | 50 | /*! \brief The central mediator class in the HiQP framework. 51 | * \author Marcus A Johansson */ 52 | class TaskManager { 53 | public: 54 | TaskManager(std::shared_ptr visualizer); 55 | ~TaskManager() noexcept; 56 | 57 | void init(unsigned int n_controls); 58 | 59 | /*! \brief Generates controls from a particular robot state. */ 60 | bool getVelocityControls(RobotStatePtr robot_state, 61 | std::vector &controls); 62 | 63 | /*! \brief Retrieves the performance measures for every active task along 64 | * with the task's name and unique identifier. */ 65 | void getTaskMeasures(std::vector& data); 66 | 67 | /// \brief Redraws all primitives using the currently set visualizer 68 | void renderPrimitives(); 69 | 70 | /// \brief Returns a shared pointer to the common geometric primitive map object 71 | inline std::shared_ptr getGeometricPrimitiveMap() { return geometric_primitive_map_; } 72 | 73 | /*! \brief Adds a new task to the task manager or updates an existing one 74 | * \return 0 if the task creation was successful, 75 | * -1 is the behaviour_parameters argument was invalid, 76 | * -2 if the task behaviour name was not recognised, 77 | * -3 if the task name was not recognised */ 78 | int setTask(const std::string& task_name, 79 | unsigned int priority, 80 | bool visible, 81 | bool active, 82 | bool monitored, 83 | const std::vector& def_params, 84 | const std::vector& dyn_params, 85 | RobotStatePtr robot_state); 86 | int removeTask(std::string task_name); 87 | int removeAllTasks(); 88 | int listAllTasks(); 89 | int activateTask(const std::string& task_name); 90 | int deactivateTask(const std::string& task_name); 91 | int monitorTask(const std::string& task_name); 92 | int demonitorTask(const std::string& task_name); 93 | 94 | int setPrimitive(const std::string& name, 95 | const std::string& type, 96 | const std::string& frame_id, 97 | bool visible, 98 | const std::vector& color, 99 | const std::vector& parameters); 100 | int removePrimitive(std::string name); 101 | int removeAllPrimitives(); 102 | int listAllPrimitives(); 103 | 104 | int removePriorityLevel(unsigned int priority); 105 | int activatePriorityLevel(unsigned int priority); 106 | int deactivatePriorityLevel(unsigned int priority); 107 | int monitorPriorityLevel(unsigned int priority); 108 | int demonitorPriorityLevel(unsigned int priority); 109 | 110 | private: 111 | TaskManager(const TaskManager& other) = delete; 112 | TaskManager(TaskManager&& other) = delete; 113 | TaskManager& operator=(const TaskManager& other) = delete; 114 | TaskManager& operator=(TaskManager&& other) noexcept = delete; 115 | 116 | typedef std::map< std::string, std::shared_ptr > TaskMap; 117 | 118 | std::shared_ptr geometric_primitive_map_; 119 | std::shared_ptr visualizer_; 120 | 121 | TaskMap task_map_; 122 | 123 | std::shared_ptr solver_; 124 | 125 | std::mutex resource_mutex_; 126 | 127 | unsigned int n_controls_; 128 | }; 129 | 130 | } // namespace hiqp 131 | 132 | #endif 133 | -------------------------------------------------------------------------------- /hiqp_core/include/hiqp/hiqp_solver.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_HIQP_SOLVER_H 18 | #define HIQP_HIQP_SOLVER_H 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | namespace hiqp 27 | { 28 | 29 | /*! \brief A stage is a compound set of tasks with the same priority level. 30 | * \author Marcus A Johansson */ 31 | struct HiQPStage { 32 | int nRows; 33 | Eigen::VectorXd e_dot_star_; 34 | Eigen::MatrixXd J_; 35 | std::vector constraint_signs_; 36 | }; 37 | 38 | /*! \brief The base class for a solver for controls from a set of stages. Keeps an internal set of stages that tasks can be appended to. 39 | * \author Marcus A Johansson */ 40 | class HiQPSolver { 41 | public: 42 | HiQPSolver() {} 43 | ~HiQPSolver() noexcept {} 44 | 45 | virtual bool solve(std::vector& solution) = 0; 46 | 47 | int clearStages() { 48 | stages_map_.clear(); 49 | return 0; 50 | } 51 | 52 | /*! \brief Appends the internal set of stages with a task. If a stage with the priority is not currently present in the stages map, it is created, otherwise the task is appended to that existing stage. */ 53 | int appendStage(std::size_t priority_level, 54 | const Eigen::VectorXd& e_dot_star, 55 | const Eigen::MatrixXd& J, 56 | const std::vector& constraint_signs) { 57 | StageMap::iterator it = stages_map_.find(priority_level); 58 | 59 | if (it == stages_map_.end()) { 60 | 61 | HiQPStage stage; 62 | stage.e_dot_star_ = e_dot_star; 63 | stage.J_ = J; 64 | stage.constraint_signs_ = constraint_signs; 65 | stage.nRows = e_dot_star.rows(); 66 | stages_map_.emplace(priority_level, stage); 67 | // DEBUG ============================================= 68 | /* std::cerr<second.e_dot_star_.rows() + e_dot_star.rows(); 78 | // DEBUG ============================================= 79 | /* std::cerr<second.J_<second.constraint_signs_.size();k++) */ 83 | /* std::cerr<second.constraint_signs_[k]<<" "; */ 84 | 85 | /* std::cerr<second.e_dot_star_.transpose()<second.e_dot_star_, e_dot_star; 90 | it->second.e_dot_star_.resize(rows); 91 | it->second.e_dot_star_ = edotstar__; 92 | Eigen::MatrixXd J__(rows, it->second.J_.cols()); 93 | J__ << it->second.J_, J; 94 | it->second.J_ = J__; 95 | it->second.constraint_signs_.insert(it->second.constraint_signs_.end(), 96 | constraint_signs.begin(), 97 | constraint_signs.end() ); 98 | it->second.nRows += e_dot_star.rows(); 99 | // DEBUG ============================================= 100 | /* std::cerr<second.J_<second.constraint_signs_.size();k++) */ 104 | /* std::cerr<second.constraint_signs_[k]<<" "; */ 105 | 106 | /* std::cerr<second.e_dot_star_.transpose()< StageMap; 115 | StageMap stages_map_; 116 | 117 | private: 118 | HiQPSolver(const HiQPSolver& other) = delete; 119 | HiQPSolver(HiQPSolver&& other) = delete; 120 | HiQPSolver& operator=(const HiQPSolver& other) = delete; 121 | HiQPSolver& operator=(HiQPSolver&& other) noexcept = delete; 122 | }; 123 | 124 | } // namespace hiqp 125 | 126 | #endif // include guard 127 | -------------------------------------------------------------------------------- /hiqp_ros/include/hiqp_ros/hiqp_service_handler.h: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef HIQP_SERVICE_HANDLER_H 18 | #define HIQP_SERVICE_HANDLER_H 19 | 20 | #include 21 | 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | class HiQPServiceHandler { 45 | public: 46 | HiQPServiceHandler() = default; 47 | 48 | ~HiQPServiceHandler() noexcept = default; 49 | 50 | void init(std::shared_ptr node_handle, 51 | std::shared_ptr task_manager, 52 | hiqp::RobotStatePtr robot_state) { 53 | node_handle_ = node_handle; 54 | task_manager_ = task_manager; 55 | robot_state_ = robot_state; 56 | } 57 | 58 | void advertiseAll(); 59 | 60 | private: 61 | HiQPServiceHandler(const HiQPServiceHandler& other) = delete; 62 | HiQPServiceHandler(HiQPServiceHandler&& other) = delete; 63 | HiQPServiceHandler& operator=(const HiQPServiceHandler& other) = delete; 64 | HiQPServiceHandler& operator=(HiQPServiceHandler&& other) noexcept = delete; 65 | 66 | /// \todo Change add_primitive to set_primitive ros service 67 | /// \todo Add show_primitive ros service 68 | /// \todo Add hide_primitive ros service 69 | 70 | /// \todo Add activate_controller ros service 71 | /// \todo Add deactivate_controller ros service 72 | 73 | bool setTask(hiqp_msgs::SetTask::Request& req, hiqp_msgs::SetTask::Response& res); 74 | bool removeTask(hiqp_msgs::RemoveTask::Request& req, hiqp_msgs::RemoveTask::Response& res); 75 | bool removeAllTasks(hiqp_msgs::RemoveAllTasks::Request& req, hiqp_msgs::RemoveAllTasks::Response& res); 76 | bool listAllTasks(hiqp_msgs::ListAllTasks::Request& req, hiqp_msgs::ListAllTasks::Response& res); 77 | bool activateTask(hiqp_msgs::ActivateTask::Request& req, hiqp_msgs::ActivateTask::Response& res); 78 | bool deactivateTask(hiqp_msgs::DeactivateTask::Request& req, hiqp_msgs::DeactivateTask::Response& res); 79 | bool monitorTask(hiqp_msgs::MonitorTask::Request& req, hiqp_msgs::MonitorTask::Response& res); 80 | bool demonitorTask(hiqp_msgs::DemonitorTask::Request& req, hiqp_msgs::DemonitorTask::Response& res); 81 | 82 | bool setPrimitive(hiqp_msgs::SetPrimitive::Request& req, hiqp_msgs::SetPrimitive::Response& res); 83 | bool removePrimitive(hiqp_msgs::RemovePrimitive::Request& req, hiqp_msgs::RemovePrimitive::Response& res); 84 | bool removeAllPrimitives(hiqp_msgs::RemoveAllPrimitives::Request& req, hiqp_msgs::RemoveAllPrimitives::Response& res); 85 | bool listAllPrimitives(hiqp_msgs::ListAllPrimitives::Request& req, hiqp_msgs::ListAllPrimitives::Response& res); 86 | 87 | bool removePriorityLevel(hiqp_msgs::RemovePriorityLevel::Request& req, hiqp_msgs::RemovePriorityLevel::Response& res); 88 | bool activatePriorityLevel(hiqp_msgs::ActivatePriorityLevel::Request& req, hiqp_msgs::ActivatePriorityLevel::Response& res); 89 | bool deactivatePriorityLevel(hiqp_msgs::DeactivatePriorityLevel::Request& req, hiqp_msgs::DeactivatePriorityLevel::Response& res); 90 | bool monitorPriorityLevel(hiqp_msgs::MonitorPriorityLevel::Request& req, hiqp_msgs::MonitorPriorityLevel::Response& res); 91 | bool demonitorPriorityLevel(hiqp_msgs::DemonitorPriorityLevel::Request& req, hiqp_msgs::DemonitorPriorityLevel::Response& res); 92 | 93 | std::shared_ptr node_handle_; 94 | std::shared_ptr task_manager_; 95 | hiqp::RobotStatePtr robot_state_; 96 | 97 | ros::ServiceServer set_task_service_; 98 | ros::ServiceServer remove_task_service_; 99 | ros::ServiceServer remove_all_tasks_service_; 100 | ros::ServiceServer list_all_tasks_service_; 101 | ros::ServiceServer activate_task_service_; 102 | ros::ServiceServer deactivate_task_service_; 103 | ros::ServiceServer monitor_task_service_; 104 | ros::ServiceServer demonitor_task_service_; 105 | 106 | ros::ServiceServer set_primitive_service_; 107 | ros::ServiceServer remove_primitive_service_; 108 | ros::ServiceServer remove_all_primitives_service_; 109 | ros::ServiceServer list_all_primitives_service_; 110 | 111 | ros::ServiceServer remove_priority_level_service_; 112 | ros::ServiceServer activate_priority_level_service_; 113 | ros::ServiceServer deactivate_priority_level_service_; 114 | ros::ServiceServer monitor_priority_level_service_; 115 | ros::ServiceServer demonitor_priority_level_service_; 116 | }; 117 | 118 | #endif -------------------------------------------------------------------------------- /hiqp_core/src/solvers/casadi_solver.cpp: -------------------------------------------------------------------------------- 1 | // The HiQP Control Framework, an optimal control framework targeted at robotics 2 | // Copyright (C) 2016 Marcus A Johansson 3 | // 4 | // This program is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | #include 23 | 24 | namespace hiqp 25 | { 26 | 27 | // For debugging purposes 28 | std::ostream& operator<<(std::ostream& os, const HiQPStage& stage) 29 | { 30 | os << "rows: " << stage.nRows << "\n" 31 | << "e_dot_star_: " << stage.e_dot_star_ << "\n" 32 | << "J_: " << stage.J_ << "\n" 33 | << "signs_: " << "["; 34 | 35 | for (int i : stage.constraint_signs_) 36 | os << i << ", "; 37 | 38 | os << "]\n"; 39 | 40 | return os; 41 | } 42 | 43 | const double kDampingFactor = 1e-3; // dont set to zero! 44 | 45 | bool CasADiSolver::solve 46 | ( 47 | std::vector& solution 48 | ) 49 | { 50 | // StageMap::iterator it2 = stages_map_.begin(); 51 | // 52 | // std::cout << "--- casadi solve ---\n"; 53 | // while (it2 != stages_map_.end()) 54 | // { 55 | // std::cout << *it2; 56 | // // std::cout << "priority = " << it2->first << "\n"; 57 | // // std::cout << "e_dot_star_ = " << it2->second.e_dot_star_ << "\n\n"; 58 | // it2++; 59 | // } 60 | std::cout << "casadi solve!\n"; 61 | 62 | unsigned int solutionSize = solution.size(); 63 | 64 | // Setup the parts of the QP problem that wont change between interations 65 | casadi::SX qdot = casadi::SX::sym("q_dot", solutionSize, 1); 66 | casadi::SX f_qdot = kDampingFactor * casadi::SX::dot(qdot, qdot); 67 | casadi::SX g_i; 68 | std::vector lbx(solutionSize, -std::numeric_limits::infinity() ); 69 | std::vector ubx(solutionSize, std::numeric_limits::infinity() ); 70 | std::vector lbg, ubg; 71 | 72 | // Iterate over all stages and solve one QP per each stage 73 | StageMap::iterator it = stages_map_.begin(); 74 | while (it != stages_map_.end()) 75 | { 76 | HiQPStage& stage = it->second; 77 | ++it; 78 | int rows = stage.nRows; 79 | 80 | casadi::SX w_p = casadi::SX::sym("w_p", rows, 1); 81 | casadi::SX f_w = 0.5 * casadi::SX::dot(w_p, w_p); 82 | casadi::SX g_p = -w_p; 83 | 84 | for (int k=0; k::infinity() ); 93 | 94 | for (int sign : stage.constraint_signs_) 95 | { 96 | switch (sign) 97 | { 98 | case -1: // less-than-or-equal-to 99 | lbg.insert( lbg.end(), 1, -std::numeric_limits::infinity() ); 100 | ubg.insert( ubg.end(), 1, 0.0 ); 101 | break; 102 | 103 | case 0: // equal-to 104 | lbg.insert( lbg.end(), 1, 0.0 ); 105 | ubg.insert( ubg.end(), 1, 0.0 ); 106 | break; 107 | 108 | case 1: // greater-than-or-equal-to 109 | lbg.insert( lbg.end(), 1, 0.0 ); 110 | ubg.insert( ubg.end(), 1, std::numeric_limits::infinity() ); 111 | break; 112 | 113 | default: // equal-to 114 | lbg.insert( lbg.end(), 1, 0.0 ); 115 | ubg.insert( ubg.end(), 1, 0.0 ); 116 | break; 117 | } 118 | } 119 | 120 | 121 | casadi::DMDict arg = {{"lbx", lbx}, 122 | {"ubx", ubx}, 123 | {"lbg", lbg}, 124 | {"ubg", ubg}}; 125 | 126 | // For debugging purposes 127 | //std::cout << "edotstar = " << stage.e_dot_star_ << "\n\n"; 128 | //std::cout << "J = " << stage.J_ << "\n\n"; 129 | 130 | // std::cout << "x = " << vertcat(qdot, w_p) << "\n\n"; 131 | // std::cout << "f_qdot = " << f_qdot << "\n\n"; 132 | // std::cout << "f_w = " << f_w << "\n\n"; 133 | // std::cout << "f = " << f_qdot + f_w << "\n\n"; 134 | // std::cout << "g_i = " << g_i << "\n\n"; 135 | // std::cout << "g_p = " << g_p << "\n\n"; 136 | // std::cout << "g = " << vertcat(g_i, g_p) << "\n\n"; 137 | // std::cout << "lbx = " << lbx << "\n\n"; 138 | // std::cout << "ubx = " << ubx << "\n\n"; 139 | // std::cout << "lbg = " << lbg << "\n\n"; 140 | // std::cout << "ubg = " << ubg << "\n\n"; 141 | 142 | casadi::SXDict qp = {{ "x", vertcat(qdot, w_p) }, 143 | { "f", f_qdot + f_w }, 144 | { "g", vertcat(g_i, g_p) }}; 145 | casadi::Function solver = qpsol("solver", "gurobi", qp); 146 | // std::cout << "here \n"; 147 | casadi::DMDict res = solver(arg); 148 | // std::cout << "here2 \n"; 149 | casadi::DM x_opt = res["x"]; 150 | std::vector x_opt_d(x_opt); 151 | 152 | for (int k=0; k