├── .gitignore ├── AUTHORS.md ├── LICENSE ├── README.md ├── robots ├── xpp_hyq │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── include │ │ └── xpp_hyq │ │ │ ├── hyqleg_inverse_kinematics.h │ │ │ ├── inverse_kinematics_hyq1.h │ │ │ ├── inverse_kinematics_hyq2.h │ │ │ └── inverse_kinematics_hyq4.h │ ├── launch │ │ ├── all.launch │ │ ├── biped.launch │ │ ├── hyq.launch │ │ ├── monoped.launch │ │ └── xpp_hyq.launch │ ├── meshes │ │ ├── leg │ │ │ ├── hipassembly.dae │ │ │ ├── lowerleg.dae │ │ │ └── upperleg.dae │ │ └── trunk │ │ │ └── trunk.dae │ ├── package.xml │ ├── rviz │ │ └── xpp_hyq.rviz │ ├── src │ │ ├── exe │ │ │ ├── urdf_visualizer_hyq1.cc │ │ │ ├── urdf_visualizer_hyq2.cc │ │ │ └── urdf_visualizer_hyq4.cc │ │ ├── hyqleg_inverse_kinematics.cc │ │ ├── inverse_kinematics_hyq1.cc │ │ ├── inverse_kinematics_hyq2.cc │ │ └── inverse_kinematics_hyq4.cc │ └── urdf │ │ ├── biped.urdf │ │ ├── hyq.urdf.xacro │ │ ├── leg.urdf.xacro │ │ ├── monoped.urdf │ │ └── trunk.urdf.xacro └── xpp_quadrotor │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── launch │ └── quadrotor.launch │ ├── meshes │ └── quadrotor_base.dae │ ├── package.xml │ ├── src │ └── exe │ │ └── urdf_visualizer_quadrotor.cc │ └── urdf │ └── quadrotor.urdf ├── xpp ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml ├── xpp_examples ├── CHANGELOG.rst ├── CMakeLists.txt ├── bags │ ├── README.txt │ ├── biped.bag │ ├── gap.bag │ ├── hyq.bag │ └── monoped.bag ├── launch │ ├── biped_bag.launch │ ├── biped_gap.launch │ ├── hyq_bag.launch │ ├── monoped_bag.launch │ ├── monoped_generate.launch │ └── quadrotor_generate.launch ├── package.xml ├── rviz │ ├── xpp_biped.rviz │ ├── xpp_hyq.rviz │ ├── xpp_monoped.rviz │ └── xpp_quadrotor.rviz └── src │ ├── monoped_pub.cc │ └── quadrotor_bag_builder.cc ├── xpp_msgs ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── xpp_msgs │ │ └── topic_names.h ├── msg │ ├── RobotParameters.msg │ ├── RobotStateCartesian.msg │ ├── RobotStateCartesianTrajectory.msg │ ├── RobotStateJoint.msg │ ├── State6d.msg │ ├── StateLin3d.msg │ └── TerrainInfo.msg └── package.xml ├── xpp_states ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── xpp_states │ │ ├── cartesian_declarations.h │ │ ├── convert.h │ │ ├── endeffector_mappings.h │ │ ├── endeffectors.h │ │ ├── joints.h │ │ ├── robot_state_cartesian.h │ │ ├── robot_state_joint.h │ │ └── state.h ├── package.xml └── src │ ├── joints.cc │ ├── robot_state_cartesian.cc │ ├── robot_state_joint.cc │ └── state.cc └── xpp_vis ├── CHANGELOG.rst ├── CMakeLists.txt ├── include └── xpp_vis │ ├── cartesian_joint_converter.h │ ├── inverse_kinematics.h │ ├── rviz_colors.h │ ├── rviz_robot_builder.h │ └── urdf_visualizer.h ├── launch └── xpp.launch ├── package.xml ├── rviz └── xpp.rviz ├── src ├── cartesian_joint_converter.cc ├── exe │ └── rviz_marker_node.cc ├── rviz_robot_builder.cc └── urdf_visualizer.cc └── test ├── gtest_main.cc └── rviz_robot_builder_test.cc /.gitignore: -------------------------------------------------------------------------------- 1 | xpp_msgs/bags/* 2 | */doc/html/* 3 | */doc/manifest.yaml 4 | 5 | -------------------------------------------------------------------------------- /AUTHORS.md: -------------------------------------------------------------------------------- 1 | Xpp was developed by Alexander W. Winkler mainly at the [Agile and Dexterous 2 | Robotics Lab](http://www.adrl.ethz.ch/doku.php). It is currently developed at the [Robotics 3 | Systems Lab](http://www.rsl.ethz.ch/). 4 | 5 | Initial contributors/input from: 6 | 7 | * Diego Pardo, Amazon Robotics 8 | * Michael Neunert, Google Deepmind 9 | * Carlos Mastalli, Laas CNRS 10 | 11 | See [github contributors](https://github.com/leggedrobotics/xpp/graphs/contributors) for further code contributors. 12 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2017, Alexander W. Winkler. 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 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * 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 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ### 2 | 3 | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kdev__xpp__ubuntu_xenial_amd64)](http://build.ros.org/view/Kdev/job/Kdev__xpp__ubuntu_xenial_amd64/) 4 | [![Documentation](https://img.shields.io/badge/docs-generated-brightgreen.svg)](http://docs.ros.org/kinetic/api/xpp/html/) 5 | [![ROS hosting](https://img.shields.io/badge/ROS-integration-blue.svg)](http://wiki.ros.org/xpp) 6 | [![License BSD-3-Clause](https://img.shields.io/badge/license-BSD--3--Clause-blue.svg)](https://tldrlegal.com/license/bsd-3-clause-license-%28revised%29#fulltext) 7 | 8 | 9 | 10 | Xpp is a [ROS package](http://wiki.ros.org/xpp) for the visualization of motion-plans for legged robots. It 11 | draws support areas, contact forces and motion trajectories in RVIZ and displays URDFs for specific robots, including a one-legged, a two-legged hopper and [HyQ]. More example motions can be seen in this [video](https://www.youtube.com/watch?v=0jE46GqzxMM&feature=youtu.be), generated by the library [towr](https://github.com/ethz-adrl/towr). 12 | 13 | 14 | ## Install 15 | The recommended way to install is through the [ROS binaries](http://wiki.ros.org/xpp): 16 | ```bash 17 | sudo apt-get install ros--xpp 18 | ``` 19 | 20 | ## Build from source 21 | In case you don't want to install from the ROS binaries, this package requires [Eigen] and [ROS]: 22 | ```bash 23 | sudo apt-get install ros--desktop-full 24 | sudo apt-get install libeigen3-dev 25 | ``` 26 | 27 | Then you can build the package in your catkin workspace 28 | 29 | ```bash 30 | cd catkin_workspace/src 31 | git clone https://github.com/leggedrobotics/xpp.git 32 | cd .. 33 | catkin_make 34 | source ./devel/setup.bash 35 | ``` 36 | 37 | #### Unit Tests 38 | 39 | Make sure everything installed correctly by running the unit tests through 40 | 41 | catkin_make run_tests 42 | 43 | or if you are using [catkin tools]. 44 | 45 | catkin build xpp_vis --no-deps --verbose --catkin-make-args run_tests 46 | 47 | 48 | ## Usage 49 | 50 | A few examples for different robots are provided in the `xpp_examples` package, e.g. 51 | [xpp_examples/src/monoped_pub.cc](https://github.com/leggedrobotics/xpp/blob/master/xpp_examples/src/monoped_pub.cc). For starters, run 52 | 53 | roslaunch xpp_examples hyq_bag.launch // or any other launch file in this package 54 | 55 | 56 | 57 | 58 | ## Add your own Robot 59 | Want to visualize your own robot, then checkout these [instructions](http://wiki.ros.org/xpp#Visualize_.2BAC8_Contribute_your_own_robot) and we're happy to receive your pull-request. 60 | See here the list of [contributors](https://github.com/leggedrobotics/xpp/graphs/contributors) who participated in this project. 61 | 62 | 63 | ## Authors 64 | [Alexander W. Winkler](https://www.alex-winkler.com) - Initial Work/Maintainer 65 | 66 | The work was carried out at the following institutions: 67 | 68 | [](https://www.ethz.ch/en.html "ETH Zurich")         [](http://www.adrl.ethz.ch/doku.php "Agile and Dexterous Robotics Lab")        [](http://www.rsl.ethz.ch/ "Robotic Systems Lab") 69 | 70 | 71 | ## Citation 72 | If you use this work in an academic context, please cite as follows: 73 | 74 | @misc{xpp_ros, 75 | author = {Alexander W. Winkler}, 76 | title = {{Xpp - A collection of ROS packages for the visualization of legged robots}}, 77 | year = {2017}, 78 | doi = {10.5281/zenodo.1037901}, 79 | url = {https://doi.org/10.5281/zenodo.1037901} 80 | } 81 | 82 | ## Bugs & Feature Requests 83 | 84 | Please report bugs, request features or ask questions using the [Issue Tracker](https://github.com/leggedrobotics/xpp/issues). 85 | 86 | [HyQ]: https://www.iit.it/research/lines/dynamic-legged-systems 87 | [ROS]: http://www.ros.org 88 | [towr]: http://wiki.ros.org/towr 89 | [rviz]: http://wiki.ros.org/rviz 90 | [catkin tools]: http://catkin-tools.readthedocs.org/ 91 | [Eigen]: http://eigen.tuxfamily.org 92 | [Fa2png]: http://fa2png.io/r/font-awesome/link/ 93 | -------------------------------------------------------------------------------- /robots/xpp_hyq/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package xpp_vis_hyq 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.10 (2019-04-05) 6 | ------------------- 7 | * add xacro dependency 8 | * Contributors: Alexander Winkler 9 | 10 | 1.0.9 (2018-07-10) 11 | ------------------ 12 | * Merge pull request #7 from zlingkang/xacropi2xacro 13 | change deprecated xacro.py to xacro 14 | * Contributors: zlingkang 15 | 16 | 1.0.8 (2018-07-07) 17 | ------------------ 18 | * avoid IK segfault for biped/hyq when not enough ee positions sent 19 | * removed limits on hyqleg joint angles 20 | * Contributors: Alexander Winkler 21 | 22 | 1.0.7 (2018-07-03) 23 | ------------------ 24 | * add rviz launch specifically for hyq 25 | * Contributors: Alexander Winkler 26 | 27 | 1.0.6 (2018-04-18) 28 | ------------------ 29 | * add missing libxpp_hyq.so library to catkin install 30 | * Contributors: Alexander Winkler 31 | 32 | 1.0.5 (2018-02-01) 33 | ------------------ 34 | * modernize CMake files 35 | * use default keyword for empty destructors 36 | * use default BSD license 37 | * Contributors: Alexander Winkler 38 | 39 | 1.0.4 (2018-01-03) 40 | ------------------ 41 | * Merge pull request #3 from leggedrobotics/devel 42 | Fix xacro bug and remove terrain and optimization specific classes. 43 | * xpp_hyq: add --inorder when processing xacro files 44 | see http://wiki.ros.org/xacro#Processing_Order 45 | * cleaned up formatting of xacro files 46 | * removed unused xacro origin tag causing run failure 47 | * Contributors: Alexander W Winkler 48 | 49 | 1.0.3 (2017-11-03) 50 | ------------------ 51 | * removed xpp_ros_conversions (now in xpp_states) 52 | * 1.0.2 53 | * update changelog 54 | * Contributors: Alexander Winkler 55 | 56 | 1.0.2 (2017-10-28) 57 | ------------------ 58 | 59 | 1.0.1 (2017-10-27) 60 | ------------------ 61 | 62 | 1.0.0 (2017-10-26) 63 | ------------------ 64 | * added separate xpp_example package and Readme.md 65 | * included install space in CMakeLists.txt 66 | * explicitly listing ros dependencies (not through catkin_package macro) 67 | * Added license to files and giving credit to contributors. 68 | * cleaned up package.xml and CMakeLists.txt files 69 | * cleaned up endeffector and joint representation 70 | * showed close connection of mono/bi/quad to hyq 71 | * added doxygen comments to header files 72 | * made xpp_vis robot independent. Added sample hopper executables 73 | * Contributors: Alexander Winkler 74 | -------------------------------------------------------------------------------- /robots/xpp_hyq/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(xpp_hyq) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | roscpp 8 | xpp_vis 9 | ) 10 | 11 | ################################### 12 | ## catkin specific configuration ## 13 | ################################### 14 | catkin_package() 15 | 16 | 17 | ########### 18 | ## Build ## 19 | ########### 20 | ## Specify additional locations of header files 21 | include_directories( 22 | include 23 | ${catkin_INCLUDE_DIRS} 24 | ) 25 | 26 | # Declare a C++ library 27 | add_library(${PROJECT_NAME} 28 | src/hyqleg_inverse_kinematics.cc 29 | src/inverse_kinematics_hyq1.cc 30 | src/inverse_kinematics_hyq2.cc 31 | src/inverse_kinematics_hyq4.cc 32 | ) 33 | 34 | ## URDF visualizers for all HyQ variants 35 | add_executable(urdf_visualizer_hyq1 src/exe/urdf_visualizer_hyq1.cc) 36 | target_link_libraries(urdf_visualizer_hyq1 37 | ${PROJECT_NAME} 38 | ${catkin_LIBRARIES} 39 | ) 40 | 41 | add_executable(urdf_visualizer_hyq2 src/exe/urdf_visualizer_hyq2.cc) 42 | target_link_libraries(urdf_visualizer_hyq2 43 | ${PROJECT_NAME} 44 | ${catkin_LIBRARIES} 45 | ) 46 | 47 | add_executable(urdf_visualizer_hyq4 src/exe/urdf_visualizer_hyq4.cc) 48 | target_link_libraries(urdf_visualizer_hyq4 49 | ${PROJECT_NAME} 50 | ${catkin_LIBRARIES} 51 | ) 52 | 53 | ############# 54 | ## Install ## 55 | ############# 56 | # Mark library for installation 57 | install( 58 | TARGETS ${PROJECT_NAME} urdf_visualizer_hyq1 urdf_visualizer_hyq2 urdf_visualizer_hyq4 59 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 60 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 61 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 62 | ) 63 | 64 | # Mark other files for installation 65 | install( 66 | DIRECTORY launch rviz meshes urdf 67 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 68 | ) 69 | -------------------------------------------------------------------------------- /robots/xpp_hyq/include/xpp_hyq/hyqleg_inverse_kinematics.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef XPP_VIS_HYQLEG_INVERSE_KINEMATICS_H_ 31 | #define XPP_VIS_HYQLEG_INVERSE_KINEMATICS_H_ 32 | 33 | #include 34 | 35 | namespace xpp { 36 | 37 | enum HyqJointID {HAA=0, HFE, KFE, HyqlegJointCount}; 38 | 39 | /** 40 | * @brief Converts a hyq foot position to joint angles. 41 | */ 42 | class HyqlegInverseKinematics { 43 | public: 44 | using Vector3d = Eigen::Vector3d; 45 | enum KneeBend { Forward, Backward }; 46 | 47 | /** 48 | * @brief Default c'tor initializing leg lengths with standard values. 49 | */ 50 | HyqlegInverseKinematics () = default; 51 | virtual ~HyqlegInverseKinematics () = default; 52 | 53 | /** 54 | * @brief Returns the joint angles to reach a Cartesian foot position. 55 | * @param ee_pos_H Foot position xyz expressed in the frame attached 56 | * at the hip-aa (H). 57 | */ 58 | Vector3d GetJointAngles(const Vector3d& ee_pos_H, KneeBend bend=Forward) const; 59 | 60 | /** 61 | * @brief Restricts the joint angles to lie inside the feasible range 62 | * @param q[in/out] Current joint angle that is adapted if it exceeds 63 | * the specified range. 64 | * @param joint Which joint (HAA, HFE, KFE) this value represents. 65 | */ 66 | void EnforceLimits(double& q, HyqJointID joint) const; 67 | 68 | private: 69 | Vector3d hfe_to_haa_z = Vector3d(0.0, 0.0, 0.08); //distance of HFE to HAA in z direction 70 | double length_thigh = 0.35; // length of upper leg 71 | double length_shank = 0.33; // length of lower leg 72 | }; 73 | 74 | } /* namespace xpp */ 75 | 76 | #endif /* XPP_VIS_HYQLEG_INVERSE_KINEMATICS_H_ */ 77 | -------------------------------------------------------------------------------- /robots/xpp_hyq/include/xpp_hyq/inverse_kinematics_hyq1.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef XPP_VIS_INVERSEKINEMATICS_HYQ1_H_ 31 | #define XPP_VIS_INVERSEKINEMATICS_HYQ1_H_ 32 | 33 | #include 34 | #include 35 | 36 | namespace xpp { 37 | 38 | /** 39 | * @brief Inverse Kinematics for one HyQ leg attached to a brick (base). 40 | */ 41 | class InverseKinematicsHyq1 : public InverseKinematics { 42 | public: 43 | InverseKinematicsHyq1() = default; 44 | virtual ~InverseKinematicsHyq1() = default; 45 | 46 | /** 47 | * @brief Returns joint angles to reach for a specific foot position. 48 | * @param pos_B 3D-position of the foot expressed in the base frame (B). 49 | */ 50 | Joints GetAllJointAngles(const EndeffectorsPos& pos_B) const override; 51 | 52 | /** 53 | * @brief Number of endeffectors (feet, hands) this implementation expects. 54 | */ 55 | int GetEECount() const override { return 1; }; 56 | 57 | private: 58 | HyqlegInverseKinematics leg; 59 | }; 60 | 61 | } /* namespace xpp */ 62 | 63 | #endif /* XPP_VIS_INVERSEKINEMATICS_HYQ1_H_ */ 64 | -------------------------------------------------------------------------------- /robots/xpp_hyq/include/xpp_hyq/inverse_kinematics_hyq2.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef XPP_VIS_INVERSEKINEMATICS_HYQ2_H_ 31 | #define XPP_VIS_INVERSEKINEMATICS_HYQ2_H_ 32 | 33 | #include 34 | #include 35 | 36 | 37 | namespace xpp { 38 | 39 | /** 40 | * @brief Inverse Kinematics for two HyQ legs attached to a brick (base). 41 | */ 42 | class InverseKinematicsHyq2 : public InverseKinematics { 43 | public: 44 | InverseKinematicsHyq2() = default; 45 | virtual ~InverseKinematicsHyq2() = default; 46 | 47 | /** 48 | * @brief Returns joint angles to reach for a specific foot position. 49 | * @param pos_B 3D-position of the foot expressed in the base frame (B). 50 | */ 51 | Joints GetAllJointAngles(const EndeffectorsPos& pos_B) const override; 52 | 53 | /** 54 | * @brief Number of endeffectors (feet, hands) this implementation expects. 55 | */ 56 | int GetEECount() const override { return 2; }; 57 | 58 | private: 59 | HyqlegInverseKinematics leg; 60 | }; 61 | 62 | } /* namespace xpp */ 63 | 64 | #endif /* XPP_VIS_INVERSEKINEMATICS_HYQ2_H_ */ 65 | -------------------------------------------------------------------------------- /robots/xpp_hyq/include/xpp_hyq/inverse_kinematics_hyq4.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef XPP_VIS_INVERSEKINEMATICS_HYQ4_H_ 31 | #define XPP_VIS_INVERSEKINEMATICS_HYQ4_H_ 32 | 33 | #include 34 | #include 35 | 36 | namespace xpp { 37 | 38 | /** 39 | * @brief Inverse kinematics function for the HyQ robot. 40 | */ 41 | class InverseKinematicsHyq4 : public InverseKinematics { 42 | public: 43 | InverseKinematicsHyq4() = default; 44 | virtual ~InverseKinematicsHyq4() = default; 45 | 46 | /** 47 | * @brief Returns joint angles to reach for a specific foot position. 48 | * @param pos_B 3D-position of the foot expressed in the base frame (B). 49 | */ 50 | Joints GetAllJointAngles(const EndeffectorsPos& pos_b) const override; 51 | 52 | /** 53 | * @brief Number of endeffectors (feet, hands) this implementation expects. 54 | */ 55 | int GetEECount() const override { return 4; }; 56 | 57 | private: 58 | Vector3d base2hip_LF_ = Vector3d(0.3735, 0.207, 0.0); 59 | HyqlegInverseKinematics leg; 60 | }; 61 | 62 | } /* namespace xpp */ 63 | 64 | #endif /* XPP_VIS_INVERSEKINEMATICS_HYQ4_H_ */ 65 | -------------------------------------------------------------------------------- /robots/xpp_hyq/launch/all.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /robots/xpp_hyq/launch/biped.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /robots/xpp_hyq/launch/hyq.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /robots/xpp_hyq/launch/monoped.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /robots/xpp_hyq/launch/xpp_hyq.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /robots/xpp_hyq/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | xpp_hyq 5 | 1.0.10 6 | 7 | HyQ-robot specific functions for visualization in the XPP Motion Framework. 8 | 9 | These include inverse kinematics as well as urdf files for a one-legged, 10 | two-legged and four legged robot with HyQ 11 | legs. 12 | 13 | The dynamic model can be found 14 | here. 15 | 16 | See also IIT. 17 | 18 | 19 | Alexander W. Winkler 20 | Diego Pardo. 21 | Carlos Mastalli 22 | Ioannis Havoutis 23 | Alexander W. Winkler 24 | BSD 25 | 26 | http://github.com/leggedrobotics/xpp 27 | http://github.com/leggedrobotics/xpp/issues 28 | 29 | catkin 30 | xacro 31 | roscpp 32 | xpp_vis 33 | 34 | -------------------------------------------------------------------------------- /robots/xpp_hyq/src/exe/urdf_visualizer_hyq1.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | using namespace xpp; 39 | 40 | int main(int argc, char *argv[]) 41 | { 42 | ::ros::init(argc, argv, "monoped_urdf_visualizer"); 43 | 44 | const std::string joint_desired_mono = "xpp/joint_mono_des"; 45 | 46 | auto ik = std::make_shared(); 47 | CartesianJointConverter inv_kin_converter(ik, 48 | xpp_msgs::robot_state_desired, 49 | joint_desired_mono); 50 | 51 | std::vector joint_names(HyqlegJointCount); 52 | joint_names.at(HAA) = "haa_joint"; 53 | joint_names.at(HFE) = "hfe_joint"; 54 | joint_names.at(KFE) = "kfe_joint"; 55 | 56 | std::string urdf = "monoped_rviz_urdf_robot_description"; 57 | UrdfVisualizer node_des(urdf, joint_names, "base", "world", 58 | joint_desired_mono, "monoped"); 59 | 60 | ::ros::spin(); 61 | 62 | return 1; 63 | } 64 | 65 | -------------------------------------------------------------------------------- /robots/xpp_hyq/src/exe/urdf_visualizer_hyq2.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | #include 39 | 40 | using namespace xpp; 41 | using namespace biped; 42 | 43 | int main(int argc, char *argv[]) 44 | { 45 | ::ros::init(argc, argv, "biped_urdf_visualizer"); 46 | 47 | const std::string joint_desired_biped = "xpp/joint_biped_des"; 48 | 49 | auto ik = std::make_shared(); 50 | CartesianJointConverter inv_kin_converter(ik, 51 | xpp_msgs::robot_state_desired, 52 | joint_desired_biped); 53 | 54 | int n_ee = ik->GetEECount(); 55 | int n_j = HyqlegJointCount; 56 | std::vector joint_names(n_ee*n_j); 57 | joint_names.at(n_j*L + HAA) = "L_haa_joint"; 58 | joint_names.at(n_j*L + HFE) = "L_hfe_joint"; 59 | joint_names.at(n_j*L + KFE) = "L_kfe_joint"; 60 | joint_names.at(n_j*R + HAA) = "R_haa_joint"; 61 | joint_names.at(n_j*R + HFE) = "R_hfe_joint"; 62 | joint_names.at(n_j*R + KFE) = "R_kfe_joint"; 63 | 64 | std::string urdf = "biped_rviz_urdf_robot_description"; 65 | UrdfVisualizer node(urdf, joint_names, "base", "world", 66 | joint_desired_biped, "biped"); 67 | 68 | ::ros::spin(); 69 | 70 | return 1; 71 | } 72 | 73 | -------------------------------------------------------------------------------- /robots/xpp_hyq/src/exe/urdf_visualizer_hyq4.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | using namespace xpp; 46 | using namespace quad; 47 | 48 | int main(int argc, char *argv[]) 49 | { 50 | ::ros::init(argc, argv, "hyq_urdf_visualizer"); 51 | 52 | const std::string joint_desired_hyq = "xpp/joint_hyq_des"; 53 | 54 | auto hyq_ik = std::make_shared(); 55 | CartesianJointConverter inv_kin_converter(hyq_ik, 56 | xpp_msgs::robot_state_desired, 57 | joint_desired_hyq); 58 | 59 | // urdf joint names 60 | int n_ee = hyq_ik->GetEECount(); 61 | int n_j = HyqlegJointCount; 62 | std::vector joint_names(n_ee*n_j); 63 | joint_names.at(n_j*LF + HAA) = "lf_haa_joint"; 64 | joint_names.at(n_j*LF + HFE) = "lf_hfe_joint"; 65 | joint_names.at(n_j*LF + KFE) = "lf_kfe_joint"; 66 | joint_names.at(n_j*RF + HAA) = "rf_haa_joint"; 67 | joint_names.at(n_j*RF + HFE) = "rf_hfe_joint"; 68 | joint_names.at(n_j*RF + KFE) = "rf_kfe_joint"; 69 | joint_names.at(n_j*LH + HAA) = "lh_haa_joint"; 70 | joint_names.at(n_j*LH + HFE) = "lh_hfe_joint"; 71 | joint_names.at(n_j*LH + KFE) = "lh_kfe_joint"; 72 | joint_names.at(n_j*RH + HAA) = "rh_haa_joint"; 73 | joint_names.at(n_j*RH + HFE) = "rh_hfe_joint"; 74 | joint_names.at(n_j*RH + KFE) = "rh_kfe_joint"; 75 | 76 | std::string urdf = "hyq_rviz_urdf_robot_description"; 77 | UrdfVisualizer hyq_desired(urdf, joint_names, "base", "world", 78 | joint_desired_hyq, "hyq_des"); 79 | 80 | ::ros::spin(); 81 | 82 | return 1; 83 | } 84 | 85 | -------------------------------------------------------------------------------- /robots/xpp_hyq/src/hyqleg_inverse_kinematics.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | 38 | namespace xpp { 39 | 40 | 41 | HyqlegInverseKinematics::Vector3d 42 | HyqlegInverseKinematics::GetJointAngles (const Vector3d& ee_pos_B, KneeBend bend) const 43 | { 44 | double q_HAA_bf, q_HAA_br, q_HFE_br; // rear bend of knees 45 | double q_HFE_bf, q_KFE_br, q_KFE_bf; // forward bend of knees 46 | 47 | Eigen::Vector3d xr; 48 | Eigen::Matrix3d R; 49 | 50 | // translate to the local coordinate of the attachment of the leg 51 | // and flip coordinate signs such that all computations can be done 52 | // for the front-left leg 53 | xr = ee_pos_B; 54 | 55 | // compute the HAA angle 56 | q_HAA_bf = q_HAA_br = -atan2(xr[Y],-xr[Z]); 57 | 58 | // rotate into the HFE coordinate system (rot around X) 59 | R << 1.0, 0.0, 0.0, 0.0, cos(q_HAA_bf), -sin(q_HAA_bf), 0.0, sin(q_HAA_bf), cos(q_HAA_bf); 60 | 61 | xr = (R * xr).eval(); 62 | 63 | // translate into the HFE coordinate system (along Z axis) 64 | xr += hfe_to_haa_z; //distance of HFE to HAA in z direction 65 | 66 | // compute square of length from HFE to foot 67 | double tmp1 = pow(xr[X],2)+pow(xr[Z],2); 68 | 69 | 70 | // compute temporary angles (with reachability check) 71 | double lu = length_thigh; // length of upper leg 72 | double ll = length_shank; // length of lower leg 73 | double alpha = atan2(-xr[Z],xr[X]) - 0.5*M_PI; // flip and rotate to match HyQ joint definition 74 | 75 | 76 | double some_random_value_for_beta = (pow(lu,2)+tmp1-pow(ll,2))/(2.*lu*sqrt(tmp1)); // this must be between -1 and 1 77 | if (some_random_value_for_beta > 1) { 78 | some_random_value_for_beta = 1; 79 | } 80 | if (some_random_value_for_beta < -1) { 81 | some_random_value_for_beta = -1; 82 | } 83 | double beta = acos(some_random_value_for_beta); 84 | 85 | // compute Hip FE angle 86 | q_HFE_bf = q_HFE_br = alpha + beta; 87 | 88 | 89 | double some_random_value_for_gamma = (pow(ll,2)+pow(lu,2)-tmp1)/(2.*ll*lu); 90 | // law of cosines give the knee angle 91 | if (some_random_value_for_gamma > 1) { 92 | some_random_value_for_gamma = 1; 93 | } 94 | if (some_random_value_for_gamma < -1) { 95 | some_random_value_for_gamma = -1; 96 | } 97 | double gamma = acos(some_random_value_for_gamma); 98 | 99 | 100 | q_KFE_bf = q_KFE_br = gamma - M_PI; 101 | 102 | // forward knee bend 103 | EnforceLimits(q_HAA_bf, HAA); 104 | EnforceLimits(q_HFE_bf, HFE); 105 | EnforceLimits(q_KFE_bf, KFE); 106 | 107 | // backward knee bend 108 | EnforceLimits(q_HAA_br, HAA); 109 | EnforceLimits(q_HFE_br, HFE); 110 | EnforceLimits(q_KFE_br, KFE); 111 | 112 | if (bend==Forward) 113 | return Vector3d(q_HAA_bf, q_HFE_bf, q_KFE_bf); 114 | else // backward 115 | return Vector3d(q_HAA_br, -q_HFE_br, -q_KFE_br); 116 | } 117 | 118 | void 119 | HyqlegInverseKinematics::EnforceLimits (double& val, HyqJointID joint) const 120 | { 121 | // totally exaggerated joint angle limits 122 | const static double haa_min = -180; 123 | const static double haa_max = 90; 124 | 125 | const static double hfe_min = -90; 126 | const static double hfe_max = 90; 127 | 128 | const static double kfe_min = -180; 129 | const static double kfe_max = 0; 130 | 131 | // reduced joint angles for optimization 132 | static const std::map max_range { 133 | {HAA, haa_max/180.0*M_PI}, 134 | {HFE, hfe_max/180.0*M_PI}, 135 | {KFE, kfe_max/180.0*M_PI} 136 | }; 137 | 138 | // reduced joint angles for optimization 139 | static const std::map min_range { 140 | {HAA, haa_min/180.0*M_PI}, 141 | {HFE, hfe_min/180.0*M_PI}, 142 | {KFE, kfe_min/180.0*M_PI} 143 | }; 144 | 145 | double max = max_range.at(joint); 146 | val = val>max? max : val; 147 | 148 | double min = min_range.at(joint); 149 | val = val 31 | 32 | #include 33 | #include 34 | 35 | namespace xpp { 36 | 37 | Joints 38 | InverseKinematicsHyq1::GetAllJointAngles(const EndeffectorsPos& x_B) const 39 | { 40 | Eigen::Vector3d offset_base_to_hip(0.0, 0.0, 0.15); 41 | Eigen::VectorXd q0 = leg.GetJointAngles(x_B.at(0) + offset_base_to_hip); 42 | 43 | return Joints({q0}); 44 | } 45 | 46 | 47 | } /* namespace xpp */ 48 | 49 | 50 | -------------------------------------------------------------------------------- /robots/xpp_hyq/src/inverse_kinematics_hyq2.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | 38 | namespace xpp { 39 | 40 | Joints 41 | InverseKinematicsHyq2::GetAllJointAngles(const EndeffectorsPos& x_B) const 42 | { 43 | using namespace biped; 44 | std::vector q_vec; 45 | 46 | // make sure always exactly 2 elements 47 | auto x_biped_B = x_B.ToImpl(); 48 | x_biped_B.resize(2, x_biped_B.front()); 49 | 50 | q_vec.push_back(leg.GetJointAngles(x_biped_B.at(L) + Vector3d(0.0, -0.1, 0.15))); 51 | q_vec.push_back(leg.GetJointAngles(x_biped_B.at(R) + Vector3d(0.0, 0.1, 0.15))); 52 | 53 | 54 | return Joints(q_vec); 55 | } 56 | 57 | } /* namespace xpp */ 58 | 59 | 60 | -------------------------------------------------------------------------------- /robots/xpp_hyq/src/inverse_kinematics_hyq4.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | namespace xpp { 36 | 37 | Joints 38 | InverseKinematicsHyq4::GetAllJointAngles(const EndeffectorsPos& x_B) const 39 | { 40 | Vector3d ee_pos_H; // foothold expressed in hip frame 41 | std::vector q_vec; 42 | 43 | // make sure always exactly 4 elements 44 | auto pos_B = x_B.ToImpl(); 45 | pos_B.resize(4, pos_B.front()); 46 | 47 | for (int ee=0; ee 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | -------------------------------------------------------------------------------- /robots/xpp_hyq/urdf/hyq.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 29 | 30 | 31 | 32 | 33 | 39 | 40 | 41 | 42 | 43 | 49 | 50 | 51 | 52 | 53 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /robots/xpp_hyq/urdf/leg.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 85 | 86 | 87 | 88 | 89 | -------------------------------------------------------------------------------- /robots/xpp_hyq/urdf/monoped.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /robots/xpp_hyq/urdf/trunk.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /robots/xpp_quadrotor/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package quadrotor_description 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.6 (2018-04-18) 6 | ------------------ 7 | 8 | 1.0.10 (2019-04-05) 9 | ------------------- 10 | * add xacro dependency to xpp_hyq and xpp_quadrotor 11 | * Contributors: Alexander Winkler 12 | 13 | 1.0.9 (2018-07-10) 14 | ------------------ 15 | * Merge pull request #7 from zlingkang/xacropi2xacro 16 | change deprecated xacro.py to xacro 17 | * Contributors: zlingkang 18 | 19 | 1.0.8 (2018-07-07) 20 | ------------------ 21 | 22 | 1.0.7 (2018-07-03) 23 | ------------------ 24 | * modified default rviz launch scripts 25 | * Merge branch 'master' of github.com:leggedrobotics/xpp 26 | * 1.0.6 27 | * update changelogs 28 | * Contributors: Alexander Winkler 29 | 30 | 1.0.5 (2018-02-01) 31 | ------------------ 32 | * update launch scripts 33 | * modernize CMake files 34 | * use default BSD license 35 | * Contributors: Alexander Winkler 36 | 37 | 1.0.4 (2018-01-03) 38 | ------------------ 39 | 40 | 1.0.3 (2017-11-03) 41 | ------------------ 42 | * 1.0.2 43 | * update changelog 44 | * Contributors: Alexander Winkler 45 | 46 | 1.0.2 (2017-10-28) 47 | ------------------ 48 | 49 | 1.0.1 (2017-10-27) 50 | ------------------ 51 | 52 | 1.0.0 (2017-10-26) 53 | ------------------ 54 | * Added license to files and giving credit to contributors. 55 | * cleaned up package.xml and CMakeLists.txt files 56 | * showed close connection of mono/bi/quad to hyq 57 | * renamed urdf parent folder to robots 58 | * Contributors: Alexander Winkler 59 | -------------------------------------------------------------------------------- /robots/xpp_quadrotor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(xpp_quadrotor) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | ## Find catkin macros and libraries 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | xpp_vis 10 | ) 11 | 12 | catkin_package() 13 | 14 | ########### 15 | ## Build ## 16 | ########### 17 | ## Specify additional locations of header files 18 | include_directories( 19 | ${catkin_INCLUDE_DIRS} 20 | ) 21 | 22 | add_executable(urdf_visualizer_quadrotor src/exe/urdf_visualizer_quadrotor.cc) 23 | target_link_libraries(urdf_visualizer_quadrotor 24 | ${catkin_LIBRARIES} 25 | ) 26 | 27 | 28 | ############# 29 | ## Install ## 30 | ############# 31 | # Mark library for installation 32 | install( 33 | TARGETS urdf_visualizer_quadrotor 34 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 35 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 36 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 37 | ) 38 | 39 | # Mark other files for installation 40 | install( 41 | DIRECTORY launch meshes urdf 42 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 43 | ) -------------------------------------------------------------------------------- /robots/xpp_quadrotor/launch/quadrotor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /robots/xpp_quadrotor/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | xpp_quadrotor 5 | 1.0.10 6 | 7 | The URDF file for a quadrotor to be used with the xpp packages and a 8 | simple rviz publisher of quadrotor tfs. 9 | 10 | Adapted from Daniel Mellinger, Nathan Michael, Vijay Kumar, 11 | "Trajectory Generation and Control for Precise Aggressive Maneuvers 12 | with Quadrotors". 13 | 14 | 15 | Diego Pardo. 16 | Alexander W. Winkler 17 | Alexander W. Winkler 18 | BSD 19 | 20 | http://github.com/leggedrobotics/xpp 21 | http://github.com/leggedrobotics/xpp/issues 22 | 23 | catkin 24 | xacro 25 | roscpp 26 | xpp_vis 27 | -------------------------------------------------------------------------------- /robots/xpp_quadrotor/src/exe/urdf_visualizer_quadrotor.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | using namespace xpp; 38 | 39 | ros::Publisher joint_state_pub; 40 | 41 | // convert cartesian to joint state message (just by extracting base) 42 | void StateCallback(const xpp_msgs::RobotStateCartesian& msg) 43 | { 44 | xpp_msgs::RobotStateJoint joint_msg; 45 | joint_msg.base = msg.base; 46 | joint_state_pub.publish(joint_msg); 47 | } 48 | 49 | 50 | int main(int argc, char *argv[]) 51 | { 52 | ::ros::init(argc, argv, "quadrotor_urdf_visualizer"); 53 | 54 | ::ros::NodeHandle n; 55 | const std::string joint_quadrotor = "xpp/joint_quadrotor_des"; 56 | ros::Subscriber cart_state_sub = n.subscribe(xpp_msgs::robot_state_desired, 1, StateCallback); 57 | joint_state_pub = n.advertise(joint_quadrotor, 1); 58 | 59 | // publish base state to RVIZ 60 | std::string urdf = "quadrotor_rviz_urdf_robot_description"; 61 | UrdfVisualizer node_des(urdf, {}, "base", "world", joint_quadrotor, "quadrotor"); 62 | 63 | ::ros::spin(); 64 | 65 | return 0; 66 | } 67 | 68 | -------------------------------------------------------------------------------- /robots/xpp_quadrotor/urdf/quadrotor.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /xpp/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package xpp 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.10 (2019-04-05) 6 | ------------------- 7 | * Improve docs (`#8 `_) 8 | * Contributors: Alexander Winkler 9 | 10 | 1.0.9 (2018-07-10) 11 | ------------------ 12 | 13 | 1.0.8 (2018-07-07) 14 | ------------------ 15 | 16 | 1.0.7 (2018-07-03) 17 | ------------------ 18 | 19 | 1.0.6 (2018-04-18) 20 | ------------------ 21 | 22 | 1.0.5 (2018-02-01) 23 | ------------------ 24 | 25 | 1.0.4 (2018-01-03) 26 | ------------------ 27 | 28 | 1.0.3 (2017-11-03) 29 | ------------------ 30 | * removed xpp_ros_conversions (now in xpp_states) 31 | * 1.0.2 32 | * update changelog 33 | * Contributors: Alexander Winkler 34 | 35 | 1.0.2 (2017-10-28) 36 | ------------------ 37 | 38 | 1.0.1 (2017-10-27) 39 | ------------------ 40 | * xpp_vis: add visualization_msg dependency 41 | * Contributors: Alexander Winkler 42 | 43 | 1.0.0 (2017-10-26) 44 | ------------------ 45 | * changed email adresse to winklera@ethz.ch 46 | * added xpp metapackage 47 | * Contributors: Alexander Winkler 48 | 49 | 0.1.0 (2017-10-24) 50 | ------------------ 51 | -------------------------------------------------------------------------------- /xpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(xpp) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /xpp/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | xpp 5 | 1.0.10 6 | 7 | Visualization of motion-plans for legged robots. It draws support areas, 8 | contact forces and motion trajectories in RVIZ and displays URDFs for 9 | specific robots, including a one-legged, a two-legged hopper and 10 | HyQ. 11 | Example motions were generated by 12 | towr. 13 | 14 | 15 | Alexander W. Winkler 16 | Alexander W. Winkler 17 | BSD 18 | 19 | http://github.com/leggedrobotics/xpp 20 | http://github.com/leggedrobotics/xpp/issues 21 | 22 | catkin 23 | xpp_states 24 | xpp_msgs 25 | xpp_vis 26 | xpp_hyq 27 | xpp_quadrotor 28 | xpp_examples 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /xpp_examples/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package xpp_examples 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.10 (2019-04-05) 6 | ------------------- 7 | * Update README.txt 8 | * Improve docs (`#8 `_) 9 | * Contributors: Alexander Winkler 10 | 11 | 1.0.9 (2018-07-10) 12 | ------------------ 13 | 14 | 1.0.8 (2018-07-07) 15 | ------------------ 16 | 17 | 1.0.7 (2018-07-03) 18 | ------------------ 19 | * only show urdfs from current examples in rviz 20 | * Contributors: Alexander Winkler 21 | 22 | 1.0.6 (2018-04-18) 23 | ------------------ 24 | 25 | 1.0.5 (2018-02-01) 26 | ------------------ 27 | * add new example bags generated with towr 28 | * update launch scripts 29 | * Contributors: Alexander Winkler 30 | 31 | 1.0.4 (2018-01-03) 32 | ------------------ 33 | * Merge pull request #3 from leggedrobotics/devel 34 | Fix xacro bug and remove terrain and optimization specific classes. 35 | * renamed some launch files 36 | * Contributors: Alexander W Winkler 37 | 38 | 1.0.3 (2017-11-03) 39 | ------------------ 40 | * removed xpp_ros_conversions (now in xpp_states) 41 | * xpp_examples: removed installation of examples lib 42 | * xpp_examples: Install bags and launch files 43 | * 1.0.2 44 | * update changelog 45 | * Contributors: Alexander Winkler 46 | 47 | 1.0.2 (2017-10-28) 48 | ------------------ 49 | 50 | 1.0.1 (2017-10-27) 51 | ------------------ 52 | 53 | 1.0.0 (2017-10-26) 54 | ------------------ 55 | * added example bag files (150MB) 56 | * added separate xpp_example package and Readme.md 57 | * Contributors: Alexander Winkler 58 | -------------------------------------------------------------------------------- /xpp_examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(xpp_examples) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | ## Find catkin macros and libraries 7 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 8 | ## is used, also find other catkin packages 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | rosbag 12 | xpp_vis 13 | xpp_hyq 14 | xpp_quadrotor 15 | ) 16 | 17 | 18 | ################################### 19 | ## catkin specific configuration ## 20 | ################################### 21 | catkin_package() 22 | 23 | 24 | ########### 25 | ## Build ## 26 | ########### 27 | ## Specify additional locations of header files 28 | include_directories( 29 | ${catkin_INCLUDE_DIRS} 30 | ) 31 | 32 | add_executable(monoped_publisher src/monoped_pub.cc) 33 | target_link_libraries(monoped_publisher 34 | ${catkin_LIBRARIES} 35 | ) 36 | 37 | add_executable(quadrotor_bag_builder src/quadrotor_bag_builder.cc) 38 | target_link_libraries(quadrotor_bag_builder 39 | ${catkin_LIBRARIES} 40 | ) 41 | 42 | 43 | ############# 44 | ## Install ## 45 | ############# 46 | # Mark library for installation 47 | install( 48 | TARGETS monoped_publisher quadrotor_bag_builder 49 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 50 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 51 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 52 | ) 53 | 54 | # Mark other files for installation 55 | install( 56 | DIRECTORY bags launch 57 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 58 | ) -------------------------------------------------------------------------------- /xpp_examples/bags/README.txt: -------------------------------------------------------------------------------- 1 | These bags were generated using TOWR: 2 | https://github.com/ethz-adrl/towr 3 | 4 | The paper explaining and analysing some of these motions (e.g. biped gap): 5 | "Gait and Trajectory Optimization for Legged Systems through Phase-based 6 | End-Effector Parameterization" 7 | https://alex-winkler.com 8 | 9 | 10 | -------------------------------------------------------------------------------- /xpp_examples/bags/biped.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/xpp/5527b2820247c712dba622872a15f0242c2c60ed/xpp_examples/bags/biped.bag -------------------------------------------------------------------------------- /xpp_examples/bags/gap.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/xpp/5527b2820247c712dba622872a15f0242c2c60ed/xpp_examples/bags/gap.bag -------------------------------------------------------------------------------- /xpp_examples/bags/hyq.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/xpp/5527b2820247c712dba622872a15f0242c2c60ed/xpp_examples/bags/hyq.bag -------------------------------------------------------------------------------- /xpp_examples/bags/monoped.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/xpp/5527b2820247c712dba622872a15f0242c2c60ed/xpp_examples/bags/monoped.bag -------------------------------------------------------------------------------- /xpp_examples/launch/biped_bag.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /xpp_examples/launch/biped_gap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /xpp_examples/launch/hyq_bag.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /xpp_examples/launch/monoped_bag.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /xpp_examples/launch/monoped_generate.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /xpp_examples/launch/quadrotor_generate.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /xpp_examples/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | xpp_examples 5 | 1.0.10 6 | 7 | Examples of how to use the xpp framework. 8 | 9 | 10 | Alexander W. Winkler 11 | Alexander W. Winkler 12 | BSD 13 | 14 | http://github.com/leggedrobotics/xpp 15 | http://github.com/leggedrobotics/xpp/issues 16 | 17 | catkin 18 | roscpp 19 | rosbag 20 | xpp_vis 21 | xpp_hyq 22 | xpp_quadrotor 23 | -------------------------------------------------------------------------------- /xpp_examples/rviz/xpp_biped.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /CartesianVisualization1 8 | - /CartesianVisualization1/Namespaces1 9 | Splitter Ratio: 0.442424 10 | Tree Height: 929 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | - /CoGTrajectory1 24 | - /video_top1 25 | - /video_side1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: "" 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 0.1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.03 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 4 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 100 52 | Reference Frame: 53 | Value: true 54 | - Class: rviz/MarkerArray 55 | Enabled: true 56 | Marker Topic: /xpp/terrain 57 | Name: Terrain 58 | Namespaces: 59 | {} 60 | Queue Size: 100 61 | Value: true 62 | - Alpha: 1 63 | Axes Length: 0.1 64 | Axes Radius: 0.01 65 | Class: rviz/Pose 66 | Color: 255; 25; 0 67 | Enabled: true 68 | Head Length: 0.1 69 | Head Radius: 0.05 70 | Name: Desired Goal 71 | Shaft Length: 0.3 72 | Shaft Radius: 0.02 73 | Shape: Axes 74 | Topic: /xpp/goal 75 | Unreliable: false 76 | Value: true 77 | - Class: rviz/MarkerArray 78 | Enabled: true 79 | Marker Topic: /xpp/rviz_markers 80 | Name: CartesianVisualization 81 | Namespaces: 82 | base_pose: false 83 | cop: true 84 | ee_force: true 85 | endeffector_pos: true 86 | friction_cone: true 87 | gravity_force: true 88 | inverted_pendulum: false 89 | range_of_motion: true 90 | support_polygons: true 91 | Queue Size: 100 92 | Value: true 93 | - Alpha: 0.5 94 | Class: rviz/RobotModel 95 | Collision Enabled: false 96 | Enabled: true 97 | Links: 98 | All Links Enabled: true 99 | Expand Joint Details: false 100 | Expand Link Details: false 101 | Expand Tree: false 102 | L_hipassembly: 103 | Alpha: 1 104 | Show Axes: false 105 | Show Trail: false 106 | Value: true 107 | L_lowerleg: 108 | Alpha: 1 109 | Show Axes: false 110 | Show Trail: false 111 | Value: true 112 | L_upperleg: 113 | Alpha: 1 114 | Show Axes: false 115 | Show Trail: false 116 | Value: true 117 | Link Tree Style: Links in Alphabetic Order 118 | R_hipassembly: 119 | Alpha: 1 120 | Show Axes: false 121 | Show Trail: false 122 | Value: true 123 | R_lowerleg: 124 | Alpha: 1 125 | Show Axes: false 126 | Show Trail: false 127 | Value: true 128 | R_upperleg: 129 | Alpha: 1 130 | Show Axes: false 131 | Show Trail: false 132 | Value: true 133 | base: 134 | Alpha: 1 135 | Show Axes: false 136 | Show Trail: false 137 | Value: true 138 | Name: Biped 139 | Robot Description: /biped_rviz_urdf_robot_description 140 | TF Prefix: biped 141 | Update Interval: 0 142 | Value: true 143 | Visual Enabled: true 144 | Enabled: true 145 | Global Options: 146 | Background Color: 255; 255; 255 147 | Fixed Frame: world 148 | Frame Rate: 30 149 | Name: root 150 | Tools: 151 | - Class: rviz/Interact 152 | Hide Inactive Objects: true 153 | - Class: rviz/MoveCamera 154 | - Class: rviz/Select 155 | - Class: rviz/FocusCamera 156 | - Class: rviz/Measure 157 | - Class: rviz/SetInitialPose 158 | Topic: /initialpose 159 | - Class: rviz/SetGoal 160 | Topic: /move_base_simple/goal 161 | - Class: rviz/PublishPoint 162 | Single click: true 163 | Topic: /clicked_point 164 | Value: true 165 | Views: 166 | Current: 167 | Class: rviz/ThirdPersonFollower 168 | Distance: 6.0754 169 | Enable Stereo Rendering: 170 | Stereo Eye Separation: 0.06 171 | Stereo Focal Distance: 1 172 | Swap Stereo Eyes: false 173 | Value: false 174 | Focal Point: 175 | X: 4.5795 176 | Y: 0.780599 177 | Z: -0.579998 178 | Name: Current View 179 | Near Clip Distance: 0.01 180 | Pitch: 0.364798 181 | Target Frame: 182 | Value: ThirdPersonFollower (rviz) 183 | Yaw: 0.49898 184 | Saved: 185 | - Angle: 3.14 186 | Class: rviz/TopDownOrtho 187 | Enable Stereo Rendering: 188 | Stereo Eye Separation: 0.06 189 | Stereo Focal Distance: 1 190 | Swap Stereo Eyes: false 191 | Value: false 192 | Name: CoGTrajectory 193 | Near Clip Distance: 0.01 194 | Scale: 582.84 195 | Target Frame: 196 | Value: TopDownOrtho (rviz) 197 | X: 0.160456 198 | Y: 0.0931166 199 | - Angle: 6.26 200 | Class: rviz/TopDownOrtho 201 | Enable Stereo Rendering: 202 | Stereo Eye Separation: 0.06 203 | Stereo Focal Distance: 1 204 | Swap Stereo Eyes: false 205 | Value: false 206 | Name: video_top 207 | Near Clip Distance: 0.01 208 | Scale: 266.776 209 | Target Frame: 210 | Value: TopDownOrtho (rviz) 211 | X: 2.27941 212 | Y: -0.724513 213 | - Class: rviz/Orbit 214 | Distance: 3.56509 215 | Enable Stereo Rendering: 216 | Stereo Eye Separation: 0.06 217 | Stereo Focal Distance: 1 218 | Swap Stereo Eyes: false 219 | Value: false 220 | Focal Point: 221 | X: 4.22615 222 | Y: -1.12562 223 | Z: 0.170358 224 | Name: video_side 225 | Near Clip Distance: 0.01 226 | Pitch: 0.434798 227 | Target Frame: 228 | Value: Orbit (rviz) 229 | Yaw: 5.15 230 | - Class: rviz/Orbit 231 | Distance: 1.72568 232 | Enable Stereo Rendering: 233 | Stereo Eye Separation: 0.06 234 | Stereo Focal Distance: 1 235 | Swap Stereo Eyes: false 236 | Value: false 237 | Focal Point: 238 | X: 0.095682 239 | Y: -0.160401 240 | Z: -0.313986 241 | Name: fixed_to_hyq 242 | Near Clip Distance: 0.01 243 | Pitch: 0.349798 244 | Target Frame: base_link 245 | Value: Orbit (rviz) 246 | Yaw: 5.115 247 | Window Geometry: 248 | Displays: 249 | collapsed: false 250 | Height: 1173 251 | Hide Left Dock: false 252 | Hide Right Dock: false 253 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000432fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004300000432000000df00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000012f00000432fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004300000432000000b800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003bf0000003efc0100000002fb0000000800540069006d00650000000000000003bf0000022a00fffffffb0000000800540069006d006501000000000000045000000000000000000000060e0000043200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 254 | Selection: 255 | collapsed: false 256 | Time: 257 | collapsed: false 258 | Tool Properties: 259 | collapsed: false 260 | Views: 261 | collapsed: false 262 | Width: 1918 263 | X: 3831 264 | Y: -8 265 | -------------------------------------------------------------------------------- /xpp_examples/rviz/xpp_monoped.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /CartesianVisualization1 8 | - /CartesianVisualization1/Namespaces1 9 | Splitter Ratio: 0.442424 10 | Tree Height: 929 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | - /CoGTrajectory1 24 | - /video_top1 25 | - /video_side1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: "" 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 0.1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.03 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 4 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 100 52 | Reference Frame: 53 | Value: true 54 | - Class: rviz/MarkerArray 55 | Enabled: true 56 | Marker Topic: /xpp/terrain 57 | Name: Terrain 58 | Namespaces: 59 | {} 60 | Queue Size: 100 61 | Value: true 62 | - Alpha: 1 63 | Axes Length: 0.1 64 | Axes Radius: 0.01 65 | Class: rviz/Pose 66 | Color: 255; 25; 0 67 | Enabled: true 68 | Head Length: 0.1 69 | Head Radius: 0.05 70 | Name: Desired Goal 71 | Shaft Length: 0.3 72 | Shaft Radius: 0.02 73 | Shape: Axes 74 | Topic: /xpp/goal 75 | Unreliable: false 76 | Value: true 77 | - Class: rviz/MarkerArray 78 | Enabled: true 79 | Marker Topic: /xpp/rviz_markers 80 | Name: CartesianVisualization 81 | Namespaces: 82 | base_pose: false 83 | cop: false 84 | ee_force: true 85 | endeffector_pos: true 86 | gravity_force: false 87 | inverted_pendulum: false 88 | range_of_motion: true 89 | support_polygons: true 90 | Queue Size: 100 91 | Value: true 92 | - Alpha: 0.5 93 | Class: rviz/RobotModel 94 | Collision Enabled: false 95 | Enabled: true 96 | Links: 97 | All Links Enabled: true 98 | Expand Joint Details: false 99 | Expand Link Details: false 100 | Expand Tree: false 101 | Link Tree Style: Links in Alphabetic Order 102 | base: 103 | Alpha: 1 104 | Show Axes: false 105 | Show Trail: false 106 | Value: true 107 | hipassembly: 108 | Alpha: 1 109 | Show Axes: false 110 | Show Trail: false 111 | Value: true 112 | lowerleg: 113 | Alpha: 1 114 | Show Axes: false 115 | Show Trail: false 116 | Value: true 117 | upperleg: 118 | Alpha: 1 119 | Show Axes: false 120 | Show Trail: false 121 | Value: true 122 | Name: Monoped 123 | Robot Description: /monoped_rviz_urdf_robot_description 124 | TF Prefix: monoped 125 | Update Interval: 0 126 | Value: true 127 | Visual Enabled: true 128 | Enabled: true 129 | Global Options: 130 | Background Color: 255; 255; 255 131 | Fixed Frame: world 132 | Frame Rate: 30 133 | Name: root 134 | Tools: 135 | - Class: rviz/Interact 136 | Hide Inactive Objects: true 137 | - Class: rviz/MoveCamera 138 | - Class: rviz/Select 139 | - Class: rviz/FocusCamera 140 | - Class: rviz/Measure 141 | - Class: rviz/SetInitialPose 142 | Topic: /initialpose 143 | - Class: rviz/SetGoal 144 | Topic: /move_base_simple/goal 145 | - Class: rviz/PublishPoint 146 | Single click: true 147 | Topic: /clicked_point 148 | Value: true 149 | Views: 150 | Current: 151 | Class: rviz/ThirdPersonFollower 152 | Distance: 5.58413 153 | Enable Stereo Rendering: 154 | Stereo Eye Separation: 0.06 155 | Stereo Focal Distance: 1 156 | Swap Stereo Eyes: false 157 | Value: false 158 | Focal Point: 159 | X: 4.14322 160 | Y: 4.33649 161 | Z: -0.579997 162 | Name: Current View 163 | Near Clip Distance: 0.01 164 | Pitch: 0.424798 165 | Target Frame: 166 | Value: ThirdPersonFollower (rviz) 167 | Yaw: 1.22398 168 | Saved: 169 | - Angle: 3.14 170 | Class: rviz/TopDownOrtho 171 | Enable Stereo Rendering: 172 | Stereo Eye Separation: 0.06 173 | Stereo Focal Distance: 1 174 | Swap Stereo Eyes: false 175 | Value: false 176 | Name: CoGTrajectory 177 | Near Clip Distance: 0.01 178 | Scale: 582.84 179 | Target Frame: 180 | Value: TopDownOrtho (rviz) 181 | X: 0.160456 182 | Y: 0.0931166 183 | - Angle: 6.26 184 | Class: rviz/TopDownOrtho 185 | Enable Stereo Rendering: 186 | Stereo Eye Separation: 0.06 187 | Stereo Focal Distance: 1 188 | Swap Stereo Eyes: false 189 | Value: false 190 | Name: video_top 191 | Near Clip Distance: 0.01 192 | Scale: 266.776 193 | Target Frame: 194 | Value: TopDownOrtho (rviz) 195 | X: 2.27941 196 | Y: -0.724513 197 | - Class: rviz/Orbit 198 | Distance: 3.56509 199 | Enable Stereo Rendering: 200 | Stereo Eye Separation: 0.06 201 | Stereo Focal Distance: 1 202 | Swap Stereo Eyes: false 203 | Value: false 204 | Focal Point: 205 | X: 4.22615 206 | Y: -1.12562 207 | Z: 0.170358 208 | Name: video_side 209 | Near Clip Distance: 0.01 210 | Pitch: 0.434798 211 | Target Frame: 212 | Value: Orbit (rviz) 213 | Yaw: 5.15 214 | - Class: rviz/Orbit 215 | Distance: 1.72568 216 | Enable Stereo Rendering: 217 | Stereo Eye Separation: 0.06 218 | Stereo Focal Distance: 1 219 | Swap Stereo Eyes: false 220 | Value: false 221 | Focal Point: 222 | X: 0.095682 223 | Y: -0.160401 224 | Z: -0.313986 225 | Name: fixed_to_hyq 226 | Near Clip Distance: 0.01 227 | Pitch: 0.349798 228 | Target Frame: base_link 229 | Value: Orbit (rviz) 230 | Yaw: 5.115 231 | Window Geometry: 232 | Displays: 233 | collapsed: false 234 | Height: 1173 235 | Hide Left Dock: false 236 | Hide Right Dock: false 237 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000432fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004300000432000000df00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000012f00000432fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004300000432000000b800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003bf0000003efc0100000002fb0000000800540069006d00650000000000000003bf0000022a00fffffffb0000000800540069006d006501000000000000045000000000000000000000060e0000043200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 238 | Selection: 239 | collapsed: false 240 | Time: 241 | collapsed: false 242 | Tool Properties: 243 | collapsed: false 244 | Views: 245 | collapsed: false 246 | Width: 1918 247 | X: 3831 248 | Y: -8 249 | -------------------------------------------------------------------------------- /xpp_examples/src/monoped_pub.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | 39 | using namespace xpp; 40 | 41 | int main(int argc, char *argv[]) 42 | { 43 | ros::init(argc, argv, "monped_publisher_node"); 44 | 45 | ros::NodeHandle n; 46 | ros::Publisher state_pub = n.advertise(xpp_msgs::robot_state_desired, 1); 47 | ROS_INFO_STREAM("Waiting for Subscriber..."); 48 | while(ros::ok() && state_pub.getNumSubscribers() == 0) 49 | ros::Rate(100).sleep(); 50 | ROS_INFO_STREAM("Subscriber to initial state connected"); 51 | 52 | 53 | // visualize the state of a one-legged hopper 54 | RobotStateCartesian hopper(1); 55 | 56 | // publishes a sequence of states for a total duration of T spaced 0.01s apart. 57 | double T = 2.0; 58 | double t = 0.0; 59 | double dt = 0.01; 60 | while (t < T) 61 | { 62 | // base and foot follow half a sine motion up and down 63 | hopper.base_.lin.p_.z() = 0.7 - 0.05*sin(2*M_PI/(2*T)*t); 64 | hopper.ee_motion_.at(0).p_.z() = 0.1*sin(2*M_PI/(2*T)*t); 65 | hopper.ee_forces_.at(0).z() = 100; // N 66 | hopper.ee_contact_.at(0) = true; 67 | 68 | state_pub.publish(Convert::ToRos(hopper)); 69 | 70 | ros::spinOnce(); 71 | ros::Duration(dt).sleep(); // pause loop so visualization has correct speed. 72 | t += dt; 73 | } 74 | 75 | 76 | return 0; 77 | } 78 | 79 | -------------------------------------------------------------------------------- /xpp_examples/src/quadrotor_bag_builder.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | 38 | using namespace xpp; 39 | 40 | int main(int argc, char *argv[]) 41 | { 42 | // creates a bag file wherever executable is run (usually ~/.ros/) 43 | rosbag::Bag bag; 44 | bag.open("quadrotor_traj.bag", rosbag::bagmode::Write); 45 | 46 | // visualize the state of a drone (only has a base) 47 | State3d base; 48 | 49 | // create a sequence of states for a total duration of T spaced 0.01s apart. 50 | double T = 2*M_PI; 51 | double dt = 0.01; 52 | double t = 1e-6; 53 | while (t < T) 54 | { 55 | // base and foot follow half a sine motion up and down 56 | base.lin.p_.z() = 0.7 - 0.5*sin(t); //[m] 57 | base.lin.p_.x() = 1.0/T*t; //[m] 58 | 59 | double roll = 30./180*M_PI*sin(t); 60 | base.ang.q = GetQuaternionFromEulerZYX(0.0, 0.0, roll); 61 | 62 | // save the state message with current time in the bag 63 | auto timestamp = ::ros::Time(t); 64 | 65 | xpp_msgs::RobotStateJoint msg; 66 | msg.base = Convert::ToRos(base); 67 | bag.write("xpp/joint_quadrotor_des", timestamp, msg); 68 | 69 | t += dt; 70 | } 71 | 72 | std::string bag_file = bag.getFileName(); // save location before closing 73 | bag.close(); 74 | 75 | // plays back the states at desired speeds. 76 | // can now also use all the tools from rosbag to pause, speed-up, inspect, 77 | // debug the trajectory. 78 | // e.g. rosbag -play -r 0.2 /path/to/bag.bag 79 | int success = system(("rosbag play " + bag_file).c_str()); 80 | 81 | return 0; 82 | } 83 | 84 | -------------------------------------------------------------------------------- /xpp_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package xpp_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.10 (2019-04-05) 6 | ------------------- 7 | * Improve docs (`#8 `_) 8 | * Contributors: Alexander Winkler 9 | 10 | 1.0.9 (2018-07-10) 11 | ------------------ 12 | 13 | 1.0.8 (2018-07-07) 14 | ------------------ 15 | 16 | 1.0.7 (2018-07-03) 17 | ------------------ 18 | 19 | 1.0.6 (2018-04-18) 20 | ------------------ 21 | 22 | 1.0.5 (2018-02-01) 23 | ------------------ 24 | * modernize CMake files 25 | * use default BSD license 26 | * Contributors: Alexander Winkler 27 | 28 | 1.0.4 (2018-01-03) 29 | ------------------ 30 | * Merge pull request #3 from leggedrobotics/devel 31 | Fix xacro bug and remove terrain and optimization specific classes. 32 | * removed optimization related topic names 33 | * rename OptParameters.msg to more concise RobotParameters.msg 34 | * removed UserCommand msg and terrain_builder class 35 | * renamed some launch files 36 | * Contributors: Alexander W Winkler 37 | 38 | 1.0.3 (2017-11-03) 39 | ------------------ 40 | * 1.0.2 41 | * update changelog 42 | * Contributors: Alexander Winkler 43 | 44 | 1.0.2 (2017-10-28) 45 | ------------------ 46 | 47 | 1.0.1 (2017-10-27) 48 | ------------------ 49 | 50 | 1.0.0 (2017-10-26) 51 | ------------------ 52 | * removed robot specific bag files from xpp_msgs 53 | * added separate xpp_example package and Readme.md 54 | * included install space in CMakeLists.txt 55 | * changed catkin pkg dependencies to roscpp 56 | * formatting 57 | * Added license to files and giving credit to contributors. 58 | * cleaned up package.xml and CMakeLists.txt files 59 | * removed biped and quadruped joint identifier 60 | * Merge remote-tracking branch 'xpp_msgs/master' 61 | Conflicts: 62 | .gitignore 63 | * Contributors: Alexander Winkler 64 | -------------------------------------------------------------------------------- /xpp_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(xpp_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | message_generation 6 | std_msgs 7 | geometry_msgs 8 | sensor_msgs 9 | ) 10 | 11 | ## Generate messages in the 'msg' folder 12 | add_message_files( 13 | FILES 14 | StateLin3d.msg 15 | State6d.msg 16 | RobotStateCartesianTrajectory.msg 17 | RobotStateCartesian.msg 18 | RobotStateJoint.msg 19 | RobotParameters.msg 20 | TerrainInfo.msg 21 | ) 22 | 23 | ## Generate added messages and services with any dependencies listed here 24 | generate_messages( 25 | DEPENDENCIES 26 | std_msgs 27 | geometry_msgs 28 | sensor_msgs 29 | ) 30 | 31 | ################################### 32 | ## catkin specific configuration ## 33 | ################################### 34 | catkin_package( 35 | INCLUDE_DIRS include 36 | CATKIN_DEPENDS 37 | message_runtime 38 | std_msgs 39 | geometry_msgs 40 | sensor_msgs 41 | ) 42 | 43 | ############# 44 | ## Install ## 45 | ############# 46 | 47 | # Mark topic names header files for installation 48 | install( 49 | DIRECTORY include/${PROJECT_NAME}/ 50 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 51 | FILES_MATCHING PATTERN "*.h" 52 | ) 53 | -------------------------------------------------------------------------------- /xpp_msgs/include/xpp_msgs/topic_names.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | /** 31 | * @file topic_names.h 32 | * 33 | * Defines ROS topic names to be used to more reliably connect publisher and 34 | * subscriber nodes. 35 | */ 36 | 37 | #ifndef XPP_MSGS_TOPIC_NAMES_H_ 38 | #define XPP_MSGS_TOPIC_NAMES_H_ 39 | 40 | #include 41 | 42 | namespace xpp_msgs { 43 | 44 | // the current robot cartesian state including base, feet, time, ... 45 | static const std::string robot_state_current("/xpp/state_curr"); 46 | 47 | // the desired state that comes from the optimizer 48 | static const std::string robot_state_desired("/xpp/state_des"); 49 | 50 | // desired joint state (equivalent to desired cartesian state 51 | static const std::string joint_desired("/xpp/joint_des"); 52 | 53 | // sequence of desired states coming from the optimizer 54 | static const std::string robot_trajectory_desired("/xpp/trajectory_des"); 55 | 56 | // parameters describing the robot kinematics 57 | static const std::string robot_parameters("/xpp/params"); 58 | 59 | // information about terrain normals and friction coefficients 60 | static const std::string terrain_info("/xpp/terrain_info"); 61 | } 62 | 63 | #endif /* XPP_MSGS_TOPIC_NAMES_H_ */ 64 | -------------------------------------------------------------------------------- /xpp_msgs/msg/RobotParameters.msg: -------------------------------------------------------------------------------- 1 | # Parameters used to generate this optimization/trajectory 2 | # Should basically save class xpp::OptimizationParameters 3 | 4 | # endeffector names (order of endeffectors, e.g. LF, RF, LH, RH) 5 | string[] ee_names 6 | 7 | geometry_msgs/Point[] nominal_ee_pos # nominal position of each endeffector 8 | geometry_msgs/Vector3 ee_max_dev # the maximum distance the endeffector can deviate from the nominal position 9 | 10 | float64 base_mass # mass of the robot base (for plotting gravity force) 11 | -------------------------------------------------------------------------------- /xpp_msgs/msg/RobotStateCartesian.msg: -------------------------------------------------------------------------------- 1 | # The state of a robot expressed in the Cartesian frame 2 | 3 | duration time_from_start # global time along trajectory 4 | 5 | # Position, velocity and acceleration of the base expressed in world frame 6 | # The orientation quaternion maps base to world frame. 7 | State6d base # base pos/vel/acc in world 8 | 9 | StateLin3d[] ee_motion # endeffector pos/vel/acc in world 10 | geometry_msgs/Vector3[] ee_forces # endeffector forces expressed in world 11 | bool[] ee_contact # True if the foot is touching the environment 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /xpp_msgs/msg/RobotStateCartesianTrajectory.msg: -------------------------------------------------------------------------------- 1 | # The state of a robot expressed in the cartesian frame 2 | 3 | # The header is used to specify the coordinate frame and the reference time for the trajectory durations 4 | std_msgs/Header header 5 | 6 | # A representation of a Cartesian trajectory 7 | RobotStateCartesian[] points 8 | -------------------------------------------------------------------------------- /xpp_msgs/msg/RobotStateJoint.msg: -------------------------------------------------------------------------------- 1 | # The state of a robot expressed in the Cartesian frame 2 | 3 | duration time_from_start # global time along trajectory 4 | 5 | # Position, velocity and acceleration of the base expressed in world frame 6 | # The orientation quaternion maps base to world frame. 7 | State6d base # base pos/vel/acc in world 8 | 9 | sensor_msgs/JointState joint_state 10 | bool[] ee_contact # True if the foot is touching the environment 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /xpp_msgs/msg/State6d.msg: -------------------------------------------------------------------------------- 1 | # The state of the 6D base of a system 2 | 3 | geometry_msgs/Pose pose # The 6D linear and angular position, orientation maps base to world 4 | geometry_msgs/Twist twist # The 6D linear and angular velocity 5 | geometry_msgs/Accel accel # The 6D linear and angular acceleration -------------------------------------------------------------------------------- /xpp_msgs/msg/StateLin3d.msg: -------------------------------------------------------------------------------- 1 | # This contains the 3D representation of a linear state, including: 2 | # position, velocity, acceleration 3 | 4 | geometry_msgs/Point pos 5 | geometry_msgs/Vector3 vel 6 | geometry_msgs/Vector3 acc -------------------------------------------------------------------------------- /xpp_msgs/msg/TerrainInfo.msg: -------------------------------------------------------------------------------- 1 | # Extending the robot state 2 | 3 | geometry_msgs/Vector3[] surface_normals # at every endeffector, expressed in world 4 | float64 friction_coeff # friction coefficient -------------------------------------------------------------------------------- /xpp_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | xpp_msgs 5 | 1.0.10 6 | 7 | ROS messages used in the XPP framework. 8 | 9 | 10 | Alexander W. Winkler 11 | Alexander W. Winkler 12 | BSD 13 | 14 | http://github.com/leggedrobotics/xpp 15 | http://github.com/leggedrobotics/xpp/issues 16 | 17 | catkin 18 | 19 | message_runtime 20 | message_generation 21 | std_msgs 22 | geometry_msgs 23 | sensor_msgs 24 | 25 | -------------------------------------------------------------------------------- /xpp_states/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package xpp_states 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.10 (2019-04-05) 6 | ------------------- 7 | * Improve docs (`#8 `_) 8 | * Contributors: Alexander Winkler 9 | 10 | 1.0.9 (2018-07-10) 11 | ------------------ 12 | 13 | 1.0.8 (2018-07-07) 14 | ------------------ 15 | * avoid IK segfault for biped/hyq when not enough ee positions sent 16 | * Contributors: Alexander Winkler 17 | 18 | 1.0.7 (2018-07-03) 19 | ------------------ 20 | * add names to biped and quadruped ids 21 | * Contributors: Alexander Winkler 22 | 23 | 1.0.6 (2018-04-18) 24 | ------------------ 25 | 26 | 1.0.5 (2018-02-01) 27 | ------------------ 28 | * modernize CMake files 29 | * use default keyword for empty destructors 30 | * use default BSD license 31 | * move terrain types to towr_ros 32 | * Contributors: Alexander Winkler 33 | 34 | 1.0.4 (2018-01-03) 35 | ------------------ 36 | 37 | 1.0.3 (2017-11-03) 38 | ------------------ 39 | * removed xpp_ros_conversions (now in xpp_states) 40 | * 1.0.2 41 | * update changelog 42 | * Merge pull request #1 from mikaelarguedas/missing_includes 43 | add missing std includes 44 | * add missing std includes 45 | * Contributors: Alexander W Winkler, Mikael Arguedas 46 | 47 | 1.0.2 (2017-10-28) 48 | ------------------ 49 | * Merge pull request `#1 `_ from mikaelarguedas/missing_includes 50 | add missing std includes 51 | * add missing std includes 52 | * Contributors: Alexander W Winkler, Mikael Arguedas 53 | 54 | 1.0.1 (2017-10-27) 55 | ------------------ 56 | 57 | 1.0.0 (2017-10-26) 58 | ------------------ 59 | * removed robot specific bag files from xpp_msgs 60 | * included install space in CMakeLists.txt 61 | * changed catkin pkg dependencies to roscpp 62 | * formatting 63 | * Added license to files and giving credit to contributors. 64 | * removed some unneccessary dependencies. 65 | * cleaned up package.xml and CMakeLists.txt files 66 | * cleaned up endeffector and joint representation 67 | * showed close connection of mono/bi/quad to hyq 68 | * made xpp_vis robot independent. Added sample hopper executables 69 | * removed biped and quadruped joint identifier 70 | * added xpp_states 71 | * Contributors: Alexander Winkler 72 | -------------------------------------------------------------------------------- /xpp_states/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(xpp_states) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED) 7 | find_package(Eigen3 REQUIRED) 8 | 9 | 10 | ################################### 11 | ## catkin specific configuration ## 12 | ################################### 13 | catkin_package( 14 | INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIR} 15 | LIBRARIES ${PROJECT_NAME} 16 | ) 17 | 18 | ########### 19 | ## Build ## 20 | ########### 21 | ## Specify additional locations of header files 22 | include_directories( 23 | include 24 | ${EIGEN3_INCLUDE_DIR} 25 | ) 26 | 27 | add_library(${PROJECT_NAME} 28 | src/state.cc 29 | src/joints.cc 30 | src/robot_state_cartesian.cc 31 | src/robot_state_joint.cc 32 | ) 33 | target_link_libraries(${PROJECT_NAME} 34 | ${catkin_LIBRARIES} 35 | ) 36 | 37 | ############# 38 | ## Install ## 39 | ############# 40 | # Mark library for installation 41 | install( 42 | TARGETS ${PROJECT_NAME} 43 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 44 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 45 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 46 | ) 47 | 48 | # Mark header files for installation 49 | install( 50 | DIRECTORY include/${PROJECT_NAME}/ 51 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 52 | FILES_MATCHING PATTERN "*.h" 53 | ) -------------------------------------------------------------------------------- /xpp_states/include/xpp_states/cartesian_declarations.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | /** 31 | * @file cartesian_declartions.h 32 | * 33 | * Defines common conventions and index values to be used in Cartesian 34 | * environments. 35 | */ 36 | #ifndef XPP_CARTESIAN_DECLARATIONS_H_ 37 | #define XPP_CARTESIAN_DECLARATIONS_H_ 38 | 39 | #include 40 | 41 | namespace xpp { 42 | 43 | // 2-dimensional 44 | static constexpr int kDim2d = 2; 45 | enum Coords2D { X_=0, Y_}; 46 | 47 | // 3-dimensional 48 | static constexpr int kDim3d = 3; 49 | enum Coords3D { X=0, Y, Z }; 50 | static Coords2D To2D(Coords3D dim) 51 | { 52 | assert(dim != Z); 53 | return static_cast(dim); 54 | }; 55 | 56 | // 6-dimensional 57 | // 'A' stands for angular, 'L' for linear. 58 | static constexpr int kDim6d = 6; // X,Y,Z, roll, pitch, yaw 59 | enum Coords6D { AX=0, AY, AZ, LX, LY, LZ }; 60 | static const Coords6D AllDim6D[] = {AX, AY, AZ, LX, LY, LZ}; 61 | 62 | } // namespace xpp 63 | 64 | #endif /* XPP_CARTESIAN_DECLARATIONS_H_ */ 65 | -------------------------------------------------------------------------------- /xpp_states/include/xpp_states/convert.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef XPP_ROS_CONVERSIONS_H_ 31 | #define XPP_ROS_CONVERSIONS_H_ 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | #include 42 | 43 | namespace xpp { 44 | 45 | /** 46 | * @brief Converts between xpp-states types and xpp-messages. 47 | * 48 | * ToXpp() : Convert from ROS message (xpp_msgs) to state (xpp_states). 49 | * ToRos() : Convert from state (xpp_states) to ROS message (xpp_msgs). 50 | */ 51 | struct Convert { 52 | 53 | static StateLin3d 54 | ToXpp(const xpp_msgs::StateLin3d& ros) 55 | { 56 | StateLin3d point; 57 | point.p_.x() = ros.pos.x; 58 | point.p_.y() = ros.pos.y; 59 | point.p_.z() = ros.pos.z; 60 | 61 | point.v_.x() = ros.vel.x; 62 | point.v_.y() = ros.vel.y; 63 | point.v_.z() = ros.vel.z; 64 | 65 | point.a_.x() = ros.acc.x; 66 | point.a_.y() = ros.acc.y; 67 | point.a_.z() = ros.acc.z; 68 | 69 | return point; 70 | } 71 | 72 | static xpp_msgs::StateLin3d 73 | ToRos(const StateLin3d& xpp) 74 | { 75 | xpp_msgs::StateLin3d ros; 76 | ros.pos.x = xpp.p_.x(); 77 | ros.pos.y = xpp.p_.y(); 78 | ros.pos.z = xpp.p_.z(); 79 | 80 | ros.vel.x = xpp.v_.x(); 81 | ros.vel.y = xpp.v_.y(); 82 | ros.vel.z = xpp.v_.z(); 83 | 84 | ros.acc.x = xpp.a_.x(); 85 | ros.acc.y = xpp.a_.y(); 86 | ros.acc.z = xpp.a_.z(); 87 | 88 | return ros; 89 | } 90 | 91 | template 92 | static Vector3d 93 | ToXpp(const T& ros) 94 | { 95 | Vector3d vec; 96 | vec << ros.x, ros.y, ros.z; 97 | return vec; 98 | } 99 | 100 | template 101 | static T 102 | ToRos(const Vector3d& xpp) 103 | { 104 | T ros; 105 | ros.x = xpp.x(); 106 | ros.y = xpp.y(); 107 | ros.z = xpp.z(); 108 | 109 | return ros; 110 | } 111 | 112 | template 113 | static Endeffectors 114 | ToXpp(const std::vector& ros) 115 | { 116 | Endeffectors xpp(ros.size()); 117 | 118 | for (auto ee : xpp.GetEEsOrdered()) 119 | xpp.at(ee) = ToXpp(ros.at(ee)); 120 | 121 | return xpp; 122 | } 123 | 124 | static Eigen::Quaterniond 125 | ToXpp(const geometry_msgs::Quaternion ros) 126 | { 127 | Eigen::Quaterniond xpp; 128 | xpp.w() = ros.w; 129 | xpp.x() = ros.x; 130 | xpp.y() = ros.y; 131 | xpp.z() = ros.z; 132 | 133 | return xpp; 134 | } 135 | 136 | static geometry_msgs::Quaternion 137 | ToRos(const Eigen::Quaterniond xpp) 138 | { 139 | geometry_msgs::Quaternion ros; 140 | ros.w = xpp.w(); 141 | ros.x = xpp.x(); 142 | ros.y = xpp.y(); 143 | ros.z = xpp.z(); 144 | 145 | return ros; 146 | } 147 | 148 | static xpp_msgs::State6d 149 | ToRos(const State3d& xpp) 150 | { 151 | xpp_msgs::State6d msg; 152 | 153 | msg.pose.position = ToRos(xpp.lin.p_); 154 | msg.twist.linear = ToRos(xpp.lin.v_); 155 | msg.accel.linear = ToRos(xpp.lin.a_); 156 | 157 | msg.pose.orientation = ToRos(xpp.ang.q); 158 | msg.twist.angular = ToRos(xpp.ang.w); 159 | msg.accel.angular = ToRos(xpp.ang.wd); 160 | 161 | return msg; 162 | } 163 | 164 | static State3d 165 | ToXpp(const xpp_msgs::State6d& ros) 166 | { 167 | State3d xpp; 168 | 169 | xpp.lin.p_ = ToXpp(ros.pose.position); 170 | xpp.lin.v_ = ToXpp(ros.twist.linear); 171 | xpp.lin.a_ = ToXpp(ros.accel.linear); 172 | 173 | xpp.ang.q = ToXpp(ros.pose.orientation); 174 | xpp.ang.w = ToXpp(ros.twist.angular); 175 | xpp.ang.wd = ToXpp(ros.accel.angular); 176 | 177 | return xpp; 178 | } 179 | 180 | static xpp_msgs::RobotStateCartesian 181 | ToRos(const RobotStateCartesian& xpp) 182 | { 183 | xpp_msgs::RobotStateCartesian ros; 184 | 185 | ros.base = ToRos(xpp.base_); 186 | ros.time_from_start = ros::Duration(xpp.t_global_); 187 | 188 | for (auto ee : xpp.ee_contact_.GetEEsOrdered()) { 189 | ros.ee_motion. push_back(ToRos(xpp.ee_motion_.at(ee))); 190 | ros.ee_contact.push_back(xpp.ee_contact_.at(ee)); 191 | ros.ee_forces. push_back(ToRos(xpp.ee_forces_.at(ee))); 192 | } 193 | 194 | return ros; 195 | } 196 | 197 | static RobotStateCartesian 198 | ToXpp(const xpp_msgs::RobotStateCartesian& ros) 199 | { 200 | 201 | int n_ee = ros.ee_motion.size(); 202 | RobotStateCartesian xpp(n_ee); 203 | 204 | xpp.base_ = ToXpp(ros.base); 205 | xpp.t_global_ = ros.time_from_start.toSec(); 206 | 207 | for (auto ee : xpp.ee_contact_.GetEEsOrdered()) { 208 | xpp.ee_motion_.at(ee) = ToXpp(ros.ee_motion.at(ee)); 209 | xpp.ee_contact_.at(ee) = ros.ee_contact.at(ee); 210 | xpp.ee_forces_.at(ee) = ToXpp(ros.ee_forces.at(ee)); 211 | } 212 | 213 | return xpp; 214 | } 215 | 216 | static xpp_msgs::RobotStateCartesianTrajectory 217 | ToRos(const std::vector& xpp) 218 | { 219 | xpp_msgs::RobotStateCartesianTrajectory msg; 220 | 221 | for (const auto state : xpp) { 222 | auto state_msg = ToRos(state); 223 | msg.points.push_back(state_msg); 224 | } 225 | 226 | return msg; 227 | } 228 | 229 | static std::vector 230 | ToXpp(const xpp_msgs::RobotStateCartesianTrajectory& ros) 231 | { 232 | std::vector xpp_vec; 233 | 234 | for (const auto ros_state : ros.points) { 235 | auto xpp = ToXpp(ros_state); 236 | xpp_vec.push_back(xpp); 237 | } 238 | 239 | return xpp_vec; 240 | } 241 | 242 | }; 243 | 244 | } // namespace xpp 245 | 246 | #endif /* XPP_ROS_CONVERSIONS_H_ */ 247 | -------------------------------------------------------------------------------- /xpp_states/include/xpp_states/endeffector_mappings.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | /** 31 | * @file endeffector_mappings.h 32 | * 33 | * Assigning some semantic information (e.g. name of the foot) to endeffector 34 | * indices. 35 | */ 36 | #ifndef XPP_STATES_ENDEFFECTOR_MAPPINGS_H_ 37 | #define XPP_STATES_ENDEFFECTOR_MAPPINGS_H_ 38 | 39 | #include 40 | #include 41 | 42 | namespace xpp { 43 | 44 | namespace biped { 45 | enum FootIDs { L=0, R }; 46 | static std::map foot_to_name = 47 | { 48 | {L, "Left" }, 49 | {R, "Right"} 50 | }; 51 | } 52 | 53 | namespace quad { 54 | enum FootIDs { LF=0, RF, LH, RH }; 55 | static std::map foot_to_name = 56 | { 57 | {LF, "Left-Front" }, 58 | {RF, "Right-Front"}, 59 | {LH, "Left-Hind" }, 60 | {RH, "Right-Hind" } 61 | }; 62 | } 63 | 64 | } // namespace xpp 65 | 66 | #endif /* XPP_STATES_ENDEFFECTOR_MAPPINGS_H_ */ 67 | -------------------------------------------------------------------------------- /xpp_states/include/xpp_states/endeffectors.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef _XPP_STATES_ENDEFFECTORS_H_ 31 | #define _XPP_STATES_ENDEFFECTORS_H_ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | 39 | namespace xpp { 40 | 41 | using EndeffectorID = uint; 42 | 43 | /** 44 | * @brief Data structure to assign values to each endeffector. 45 | * 46 | * Common values are xyz-positions (Vector3d), contact-flags (bool), or joints 47 | * angles of each leg. 48 | * 49 | * This gives a unified interface, and also holds for boolean values (which 50 | * is not possible using an std::vector). It allows to restrict default 51 | * constructors, define addition and subtraction of all endeffectors and 52 | * other convenience functions associated specifically to endeffectors. 53 | * 54 | * The idea is that this class is an enhanced STL container, but complies to the 55 | * same interface, (e.g at()). However, in case this unified interface is 56 | * burdensome, you can always access the underlying STL-deque container directly. 57 | */ 58 | template 59 | class Endeffectors { 60 | public: 61 | using Container = std::deque; // only to avoid faulty std::vector 62 | using EndeffectorsT = Endeffectors; 63 | 64 | Endeffectors (int n_ee = 0); 65 | virtual ~Endeffectors () = default; 66 | 67 | /** 68 | * @brief Sets the number of endeffectors. 69 | */ 70 | void SetCount(int n_ee); 71 | 72 | /** 73 | * @brief Sets each endeffector to the same value. 74 | */ 75 | void SetAll(const T& value); 76 | 77 | /** 78 | * @returns Number of endeffectors this structure holds. 79 | */ 80 | int GetEECount() const; 81 | 82 | /** 83 | * @returns All endeffector IDs from 0 to the number of endeffectors. 84 | */ 85 | std::vector GetEEsOrdered() const; 86 | 87 | /** 88 | * @brief Read/write access to the endeffector stored at index ee. 89 | * @param ee Endeffector index/position. 90 | */ 91 | T& at(EndeffectorID ee); 92 | 93 | /** 94 | * @brief Read access to the endeffector stored at index ee. 95 | * @param ee Endeffector index/position. 96 | */ 97 | const T& at(EndeffectorID ee) const; 98 | 99 | const EndeffectorsT operator-(const EndeffectorsT& rhs) const; 100 | const EndeffectorsT operator/(double scalar) const; 101 | 102 | /** 103 | * @returns true if only one element differs from other. 104 | */ 105 | bool operator!=(const Endeffectors& other) const; 106 | 107 | /** 108 | * @returns a returns a copy(!) of the underlying STL-deque container. 109 | */ 110 | Container ToImpl() const; 111 | 112 | private: 113 | Container ee_; 114 | }; 115 | 116 | // convenience typedefs, can also be extended to derived classes if desired. 117 | using EndeffectorsPos = Endeffectors; 118 | using EndeffectorsVel = Endeffectors; 119 | using EndeffectorsAcc = Endeffectors; 120 | 121 | /** 122 | * @brief Bundles the position, velocity and acceleration of all endeffectors. 123 | * as well as appending a EndeffectorMotion specific convenience function. 124 | */ 125 | class EndeffectorsMotion : public Endeffectors { 126 | public: 127 | /** 128 | * @brief Extract only either the pos, vel or acc from all endeffectors. 129 | * @param deriv Derivative being either position, velocity or acceleration. 130 | */ 131 | Endeffectors Get (MotionDerivative deriv) const 132 | { 133 | Endeffectors val(GetEECount()); 134 | for (auto ee : GetEEsOrdered()) 135 | val.at(ee) = at(ee).GetByIndex(deriv); 136 | 137 | return val; 138 | } 139 | }; 140 | 141 | 142 | /** 143 | * @brief Bundles the contact state of all endeffectors. 144 | * 145 | * Says if an endeffector is currently touching the environment or not. 146 | * This is often an important criteria for motion planning, as only in the 147 | * contact state can forces be exerted that move the body. 148 | */ 149 | class EndeffectorsContact : public Endeffectors { 150 | public: 151 | /** 152 | * @brief Constructs a state, the default being 0 feet, none in contact. 153 | * @param n_ee Number of endeffectors. 154 | * @param in_contact True if all legs should be in contact, false otherwise. 155 | */ 156 | EndeffectorsContact (int n_ee=0, bool in_contact=false) 157 | :Endeffectors(n_ee) { SetAll(in_contact);}; 158 | 159 | /** 160 | * @brief The number of endeffectors in contact with the environment. 161 | */ 162 | int GetContactCount() const 163 | { 164 | int count = 0; 165 | for (auto ee : GetEEsOrdered()) 166 | if (at(ee)) 167 | count++; 168 | 169 | return count; 170 | } 171 | }; 172 | 173 | 174 | // implementations 175 | template 176 | Endeffectors::Endeffectors (int n_ee) 177 | { 178 | SetCount(n_ee); 179 | } 180 | 181 | template 182 | void 183 | Endeffectors::SetCount (int n_ee) 184 | { 185 | ee_.resize(n_ee); 186 | } 187 | 188 | template 189 | void 190 | Endeffectors::SetAll (const T& value) 191 | { 192 | std::fill(ee_.begin(), ee_.end(), value); 193 | } 194 | 195 | template 196 | T& 197 | Endeffectors::at (EndeffectorID idx) 198 | { 199 | return ee_.at(idx); 200 | } 201 | 202 | template 203 | const T& 204 | Endeffectors::at (EndeffectorID idx) const 205 | { 206 | return ee_.at(idx); 207 | } 208 | 209 | template 210 | int 211 | Endeffectors::GetEECount () const 212 | { 213 | return ee_.size(); 214 | } 215 | 216 | template 217 | typename Endeffectors::Container 218 | Endeffectors::ToImpl () const 219 | { 220 | return ee_; 221 | } 222 | 223 | template 224 | std::vector 225 | Endeffectors::GetEEsOrdered () const 226 | { 227 | std::vector vec; 228 | for (int i=0; i 235 | const typename Endeffectors::EndeffectorsT 236 | Endeffectors::operator - (const EndeffectorsT& rhs) const 237 | { 238 | EndeffectorsT result(ee_.size()); 239 | for (auto i : GetEEsOrdered()) 240 | result.at(i) = ee_.at(i) - rhs.at(i); 241 | 242 | return result; 243 | } 244 | 245 | template 246 | const typename Endeffectors::EndeffectorsT 247 | Endeffectors::operator / (double scalar) const 248 | { 249 | EndeffectorsT result(ee_.size()); 250 | for (auto i : GetEEsOrdered()) 251 | result.at(i) = ee_.at(i)/scalar; 252 | 253 | return result; 254 | } 255 | 256 | template 257 | std::ostream& operator<<(std::ostream& stream, Endeffectors endeffectors) 258 | { 259 | for (EndeffectorID ee : endeffectors.GetEEsOrdered()) 260 | stream << endeffectors.at(ee) << ", "; 261 | 262 | return stream; 263 | } 264 | 265 | template 266 | bool 267 | Endeffectors::operator!=(const Endeffectors& other) const 268 | { 269 | for (auto ee : GetEEsOrdered()) { 270 | if (ee_.at(ee) != other.at(ee)) 271 | return true; 272 | } 273 | return false; 274 | } 275 | 276 | } /* namespace xpp */ 277 | 278 | #endif /* _XPP_STATES_ENDEFFECTORS_H_ */ 279 | -------------------------------------------------------------------------------- /xpp_states/include/xpp_states/joints.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef _XPP_STATES_JOINTS_H_ 31 | #define _XPP_STATES_JOINTS_H_ 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | namespace xpp { 39 | 40 | /** 41 | * @brief Container to access joint values of each endeffectors. 42 | * 43 | * The idea is that every joint affects only one specific endeffector, so the 44 | * joints are grouped in this fashion. They can also be transformed to or set 45 | * from an undiscriminative Eigen::VectorXd. This however is not recommended, 46 | * as this Cartesian <->joint relationship is then lost/hidden. 47 | */ 48 | class Joints : public Endeffectors { 49 | public: 50 | using Base = Endeffectors; 51 | using Base::GetEECount; 52 | using Base::at; 53 | using EEOrder = std::vector; 54 | using JointID = uint; 55 | 56 | /** 57 | * @brief Constructs joint values all set to value. 58 | * @param n_ee total number of endeffectors. 59 | * @param n_joints_per_ee number of joints for each endeffector. 60 | * @param value same joint value set for each joint. 61 | * 62 | * Constructing joints just from the number of joints is not permitted, 63 | * as we enforce every joint to be assigned to an endeffector. This is 64 | * restrictive, however it helps to avoid bugs and makes later computations 65 | * easier. 66 | */ 67 | explicit Joints (int n_ee, int n_joints_per_ee, double value = 0.0); 68 | 69 | /** 70 | * @brief Converts a vector of leg joints into a Joint representation. 71 | * @param joints Joint values of each endeffector 72 | * 73 | * Attention: Each endeffector must have the same number of joints. 74 | */ 75 | explicit Joints (const std::vector& joints); 76 | virtual ~Joints () = default; 77 | 78 | /** 79 | * @brief Converts joint values to Eigen vector. 80 | * 81 | * The endeffectors are starting from zero and for each endeffector 82 | * the joints are appended in the order they where inserted. 83 | */ 84 | VectorXd ToVec() const; 85 | 86 | /** 87 | * @brief Converts joint values to Eigen vector according to specific order. 88 | * @param ee_order The order in which the endeffector's joints are appended. 89 | */ 90 | VectorXd ToVec(const EEOrder& ee_order) const; 91 | 92 | /** 93 | * @brief Sets joints values from Eigen vector. 94 | * @param q The Eigen Vector of joint values. 95 | * 96 | * The vector q is interpreted as if the top n_joints_per_leg values 97 | * rows belong to endeffector 0, the next n_joints_per_leg values to 98 | * endeffector 1 and so on. 99 | */ 100 | void SetFromVec(const VectorXd& q); 101 | 102 | /** 103 | * @brief Sets joint values from Eigen vector in specific order. 104 | * @param q The Eigen Vector of joint values. 105 | * @param ee_order Describes the internal order of q. 106 | */ 107 | void SetFromVec(const VectorXd& q, const EEOrder& ee_order); 108 | 109 | /** 110 | * @returns read/write access of the joint value at index joint. 111 | */ 112 | double& GetJoint(JointID joint); 113 | 114 | /** 115 | * @returns read access of the joint value at index joint. 116 | */ 117 | double GetJoint(JointID joint) const; 118 | 119 | int GetNumJoints() const; 120 | int GetNumJointsPerEE() const; 121 | 122 | private: 123 | int n_joints_per_leg_; 124 | int n_joints_; 125 | }; 126 | 127 | } /* namespace xpp */ 128 | 129 | #endif /* _XPP_STATES_JOINTS_H_ */ 130 | -------------------------------------------------------------------------------- /xpp_states/include/xpp_states/robot_state_cartesian.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef _XPP_STATES_ROBOT_STATE_CARTESIAN_H_ 31 | #define _XPP_STATES_ROBOT_STATE_CARTESIAN_H_ 32 | 33 | #include 34 | #include 35 | 36 | namespace xpp { 37 | 38 | /** 39 | * @brief Defines a complete robot state in Cartesian space. 40 | * 41 | * This is the alternative represenation to defining the robot by it's joint 42 | * angles. Here each endeffector (foot, hand) is given by a 3D-state and 43 | * joints angles could be obtained through Inverse Kinematics. 44 | * 45 | * see also robot_state_joint.h. 46 | */ 47 | class RobotStateCartesian { 48 | public: 49 | 50 | /** 51 | * @brief Constructs a zero initialized robot state with n_ee endeffectors. 52 | * @param n_ee Number of endeffectors. 53 | */ 54 | RobotStateCartesian(int n_ee); 55 | ~RobotStateCartesian() = default; 56 | 57 | State3d base_; 58 | EndeffectorsMotion ee_motion_; 59 | Endeffectors ee_forces_; 60 | EndeffectorsContact ee_contact_; 61 | double t_global_; 62 | }; 63 | 64 | } /* namespace xpp */ 65 | 66 | #endif /* _XPP_STATES_ROBOT_STATE_CARTESIAN_H_ */ 67 | -------------------------------------------------------------------------------- /xpp_states/include/xpp_states/robot_state_joint.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef XPP_STATES_ROBOT_STATE_JOINT_H_ 31 | #define XPP_STATES_ROBOT_STATE_JOINT_H_ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace xpp { 38 | 39 | /** 40 | * @brief Defines a complete robot state in joint space. 41 | * 42 | * This is the alternative representation to defining the robot by its 43 | * endeffector state. 44 | * 45 | * see also robot_state_joint.h. 46 | */ 47 | class RobotStateJoint { 48 | public: 49 | /** 50 | * @brief Constructs a zero initialized robot state. 51 | * @param n_ee Number of endeffectors (hands, feet). 52 | * @param n_joints_per_ee Number of joints for each endeffector. 53 | */ 54 | RobotStateJoint (int n_ee, int n_joints_per_ee); 55 | virtual ~RobotStateJoint () = default; 56 | 57 | State3d base_; 58 | Joints q_, qd_, qdd_; 59 | Joints torques_; 60 | EndeffectorsContact ee_contact_; 61 | double t_global_; 62 | }; 63 | 64 | } /* namespace xpp */ 65 | 66 | #endif /* XPP_STATES_ROBOT_STATE_JOINT_H_ */ 67 | -------------------------------------------------------------------------------- /xpp_states/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | xpp_states 5 | 1.0.10 6 | 7 | Common definitions (positions, velocities, angular angles, 8 | angular rates) and robot definitions in Cartesian and joint state 9 | used in the Xpp Motion Framework, as well as conversions to/from 10 | xpp_msgs. 11 | 12 | 13 | Alexander W. Winkler 14 | Alexander W. Winkler 15 | BSD 16 | 17 | http://github.com/leggedrobotics/xpp 18 | http://github.com/leggedrobotics/xpp/issues 19 | 20 | catkin 21 | eigen 22 | -------------------------------------------------------------------------------- /xpp_states/src/joints.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | namespace xpp { 33 | 34 | 35 | Joints::Joints (int n_ee, int n_joints_per_leg, double value) 36 | : Base(n_ee) 37 | { 38 | n_joints_per_leg_ = n_joints_per_leg; 39 | n_joints_ = n_ee * n_joints_per_leg; 40 | 41 | SetAll(VectorXd::Constant(n_joints_per_leg, value)); 42 | } 43 | 44 | Joints::Joints (const std::vector& q_vec) 45 | : Joints(q_vec.size(), q_vec.front().rows()) 46 | { 47 | for (auto ee : GetEEsOrdered()) 48 | at(ee) = q_vec.at(ee); 49 | } 50 | 51 | int 52 | Joints::GetNumJoints () const 53 | { 54 | return n_joints_; 55 | } 56 | 57 | int 58 | Joints::GetNumJointsPerEE () const 59 | { 60 | return n_joints_per_leg_; 61 | } 62 | 63 | VectorXd 64 | Joints::ToVec (const EEOrder& ee_order) const 65 | { 66 | VectorXd q_combined(n_joints_); 67 | int j = 0; 68 | 69 | for (auto ee : ee_order) { 70 | q_combined.middleRows(j, n_joints_per_leg_) = at(ee); 71 | j += n_joints_per_leg_; 72 | } 73 | 74 | return q_combined; 75 | } 76 | 77 | void 78 | Joints::SetFromVec (const VectorXd& xpp, const EEOrder& ee_order) 79 | { 80 | int j = 0; 81 | 82 | for (auto ee : ee_order) { 83 | at(ee) = xpp.middleRows(j, n_joints_per_leg_); 84 | j += n_joints_per_leg_; 85 | } 86 | } 87 | 88 | VectorXd 89 | Joints::ToVec () const 90 | { 91 | return ToVec(GetEEsOrdered()); 92 | } 93 | 94 | void 95 | Joints::SetFromVec (const VectorXd& q) 96 | { 97 | SetFromVec(q, GetEEsOrdered()); 98 | } 99 | 100 | double& 101 | Joints::GetJoint (JointID joint) 102 | { 103 | div_t result = std::div(joint, n_joints_per_leg_); 104 | EndeffectorID ee = result.quot; 105 | return at(ee)[result.rem]; 106 | } 107 | 108 | double 109 | Joints::GetJoint (JointID joint) const 110 | { 111 | return ToVec()[joint]; 112 | } 113 | 114 | } /* namespace xpp */ 115 | -------------------------------------------------------------------------------- /xpp_states/src/robot_state_cartesian.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | namespace xpp { 33 | 34 | RobotStateCartesian::RobotStateCartesian(int n_ee) 35 | { 36 | ee_motion_.SetCount(n_ee); 37 | ee_forces_.SetCount(n_ee); 38 | ee_contact_.SetCount(n_ee); 39 | ee_contact_.SetAll(true); 40 | t_global_ = 0.0; 41 | }; 42 | 43 | } /* namespace xpp */ 44 | 45 | -------------------------------------------------------------------------------- /xpp_states/src/robot_state_joint.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | namespace xpp { 33 | 34 | RobotStateJoint::RobotStateJoint (int n_ee, int n_joints_per_ee) 35 | :q_(n_ee, n_joints_per_ee), 36 | qd_(n_ee, n_joints_per_ee), 37 | qdd_(n_ee, n_joints_per_ee), 38 | torques_(n_ee, n_joints_per_ee) 39 | { 40 | ee_contact_.SetCount(n_ee); 41 | ee_contact_.SetAll(true); 42 | t_global_ = 0.0; 43 | } 44 | 45 | } /* namespace xpp */ 46 | -------------------------------------------------------------------------------- /xpp_states/src/state.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | #include 33 | 34 | namespace xpp { 35 | 36 | StateLinXd::StateLinXd (int dim) 37 | { 38 | kNumDim = dim; 39 | p_ = VectorXd::Zero(dim); 40 | v_ = VectorXd::Zero(dim); 41 | a_ = VectorXd::Zero(dim); 42 | } 43 | 44 | StateLinXd::StateLinXd (const VectorXd& _p, 45 | const VectorXd& _v, 46 | const VectorXd& _a) 47 | :StateLinXd(_p.rows()) 48 | { 49 | p_ = _p; 50 | v_ = _v; 51 | a_ = _a; 52 | } 53 | 54 | StateLinXd::StateLinXd (const VectorXd& _p) 55 | :StateLinXd(_p.rows()) 56 | { 57 | p_ = _p; 58 | } 59 | 60 | const VectorXd 61 | StateLinXd::GetByIndex (MotionDerivative deriv) const 62 | { 63 | switch (deriv) { 64 | case kPos: return p_; break; 65 | case kVel: return v_; break; 66 | case kAcc: return a_; break; 67 | default: throw std::runtime_error("[StateLinXd::GetByIndex] derivative not part of state"); 68 | } 69 | } 70 | 71 | VectorXd& 72 | StateLinXd::GetByIndex (MotionDerivative deriv) 73 | { 74 | switch (deriv) { 75 | case kPos: return p_; break; 76 | case kVel: return v_; break; 77 | case kAcc: return a_; break; 78 | default: throw std::runtime_error("[StateLinXd::GetByIndex] derivative not part of state"); 79 | } 80 | } 81 | 82 | StateLin3d::StateLin3d (const StateLinXd& state_xd) : StateLinXd(3) 83 | { 84 | assert(state_xd.kNumDim == 3); 85 | 86 | p_ = state_xd.p_; 87 | v_ = state_xd.v_; 88 | a_ = state_xd.a_; 89 | } 90 | 91 | StateLin2d 92 | StateLin3d::Get2D() const 93 | { 94 | StateLin2d p2d; 95 | p2d.p_ = p_.topRows(); 96 | p2d.v_ = v_.topRows(); 97 | p2d.a_ = a_.topRows(); 98 | return p2d; 99 | } 100 | 101 | Vector6d 102 | State3d::Get6dVel () const 103 | { 104 | Vector6d h_xd; 105 | h_xd.segment(AX, 3) = ang.w; 106 | h_xd.segment(LX, 3) = lin.v_; 107 | return h_xd; 108 | } 109 | 110 | Vector6d 111 | State3d::Get6dAcc () const 112 | { 113 | Vector6d h_xdd; 114 | h_xdd.segment(AX, 3) = ang.wd; 115 | h_xdd.segment(LX, 3) = lin.a_; 116 | return h_xdd; 117 | } 118 | 119 | } // namespace xpp 120 | 121 | -------------------------------------------------------------------------------- /xpp_vis/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package xpp_vis 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.10 (2019-04-05) 6 | ------------------- 7 | * checking sum of the contact forces was hiding negative forces. Use norm instead (`#10 `_) 8 | * Improve docs (`#8 `_) 9 | * Contributors: Alexander Winkler, Ruben Grandia 10 | 11 | 1.0.9 (2018-07-10) 12 | ------------------ 13 | 14 | 1.0.8 (2018-07-07) 15 | ------------------ 16 | * update catkin syntax for unit tests 17 | * Fix unit test through visualizer improvement. 18 | * fix wrongly displaying old markers when switching robots. 19 | * make printouts in DEVEL only 20 | * Contributors: Alexander Winkler 21 | 22 | 1.0.7 (2018-07-03) 23 | ------------------ 24 | * modified default rviz launch scripts 25 | * Contributors: Alexander Winkler 26 | 27 | 1.0.6 (2018-04-18) 28 | ------------------ 29 | 30 | 1.0.5 (2018-02-01) 31 | ------------------ 32 | * update launch scripts 33 | * cleaned-up some cmake files 34 | * modernize CMake files 35 | * move terrain types to towr_ros 36 | * Contributors: Alexander Winkler 37 | 38 | 1.0.4 (2018-01-03) 39 | ------------------ 40 | * Merge pull request #3 from leggedrobotics/devel 41 | Fix xacro bug and remove terrain and optimization specific classes. 42 | * removed optimization related topic names 43 | * rename OptParameters.msg to more concise RobotParameters.msg 44 | * removed UserCommand msg and terrain_builder class 45 | * use =default for empty c'tor/d'tor. 46 | * [smell] removed quiet return function that gives no warning. 47 | * Contributors: Alexander W Winkler 48 | 49 | 1.0.3 (2017-11-03) 50 | ------------------ 51 | * changed rviz default config name 52 | * removed xpp_ros_conversions (now in xpp_states) 53 | * 1.0.2 54 | * update changelog 55 | * Merge pull request #1 from mikaelarguedas/missing_includes 56 | add missing std includes 57 | * add missing std includes 58 | * Contributors: Alexander W Winkler, Mikael Arguedas 59 | 60 | 1.0.2 (2017-10-28) 61 | ------------------ 62 | * Merge pull request `#1 `_ from mikaelarguedas/missing_includes 63 | add missing std includes 64 | * add missing std includes 65 | * Contributors: Alexander W Winkler, Mikael Arguedas 66 | 67 | 1.0.1 (2017-10-27) 68 | ------------------ 69 | * xpp_vis: add visualization_msg dependency 70 | * Contributors: Alexander W Winkler 71 | 72 | 1.0.0 (2017-10-26) 73 | ------------------ 74 | * removed robot specific bag files from xpp_msgs 75 | * added separate xpp_example package and Readme.md 76 | * fixed unit test 77 | * included install space in CMakeLists.txt 78 | * explicitly listing ros dependencies (not through catkin_package macro) 79 | * removed user interface node and deprecated keyboard dependency. 80 | * changed catkin pkg dependencies to roscpp 81 | * Added license to files and giving credit to contributors. 82 | * removed some unneccessary dependencies. 83 | * cleaned up package.xml and CMakeLists.txt files 84 | * cleaned up endeffector and joint representation 85 | * showed close connection of mono/bi/quad to hyq 86 | * added doxygen comments to header files 87 | * made xpp_vis robot independent. Added sample hopper executables 88 | * removed biped and quadruped joint identifier 89 | * moved includes into subfolder xpp_vis 90 | * added xpp_vis 91 | * Contributors: Alexander W Winkler 92 | -------------------------------------------------------------------------------- /xpp_vis/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(xpp_vis) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | ## Find catkin macros and libraries 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | tf 10 | kdl_parser 11 | robot_state_publisher 12 | visualization_msgs 13 | xpp_states 14 | xpp_msgs 15 | ) 16 | 17 | ################################### 18 | ## catkin specific configuration ## 19 | ################################### 20 | ## The catkin_package macro generates cmake config files for your package 21 | catkin_package( 22 | INCLUDE_DIRS include 23 | LIBRARIES ${PROJECT_NAME} 24 | CATKIN_DEPENDS xpp_states xpp_msgs 25 | ) 26 | 27 | 28 | ########### 29 | ## Build ## 30 | ########### 31 | ## Specify additional locations of header files 32 | include_directories( 33 | include 34 | ${catkin_INCLUDE_DIRS} 35 | ) 36 | 37 | # Declare a C++ library 38 | add_library(${PROJECT_NAME} 39 | src/urdf_visualizer.cc 40 | src/cartesian_joint_converter.cc 41 | src/rviz_robot_builder.cc 42 | ) 43 | target_link_libraries(${PROJECT_NAME} 44 | ${catkin_LIBRARIES} 45 | ) 46 | 47 | 48 | # some executable nodes 49 | add_executable(rviz_marker_node src/exe/rviz_marker_node.cc) 50 | target_link_libraries(rviz_marker_node 51 | ${PROJECT_NAME} 52 | ${catkin_LIBRARIES} 53 | ) 54 | 55 | 56 | ############# 57 | ## Install ## 58 | ############# 59 | # Mark library for installation 60 | install( 61 | TARGETS ${PROJECT_NAME} rviz_marker_node 62 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 63 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 64 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 65 | ) 66 | 67 | # Mark header files for installation 68 | install( 69 | DIRECTORY include/${PROJECT_NAME}/ 70 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 71 | FILES_MATCHING PATTERN "*.h" 72 | ) 73 | 74 | # Mark other files for installation 75 | install( 76 | DIRECTORY launch rviz 77 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 78 | ) 79 | 80 | 81 | ############# 82 | ## Testing ## 83 | ############# 84 | if (CATKIN_ENABLE_TESTING) 85 | catkin_add_gtest(${PROJECT_NAME}_test 86 | test/gtest_main.cc 87 | test/rviz_robot_builder_test.cc 88 | ) 89 | target_link_libraries(${PROJECT_NAME}_test 90 | ${PROJECT_NAME} 91 | ${catkin_LIBRARIES} 92 | ) 93 | endif() 94 | -------------------------------------------------------------------------------- /xpp_vis/include/xpp_vis/cartesian_joint_converter.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef XPP_VIS_CARTESIAN_JOINT_CONVERTER_H_ 31 | #define XPP_VIS_CARTESIAN_JOINT_CONVERTER_H_ 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | #include 39 | 40 | #include "inverse_kinematics.h" 41 | 42 | namespace xpp { 43 | 44 | /** 45 | * @brief Converts a Cartesian robot representation to joint angles. 46 | * 47 | * This class subscribes to a Cartesian robot state message and publishes 48 | * the, through inverse kinematics converted, joint state message. This 49 | * can then be used to visualize URDFs in RVIZ. 50 | */ 51 | class CartesianJointConverter { 52 | public: 53 | /** 54 | * @brief Creates a converter initializing the subscriber and publisher. 55 | * @param ik The %InverseKinematics to use for conversion. 56 | * @param cart_topic The ROS topic containing the Cartesian robot state. 57 | * @param joint_topic The ROS topic to publish for the URDF visualization. 58 | */ 59 | CartesianJointConverter (const InverseKinematics::Ptr& ik, 60 | const std::string& cart_topic, 61 | const std::string& joint_topic); 62 | virtual ~CartesianJointConverter () = default; 63 | 64 | private: 65 | void StateCallback(const xpp_msgs::RobotStateCartesian& msg); 66 | 67 | ros::Subscriber cart_state_sub_; 68 | ros::Publisher joint_state_pub_; 69 | 70 | InverseKinematics::Ptr inverse_kinematics_; 71 | }; 72 | 73 | } /* namespace xpp */ 74 | 75 | #endif /* XPP_VIS_CARTESIAN_JOINT_CONVERTER_H_ */ 76 | -------------------------------------------------------------------------------- /xpp_vis/include/xpp_vis/inverse_kinematics.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef XPP_VIS_INVERSE_KINEMATICS_H_ 31 | #define XPP_VIS_INVERSE_KINEMATICS_H_ 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | namespace xpp { 40 | 41 | /** 42 | * @brief Converts Cartesian endeffector positions into joint angles. 43 | * 44 | * This class is responsible for calculating the joint angles of a robot 45 | * given an endeffector position (=inverse kinematics). 46 | * Base class that every inverse dynamics class must conform with. 47 | */ 48 | class InverseKinematics { 49 | public: 50 | using Ptr = std::shared_ptr; 51 | using Vector3d = Eigen::Vector3d; 52 | 53 | InverseKinematics () = default; 54 | virtual ~InverseKinematics () = default; 55 | 56 | /** 57 | * @brief Calculates the joint angles to reach a position @ pos_b. 58 | * @param pos_b 3D-position of the endeffector expressed in base frame. 59 | * @return Joints angles of the robot. 60 | */ 61 | virtual Joints GetAllJointAngles(const EndeffectorsPos& pos_b) const = 0; 62 | 63 | /** 64 | * @brief Number of endeffectors (feet, hands) this implementation expects. 65 | */ 66 | virtual int GetEECount() const = 0; 67 | }; 68 | 69 | } /* namespace xpp */ 70 | 71 | #endif /* XPP_VIS_INVERSE_KINEMATICS_H_ */ 72 | -------------------------------------------------------------------------------- /xpp_vis/include/xpp_vis/rviz_colors.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef XPP_VIS_RVIZ_COLORS_H_ 31 | #define XPP_VIS_RVIZ_COLORS_H_ 32 | 33 | #include 34 | 35 | namespace xpp { 36 | 37 | /** 38 | * @brief Supplies some predefined colors to be used for RVIZ markers. 39 | * 40 | * A static object "color" of this is creates, so in order to use the colors, 41 | * type e.g. color.red; 42 | */ 43 | struct ColorT { 44 | 45 | ColorT() { 46 | // set alpha for all 47 | red.a = green.a = blue.a = white.a = brown.a 48 | = yellow.a = purple.a = black.a = gray.a = wheat.a = 1.0; 49 | 50 | black.r = black.g = black.b = 0.1; 51 | gray.r = gray.g = gray.b = 0.7; 52 | white.b = white.g = white.r = 1.0; 53 | red.r = 1.0; red.g = 0.0; red.b = 0.0; 54 | green.r = 0.0; green.g = 150./255; green.b = 76./255; 55 | blue.r = 0.0; blue.g = 102./255; blue.b = 204./255; 56 | dblue.r = 0.0; dblue.g = 0./255; dblue.b = 128./255; 57 | brown.r = 122./255; brown.g = 61./255; brown.b = 0.0; 58 | yellow.r = 204./255; yellow.g = 204./255; yellow.b = 0.0; 59 | purple.r = 72./255; purple.g = 61./255; purple.b = 139./255; 60 | wheat.r = 245./355; wheat.g = 222./355; wheat.b = 179./355; 61 | }; 62 | 63 | virtual ~ColorT() {}; 64 | 65 | std_msgs::ColorRGBA red, green, blue, dblue, white, brown, 66 | yellow, purple, black, gray, wheat; 67 | }; 68 | 69 | static ColorT color; // create instance for efficient access 70 | 71 | } /* namespace xpp */ 72 | 73 | #endif /* XPP_VIS_RVIZ_COLORS_H_ */ 74 | -------------------------------------------------------------------------------- /xpp_vis/include/xpp_vis/rviz_robot_builder.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef XPP_VIS_RVIZ_ROBOT_BUILDER_H_ 31 | #define XPP_VIS_RVIZ_ROBOT_BUILDER_H_ 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | 46 | namespace xpp { 47 | 48 | /** 49 | * @brief Constructs RVIZ markers that visualize a Cartesian robot state. 50 | * 51 | * This class is responsible for converting robot states into beautiful RVIZ 52 | * markers. The visualize quantities such as 6D base state, endeffector 53 | * positions, contact state of these, endeffector forces, support areas and 54 | * center of pressure. 55 | */ 56 | class RvizRobotBuilder { 57 | public: 58 | using Marker = visualization_msgs::Marker; 59 | using MarkerVec = std::vector; 60 | using MarkerArray = visualization_msgs::MarkerArray; 61 | 62 | using ContactState = EndeffectorsContact; 63 | using EEPos = EndeffectorsPos; 64 | using EEForces = Endeffectors; 65 | using TerrainNormals = Endeffectors; 66 | using RobotState = RobotStateCartesian; 67 | 68 | public: 69 | /** 70 | * @brief Builds an uninitialized visualizer. 71 | */ 72 | RvizRobotBuilder(); 73 | virtual ~RvizRobotBuilder () = default; 74 | 75 | /** 76 | * @brief Constructs the RVIZ markers from the ROS message. 77 | * @param msg The ROS message describing the Cartesian robot state. 78 | * @return The array of RVIZ markers to be published. 79 | */ 80 | MarkerArray BuildRobotState(const xpp_msgs::RobotStateCartesian& msg) const; 81 | 82 | /** 83 | * @brief Provides additional robot info that can be used for visualization. 84 | * @param msg The ROS message. 85 | * 86 | * This additional information includes such values as the robots mass 87 | * (for gravity force visualization), the robots nominal endeffector 88 | * configuration, etc (see %OptParameters). 89 | */ 90 | void SetRobotParameters(const xpp_msgs::RobotParameters& msg); 91 | 92 | /** 93 | * @brief Additional information that can be used for visualization. 94 | * @param msg The ROS message. 95 | * 96 | * This information is related to the terrain, such as the current terrain 97 | * normals at the endeffectors and friction coefficient 98 | * (for friction cone visualization). 99 | */ 100 | void SetTerrainParameters(const xpp_msgs::TerrainInfo& msg); 101 | 102 | private: 103 | // various modular functions that are stitched together to generate the 104 | // robot state. 105 | // pos_W = position expressed in world frame 106 | // f_W = forces expressed in world frame. 107 | // c = which leg is currently in contact with the environment. 108 | MarkerVec CreateEEPositions(const EEPos& pos_W, 109 | const ContactState& c) const; 110 | MarkerVec CreateEEForces(const EEForces& f_W, 111 | const EEPos& pos_W, 112 | const ContactState& c) const; 113 | MarkerVec CreateFrictionCones(const EEPos& pos_W, 114 | const ContactState& c) const; 115 | MarkerVec CreateSupportArea(const ContactState& c, 116 | const EEPos& pos_W) const; 117 | MarkerVec CreateRangeOfMotion(const State3d& base) const; 118 | Marker CreateGravityForce (const Vector3d& base_pos) const; 119 | Marker CreateBasePose(const Vector3d& pos, 120 | Eigen::Quaterniond ori, 121 | const ContactState& c) const; 122 | Marker CreateCopPos(const EEForces& f_W, 123 | const EEPos& pos_W) const; 124 | Marker CreatePendulum(const Vector3d& base_pos, 125 | const EEForces& f_W, 126 | const EEPos& pos_W) const; 127 | Marker CreateFrictionCone(const Vector3d& pos_W, 128 | const Vector3d& terrain_normal, 129 | double friction_coeff) const; 130 | Marker CreateForceArrow(const Vector3d& f, 131 | const Vector3d& pos) const; 132 | Marker CreateSphere(const Vector3d& pos, 133 | double diameter = 0.03) const; 134 | Marker CreateBox(const Vector3d& pos, Eigen::Quaterniond ori, 135 | const Vector3d& edge_length) const; 136 | 137 | xpp_msgs::RobotParameters params_msg_; 138 | xpp_msgs::TerrainInfo terrain_msg_; 139 | 140 | const std::string frame_id_ = "world"; 141 | 142 | /** 143 | * Makes sure no artifacts remain from previous visualization. 144 | * @param max_size Number of marker for this rviz vector, should be constant. 145 | * @param vec The vector of rviz markers. 146 | */ 147 | void FillWithInvisible(int max_size, MarkerVec& vec) const; 148 | const static int max_ee_ = 10; // maximum number of endeffectors 149 | }; 150 | 151 | } /* namespace xpp */ 152 | 153 | #endif /* XPP_VIS_RVIZ_ROBOT_BUILDER_H_ */ 154 | -------------------------------------------------------------------------------- /xpp_vis/include/xpp_vis/urdf_visualizer.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #ifndef XPP_VIS_URDF_VISUALIZER_H_ 31 | #define XPP_VIS_URDF_VISUALIZER_H_ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include 49 | #include 50 | 51 | 52 | namespace xpp { 53 | 54 | /** 55 | * @brief Publishes RVIZ transforms used to visualize a robot's URDF. 56 | * 57 | * This class is responsible for converting an xpp_msgs::RobotStateJoint 58 | * robot message and publishing the corresponding RVIZ transforms. 59 | */ 60 | class UrdfVisualizer { 61 | public: 62 | using URDFName = std::string; 63 | using UrdfnameToJointAngle = std::map; 64 | 65 | /** 66 | * @brief Constructs the visualizer for a specific URDF %urdf_name. 67 | * @param urdf_name Robot description variable on the ROS parameter server. 68 | * @param joint_names_in_urdf The names of the joints in the URDF file 69 | * ordered in the same way as in the xpp convention 70 | * (see inverse kinematics). 71 | * @param base_link_in_urdf The name of the base_link in the URDF file. 72 | * @param rviz_fixed_frame The Fixed Frame name specified in RVIZ. 73 | * @param state_topic The topic of the xpp_msgs::RobotStateJoint ROS message 74 | * to subscribe to. 75 | * @param tf_prefix In case multiple URDFS are loaded, each can be given a 76 | * unique tf_prefix in RIVZ to visualize different states simultaneously. 77 | */ 78 | UrdfVisualizer(const std::string& urdf_name, 79 | const std::vector& joint_names_in_urdf, 80 | const URDFName& base_link_in_urdf, 81 | const std::string& rviz_fixed_frame, 82 | const std::string& state_topic, 83 | const std::string& tf_prefix = ""); 84 | virtual ~UrdfVisualizer() = default; 85 | 86 | private: 87 | ros::Subscriber state_sub_des_; 88 | tf::TransformBroadcaster tf_broadcaster_; 89 | std::shared_ptr robot_publisher; 90 | 91 | void StateCallback(const xpp_msgs::RobotStateJoint& msg); 92 | 93 | UrdfnameToJointAngle AssignAngleToURDFJointName(const sensor_msgs::JointState &msg) const; 94 | geometry_msgs::TransformStamped GetBaseFromRos(const ::ros::Time& stamp, 95 | const geometry_msgs::Pose &msg) const; 96 | 97 | std::vector joint_names_in_urdf_; 98 | URDFName base_joint_in_urdf_; 99 | 100 | std::string state_msg_name_; 101 | std::string rviz_fixed_frame_; 102 | std::string tf_prefix_; 103 | }; 104 | 105 | } // namespace xpp 106 | 107 | #endif /* XPP_VIS_URDF_VISUALIZER_H_ */ 108 | -------------------------------------------------------------------------------- /xpp_vis/launch/xpp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /xpp_vis/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | xpp_vis 5 | 1.0.10 6 | 7 | Visualization for the XPP Motion Framework. 8 | 9 | 10 | Alexander W. Winkler 11 | Alexander W. Winkler 12 | BSD 13 | 14 | http://github.com/leggedrobotics/xpp 15 | http://github.com/leggedrobotics/xpp/issues 16 | 17 | catkin 18 | 19 | roscpp 20 | tf 21 | kdl_parser 22 | robot_state_publisher 23 | visualization_msgs 24 | xpp_states 25 | xpp_msgs 26 | 27 | rosunit 28 | 29 | -------------------------------------------------------------------------------- /xpp_vis/rviz/xpp.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Grid1/Offset1 8 | - /CartesianVisualization1 9 | - /CartesianVisualization1/Namespaces1 10 | Splitter Ratio: 0.442424 11 | Tree Height: 929 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.588679 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | - /CoGTrajectory1 25 | - /video_top1 26 | - /video_side1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz/Time 30 | Experimental: false 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: "" 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 0.1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.03 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 5 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 100 53 | Reference Frame: 54 | Value: true 55 | - Class: rviz/MarkerArray 56 | Enabled: true 57 | Marker Topic: /xpp/terrain 58 | Name: Terrain 59 | Namespaces: 60 | {} 61 | Queue Size: 100 62 | Value: true 63 | - Alpha: 1 64 | Axes Length: 0.1 65 | Axes Radius: 0.01 66 | Class: rviz/Pose 67 | Color: 255; 25; 0 68 | Enabled: true 69 | Head Length: 0.1 70 | Head Radius: 0.05 71 | Name: Desired Goal 72 | Shaft Length: 0.3 73 | Shaft Radius: 0.02 74 | Shape: Axes 75 | Topic: /xpp/goal 76 | Unreliable: false 77 | Value: true 78 | - Class: rviz/MarkerArray 79 | Enabled: true 80 | Marker Topic: /xpp/rviz_markers 81 | Name: CartesianVisualization 82 | Namespaces: 83 | {} 84 | Queue Size: 100 85 | Value: true 86 | Enabled: true 87 | Global Options: 88 | Background Color: 255; 255; 255 89 | Fixed Frame: world 90 | Frame Rate: 30 91 | Name: root 92 | Tools: 93 | - Class: rviz/Interact 94 | Hide Inactive Objects: true 95 | - Class: rviz/MoveCamera 96 | - Class: rviz/Select 97 | - Class: rviz/FocusCamera 98 | - Class: rviz/Measure 99 | - Class: rviz/SetInitialPose 100 | Topic: /initialpose 101 | - Class: rviz/SetGoal 102 | Topic: /move_base_simple/goal 103 | - Class: rviz/PublishPoint 104 | Single click: true 105 | Topic: /clicked_point 106 | Value: true 107 | Views: 108 | Current: 109 | Class: rviz/ThirdPersonFollower 110 | Distance: 4.37566 111 | Enable Stereo Rendering: 112 | Stereo Eye Separation: 0.06 113 | Stereo Focal Distance: 1 114 | Swap Stereo Eyes: false 115 | Value: false 116 | Focal Point: 117 | X: 0.993474 118 | Y: -0.724448 119 | Z: -0.579998 120 | Name: Current View 121 | Near Clip Distance: 0.01 122 | Pitch: 0.149798 123 | Target Frame: 124 | Value: ThirdPersonFollower (rviz) 125 | Yaw: 1.53399 126 | Saved: 127 | - Angle: 3.14 128 | Class: rviz/TopDownOrtho 129 | Enable Stereo Rendering: 130 | Stereo Eye Separation: 0.06 131 | Stereo Focal Distance: 1 132 | Swap Stereo Eyes: false 133 | Value: false 134 | Name: CoGTrajectory 135 | Near Clip Distance: 0.01 136 | Scale: 582.84 137 | Target Frame: 138 | Value: TopDownOrtho (rviz) 139 | X: 0.160456 140 | Y: 0.0931166 141 | - Angle: 6.26 142 | Class: rviz/TopDownOrtho 143 | Enable Stereo Rendering: 144 | Stereo Eye Separation: 0.06 145 | Stereo Focal Distance: 1 146 | Swap Stereo Eyes: false 147 | Value: false 148 | Name: video_top 149 | Near Clip Distance: 0.01 150 | Scale: 266.776 151 | Target Frame: 152 | Value: TopDownOrtho (rviz) 153 | X: 2.27941 154 | Y: -0.724513 155 | - Class: rviz/Orbit 156 | Distance: 3.56509 157 | Enable Stereo Rendering: 158 | Stereo Eye Separation: 0.06 159 | Stereo Focal Distance: 1 160 | Swap Stereo Eyes: false 161 | Value: false 162 | Focal Point: 163 | X: 4.22615 164 | Y: -1.12562 165 | Z: 0.170358 166 | Name: video_side 167 | Near Clip Distance: 0.01 168 | Pitch: 0.434798 169 | Target Frame: 170 | Value: Orbit (rviz) 171 | Yaw: 5.15 172 | - Class: rviz/Orbit 173 | Distance: 1.72568 174 | Enable Stereo Rendering: 175 | Stereo Eye Separation: 0.06 176 | Stereo Focal Distance: 1 177 | Swap Stereo Eyes: false 178 | Value: false 179 | Focal Point: 180 | X: 0.095682 181 | Y: -0.160401 182 | Z: -0.313986 183 | Name: fixed_to_hyq 184 | Near Clip Distance: 0.01 185 | Pitch: 0.349798 186 | Target Frame: base_link 187 | Value: Orbit (rviz) 188 | Yaw: 5.115 189 | Window Geometry: 190 | Displays: 191 | collapsed: false 192 | Height: 1173 193 | Hide Left Dock: false 194 | Hide Right Dock: false 195 | QMainWindow State: 000000ff00000000fd0000000400000000000001da00000432fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004300000432000000df00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000012f00000432fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004300000432000000b800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003bf0000003efc0100000002fb0000000800540069006d00650000000000000003bf0000022a00fffffffb0000000800540069006d006501000000000000045000000000000000000000059e0000043200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 196 | Selection: 197 | collapsed: false 198 | Time: 199 | collapsed: false 200 | Tool Properties: 201 | collapsed: false 202 | Views: 203 | collapsed: false 204 | Width: 1918 205 | X: 3831 206 | Y: -8 207 | -------------------------------------------------------------------------------- /xpp_vis/src/cartesian_joint_converter.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | namespace xpp { 38 | 39 | CartesianJointConverter::CartesianJointConverter (const InverseKinematics::Ptr& ik, 40 | const std::string& cart_topic, 41 | const std::string& joint_topic) 42 | { 43 | inverse_kinematics_ = ik; 44 | 45 | ::ros::NodeHandle n; 46 | cart_state_sub_ = n.subscribe(cart_topic, 1, &CartesianJointConverter::StateCallback, this); 47 | ROS_DEBUG("Subscribed to: %s", cart_state_sub_.getTopic().c_str()); 48 | 49 | joint_state_pub_ = n.advertise(joint_topic, 1); 50 | ROS_DEBUG("Publishing to: %s", joint_state_pub_.getTopic().c_str()); 51 | } 52 | 53 | void 54 | CartesianJointConverter::StateCallback (const xpp_msgs::RobotStateCartesian& cart_msg) 55 | { 56 | auto cart = Convert::ToXpp(cart_msg); 57 | 58 | // transform feet from world -> base frame 59 | Eigen::Matrix3d B_R_W = cart.base_.ang.q.normalized().toRotationMatrix().inverse(); 60 | EndeffectorsPos ee_B(cart.ee_motion_.GetEECount()); 61 | for (auto ee : ee_B.GetEEsOrdered()) 62 | ee_B.at(ee) = B_R_W * (cart.ee_motion_.at(ee).p_ - cart.base_.lin.p_); 63 | 64 | Eigen::VectorXd q = inverse_kinematics_->GetAllJointAngles(ee_B).ToVec(); 65 | 66 | xpp_msgs::RobotStateJoint joint_msg; 67 | joint_msg.base = cart_msg.base; 68 | joint_msg.ee_contact = cart_msg.ee_contact; 69 | joint_msg.time_from_start = cart_msg.time_from_start; 70 | joint_msg.joint_state.position = std::vector(q.data(), q.data()+q.size()); 71 | // Attention: Not filling joint velocities or torques 72 | 73 | joint_state_pub_.publish(joint_msg); 74 | } 75 | 76 | } /* namespace xpp */ 77 | -------------------------------------------------------------------------------- /xpp_vis/src/exe/rviz_marker_node.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | 39 | static ros::Publisher rviz_marker_pub; 40 | static xpp::RvizRobotBuilder robot_builder; 41 | 42 | static void StateCallback (const xpp_msgs::RobotStateCartesian& state_msg) 43 | { 44 | auto rviz_marker_msg = robot_builder.BuildRobotState(state_msg); 45 | rviz_marker_pub.publish(rviz_marker_msg); 46 | } 47 | 48 | static void TerrainInfoCallback (const xpp_msgs::TerrainInfo& terrain_msg) 49 | { 50 | robot_builder.SetTerrainParameters(terrain_msg); 51 | } 52 | 53 | static void ParamsCallback (const xpp_msgs::RobotParameters& params_msg) 54 | { 55 | robot_builder.SetRobotParameters(params_msg); 56 | } 57 | 58 | int main(int argc, char *argv[]) 59 | { 60 | using namespace ros; 61 | 62 | init(argc, argv, "rviz_marker_visualizer"); 63 | 64 | NodeHandle n; 65 | 66 | Subscriber parameters_sub; 67 | parameters_sub = n.subscribe(xpp_msgs::robot_parameters, 1, ParamsCallback); 68 | 69 | Subscriber state_sub_curr, state_sub_des, terrain_info_sub; 70 | state_sub_des = n.subscribe(xpp_msgs::robot_state_desired, 1, StateCallback); 71 | terrain_info_sub = n.subscribe(xpp_msgs::terrain_info, 1, TerrainInfoCallback); 72 | 73 | rviz_marker_pub = n.advertise("xpp/rviz_markers", 1); 74 | 75 | spin(); 76 | 77 | return 1; 78 | } 79 | -------------------------------------------------------------------------------- /xpp_vis/src/urdf_visualizer.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | namespace xpp { 33 | 34 | UrdfVisualizer::UrdfVisualizer(const std::string& urdf_name, 35 | const std::vector& joint_names_in_urdf, 36 | const URDFName& base_joint_in_urdf, 37 | const std::string& fixed_frame, 38 | const std::string& state_topic, 39 | const std::string& tf_prefix) 40 | { 41 | joint_names_in_urdf_ = joint_names_in_urdf; 42 | base_joint_in_urdf_ = base_joint_in_urdf; 43 | rviz_fixed_frame_ = fixed_frame; 44 | tf_prefix_ = tf_prefix; 45 | 46 | ::ros::NodeHandle nh; 47 | state_sub_des_ = nh.subscribe(state_topic, 1, &UrdfVisualizer::StateCallback, this); 48 | ROS_DEBUG("Subscribed to: %s", state_sub_des_.getTopic().c_str()); 49 | 50 | // Load model from file 51 | KDL::Tree my_kdl_tree; 52 | urdf::Model my_urdf_model; 53 | bool model_ok = my_urdf_model.initParam(urdf_name); 54 | if(!model_ok) 55 | { 56 | ROS_ERROR("Invalid URDF File"); 57 | exit(EXIT_FAILURE); 58 | } 59 | ROS_DEBUG("URDF successfully parsed"); 60 | kdl_parser::treeFromUrdfModel(my_urdf_model, my_kdl_tree); 61 | ROS_DEBUG("Robot tree is ready"); 62 | 63 | robot_publisher = std::make_shared(my_kdl_tree); 64 | } 65 | 66 | void 67 | UrdfVisualizer::StateCallback(const xpp_msgs::RobotStateJoint& msg) 68 | { 69 | auto joint_positions = AssignAngleToURDFJointName(msg.joint_state); 70 | auto W_X_B_message = GetBaseFromRos(::ros::Time::now(), msg.base.pose); 71 | 72 | tf_broadcaster_.sendTransform(W_X_B_message); 73 | robot_publisher->publishTransforms(joint_positions, ::ros::Time::now(), tf_prefix_); 74 | robot_publisher->publishFixedTransforms(tf_prefix_); 75 | } 76 | 77 | UrdfVisualizer::UrdfnameToJointAngle 78 | UrdfVisualizer::AssignAngleToURDFJointName(const sensor_msgs::JointState &msg) const 79 | { 80 | UrdfnameToJointAngle q; 81 | 82 | for (int i=0; i 31 | #include //std::getenv 32 | #include 33 | 34 | 35 | GTEST_API_ int main(int argc, char **argv) { 36 | printf("Running main() from gtest_main.cc\n"); 37 | 38 | testing::GTEST_FLAG(print_time) = true; 39 | testing::InitGoogleTest(&argc, argv); 40 | return RUN_ALL_TESTS(); 41 | } 42 | -------------------------------------------------------------------------------- /xpp_vis/test/rviz_robot_builder_test.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | #include 33 | 34 | using namespace xpp; 35 | 36 | TEST(RvizRobotBuilder, BuildRobotState) 37 | { 38 | RvizRobotBuilder builder; 39 | 40 | // defines the robot state 41 | xpp_msgs::RobotParameters params_msg; 42 | params_msg.base_mass = 30.0; // [kg] 43 | params_msg.ee_max_dev.x = params_msg.ee_max_dev.x = params_msg.ee_max_dev.z = 0.1; 44 | builder.SetRobotParameters(params_msg); 45 | 46 | // create a biped robot standing 0.6m tall 47 | xpp_msgs::RobotStateCartesian state_msg; 48 | state_msg.ee_motion.resize(2); 49 | state_msg.ee_forces.resize(2); 50 | state_msg.base.pose.position.z = 0.6; // [m] 51 | state_msg.ee_contact = { true, true }; 52 | state_msg.ee_motion.at(0).pos.y = 0.2; // leg leg 53 | state_msg.ee_motion.at(1).pos.y = -0.2; // right leg 54 | state_msg.ee_forces.at(0).z = params_msg.base_mass*9.81/2.; // leg leg 55 | state_msg.ee_forces.at(1).z = params_msg.base_mass*9.81/2.; // right leg 56 | 57 | auto rviz_markers = builder.BuildRobotState(state_msg); 58 | 59 | // mostly checking for segfaults here 60 | EXPECT_FALSE(rviz_markers.markers.empty()); 61 | } 62 | --------------------------------------------------------------------------------