├── LICENSE ├── README.md ├── assets ├── architecture.png └── seiko_talos_multicontact.png ├── inria_controller ├── CMakeLists.txt ├── config │ └── inria_controller.yaml ├── include │ └── inria_controller │ │ ├── ControllerBase.hpp │ │ ├── ControllerTalos.hpp │ │ ├── RhIOEigen.hpp │ │ ├── TaskBase.hpp │ │ ├── TaskBaseTalos.hpp │ │ ├── TaskSEIKO.hpp │ │ └── ThreadManager.hpp ├── package.xml ├── plugins.xml ├── scripts │ ├── startTalos.sh │ └── stopTalos.sh └── src │ ├── ControllerBase.cpp │ ├── ControllerTalos.cpp │ ├── TaskBase.cpp │ ├── TaskSEIKO.cpp │ └── ThreadManager.cpp ├── inria_maths ├── CMakeLists.txt ├── include │ └── inria_maths │ │ ├── Angle.h │ │ ├── AxisAngle.h │ │ ├── Clamp.h │ │ ├── Deadband.h │ │ ├── Euler.h │ │ ├── FilterDifferentiator2.hpp │ │ ├── FilterExponential.hpp │ │ ├── FilterExponentialRotation.hpp │ │ ├── FilterInterpolation.hpp │ │ ├── FilterPoseCommand.hpp │ │ ├── FilterVelCommand.hpp │ │ ├── IntegrateDOFVect.h │ │ ├── Polynomial.hpp │ │ ├── PoseCommandProcessing.hpp │ │ ├── Skew.h │ │ └── TrajectoryBangBangAcc.hpp ├── package.xml └── src │ ├── FilterPoseCommand.cpp │ ├── Polynomial.cpp │ ├── PoseCommandProcessing.cpp │ └── TrajectoryBangBangAcc.cpp ├── inria_model ├── CMakeLists.txt ├── include │ └── inria_model │ │ ├── Model.hpp │ │ ├── PinocchioInterface.hpp │ │ ├── RBDLRootUpdate.h │ │ ├── SEIKOController.hpp │ │ ├── SEIKORetargeting.hpp │ │ ├── SEIKOTalos.hpp │ │ ├── SEIKOWrapper.hpp │ │ └── TalosDOFs.h ├── package.xml └── src │ ├── Model.cpp │ ├── PinocchioInterface.cpp │ ├── RBDLRootUpdate.cpp │ ├── SEIKOController.cpp │ ├── SEIKORetargeting.cpp │ ├── SEIKOTalos.cpp │ └── SEIKOWrapper.cpp ├── inria_talos_description ├── CMakeLists.txt ├── LICENSE ├── meshes │ ├── arm │ │ ├── arm_1.STL │ │ ├── arm_1.glb │ │ ├── arm_1_collision.STL │ │ ├── arm_1_collision.glb │ │ ├── arm_2.STL │ │ ├── arm_2.glb │ │ ├── arm_2_collision.STL │ │ ├── arm_2_collision.glb │ │ ├── arm_3.STL │ │ ├── arm_3.glb │ │ ├── arm_3_collision.STL │ │ ├── arm_3_collision.glb │ │ ├── arm_4.STL │ │ ├── arm_4.glb │ │ ├── arm_4_collision.STL │ │ ├── arm_4_collision.glb │ │ ├── arm_5-v2.glb │ │ ├── arm_5.STL │ │ ├── arm_5.glb │ │ ├── arm_5_collision.STL │ │ ├── arm_5_collision.glb │ │ ├── arm_6.STL │ │ ├── arm_6.glb │ │ ├── arm_6_collision.STL │ │ ├── arm_6_collision.glb │ │ ├── arm_7.STL │ │ ├── arm_7.glb │ │ ├── arm_7_collision.STL │ │ └── arm_7_collision.glb │ ├── bart.STL │ ├── bart_light.STL │ ├── dummy_hand.STL │ ├── femur.STL │ ├── femur_light.STL │ ├── foot.STL │ ├── gripper │ │ ├── base_link.STL │ │ ├── base_link.glb │ │ ├── base_link_collision.STL │ │ ├── base_link_collision.glb │ │ ├── fingertip.STL │ │ ├── fingertip.glb │ │ ├── fingertip_collision.STL │ │ ├── fingertip_collision.glb │ │ ├── gripper_motor_double.STL │ │ ├── gripper_motor_double.glb │ │ ├── gripper_motor_double_collision.STL │ │ ├── gripper_motor_double_collision.glb │ │ ├── gripper_motor_single.STL │ │ ├── gripper_motor_single.glb │ │ ├── gripper_motor_single_collision.STL │ │ ├── gripper_motor_single_collision.glb │ │ ├── gripper_simple.STL │ │ ├── gripper_simple_collision.STL │ │ ├── gripper_tweezers.STL │ │ ├── gripper_tweezers_collision.STL │ │ ├── inner_double.STL │ │ ├── inner_double.glb │ │ ├── inner_double_collision.STL │ │ ├── inner_double_collision.glb │ │ ├── inner_single.STL │ │ ├── inner_single.glb │ │ ├── inner_single_collision.STL │ │ └── inner_single_collision.glb │ ├── head │ │ ├── head_1.glb │ │ ├── head_1.stl │ │ ├── head_1_collision.glb │ │ ├── head_1_collision.stl │ │ ├── head_2.glb │ │ ├── head_2_collision.glb │ │ ├── head_2_default.stl │ │ ├── head_2_default_collision.stl │ │ ├── head_2_lidar.stl │ │ └── head_2_lidar_collision.stl │ ├── hipZ.STL │ ├── hipZ_light.STL │ ├── sensors │ │ ├── orbbec │ │ │ ├── orbbec.STL │ │ │ └── orbbec.glb │ │ ├── ouster │ │ │ └── os1_64.dae │ │ └── xtion_pro_live │ │ │ ├── xtion_pro_live.dae │ │ │ ├── xtion_pro_live.glb │ │ │ └── xtion_pro_live.png │ ├── stump.STL │ ├── stump_reduced.STL │ ├── super-simple-bart.STL │ ├── super-simple-femur.STL │ ├── super-simple-tibia.STL │ ├── talos_dummy.stl │ ├── tibia.STL │ ├── tibia_light.STL │ ├── torso │ │ ├── base_link.STL │ │ ├── base_link.glb │ │ ├── base_link_collision.STL │ │ ├── base_link_collision.glb │ │ ├── torso_1.STL │ │ ├── torso_1.dae-backup │ │ ├── torso_1.glb │ │ ├── torso_2.STL │ │ ├── torso_2.dae-backup │ │ ├── torso_2.glb │ │ ├── torso_2_collision.STL │ │ ├── torso_2_collision.glb │ │ └── torso_dummy.glb │ └── v2 │ │ ├── ankle_X_collision.glb │ │ ├── ankle_X_collision.stl │ │ ├── ankle_X_lo_res.glb │ │ ├── ankle_X_lo_res.stl │ │ ├── ankle_Y_collision.glb │ │ ├── ankle_Y_collision.stl │ │ ├── ankle_Y_lo_res.glb │ │ ├── ankle_Y_lo_res.stl │ │ ├── base_link_collision.glb │ │ ├── base_link_collision.stl │ │ ├── base_link_lo_res.glb │ │ ├── base_link_lo_res.stl │ │ ├── hip_x_collision.glb │ │ ├── hip_x_collision.stl │ │ ├── hip_x_lo_res.glb │ │ ├── hip_x_lo_res.stl │ │ ├── hip_y_collision.glb │ │ ├── hip_y_collision.stl │ │ ├── hip_y_lo_res.glb │ │ ├── hip_y_lo_res.stl │ │ ├── hip_z_collision.glb │ │ ├── hip_z_collision.stl │ │ ├── hip_z_lo_res.glb │ │ ├── hip_z_lo_res.stl │ │ ├── knee_collision.glb │ │ ├── knee_collision.stl │ │ ├── knee_lo_res.glb │ │ └── knee_lo_res.stl ├── package.xml └── urdf │ └── talos_stump.urdf └── inria_utils ├── CMakeLists.txt ├── include └── inria_utils │ ├── BufferReaderWriter.hpp │ ├── CircularBuffer.hpp │ ├── Filesystem.h │ ├── Scheduling.h │ └── TransportValueUDP.hpp ├── package.xml └── src └── Filesystem.cpp /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2023, Quentin Rouxel 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SEIKO Controller:
Multi-Contact Whole-Body Force Control for Position-Controlled Robots 2 | 3 | **Website**: https://hucebot.github.io/seiko_controller_website \ 4 | **Video**: https://youtu.be/RGkZS57_6Nk \ 5 | **Paper**: https://ieeexplore.ieee.org/abstract/document/10517393 \ 6 | **ArXiv**: https://arxiv.org/abs/2312.16465 \ 7 | **HAL**: https://hal.univ-lorraine.fr/hal-04362547 \ 8 | **Authors**: Quentin Rouxel, [Serena Ivaldi](https://members.loria.fr/SIvaldi), [Jean-Baptiste Mouret](https://members.loria.fr/JBMouret) 9 | 10 | ![SEIKO Talos Multi-Contact](/assets/seiko_talos_multicontact.png) 11 | 12 | This repository contains the implementation of **SEIKO** (Sequential Equilibrium Inverse Kinematic Optimization) associated with the paper 13 | *Multi-Contact Whole-Body Force Control for Position-Controlled Robots*, developed at [INRIA Nancy](https://www.inria.fr/en/inria-centre-universite-lorraine), [Larsen Team](https://www.inria.fr/en/larsen), in 2023 ; and the paper 14 | *Multicontact Motion Retargeting Using Whole-Body Optimization of Full Kinematics and Sequential Force Equilibrium*, developed at the [University of Edinburgh](https://groups.inf.ed.ac.uk/advr/) in 2021. 15 | This is the code that was run on the [Talos humanoid robot](https://pal-robotics.com/robots/talos) during multi-contact experiments. 16 | This repository does NOT include the implementation of various tests, simulation environment, viewer and human-interface used to teleoperate the robot. 17 | 18 | ## Implementation Overview 19 | 20 | ![SEIKO Pipeline Architecture](/assets/architecture.png) 21 | 22 | * Implementation of [SEIKO Retargeting](https://ieeexplore.ieee.org/abstract/document/9728754) QP, Cartesian input commands processing and contact switch procedure: 23 | ``` 24 | inria_model/include/inria_model/SEIKORetargeting.hpp 25 | inria_model/src/SEIKORetargeting.cpp 26 | inria_model/include/inria_model/SEIKOWrapper.hpp 27 | inria_model/src/SEIKOWrapper.cpp 28 | ``` 29 | * Parameters of SEIKO Retargeting for Talos robot: 30 | ``` 31 | inria_model/include/inria_model/SEIKOTalos.hpp 32 | inria_model/src/SEIKOTalos.cpp 33 | ``` 34 | * Implementation of SEIKO Controller QP: 35 | ``` 36 | inria_model/include/inria_model/SEIKOController.hpp 37 | inria_model/src/SEIKOController.cpp 38 | ``` 39 | * Main controller pipeline thread running at 500 Hz: 40 | ``` 41 | inria_controller/include/inria_controller/TaskSEIKO.hpp 42 | inria_controller/src/TaskSEIKO.cpp 43 | ``` 44 | * [ROS_Control](http://wiki.ros.org/ros_control) lowlevel controller running in position-control mode at 2 kHz and performing joint position commands interpolation and safety checks: 45 | ``` 46 | inria_controller/include/inria_controller/ControllerBase.hpp 47 | inria_controller/src/ControllerBase.cpp 48 | inria_controller/include/inria_controller/ControllerTalos.hpp 49 | inria_controller/src/ControllerTalos.cpp 50 | ``` 51 | * Wrappers around *RBDL* and *Pinocchio* libraries to manipulate the robot's model: 52 | ``` 53 | inria_model/include/inria_model/Model.hpp 54 | inria_model/src/Model.cpp 55 | inria_model/include/inria_model/PinocchioInterface.hpp 56 | inria_model/src/PinocchioInterface.cpp 57 | ``` 58 | * URDF model of the Talos robot with 3d-printed ball-shaped right hand effector: 59 | ``` 60 | inria_talos_description/urdf/talos_stump.urdf 61 | ``` 62 | * Starting script to load and run the *ROS_Control* controller on the robot: 63 | ``` 64 | inria_controller/scripts/startTalos.sh 65 | ``` 66 | 67 | ## Dependencies 68 | 69 | This implementation is intended to be compiled with [catkin](http://wiki.ros.org/catkin) under [ROS Melodic](http://wiki.ros.org/melodic). 70 | It requires the following catkin packages: 71 | * **RBDL** (rigid body dynamics) \ 72 | https://gitlab.inria.fr/locolearn/rbdl_catkin 73 | * **Pinocchio** (rigid body dynamics for analytical derivatives) \ 74 | https://github.com/stack-of-tasks/pinocchio 75 | * **eiquadprog** (QP solver) \ 76 | https://github.com/stack-of-tasks/eiquadprog 77 | * **RhIO** (user and command interface, monitoring, configuration, logging) \ 78 | https://gitlab.inria.fr/locolearn/rhio 79 | * **ZMQ** (network transport for RhIO) \ 80 | https://gitlab.inria.fr/locolearn/libzmq_catkin 81 | 82 | ## Citation 83 | 84 | **[SEIKO Controller](https://ieeexplore.ieee.org/abstract/document/10517393):** 85 | ``` 86 | @article{rouxel2024multicontact, 87 | title={Multi-Contact Whole-Body Force Control for Position-Controlled Robots}, 88 | author={Rouxel, Quentin and Ivaldi, Serena and Mouret, Jean-Baptiste}, 89 | journal={IEEE Robotics and Automation Letters}, 90 | year={2024}, 91 | publisher={IEEE} 92 | } 93 | 94 | ``` 95 | **[SEIKO Retargeting](https://ieeexplore.ieee.org/abstract/document/9728754):** 96 | ``` 97 | @article{rouxel2022multicontact, 98 | title={Multicontact motion retargeting using whole-body optimization of full kinematics and sequential force equilibrium}, 99 | author={Rouxel, Quentin and Yuan, Kai and Wen, Ruoshi and Li, Zhibin}, 100 | journal={IEEE/ASME Transactions on Mechatronics}, 101 | volume={27}, 102 | number={5}, 103 | pages={4188--4198}, 104 | year={2022}, 105 | publisher={IEEE} 106 | } 107 | ``` 108 | 109 | ## License 110 | 111 | Licensed under the [BSD License](LICENSE) 112 | 113 | -------------------------------------------------------------------------------- /assets/architecture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/assets/architecture.png -------------------------------------------------------------------------------- /assets/seiko_talos_multicontact.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/assets/seiko_talos_multicontact.png -------------------------------------------------------------------------------- /inria_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.12) 2 | project(inria_controller) 3 | 4 | find_package(catkin REQUIRED 5 | controller_interface 6 | hardware_interface 7 | pluginlib 8 | realtime_tools 9 | urdf 10 | rhio_common 11 | rhio_server 12 | inria_utils 13 | inria_maths 14 | inria_model 15 | ) 16 | find_package(Eigen3 REQUIRED) 17 | find_package(pinocchio REQUIRED) 18 | 19 | catkin_package( 20 | INCLUDE_DIRS include 21 | LIBRARIES ${PROJECT_NAME} 22 | CATKIN_DEPENDS 23 | controller_interface 24 | hardware_interface 25 | pluginlib 26 | realtime_tools 27 | urdf 28 | rhio_common 29 | rhio_server 30 | inria_utils 31 | inria_maths 32 | inria_model 33 | ) 34 | 35 | include_directories( 36 | include 37 | ${catkin_INCLUDE_DIRS} 38 | ${EIGEN3_INCLUDE_DIR} 39 | ${pinocchio_INCLUDE_DIRS} 40 | ) 41 | 42 | add_compile_options(-std=c++17 -Wno-invalid-partial-specialization) 43 | 44 | add_library(${PROJECT_NAME} 45 | src/ControllerBase.cpp 46 | src/ControllerTalos.cpp 47 | src/TaskBase.cpp 48 | src/ThreadManager.cpp 49 | src/TaskSEIKO.cpp 50 | ) 51 | target_link_libraries(${PROJECT_NAME} 52 | ${catkin_LIBRARIES} 53 | ) 54 | 55 | install(TARGETS ${PROJECT_NAME} DESTINATION lib) 56 | install(DIRECTORY include/ DESTINATION include) 57 | install(FILES plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 58 | install(DIRECTORY config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config) 59 | install( 60 | FILES 61 | scripts/startTalos.sh 62 | scripts/stopTalos.sh 63 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 64 | PERMISSIONS OWNER_READ OWNER_WRITE OWNER_EXECUTE 65 | ) 66 | 67 | -------------------------------------------------------------------------------- /inria_controller/config/inria_controller.yaml: -------------------------------------------------------------------------------- 1 | inria_controller/ControllerTalos: 2 | type: "inria_controller/ControllerTalos" 3 | 4 | -------------------------------------------------------------------------------- /inria_controller/include/inria_controller/ControllerTalos.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_CONTROLLER_CONTROLLERTALOS_HPP 2 | #define INRIA_CONTROLLER_CONTROLLERTALOS_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace inria { 11 | 12 | /** 13 | * ControllerTalos 14 | * 15 | * ROS control real time controller 16 | * for Talos robot. 17 | */ 18 | class ControllerTalos : public ControllerBase< 19 | ControllerTalosState_t, ControllerTalosCommand_t> 20 | { 21 | public: 22 | 23 | /** 24 | * Specific Talos initialization 25 | */ 26 | ControllerTalos(); 27 | 28 | /** 29 | * Initialization from a non-realtime thread. 30 | * Manual initialization in order to claim and 31 | * access to all robot sensors 32 | * 33 | * @param hw The specific hardware interface 34 | * to the robot used by this controller. 35 | * @param root_nh A NodeHandle in the root of the controller 36 | * manager namespace. 37 | * This is where the ROS interfaces are setup 38 | * (publishers, subscribers, services). 39 | * @param controller_nh A NodeHandle in the namespace of 40 | * the controller. 41 | * This is where the controller-specific configuration resides. 42 | * @param[out] claimed_resources The resources 43 | * claimed by this controller. 44 | * @returns True if initialization was successful 45 | * and the controller is ready to be started. 46 | */ 47 | virtual bool initRequest( 48 | hardware_interface::RobotHW* robot_hw, 49 | ros::NodeHandle& root_nh, 50 | ros::NodeHandle& controller_nh, 51 | controller_interface::ControllerBase::ClaimedResources& 52 | claimed_resources) override; 53 | 54 | private: 55 | 56 | /** 57 | * Receiver for UDP value assignment forwarded to RhIO 58 | */ 59 | TransportValueUDPServer _serverUDP; 60 | 61 | /** 62 | * Model frame indexes for state estimation 63 | */ 64 | unsigned int _indexFrameInModelFootSoleLeft; 65 | unsigned int _indexFrameInModelFootSoleRight; 66 | unsigned int _indexFrameInModelFootFTLeft; 67 | unsigned int _indexFrameInModelFootFTRight; 68 | unsigned int _indexFrameInModelHandFrameLeft; 69 | unsigned int _indexFrameInModelHandFrameRight; 70 | unsigned int _indexFrameInModelHandFTLeft; 71 | unsigned int _indexFrameInModelHandFTRight; 72 | unsigned int _indexFrameInModelIMULink; 73 | unsigned int _indexFrameInModelTorsoLink; 74 | size_t _indexInContainerFTFootLeft; 75 | size_t _indexInContainerFTFootRight; 76 | size_t _indexInContainerFTHandLeft; 77 | size_t _indexInContainerFTHandRight; 78 | size_t _indexInContainerIMUBaseLink; 79 | 80 | /** 81 | * Offset biases and circular buffers for feet 82 | * force-torque and joint torque sensors calibration. 83 | * 4000 samples at 2kHz = 2 seconds. 84 | */ 85 | Eigen::Vector6d _biasWrenchFootLeft; 86 | Eigen::Vector6d _biasWrenchFootRight; 87 | Eigen::Vector6d _biasWrenchHandLeft; 88 | Eigen::Vector6d _biasWrenchHandRight; 89 | Eigen::VectorXd _biasTorqueJoint; 90 | CircularBuffer _bufferWrenchFootLeft; 91 | CircularBuffer _bufferWrenchFootRight; 92 | CircularBuffer _bufferWrenchHandLeft; 93 | CircularBuffer _bufferWrenchHandRight; 94 | CircularBuffer _bufferTorqueJoint; 95 | 96 | /** 97 | * RhIO wrapper for tare configuration and 98 | * wrench state monitoring 99 | */ 100 | RhIO::WrapperBool _rhioIsTransformFT; 101 | RhIO::WrapperBool _rhioAskTare; 102 | RhIO::WrapperBool _rhioAskTareFeet; 103 | RhIO::WrapperBool _rhioAskTareHands; 104 | RhIO::WrapperVect4d _rhioStateBaseQuat; 105 | RhIO::WrapperVect3d _rhioStateFootLeftTorque; 106 | RhIO::WrapperVect3d _rhioStateFootLeftForce; 107 | RhIO::WrapperVect2d _rhioStateFootLeftCoP; 108 | RhIO::WrapperVect2d _rhioStateFootLeftFriction; 109 | RhIO::WrapperVect3d _rhioStateFootRightForce; 110 | RhIO::WrapperVect3d _rhioStateFootRightTorque; 111 | RhIO::WrapperVect2d _rhioStateFootRightCoP; 112 | RhIO::WrapperVect2d _rhioStateFootRightFriction; 113 | RhIO::WrapperVect3d _rhioStateHandLeftTorque; 114 | RhIO::WrapperVect3d _rhioStateHandLeftForce; 115 | RhIO::WrapperVect3d _rhioStateHandRightTorque; 116 | RhIO::WrapperVect3d _rhioStateHandRightForce; 117 | 118 | /** 119 | * Indexes for gripper hardware resource 120 | */ 121 | size_t _indexInContainerJointPosGripperLeft; 122 | size_t _indexInContainerJointPosGripperRight; 123 | 124 | /** 125 | * RhIO wrapper and filter for target 126 | * gripper closing position ratio 127 | */ 128 | RhIO::WrapperFloat _rhioGripperLeftPosRatio; 129 | RhIO::WrapperFloat _rhioGripperRightPosRatio; 130 | RhIO::WrapperFloat _rhioGripperLeftCurrentRatio; 131 | RhIO::WrapperFloat _rhioGripperRightCurrentRatio; 132 | FilterBangBangAcc _filterPosGripperLeft; 133 | FilterBangBangAcc _filterPosGripperRight; 134 | 135 | /** 136 | * Overload floating base state estimation and 137 | * state estimation for feet wrench estimation 138 | */ 139 | virtual void updateAssignTaskState(double time, double dt) override; 140 | 141 | /** 142 | * Overload base command processing 143 | */ 144 | virtual void updateCommandInterpolation(double time, double dt) override; 145 | }; 146 | 147 | } 148 | 149 | #endif 150 | 151 | -------------------------------------------------------------------------------- /inria_controller/include/inria_controller/RhIOEigen.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_CONTROLLER_RHIOEIGEN_HPP 2 | #define INRIA_CONTROLLER_RHIOEIGEN_HPP 3 | 4 | #include 5 | 6 | /** 7 | * Eigen specialized extension for RhIO wrapper 8 | */ 9 | namespace RhIO { 10 | 11 | template <> 12 | inline void WrapperVect4d::operator=(const Eigen::Quaterniond& quat) 13 | { 14 | set(0, quat.x()); 15 | set(1, quat.y()); 16 | set(2, quat.z()); 17 | set(3, quat.w()); 18 | } 19 | 20 | template <> 21 | inline WrapperVect2d::operator Eigen::Vector2d() const 22 | { 23 | Eigen::Vector2d vect; 24 | vect.x() = get(0); 25 | vect.y() = get(1); 26 | return vect; 27 | } 28 | 29 | template <> 30 | inline WrapperVect3d::operator Eigen::Vector3d() const 31 | { 32 | Eigen::Vector3d vect; 33 | vect.x() = get(0); 34 | vect.y() = get(1); 35 | vect.z() = get(2); 36 | return vect; 37 | } 38 | 39 | template <> 40 | inline WrapperVect4d::operator Eigen::Quaterniond() const 41 | { 42 | Eigen::Quaterniond quat; 43 | quat.x() = get(0); 44 | quat.y() = get(1); 45 | quat.z() = get(2); 46 | quat.w() = get(3); 47 | return quat; 48 | } 49 | 50 | } 51 | 52 | #endif 53 | 54 | -------------------------------------------------------------------------------- /inria_controller/include/inria_controller/TaskBase.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_CONTROLLER_TASKBASE_HPP 2 | #define INRIA_CONTROLLER_TASKBASE_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace inria { 14 | 15 | /** 16 | * State structure for communication from RT controller 17 | * to non RT tasks. 18 | * The joint order is defined by the URDF model used. 19 | */ 20 | struct ControllerBaseState_t { 21 | //If false, this state structure is not valid 22 | bool isValid; 23 | //Time at RT controller estimation (in ROS time) 24 | double time; 25 | //Is the task running 26 | bool isRunning; 27 | //Measured joint position, velocity 28 | //and torque in model order 29 | Eigen::VectorXd jointPos; 30 | Eigen::VectorXd jointVel; 31 | Eigen::VectorXd jointTau; 32 | //Send joint position command 33 | Eigen::VectorXd jointCmd; 34 | 35 | /** 36 | * Constructor with memory allocation. 37 | * @param sizeJoint Number of joint in model. 38 | */ 39 | ControllerBaseState_t(size_t sizeJoint) : 40 | isValid(false), 41 | time(0.0), 42 | isRunning(false), 43 | jointPos(Eigen::VectorXd::Zero(sizeJoint)), 44 | jointVel(Eigen::VectorXd::Zero(sizeJoint)), 45 | jointTau(Eigen::VectorXd::Zero(sizeJoint)), 46 | jointCmd(Eigen::VectorXd::Zero(sizeJoint)) 47 | { 48 | reset(); 49 | } 50 | 51 | /** 52 | * Reset to invalid 53 | */ 54 | void reset() 55 | { 56 | isValid = false; 57 | time = 0.0; 58 | isRunning = false; 59 | jointPos.setZero(); 60 | jointVel.setZero(); 61 | jointTau.setZero(); 62 | jointCmd.setZero(); 63 | } 64 | }; 65 | 66 | /** 67 | * Command structure for communication from non RT 68 | * tasks to RT controller. 69 | * The joint order is defined by the URDF model used. 70 | */ 71 | struct ControllerBaseCommand_t { 72 | //Time associated to RT controller state timestamp 73 | //used to compute this command (in ROS time) 74 | double timeState; 75 | //Future time (after timeState) for which 76 | //this command is computed (in ROS time) 77 | double timeCmd; 78 | //Position command for each joint in model order 79 | Eigen::VectorXd jointCmd; 80 | //Boolean indicating if associated joint command is valid 81 | Eigen::VectorXi jointIsUsed; 82 | 83 | /** 84 | * Constructor with memory allocation. 85 | * @param sizeJoint Number of joint in model. 86 | */ 87 | ControllerBaseCommand_t(size_t sizeJoint) : 88 | timeState(0.0), 89 | timeCmd(0.0), 90 | jointCmd(Eigen::VectorXd::Zero(sizeJoint)), 91 | jointIsUsed(Eigen::VectorXi::Zero(sizeJoint)) 92 | { 93 | } 94 | 95 | /** 96 | * Reset to invalid 97 | */ 98 | void reset() 99 | { 100 | timeState = 0.0; 101 | timeCmd = 0.0; 102 | jointCmd.setZero(); 103 | jointIsUsed.setZero(); 104 | } 105 | }; 106 | 107 | template 108 | class ThreadManager; 109 | 110 | /** 111 | * TaskBase 112 | * 113 | * Base class for control tasks running 114 | * in a non RT thread. 115 | * @param T_State Type for controller state data. 116 | * @param T_Command Type for controller command data; 117 | */ 118 | template 119 | class TaskBase 120 | { 121 | public: 122 | 123 | /** 124 | * Empty initialization 125 | */ 126 | TaskBase(); 127 | 128 | /** 129 | * Virtual destructor 130 | */ 131 | virtual ~TaskBase(); 132 | 133 | /** 134 | * Return task name 135 | */ 136 | std::string getName() const; 137 | 138 | /** 139 | * Return task scheduling frequency 140 | */ 141 | double getFrequency() const; 142 | 143 | /** 144 | * Initialize the task with the 145 | * declared model 146 | */ 147 | void doInit(const Model& model); 148 | 149 | /** 150 | * Run one update step of the task 151 | */ 152 | void doUpdate(); 153 | 154 | /** 155 | * Lock free RT interface to write 156 | * state from controller to task and 157 | * read command from task to controller. 158 | */ 159 | T_State& refState(); 160 | void pushState(); 161 | void pullCommand(); 162 | const T_Command& refCommand() const; 163 | 164 | protected: 165 | 166 | /** 167 | * Task name. 168 | * 169 | * @return a unique constant 170 | * string name for the task. 171 | */ 172 | virtual const char* name() const = 0; 173 | 174 | /** 175 | * Task defined task scheduling frequency 176 | * (by thread manager) 177 | * 178 | * @return desire frequency in Hertz 179 | */ 180 | virtual double schedulingFrequency() const = 0; 181 | 182 | /** 183 | * Task defined prediction ahead time step 184 | * for lowlevel interpolation. 185 | * 186 | * @return time step in seconds. 187 | */ 188 | virtual double aheadTimeStep() const = 0; 189 | 190 | /** 191 | * Task defined no RT task initialization 192 | * run at controller initialization. 193 | * 194 | * @param rhioNode Task specific RhIO node. 195 | * @param pathModel Path to used URDF model. 196 | */ 197 | virtual void init( 198 | RhIO::IONode& rhioNode, 199 | const std::string& pathModel) = 0; 200 | 201 | /** 202 | * Task defined RT update step. 203 | * 204 | * @param dt_task Duration since last call in second. 205 | * @param dt_ahead Time step after state time for which to 206 | * compute the command in second. 207 | * @param state Last (const) estimated measured state 208 | * from RT controller. 209 | * @param command Joint command structure to be send 210 | * to RT controller. 211 | */ 212 | virtual void update( 213 | double dt_task, 214 | double dt_ahead, 215 | const T_State& state, 216 | T_Command& command) = 0; 217 | 218 | private: 219 | 220 | /** 221 | * Lock free buffers for state and command communication 222 | * between RT controller and the non RT task 223 | */ 224 | std::unique_ptr> 225 | _bufferState; 226 | std::unique_ptr> 227 | _bufferCommand; 228 | 229 | /** 230 | * RhIO node associated to the task 231 | */ 232 | RhIO::IONode* _rhioNode; 233 | 234 | /** 235 | * Task computation time and period statistics 236 | */ 237 | RhIO::WrapperFloat _rhioTimingDuration; 238 | RhIO::WrapperFloat _rhioTimingPeriodTask; 239 | RhIO::WrapperFloat _rhioTimingPeriodReal; 240 | double _timeState; 241 | std::chrono::time_point _timeUpdate; 242 | bool _isTimeInit; 243 | }; 244 | 245 | } 246 | 247 | #endif 248 | 249 | -------------------------------------------------------------------------------- /inria_controller/include/inria_controller/TaskBaseTalos.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_CONTROLLER_TASKBASETALOS_HPP 2 | #define INRIA_CONTROLLER_TASKBASETALOS_HPP 3 | 4 | #include 5 | 6 | namespace inria { 7 | 8 | /** 9 | * TaskBaseTalos 10 | * 11 | * Specialized State, Command structures and 12 | * TaskBase class for Talos robot. 13 | */ 14 | struct ControllerTalosState_t : public ControllerBaseState_t { 15 | //Estimated floating base orientation 16 | Eigen::Quaterniond quatBase; 17 | //Measured and estimated feet wrenches 18 | Eigen::Vector6d wrenchFootLeft; 19 | Eigen::Vector6d wrenchFootRight; 20 | //Measured and estimated hand wrenches 21 | Eigen::Vector6d wrenchHandLeft; 22 | Eigen::Vector6d wrenchHandRight; 23 | //Measured torso IMU orientation 24 | Eigen::Quaterniond quatTorsoIMU; 25 | 26 | /** 27 | * Import Base constructor 28 | */ 29 | using ControllerBaseState_t::ControllerBaseState_t; 30 | 31 | /** 32 | * Reset overload 33 | */ 34 | void reset() 35 | { 36 | this->ControllerBaseState_t::reset(); 37 | quatBase.setIdentity(); 38 | wrenchFootLeft.setZero(); 39 | wrenchFootRight.setZero(); 40 | wrenchHandLeft.setZero(); 41 | wrenchHandRight.setZero(); 42 | quatTorsoIMU.setIdentity(); 43 | } 44 | }; 45 | struct ControllerTalosCommand_t : public ControllerBaseCommand_t { 46 | using ControllerBaseCommand_t::ControllerBaseCommand_t; 47 | }; 48 | class TaskBaseTalos : public TaskBase {}; 49 | 50 | } 51 | 52 | #endif 53 | 54 | -------------------------------------------------------------------------------- /inria_controller/include/inria_controller/TaskSEIKO.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_CONTROLLER_TASKSEIKO_HPP 2 | #define INRIA_CONTROLLER_TASKSEIKO_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace inria { 16 | 17 | /** 18 | * TaskSEIKO 19 | * 20 | * Whole body position control using SEIKO 21 | * for Talos robot and whole body admittance 22 | * stabilization controller using flexiblity model 23 | */ 24 | class TaskSEIKO : public TaskBaseTalos 25 | { 26 | protected: 27 | 28 | /** 29 | * Task name defining the task. 30 | */ 31 | virtual const char* name() const override; 32 | 33 | /** 34 | * Task defined task scheduling frequency 35 | * (by thread manager) 36 | * 37 | * @return desire frequency in Hertz 38 | */ 39 | virtual double schedulingFrequency() const override; 40 | 41 | /** 42 | * Task defined prediction ahead time step 43 | * for lowlevel interpolation. 44 | */ 45 | virtual double aheadTimeStep() const override; 46 | 47 | /** 48 | * Task defined no RT task initialization 49 | * run at controller initialization. 50 | */ 51 | virtual void init( 52 | RhIO::IONode& rhioNode, 53 | const std::string& pathModel) override; 54 | 55 | /** 56 | * Task defined RT update step. 57 | */ 58 | virtual void update( 59 | double dt_task, 60 | double dt_ahead, 61 | const ControllerTalosState_t& state, 62 | ControllerTalosCommand_t& command) override; 63 | 64 | public: 65 | 66 | /** 67 | * Structure for exposing SEIKO end-effectors 68 | * command and monitoring to RhIO 69 | */ 70 | struct Effector_t { 71 | //Effector contact type 72 | bool isPoint; 73 | //Command inputs 74 | RhIO::WrapperBool askSwitchEnable; 75 | RhIO::WrapperBool askSwitchDisable; 76 | RhIO::WrapperBool cmdIsPushMode; 77 | RhIO::WrapperFloat cmdVelForceNormal; 78 | RhIO::WrapperBool cmdIsClutch; 79 | RhIO::WrapperVect3d cmdRawPos; 80 | RhIO::WrapperVect4d cmdRawQuat; 81 | RhIO::WrapperVect3d cmdVelLin; 82 | RhIO::WrapperVect3d cmdVelAng; 83 | //Velocity admittance applied 84 | RhIO::WrapperVect3d admVelLin; 85 | RhIO::WrapperVect3d admVelAng; 86 | //Monitoring SEIKO state 87 | RhIO::WrapperBool isEnabled; 88 | RhIO::WrapperVect3d targetPos; 89 | RhIO::WrapperVect3d goalPos; 90 | RhIO::WrapperVect4d contactQuat; 91 | //Monitoring controller state 92 | RhIO::WrapperVect3d deltaForce; 93 | RhIO::WrapperVect3d deltaTorque; 94 | RhIO::WrapperVect3d goalForce; 95 | RhIO::WrapperVect3d goalTorque; 96 | RhIO::WrapperVect3d readForce; 97 | RhIO::WrapperVect3d readTorque; 98 | RhIO::WrapperVect3d filteredForce; 99 | RhIO::WrapperVect3d filteredTorque; 100 | RhIO::WrapperVect3d velForce; 101 | RhIO::WrapperVect3d velTorque; 102 | RhIO::WrapperVect3d errorForce; 103 | RhIO::WrapperVect3d errorTorque; 104 | RhIO::WrapperVect3d effortForce; 105 | RhIO::WrapperVect3d effortTorque; 106 | RhIO::WrapperVect3d solForce; 107 | RhIO::WrapperVect3d solTorque; 108 | RhIO::WrapperVect3d stateForce; 109 | RhIO::WrapperVect3d stateTorque; 110 | }; 111 | 112 | /** 113 | * Integrated goal and measured models 114 | */ 115 | Model _modelGoal; 116 | Model _modelRead; 117 | 118 | /** 119 | * SEIKO Retargeting for Talos robot 120 | */ 121 | SEIKOTalos _retargeting; 122 | 123 | /** 124 | * Whole body feet and hands wrench estimation 125 | * from FT and joint torque sensors and velocity 126 | * differentiation filter 127 | */ 128 | FilterDifferentiator2 _filterVelFootLeft; 129 | FilterDifferentiator2 _filterVelFootRight; 130 | FilterDifferentiator2 _filterVelHandLeft; 131 | FilterDifferentiator2 _filterVelHandRight; 132 | 133 | /** 134 | * Whole body admittance controller with 135 | * flexibility model 136 | */ 137 | SEIKOController _controller; 138 | 139 | /** 140 | * Map container for RhIO end-effector wrappers 141 | */ 142 | std::map _effectors; 143 | 144 | /** 145 | * State and parameters for measured wrench 146 | * and joint torque complimentary filtering 147 | */ 148 | bool _filteredComplimentaryIsInit; 149 | Eigen::Vector6d _lastDeltaWrenchLeft; 150 | Eigen::Vector6d _lastDeltaWrenchRight; 151 | Eigen::Vector3d _lastDeltaForceLeft; 152 | Eigen::Vector3d _lastDeltaForceRight; 153 | Eigen::Vector6d _filteredWrenchLeft; 154 | Eigen::Vector6d _filteredWrenchRight; 155 | Eigen::Vector3d _filteredForceLeft; 156 | Eigen::Vector3d _filteredForceRight; 157 | RhIO::WrapperFloat _rhioParamCutoffFreqComplimentary; 158 | 159 | /** 160 | * Controller parameters 161 | */ 162 | RhIO::WrapperBool _rhioParamControllerEnabled; 163 | RhIO::WrapperFloat _rhioParamGainP; 164 | RhIO::WrapperFloat _rhioParamGainD; 165 | RhIO::WrapperFloat _rhioParamMaxTauRatio; 166 | RhIO::WrapperFloat _rhioParamMaxPosCmdOffset; 167 | 168 | /** 169 | * Effector velocity admittance parameters 170 | */ 171 | RhIO::WrapperBool _rhioParamAdmEnabled; 172 | RhIO::WrapperFloat _rhioParamAdmPointLinDeadbband; 173 | RhIO::WrapperFloat _rhioParamAdmPointLinGain; 174 | RhIO::WrapperFloat _rhioParamAdmPointLinMax; 175 | RhIO::WrapperFloat _rhioParamAdmPlaneLinDeadbband; 176 | RhIO::WrapperFloat _rhioParamAdmPlaneLinGain; 177 | RhIO::WrapperFloat _rhioParamAdmPlaneLinMax; 178 | RhIO::WrapperFloat _rhioParamAdmPlaneAngDeadbband; 179 | RhIO::WrapperFloat _rhioParamAdmPlaneAngGain; 180 | RhIO::WrapperFloat _rhioParamAdmPlaneAngMax; 181 | 182 | /** 183 | * Additional monitoring 184 | */ 185 | RhIO::WrapperBool _rhioIsExperiment; 186 | RhIO::WrapperFloat _rhioDurationRetargeting; 187 | RhIO::WrapperFloat _rhioDurationController; 188 | RhIO::WrapperArray _rhioGoalModel; 189 | RhIO::WrapperArray _rhioReadModel; 190 | RhIO::WrapperArray _rhioFlexModel; 191 | RhIO::WrapperArray _rhioCmdOffset; 192 | RhIO::WrapperArray _rhioStiffnessRatio; 193 | RhIO::WrapperArray _rhioJointTauGoalRatio; 194 | RhIO::WrapperArray _rhioJointTauReadRatio; 195 | RhIO::WrapperArray _rhioJointTauLimitRatio; 196 | 197 | /** 198 | * Final command interpolation filter 199 | */ 200 | FilterInterpolation _filterInterpolation; 201 | }; 202 | 203 | } 204 | 205 | #endif 206 | 207 | -------------------------------------------------------------------------------- /inria_controller/include/inria_controller/ThreadManager.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_CONTROLLER_THREADMANAGER_HPP 2 | #define INRIA_CONTROLLER_THREADMANAGER_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace inria { 14 | 15 | /** 16 | * ThreadManager 17 | * 18 | * Class creating and 19 | * managing non real-time thread 20 | * within RT controller process. 21 | * @param T_State Type for controller state data. 22 | * @param T_Command Type for controller command data; 23 | */ 24 | template 25 | class ThreadManager 26 | { 27 | public: 28 | 29 | /** 30 | * Empty initialization 31 | */ 32 | ThreadManager(); 33 | 34 | /** 35 | * Stop thread 36 | * if still running 37 | */ 38 | ~ThreadManager(); 39 | 40 | /** 41 | * Copy and assignment are disabled 42 | * to prevent memory management issues 43 | */ 44 | ThreadManager(const ThreadManager&) = delete; 45 | ThreadManager& operator=(const ThreadManager&) = delete; 46 | 47 | /** 48 | * Initialization of non RT thread 49 | * 50 | * @param model Model instance used. 51 | * @param tasks Container of allocated TaskBase* 52 | * to be initialized and managed. 53 | */ 54 | void initThread( 55 | const Model& model, 56 | const std::vector*>& tasks); 57 | 58 | /** 59 | * Start non RT thread 60 | */ 61 | void startThread(); 62 | 63 | /** 64 | * Ask non RT thread to stop 65 | */ 66 | void stopThread(); 67 | 68 | /** 69 | * If true, error has occurred in task and 70 | * the controller must be fully stopped 71 | */ 72 | bool isError() const; 73 | 74 | /** 75 | * Lock free RT interface to write state from controller 76 | * to task and read command from task to controller. 77 | * WARNING: isTask() must be true before calling 78 | * the other task getters. 79 | * NOTE: refState() is always valid even if no task 80 | * is currently running. 81 | */ 82 | bool isTask() const; 83 | std::string nameTask() const; 84 | T_State& refState(); 85 | void pushState(); 86 | void pullCommand(); 87 | const T_Command& refCommand() const; 88 | 89 | private: 90 | 91 | /** 92 | * Container for polymorphic allocated 93 | * tasks instance 94 | */ 95 | std::vector*> _containerTasks; 96 | 97 | /** 98 | * Default state structure when no task is selected 99 | */ 100 | std::unique_ptr _defaultState; 101 | 102 | /** 103 | * System non RT thread 104 | */ 105 | std::thread* _thread; 106 | 107 | /** 108 | * Currently enabled task instance 109 | * (stored in _containerTasks) or nullptr 110 | */ 111 | std::atomic*> _task; 112 | 113 | /** 114 | * RhIO wrapper to controller state 115 | */ 116 | RhIO::WrapperInt _rhioState; 117 | 118 | /** 119 | * If false, non RT thread should stop 120 | */ 121 | std::atomic _isRunning; 122 | 123 | /** 124 | * True if an error has occurred 125 | * in one running threads. 126 | * Static to be accessed in signalHandler. 127 | */ 128 | static std::atomic _isError; 129 | 130 | /** 131 | * Non RT thread main function 132 | */ 133 | void threadMain(); 134 | 135 | /** 136 | * Configure signal catching 137 | */ 138 | void setupSignals(); 139 | 140 | /** 141 | * Catch segmentation fault and 142 | * FPE signal error to be able to 143 | * trigger the emergency mode 144 | */ 145 | static void signalHandler( 146 | int sig, siginfo_t* siginfo, void* context); 147 | }; 148 | 149 | } 150 | 151 | #endif 152 | 153 | -------------------------------------------------------------------------------- /inria_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | inria_controller 4 | 0.0.0 5 | 6 | Main ROS control real time controller 7 | for the Talos robot at INRIA Nancy 8 | 9 | Quentin Rouxel 10 | BSD 11 | 12 | catkin 13 | controller_interface 14 | hardware_interface 15 | pluginlib 16 | realtime_tools 17 | urdf 18 | rhio_common 19 | rhio_server 20 | inria_utils 21 | inria_maths 22 | inria_model 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /inria_controller/plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Position controller for Talos robot 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /inria_controller/scripts/startTalos.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #Configure network 4 | if [ "$HOSTNAME" = talos-5c ]; then 5 | sudo sync 6 | sudo ip route add 224.0.0.1 dev eth0 7 | fi 8 | 9 | #Stop default Pal controllers 10 | echo "Stopping default PAL controllers" 11 | rosrun controller_manager controller_manager stop torso_current_limit_controller > /dev/null 12 | rosrun controller_manager controller_manager stop left_leg_current_limit_controller > /dev/null 13 | rosrun controller_manager controller_manager stop right_leg_current_limit_controller > /dev/null 14 | rosrun controller_manager controller_manager stop head_current_limit_controller > /dev/null 15 | rosrun controller_manager controller_manager stop left_gripper_current_limit_controller > /dev/null 16 | rosrun controller_manager controller_manager stop right_gripper_current_limit_controller > /dev/null 17 | rosrun controller_manager controller_manager stop left_arm_current_limit_controller > /dev/null 18 | rosrun controller_manager controller_manager stop right_arm_current_limit_controller > /dev/null 19 | 20 | #Disable PAL statistic on real hardware 21 | if [ "$HOSTNAME" = talos-5c ]; then 22 | pal-stop statsdcc 23 | fi 24 | 25 | #Load plugin config 26 | rosparam load `rospack find inria_controller`/config/inria_controller.yaml 27 | sleep 1 28 | 29 | #Load controller 30 | rosrun controller_manager controller_manager load inria_controller/ControllerTalos 31 | sleep 1 32 | 33 | #Start controller 34 | rosrun controller_manager controller_manager start inria_controller/ControllerTalos 35 | sleep 1 36 | 37 | #Enabled RT thread CPU isolation on real hardware 38 | if [ "$HOSTNAME" = talos-5c ]; then 39 | sudo cset shield --reset 40 | sudo cset shield --cpu=0,2 41 | LWP1=`ps -eLo lwp,comm | grep inria_control | sed 's/^ *//g' | cut -d " " -f1` 42 | LWP2=`ps -eLo lwp,comm | grep inria_task | sed 's/^ *//g' | cut -d " " -f1` 43 | sudo cset shield --shield --pid $LWP1 44 | sudo cset shield --shield --pid $LWP2 45 | fi 46 | 47 | -------------------------------------------------------------------------------- /inria_controller/scripts/stopTalos.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #Stop controller 4 | rosrun controller_manager controller_manager stop inria_controller/ControllerTalos 5 | sleep 1 6 | 7 | #Unload controller 8 | rosrun controller_manager controller_manager unload inria_controller/ControllerTalos 9 | 10 | -------------------------------------------------------------------------------- /inria_controller/src/TaskBase.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace inria { 5 | 6 | template 7 | TaskBase::TaskBase() : 8 | _bufferState(), 9 | _bufferCommand(), 10 | _rhioNode(nullptr), 11 | _rhioTimingDuration(), 12 | _rhioTimingPeriodTask(), 13 | _rhioTimingPeriodReal(), 14 | _timeState(0.0), 15 | _timeUpdate(), 16 | _isTimeInit(false) 17 | { 18 | } 19 | 20 | template 21 | TaskBase::~TaskBase() 22 | { 23 | } 24 | 25 | template 26 | std::string TaskBase::getName() const 27 | { 28 | return std::string(this->name()); 29 | } 30 | 31 | template 32 | double TaskBase::getFrequency() const 33 | { 34 | return this->schedulingFrequency(); 35 | } 36 | 37 | template 38 | void TaskBase::doInit(const Model& model) 39 | { 40 | //Allocate lock free buffers 41 | size_t sizeJoint = model.sizeJoint(); 42 | _bufferState = std::make_unique< 43 | BufferReaderWriter>(sizeJoint); 44 | _bufferCommand = std::make_unique< 45 | BufferReaderWriter>(sizeJoint); 46 | //Model configuration 47 | std::string pathModel = model.getPathURDF(); 48 | //RhIO configuration 49 | RhIO::Root.newChild("/tasks/" + std::string(this->name())); 50 | _rhioNode = &RhIO::Root.child("/tasks/" + std::string(this->name())); 51 | _rhioTimingDuration.create(*_rhioNode, "timing_duration") 52 | ->comment("Task computation time") 53 | ->defaultValue(0.0); 54 | _rhioTimingPeriodTask.create(*_rhioNode, "timing_period_task") 55 | ->comment("Task update elapsed time") 56 | ->defaultValue(0.0); 57 | _rhioTimingPeriodReal.create(*_rhioNode, "timing_period_real") 58 | ->comment("Task update computed period time") 59 | ->defaultValue(0.0); 60 | //Call specific task initialization 61 | this->init(*_rhioNode, pathModel); 62 | _isTimeInit = false; 63 | } 64 | 65 | template 66 | void TaskBase::doUpdate() 67 | { 68 | std::chrono::time_point timeStart = 69 | std::chrono::steady_clock::now(); 70 | //Retrieve state from controller 71 | _bufferState->getFromReader()->reset(); 72 | _bufferState->pullFromReader(); 73 | const T_State& state = *(_bufferState->getFromReader()); 74 | //Abord update of the latest state is not valid 75 | if (!state.isValid) { 76 | return; 77 | } 78 | //Reset command structure 79 | T_Command& command = *(_bufferCommand->getFromWriter()); 80 | command.reset(); 81 | //Get state timing 82 | if (!_isTimeInit) { 83 | _timeState = state.time; 84 | _timeUpdate = std::chrono::steady_clock::now(); 85 | _isTimeInit = true; 86 | } 87 | double dt_task = state.time - _timeState; 88 | if (dt_task < 0.0) { 89 | throw std::logic_error( 90 | "inria::TaskBase::doUpdate: " 91 | "Negative state time step: " 92 | + std::to_string(dt_task)); 93 | } 94 | //Call specific task update 95 | double dt_ahead = this->aheadTimeStep(); 96 | if (dt_ahead < 0.0) { 97 | throw std::logic_error( 98 | "inria::TaskBase::doUpdate: " 99 | "Negative ahead time step: " 100 | + std::to_string(dt_task)); 101 | } 102 | this->update(dt_task, dt_ahead, state, command); 103 | //Send command structure to RT controller 104 | command.timeState = state.time; 105 | command.timeCmd = state.time + dt_ahead; 106 | _bufferCommand->pushFromWriter(); 107 | //Timing statistics 108 | std::chrono::time_point timeEnd = 109 | std::chrono::steady_clock::now(); 110 | std::chrono::duration duration2 = 111 | timeEnd - timeStart; 112 | std::chrono::duration duration3 = 113 | timeStart - _timeUpdate; 114 | _rhioTimingDuration = duration2.count(); 115 | _rhioTimingPeriodTask = dt_task; 116 | _rhioTimingPeriodReal = duration3.count(); 117 | _timeState = state.time; 118 | _timeUpdate = timeStart; 119 | } 120 | 121 | template 122 | T_State& TaskBase::refState() 123 | { 124 | return *_bufferState->getFromWriter(); 125 | } 126 | template 127 | void TaskBase::pushState() 128 | { 129 | _bufferState->pushFromWriter(); 130 | } 131 | template 132 | void TaskBase::pullCommand() 133 | { 134 | _bufferCommand->getFromReader()->reset(); 135 | _bufferCommand->pullFromReader(); 136 | } 137 | template 138 | const T_Command& TaskBase::refCommand() const 139 | { 140 | return *_bufferCommand->getFromReader(); 141 | } 142 | 143 | //Explicit template instantiation for robot specialisations 144 | template class TaskBase; 145 | 146 | } 147 | 148 | -------------------------------------------------------------------------------- /inria_maths/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.12) 2 | project(inria_maths) 3 | 4 | find_package(catkin REQUIRED 5 | inria_utils 6 | ) 7 | find_package(Eigen3 REQUIRED) 8 | 9 | catkin_package( 10 | INCLUDE_DIRS include 11 | LIBRARIES ${PROJECT_NAME} 12 | CATKIN_DEPENDS 13 | inria_utils 14 | ) 15 | 16 | include_directories( 17 | include 18 | ${catkin_INCLUDE_DIRS} 19 | ${EIGEN3_INCLUDE_DIR} 20 | ) 21 | 22 | add_compile_options(-std=c++17) 23 | 24 | add_library(${PROJECT_NAME} 25 | src/Polynomial.cpp 26 | src/TrajectoryBangBangAcc.cpp 27 | src/FilterPoseCommand.cpp 28 | src/PoseCommandProcessing.cpp 29 | ) 30 | target_link_libraries(${PROJECT_NAME} 31 | ${catkin_LIBRARIES} 32 | ) 33 | 34 | install(TARGETS ${PROJECT_NAME} DESTINATION lib) 35 | install(DIRECTORY include/ DESTINATION include) 36 | 37 | -------------------------------------------------------------------------------- /inria_maths/include/inria_maths/Angle.h: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_MATHS_ANGLE_H 2 | #define INRIA_MATHS_ANGLE_H 3 | 4 | #include 5 | 6 | namespace inria { 7 | 8 | /** 9 | * Angle degrees to radians and 10 | * radians to degrees conversion 11 | */ 12 | constexpr double RadToDeg(double angle) 13 | { 14 | return angle*180.0/M_PI; 15 | } 16 | constexpr double DegToRad(double angle) 17 | { 18 | return angle*M_PI/180.0; 19 | } 20 | 21 | /** 22 | * Angle range normalization. 23 | * 24 | * @param angle in radian 25 | * @return given angle in radian 26 | * bounded between -PI and PI 27 | */ 28 | inline double AngleBound(double angle) 29 | { 30 | return 31 | angle 32 | - 2.0*M_PI*std::floor((angle + M_PI)/(2.0*M_PI)); 33 | } 34 | 35 | /** 36 | * Compute angular distance 37 | * @param angleSrc angle source in radian 38 | * @param angleDst angle destination in radian 39 | * @return oriented distance from angleSrc to angleDst 40 | * within -PI/2:PI/2 radian 41 | */ 42 | inline double AngleDistance(double angleSrc, double angleDst) 43 | { 44 | angleSrc = AngleBound(angleSrc); 45 | angleDst = AngleBound(angleDst); 46 | 47 | double max, min; 48 | if (angleSrc > angleDst) { 49 | max = angleSrc; 50 | min = angleDst; 51 | } else { 52 | max = angleDst; 53 | min = angleSrc; 54 | } 55 | 56 | double dist1 = max-min; 57 | double dist2 = 2.0*M_PI - max + min; 58 | 59 | if (dist1 < dist2) { 60 | if (angleSrc > angleDst) { 61 | return -dist1; 62 | } else { 63 | return dist1; 64 | } 65 | } else { 66 | if (angleSrc > angleDst) { 67 | return dist2; 68 | } else { 69 | return -dist2; 70 | } 71 | } 72 | } 73 | 74 | /** 75 | * Compute a weighted average 76 | * between the two given angles in radian. 77 | * 78 | * @param weight1 79 | * @param angle1 in radian 80 | * @param weight2 81 | * @param angle2 in radian 82 | * @return averaged angle in -PI:PI. 83 | */ 84 | inline double AngleWeightedAverage( 85 | double weight1, double angle1, double weight2, double angle2) 86 | { 87 | double x1 = std::cos(angle1); 88 | double y1 = std::sin(angle1); 89 | double x2 = std::cos(angle2); 90 | double y2 = std::sin(angle2); 91 | 92 | double meanX = weight1*x1 + weight2*x2; 93 | double meanY = weight1*y1 + weight2*y2; 94 | 95 | return std::atan2(meanY, meanX); 96 | } 97 | 98 | } 99 | 100 | #endif 101 | 102 | -------------------------------------------------------------------------------- /inria_maths/include/inria_maths/AxisAngle.h: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_MATHS_AXISANGLE_H 2 | #define INRIA_MATHS_AXISANGLE_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace inria { 9 | 10 | /** 11 | * Axis angle representation. 12 | * The direction is the rotation axis and 13 | * the norm is the angle in radian. 14 | */ 15 | 16 | /** 17 | * Compute and return the rotation matrix with given 18 | * axis. Identity is returned if null vector is given. 19 | * Rotation angle have to be in -M_PI:M_PI 20 | */ 21 | inline Eigen::Matrix3d AxisToMatrix(const Eigen::Vector3d& axis) 22 | { 23 | double theta = axis.norm(); 24 | if (fabs(theta) > M_PI) { 25 | throw std::logic_error("AxisAngle unbounded angle (in -M_PI:M_PI)"); 26 | } 27 | if (theta <= 0.0) { 28 | return Eigen::Matrix3d::Identity(); 29 | } else { 30 | Eigen::Vector3d vect = axis.normalized(); 31 | Eigen::AngleAxisd axisAngle(theta, vect); 32 | return axisAngle.matrix(); 33 | } 34 | } 35 | 36 | /** 37 | * Convert and return the rotation axis vector from 38 | * given rotation matrix. 39 | * Null vector is returned if identity matrix is given. 40 | * No check is done on input matrix format. 41 | * See https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation 42 | */ 43 | inline Eigen::Vector3d MatrixToAxis(const Eigen::Matrix3d& mat) 44 | { 45 | double val = (mat.trace()-1.0)/2.0; 46 | if (std::fabs(val) > 1.1) { 47 | throw std::logic_error("inria::MatrixToAxis: Invalid input matrix"); 48 | } 49 | //Clamp in case of small numerical errors 50 | if (val > 1.0) val = 1.0; 51 | if (val < -1.0) val = -1.0; 52 | double theta = std::acos(val); 53 | //Double check for NaN (acos) 54 | if (std::isnan(theta)) { 55 | throw std::logic_error("inria::MatrixToAxis: Error NaN"); 56 | } 57 | Eigen::Vector3d axis = Eigen::Vector3d::Zero(); 58 | if (std::fabs(theta) > 1e-6) { 59 | axis(0) = mat(2,1)-mat(1,2); 60 | axis(1) = mat(0,2)-mat(2,0); 61 | axis(2) = mat(1,0)-mat(0,1); 62 | axis *= theta/(2.0*std::sin(theta)); 63 | } 64 | 65 | return axis; 66 | } 67 | 68 | /** 69 | * Conversion from rotation axis 70 | * (first or second) differential to actual 71 | * angular velocity or acceleration in world frame 72 | * Reference: 73 | * Representing attitude: Euler angles, unit quaternions, and rotation vectors 74 | * Diebel 2006 75 | * Page 21 (eq. 259) 76 | * @param axis World to moving frame rotation axis (rotation vector) 77 | * @param axisDiff Time differential of axis (rotation vector rate) 78 | * @return rotation velocity or acceleration of moving frame 79 | * in world frame (angular velocity/acceleration) 80 | */ 81 | inline Eigen::Vector3d AxisDiffToAngularDiff( 82 | const Eigen::Vector3d& axis, const Eigen::Vector3d& axisDiff) 83 | { 84 | double v = axis.norm(); 85 | double v1 = axis(0); 86 | double v2 = axis(1); 87 | double v3 = axis(2); 88 | double cv2 = cos(v/2.0); 89 | double sv2 = sin(v/2.0); 90 | double a = cv2*v - 2.0*sv2; 91 | //Check for identity rotation 92 | if (v < 1e-6) { 93 | return axisDiff; 94 | } 95 | 96 | //Eq. 237 97 | Eigen::Matrix W; 98 | W << 99 | -v1*sv2, v*cv2, -v3*sv2, v2*sv2, 100 | -v2*sv2, v3*sv2, v*cv2, -v1*sv2, 101 | -v3*sv2, -v2*sv2, v1*sv2, v*cv2; 102 | W *= 1.0/v; 103 | //Eq. 214 104 | Eigen::Matrix G; 105 | G << 106 | -v1*v*v*sv2, -v2*v*v*sv2, -v3*v*v*sv2, 107 | 2.0*v*v*sv2 + v1*v1*a, v1*v2*a, v1*v3*a, 108 | v1*v2*a, 2.0*v*v*sv2 + v2*v2*a, v2*v3*a, 109 | v1*v3*a, v2*v3*a, 2.0*v*v*sv2 + v3*v3*a; 110 | G *= 1.0/(2.0*v*v*v); 111 | 112 | //Eq. 259 113 | return 2.0*W*G*axisDiff; 114 | } 115 | 116 | } 117 | 118 | #endif 119 | 120 | -------------------------------------------------------------------------------- /inria_maths/include/inria_maths/Clamp.h: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_MATHS_CLAMP_H 2 | #define INRIA_MATHS_CLAMP_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace inria { 10 | 11 | /** 12 | * Bound and return the given value within [min:max] range 13 | */ 14 | template 15 | inline T ClampRange(T value, T min, T max) 16 | { 17 | if (max < min) { 18 | throw std::logic_error( 19 | "inria::ClampRange: Invalid range."); 20 | } 21 | return std::min(std::max(value, min), max); 22 | } 23 | 24 | /** 25 | * Bound and return the given value within [-max:max] range 26 | */ 27 | template 28 | inline T ClampAbsolute(T value, T max) 29 | { 30 | if (max < 0.0) { 31 | throw std::logic_error( 32 | "inria::ClampAbsolute: Invalid absolute bound."); 33 | } 34 | return ClampRange(value, -max, max); 35 | } 36 | 37 | /** 38 | * Clamp a vector norm according 39 | * to given maximum norm limit 40 | */ 41 | inline Eigen::VectorXd ClampVectorNorm( 42 | const Eigen::VectorXd& vect, 43 | double limitMaxNorm) 44 | { 45 | double currentNorm = vect.norm(); 46 | if (currentNorm > limitMaxNorm) { 47 | return limitMaxNorm*vect.normalized(); 48 | } else { 49 | return vect; 50 | } 51 | } 52 | 53 | /** 54 | * Clamp a vector absolute components 55 | * according to given maximum component limit 56 | */ 57 | inline Eigen::VectorXd ClampVectorMaxComponent( 58 | const Eigen::VectorXd& vect, 59 | double limitMaxValue) 60 | { 61 | double currentMax = vect.cwiseAbs().maxCoeff(); 62 | if (currentMax > limitMaxValue) { 63 | return limitMaxValue*vect.normalized(); 64 | } else { 65 | return vect; 66 | } 67 | } 68 | 69 | /** 70 | * Clamp all absolute vector components 71 | * according to given maximum component limit 72 | */ 73 | inline Eigen::VectorXd ClampVectorAllComponent( 74 | const Eigen::VectorXd& vect, 75 | double limitMaxValue) 76 | { 77 | Eigen::VectorXd cpyVect = vect; 78 | for (size_t i=0;i<(size_t)cpyVect.size();i++) { 79 | cpyVect(i) = ClampAbsolute(cpyVect(i), limitMaxValue); 80 | } 81 | return cpyVect; 82 | } 83 | 84 | /** 85 | * Compute and return the signed distance from given value 86 | * to the bounds of the [min:max] range if value is outside 87 | * or zero if it is inside. 88 | */ 89 | template 90 | inline T ClampDistFromRange(T value, T min, T max) 91 | { 92 | if (max < min) { 93 | throw std::logic_error( 94 | "inria::ClampDistFromRange: Invalid range."); 95 | } 96 | 97 | if (value > max) { 98 | return max - value; 99 | } 100 | if (value < min) { 101 | return min - value; 102 | } else { 103 | return 0.0; 104 | } 105 | } 106 | 107 | /** 108 | * Compute and return the signed distance from given value 109 | * to the bounds of the [-max:max] range if value is outside 110 | * or zero if it is inside. 111 | */ 112 | template 113 | inline T ClampDistFromAbsolute(T value, T max) 114 | { 115 | if (max < 0.0) { 116 | throw std::logic_error( 117 | "inria::ClampDistFromAbsolute: Invalid absolute bound."); 118 | } 119 | return ClampDistFromRange(value, -max, max); 120 | } 121 | 122 | /** 123 | * Add delta vector to given point position or orientation while 124 | * clamping the (sphere) distance to a given reference. 125 | * The clamping only happens along the direction 126 | * of the given update vector. 127 | * 128 | * @param center Position or orientation reference 129 | * at the center of the valid space. 130 | * @param radius The sphere radius around center 131 | * defining the valid space. 132 | * @param point Position or orientation Initial point 133 | * from where to compute the update. 134 | * @param delta Desired velocity or axis angle vector 135 | * to add to point. 136 | */ 137 | inline Eigen::Vector3d ClampPositionUpdateIntoSphere( 138 | const Eigen::Vector3d& center, 139 | double radius, 140 | const Eigen::Vector3d& point, 141 | const Eigen::Vector3d& delta) 142 | { 143 | double deltaNorm = delta.norm(); 144 | if (deltaNorm < 1e-6) { 145 | //No update delta 146 | return point; 147 | } 148 | 149 | //Compute solutions for line sphere intersection 150 | Eigen::Vector3d deltaVect = delta.normalized(); 151 | Eigen::Vector3d tmpVect = point - center; 152 | double discriminant = 153 | std::pow(deltaVect.dot(tmpVect), 2) - (tmpVect.squaredNorm() - radius*radius); 154 | if (discriminant >= 0.0) { 155 | //Select the farer point on the sphere in the direction of the 156 | //delta vector or the closer if the directions are both negative 157 | double solution1 = -deltaVect.dot(tmpVect) + std::sqrt(discriminant); 158 | double solution2 = -deltaVect.dot(tmpVect) - std::sqrt(discriminant); 159 | double length = std::max(solution1, solution2); 160 | //Clamp this delta to update length 161 | length = std::min(deltaNorm, length); 162 | //Return updated point 163 | return point + length*deltaVect; 164 | } else { 165 | //No solution 166 | return point; 167 | } 168 | } 169 | inline Eigen::Matrix3d ClampOrientationUpdateIntoSphere( 170 | const Eigen::Matrix3d& center, 171 | double radius, 172 | const Eigen::Matrix3d& point, 173 | const Eigen::Vector3d& delta) 174 | { 175 | double deltaNorm = delta.norm(); 176 | if (deltaNorm < 1e-6) { 177 | //No update delta 178 | return point; 179 | } 180 | 181 | //Compute solutions for line sphere intersection 182 | Eigen::Vector3d deltaVect = delta.normalized(); 183 | Eigen::Vector3d tmpVect = MatrixToAxis(point*center.transpose()); 184 | double discriminant = 185 | std::pow(deltaVect.dot(tmpVect), 2) - (tmpVect.squaredNorm() - radius*radius); 186 | if (discriminant >= 0.0) { 187 | //Select the farer point on the sphere in the direction of the 188 | //delta vector or the closer if the directions are both negative 189 | double solution1 = -deltaVect.dot(tmpVect) + std::sqrt(discriminant); 190 | double solution2 = -deltaVect.dot(tmpVect) - std::sqrt(discriminant); 191 | double length = std::max(solution1, solution2); 192 | //Clamp this delta to update length 193 | length = std::min(deltaNorm, length); 194 | //Return updated point 195 | return AxisToMatrix(length*deltaVect)*point; 196 | } else { 197 | //No solution 198 | return point; 199 | } 200 | } 201 | 202 | } 203 | 204 | #endif 205 | 206 | -------------------------------------------------------------------------------- /inria_maths/include/inria_maths/Deadband.h: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_MATHS_DEADBAND_H 2 | #define INRIA_MATHS_DEADBAND_H 3 | 4 | namespace inria { 5 | 6 | /** 7 | * Deadband filter around zero. 8 | * 9 | * @param value Value to be filtered. 10 | * @param width Half range width (centered around zero) 11 | * on which the input value is zeroed. 12 | * @param isContinuous If false, the neutral zone is simply applied 13 | * and a discontinuity at +/- width value is present. 14 | * If true, an offset is applied on value to have smooth linear slope. 15 | * 16 | * @return the filtered value. 17 | */ 18 | inline double Deadband( 19 | double value, double width, bool isContinuous) 20 | { 21 | if (width < 0.0) { 22 | width = 0.0; 23 | } 24 | 25 | if (isContinuous) { 26 | if (value >= width) { 27 | return value - width; 28 | } else if (value <= -width) { 29 | return value + width; 30 | } else { 31 | return 0.0; 32 | } 33 | } else { 34 | if (std::fabs(value) <= width) { 35 | return 0.0; 36 | } else { 37 | return value; 38 | } 39 | } 40 | } 41 | 42 | /** 43 | * Apply Deadband filter on given 44 | * vector norm. 45 | */ 46 | template 47 | inline T DeadbandVectorNorm( 48 | const T& vect, double width, bool isContinuous) 49 | { 50 | if (width < 0.0) { 51 | width = 0.0; 52 | } 53 | 54 | double norm = vect.norm(); 55 | if (isContinuous) { 56 | if (norm >= width) { 57 | return vect.normalized()*(norm - width); 58 | } else { 59 | return 0.0*vect; 60 | } 61 | } else { 62 | if (norm <= width) { 63 | return 0.0*vect; 64 | } else { 65 | return vect; 66 | } 67 | } 68 | } 69 | 70 | /** 71 | * Apply Deadband filter on all 72 | * components of given vector. 73 | */ 74 | template 75 | inline T DeadbandVectorComponent( 76 | const T& vect, double width, bool isContinuous) 77 | { 78 | T vectOut = vect; 79 | for (size_t i=0;i 5 | #include 6 | 7 | namespace inria { 8 | 9 | /** 10 | * Manually convert the given rotation matrix 11 | * to [Roll, Pitch, Yaw] ZYX intrinsic euler 12 | * angle (Better range than Eigen conversion). 13 | */ 14 | inline Eigen::Vector3d MatrixToEulerIntrinsic( 15 | const Eigen::Matrix3d& mat) 16 | { 17 | //Eigen euler angles and with better range) 18 | Eigen::Vector3d angles; 19 | //Roll 20 | angles.x() = std::atan2(mat(2, 1), mat(2, 2)); 21 | //Pitch 22 | angles.y() = std::atan2(-mat(2, 0), 23 | sqrt(mat(0, 0)*mat(0, 0) 24 | + mat(1, 0)*mat(1, 0))); 25 | //Yaw 26 | angles.z() = std::atan2(mat(1, 0), mat(0, 0)); 27 | 28 | return angles; 29 | } 30 | 31 | /** 32 | * Convert given Euler angles in [Roll, Pitch, Yaw] 33 | * ZYX intrinsic format to rotation matrix 34 | */ 35 | inline Eigen::Matrix3d EulerIntrinsicToMatrix( 36 | const Eigen::Vector3d& angles) 37 | { 38 | Eigen::AngleAxisd yawRot(angles.z(), Eigen::Vector3d::UnitZ()); 39 | Eigen::AngleAxisd pitchRot(angles.y(), Eigen::Vector3d::UnitY()); 40 | Eigen::AngleAxisd rollRot(angles.x(), Eigen::Vector3d::UnitX()); 41 | Eigen::Quaternion quat = yawRot * pitchRot * rollRot; 42 | return quat.matrix(); 43 | } 44 | 45 | } 46 | 47 | #endif 48 | 49 | -------------------------------------------------------------------------------- /inria_maths/include/inria_maths/FilterDifferentiator2.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_MATHS_FILTERDIFFERENTIATOR2_HPP 2 | #define INRIA_MATHS_FILTERDIFFERENTIATOR2_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace inria { 10 | 11 | /** 12 | * FilterDifferentiator2 13 | * 14 | * Lowpass filtered finite difference differentiator 15 | */ 16 | template 17 | class FilterDifferentiator2 18 | { 19 | public: 20 | 21 | /** 22 | * Initialization. 23 | * 24 | * @param historySize Number of past data points 25 | * to store in the rolling buffer 26 | */ 27 | FilterDifferentiator2(unsigned int historySize = 100) : 28 | _deltaTime(0.1), 29 | _filterLowpass(), 30 | _isInitialized(false), 31 | _head(0), 32 | _bufferIn(), 33 | _time(0.0) 34 | { 35 | if (historySize < 1) { 36 | throw std::logic_error( 37 | "inria::FilterDifferentiator: Invalid history size"); 38 | } 39 | _bufferIn.resize(historySize+1); 40 | reset(); 41 | } 42 | 43 | /** 44 | * @return read or write and read direct 45 | * access to the lowpass cutoff frequency. 46 | */ 47 | const double& cutoffFrequency() const 48 | { 49 | return _filterLowpass.cutoffFrequency(); 50 | } 51 | double& cutoffFrequency() 52 | { 53 | return _filterLowpass.cutoffFrequency(); 54 | } 55 | 56 | /** 57 | * @return read or write and read direct 58 | * access to the maximum time difference 59 | */ 60 | const double& timeDelta() const 61 | { 62 | return _deltaTime; 63 | } 64 | double& timeDelta() 65 | { 66 | return _deltaTime; 67 | } 68 | 69 | /** 70 | * Update the internal filtered 71 | * state with in input value. 72 | * 73 | * @param value The new input value 74 | * to be differentiated. 75 | */ 76 | void update(const T& input, double dt) 77 | { 78 | //Initialization if needed 79 | if (!_isInitialized) { 80 | reset(input); 81 | } 82 | 83 | //Update internal time 84 | _time += dt; 85 | 86 | //Append the new value to 87 | //input rolling buffer 88 | _bufferIn[rollingIndex(0)] = std::make_pair(_time, input); 89 | 90 | //Find the old data index with 91 | //at most _deltaTime time difference 92 | size_t index = _bufferIn.size()-1; 93 | for (size_t i=1;i<_bufferIn.size()-1;i++) { 94 | if ( 95 | _bufferIn[rollingIndex(i)].first <= _bufferIn[rollingIndex(i+1)].first || 96 | _bufferIn[rollingIndex(0)].first - _bufferIn[rollingIndex(i)].first >= _deltaTime 97 | ) { 98 | index = i; 99 | break; 100 | } 101 | } 102 | //Compute the diff 103 | T diff = 104 | (1.0/(_bufferIn[rollingIndex(0)].first - _bufferIn[rollingIndex(index)].first)) * 105 | (_bufferIn[rollingIndex(0)].second - _bufferIn[rollingIndex(index)].second); 106 | //Update the lowpass filter 107 | _filterLowpass.update(diff, dt); 108 | 109 | //Update rolling buffer index 110 | _head = (_head+1) % _bufferIn.size(); 111 | } 112 | 113 | /** 114 | * @return read access to filtered 115 | * differentiated value. 116 | */ 117 | const T& value() const 118 | { 119 | return _filterLowpass.value(); 120 | } 121 | 122 | /** 123 | * Set the internal state as 124 | * uninitialized. Its value will be 125 | * assigned at next call of update(). 126 | */ 127 | void reset() 128 | { 129 | _isInitialized = false; 130 | } 131 | 132 | /** 133 | * Reset the internal state to 134 | * given value. 135 | * 136 | * @param value Value to be assign 137 | * to current internal state. 138 | */ 139 | void reset(const T& value) 140 | { 141 | _isInitialized = true; 142 | _head = 0; 143 | _time = 0.0; 144 | for (size_t i=0;i<_bufferIn.size();i++) { 145 | _bufferIn[i] = std::make_pair(0.0, value); 146 | } 147 | } 148 | 149 | private: 150 | 151 | /** 152 | * Maximum time difference 153 | * for finite difference lookup 154 | */ 155 | double _deltaTime; 156 | 157 | /** 158 | * Internal lowpass filter 159 | */ 160 | FilterExponential _filterLowpass; 161 | 162 | /** 163 | * If false, the current state is 164 | * not yet initialized and should be 165 | * set at next update() call. 166 | */ 167 | bool _isInitialized; 168 | 169 | /** 170 | * Begin index inside rolling buffer. 171 | * Index to the next cell to write. 172 | */ 173 | int _head; 174 | 175 | /** 176 | * Rolling buffer for 177 | * pair of time and input values 178 | */ 179 | std::vector> _bufferIn; 180 | 181 | /** 182 | * Internal time counter 183 | */ 184 | double _time; 185 | 186 | /** 187 | * Utility function computing buffer 188 | * index in rolling buffer. 189 | * 190 | * @param pos Expected position relative to head. 191 | * @return the index in rolling buffer. 192 | */ 193 | size_t rollingIndex(int pos) const 194 | { 195 | int size = _bufferIn.size(); 196 | int index = _head - pos; 197 | while (index < 0) { 198 | index += size; 199 | } 200 | return index % size; 201 | } 202 | }; 203 | 204 | } 205 | 206 | #endif 207 | 208 | -------------------------------------------------------------------------------- /inria_maths/include/inria_maths/FilterExponential.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_MATHS_FILTEREXPONENTIAL_HPP 2 | #define INRIA_MATHS_FILTEREXPONENTIAL_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace inria { 8 | 9 | /** 10 | * FilterExponential 11 | * 12 | * Type generic implementation 13 | * of the (varying time step) 14 | * exponential filter. 15 | * val_{t+1} = coeff*val_{t} + (1.0-coeff)*input 16 | * The coefficient is computed dynamically 17 | * according to the sampling time step. 18 | * 19 | * Not thread safe but the functions update(), 20 | * cutoffFrequency() and value() are RT safe 21 | * (depending on the underlying template type). 22 | */ 23 | template 24 | class FilterExponential 25 | { 26 | public: 27 | 28 | /** 29 | * Initialization with a default 30 | * time constant. 31 | * 32 | * @param cutoffFreq Optional default 33 | * smoothing cutoff frequency in seconds. 34 | */ 35 | FilterExponential(double cutoffFreq = 1000.0) : 36 | _state(T()), 37 | _isInitialized(false), 38 | _cutoffFreq(cutoffFreq) 39 | { 40 | reset(); 41 | } 42 | 43 | /** 44 | * Update the internal filtered state with 45 | * in input value and the time step. 46 | * 47 | * @param value The new input value to be filtered. 48 | * @param dt The sampling time step in seconds. 49 | */ 50 | void update( 51 | const T& value, 52 | double dt) 53 | { 54 | //Compute the exponential smoothing coefficient 55 | //between 0.0 and 1.0. 56 | //0.0: No filter. Use purely raw input. 57 | //1.0: Full filter. Do not use input data. 58 | double coeff = getAlphaFromFreq(_cutoffFreq, dt); 59 | 60 | if (!_isInitialized) { 61 | //Assignment in initialization case 62 | _isInitialized = true; 63 | _state = value; 64 | } else { 65 | //Exponential update 66 | _state = computeAlphaFilter(_state, value, coeff); 67 | } 68 | } 69 | 70 | /** 71 | * @return read or write and read direct 72 | * access to the lowpass cutoff frequency. 73 | */ 74 | const double& cutoffFrequency() const 75 | { 76 | return _cutoffFreq; 77 | } 78 | double& cutoffFrequency() 79 | { 80 | return _cutoffFreq; 81 | } 82 | 83 | /** 84 | * @return read access to filtered value. 85 | */ 86 | const T& value() const 87 | { 88 | return _state; 89 | } 90 | 91 | /** 92 | * Set the internal state as 93 | * uninitialized. Its value will be 94 | * assigned at next call of update(). 95 | */ 96 | void reset() 97 | { 98 | _isInitialized = false; 99 | } 100 | 101 | /** 102 | * Reset the internal state to 103 | * given value. 104 | * 105 | * @param value Value to be assign 106 | * to current internal state. 107 | */ 108 | void reset(const T& value) 109 | { 110 | _isInitialized = true; 111 | _state = value; 112 | } 113 | 114 | /** 115 | * Compute the exponential smoothing coefficient 116 | * between 0.0 and 1.0. 117 | * 0.0: No filter. Only update value is used. 118 | * 1.0: No update. Only state value is used. 119 | * 120 | * @param freq Cutoff frequency in Hz. 121 | * @param dt Time step in seconds. 122 | * @eturn the smoothing alpha coefficient between 0.0 and 1.0. 123 | */ 124 | static double getAlphaFromFreq(double freq, double dt) 125 | { 126 | double omega = 2.0*M_PI*freq; 127 | double coeff = (1.0-omega*dt/2.0)/(1.0+omega*dt/2.0); 128 | 129 | //Clamp smoothing coefficient 130 | if (coeff < 0.0) { 131 | coeff = 0.0; 132 | } 133 | if (coeff > 1.0) { 134 | coeff = 1.0; 135 | } 136 | 137 | return coeff; 138 | } 139 | 140 | /** 141 | * Compute the exponential alpha filter from 142 | * given smoothing coefficient. 143 | * 0.0: No filter. Only update value is used. 144 | * 1.0: No update. Only state value is used. 145 | * 146 | * @param valueState Original state value. 147 | * @param valueUpdate Update input value. 148 | * @param coeff Alpha coefficient between 0.0 and 1.0. 149 | * @return filtered value. 150 | */ 151 | static T computeAlphaFilter( 152 | const T& valueState, 153 | const T& valueUpdate, 154 | double coeff) 155 | { 156 | return coeff*valueState + (1.0-coeff)*valueUpdate; 157 | } 158 | 159 | private: 160 | 161 | /** 162 | * Internal typed state. 163 | */ 164 | T _state; 165 | 166 | /** 167 | * If false, the current state is 168 | * not yet initialized and should be 169 | * set at next update() call. 170 | */ 171 | bool _isInitialized; 172 | 173 | /** 174 | * Cutoff frequency of the lowpass filter 175 | */ 176 | double _cutoffFreq; 177 | }; 178 | 179 | } 180 | 181 | #endif 182 | 183 | -------------------------------------------------------------------------------- /inria_maths/include/inria_maths/FilterExponentialRotation.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_MATHS_FILTEREXPONENTIALROTATION_HPP 2 | #define INRIA_MATHS_FILTEREXPONENTIALROTATION_HPP 3 | 4 | #include 5 | 6 | namespace inria { 7 | 8 | /** 9 | * FilterExponentialRotation 10 | * 11 | * Implementation of the exponential 12 | * filter for 3d rotation matrix and quaternion 13 | * using quaternion interpolation. 14 | * val_{t+1} = coeff*val_{t} + (1.0-coeff)*input 15 | * The coefficient is computed dynamically 16 | * according to the sampling time step. 17 | * 18 | * Not thread safe but the functions update(), 19 | * cutoffFrequency() and valueMatrix(), valueQuaternion() 20 | * are RT safe. 21 | */ 22 | class FilterExponentialRotation 23 | { 24 | public: 25 | 26 | /** 27 | * Initialization with a default 28 | * time constant. 29 | * 30 | * @param cutoffFreq Optional default 31 | * smoothing cutoff frequency in seconds. 32 | */ 33 | FilterExponentialRotation(double cutoffFreq = 1000.0) : 34 | _state(Eigen::Quaterniond::Identity()), 35 | _isInitialized(false), 36 | _cutoffFreq(cutoffFreq) 37 | { 38 | reset(); 39 | } 40 | 41 | /** 42 | * Update the internal filtered state with 43 | * in input value and the time step. 44 | * 45 | * @param value The new rotation matrix 46 | * or quaternion be filtered. 47 | * @param dt The sampling time step in seconds. 48 | */ 49 | void update( 50 | const Eigen::Matrix3d& value, 51 | double dt) 52 | { 53 | update(Eigen::Quaterniond(value), dt); 54 | } 55 | void update( 56 | const Eigen::Quaterniond& value, 57 | double dt) 58 | { 59 | //Compute the exponential smoothing coefficient 60 | //between 0.0 and 1.0. 61 | //0.0: No filter. Use purely raw input. 62 | //1.0: Full filter. Do not use input data. 63 | double omega = 2.0*M_PI*_cutoffFreq; 64 | double coeff = (1.0-omega*dt/2.0)/(1.0+omega*dt/2.0); 65 | 66 | //Clamp smoothing coefficient 67 | if (coeff < 0.0) { 68 | coeff = 0.0; 69 | } 70 | if (coeff > 1.0) { 71 | coeff = 1.0; 72 | } 73 | 74 | if (!_isInitialized) { 75 | reset(value); 76 | } else { 77 | _state = 78 | _state.slerp(1.0-coeff, value); 79 | } 80 | } 81 | 82 | /** 83 | * @return read or write and read direct 84 | * access to the lowpass cutoff frequency. 85 | */ 86 | const double& cutoffFrequency() const 87 | { 88 | return _cutoffFreq; 89 | } 90 | double& cutoffFrequency() 91 | { 92 | return _cutoffFreq; 93 | } 94 | 95 | /** 96 | * @return read access to filtered value 97 | * converted back as rotation matrix or quaternion 98 | */ 99 | Eigen::Matrix3d valueMatrix() const 100 | { 101 | return _state.toRotationMatrix(); 102 | } 103 | Eigen::Quaterniond valueQuaternion() const 104 | { 105 | return _state; 106 | } 107 | 108 | /** 109 | * Set the internal state as 110 | * uninitialized. Its value will be 111 | * assigned at next call of update(). 112 | */ 113 | void reset() 114 | { 115 | _isInitialized = false; 116 | } 117 | 118 | /** 119 | * Reset the internal state to 120 | * given rotation matrix or quaternion. 121 | * 122 | * @param value Orientation to be assign 123 | * to current internal state. 124 | */ 125 | void reset(const Eigen::Matrix3d& mat) 126 | { 127 | _isInitialized = true; 128 | _state = Eigen::Quaterniond(mat); 129 | } 130 | void reset(const Eigen::Quaterniond& quat) 131 | { 132 | _isInitialized = true; 133 | _state = quat; 134 | } 135 | 136 | private: 137 | 138 | /** 139 | * Internal state. 140 | */ 141 | Eigen::Quaterniond _state; 142 | 143 | /** 144 | * If false, the current state is 145 | * not yet initialized and should be 146 | * set at next update() call. 147 | */ 148 | bool _isInitialized; 149 | 150 | /** 151 | * Cutoff frequency of the lowpass filter 152 | */ 153 | double _cutoffFreq; 154 | }; 155 | 156 | } 157 | 158 | #endif 159 | 160 | -------------------------------------------------------------------------------- /inria_maths/include/inria_maths/FilterInterpolation.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_MATHS_FILTERINTERPOLATION_HPP 2 | #define INRIA_MATHS_FILTERINTERPOLATION_HPP 3 | 4 | #include 5 | 6 | namespace inria { 7 | 8 | /** 9 | * FilterInterpolation 10 | * 11 | * Dynamic linear interpolation as filter 12 | */ 13 | template 14 | class FilterInterpolation 15 | { 16 | public: 17 | 18 | /** 19 | * Initialization. 20 | */ 21 | FilterInterpolation() : 22 | _timeState(-1.0), 23 | _timeLength(-1.0), 24 | _valueStart(), 25 | _valueEnd() 26 | { 27 | } 28 | 29 | /** 30 | * Reset interpolation with starting current state. 31 | * 32 | * @param valueStart Initial value for interpolation. 33 | * @param timeLength Interpolation time length. 34 | */ 35 | void reset(const T& valueStart, double timeLength = -1.0) 36 | { 37 | _timeState = timeLength; 38 | _timeLength = timeLength; 39 | _valueStart = valueStart; 40 | _valueEnd = valueStart; 41 | } 42 | 43 | /** 44 | * Update interpolation state. 45 | * 46 | * @param valueEnd Final value to interpolate toward. 47 | * @param dt Time step. 48 | */ 49 | void update(const T& valueEnd, double dt) 50 | { 51 | //Reset if not initialized 52 | if (_timeState <= 0.0 || _timeLength <= 0.0) { 53 | reset(valueEnd); 54 | } 55 | 56 | _timeState -= dt; 57 | _valueEnd = valueEnd; 58 | } 59 | 60 | /** 61 | * @return read access to filtered interpolated value 62 | */ 63 | T value() const 64 | { 65 | if (_timeState <= 0.0 || _timeLength <= 0.0) { 66 | return _valueEnd; 67 | } else { 68 | double alpha = _timeState/_timeLength; 69 | if (alpha > 1.0) { 70 | alpha = 1.0; 71 | } 72 | if (alpha < 0.0) { 73 | alpha = 0.0; 74 | } 75 | return alpha*_valueStart + (1.0-alpha)*_valueEnd; 76 | } 77 | } 78 | 79 | private: 80 | 81 | /** 82 | * Remaining time until 83 | * interpolation end 84 | */ 85 | double _timeState; 86 | 87 | /** 88 | * Interpolation time length 89 | * from start to end 90 | */ 91 | double _timeLength; 92 | 93 | /** 94 | * Generic initial and final value 95 | * for linear interpolation 96 | */ 97 | T _valueStart; 98 | T _valueEnd; 99 | }; 100 | 101 | } 102 | 103 | #endif 104 | 105 | -------------------------------------------------------------------------------- /inria_maths/include/inria_maths/FilterPoseCommand.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_MATHS_FILTERPOSECOMMAND_HPP 2 | #define INRIA_MATHS_FILTERPOSECOMMAND_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace inria { 11 | 12 | /** 13 | * FilterPoseCommand 14 | * 15 | * Filter for Cartesian pose command using position 16 | * and velocity command. Apply lowpass and bangbangacc filters 17 | * in addition to special clamping scheme in velocity command 18 | */ 19 | class FilterPoseCommand 20 | { 21 | public: 22 | 23 | /** 24 | * Default initialization 25 | */ 26 | FilterPoseCommand(); 27 | 28 | /** 29 | * Assign filter parameters 30 | * 31 | * @param useRawVRPose If true, input pose is a raw 32 | * pose from VR world with Z up and is processed 33 | * with isClutch. If false, input pose is directly 34 | * that target pose in world frame and velocity command 35 | * is not used. 36 | * @param isLocalFrame If true, commands are assumed 37 | * given in local effector frame, else world frame. 38 | * @param scalingLin Scaling apply on linear motion of 39 | * offset pôse commands. 40 | * @param clampRadiusLin Radius for sphere clamping 41 | * of linear velocity commands. 42 | * @param clampRadiusAng Radius for sphere clamping 43 | * of angular velocity commands. 44 | * @param cutoffFreq Cutfoff frequency of lowpass filter. 45 | * @param maxVelLin Maximum linear velocity. 46 | * @param maxAccLin Maximum linear acceleration 47 | * @param maxVelAng Maximum angular velocity. 48 | * @param maxAccAng Maximum angular acceleration. 49 | */ 50 | void setParameters( 51 | bool useRawVRPose, 52 | bool isLocalFrame, 53 | double scalingLin, 54 | double clampRadiusLin, double clampRadiusAng, 55 | double cutoffFreq, 56 | double maxVelLin, double maxAccLin, 57 | double maxVelAng, double maxAccAng); 58 | 59 | /** 60 | * Update internal state with given pose offset 61 | * and velocity command. 62 | * 63 | * @param dt Time step. 64 | * @param isClutch If true, the motion of raw pose 65 | * should be interated. 66 | * @param rawPos Raw position input with Z as up. 67 | * àparam rawMat Raw orientation input with Z as up. 68 | * @param velLin Linear velocity command to be integrated. 69 | * @param velAng Angular velocity command to be integrated 70 | * @param centerPos Center position used in clamping 71 | * not to move too far away (radiusLin at max). 72 | * @param centerMat Center orientation used in clamping 73 | * not to move too far away (radiusAng at max). 74 | */ 75 | void update( 76 | double dt, 77 | bool isClutch, 78 | const Eigen::Vector3d& rawPos, 79 | const Eigen::Matrix3d& rawMat, 80 | const Eigen::Vector3d& velLin, 81 | const Eigen::Vector3d& velAng, 82 | const Eigen::Vector3d& centerPos, 83 | const Eigen::Matrix3d& centerMat); 84 | 85 | /** 86 | * @return internal integrated anchor pose 87 | */ 88 | const Eigen::Vector3d& anchorPos() const; 89 | const Eigen::Matrix3d& anchorMat() const; 90 | 91 | /** 92 | * @return internal pre-filtering target pose 93 | */ 94 | const Eigen::Vector3d& targetPos() const; 95 | const Eigen::Matrix3d& targetMat() const; 96 | 97 | /** 98 | * @return filtered position or orientation 99 | */ 100 | const Eigen::Vector3d& valuePos() const; 101 | const Eigen::Matrix3d& valueMat() const; 102 | 103 | /** 104 | * Reset internal filter state to 105 | * given position and orientation. 106 | */ 107 | void reset( 108 | const Eigen::Vector3d& pos, 109 | const Eigen::Matrix3d& mat); 110 | 111 | private: 112 | 113 | /** 114 | * If false, raw input pose offset will 115 | * be initialized at next update() call 116 | */ 117 | bool _isInitialized; 118 | 119 | /** 120 | * If true, the input raw pose is assumed to be 121 | * a non processed pose from VR world (with Z up). 122 | * If false, the input pose is the pose target 123 | * in world frame (isClutch and velocity command 124 | * are not used). 125 | */ 126 | bool _useRawVRPose; 127 | 128 | /** 129 | * If true, commands are assmed to be in local 130 | * effector frame, else in world frame 131 | */ 132 | bool _isLocalFrame; 133 | 134 | /** 135 | * Radius for velocity command sphere clamping 136 | */ 137 | double _clampRadiusLin; 138 | double _clampRadiusAng; 139 | 140 | /** 141 | * Anchor position and orientation 142 | * integrated reference for velocity and position 143 | * command pre-filtering 144 | */ 145 | Eigen::Vector3d _anchorPos; 146 | Eigen::Matrix3d _anchorMat; 147 | 148 | /** 149 | * Target (anchor + offset) position and orientation 150 | * pre-filtering 151 | */ 152 | Eigen::Vector3d _targetPos; 153 | Eigen::Matrix3d _targetMat; 154 | 155 | /** 156 | * Input pose processing from raw not offset pose 157 | */ 158 | PoseCommandProcessing _poseProcessing; 159 | 160 | /** 161 | * Lowpass filters for position and orientation 162 | */ 163 | FilterExponential _filterLowpassPos; 164 | FilterExponentialRotation _filterLowpassMat; 165 | 166 | /** 167 | * Max velocity and acceleration filters for 168 | * position and orientation 169 | */ 170 | FilterBangBangAcc _filterBangbangPos; 171 | FilterBangBangAccRotation _filterBangbangMat; 172 | }; 173 | 174 | } 175 | 176 | #endif 177 | 178 | -------------------------------------------------------------------------------- /inria_maths/include/inria_maths/FilterVelCommand.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_MATHS_FILTERVELCOMMAND_HPP 2 | #define INRIA_MATHS_FILTERVELCOMMAND_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace inria { 8 | 9 | /** 10 | * FilterVelCommand 11 | * 12 | * Filter velocity command integration into 13 | * a filtered position command that is clamped not to be 14 | * too far from a state position. 15 | */ 16 | template 17 | class FilterVelCommand 18 | { 19 | public: 20 | 21 | /** 22 | * Default initialization 23 | */ 24 | FilterVelCommand() : 25 | _isInitialized(false), 26 | _clampRadius(), 27 | _target(), 28 | _filterLowpass(), 29 | _filterBangbang() 30 | { 31 | } 32 | 33 | /** 34 | * Assign filter parameters 35 | * 36 | * @param clampRadius Distance for clamping signal from state. 37 | * @param cutoffFreq Cutoff frequency of lowpass filter. 38 | * @param maxVel Maximum signal velocity. 39 | * @param maxAcc Maximum signal acceleration. 40 | */ 41 | void setParameters( 42 | double clampRadius, 43 | double cutoffFreq, 44 | double maxVel, 45 | double maxAcc) 46 | { 47 | _clampRadius = clampRadius; 48 | _filterLowpass.cutoffFrequency() = cutoffFreq; 49 | _filterBangbang.maxVel() = maxVel; 50 | _filterBangbang.maxAcc() = maxAcc; 51 | } 52 | 53 | /** 54 | * Reset internal filter to uninitialized 55 | */ 56 | void reset() 57 | { 58 | _isInitialized = false; 59 | } 60 | 61 | /** 62 | * Update internal state with given 63 | * velocity command. 64 | * 65 | * @param dt Time step. 66 | * @param vel Velocity command to be integrated. 67 | * @param state Current command state from which 68 | * the target signal can not be farer than the 69 | * clamping radius. 70 | */ 71 | void update( 72 | double dt, 73 | const T& vel, const T& state) 74 | { 75 | //Initialization 76 | if (!_isInitialized) { 77 | _isInitialized = true; 78 | _target = state; 79 | _filterLowpass.reset(_target); 80 | _filterBangbang.reset(_target); 81 | } 82 | 83 | //Velocity integration 84 | _target += dt*vel; 85 | //Clamping to state sphere 86 | if (_target > state + _clampRadius) { 87 | _target = state + _clampRadius; 88 | } 89 | if (_target < state - _clampRadius) { 90 | _target = state - _clampRadius; 91 | } 92 | //Update filters 93 | _filterLowpass.update(_target, dt); 94 | _filterBangbang.update(_filterLowpass.value(), dt); 95 | } 96 | 97 | /** 98 | * @return internal pre-filtering target position 99 | */ 100 | const T& target() const 101 | { 102 | return _target; 103 | } 104 | 105 | /** 106 | * @return filtered position command signal 107 | */ 108 | const T& value() const 109 | { 110 | return _filterBangbang.value(); 111 | } 112 | 113 | private: 114 | 115 | /** 116 | * If false, the filter will be initialized 117 | * at next update() call 118 | */ 119 | bool _isInitialized; 120 | 121 | /** 122 | * Radius for command clamping 123 | */ 124 | double _clampRadius; 125 | 126 | /** 127 | * Target position before filtering 128 | */ 129 | T _target; 130 | 131 | /** 132 | * Lowpass and max velocity and acceleration filters 133 | */ 134 | FilterExponential _filterLowpass; 135 | FilterBangBangAcc _filterBangbang; 136 | }; 137 | 138 | } 139 | 140 | #endif 141 | 142 | -------------------------------------------------------------------------------- /inria_maths/include/inria_maths/IntegrateDOFVect.h: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_MATHS_INTEGRATEDOFVECT_H 2 | #define INRIA_MATHS_INTEGRATEDOFVECT_H 3 | 4 | #include 5 | #include 6 | 7 | namespace inria { 8 | 9 | /** 10 | * Increment a degree of freedom position vector with 11 | * a delta velocity vector while using the special 12 | * RBDL storing convention for position. 13 | * Handle the problem of the floating base orientation 14 | * represented as a quaternion. 15 | * 16 | * @param vectPos Position DOF vector in RBDL 17 | * format (sizeVectPos()). 18 | * @param vectDelta Delta DOF vector in RBDL 19 | * velocity format (sizeVectVel()). 20 | * @return the integrated degree of freedom 21 | * position vector taking into account the quaternion 22 | * orientation (vectPos + vectDelta). 23 | */ 24 | inline Eigen::VectorXd IntegrateDOFVect( 25 | const Eigen::VectorXd& vectPos, 26 | const Eigen::VectorXd& vectDelta) 27 | { 28 | //Size checks 29 | size_t sizeDOF = vectDelta.size(); 30 | if ( 31 | sizeDOF <= 7 || 32 | (size_t)vectPos.size() != sizeDOF+1 33 | ) { 34 | throw std::logic_error( 35 | "inria::IntegrateDOFVect: Invalid input vectors."); 36 | } 37 | size_t sizeJoint = sizeDOF-6; 38 | 39 | Eigen::VectorXd vectNew = vectPos; 40 | Eigen::Quaterniond quatOld = Eigen::Quaterniond( 41 | vectPos(sizeDOF), 42 | vectPos(3), 43 | vectPos(4), 44 | vectPos(5)); 45 | Eigen::Vector3d diff( 46 | vectDelta(3), 47 | vectDelta(4), 48 | vectDelta(5)); 49 | vectNew.segment(0, 3) += vectDelta.segment(0, 3); 50 | vectNew.segment(6, sizeJoint) += vectDelta.segment(6, sizeJoint); 51 | Eigen::Quaterniond quatDelta; 52 | if (diff.norm() > 1e-10) { 53 | quatDelta = Eigen::Quaterniond( 54 | Eigen::AngleAxisd(diff.norm(), diff.normalized())); 55 | } else { 56 | quatDelta.setIdentity(); 57 | } 58 | Eigen::Quaterniond quatNew = quatOld*quatDelta; 59 | quatNew.normalize(); 60 | vectNew(sizeDOF) = quatNew.w(); 61 | vectNew(3) = quatNew.x(); 62 | vectNew(4) = quatNew.y(); 63 | vectNew(5) = quatNew.z(); 64 | 65 | return vectNew; 66 | } 67 | 68 | } 69 | 70 | #endif 71 | 72 | -------------------------------------------------------------------------------- /inria_maths/include/inria_maths/Polynomial.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_MATHS_POLYNOM_HPP 2 | #define INRIA_MATHS_POLYNOM_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace inria { 9 | 10 | /** 11 | * Polynomial 12 | * 13 | * Simple one dimensional 14 | * polynomial class for spline 15 | * generation 16 | */ 17 | class Polynomial 18 | { 19 | public: 20 | 21 | /** 22 | * Default and initial 23 | * degree initialization 24 | * 25 | * @param degree Initial degree 26 | * for memory allocation 27 | */ 28 | Polynomial(); 29 | Polynomial(unsigned int degree); 30 | 31 | /** 32 | * Access to all coefficients. 33 | * 34 | * @return a reference to coefficients container 35 | * indexed from constant to higher degree. 36 | */ 37 | const std::vector& getCoefs() const; 38 | std::vector& getCoefs(); 39 | 40 | /** 41 | * Access to a coefficient. 42 | * 43 | * @param index A coefficient index 44 | * from 0 (constant) to degree()+1. 45 | * @return a reference to the coefficient. 46 | */ 47 | const double& operator()(size_t index) const; 48 | double& operator()(size_t index); 49 | 50 | /** 51 | * @return the degree of the polynomial. 52 | * (size_t)-1 mean empty polynomial. 53 | * There is degree()+1 coefficients. 54 | */ 55 | size_t degree() const; 56 | 57 | /** 58 | * Evaluation of the polynomial and 59 | * its first, second, third and fourth 60 | * derivatives. 61 | * 62 | * @param t The evaluation point. 63 | * @return Polynomial evaluation at t. 64 | */ 65 | double pos(double t) const; 66 | double vel(double t) const; 67 | double acc(double t) const; 68 | double jerk(double t) const; 69 | 70 | /** 71 | * Multiply and update all coefficients 72 | * with the given constant. 73 | * 74 | * @param coef The value to multiply. 75 | */ 76 | void operator*=(double coef); 77 | 78 | /** 79 | * Reset internal data 80 | * and build a cubic or quintic 81 | * polynomial with given boundary 82 | * conditions between 0 and time. 83 | * Throw std::logic_error if given time 84 | * is zero or negative. 85 | * 86 | * @param time Given time of final 87 | * boundary conditions. 88 | * @param pos1 Initial position at t=0 89 | * @param vel1 Initial velocity at t=0 90 | * @param acc1 Initial acceleration at t=0 91 | * @param pos2 Final position at t=time 92 | * @param vel2 Final velocity at t=time 93 | * @param acc2 Final acceleration at t=time 94 | */ 95 | void fitCubic( 96 | double time, 97 | double pos1, double vel1, 98 | double pos2, double vel2); 99 | void fitQuintic( 100 | double time, 101 | double pos1, double vel1, double acc1, 102 | double pos2, double vel2, double acc2); 103 | 104 | private: 105 | 106 | /** 107 | * Polynomial coeficients 108 | * indexed from constant 109 | * to higher degrees 110 | */ 111 | std::vector _coefs; 112 | }; 113 | 114 | } 115 | 116 | #endif 117 | 118 | -------------------------------------------------------------------------------- /inria_maths/include/inria_maths/PoseCommandProcessing.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_MATHS_POSECOMMANDPROCESSING_HPP 2 | #define INRIA_MATHS_POSECOMMANDPROCESSING_HPP 3 | 4 | #include 5 | 6 | namespace inria { 7 | 8 | /** 9 | * PoseCommandProcessing 10 | * 11 | * Processing of raw pose outputted by 12 | * the Vive tracking. 13 | * Up is assumed to be positive Z axis. 14 | */ 15 | struct PoseCommandProcessing 16 | { 17 | //Scaling factor parameter applied on 18 | //integrated translation motion. 19 | double scalingLin; 20 | //Calibration offset pose 21 | Eigen::Vector3d posOffset; 22 | Eigen::Matrix3d matOffset; 23 | //Pose of the hand offset at the origin 24 | //when trigger is not pushed 25 | Eigen::Vector3d posCentered; 26 | Eigen::Matrix3d matCentered; 27 | //Anchor and integrated pose from 28 | //motion of centered pose 29 | bool isIntegrating; 30 | Eigen::Vector3d posAnchor; 31 | Eigen::Matrix3d matAnchor; 32 | Eigen::Vector3d posIntegrated; 33 | Eigen::Matrix3d matIntegrated; 34 | 35 | /** 36 | * Empty initialization 37 | */ 38 | PoseCommandProcessing(); 39 | 40 | /** 41 | * Update internal pose processing 42 | * with given raw pose. 43 | * 44 | * @param isClutch If true, pose motion integration 45 | * is enabled. Anchor coordinate frame is reset and offset 46 | * when clutch is enabled. 47 | * @param posRaw Absolute position with A as up. 48 | * @param matRaw Absolute orientation with Z as up. 49 | */ 50 | void update( 51 | bool isClutch, 52 | const Eigen::Vector3d& posRaw, 53 | const Eigen::Matrix3d& matRaw); 54 | }; 55 | 56 | } 57 | 58 | #endif 59 | 60 | -------------------------------------------------------------------------------- /inria_maths/include/inria_maths/Skew.h: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_MATHS_SKEW_H 2 | #define INRIA_MATHS_SKEW_H 3 | 4 | #include 5 | 6 | namespace inria { 7 | 8 | /** 9 | * Build and return the skew rotation matrix 10 | * from given angular velocity vector 11 | */ 12 | inline Eigen::Matrix3d SkewMatrix(const Eigen::Vector3d& vect) 13 | { 14 | Eigen::Matrix3d mat; 15 | mat << 16 | 0.0, -vect.z(), +vect.y(), 17 | +vect.z(), 0.0, -vect.x(), 18 | -vect.y(), +vect.x(), 0.0; 19 | return mat; 20 | } 21 | 22 | } 23 | 24 | #endif 25 | 26 | -------------------------------------------------------------------------------- /inria_maths/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | inria_maths 4 | 0.0.0 5 | 6 | Various maths tools 7 | 8 | Quentin Rouxel 9 | BSD 10 | 11 | catkin 12 | inria_utils 13 | 14 | -------------------------------------------------------------------------------- /inria_maths/src/FilterPoseCommand.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | namespace inria { 6 | 7 | FilterPoseCommand::FilterPoseCommand() : 8 | _isInitialized(false), 9 | _isLocalFrame(false), 10 | _useRawVRPose(true), 11 | _clampRadiusLin(), 12 | _clampRadiusAng(), 13 | _anchorPos(), 14 | _anchorMat(), 15 | _targetPos(), 16 | _targetMat(), 17 | _poseProcessing(), 18 | _filterLowpassPos(), 19 | _filterLowpassMat(), 20 | _filterBangbangPos(), 21 | _filterBangbangMat() 22 | { 23 | //Default parameters and state 24 | setParameters(true, false, 1.0, 0.01, 0.1, 1.0, 0.01, 0.02, 0.1, 0.2); 25 | reset(Eigen::Vector3d::Zero(), Eigen::Matrix3d::Identity()); 26 | } 27 | 28 | void FilterPoseCommand::setParameters( 29 | bool useRawVRPose, 30 | bool isLocalFrame, 31 | double scalingLin, 32 | double clampRadiusLin, double clampRadiusAng, 33 | double cutoffFreq, 34 | double maxVelLin, double maxAccLin, 35 | double maxVelAng, double maxAccAng) 36 | { 37 | _useRawVRPose = useRawVRPose; 38 | _isLocalFrame = isLocalFrame; 39 | _poseProcessing.scalingLin = scalingLin; 40 | _clampRadiusLin = clampRadiusLin; 41 | _clampRadiusAng = clampRadiusAng; 42 | _filterLowpassPos.cutoffFrequency() = cutoffFreq; 43 | _filterLowpassMat.cutoffFrequency() = cutoffFreq; 44 | _filterBangbangPos.maxVel() = maxVelLin; 45 | _filterBangbangPos.maxAcc() = maxAccLin; 46 | _filterBangbangMat.maxVel() = maxVelAng; 47 | _filterBangbangMat.maxAcc() = maxAccAng; 48 | } 49 | 50 | void FilterPoseCommand::update( 51 | double dt, 52 | bool isClutch, 53 | const Eigen::Vector3d& rawPos, 54 | const Eigen::Matrix3d& rawMat, 55 | const Eigen::Vector3d& velLin, 56 | const Eigen::Vector3d& velAng, 57 | const Eigen::Vector3d& centerPos, 58 | const Eigen::Matrix3d& centerMat) 59 | { 60 | if (_useRawVRPose) { 61 | //Initial input offset 62 | if (!_isInitialized) { 63 | _poseProcessing.posOffset = rawPos; 64 | _poseProcessing.matOffset = rawMat; 65 | _isInitialized = true; 66 | } 67 | //Integrate the anchor pose and the raw pose clutch is released 68 | if (!isClutch && _poseProcessing.isIntegrating) { 69 | _anchorPos = _targetPos; 70 | _anchorMat = _targetMat; 71 | //Re-project the anchor on the clamping sphere 72 | //when the target went too far away from the reference center 73 | Eigen::Vector3d vectLin = 74 | _anchorPos-centerPos; 75 | Eigen::Vector3d vectAng = 76 | MatrixToAxis(_anchorMat*centerMat.transpose()); 77 | if (vectLin.norm() > _clampRadiusLin) { 78 | _anchorPos = 79 | centerPos + 80 | _clampRadiusLin*vectLin.normalized(); 81 | } 82 | if (vectAng.norm() > _clampRadiusAng) { 83 | _anchorMat = 84 | AxisToMatrix(_clampRadiusAng*vectAng.normalized()) 85 | * centerMat; 86 | } 87 | } 88 | //Raw input pose processing 89 | _poseProcessing.update( 90 | isClutch, rawPos, rawMat); 91 | //Integration of velocity command and clamping into 92 | //sphere to prevent the integrated pose to move too 93 | //far from given center pose. 94 | Eigen::Vector3d deltaLin; 95 | Eigen::Vector3d deltaAng; 96 | if (_isLocalFrame) { 97 | deltaLin = dt*centerMat*velLin; 98 | deltaAng = dt*centerMat*velAng; 99 | } else { 100 | deltaLin = dt*velLin; 101 | deltaAng = dt*velAng; 102 | } 103 | _anchorPos = ClampPositionUpdateIntoSphere( 104 | centerPos, _clampRadiusLin, _anchorPos, deltaLin); 105 | _anchorMat = ClampOrientationUpdateIntoSphere( 106 | centerMat, _clampRadiusAng, _anchorMat, deltaAng); 107 | //Apply offset processed pose to anchor pose 108 | if (_isLocalFrame) { 109 | _targetPos = 110 | centerMat*_poseProcessing.posCentered + _anchorPos; 111 | _targetMat = 112 | _anchorMat*_poseProcessing.matCentered; 113 | } else { 114 | _targetPos = 115 | _poseProcessing.posCentered + _anchorPos; 116 | _targetMat = 117 | _poseProcessing.matCentered*_anchorMat; 118 | } 119 | } else { 120 | _targetPos = rawPos; 121 | _targetMat = rawMat; 122 | } 123 | //Update lowpass filters 124 | _filterLowpassPos.update(_targetPos, dt); 125 | _filterLowpassMat.update(_targetMat, dt); 126 | //Update bangbang filters 127 | _filterBangbangPos.update(_filterLowpassPos.value(), dt); 128 | _filterBangbangMat.update(_filterLowpassMat.valueMatrix(), dt); 129 | } 130 | 131 | const Eigen::Vector3d& FilterPoseCommand::anchorPos() const 132 | { 133 | return _anchorPos; 134 | } 135 | const Eigen::Matrix3d& FilterPoseCommand::anchorMat() const 136 | { 137 | return _anchorMat; 138 | } 139 | 140 | const Eigen::Vector3d& FilterPoseCommand::targetPos() const 141 | { 142 | return _targetPos; 143 | } 144 | const Eigen::Matrix3d& FilterPoseCommand::targetMat() const 145 | { 146 | return _targetMat; 147 | } 148 | 149 | const Eigen::Vector3d& FilterPoseCommand::valuePos() const 150 | { 151 | return _filterBangbangPos.value(); 152 | } 153 | const Eigen::Matrix3d& FilterPoseCommand::valueMat() const 154 | { 155 | return _filterBangbangMat.value(); 156 | } 157 | 158 | void FilterPoseCommand::reset( 159 | const Eigen::Vector3d& pos, 160 | const Eigen::Matrix3d& mat) 161 | { 162 | _isInitialized = false; 163 | _poseProcessing.isIntegrating = false; 164 | _anchorPos = pos; 165 | _anchorMat = mat; 166 | _targetPos = pos; 167 | _targetMat = mat; 168 | _filterLowpassPos.reset(pos); 169 | _filterLowpassMat.reset(mat); 170 | _filterBangbangPos.reset(pos); 171 | _filterBangbangMat.reset(mat); 172 | } 173 | 174 | } 175 | 176 | -------------------------------------------------------------------------------- /inria_maths/src/Polynomial.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace inria { 5 | 6 | Polynomial::Polynomial() : 7 | _coefs() 8 | { 9 | } 10 | Polynomial::Polynomial(unsigned int degree) : 11 | _coefs() 12 | { 13 | _coefs.resize(degree+1, 0.0); 14 | } 15 | 16 | const std::vector& Polynomial::getCoefs() const 17 | { 18 | return _coefs; 19 | } 20 | std::vector& Polynomial::getCoefs() 21 | { 22 | return _coefs; 23 | } 24 | 25 | const double& Polynomial::operator()(size_t index) const 26 | { 27 | return _coefs.at(index); 28 | } 29 | double& Polynomial::operator()(size_t index) 30 | { 31 | return _coefs.at(index); 32 | } 33 | 34 | size_t Polynomial::degree() const 35 | { 36 | return _coefs.size()-1; 37 | } 38 | 39 | double Polynomial::pos(double t) const 40 | { 41 | double tt = 1.0; 42 | double val = 0.0; 43 | for (size_t i=0;i<_coefs.size();i++) { 44 | val += tt*_coefs[i]; 45 | tt *= t; 46 | } 47 | return val; 48 | } 49 | double Polynomial::vel(double t) const 50 | { 51 | double tt = 1.0; 52 | double val = 0.0; 53 | for (size_t i=1;i<_coefs.size();i++) { 54 | val += i*tt*_coefs[i]; 55 | tt *= t; 56 | } 57 | return val; 58 | } 59 | double Polynomial::acc(double t) const 60 | { 61 | double tt = 1.0; 62 | double val = 0.0; 63 | for (size_t i=2;i<_coefs.size();i++) { 64 | val += (i-1)*i*tt*_coefs[i]; 65 | tt *= t; 66 | } 67 | return val; 68 | } 69 | double Polynomial::jerk(double t) const 70 | { 71 | double tt = 1.0; 72 | double val = 0.0; 73 | for (size_t i=3;i<_coefs.size();i++) { 74 | val += (i-2)*(i-1)*i*tt*_coefs[i]; 75 | tt *= t; 76 | } 77 | return val; 78 | } 79 | 80 | void Polynomial::operator*=(double coef) 81 | { 82 | for (size_t i=0;i<_coefs.size();i++) { 83 | _coefs[i] *= coef; 84 | } 85 | } 86 | 87 | void Polynomial::fitCubic( 88 | double time, 89 | double pos1, double vel1, 90 | double pos2, double vel2) 91 | { 92 | if (time <= 1e-6) { 93 | throw std::logic_error( 94 | "inria::Polynomial:fitCubic: " 95 | "Invalid time interval: " 96 | + std::to_string(time)); 97 | } 98 | double t = time; 99 | double t2 = t*t; 100 | double t3 = t2*t; 101 | getCoefs().resize(4); 102 | getCoefs()[0] = pos1; 103 | getCoefs()[1] = vel1; 104 | getCoefs()[3] = (vel2 - vel1 - 2.0*(pos2-pos1-vel1*t)/t)/t2; 105 | getCoefs()[2] = (pos2 - pos1 - vel1*t - getCoefs()[3]*t3)/t2; 106 | } 107 | void Polynomial::fitQuintic( 108 | double time, 109 | double pos1, double vel1, double acc1, 110 | double pos2, double vel2, double acc2) 111 | { 112 | if (time <= 1e-6) { 113 | throw std::logic_error( 114 | "inria::Polynomial:fitQuintic: " 115 | "Invalid time interval: " 116 | + std::to_string(time)); 117 | } 118 | double t = time; 119 | double t2 = t*t; 120 | double t3 = t2*t; 121 | double t4 = t3*t; 122 | double t5 = t4*t; 123 | getCoefs().resize(6); 124 | getCoefs()[0] = pos1; 125 | getCoefs()[1] = vel1; 126 | getCoefs()[2] = acc1/2; 127 | getCoefs()[3] = -(-acc2*t2+3*acc1*t2+8*vel2*t+12*vel1*t-20*pos2+20*pos1)/(2*t3); 128 | getCoefs()[4] = (-2*acc2*t2+3*acc1*t2+14*vel2*t+16*vel1*t-30*pos2+30*pos1)/(2*t4); 129 | getCoefs()[5] = -(-acc2*t2+acc1*t2+6*vel2*t+6*vel1*t-12*pos2+12*pos1)/(2*t5); 130 | } 131 | 132 | } 133 | 134 | -------------------------------------------------------------------------------- /inria_maths/src/PoseCommandProcessing.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace inria { 5 | 6 | PoseCommandProcessing::PoseCommandProcessing() : 7 | scalingLin(1.0), 8 | posOffset(Eigen::Vector3d::Zero()), 9 | matOffset(Eigen::Matrix3d::Identity()), 10 | posCentered(Eigen::Vector3d::Zero()), 11 | matCentered(Eigen::Matrix3d::Identity()), 12 | isIntegrating(false), 13 | posAnchor(Eigen::Vector3d::Zero()), 14 | matAnchor(Eigen::Matrix3d::Identity()), 15 | posIntegrated(Eigen::Vector3d::Zero()), 16 | matIntegrated(Eigen::Matrix3d::Identity()) 17 | { 18 | } 19 | 20 | void PoseCommandProcessing::update( 21 | bool isClutch, 22 | const Eigen::Vector3d& posRaw, 23 | const Eigen::Matrix3d& matRaw) 24 | { 25 | //Reset offset when the clutch is not enabled 26 | if (!isClutch) { 27 | posOffset = posRaw; 28 | matOffset = matRaw; 29 | } 30 | //Integrate pose when the clutch is released 31 | if (isIntegrating && !isClutch) { 32 | posAnchor = posIntegrated; 33 | matAnchor = matIntegrated; 34 | } 35 | //Compute offset pose from raw pose. 36 | //The orientation offset is split into the yaw component 37 | //and the remaining roll/pitch components. 38 | Eigen::Vector3d tmpEuler1 = MatrixToEulerIntrinsic(matOffset); 39 | Eigen::Vector3d tmpEuler2 = MatrixToEulerIntrinsic(matOffset); 40 | tmpEuler1.x() = 0.0; 41 | tmpEuler1.y() = 0.0; 42 | tmpEuler2.z() = 0.0; 43 | Eigen::Matrix3d tmpMat1 = EulerIntrinsicToMatrix(tmpEuler1); 44 | Eigen::Matrix3d tmpMat2 = EulerIntrinsicToMatrix(tmpEuler2); 45 | posCentered = 46 | tmpMat1.transpose()*scalingLin*(posRaw - posOffset); 47 | matCentered = 48 | tmpMat1.transpose()*matRaw*tmpMat2.transpose(); 49 | //Compute integrated poses. Reference pose is integrated 50 | //when the clutch is released. Centered pose is added to 51 | //the reference pose in the frame of the reference pose. 52 | if (isIntegrating) { 53 | posIntegrated = posAnchor + matAnchor*posCentered; 54 | matIntegrated = matAnchor*matCentered; 55 | } else { 56 | posIntegrated = posAnchor; 57 | matIntegrated = matAnchor; 58 | } 59 | isIntegrating = isClutch; 60 | } 61 | 62 | } 63 | 64 | -------------------------------------------------------------------------------- /inria_maths/src/TrajectoryBangBangAcc.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | namespace inria { 6 | 7 | TrajectoryBangBangAcc::TrajectoryBangBangAcc( 8 | double posInit, double velInit, 9 | double posEnd, double velEnd, 10 | double maxVel, double maxAcc) : 11 | _timeLength(0.0), 12 | _posInit(posInit), 13 | _posEnd(posEnd), 14 | _isPhaseMode(false), 15 | _velInit(velInit), 16 | _velMiddle(0.0), 17 | _velEnd(velEnd), 18 | _accBegin(0.0), 19 | _accEnd(0.0), 20 | _timeAccBegin(0.0), 21 | _timeMiddle(0.0), 22 | _timeAccEnd(0.0), 23 | _timePoly1(0.0), 24 | _timePoly2(0.0), 25 | _poly1(), 26 | _poly2() 27 | { 28 | //Check limits validity 29 | if (maxVel <= 0.0 || maxAcc <= 0.0) { 30 | throw std::logic_error( 31 | "inria::TrajectoryBangBangAcc: " 32 | "Invalid limits."); 33 | } 34 | //Forbid out of bound starting 35 | //or final velocity 36 | double epsilon = 1e-3; 37 | if ( 38 | std::fabs(velInit) > maxVel+epsilon || 39 | std::fabs(velEnd) > maxVel+epsilon 40 | ) { 41 | throw std::logic_error( 42 | "inria::TrajectoryBangBangAcc: " 43 | "Out of limit velocity."); 44 | } 45 | 46 | //Assign saturated velocity and acceleration/deceleration 47 | //limit depending on motion direction 48 | if (posEnd >= posInit) { 49 | _velMiddle = maxVel; 50 | _accBegin = maxAcc; 51 | _accEnd = -maxAcc; 52 | } else { 53 | _velMiddle = -maxVel; 54 | _accBegin = -maxAcc; 55 | _accEnd = maxAcc; 56 | } 57 | //Compute acceleration and deceleration 58 | //time to/from maximum velocity 59 | _timeAccBegin = (_velMiddle-velInit)/_accBegin; 60 | _timeAccEnd = (velEnd-_velMiddle)/_accEnd; 61 | //Numerical error safety 62 | if (_timeAccBegin < 0.0) { 63 | _timeAccBegin = 0.0; 64 | } 65 | if (_timeAccEnd < 0.0) { 66 | _timeAccEnd = 0.0; 67 | } 68 | 69 | //Compute the distance travelled during 70 | //acceleration and deceleration time 71 | double distAccBegin = std::fabs( 72 | velInit*_timeAccBegin + 73 | 0.5*_accBegin*_timeAccBegin*_timeAccBegin); 74 | double distAccEnd = std::fabs( 75 | _velMiddle*_timeAccEnd + 76 | 0.5*_accEnd*_timeAccEnd*_timeAccEnd); 77 | //Total distance to travel 78 | double distNow = std::fabs(posEnd - posInit); 79 | 80 | if (distNow >= (distAccBegin + distAccEnd)) { 81 | //If the distance to travel is larger and the 82 | //acceleration and deceleration distance, a 3 phases 83 | //trajectory with saturated velocity in the middle 84 | //is used 85 | _isPhaseMode = true; 86 | //Distance travelled at maximum velocity 87 | double distMiddle = distNow - (distAccBegin+distAccEnd); 88 | //Time spend at maximum velocity 89 | _timeMiddle = std::fabs(distMiddle/_velMiddle); 90 | //Compute total trajectory time 91 | _timeLength = _timeAccBegin + _timeMiddle + _timeAccEnd; 92 | } else { 93 | //Else the distance is too short to reach the maximum 94 | //velocity. A two phase acceleration/deceleration phase 95 | //trajectory is optimized for (second order polynomial 96 | //spline with velocity continuity and maximum acceleration). 97 | _isPhaseMode = false; 98 | //Solve for time length of each of the two phases 99 | //(second order equation) to reach desired position and 100 | //velocity using positive then negative maximum acceleration 101 | _timePoly1 = 0.0; 102 | _timePoly2 = 0.0; 103 | double acc = (posEnd >= posInit) ? maxAcc : -maxAcc; 104 | double deltaPositive = 105 | velInit*velInit + 106 | velEnd*velEnd + 107 | 2.0*acc*(posEnd-posInit); 108 | if (deltaPositive < 0.0) { 109 | throw std::logic_error( 110 | "inria::TrajectoryBangBangAcc: " 111 | "Negative discriminant."); 112 | } 113 | double rootTime1 = 114 | (std::sqrt(2.0)*std::sqrt(deltaPositive)-2.0*velInit)/(2.0*acc); 115 | double rootTime2 = 116 | (-std::sqrt(2.0)*std::sqrt(deltaPositive)-2.0*velInit)/(2.0*acc); 117 | //Select the positive time length solution for first phase 118 | _timePoly1 = std::max(rootTime1, rootTime2); 119 | //Compute the second phase time length 120 | _timePoly2 = _timePoly1 - (velEnd-velInit)/acc; 121 | //If the target vel is too hight and the target position is too close, 122 | //it can not be reached directly. The other solution is to go away from 123 | //the target position and then go toward it again. 124 | //Another two phases constant acceleration is computed (same maths) 125 | //but with the acceleration inversed (first decelerate, then accelerate). 126 | if (_timePoly1 < 0.0 || _timePoly2 < 0.0) { 127 | //Compute the same solution with acc = -acc 128 | double deltaNegative = 129 | velInit*velInit + 130 | velEnd*velEnd + 131 | 2.0*(-acc)*(posEnd-posInit); 132 | if (deltaNegative >= 0.0) { 133 | double rootTime3 = 134 | (std::sqrt(2.0)*std::sqrt(deltaNegative)-2.0*velInit)/(-2.0*acc); 135 | double rootTime4 = 136 | (-std::sqrt(2.0)*std::sqrt(deltaNegative)-2.0*velInit)/(-2.0*acc); 137 | //Select the positive time length solution for first phase 138 | double time1 = std::max(rootTime3, rootTime4); 139 | //Compute the second phase time length 140 | double time2 = time1 - (velEnd-velInit)/(-acc); 141 | //If this solution is feasible, then we use it 142 | if (time1 >= 0.0 && time2 >= 0.0) { 143 | _timePoly1 = time1; 144 | _timePoly2 = time2; 145 | acc = -acc; 146 | } 147 | } 148 | } 149 | //Clamp the phase time to zero if previous case has not found a solution 150 | //(the target state is then not reached, but this should not happen...) 151 | _timePoly1 = std::max(_timePoly1, 0.0); 152 | _timePoly2 = std::max(_timePoly2, 0.0); 153 | //Assign total trajectory length 154 | _timeLength = _timePoly1 + _timePoly2; 155 | //Assign the two part polynomials 156 | _poly1 = inria::Polynomial(2); 157 | _poly1(0) = posInit; 158 | _poly1(1) = velInit; 159 | _poly1(2) = 0.5*acc; 160 | _poly2 = inria::Polynomial(2); 161 | _poly2(0) = _poly1.pos(_timePoly1); 162 | _poly2(1) = _poly1.vel(_timePoly1); 163 | _poly2(2) = -0.5*acc; 164 | } 165 | } 166 | 167 | double TrajectoryBangBangAcc::pos(double t) const 168 | { 169 | //Outbound extrapolation 170 | if (t < 0.0) { 171 | return _posInit - _velInit*t; 172 | } 173 | if (t > _timeLength) { 174 | return _posEnd + (t-_timeLength)*_velEnd; 175 | } 176 | 177 | if (_isPhaseMode) { 178 | if (t <= _timeAccBegin) { 179 | return _posInit + _velInit*t + 0.5*_accBegin*t*t; 180 | } else if (t <= _timeAccBegin+_timeMiddle) { 181 | double tt = t - _timeAccBegin; 182 | return _posInit + _velInit*_timeAccBegin 183 | + 0.5*_accBegin*_timeAccBegin*_timeAccBegin 184 | + _velMiddle*tt; 185 | } else { 186 | double tt = t - _timeAccBegin - _timeMiddle; 187 | return _posInit + _velInit*_timeAccBegin 188 | + 0.5*_accBegin*_timeAccBegin*_timeAccBegin 189 | + _velMiddle*_timeMiddle 190 | + _velMiddle*tt + 0.5*_accEnd*tt*tt; 191 | } 192 | } else { 193 | if (t <= _timePoly1) { 194 | return _poly1.pos(t); 195 | } else { 196 | return _poly2.pos(t-_timePoly1); 197 | } 198 | } 199 | } 200 | double TrajectoryBangBangAcc::vel(double t) const 201 | { 202 | //Outbound extrapolation 203 | if (t < 0.0) { 204 | return _velInit; 205 | } 206 | if (t > _timeLength) { 207 | return _velEnd; 208 | } 209 | 210 | if (_isPhaseMode) { 211 | if (t <= _timeAccBegin) { 212 | return _velInit + _accBegin*t; 213 | } else if (t <= _timeAccBegin+_timeMiddle) { 214 | return _velMiddle; 215 | } else { 216 | double tt = t - _timeAccBegin - _timeMiddle; 217 | return _velMiddle + _accEnd*tt; 218 | } 219 | } else { 220 | if (t <= _timePoly1) { 221 | return _poly1.vel(t); 222 | } else { 223 | return _poly2.vel(t-_timePoly1); 224 | } 225 | } 226 | } 227 | double TrajectoryBangBangAcc::acc(double t) const 228 | { 229 | //Outbound extrapolation 230 | if (t < 0.0 || t > _timeLength) { 231 | return 0.0; 232 | } 233 | 234 | if (_isPhaseMode) { 235 | if (t <= _timeAccBegin) { 236 | return _accBegin; 237 | } else if (t <= _timeAccBegin+_timeMiddle) { 238 | return 0.0; 239 | } else { 240 | return _accEnd; 241 | } 242 | } else { 243 | if (t <= _timePoly1) { 244 | return _poly1.acc(t); 245 | } else { 246 | return _poly2.acc(t-_timePoly1); 247 | } 248 | } 249 | } 250 | 251 | double TrajectoryBangBangAcc::min() const 252 | { 253 | return 0.0; 254 | } 255 | double TrajectoryBangBangAcc::max() const 256 | { 257 | return _timeLength; 258 | } 259 | 260 | } 261 | 262 | -------------------------------------------------------------------------------- /inria_model/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.12) 2 | project(inria_model) 3 | 4 | find_package(catkin REQUIRED 5 | urdf 6 | rbdl_catkin 7 | inria_maths 8 | inria_utils 9 | ) 10 | find_package(Eigen3 REQUIRED) 11 | find_package(pinocchio REQUIRED) 12 | 13 | catkin_package( 14 | INCLUDE_DIRS include 15 | LIBRARIES ${PROJECT_NAME} 16 | CATKIN_DEPENDS 17 | urdf 18 | rbdl_catkin 19 | eiquadprog 20 | inria_maths 21 | inria_utils 22 | ) 23 | 24 | include_directories( 25 | include 26 | ${catkin_INCLUDE_DIRS} 27 | ${EIGEN3_INCLUDE_DIR} 28 | ${pinocchio_INCLUDE_DIRS} 29 | ) 30 | link_directories( 31 | ${CMAKE_INSTALL_PREFIX}/lib/ 32 | ) 33 | 34 | add_compile_options(-std=c++17 -Wno-invalid-partial-specialization) 35 | 36 | add_library(${PROJECT_NAME} 37 | src/Model.cpp 38 | src/RBDLRootUpdate.cpp 39 | src/PinocchioInterface.cpp 40 | src/SEIKORetargeting.cpp 41 | src/SEIKOWrapper.cpp 42 | src/SEIKOTalos.cpp 43 | src/SEIKOController.cpp 44 | ) 45 | target_link_libraries(${PROJECT_NAME} 46 | ${catkin_LIBRARIES} 47 | ${pinocchio_LIBRARIES} 48 | ) 49 | 50 | install(TARGETS ${PROJECT_NAME} DESTINATION lib) 51 | install(DIRECTORY include/ DESTINATION include) 52 | 53 | -------------------------------------------------------------------------------- /inria_model/include/inria_model/RBDLRootUpdate.h: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_MODEL_RBDLROOTUPDATE_H 2 | #define INRIA_MODEL_RBDLROOTUPDATE_H 3 | 4 | #include 5 | #include 6 | 7 | namespace inria { 8 | 9 | /** 10 | * Build a new RBDL model tree 11 | * copied from given one and 12 | * update the tree root. 13 | * 14 | * @param modelOld RBDL model copied from. 15 | * modelOld is not updated. 16 | * The given model should not already 17 | * contains a floating base. 18 | * @param newRootBodyId RBDL body Id 19 | * of the new root given within modelOld. 20 | * @param addFloatingBase If true, 21 | * a 6 DoFs floating base (position 22 | * and spherical joint using quaternion format) 23 | * is added between world origin and tree root. 24 | * @param indexesNewToOld Build the mapping 25 | * from new joint indexes in modelNew to old 26 | * ones in modelOld. 27 | * @return a new RBDL model where 28 | * newRootBodyId is the new tree 29 | * root after the optional floating base. 30 | */ 31 | RigidBodyDynamics::Model RBDLRootUpdate( 32 | RigidBodyDynamics::Model& modelOld, 33 | size_t newRootBodyId, 34 | bool addFloatingBase, 35 | std::vector& indexesNewToOld); 36 | 37 | } 38 | 39 | #endif 40 | 41 | -------------------------------------------------------------------------------- /inria_model/include/inria_model/SEIKOController.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_MODEL_SEIKOCONTROLLER_HPP 2 | #define INRIA_MODEL_SEIKOCONTROLLER_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace inria { 12 | 13 | /** 14 | * SEIKOController 15 | * 16 | * Sequential Equilibrium Inverse Kinematic Optimization 17 | * for whole body admittance control on position controlled system 18 | * using explicit flexibility joint model. 19 | */ 20 | class SEIKOController 21 | { 22 | public: 23 | 24 | /** 25 | * Structure for point (3d) and plane (6d) contacts. 26 | * WARNING: For point contact, all 3d forces 27 | * are expressed in the contact frame defined by contactMat. 28 | * Contact frame is defined with respect to world frame and is assumed 29 | * fixed as long as the contact is enabled. 30 | */ 31 | struct ContactPoint_t { 32 | /** 33 | * Configuration 34 | */ 35 | //Cartesian frame id and name 36 | size_t frameId; 37 | std::string frameName; 38 | //Weight for force change target 39 | Eigen::Vector3d weightForce; 40 | //Weight for effector position task when disabled 41 | double weightPos; 42 | //Weight for effector orientation when enabled 43 | double weightMat; 44 | /** 45 | * Input 46 | */ 47 | //If true, force is assumed 48 | //to be applied on this contact 49 | bool isEnabled; 50 | //Contact surface orientation expressed in world 51 | //frame (aligning Z axis as surface normal) 52 | Eigen::Matrix3d contactMat; 53 | //Desired contact force change 54 | Eigen::Vector3d targetDeltaForce; 55 | /** 56 | * State 57 | */ 58 | //Integrated estimated force solution 59 | Eigen::Vector3d stateForce; 60 | /** 61 | * Cache 62 | */ 63 | //Cache desired Cartesian pose 64 | Eigen::Vector3d targetPos; 65 | Eigen::Matrix3d targetMat; 66 | //Cache Jacobian computed with corrected model 67 | Eigen::MatrixXd jacWorld; 68 | //Cache last computed force change 69 | Eigen::Vector3d solutionForce; 70 | }; 71 | struct ContactPlane_t { 72 | /** 73 | * Configuration 74 | */ 75 | //Cartesian frame id and name 76 | size_t frameId; 77 | std::string frameName; 78 | //Weight for wrench change target 79 | Eigen::Vector6d weightWrench; 80 | //Weight for effector pose task when disabled 81 | double weightPos; 82 | double weightMat; 83 | /** 84 | * Input 85 | */ 86 | //If true, wrench is assumed 87 | //to be applied on this contact 88 | bool isEnabled; 89 | //Desired contact wrench change 90 | Eigen::Vector6d targetDeltaWrench; 91 | /** 92 | * State 93 | */ 94 | //Integrated estimated wrench solution 95 | Eigen::Vector6d stateWrench; 96 | /** 97 | * Cache 98 | */ 99 | //Cache desired Cartesian pose 100 | Eigen::Vector3d targetPos; 101 | Eigen::Matrix3d targetMat; 102 | //Cache Jacobian computed with corrected model 103 | Eigen::MatrixXd jacBody; 104 | Eigen::MatrixXd jacWorld; 105 | //Cache last computed wrench change 106 | Eigen::Vector6d solutionWrench; 107 | }; 108 | /** 109 | * Structure for posture and joint quantities. 110 | */ 111 | struct Configuration_t { 112 | /** 113 | * Configuration 114 | */ 115 | //If false, state and command integration is disabled 116 | //(only for runSEIKO, not runInit) 117 | bool isIntegrating; 118 | //Gain applied on disabled effectors pose task delta 119 | double gainPose; 120 | //Penalty weight on floating base change regularization 121 | double weightRegBase; 122 | //Weight on joint elastic energy minimization 123 | double weightElasticEnergy; 124 | //Penalty weight on joint command change regularization (sizeJoint()) 125 | Eigen::VectorXd weightRegCmdChange; 126 | //Penalty weight on joint command offset regularization (sizeJoint()) 127 | Eigen::VectorXd weightRegCmdOffset; 128 | //Joint stiffness default gain to convert from 129 | //joint position to joint torque (sizeJoint()) 130 | Eigen::VectorXd stiffnessJoint; 131 | //Joint lower and upper angle position limits (sizeJoint()) 132 | Eigen::VectorXd limitPosLowerJoint; 133 | Eigen::VectorXd limitPosUpperJoint; 134 | //Joint absolute position command offset limit 135 | double limitPosCmdOffset; 136 | //Joint absolute torque limits (sizeJoint()) 137 | Eigen::VectorXd limitTauJoint; 138 | //Ratio margin for stiffness joint estimation limits 139 | double limitRatioStiffness; 140 | /** 141 | * Input 142 | */ 143 | //Desired posture from SEIKO is given by the model 144 | /** 145 | * State 146 | */ 147 | //Integrated estimated posture solution 148 | //under flexibility (sizeVectPos()) 149 | Eigen::VectorXd statePosture; 150 | //Integrated joint position offset on command (sizeJoint()) 151 | Eigen::VectorXd cmdOffsetJoint; 152 | //Integrated joint stiffness gain (sizeJoint()) 153 | Eigen::VectorXd stateStiffnessJoint; 154 | //Integrated additional base external wrench ([lin, ang]) 155 | Eigen::Vector6d stateExternalWrench; 156 | /** 157 | * Cache 158 | */ 159 | //Floating base and joint position desired command 160 | //retrieved from model (sizeVectPos()) 161 | Eigen::VectorXd targetPosture; 162 | //Last computed posture change solution (sizeVectVel()) 163 | Eigen::VectorXd solutionPosture; 164 | //Last computed command position offset change (sizeJoint()) 165 | Eigen::VectorXd solutionCommand; 166 | //Last computed resulting joint command with 167 | //desired position added to command offset 168 | Eigen::VectorXd commandJoint; 169 | //Last computed state joint torque 170 | Eigen::VectorXd torqueJoint; 171 | //Last computed joint stiffness gain change (sizeJoint()) 172 | Eigen::VectorXd solutionStiffnessJoint; 173 | //Last computed base external wrench change ([lin, ang]) 174 | Eigen::Vector6d solutionExternalWrench; 175 | //Last computed bias term norms for floating base 176 | //and joint parts of the equilibrium equation 177 | double biasBase; 178 | double biasJoint; 179 | }; 180 | 181 | /** 182 | * Empty initialization. 183 | */ 184 | SEIKOController(); 185 | 186 | /** 187 | * Initialization with a Model. 188 | * The model instance must be valid during 189 | * the whole use of SEIKOController. 190 | * 191 | * @param model Reference to the model 192 | * containing the desired posture previously computed 193 | * by multi-contact SEIKO. 194 | */ 195 | SEIKOController(Model& model); 196 | 197 | /** 198 | * Define a new potential contact plane 199 | * (6-DOFs) or contact point (3-DOFs) 200 | * with default configuration. 201 | * 202 | * @param frameName Model frame name in kinematics tree. 203 | */ 204 | void addContactPoint(const std::string& frameName); 205 | void addContactPlane(const std::string& frameName); 206 | 207 | /** 208 | * Reset controller internal state 209 | */ 210 | void reset(); 211 | 212 | /** 213 | * Set given effector as enabled 214 | * @param frameName Model frame name of previously defined contact. 215 | */ 216 | void askSwitchingEnable(const std::string& frameName); 217 | 218 | /** 219 | * Return read write reference toward point or plane 220 | * structure from its contact name 221 | * @param frameName Model frame name of previously defined contact. 222 | */ 223 | const ContactPoint_t& getPoint(const std::string& frameName) const; 224 | ContactPoint_t& getPoint(const std::string& frameName); 225 | const ContactPlane_t& getPlane(const std::string& frameName) const; 226 | ContactPlane_t& getPlane(const std::string& frameName); 227 | 228 | /** 229 | * Return read write reference toward configuration with 230 | * posture and joint structure 231 | */ 232 | const Configuration_t& getConfiguration() const; 233 | Configuration_t& getConfiguration(); 234 | 235 | /** 236 | * Solve the quadratic program of the SQP computing 237 | * the flexibility posture from given fixed joint command. 238 | * Used parameters: 239 | * weightRegBase, weightElasticEnergy, weightForce, weightWrench. 240 | * 241 | * @return true if the QP succeed, else 242 | * false if no feasible solution is found. 243 | */ 244 | bool runInit(); 245 | 246 | /** 247 | * Solve the quadratic program 248 | * using reduced fast formulation. 249 | * Used parameters: 250 | * weightRegBase, weightRegCmdChange, weightRegCmdOffset, 251 | * weightForce, weightWrench. 252 | * 253 | * @param dt Time step in seconds. 254 | * @return true if the QP succeed, else 255 | * false if no feasible solution is found. 256 | */ 257 | bool runSEIKO(double dt); 258 | 259 | private: 260 | 261 | /** 262 | * Pointer to model instance 263 | * containing desired posture 264 | */ 265 | Model* _model; 266 | 267 | /** 268 | * Pinocchio model interface instance 269 | */ 270 | PinocchioInterface _pinocchio; 271 | 272 | /** 273 | * Container for defined Cartesian 274 | * contact planes and points and 275 | * mapping from frame name to index 276 | */ 277 | std::vector _contactPoint; 278 | std::vector _contactPlane; 279 | std::map _mappingPoint; 280 | std::map _mappingPlane; 281 | 282 | /** 283 | * Posture and joint configuration 284 | */ 285 | Configuration_t _configuration; 286 | }; 287 | 288 | } 289 | 290 | #endif 291 | 292 | -------------------------------------------------------------------------------- /inria_model/include/inria_model/SEIKOTalos.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_MODEL_SEIKOTALOS_HPP 2 | #define INRIA_MODEL_SEIKOTALOS_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace inria { 8 | 9 | /** 10 | * SEIKOTalos 11 | * 12 | * Sequential Equilibrium Inverse Kinematic Optimization 13 | * configured and specialized for Talos robot. 14 | */ 15 | class SEIKOTalos : public SEIKOWrapper 16 | { 17 | public: 18 | 19 | /** 20 | * SEIKO parameters 21 | */ 22 | double jointPosMargin = DegToRad(15.0); 23 | double jointVelLimit = 0.2; 24 | double jointTauLimitRatio = 0.65; 25 | double jointWeightPos = 10.0; 26 | double jointWeightVel = 1000.0; 27 | double jointWeightTauArms = 0.005; 28 | double jointWeightTauOthers = 0.00002; 29 | double jointClampPos = DegToRad(10.0); 30 | double weightPosFoot = 1e5; 31 | double weightPosHand = 5e3; 32 | double weightMatFoot = 1e4; 33 | double weightMatHand = 1e2; 34 | double weightWrenchEnabled = 0.002; 35 | double weightForceEnabled = 0.005; 36 | double weightWrenchDisabling = 1e4; 37 | double weightForceDisabling = 1e4; 38 | double weightForcePushMode = 1e4; 39 | double forceMinFoot = 100.0; 40 | double forceMaxFoot = 1000.0; 41 | double forceMinHand = 10.0; 42 | double forceMaxHand = 150.0; 43 | double limitVelForceFoot = 60.0; 44 | double limitVelTorqueFoot = 0.04*60.0; 45 | double limitVelForceHand = 10.0; 46 | double frictionCoef = 0.6; 47 | double clampPos = 0.04; 48 | double clampMat = DegToRad(10.0); 49 | double limitCOPX = 0.04; 50 | double limitCOPY = 0.03; 51 | double weightIneqPos = 1e2; 52 | double weightIneqVel = 1e2; 53 | double weightIneqForce = 1e2; 54 | double weightIneqID = 1.0; 55 | 56 | /** 57 | * Pose filter parameters 58 | */ 59 | bool useRawVRPose = true; 60 | bool isLocalFrame = false; 61 | double scalingLin = 1.0; 62 | double clampRadiusLin = 0.03; 63 | double clampRadiusAng = 0.2; 64 | double cutoffFreq = 2.0; 65 | double maxVelLin = 0.2; 66 | double maxAccLin = 2.0; 67 | double maxVelAng = 1.0; 68 | double maxAccAng = 3.0; 69 | 70 | /** 71 | * Force velocity command parameters 72 | */ 73 | double clampRadiusForce = 5.0; 74 | double maxVelForce = limitVelForceHand; 75 | double maxAccForce = 10.0*limitVelForceHand; 76 | 77 | public: 78 | 79 | /** 80 | * Constructor inheritance 81 | */ 82 | using SEIKOWrapper::SEIKOWrapper; 83 | 84 | /** 85 | * Define and setup contact frames. 86 | * Must be called only once. 87 | */ 88 | void setup(); 89 | 90 | /** 91 | * Reset SEIKO and internal state 92 | * to default. 93 | * 94 | * @return true if the reset static ID is successful. 95 | */ 96 | bool reset(); 97 | 98 | /** 99 | * Read access to frame names for end-effectors 100 | */ 101 | const std::string& nameFrameFootLeft() const; 102 | const std::string& nameFrameFootRight() const; 103 | const std::string& nameFrameHandLeft() const; 104 | const std::string& nameFrameHandRight() const; 105 | 106 | private: 107 | 108 | /** 109 | * Feet and hands contact names 110 | */ 111 | std::string _nameFootLeft; 112 | std::string _nameFootRight; 113 | std::string _nameHandLeft; 114 | std::string _nameHandRight; 115 | }; 116 | 117 | } 118 | 119 | #endif 120 | 121 | -------------------------------------------------------------------------------- /inria_model/include/inria_model/SEIKOWrapper.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_MODEL_SEIKOWRAPPER_HPP 2 | #define INRIA_MODEL_SEIKOWRAPPER_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace inria { 12 | 13 | /** 14 | * SEIKOWrapper 15 | * 16 | * Sequential Equilibrium Inverse Kinematic Optimization 17 | * wrapper with contact switching and target filtering features 18 | */ 19 | class SEIKOWrapper : public SEIKORetargeting 20 | { 21 | public: 22 | 23 | /** 24 | * Structure for end-effector command state 25 | */ 26 | struct EndEffector_t { 27 | //Frame name 28 | std::string name; 29 | //If true, the contact is a point contact, 30 | //else a plane contact 31 | bool isPoint; 32 | //If true, the contact is trying to be disabled 33 | bool isSwitchingDisable; 34 | //If true, the contact wrench or force is servo to target 35 | bool isPushMode; 36 | //Desired normal force limits configuration. 37 | //Actual limits are changed to allow contact switching. 38 | double limitNormalForceMin; 39 | double limitNormalForceMax; 40 | //Config contact force or wrench weights when 41 | //the contact is enabled and during disabling 42 | //and during push force command mode 43 | double weightForceEnabled; 44 | double weightForceDisabling; 45 | double weightForcePushMode; 46 | //Last provided raw pose commands 47 | bool isClutch; 48 | Eigen::Vector3d rawPos; 49 | Eigen::Matrix3d rawMat; 50 | //Last provided velocity commands 51 | Eigen::Vector3d velLin; 52 | Eigen::Vector3d velAng; 53 | //Last provided normal force velocity command 54 | double velForceNormal; 55 | //Command filter 56 | FilterPoseCommand filterPose; 57 | FilterVelCommand filterForce; 58 | //Torque and force target for push mode 59 | Eigen::Vector3d targetTorque; 60 | Eigen::Vector3d targetForce; 61 | }; 62 | 63 | /** 64 | * Constructor inheritance 65 | */ 66 | using SEIKORetargeting::SEIKORetargeting; 67 | 68 | /** 69 | * Define and setup contact frames. 70 | * Must be called only once. 71 | */ 72 | void setup(); 73 | 74 | /** 75 | * Do not use internal pose command filtering. 76 | * SEIKO targets must be externally provided. 77 | * 78 | * @param noFiltersAndCommands If true, filters and commands 79 | * are disabled. If false, they are enabled back. 80 | */ 81 | void disableFiltersAndCommands(bool noFiltersAndCommands); 82 | 83 | /** 84 | * Reset SEIKO and internal state 85 | * to default. 86 | * 87 | * @return true if the reset static ID is successful. 88 | */ 89 | bool reset(); 90 | 91 | /** 92 | * Set raw input pose command for end-effector 93 | * 94 | * @param frameName Defined contact frame name. 95 | * @param isClutch If true, pose motion integration 96 | * is enabled. Anchor coordinate frame is reset and offset 97 | * when clutch is enabled. 98 | * @param posRaw Absolute raw position with A as up. 99 | * @param matRaw Absolute raw orientation with Z as up. 100 | */ 101 | void setTargetPose( 102 | const std::string& frameName, 103 | bool isClutch, 104 | const Eigen::Vector3d& rawPos, 105 | const Eigen::Matrix3d& rawMat); 106 | 107 | /** 108 | * Set velocity pose command for end-effector 109 | * 110 | * @param frameName Defined contact frame name. 111 | * @param velLin Linear velocity command. 112 | * @param velAng Angular velocity command. 113 | */ 114 | void setTargetVel( 115 | const std::string& frameName, 116 | const Eigen::Vector3d& velLin, 117 | const Eigen::Vector3d& velAng); 118 | 119 | /** 120 | * Set or unset push mode 121 | * (control target wrench or force). 122 | * 123 | * @param frameName Defined contact frame name. 124 | * @param isEnabled If true, push mode is enabled. 125 | */ 126 | void setPushMode( 127 | const std::string& frameName, 128 | bool isEnabled); 129 | 130 | /** 131 | * Set normal force velocity command. 132 | * Only used in push mode. 133 | * 134 | * @param frameName Defined contact frame name. 135 | * @param vel Velocity command. 136 | */ 137 | void setForceNormalVel( 138 | const std::string& frameName, 139 | double vel); 140 | 141 | /** 142 | * Direct access to torque and force targets for push mode. 143 | * 144 | * @param frameName Defined contact frame name. 145 | * @return access to target. 146 | */ 147 | Eigen::Vector3d& refTargetTorque( 148 | const std::string& frameName); 149 | Eigen::Vector3d& refTargetForce( 150 | const std::string& frameName); 151 | 152 | /** 153 | * Ask for contact switching procedure. 154 | * 155 | * @param frameName Defined contact frame name. 156 | * @param isEnabled True if the contact should be 157 | * enabled, false if it should be disabled. 158 | */ 159 | void askSwitching( 160 | const std::string& frameName, 161 | bool isEnabled); 162 | 163 | /** 164 | * Update command filters, update switching procedure, 165 | * compute a SEIKO step, integrate the resulting deltas, 166 | * update the underlying model and compute ahead commands. 167 | * 168 | * @param dt_task SEIKO task time step. 169 | * @param dt_ahead Time step to compute ahead command. 170 | * @return true if SEIKO QP is successful. 171 | */ 172 | bool update(double dt_task, double dt_ahead); 173 | 174 | /** 175 | * @return read access to last computed 176 | * ahead joint position vector. 177 | */ 178 | const Eigen::VectorXd& getAheadJointPos() const; 179 | 180 | /** 181 | * Read only access to effector command structure. 182 | * @param frameName Defined contact frame name. 183 | * @return read only reference to effector structure. 184 | */ 185 | const EndEffector_t& stateEffector( 186 | const std::string& frameName) const; 187 | 188 | /** 189 | * Check if a contact is switching. 190 | * @param frameName Defined contact frame name. 191 | * @return true if either the contact is currently 192 | * enabling or disabling. 193 | */ 194 | bool stateIsContactSwitching( 195 | const std::string& frameName) const; 196 | 197 | protected: 198 | 199 | /** 200 | * Map containing command state for all 201 | * declared end-effectors 202 | */ 203 | std::map _commands; 204 | 205 | /** 206 | * Specific coefficient to reduce angular 207 | * limit margin on some joints 208 | */ 209 | Eigen::VectorXd _jointPosMarginCoef; 210 | 211 | /** 212 | * Normalized weight vectors for forces and wrenches 213 | */ 214 | Eigen::Vector6d _vectWeightWrench; 215 | Eigen::Vector3d _vectWeightForce; 216 | 217 | /** 218 | * Smoothing initialization ratio in [0:1] 219 | * applied on computed velocity after reset 220 | */ 221 | double _initRatio; 222 | 223 | /** 224 | * If true, all filters and command features are 225 | * disabled. SEIKO pose targets are assumed to 226 | * be externally provided. 227 | */ 228 | bool _noFiltersAndCommands; 229 | 230 | /** 231 | * Last computed joint position vector 232 | * for ahead time step from SEIKO output 233 | */ 234 | Eigen::VectorXd _aheadJointPos; 235 | 236 | /** 237 | * Define a contact to underlying SEIKO and wrapper 238 | * @param nameFrame URDF frame name to configure as contact. 239 | * @param isPoint If true the contact is point, else is plane. 240 | */ 241 | void addContact( 242 | const std::string nameFrame, 243 | bool isPoint); 244 | 245 | /** 246 | * Update and return the position vector 247 | * ahead from last SEIKO computed output. 248 | * 249 | * @param dt Ahead time step from last SEIKO state. 250 | * @return position vector of size sizeJoint(). 251 | */ 252 | void computeAheadJointPos(double dt); 253 | }; 254 | 255 | } 256 | 257 | #endif 258 | 259 | -------------------------------------------------------------------------------- /inria_model/include/inria_model/TalosDOFs.h: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_MODEL_TALOSDOFS_H 2 | #define INRIA_MODEL_TALOSDOFS_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace inria { 9 | 10 | static const std::map PostureDefault = { 11 | {"leg_left_1_joint", 0.0}, 12 | {"leg_left_2_joint", 0.0}, 13 | {"leg_left_3_joint", -0.317392}, 14 | {"leg_left_4_joint", 0.610042}, 15 | {"leg_left_5_joint", -0.29265}, 16 | {"leg_left_6_joint", 0.0}, 17 | {"leg_right_1_joint", 0.0}, 18 | {"leg_right_2_joint", 0.0}, 19 | {"leg_right_3_joint", -0.317392}, 20 | {"leg_right_4_joint", 0.610042}, 21 | {"leg_right_5_joint", -0.29265}, 22 | {"leg_right_6_joint", 0.0}, 23 | {"torso_1_joint", 0.0}, 24 | {"torso_2_joint", 0.0}, 25 | {"arm_left_1_joint", 0.218615}, 26 | {"arm_left_2_joint", 0.566335}, 27 | {"arm_left_3_joint", -0.582134}, 28 | {"arm_left_4_joint", -1.4169}, 29 | {"arm_left_5_joint", 0.315339}, 30 | {"arm_left_6_joint", 0.0615923}, 31 | {"arm_left_7_joint", -0.146577}, 32 | {"gripper_left_inner_double_joint", -2e-1}, 33 | {"gripper_left_fingertip_1_joint", 2e-1}, 34 | {"gripper_left_fingertip_2_joint", 2e-1}, 35 | {"gripper_left_inner_single_joint", 2e-1}, 36 | {"gripper_left_fingertip_3_joint", 2e-1}, 37 | {"gripper_left_joint", -0.05}, 38 | {"gripper_left_motor_single_joint", 2e-1}, 39 | {"arm_right_1_joint", -0.218505}, 40 | {"arm_right_2_joint", -0.566321}, 41 | {"arm_right_3_joint", 0.582227}, 42 | {"arm_right_4_joint", -1.41689}, 43 | {"arm_right_5_joint", -0.315343}, 44 | {"arm_right_6_joint", -0.0617214}, 45 | {"arm_right_7_joint", -0.146588}, 46 | {"gripper_right_inner_double_joint", -2e-1}, 47 | {"gripper_right_fingertip_1_joint", 2e-1}, 48 | {"gripper_right_fingertip_2_joint", 2e-1}, 49 | {"gripper_right_inner_single_joint", 2e-1}, 50 | {"gripper_right_fingertip_3_joint", 2e-1}, 51 | {"gripper_right_joint", -0.05}, 52 | {"gripper_right_motor_single_joint", 2e-1}, 53 | {"head_1_joint", 0.0}, 54 | {"head_2_joint", 0.0}, 55 | 56 | }; 57 | 58 | static const std::map StiffnessJoint = { 59 | {"head_1_joint", 300.0}, 60 | {"head_2_joint", 300.0}, 61 | {"torso_1_joint", 10000.0}, 62 | {"torso_2_joint", 10000.0}, 63 | {"arm_left_1_joint", 10000.0}, 64 | {"arm_left_2_joint", 10000.0}, 65 | {"arm_left_3_joint", 5000.0}, 66 | {"arm_left_4_joint", 10000.0}, 67 | {"arm_left_5_joint", 3000.0}, 68 | {"arm_left_6_joint", 3000.0}, 69 | {"arm_left_7_joint", 3000.0}, 70 | {"arm_right_1_joint", 10000.0}, 71 | {"arm_right_2_joint", 10000.0}, 72 | {"arm_right_3_joint", 5000.0}, 73 | {"arm_right_4_joint", 10000.0}, 74 | {"arm_right_5_joint", 3000.0}, 75 | {"arm_right_6_joint", 3000.0}, 76 | {"arm_right_7_joint", 3000.0}, 77 | {"leg_left_1_joint", 5000.0}, 78 | {"leg_left_2_joint", 5000.0}, 79 | {"leg_left_3_joint", 5000.0}, 80 | {"leg_left_4_joint", 5000.0}, 81 | {"leg_left_5_joint", 5000.0}, 82 | {"leg_left_6_joint", 5000.0}, 83 | {"leg_right_1_joint", 5000.0}, 84 | {"leg_right_2_joint", 5000.0}, 85 | {"leg_right_3_joint", 5000.0}, 86 | {"leg_right_4_joint", 5000.0}, 87 | {"leg_right_5_joint", 5000.0}, 88 | {"leg_right_6_joint", 5000.0}, 89 | {"gripper_left_joint", 1000.0}, 90 | {"gripper_left_inner_double_joint", 20.0}, 91 | {"gripper_left_fingertip_1_joint", 20.0}, 92 | {"gripper_left_fingertip_2_joint", 20.0}, 93 | {"gripper_left_motor_single_joint", 20.0}, 94 | {"gripper_left_fingertip_3_joint", 20.0}, 95 | {"gripper_left_inner_single_joint", 20.0}, 96 | {"gripper_right_joint", 1000.0}, 97 | {"gripper_right_inner_double_joint", 20.0}, 98 | {"gripper_right_fingertip_1_joint", 20.0}, 99 | {"gripper_right_fingertip_2_joint", 20.0}, 100 | {"gripper_right_motor_single_joint", 20.0}, 101 | {"gripper_right_fingertip_3_joint", 20.0}, 102 | {"gripper_right_inner_single_joint", 20.0}, 103 | }; 104 | 105 | static const double FootHalfSizeX = 0.20/2.0; 106 | static const double FootHalfSizeY = 0.13/2.0; 107 | 108 | } 109 | 110 | #endif 111 | 112 | -------------------------------------------------------------------------------- /inria_model/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | inria_model 4 | 0.0.0 5 | 6 | Wrapper around RBDL and Pinocchio model API along with 7 | model based computation and optimization 8 | 9 | Quentin Rouxel 10 | BSD 11 | 12 | catkin 13 | urdf 14 | rbdl_catkin 15 | pinocchio 16 | eiquadprog 17 | inria_maths 18 | inria_utils 19 | 20 | -------------------------------------------------------------------------------- /inria_model/src/SEIKOTalos.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace inria { 5 | 6 | void SEIKOTalos::setup() 7 | { 8 | //Frame names 9 | _nameFootLeft = "left_sole_link"; 10 | _nameFootRight = "right_sole_link"; 11 | _nameHandLeft = "left_hand_frame"; 12 | _nameHandRight = "right_hand_frame"; 13 | 14 | //Setup contacts 15 | this->addContact(_nameFootLeft, false); 16 | this->addContact(_nameFootRight, false); 17 | this->addContact(_nameHandLeft, true); 18 | this->addContact(_nameHandRight, true); 19 | 20 | //Call to base setup 21 | this->SEIKOWrapper::setup(); 22 | 23 | //Specific configuration for joint angular limit margin 24 | _jointPosMarginCoef.setOnes(); 25 | if (this->_model->getMappingDOFs().count("head_1_joint") != 0) { 26 | _jointPosMarginCoef(this->_model->getIndexJoint("head_1_joint")) = 0.1; 27 | } 28 | if (this->_model->getMappingDOFs().count("head_2_joint") != 0) { 29 | _jointPosMarginCoef(this->_model->getIndexJoint("head_2_joint")) = 0.1; 30 | } 31 | if (this->_model->getMappingDOFs().count("torso_2_joint") != 0) { 32 | _jointPosMarginCoef(this->_model->getIndexJoint("torso_2_joint")) = 0.7; 33 | } 34 | } 35 | 36 | bool SEIKOTalos::reset() 37 | { 38 | //Assign SEIKO and filters configuration 39 | 40 | //Contact wrench and force weight vectors 41 | //normalization. Put less (regularization) weight 42 | //on normal forces. 43 | _vectWeightWrench(0) = 1.0; 44 | _vectWeightWrench(1) = 1.0; 45 | _vectWeightWrench(2) = 1.0; 46 | _vectWeightWrench(3) = 1.0; 47 | _vectWeightWrench(4) = 1.0; 48 | _vectWeightWrench(5) = 0.01; 49 | _vectWeightForce(0) = 1.0; 50 | _vectWeightForce(1) = 1.0; 51 | _vectWeightForce(2) = 0.01; 52 | 53 | //Centroidal momentum regularization 54 | this->setMomentumWeight( 55 | 1e-6*Eigen::Vector3d::Ones(), 1e-6*Eigen::Vector3d::Ones()); 56 | 57 | //Joint parameters 58 | this->setJointLimitPos( 59 | this->_model->jointLimitsLower() + jointPosMargin*_jointPosMarginCoef, 60 | this->_model->jointLimitsUpper() - jointPosMargin*_jointPosMarginCoef); 61 | this->setJointLimitVelAbs( 62 | jointVelLimit*this->_model->jointLimitsVelocity()); 63 | this->setJointLimitTauAbs( 64 | jointTauLimitRatio*this->_model->jointLimitsTorque()); 65 | this->setJointWeightPos( 66 | jointWeightPos*Eigen::VectorXd::Ones(this->_sizeJoint)); 67 | this->setJointWeightVel( 68 | jointWeightVel*Eigen::VectorXd::Ones(this->_sizeJoint)); 69 | this->setJointWeightTau( 70 | jointWeightTauOthers*Eigen::VectorXd::Ones(this->_sizeJoint)); 71 | this->setJointClampPos( 72 | jointClampPos); 73 | 74 | //Set joint tau regularization weight 75 | Eigen::VectorXd vectJointWeightTau = jointWeightTauOthers*Eigen::VectorXd::Ones(this->_sizeJoint); 76 | vectJointWeightTau(this->_model->getIndexJoint("arm_left_1_joint")) = jointWeightTauArms; 77 | vectJointWeightTau(this->_model->getIndexJoint("arm_left_2_joint")) = jointWeightTauArms; 78 | vectJointWeightTau(this->_model->getIndexJoint("arm_left_3_joint")) = jointWeightTauArms; 79 | vectJointWeightTau(this->_model->getIndexJoint("arm_left_4_joint")) = jointWeightTauArms; 80 | vectJointWeightTau(this->_model->getIndexJoint("arm_left_5_joint")) = jointWeightTauArms; 81 | vectJointWeightTau(this->_model->getIndexJoint("arm_left_6_joint")) = jointWeightTauArms; 82 | vectJointWeightTau(this->_model->getIndexJoint("arm_left_7_joint")) = jointWeightTauArms; 83 | vectJointWeightTau(this->_model->getIndexJoint("arm_right_1_joint")) = jointWeightTauArms; 84 | vectJointWeightTau(this->_model->getIndexJoint("arm_right_2_joint")) = jointWeightTauArms; 85 | vectJointWeightTau(this->_model->getIndexJoint("arm_right_3_joint")) = jointWeightTauArms; 86 | vectJointWeightTau(this->_model->getIndexJoint("arm_right_4_joint")) = jointWeightTauArms; 87 | this->setJointWeightTau(vectJointWeightTau); 88 | 89 | //Set inequality weights 90 | this->setWeightIneqPos(weightIneqPos); 91 | this->setWeightIneqVel(weightIneqVel); 92 | this->setWeightIneqForce(weightIneqForce); 93 | this->setWeightIneqID(weightIneqID); 94 | 95 | //Cartesian pose parameters 96 | this->setWeightPose(_nameFootLeft, 97 | weightPosFoot, weightMatFoot); 98 | this->setWeightPose(_nameFootRight, 99 | weightPosFoot, weightMatFoot); 100 | this->setWeightPose(_nameHandLeft, 101 | weightPosHand, weightMatHand); 102 | this->setWeightPose(_nameHandRight, 103 | weightPosHand, weightMatHand); 104 | this->setClampPose(_nameFootLeft, 105 | clampPos, clampMat); 106 | this->setClampPose(_nameFootRight, 107 | clampPos, clampMat); 108 | this->setClampPose(_nameHandLeft, 109 | clampPos, clampMat); 110 | this->setClampPose(_nameHandRight, 111 | clampPos, clampMat); 112 | this->setContactLimitFriction(_nameFootLeft, 113 | frictionCoef); 114 | this->setContactLimitFriction(_nameFootRight, 115 | frictionCoef); 116 | this->setContactLimitFriction(_nameHandLeft, 117 | frictionCoef); 118 | this->setContactLimitFriction(_nameHandRight, 119 | frictionCoef); 120 | this->setNormalForceLimits(_nameFootLeft, 121 | forceMinFoot, forceMaxFoot); 122 | this->setNormalForceLimits(_nameFootRight, 123 | forceMinFoot, forceMaxFoot); 124 | this->setNormalForceLimits(_nameHandLeft, 125 | forceMinHand, forceMaxHand); 126 | this->setNormalForceLimits(_nameHandRight, 127 | forceMinHand, forceMaxHand); 128 | this->setLimitVelTorque(_nameFootLeft, 129 | limitVelTorqueFoot); 130 | this->setLimitVelTorque(_nameFootRight, 131 | limitVelTorqueFoot); 132 | this->setLimitVelForce(_nameFootLeft, 133 | limitVelForceFoot); 134 | this->setLimitVelForce(_nameFootRight, 135 | limitVelForceFoot); 136 | this->setLimitVelForce(_nameHandLeft, 137 | limitVelForceHand); 138 | this->setLimitVelForce(_nameHandRight, 139 | limitVelForceHand); 140 | 141 | //Plane contact parameters 142 | this->setContactPlaneLimitCOP(_nameFootLeft, 143 | limitCOPX, limitCOPY); 144 | this->setContactPlaneLimitCOP(_nameFootRight, 145 | limitCOPX, limitCOPY); 146 | this->setWeightWrench(_nameFootLeft, 147 | weightWrenchEnabled*_vectWeightWrench); 148 | this->setWeightWrench(_nameFootRight, 149 | weightWrenchEnabled*_vectWeightWrench); 150 | 151 | //Point contact parameters 152 | this->setWeightForce(_nameHandLeft, 153 | weightForceEnabled*_vectWeightForce); 154 | this->setWeightForce(_nameHandRight, 155 | weightForceEnabled*_vectWeightForce); 156 | 157 | //Contact command structure parameters 158 | for (auto& it : _commands) { 159 | if (it.second.isPoint) { 160 | it.second.limitNormalForceMin = forceMinHand; 161 | it.second.limitNormalForceMax = forceMaxHand; 162 | it.second.weightForceEnabled = weightForceEnabled; 163 | it.second.weightForceDisabling = weightForceDisabling; 164 | } else { 165 | it.second.limitNormalForceMin = forceMinFoot; 166 | it.second.limitNormalForceMax = forceMaxFoot; 167 | it.second.weightForceEnabled = weightWrenchEnabled; 168 | it.second.weightForceDisabling = weightWrenchDisabling; 169 | } 170 | it.second.weightForcePushMode = weightForcePushMode; 171 | //Configure command pose filters 172 | it.second.filterPose.setParameters( 173 | useRawVRPose, 174 | isLocalFrame, 175 | scalingLin, 176 | clampRadiusLin, clampRadiusAng, 177 | cutoffFreq, 178 | maxVelLin, maxAccLin, 179 | maxVelAng, maxAccAng); 180 | //Configure command force filter 181 | it.second.filterForce.setParameters( 182 | clampRadiusForce, 183 | cutoffFreq, 184 | maxVelForce, maxAccForce); 185 | } 186 | 187 | //Reset default contact state 188 | this->toggleContact(_nameFootLeft, true); 189 | this->toggleContact(_nameFootRight, true); 190 | this->toggleContact(_nameHandLeft, false); 191 | this->toggleContact(_nameHandRight, false); 192 | 193 | //Call base reset 194 | return this->SEIKOWrapper::reset(); 195 | } 196 | 197 | const std::string& SEIKOTalos::nameFrameFootLeft() const 198 | { 199 | return _nameFootLeft; 200 | } 201 | const std::string& SEIKOTalos::nameFrameFootRight() const 202 | { 203 | return _nameFootRight; 204 | } 205 | const std::string& SEIKOTalos::nameFrameHandLeft() const 206 | { 207 | return _nameHandLeft; 208 | } 209 | const std::string& SEIKOTalos::nameFrameHandRight() const 210 | { 211 | return _nameHandRight; 212 | } 213 | 214 | } 215 | 216 | -------------------------------------------------------------------------------- /inria_talos_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.12) 2 | project(inria_talos_description) 3 | 4 | find_package(catkin REQUIRED) 5 | catkin_package() 6 | 7 | install(DIRECTORY urdf/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf/) 8 | install(DIRECTORY meshes/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes/) 9 | 10 | -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_1.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_1.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_1.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_1.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_1_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_1_collision.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_1_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_1_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_2.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_2.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_2.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_2.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_2_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_2_collision.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_2_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_2_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_3.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_3.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_3.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_3.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_3_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_3_collision.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_3_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_3_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_4.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_4.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_4.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_4.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_4_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_4_collision.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_4_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_4_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_5-v2.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_5-v2.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_5.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_5.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_5.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_5.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_5_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_5_collision.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_5_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_5_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_6.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_6.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_6.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_6.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_6_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_6_collision.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_6_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_6_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_7.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_7.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_7.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_7.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_7_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_7_collision.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/arm/arm_7_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/arm/arm_7_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/bart.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/bart.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/bart_light.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/bart_light.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/dummy_hand.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/dummy_hand.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/femur.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/femur.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/femur_light.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/femur_light.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/foot.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/foot.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/base_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/base_link.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/base_link.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/base_link.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/base_link_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/base_link_collision.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/base_link_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/base_link_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/fingertip.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/fingertip.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/fingertip.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/fingertip.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/fingertip_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/fingertip_collision.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/fingertip_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/fingertip_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/gripper_motor_double.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/gripper_motor_double.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/gripper_motor_double.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/gripper_motor_double.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/gripper_motor_double_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/gripper_motor_double_collision.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/gripper_motor_double_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/gripper_motor_double_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/gripper_motor_single.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/gripper_motor_single.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/gripper_motor_single.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/gripper_motor_single.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/gripper_motor_single_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/gripper_motor_single_collision.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/gripper_motor_single_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/gripper_motor_single_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/gripper_simple.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/gripper_simple.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/gripper_simple_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/gripper_simple_collision.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/gripper_tweezers.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/gripper_tweezers.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/gripper_tweezers_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/gripper_tweezers_collision.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/inner_double.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/inner_double.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/inner_double.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/inner_double.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/inner_double_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/inner_double_collision.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/inner_double_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/inner_double_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/inner_single.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/inner_single.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/inner_single.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/inner_single.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/inner_single_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/inner_single_collision.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/gripper/inner_single_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/gripper/inner_single_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/head/head_1.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/head/head_1.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/head/head_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/head/head_1.stl -------------------------------------------------------------------------------- /inria_talos_description/meshes/head/head_1_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/head/head_1_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/head/head_1_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/head/head_1_collision.stl -------------------------------------------------------------------------------- /inria_talos_description/meshes/head/head_2.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/head/head_2.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/head/head_2_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/head/head_2_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/head/head_2_default.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/head/head_2_default.stl -------------------------------------------------------------------------------- /inria_talos_description/meshes/head/head_2_default_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/head/head_2_default_collision.stl -------------------------------------------------------------------------------- /inria_talos_description/meshes/head/head_2_lidar.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/head/head_2_lidar.stl -------------------------------------------------------------------------------- /inria_talos_description/meshes/head/head_2_lidar_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/head/head_2_lidar_collision.stl -------------------------------------------------------------------------------- /inria_talos_description/meshes/hipZ.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/hipZ.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/hipZ_light.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/hipZ_light.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/sensors/orbbec/orbbec.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/sensors/orbbec/orbbec.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/sensors/orbbec/orbbec.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/sensors/orbbec/orbbec.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/sensors/xtion_pro_live/xtion_pro_live.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/sensors/xtion_pro_live/xtion_pro_live.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/sensors/xtion_pro_live/xtion_pro_live.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/sensors/xtion_pro_live/xtion_pro_live.png -------------------------------------------------------------------------------- /inria_talos_description/meshes/stump.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/stump.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/stump_reduced.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/stump_reduced.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/super-simple-bart.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/super-simple-bart.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/super-simple-femur.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/super-simple-femur.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/super-simple-tibia.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/super-simple-tibia.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/talos_dummy.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/talos_dummy.stl -------------------------------------------------------------------------------- /inria_talos_description/meshes/tibia.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/tibia.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/tibia_light.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/tibia_light.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/torso/base_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/torso/base_link.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/torso/base_link.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/torso/base_link.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/torso/base_link_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/torso/base_link_collision.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/torso/base_link_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/torso/base_link_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/torso/torso_1.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/torso/torso_1.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/torso/torso_1.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/torso/torso_1.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/torso/torso_2.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/torso/torso_2.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/torso/torso_2.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/torso/torso_2.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/torso/torso_2_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/torso/torso_2_collision.STL -------------------------------------------------------------------------------- /inria_talos_description/meshes/torso/torso_2_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/torso/torso_2_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/torso/torso_dummy.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/torso/torso_dummy.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/ankle_X_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/ankle_X_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/ankle_X_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/ankle_X_collision.stl -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/ankle_X_lo_res.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/ankle_X_lo_res.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/ankle_X_lo_res.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/ankle_X_lo_res.stl -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/ankle_Y_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/ankle_Y_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/ankle_Y_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/ankle_Y_collision.stl -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/ankle_Y_lo_res.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/ankle_Y_lo_res.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/ankle_Y_lo_res.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/ankle_Y_lo_res.stl -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/base_link_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/base_link_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/base_link_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/base_link_collision.stl -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/base_link_lo_res.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/base_link_lo_res.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/base_link_lo_res.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/base_link_lo_res.stl -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/hip_x_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/hip_x_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/hip_x_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/hip_x_collision.stl -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/hip_x_lo_res.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/hip_x_lo_res.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/hip_x_lo_res.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/hip_x_lo_res.stl -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/hip_y_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/hip_y_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/hip_y_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/hip_y_collision.stl -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/hip_y_lo_res.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/hip_y_lo_res.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/hip_y_lo_res.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/hip_y_lo_res.stl -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/hip_z_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/hip_z_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/hip_z_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/hip_z_collision.stl -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/hip_z_lo_res.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/hip_z_lo_res.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/hip_z_lo_res.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/hip_z_lo_res.stl -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/knee_collision.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/knee_collision.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/knee_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/knee_collision.stl -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/knee_lo_res.glb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/knee_lo_res.glb -------------------------------------------------------------------------------- /inria_talos_description/meshes/v2/knee_lo_res.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hucebot/seiko_controller_code/972e3a69af9334eec324e61566433fabf05bf061/inria_talos_description/meshes/v2/knee_lo_res.stl -------------------------------------------------------------------------------- /inria_talos_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | inria_talos_description 4 | 0.0.0 5 | 6 | Customized model of Talos humanoid robot 7 | 8 | Quentin Rouxel 9 | BSD 10 | 11 | catkin 12 | 13 | 14 | -------------------------------------------------------------------------------- /inria_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.12) 2 | project(inria_utils) 3 | 4 | find_package(catkin REQUIRED 5 | rospack 6 | roscpp 7 | ) 8 | find_package(Eigen3 REQUIRED) 9 | 10 | catkin_package( 11 | INCLUDE_DIRS include 12 | LIBRARIES ${PROJECT_NAME} 13 | CATKIN_DEPENDS 14 | rospack 15 | roscpp 16 | ) 17 | 18 | include_directories( 19 | include 20 | ${catkin_INCLUDE_DIRS} 21 | ${EIGEN3_INCLUDE_DIR} 22 | ) 23 | 24 | add_compile_options(-std=c++17) 25 | 26 | add_library(${PROJECT_NAME} 27 | src/Filesystem.cpp 28 | ) 29 | target_link_libraries(${PROJECT_NAME} 30 | ${catkin_LIBRARIES} 31 | atomic 32 | ) 33 | 34 | install(TARGETS ${PROJECT_NAME} DESTINATION lib) 35 | install(DIRECTORY include/ DESTINATION include) 36 | 37 | -------------------------------------------------------------------------------- /inria_utils/include/inria_utils/BufferReaderWriter.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_UTILS_BUFFERREADERWRITER_HPP 2 | #define INRIA_UTILS_BUFFERREADERWRITER_HPP 3 | 4 | #include 5 | 6 | namespace inria { 7 | 8 | /** 9 | * BufferReaderWriter 10 | * 11 | * Lock-free triple buffers 12 | * to share data from single reader to single writer 13 | * thread using a push/pull semantic. 14 | * The writer can always update its own data instance 15 | * then export the data using push. 16 | * The reader can always read its own data 17 | * instance then import new data using pull. 18 | * 19 | * Template parameters: 20 | * @param T Type of stored data 21 | */ 22 | template 23 | class BufferReaderWriter 24 | { 25 | public: 26 | 27 | /** 28 | * Initialization with externally 29 | * allocated data instances. 30 | * Forwarded arguments of constructor 31 | * for T is given. 32 | */ 33 | template 34 | BufferReaderWriter(Args&&... args) : 35 | _dataReader(new T(std::forward(args)...)), 36 | _dataWriter(new T(std::forward(args)...)), 37 | _dataShared(new T(std::forward(args)...)) 38 | { 39 | } 40 | 41 | /** 42 | * Deallocation of internal data 43 | */ 44 | ~BufferReaderWriter() 45 | { 46 | if (_dataReader != nullptr) { 47 | delete _dataReader; 48 | } 49 | if (_dataWriter != nullptr) { 50 | delete _dataWriter; 51 | } 52 | if (_dataShared != nullptr) { 53 | delete _dataShared; 54 | } 55 | } 56 | 57 | /** 58 | * Copy and assignment are disabled 59 | * to prevent memory management issues 60 | */ 61 | BufferReaderWriter(const BufferReaderWriter&) = delete; 62 | BufferReaderWriter& operator=(const BufferReaderWriter&) = delete; 63 | 64 | /** 65 | * Reset all internal buffer with 66 | * given value. 67 | * (NOT thread safe). 68 | */ 69 | void reset(const T& value) 70 | { 71 | *_dataReader = value; 72 | *_dataWriter = value; 73 | *_dataShared.load() = value; 74 | } 75 | 76 | /** 77 | * Access to reader 78 | * private data instance. 79 | * (Read access is used to reset the 80 | * internal buffer after reading it) 81 | */ 82 | const T* getFromReader() const noexcept 83 | { 84 | return _dataReader; 85 | } 86 | T* getFromReader() noexcept 87 | { 88 | return _dataReader; 89 | } 90 | 91 | /** 92 | * Atomically swap reader and 93 | * shared instances. 94 | * Import last pushed writer data. 95 | */ 96 | void pullFromReader() noexcept 97 | { 98 | T* tmp = _dataShared.exchange(_dataReader); 99 | _dataReader = tmp; 100 | } 101 | 102 | /** 103 | * Access to writer 104 | * private data instance 105 | */ 106 | T* getFromWriter() noexcept 107 | { 108 | return _dataWriter; 109 | } 110 | 111 | /** 112 | * Atomically swap reader and 113 | * shared instances. 114 | * Export currently written data instance. 115 | */ 116 | void pushFromWriter() noexcept 117 | { 118 | T* tmp = _dataShared.exchange(_dataWriter); 119 | _dataWriter = tmp; 120 | } 121 | 122 | private: 123 | 124 | /** 125 | * The allocated data privately owned 126 | * by the reader and the writer 127 | */ 128 | T* _dataReader; 129 | T* _dataWriter; 130 | 131 | /** 132 | * The allocated data shared 133 | * and waiting to be pulled 134 | */ 135 | std::atomic _dataShared; 136 | }; 137 | 138 | } 139 | 140 | #endif 141 | 142 | -------------------------------------------------------------------------------- /inria_utils/include/inria_utils/CircularBuffer.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_UTILS_CIRCULARBUFFER_HPP 2 | #define INRIA_UTILS_CIRCULARBUFFER_HPP 3 | 4 | namespace inria { 5 | 6 | /** 7 | * CircularBuffer 8 | * 9 | * Template type circular buffer 10 | * with static memory allocation. 11 | * Real time safe but NOT thread safe. 12 | */ 13 | template 14 | class CircularBuffer 15 | { 16 | public: 17 | 18 | /** 19 | * Default initialization 20 | */ 21 | CircularBuffer() : 22 | _buffer(), 23 | _head(0), 24 | _size(0), 25 | _neutral() 26 | { 27 | clear(); 28 | } 29 | 30 | /** 31 | * @return the number of written 32 | * element in the buffer 33 | */ 34 | size_t size() const 35 | { 36 | return _size; 37 | } 38 | 39 | /** 40 | * Reset internal buffer to empty 41 | */ 42 | void clear() 43 | { 44 | _head = 0; 45 | _size = 0; 46 | } 47 | 48 | /** 49 | * Append given data element into 50 | * the circular buffer 51 | */ 52 | void append(const T& value) 53 | { 54 | _buffer[_head] = value; 55 | if (_size < SIZE) { 56 | _size++; 57 | } 58 | _head++; 59 | if (_head >= SIZE) { 60 | _head = 0; 61 | } 62 | } 63 | 64 | /** 65 | * @return the stored value indexed backward 66 | * in time by its index (from 0 to size()-1). 67 | */ 68 | const T& get(size_t index) const 69 | { 70 | if (_size == 0) { 71 | //Return default value if the buffer 72 | //is not yet initialized 73 | return _neutral; 74 | } 75 | if (index >= _size) { 76 | //Return the oldest element if index 77 | //is outside available data 78 | index = _size-1; 79 | } 80 | if (index+1 <= _head) { 81 | return _buffer[_head-1-index]; 82 | } else { 83 | return _buffer[SIZE-(index+1-_head)]; 84 | } 85 | } 86 | 87 | private: 88 | 89 | /** 90 | * Statically allocated data 91 | * circular buffer 92 | */ 93 | T _buffer[SIZE]; 94 | 95 | /** 96 | * Index to next buffer element 97 | * to be written 98 | */ 99 | size_t _head; 100 | 101 | /** 102 | * Current buffer used 103 | * elements size 104 | */ 105 | size_t _size; 106 | 107 | /** 108 | * Default neutral value used in get() 109 | * for uninitialized buffer 110 | */ 111 | T _neutral; 112 | }; 113 | 114 | } 115 | 116 | #endif 117 | 118 | -------------------------------------------------------------------------------- /inria_utils/include/inria_utils/Filesystem.h: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_UTILS_FILESYSTEM_H 2 | #define INRIA_UTILS_FILESYSTEM_H 3 | 4 | #include 5 | 6 | namespace inria { 7 | 8 | /** 9 | * Resolve system absolute or relative path 10 | * to handle finding ROS package path. 11 | * Thread safe but non RT safe. 12 | * 13 | * @param path Absolute, relative or 14 | * ROS package path to be resolved. 15 | * @return the resolved system path or 16 | * the identity path if no ROS package 17 | * can be found. 18 | */ 19 | std::string SystemResolvePath( 20 | const std::string& path); 21 | 22 | } 23 | 24 | #endif 25 | 26 | -------------------------------------------------------------------------------- /inria_utils/include/inria_utils/Scheduling.h: -------------------------------------------------------------------------------- 1 | #ifndef INRIA_UTILS_SCHEDULING_H 2 | #define INRIA_UTILS_SCHEDULING_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace inria { 14 | 15 | /** 16 | * @return Linux system process 17 | * and thread unique id 18 | */ 19 | inline pid_t SystemGetProcessId() 20 | { 21 | return getpid(); 22 | } 23 | inline pid_t SystemGetThreadId() 24 | { 25 | return (pid_t)syscall(SYS_gettid); 26 | } 27 | 28 | /** 29 | * Configure the calling thread as 30 | * real-time FIFO scheduling with 31 | * max priority 32 | */ 33 | inline void SystemSetThreadRealTime() 34 | { 35 | struct sched_param param; 36 | param.sched_priority = 99; 37 | int isError = sched_setscheduler( 38 | 0, SCHED_FIFO, ¶m); 39 | if (isError != 0) { 40 | throw std::runtime_error( 41 | "inria::SystemSetThreadRealTime: " 42 | "Enabling real time thread failed: " + 43 | std::string(strerror(errno))); 44 | } 45 | } 46 | 47 | /** 48 | * Configure the calling thread CPU affinity 49 | * to the single given core number. 50 | * 51 | * @param num Core number on which the 52 | * calling thread must run (in 0..CPU_COUNT-1) 53 | */ 54 | inline void SystemBindThreadToCPU(unsigned int num) 55 | { 56 | cpu_set_t mask; 57 | CPU_ZERO(&mask); 58 | CPU_SET(num, &mask); 59 | int isError = sched_setaffinity( 60 | 0, sizeof(mask), &mask); 61 | if (isError != 0) { 62 | throw std::runtime_error( 63 | "inria::SystemBindThreadToCPU: " 64 | "Binding thread to CPU failed: " + 65 | std::string(strerror(errno))); 66 | } 67 | } 68 | 69 | /** 70 | * Set the calling thread name. 71 | * 72 | * @param name The name to be assigned 73 | * to the calling thread. If longer than 74 | * 15 characters, the name will be truncated. 75 | */ 76 | inline void SystemSetThreadName(const std::string& name) 77 | { 78 | int isError = prctl( 79 | PR_SET_NAME, 80 | name.c_str(), 0, 0, 0); 81 | if (isError != 0) { 82 | throw std::runtime_error( 83 | "inria::SystemSetThread: " 84 | "Set thread name failed: " + 85 | name); 86 | } 87 | } 88 | 89 | /** 90 | * @return the name of the calling thread 91 | */ 92 | inline std::string SystemGetThreadName() 93 | { 94 | char buffer[16]; 95 | prctl(PR_GET_NAME,buffer, 0, 0, 0); 96 | return std::string(buffer); 97 | } 98 | 99 | } 100 | 101 | #endif 102 | 103 | -------------------------------------------------------------------------------- /inria_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | inria_utils 4 | 0.0.0 5 | 6 | Various utility tools 7 | 8 | Quentin Rouxel 9 | BSD 10 | 11 | catkin 12 | rospack 13 | roscpp 14 | 15 | -------------------------------------------------------------------------------- /inria_utils/src/Filesystem.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace inria { 9 | 10 | /** 11 | * Global instance of the ROS pack 12 | * crawler protected from thread access 13 | */ 14 | static std::mutex globalMutex; 15 | static bool globalIsInitialized = false; 16 | static rospack::Rospack globalRP; 17 | 18 | /** 19 | * Use ROSPack to retrieve the absolute 20 | * name of a ROS package. 21 | * Thread safe. 22 | * 23 | * @param name ROS name to be search. 24 | * @return the absolute package path 25 | * or an empty string if not found. 26 | */ 27 | static std::string ROSGetPackagePath(const std::string& name) 28 | { 29 | //Thread safe global access 30 | globalMutex.lock(); 31 | if (!globalIsInitialized) { 32 | //Global ROSpack crawler 33 | //initialization on the first call 34 | globalIsInitialized = true; 35 | globalRP.setQuiet(true); 36 | //Get search paths from environment 37 | std::vector searchPaths; 38 | globalRP.getSearchPathFromEnv(searchPaths); 39 | //Explore every ROS packages 40 | globalRP.crawl(searchPaths, false); 41 | } 42 | //Find the package path 43 | std::string path; 44 | bool isSuccess = globalRP.find(name, path); 45 | globalMutex.unlock(); 46 | 47 | if (isSuccess) { 48 | return path; 49 | } else { 50 | return ""; 51 | } 52 | } 53 | 54 | std::string SystemResolvePath( 55 | const std::string& path) 56 | { 57 | //Empty case 58 | if (path.length() == 0) { 59 | return ""; 60 | } 61 | 62 | //Absolute or local file path 63 | if (path[0] == '/' || path[0] == '.') { 64 | return path; 65 | } 66 | 67 | //Local home case 68 | if (path[0] == '~') { 69 | const char* pathHome = std::getenv("HOME"); 70 | if (pathHome != nullptr) { 71 | return std::string(pathHome) + path.substr(1); 72 | } else { 73 | return path; 74 | } 75 | } 76 | 77 | //URDF path convention 78 | if (path.substr(0, 7) == "file://") { 79 | return path.substr(7); 80 | } 81 | std::string tmpPath = path; 82 | if (path.substr(0, 10) == "package://") { 83 | tmpPath = path.substr(10); 84 | } 85 | if (path.substr(0, 8) == "model://") { 86 | tmpPath = path.substr(8); 87 | } 88 | 89 | //Try to retrieve from ROS the absolute path of 90 | //a package from its name if possible or 91 | //return the original path else 92 | size_t index = tmpPath.find_first_of("/"); 93 | std::string partPackageName = tmpPath; 94 | std::string partLast = ""; 95 | if (index != std::string::npos) { 96 | partPackageName = tmpPath.substr(0, index); 97 | partLast = tmpPath.substr(index+1); 98 | } 99 | 100 | //Retrieve the ROS absolute path 101 | //of a package name if it exists 102 | std::string partPackagePath = 103 | ROSGetPackagePath(partPackageName); 104 | 105 | if (partPackagePath.length() > 0) { 106 | return partPackagePath + "/" + partLast; 107 | } else { 108 | return tmpPath; 109 | } 110 | } 111 | 112 | } 113 | 114 | --------------------------------------------------------------------------------