├── .gitmodules ├── .travis.yml ├── LICENSE ├── README.md ├── champ ├── CMakeLists.txt └── package.xml ├── champ_base ├── .gitignore ├── CMakeLists.txt ├── config │ ├── ekf │ │ ├── base_to_footprint.yaml │ │ └── footprint_to_odom.yaml │ └── velocity_smoother │ │ └── velocity_smoother.yaml ├── include │ ├── actuator.h │ ├── message_relay.h │ ├── quadruped_controller.h │ └── state_estimation.h ├── package.xml └── src │ ├── message_relay.cpp │ ├── message_relay_node.cpp │ ├── quadruped_controller.cpp │ ├── quadruped_controller_node.cpp │ ├── state_estimation.cpp │ └── state_estimation_node.cpp ├── champ_bringup ├── CMakeLists.txt ├── launch │ ├── bringup.launch │ ├── include │ │ ├── laser │ │ │ ├── d435.launch │ │ │ ├── hokuyo.launch │ │ │ ├── kinect.launch │ │ │ ├── lms1xx.launch │ │ │ ├── realsense.launch │ │ │ ├── rplidar.launch │ │ │ ├── sim.launch │ │ │ ├── sweep.launch │ │ │ ├── xv11.launch │ │ │ └── ydlidar.launch │ │ └── velocity_smoother.launch │ └── joints_gui.launch ├── package.xml └── scripts │ └── joint_calibrator_relay.py ├── champ_config ├── CMakeLists.txt ├── config │ ├── gait │ │ └── gait.yaml │ ├── joints │ │ └── joints.yaml │ ├── links │ │ └── links.yaml │ ├── move_base │ │ ├── base_local_planner_holonomic_params.yaml │ │ ├── costmap_common_params.yaml │ │ ├── global_costmap_params.yaml │ │ ├── local_costmap_params.yaml │ │ └── move_base_params.yaml │ └── ros_control │ │ └── ros_control.yaml ├── include │ ├── gait_config.h │ ├── hardware_config.h │ └── quadruped_description.h ├── launch │ ├── bringup.launch │ ├── gazebo.launch │ ├── include │ │ ├── amcl.launch │ │ ├── gmapping.launch │ │ └── move_base.launch │ ├── navigate.launch │ ├── slam.launch │ └── spawn_robot.launch ├── maps │ ├── map.pgm │ └── map.yaml ├── package.xml ├── setup.bash └── worlds │ ├── default.world │ └── outdoor.world ├── champ_description ├── CMakeLists.txt ├── launch │ ├── description.launch │ └── view_urdf.launch ├── meshes │ ├── arm │ │ ├── base.stl │ │ ├── lower_arm.stl │ │ ├── upper_arm.stl │ │ ├── wrist1.stl │ │ └── wrist2.stl │ ├── base.stl │ ├── hip.stl │ ├── lower_leg.stl │ └── upper_leg.stl ├── package.xml ├── rviz │ └── urdf_viewer.rviz └── urdf │ ├── accessories.urdf.xacro │ ├── champ.urdf │ ├── champ.urdf.xacro │ ├── champ_arm.urdf │ ├── champ_velodyne.urdf │ ├── gen_urdf │ ├── leg.urdf.xacro │ └── properties.urdf.xacro ├── champ_gazebo ├── CMakeLists.txt ├── config │ └── ros_control.yaml ├── launch │ ├── gazebo.launch │ ├── spawn_robot.launch │ └── spawn_world.launch ├── package.xml ├── scripts │ ├── imu_sensor.py │ ├── odometry.py │ └── odometry_tf.py ├── src │ └── contact_sensor.cpp └── worlds │ ├── default.world │ └── outdoor.world ├── champ_msgs ├── CMakeLists.txt ├── msg │ ├── Contacts.msg │ ├── ContactsStamped.msg │ ├── Imu.msg │ ├── Joints.msg │ ├── PID.msg │ ├── Point.msg │ ├── PointArray.msg │ ├── Pose.msg │ └── Velocities.msg └── package.xml ├── champ_navigation ├── CMakeLists.txt ├── launch │ ├── navigate.launch │ ├── navigation │ │ ├── amcl.launch │ │ └── move_base.launch │ ├── octo_slam.launch │ ├── slam.launch │ └── slam │ │ ├── gmapping.launch │ │ ├── hector_mapping.launch │ │ ├── octomap.launch │ │ └── velodyne_pointcloud_to_laser.launch ├── maps │ ├── map.pgm │ ├── map.yaml │ ├── octomap.ot │ ├── octomap.pgm │ └── octomap.yaml ├── package.xml ├── param │ ├── base_local_planner_holonomic_params.yaml │ ├── costmap_common_params.yaml │ ├── global_costmap_params.yaml │ ├── local_costmap_params.yaml │ └── move_base_params.yaml └── rviz │ └── navigate.rviz └── docs └── images ├── leg_stretched.png ├── navigation.gif ├── robots.gif └── slam.gif /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "champ/include/champ"] 2 | path = champ/include/champ 3 | url = https://github.com/chvmp/libchamp 4 | branch = master 5 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | language: generic 3 | 4 | matrix: 5 | include: 6 | - dist: xenial 7 | env: ROSDISTRO=kinetic 8 | os: linux 9 | arch: amd64 10 | 11 | - dist: xenial 12 | env: ROSDISTRO=kinetic 13 | os: linux 14 | arch: arm64 15 | 16 | - dist: bionic 17 | env: ROSDISTRO=melodic 18 | os: linux 19 | arch: amd64 20 | 21 | - dist: bionic 22 | env: ROSDISTRO=melodic 23 | os: linux 24 | arch: arm64 25 | 26 | branches: 27 | only: 28 | - master 29 | 30 | compiler: 31 | - gcc 32 | 33 | install: 34 | - sudo apt update 35 | - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' 36 | - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - 37 | - sudo apt update 38 | - sudo apt install -y dpkg 39 | - sudo apt install -y ros-$ROSDISTRO-ros-base 40 | - sudo apt install -y python-rosdep 41 | - sudo `which rosdep` init 42 | - rosdep update 43 | - rosdep install --default-yes --from-paths . --ignore-src --rosdistro $ROSDISTRO 44 | - source /opt/ros/$ROSDISTRO/setup.bash 45 | - export PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages 46 | - sudo apt update 47 | - mkdir -p catkin_ws/src && cd catkin_ws/src 48 | - git clone --recursive https://github.com/chvmp/champ.git 49 | - cd ../ 50 | - rosdep install --from-paths src --ignore-src -r -y 51 | - catkin_make 52 | 53 | notifications: 54 | email: 55 | - jimenojmm@gmail.com 56 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2019-2020, Juan Miguel Jimeno 2 | 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 | * Redistributions of source code must retain the above copyright 7 | notice, this list of conditions and the following disclaimer. 8 | * Redistributions in binary form must reproduce the above copyright 9 | notice, this list of conditions and the following disclaimer in the 10 | documentation and/or other materials provided with the distribution. 11 | * Neither the name of the copyright holder nor the names of its 12 | contributors may be used to endorse or promote products derived 13 | from this software without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 19 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | # champ [![Build Status](https://travis-ci.org/chvmp/champ.svg?branch=master)](https://travis-ci.org/chvmp/champ) 3 | ROS Packages for CHAMP Quadruped Controller. 4 | 5 | ![champ](https://raw.githubusercontent.com/chvmp/champ/master/docs/images/robots.gif) 6 | 7 | CHAMP is an open source development framework for building new quadrupedal robots and developing new control algorithms. The control framework is based on [*"Hierarchical controller for highly dynamic locomotion utilizing pattern modulation and impedance control : implementation on the MIT Cheetah robot"*](https://dspace.mit.edu/handle/1721.1/85490). 8 | 9 | Core Features: 10 | 11 | - Fully Autonomous (using ROS navigation Stack). 12 | - [Setup-assistant](https://github.com/chvmp/champ_setup_assistant) to configure newly built robots. 13 | - Collection of pre-configured [URDFs](https://github.com/chvmp/robots) like Anymal, MIT Mini Cheetah, Boston Dynamic's Spot and LittleDog. 14 | - Gazebo simulation environment. 15 | - Compatible with DIY quadruped projects like [SpotMicroAI](https://spotmicroai.readthedocs.io/en/latest/) and [OpenQuadruped](https://github.com/adham-elarabawy/open-quadruped). 16 | - Demo Applications like [TOWR](https://github.com/ethz-adrl/towr) and [chicken head](https://github.com/chvmp/chicken_head) stabilization. 17 | - Lightweight C++ header-only [library](https://github.com/chvmp/libchamp) that can run on both SBC and micro-controllers. 18 | 19 | Tested on: 20 | 21 | - Ubuntu 16.04 (ROS Kinetic) 22 | - Ubuntu 18.04 (ROS Melodic) 23 | 24 | ## 1. Installation 25 | 26 | ### 1.1 Clone and install all dependencies: 27 | 28 | sudo apt install -y python-rosdep 29 | cd /src 30 | git clone --recursive https://github.com/chvmp/champ 31 | git clone https://github.com/chvmp/champ_teleop 32 | cd .. 33 | rosdep install --from-paths src --ignore-src -r -y 34 | 35 | If you want to use any of the pre-configured robots like Anymal, Mini Cheetah, or Spot, follow the instructions [here](https://github.com/chvmp/robots). 36 | 37 | ### 1.2 Build your workspace: 38 | 39 | cd 40 | catkin_make 41 | source /devel/setup.bash 42 | 43 | ## 2. Quick Start 44 | 45 | You don't need a physical robot to run the following demos. If you're building a physical robot, you can find out more how to configure and run a new robot in step 3. 46 | 47 | ### 2.1 Walking demo in RVIZ: 48 | 49 | #### 2.1.1 Run the base driver: 50 | 51 | roslaunch champ_config bringup.launch rviz:=true 52 | 53 | #### 2.1.2 Run the teleop node: 54 | 55 | roslaunch champ_teleop teleop.launch 56 | 57 | If you want to use a [joystick](https://www.logitechg.com/en-hk/products/gamepads/f710-wireless-gamepad.html) add joy:=true as an argument. 58 | 59 | 60 | ### 2.2 SLAM demo: 61 | 62 | #### 2.2.1 Run the Gazebo environment: 63 | 64 | roslaunch champ_config gazebo.launch 65 | 66 | #### 2.2.2 Run gmapping package and move_base: 67 | 68 | roslaunch champ_config slam.launch rviz:=true 69 | 70 | To start mapping: 71 | 72 | - Click '2D Nav Goal'. 73 | - Click and drag at the position you want the robot to go. 74 | 75 | ![champ](https://raw.githubusercontent.com/chvmp/champ/master/docs/images/slam.gif) 76 | 77 | - Save the map by running: 78 | 79 | roscd champ_config/maps 80 | rosrun map_server map_saver 81 | 82 | ### 2.3 Autonomous Navigation: 83 | 84 | #### 2.3.1 Run the Gazebo environment: 85 | 86 | roslaunch champ_config gazebo.launch 87 | 88 | #### 2.3.2 Run amcl and move_base: 89 | 90 | roslaunch champ_config navigate.launch rviz:=true 91 | 92 | To navigate: 93 | 94 | - Click '2D Nav Goal'. 95 | - Click and drag at the position you want the robot to go. 96 | 97 | ![champ](https://raw.githubusercontent.com/chvmp/champ/master/docs/images/navigation.gif) 98 | 99 | ## 3. Running your own robot: 100 | 101 | There are two ways to run CHAMP on a real robot: 102 | 103 | Linux Machine 104 | - Use this ROS package to calculate the joint angles and send it to a hardware interface to control your actuators. You can follow these [guidelines](https://github.com/chvmp/champ/wiki/Hardware-Integration) to create your actuators' interface. 105 | 106 | Lightweight Version 107 | - Run CHAMP's [lightweight version](https://github.com/chvmp/firmware) on Teensy series microcontrollers and use it to directly control your actuators. 108 | 109 | ### 3.1 Generate robot configuration 110 | 111 | - First generate a configuration package using [champ_setup_assistant](https://github.com/chvmp/champ_setup_assistant). Follow the instructions in the README to configure your own robot. The generated package contains: 112 | 113 | - URDF path to your robot. 114 | - Joints and Links map to help the controller know the semantics of the robot. 115 | - Gait parameters. 116 | - Hardware Drivers. 117 | - Navigation parameters (move_base, amcl and gmapping). 118 | - Microcontroller header files for gait and lightweight robot description. This only applies to robot builds that use microcontroller to run the quadruped controller. 119 | 120 | As a reference, you can check out the collection of robots that have been pre-configured [here](https://github.com/chvmp/robots). In the list are some of the popular quadruped robots like Anymal, MIT Mini Cheetah, Boston Dynamic's LittleDog, and SpotMicroAI. Feel free to download the configuration packages in your catkin workspaces 'src' directory to try. 121 | 122 | - Next, build your workspace so your newly generated package can be found: 123 | 124 | cd 125 | catkin_make 126 | 127 | ### 3.2 Base Driver: 128 | 129 | This will run the quadruped controller and all sensor/hardware drivers: 130 | 131 | roslaunch bringup.launch 132 | 133 | Available Parameters: 134 | 135 | - **rviz** - Launch together with RVIZ. Default: false 136 | 137 | - **lite** - Always set this to true if you're using a microcontroller to run the algorithms. Default false. 138 | 139 | Example Usage: 140 | 141 | View your newly configured robot: 142 | 143 | roslaunch bringup.launch rviz:true 144 | 145 | Run real robot with a microcontroller: 146 | 147 | roslaunch bringup.launch lite:=true 148 | 149 | 150 | ### 3.3 Creating a map: 151 | The base driver described in 3.2 must be running to run gmapping and move_base. 152 | 153 | Run gmapping package and move_base: 154 | 155 | roslaunch slam.launch 156 | 157 | To open RVIZ and view the map: 158 | 159 | roscd champ_navigation/rviz 160 | rviz -d navigate.rviz 161 | 162 | To start mapping: 163 | - Click '2D Nav Goal'. 164 | - Click and drag at the position you want the robot to go. 165 | 166 | ![champ](https://raw.githubusercontent.com/chvmp/champ/master/docs/images/slam.gif) 167 | 168 | - Save the map by running: 169 | 170 | roscd /maps 171 | rosrun map_server map_saver 172 | 173 | ### 3.4 Autonomous Navigation: 174 | 175 | The base driver described in 3.2 must be running to run amcl and move_base. 176 | 177 | Run amcl and move_base: 178 | 179 | roslaunch navigate.launch 180 | 181 | To open RVIZ and view the map: 182 | 183 | roscd champ_navigation/rviz 184 | rviz -d navigate.rviz 185 | 186 | To navigate: 187 | 188 | - Click '2D Nav Goal'. 189 | - Click and drag at the position you want the robot to go. 190 | 191 | ![champ](https://raw.githubusercontent.com/chvmp/champ/master/docs/images/navigation.gif) 192 | 193 | ### 3.5 Running your robot in Gazebo 194 | 195 | Run Gazebo and the base driver in simulation mode: 196 | 197 | roslaunch gazebo.launch 198 | 199 | * Take note that in order for this to work, the URDF has to be Gazebo compatible and has [ros_control](http://gazebosim.org/tutorials/?tut=ros_control) capability. The controllers have been set-up so all you need is to add the transmission of the actuators. You also need to get the physics parameters right like your mass, inertia, and foot friction. 200 | 201 | Some useful resources on getting these parameters right: 202 | 203 | - Inertial Calculation - https://github.com/tu-darmstadt-ros-pkg/hector_models/blob/indigo-devel/hector_xacro_tools/urdf/inertia_tensors.urdf.xacro 204 | 205 | - List of Moment of Inertia - https://en.wikipedia.org/wiki/List_of_moments_of_inertia 206 | 207 | - Gazebo inertial parameters - http://gazebosim.org/tutorials?tut=inertia&cat=build_robot#Overview 208 | 209 | You can also check out [this](https://github.com/moribots/spot_mini_mini/pull/7) pull request as an example. 210 | 211 | ### 3.6 Spawning multiple robots in Gazebo 212 | 213 | Run Gazebo and default simulation world: 214 | 215 | roslaunch champ_gazebo spawn_world.launch 216 | 217 | You can also load your own world file by passing your world's path to 'gazebo_world' argument: 218 | 219 | roslaunch champ_gazebo spawn_world.launch gazebo_world:= 220 | 221 | Spawning a robot: 222 | 223 | roslaunch champ_config spawn_robot.launch robot_name:= world_init_x:= world_init_y:= 224 | 225 | 226 | * Every instance of the spawned robot must have a unique robot name to prevent the topics and transforms from clashing. 227 | 228 | 229 | ## 4. Tuning gait parameters 230 | 231 | The gait configuration for your robot can be found in /gait/gait.yaml. 232 | 233 | ![CHAMP Setup Assistant](https://raw.githubusercontent.com/chvmp/champ_setup_assistant/master/docs/images/gait_parameters.png) 234 | 235 | - **Knee Orientation** - How the knees should be bent. You can can configure the robot to follow the following orientation .>> .>< .<< .<> where dot is the front side of the robot. 236 | 237 | - **Max Linear Velocity X** (meters/second) - Robot's maximum forward/reverse speed. 238 | 239 | - **Max Linear Velocity Y** (meteres/second) - Robot's maximum speed when moving sideways. 240 | 241 | - **Max Angular Velocity Z** (radians/second)- Robot's maximum rotational speed. 242 | 243 | - **Stance Duration** (seconds)- How long should each leg spend on the ground while walking. You can set this to default(0.25) if you're not sure. The higher the stance duration the further the displacement is from the reference point. 244 | 245 | - **Leg Swing Height** (meters)- Trajectory height during swing phase. 246 | 247 | - **Leg Stance Height** (meters)- Trajectory depth during stance phase. 248 | 249 | - **Robot Walking Height** (meters) - Distance from hip to the ground while walking. Take note that setting this parameter too high can get your robot unstable. 250 | 251 | - **CoM X Translation** (meters) - You can use this parameter to move the reference point in the X axis. This is useful when you want to compensate for the weight if the center of mass is not in the middle of the robot (from front hip to rear hip). For instance, if you find that the robot is heavier at the back, you'll set a negative value to shift the reference point to the back. 252 | 253 | - **Odometry Scaler** - You can use this parameter as a multiplier to the calculated velocities for dead reckoning. This can be useful to compensate odometry errors on open-loop systems. Normally this value ranges from 1.0 to 1.20. 254 | -------------------------------------------------------------------------------- /champ/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | cmake_minimum_required(VERSION 2.8.3) 3 | project(champ) 4 | 5 | find_package(catkin 6 | REQUIRED 7 | COMPONENTS 8 | ) 9 | 10 | catkin_package( 11 | INCLUDE_DIRS include include/${PROJECT_NAME} 12 | ) 13 | 14 | include_directories( 15 | include/${PROJECT_NAME} 16 | ${catkin_INCLUDE_DIRS} 17 | ) 18 | 19 | install(DIRECTORY include/${PROJECT_NAME} 20 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 21 | FILES_MATCHING PATTERN "*.h" 22 | PATTERN ".git" EXCLUDE 23 | ) -------------------------------------------------------------------------------- /champ/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | champ 4 | 0.1.0 5 | CHAMP Core Library 6 | 7 | Juan Miguel Jimeno 8 | Juan Miguel Jimeno 9 | 10 | BSD 11 | 12 | catkin 13 | 14 | -------------------------------------------------------------------------------- /champ_base/.gitignore: -------------------------------------------------------------------------------- 1 | include/libchamp -------------------------------------------------------------------------------- /champ_base/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(champ_base) 3 | 4 | add_compile_options(-std=c++17) 5 | 6 | find_package(catkin 7 | REQUIRED 8 | COMPONENTS 9 | roscpp 10 | rospy 11 | champ_msgs 12 | visualization_msgs 13 | geometry_msgs 14 | sensor_msgs 15 | trajectory_msgs 16 | nav_msgs 17 | tf 18 | tf2_ros 19 | urdf 20 | champ 21 | message_filters 22 | ) 23 | 24 | catkin_package( 25 | INCLUDE_DIRS include 26 | LIBRARIES champ_base 27 | CATKIN_DEPENDS 28 | roscpp 29 | rospy 30 | champ_msgs 31 | visualization_msgs 32 | geometry_msgs 33 | sensor_msgs 34 | trajectory_msgs 35 | nav_msgs 36 | tf 37 | tf2_ros 38 | urdf 39 | message_filters 40 | ) 41 | 42 | #Download velocity smoother 43 | set(YOCS_URL "https://github.com/chvmp/yocs_velocity_smoother") 44 | set(YOCS_DOWNLOAD_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../..) 45 | set(YOCS_DEPENDENCY_PATH ${YOCS_DOWNLOAD_PATH}/yocs_velocity_smoother) 46 | 47 | if (NOT EXISTS "${YOCS_DEPENDENCY_PATH}") 48 | message(STATUS "Downloading yocs_velocity_smoother") 49 | message(STATUS "${YOCS_DOWNLOAD_PATH}") 50 | 51 | execute_process( 52 | COMMAND git clone ${YOCS_URL} ${YOCS_DEPENDENCY_PATH} 53 | ) 54 | 55 | execute_process( 56 | WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../../.. 57 | COMMAND catkin_make --pkg yocs_velocity_smoother 58 | ) 59 | endif() 60 | #end of velocity smoother 61 | 62 | include_directories( 63 | include 64 | ${champ_INCLUDE_DIRS} 65 | ${champ_INCLUDE_DIRS}/champ/ 66 | ${champ_INCLUDE_DIRS}/champ/champ/ 67 | ${catkin_INCLUDE_DIRS} 68 | ) 69 | 70 | install(DIRECTORY config 71 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 72 | ) 73 | 74 | add_library(quadruped_controller src/quadruped_controller.cpp) 75 | add_library(message_relay src/message_relay.cpp) 76 | add_library(state_estimation src/state_estimation.cpp) 77 | 78 | add_executable(quadruped_controller_node src/quadruped_controller_node.cpp) 79 | target_link_libraries(quadruped_controller_node quadruped_controller ${catkin_LIBRARIES}) 80 | add_dependencies(quadruped_controller_node ${catkin_EXPORTED_TARGETS}) 81 | add_dependencies(quadruped_controller ${catkin_EXPORTED_TARGETS}) 82 | 83 | add_executable(message_relay_node src/message_relay_node.cpp) 84 | target_link_libraries(message_relay_node message_relay ${catkin_LIBRARIES}) 85 | add_dependencies(message_relay_node ${catkin_EXPORTED_TARGETS}) 86 | add_dependencies(message_relay ${catkin_EXPORTED_TARGETS}) 87 | 88 | add_executable(state_estimation_node src/state_estimation_node.cpp) 89 | target_link_libraries(state_estimation_node state_estimation ${catkin_LIBRARIES}) 90 | add_dependencies(state_estimation_node ${catkin_EXPORTED_TARGETS}) 91 | add_dependencies(state_estimation ${catkin_EXPORTED_TARGETS}) 92 | 93 | install(TARGETS 94 | quadruped_controller 95 | quadruped_controller_node 96 | message_relay 97 | message_relay_node 98 | state_estimation 99 | state_estimation_node 100 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 101 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 102 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 103 | ) -------------------------------------------------------------------------------- /champ_base/config/ekf/base_to_footprint.yaml: -------------------------------------------------------------------------------- 1 | frequency: 50 2 | 3 | transform_timeout: 0.01 4 | transform_time_offset: 0.045 5 | 6 | two_d_mode: false 7 | # diagnostics_agg: true 8 | 9 | #x , y , z, 10 | #roll , pitch , yaw, 11 | #vx , vy , vz, 12 | #vroll , vpitch, vyaw, 13 | #ax , ay , az 14 | 15 | pose0: base_to_footprint_pose 16 | pose0_config: [true, true, true, 17 | true, true, true, 18 | false, false, false, 19 | false, false, false, 20 | false, false, false] 21 | 22 | imu0: imu/data 23 | imu0_config: [false, false, false, 24 | false, false, false, 25 | true, true, true, 26 | false, false, false, 27 | false, false, false] 28 | 29 | odom_frame: base_footprint 30 | world_frame: base_footprint -------------------------------------------------------------------------------- /champ_base/config/ekf/footprint_to_odom.yaml: -------------------------------------------------------------------------------- 1 | frequency: 50 2 | 3 | transform_timeout: 0.01 4 | # transform_time_offset: 0.05 5 | 6 | two_d_mode: true 7 | # diagnostics_agg: true 8 | 9 | #x , y , z, 10 | #roll , pitch , yaw, 11 | #vx , vy , vz, 12 | #vroll , vpitch, vyaw, 13 | #ax , ay , az 14 | 15 | odom0: odom/raw 16 | odom0_config: [false, false, false, 17 | false, false, false, 18 | true, true, false, 19 | false, false, true, 20 | false, false, false] 21 | 22 | imu0: imu/data 23 | imu0_config: [false, false, false, 24 | false, false, true, 25 | false, false, false, 26 | false, false, true, 27 | false, false, false] 28 | 29 | odom_frame: odom 30 | base_link_frame: base_footprint 31 | world_frame: odom -------------------------------------------------------------------------------- /champ_base/config/velocity_smoother/velocity_smoother.yaml: -------------------------------------------------------------------------------- 1 | # Example configuration: 2 | # - velocity limits are around a 10% above the physical limits 3 | # - acceleration limits are just low enough to avoid jerking 4 | 5 | # Mandatory parameters 6 | speed_lim_v_x: 0.5 7 | speed_lim_v_y: 0.35 8 | 9 | speed_lim_w: 0.4 10 | 11 | accel_lim_v: 1.0 12 | accel_lim_w: 2.0 13 | 14 | # Optional parameters 15 | frequency: 10.0 16 | decel_factor: 1.0 17 | 18 | # Robot velocity feedback type: 19 | # 0 - none 20 | # 1 - odometry 21 | # 2 - end robot commands 22 | robot_feedback: 2 -------------------------------------------------------------------------------- /champ_base/include/actuator.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2019-2020, Juan Miguel Jimeno 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the copyright holder nor the names of its 13 | contributors may be used to endorse or promote products derived 14 | from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef ACTUATOR_H 29 | #define ACTUATOR_H 30 | 31 | namespace champ 32 | { 33 | class Actuator 34 | { 35 | float thetas_[12]; 36 | float prev_angle_; 37 | 38 | public: 39 | Actuator() 40 | { 41 | } 42 | 43 | void moveJoints(float joint_positions[12]) 44 | { 45 | for(unsigned int i = 0; i < 12; i++) 46 | { 47 | moveJoint(i, joint_positions[i]); 48 | } 49 | } 50 | 51 | void moveJoint(unsigned int leg_id, float joint_position) 52 | { 53 | //until a proper hardware interface is done, hardware integration can be done here 54 | 55 | //joint_position can be passed to the hardware api to set the joint position of the actuator 56 | 57 | //this stores the set joint_position by the controller for pseudo feedback. 58 | float delta = (joint_position - thetas_[leg_id]); 59 | //the random number is just to add noise to the reading 60 | thetas_[leg_id] = thetas_[leg_id] + (delta * (((rand() % 80) + 70) / 100.0)); 61 | } 62 | 63 | void getJointPositions(float joint_position[12]) 64 | { 65 | for(unsigned int i = 0; i < 12; i++) 66 | { 67 | joint_position[i] = getJointPosition(i); 68 | } 69 | } 70 | 71 | float getJointPosition(unsigned int leg_id) 72 | { 73 | //virtually this returns the stored joint_position set by the controller 74 | //until a proper hardware interface is done, this can be used to return 75 | //real actuator feedback 76 | return thetas_[leg_id]; 77 | } 78 | }; 79 | } 80 | #endif 81 | -------------------------------------------------------------------------------- /champ_base/include/message_relay.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2019-2020, Juan Miguel Jimeno 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the copyright holder nor the names of its 13 | contributors may be used to endorse or promote products derived 14 | from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef MESSAGE_RELAY_H 29 | #define QUADRUPED_CONTROLLER_H 30 | 31 | #include "ros/ros.h" 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | class MessageRelay 53 | { 54 | ros::Subscriber imu_raw_subscriber_; 55 | ros::Subscriber joints_raw_subscriber_; 56 | ros::Subscriber foot_contacts_subscriber_; 57 | 58 | ros::Publisher imu_publisher_; 59 | ros::Publisher mag_publisher_; 60 | ros::Publisher joint_states_publisher_; 61 | ros::Publisher joint_commands_publisher_; 62 | ros::Publisher foot_contacts_publisher_; 63 | 64 | std::vector joint_names_; 65 | std::string node_namespace_; 66 | std::string imu_frame_; 67 | 68 | bool in_gazebo_; 69 | bool has_imu_; 70 | 71 | sensor_msgs::Imu imu_data_; 72 | 73 | void IMURawCallback_(const champ_msgs::Imu::ConstPtr& msg); 74 | void jointStatesRawCallback_(const champ_msgs::Joints::ConstPtr& msg); 75 | void footContactCallback_(const champ_msgs::Contacts::ConstPtr& msg); 76 | 77 | public: 78 | MessageRelay(ros::NodeHandle *nh, ros::NodeHandle *pnh); 79 | }; 80 | 81 | #endif -------------------------------------------------------------------------------- /champ_base/include/quadruped_controller.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2019-2020, Juan Miguel Jimeno 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the copyright holder nor the names of its 13 | contributors may be used to endorse or promote products derived 14 | from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef QUADRUPED_CONTROLLER_H 29 | #define QUADRUPED_CONTROLLER_H 30 | 31 | #include "ros/ros.h" 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #include 44 | #include 45 | #include "tf/transform_datatypes.h" 46 | 47 | #include 48 | #include 49 | #include 50 | 51 | class QuadrupedController 52 | { 53 | ros::Subscriber cmd_vel_subscriber_; 54 | ros::Subscriber cmd_pose_subscriber_; 55 | 56 | ros::Publisher joint_states_publisher_; 57 | ros::Publisher joint_commands_publisher_; 58 | ros::Publisher foot_contacts_publisher_; 59 | 60 | ros::Timer loop_timer_; 61 | 62 | champ::Velocities req_vel_; 63 | champ::Pose req_pose_; 64 | 65 | champ::GaitConfig gait_config_; 66 | 67 | champ::QuadrupedBase base_; 68 | champ::BodyController body_controller_; 69 | champ::LegController leg_controller_; 70 | champ::Kinematics kinematics_; 71 | 72 | std::vector joint_names_; 73 | 74 | bool publish_foot_contacts_; 75 | bool publish_joint_states_; 76 | bool publish_joint_control_; 77 | bool in_gazebo_; 78 | 79 | void controlLoop_(const ros::TimerEvent& event); 80 | 81 | void publishJoints_(float target_joints[12]); 82 | void publishFootContacts_(bool foot_contacts[4]); 83 | 84 | void cmdVelCallback_(const geometry_msgs::Twist::ConstPtr& msg); 85 | void cmdPoseCallback_(const geometry_msgs::Pose::ConstPtr& msg); 86 | 87 | public: 88 | QuadrupedController(ros::NodeHandle *nh, ros::NodeHandle *pnh); 89 | }; 90 | 91 | #endif -------------------------------------------------------------------------------- /champ_base/include/state_estimation.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2019-2020, Juan Miguel Jimeno 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the copyright holder nor the names of its 13 | contributors may be used to endorse or promote products derived 14 | from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef STATE_ESTIMATION_H 29 | #define STATE_ESTIMATION_H 30 | 31 | #include "ros/ros.h" 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | #include 51 | #include 52 | #include 53 | #include 54 | 55 | class StateEstimation 56 | { 57 | typedef message_filters::sync_policies::ApproximateTime SyncPolicy; 58 | typedef message_filters::Synchronizer Sync; 59 | boost::shared_ptr sync; 60 | 61 | message_filters::Subscriber joint_states_subscriber_; 62 | message_filters::Subscriber foot_contacts_subscriber_; 63 | ros::Subscriber imu_subscriber_; 64 | 65 | ros::Publisher footprint_to_odom_publisher_; 66 | ros::Publisher base_to_footprint_publisher_; 67 | ros::Publisher foot_publisher_; 68 | 69 | tf2_ros::TransformBroadcaster base_broadcaster_; 70 | 71 | ros::Timer odom_data_timer_; 72 | ros::Timer base_pose_timer_; 73 | 74 | champ::Velocities current_velocities_; 75 | geometry::Transformation current_foot_positions_[4]; 76 | geometry::Transformation target_foot_positions_[4]; 77 | 78 | float x_pos_; 79 | float y_pos_; 80 | float heading_; 81 | ros::Time last_vel_time_; 82 | ros::Time last_sync_time_; 83 | sensor_msgs::ImuConstPtr last_imu_; 84 | 85 | champ::GaitConfig gait_config_; 86 | 87 | champ::QuadrupedBase base_; 88 | champ::Odometry odometry_; 89 | 90 | std::vector joint_names_; 91 | std::string base_name_; 92 | std::string node_namespace_; 93 | std::string odom_frame_; 94 | std::string base_footprint_frame_; 95 | std::string base_link_frame_; 96 | bool orientation_from_imu_; 97 | 98 | void publishFootprintToOdom_(const ros::TimerEvent& event); 99 | void publishBaseToFootprint_(const ros::TimerEvent& event); 100 | void synchronized_callback_(const sensor_msgs::JointStateConstPtr&, const champ_msgs::ContactsStampedConstPtr&); 101 | void imu_callback_(const sensor_msgs::ImuConstPtr&); 102 | 103 | visualization_msgs::Marker createMarker_(geometry::Transformation foot_pos, int id, std::string frame_id); 104 | 105 | public: 106 | StateEstimation(ros::NodeHandle *nh, ros::NodeHandle *pnh); 107 | }; 108 | 109 | #endif -------------------------------------------------------------------------------- /champ_base/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | champ_base 4 | 0.1.0 5 | CHAMP ROS Hardware and Software Drivers 6 | 7 | Juan Miguel Jimeno 8 | Juan Miguel Jimeno 9 | 10 | BSD 11 | 12 | catkin 13 | roslaunch 14 | roscpp 15 | rospy 16 | champ 17 | champ_msgs 18 | visualization_msgs 19 | geometry_msgs 20 | sensor_msgs 21 | trajectory_msgs 22 | nav_msgs 23 | tf 24 | tf2_ros 25 | urdf 26 | message_filters 27 | 28 | -------------------------------------------------------------------------------- /champ_base/src/message_relay.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2019-2020, Juan Miguel Jimeno 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the copyright holder nor the names of its 13 | contributors may be used to endorse or promote products derived 14 | from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include 29 | 30 | MessageRelay::MessageRelay(ros::NodeHandle *nh, ros::NodeHandle *pnh) 31 | { 32 | imu_data_.orientation.w = 1.0; 33 | 34 | foot_contacts_publisher_ = nh->advertise("foot_contacts", 1); 35 | 36 | joint_states_publisher_ = nh->advertise("joint_states", 1); 37 | joint_commands_publisher_ = nh->advertise("joint_group_position_controller/command", 1); 38 | 39 | imu_raw_subscriber_ = nh->subscribe("imu/raw", 1, &MessageRelay::IMURawCallback_, this); 40 | joints_raw_subscriber_ = nh->subscribe("joint_states/raw", 1, &MessageRelay::jointStatesRawCallback_, this); 41 | foot_contacts_subscriber_ = nh->subscribe("foot_contacts/raw", 1, &MessageRelay::footContactCallback_, this); 42 | 43 | pnh->getParam("gazebo", in_gazebo_); 44 | pnh->getParam("has_imu", has_imu_); 45 | 46 | joint_names_ = champ::URDF::getJointNames(nh); 47 | 48 | node_namespace_ = ros::this_node::getNamespace(); 49 | if(node_namespace_.length() > 1) 50 | { 51 | node_namespace_.replace(0, 1, ""); 52 | node_namespace_.push_back('/'); 53 | } 54 | else 55 | { 56 | node_namespace_ = ""; 57 | } 58 | 59 | if(has_imu_) 60 | { 61 | imu_publisher_ = nh->advertise("imu/data", 1); 62 | mag_publisher_ = nh->advertise("imu/mag", 1); 63 | } 64 | 65 | imu_frame_ = node_namespace_ + "imu_link"; 66 | } 67 | 68 | void MessageRelay::IMURawCallback_(const champ_msgs::Imu::ConstPtr& msg) 69 | { 70 | sensor_msgs::Imu imu_data_msg; 71 | sensor_msgs::MagneticField imu_mag_msg; 72 | 73 | imu_data_msg.header.stamp = ros::Time::now(); 74 | imu_data_msg.header.frame_id = imu_frame_; 75 | 76 | imu_data_msg.orientation.w = msg->orientation.w; 77 | imu_data_msg.orientation.x = msg->orientation.x; 78 | imu_data_msg.orientation.y = msg->orientation.y; 79 | imu_data_msg.orientation.z = msg->orientation.z; 80 | 81 | imu_data_msg.linear_acceleration.x = msg->linear_acceleration.x; 82 | imu_data_msg.linear_acceleration.y = msg->linear_acceleration.y; 83 | imu_data_msg.linear_acceleration.z = msg->linear_acceleration.z; 84 | 85 | imu_data_msg.angular_velocity.x = msg->angular_velocity.x; 86 | imu_data_msg.angular_velocity.y = msg->angular_velocity.y; 87 | imu_data_msg.angular_velocity.z = msg->angular_velocity.z; 88 | 89 | imu_data_msg.orientation_covariance[0] = 0.0025; 90 | imu_data_msg.orientation_covariance[4] = 0.0025; 91 | imu_data_msg.orientation_covariance[8] = 0.0025; 92 | 93 | imu_data_msg.angular_velocity_covariance[0] = 0.000001; 94 | imu_data_msg.angular_velocity_covariance[4] = 0.000001; 95 | imu_data_msg.angular_velocity_covariance[8] = 0.000001; 96 | 97 | imu_data_msg.linear_acceleration_covariance[0] = 0.0001; 98 | imu_data_msg.linear_acceleration_covariance[4] = 0.0001; 99 | imu_data_msg.linear_acceleration_covariance[8] = 0.0001; 100 | 101 | imu_mag_msg.header.stamp = ros::Time::now(); 102 | imu_mag_msg.header.frame_id = imu_frame_; 103 | 104 | imu_mag_msg.magnetic_field.x = msg->magnetic_field.x; 105 | imu_mag_msg.magnetic_field.y = msg->magnetic_field.y; 106 | imu_mag_msg.magnetic_field.z = msg->magnetic_field.z; 107 | 108 | imu_mag_msg.magnetic_field_covariance[0] = 0.000001; 109 | imu_mag_msg.magnetic_field_covariance[4] = 0.000001; 110 | imu_mag_msg.magnetic_field_covariance[8] = 0.000001; 111 | 112 | if(has_imu_) 113 | { 114 | imu_publisher_.publish(imu_data_msg); 115 | mag_publisher_.publish(imu_mag_msg); 116 | } 117 | } 118 | 119 | void MessageRelay::jointStatesRawCallback_(const champ_msgs::Joints::ConstPtr& msg) 120 | { 121 | if(in_gazebo_) 122 | { 123 | trajectory_msgs::JointTrajectory joints_cmd_msg; 124 | joints_cmd_msg.header.stamp = ros::Time::now(); 125 | joints_cmd_msg.joint_names = joint_names_; 126 | 127 | trajectory_msgs::JointTrajectoryPoint point; 128 | point.positions.resize(12); 129 | 130 | point.time_from_start = ros::Duration(1.0 / 60.0); 131 | for(size_t i = 0; i < 12; i++) 132 | { 133 | point.positions[i] = msg->position[i]; 134 | } 135 | 136 | joints_cmd_msg.points.push_back(point); 137 | joint_commands_publisher_.publish(joints_cmd_msg); 138 | } 139 | else 140 | { 141 | sensor_msgs::JointState joints_msg; 142 | 143 | joints_msg.header.stamp = ros::Time::now(); 144 | joints_msg.name.resize(joint_names_.size()); 145 | joints_msg.position.resize(joint_names_.size()); 146 | joints_msg.name = joint_names_; 147 | 148 | for (size_t i = 0; i < joint_names_.size(); ++i) 149 | { 150 | joints_msg.position[i]= msg->position[i]; 151 | } 152 | 153 | joint_states_publisher_.publish(joints_msg); 154 | } 155 | } 156 | 157 | void MessageRelay::footContactCallback_(const champ_msgs::Contacts::ConstPtr& msg) 158 | { 159 | champ_msgs::ContactsStamped contacts_msg; 160 | contacts_msg.header.stamp = ros::Time::now(); 161 | contacts_msg.contacts.resize(4); 162 | 163 | for(size_t i = 0; i < 4; i++) 164 | { 165 | contacts_msg.contacts[i] = msg->contacts[i]; 166 | } 167 | 168 | foot_contacts_publisher_.publish(contacts_msg); 169 | } -------------------------------------------------------------------------------- /champ_base/src/message_relay_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2019-2020, Juan Miguel Jimeno 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the copyright holder nor the names of its 13 | contributors may be used to endorse or promote products derived 14 | from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include "ros/ros.h" 29 | #include "message_relay.h" 30 | 31 | int main(int argc, char** argv ) 32 | { 33 | ros::init(argc, argv, "message_relay_node"); 34 | 35 | ros::NodeHandle nh(""); 36 | ros::NodeHandle nh_private("~"); 37 | 38 | MessageRelay relay(&nh, &nh_private); 39 | 40 | ros::spin(); 41 | return 0; 42 | } -------------------------------------------------------------------------------- /champ_base/src/quadruped_controller.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2019-2020, Juan Miguel Jimeno 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the copyright holder nor the names of its 13 | contributors may be used to endorse or promote products derived 14 | from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include 29 | 30 | champ::PhaseGenerator::Time rosTimeToChampTime(const ros::Time& time) 31 | { 32 | return time.toNSec() / 1000ul; 33 | } 34 | 35 | QuadrupedController::QuadrupedController(ros::NodeHandle *nh, ros::NodeHandle *pnh): 36 | body_controller_(base_), 37 | leg_controller_(base_, rosTimeToChampTime(ros::Time::now())), 38 | kinematics_(base_) 39 | { 40 | std::string joint_control_topic = "joint_group_position_controller/command"; 41 | std::string knee_orientation; 42 | double loop_rate = 200.0; 43 | 44 | nh->getParam("gait/pantograph_leg", gait_config_.pantograph_leg); 45 | nh->getParam("gait/max_linear_velocity_x", gait_config_.max_linear_velocity_x); 46 | nh->getParam("gait/max_linear_velocity_y", gait_config_.max_linear_velocity_y); 47 | nh->getParam("gait/max_angular_velocity_z", gait_config_.max_angular_velocity_z); 48 | nh->getParam("gait/com_x_translation", gait_config_.com_x_translation); 49 | nh->getParam("gait/swing_height", gait_config_.swing_height); 50 | nh->getParam("gait/stance_depth", gait_config_.stance_depth); 51 | nh->getParam("gait/stance_duration", gait_config_.stance_duration); 52 | nh->getParam("gait/nominal_height", gait_config_.nominal_height); 53 | nh->getParam("gait/knee_orientation", knee_orientation); 54 | pnh->getParam("publish_foot_contacts", publish_foot_contacts_); 55 | pnh->getParam("publish_joint_states", publish_joint_states_); 56 | pnh->getParam("publish_joint_control", publish_joint_control_); 57 | pnh->getParam("gazebo", in_gazebo_); 58 | pnh->getParam("joint_controller_topic", joint_control_topic); 59 | pnh->getParam("loop_rate", loop_rate); 60 | 61 | cmd_vel_subscriber_ = nh->subscribe("cmd_vel/smooth", 1, &QuadrupedController::cmdVelCallback_, this); 62 | cmd_pose_subscriber_ = nh->subscribe("body_pose", 1, &QuadrupedController::cmdPoseCallback_, this); 63 | 64 | if(publish_joint_control_) 65 | { 66 | joint_commands_publisher_ = nh->advertise(joint_control_topic, 1); 67 | } 68 | 69 | if(publish_joint_states_ && !in_gazebo_) 70 | { 71 | joint_states_publisher_ = nh->advertise("joint_states", 1); 72 | } 73 | 74 | if(publish_foot_contacts_ && !in_gazebo_) 75 | { 76 | foot_contacts_publisher_ = nh->advertise("foot_contacts", 1); 77 | } 78 | 79 | gait_config_.knee_orientation = knee_orientation.c_str(); 80 | 81 | base_.setGaitConfig(gait_config_); 82 | champ::URDF::loadFromServer(base_, nh); 83 | joint_names_ = champ::URDF::getJointNames(nh); 84 | 85 | loop_timer_ = pnh->createTimer(ros::Duration(1 / loop_rate), 86 | &QuadrupedController::controlLoop_, 87 | this); 88 | 89 | req_pose_.position.z = gait_config_.nominal_height; 90 | } 91 | 92 | void QuadrupedController::controlLoop_(const ros::TimerEvent& event) 93 | { 94 | float target_joint_positions[12]; 95 | geometry::Transformation target_foot_positions[4]; 96 | bool foot_contacts[4]; 97 | 98 | body_controller_.poseCommand(target_foot_positions, req_pose_); 99 | leg_controller_.velocityCommand(target_foot_positions, req_vel_, rosTimeToChampTime(ros::Time::now())); 100 | kinematics_.inverse(target_joint_positions, target_foot_positions); 101 | 102 | publishFootContacts_(foot_contacts); 103 | publishJoints_(target_joint_positions); 104 | } 105 | 106 | void QuadrupedController::cmdVelCallback_(const geometry_msgs::Twist::ConstPtr& msg) 107 | { 108 | req_vel_.linear.x = msg->linear.x; 109 | req_vel_.linear.y = msg->linear.y; 110 | req_vel_.angular.z = msg->angular.z; 111 | } 112 | 113 | void QuadrupedController::cmdPoseCallback_(const geometry_msgs::Pose::ConstPtr& msg) 114 | { 115 | tf::Quaternion quat( 116 | msg->orientation.x, 117 | msg->orientation.y, 118 | msg->orientation.z, 119 | msg->orientation.w); 120 | tf::Matrix3x3 m(quat); 121 | double roll, pitch, yaw; 122 | m.getRPY(roll, pitch, yaw); 123 | 124 | req_pose_.orientation.roll = roll; 125 | req_pose_.orientation.pitch = pitch; 126 | req_pose_.orientation.yaw = yaw; 127 | 128 | req_pose_.position.x = msg->position.x; 129 | req_pose_.position.y = msg->position.y; 130 | req_pose_.position.z = msg->position.z + gait_config_.nominal_height; 131 | } 132 | 133 | void QuadrupedController::publishJoints_(float target_joints[12]) 134 | { 135 | if(publish_joint_control_) 136 | { 137 | trajectory_msgs::JointTrajectory joints_cmd_msg; 138 | joints_cmd_msg.header.stamp = ros::Time::now(); 139 | joints_cmd_msg.joint_names = joint_names_; 140 | 141 | trajectory_msgs::JointTrajectoryPoint point; 142 | point.positions.resize(12); 143 | 144 | point.time_from_start = ros::Duration(1.0 / 60.0); 145 | for(size_t i = 0; i < 12; i++) 146 | { 147 | point.positions[i] = target_joints[i]; 148 | } 149 | 150 | joints_cmd_msg.points.push_back(point); 151 | joint_commands_publisher_.publish(joints_cmd_msg); 152 | } 153 | 154 | if(publish_joint_states_ && !in_gazebo_) 155 | { 156 | sensor_msgs::JointState joints_msg; 157 | 158 | joints_msg.header.stamp = ros::Time::now(); 159 | joints_msg.name.resize(joint_names_.size()); 160 | joints_msg.position.resize(joint_names_.size()); 161 | joints_msg.name = joint_names_; 162 | 163 | for (size_t i = 0; i < joint_names_.size(); ++i) 164 | { 165 | joints_msg.position[i]= target_joints[i]; 166 | } 167 | 168 | joint_states_publisher_.publish(joints_msg); 169 | } 170 | } 171 | 172 | void QuadrupedController::publishFootContacts_(bool foot_contacts[4]) 173 | { 174 | if(publish_foot_contacts_ && !in_gazebo_) 175 | { 176 | champ_msgs::ContactsStamped contacts_msg; 177 | contacts_msg.header.stamp = ros::Time::now(); 178 | contacts_msg.contacts.resize(4); 179 | 180 | for(size_t i = 0; i < 4; i++) 181 | { 182 | //This is only published when there's no feedback on the robot 183 | //that a leg is in contact with the ground 184 | //For such cases, we use the stance phase in the gait for foot contacts 185 | contacts_msg.contacts[i] = base_.legs[i]->gait_phase(); 186 | } 187 | 188 | foot_contacts_publisher_.publish(contacts_msg); 189 | } 190 | } 191 | -------------------------------------------------------------------------------- /champ_base/src/quadruped_controller_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2019-2020, Juan Miguel Jimeno 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the copyright holder nor the names of its 13 | contributors may be used to endorse or promote products derived 14 | from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include "ros/ros.h" 29 | #include "quadruped_controller.h" 30 | 31 | int main(int argc, char** argv ) 32 | { 33 | ros::init(argc, argv, "quadruped_controller_node"); 34 | 35 | ros::NodeHandle nh(""); 36 | ros::NodeHandle nh_private("~"); 37 | 38 | QuadrupedController champ(&nh, &nh_private); 39 | 40 | ros::spin(); 41 | return 0; 42 | } -------------------------------------------------------------------------------- /champ_base/src/state_estimation.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2019-2020, Juan Miguel Jimeno 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the copyright holder nor the names of its 13 | contributors may be used to endorse or promote products derived 14 | from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include 29 | 30 | champ::Odometry::Time rosTimeToChampTime(const ros::Time& time) 31 | { 32 | return time.toNSec() / 1000ul; 33 | } 34 | 35 | StateEstimation::StateEstimation(ros::NodeHandle *nh, ros::NodeHandle *pnh): 36 | odometry_(base_, rosTimeToChampTime(ros::Time::now())) 37 | { 38 | joint_states_subscriber_.subscribe(*nh, "joint_states", 1); 39 | foot_contacts_subscriber_ .subscribe(*nh, "foot_contacts", 1); 40 | 41 | sync.reset(new Sync(SyncPolicy(10), joint_states_subscriber_, foot_contacts_subscriber_)); 42 | sync->registerCallback(boost::bind(&StateEstimation::synchronized_callback_, this, _1, _2)); 43 | 44 | footprint_to_odom_publisher_ = nh->advertise("odom/raw", 1); 45 | base_to_footprint_publisher_ = nh->advertise("base_to_footprint_pose", 1); 46 | foot_publisher_ = nh->advertise("foot", 1); 47 | 48 | nh->getParam("links_map/base", base_name_); 49 | nh->getParam("gait/odom_scaler", gait_config_.odom_scaler); 50 | pnh->param("orientation_from_imu", orientation_from_imu_, false); 51 | 52 | if (orientation_from_imu_) 53 | imu_subscriber_ = nh->subscribe("imu/data", 1, &StateEstimation::imu_callback_, this); 54 | 55 | base_.setGaitConfig(gait_config_); 56 | champ::URDF::loadFromServer(base_, nh); 57 | joint_names_ = champ::URDF::getJointNames(nh); 58 | 59 | node_namespace_ = ros::this_node::getNamespace(); 60 | if(node_namespace_.length() > 1) 61 | { 62 | node_namespace_.replace(0, 1, ""); 63 | node_namespace_.push_back('/'); 64 | } 65 | else 66 | { 67 | node_namespace_ = ""; 68 | } 69 | 70 | odom_frame_ = node_namespace_ + "odom"; 71 | base_footprint_frame_ = node_namespace_ + "base_footprint"; 72 | base_link_frame_ = node_namespace_ + base_name_; 73 | 74 | odom_data_timer_ = pnh->createTimer(ros::Duration(0.02), 75 | &StateEstimation::publishFootprintToOdom_, 76 | this); 77 | 78 | base_pose_timer_ = pnh->createTimer(ros::Duration(0.02), 79 | &StateEstimation::publishBaseToFootprint_, 80 | this); 81 | } 82 | 83 | void StateEstimation::synchronized_callback_(const sensor_msgs::JointStateConstPtr& joints_msg, const champ_msgs::ContactsStampedConstPtr& contacts_msg) 84 | { 85 | last_sync_time_ = ros::Time::now(); 86 | 87 | float current_joint_positions[12]; 88 | 89 | for(size_t i = 0; i < joints_msg->name.size(); i++) 90 | { 91 | std::vector::iterator itr = std::find(joint_names_.begin(), joint_names_.end(), joints_msg->name[i]); 92 | 93 | int index = std::distance(joint_names_.begin(), itr); 94 | current_joint_positions[index] = joints_msg->position[i]; 95 | } 96 | 97 | base_.updateJointPositions(current_joint_positions); 98 | 99 | for(size_t i = 0; i < 4; i++) 100 | { 101 | base_.legs[i]->in_contact(contacts_msg->contacts[i]); 102 | } 103 | } 104 | 105 | void StateEstimation::imu_callback_(const sensor_msgs::ImuConstPtr& msg) 106 | { 107 | last_imu_ = msg; 108 | } 109 | 110 | void StateEstimation::publishFootprintToOdom_(const ros::TimerEvent& event) 111 | { 112 | odometry_.getVelocities(current_velocities_, rosTimeToChampTime(ros::Time::now())); 113 | 114 | ros::Time current_time = ros::Time::now(); 115 | 116 | double vel_dt = (current_time - last_vel_time_).toSec(); 117 | last_vel_time_ = current_time; 118 | 119 | //rotate in the z axis 120 | //https://en.wikipedia.org/wiki/Rotation_matrix 121 | double delta_heading = current_velocities_.angular.z * vel_dt; 122 | double delta_x = (current_velocities_.linear.x * cos(heading_) - current_velocities_.linear.y * sin(heading_)) * vel_dt; //m 123 | double delta_y = (current_velocities_.linear.x * sin(heading_) + current_velocities_.linear.y * cos(heading_)) * vel_dt; //m 124 | 125 | //calculate current position of the robot 126 | x_pos_ += delta_x; 127 | y_pos_ += delta_y; 128 | heading_ += delta_heading; 129 | 130 | //calculate robot's heading_ in quaternion angle 131 | tf2::Quaternion odom_quat; 132 | odom_quat.setRPY(0, 0, heading_); 133 | 134 | nav_msgs::Odometry odom; 135 | odom.header.stamp = current_time; 136 | odom.header.frame_id = odom_frame_; 137 | odom.child_frame_id = base_footprint_frame_; 138 | 139 | //robot's position in x,y, and z 140 | odom.pose.pose.position.x = x_pos_; 141 | odom.pose.pose.position.y = y_pos_; 142 | odom.pose.pose.position.z = 0.0; 143 | //robot's heading_ in quaternion 144 | odom.pose.pose.orientation.x = odom_quat.x(); 145 | odom.pose.pose.orientation.y = odom_quat.y(); 146 | odom.pose.pose.orientation.z = odom_quat.z(); 147 | odom.pose.pose.orientation.w = odom_quat.w(); 148 | odom.pose.covariance[0] = 0.25; 149 | odom.pose.covariance[7] = 0.25; 150 | odom.pose.covariance[35] = 0.017; 151 | 152 | odom.twist.twist.linear.x = current_velocities_.linear.x; 153 | odom.twist.twist.linear.y = current_velocities_.linear.y; 154 | odom.twist.twist.linear.z = 0.0; 155 | 156 | odom.twist.twist.angular.x = 0.0; 157 | odom.twist.twist.angular.y = 0.0; 158 | odom.twist.twist.angular.z = current_velocities_.angular.z; 159 | 160 | odom.twist.covariance[0] = 0.3; 161 | odom.twist.covariance[7] = 0.3; 162 | odom.twist.covariance[35] = 0.017; 163 | 164 | footprint_to_odom_publisher_.publish(odom); 165 | } 166 | 167 | visualization_msgs::Marker StateEstimation::createMarker_(geometry::Transformation foot_pos, int id, std::string frame_id) 168 | { 169 | visualization_msgs::Marker foot_marker; 170 | 171 | foot_marker.header.frame_id = frame_id; 172 | 173 | foot_marker.type = visualization_msgs::Marker::SPHERE; 174 | foot_marker.action = visualization_msgs::Marker::ADD; 175 | foot_marker.id = id; 176 | 177 | foot_marker.pose.position.x = foot_pos.X(); 178 | foot_marker.pose.position.y = foot_pos.Y(); 179 | foot_marker.pose.position.z = foot_pos.Z(); 180 | 181 | foot_marker.pose.orientation.x = 0.0; 182 | foot_marker.pose.orientation.y = 0.0; 183 | foot_marker.pose.orientation.z = 0.0; 184 | foot_marker.pose.orientation.w = 1.0; 185 | 186 | foot_marker.scale.x = 0.025; 187 | foot_marker.scale.y = 0.025; 188 | foot_marker.scale.z = 0.025; 189 | 190 | foot_marker.color.r = 0.780; 191 | foot_marker.color.g = 0.082; 192 | foot_marker.color.b = 0.521; 193 | foot_marker.color.a = 0.5; 194 | 195 | return foot_marker; 196 | } 197 | 198 | void StateEstimation::publishBaseToFootprint_(const ros::TimerEvent& event) 199 | { 200 | base_.getFootPositions(current_foot_positions_); 201 | 202 | visualization_msgs::MarkerArray marker_array; 203 | float robot_height = 0.0, all_height = 0.0; 204 | int foot_in_contact = 0; 205 | geometry::Transformation touching_feet[4]; 206 | bool no_contact = false; 207 | 208 | for(size_t i = 0; i < 4; i++) 209 | { 210 | marker_array.markers.push_back(createMarker_(current_foot_positions_[i], i, base_link_frame_)); 211 | if(base_.legs[i]->in_contact()) 212 | { 213 | robot_height += current_foot_positions_[i].Z(); 214 | touching_feet[foot_in_contact] = current_foot_positions_[i]; 215 | foot_in_contact++; 216 | } 217 | all_height += current_foot_positions_[i].Z(); 218 | } 219 | 220 | if (foot_in_contact == 0) 221 | { 222 | no_contact = true; 223 | robot_height = all_height; 224 | foot_in_contact = 4; 225 | for (size_t i = 0; i < 4; ++i) 226 | touching_feet[i] = current_foot_positions_[i]; 227 | } 228 | 229 | if(foot_publisher_.getNumSubscribers()) 230 | { 231 | foot_publisher_.publish(marker_array); 232 | } 233 | 234 | tf2::Vector3 x_axis(1, 0, 0); 235 | tf2::Vector3 y_axis(0, 1, 0); 236 | tf2::Vector3 z_axis(0, 0, 1); 237 | 238 | // if the IMU provides good orientation estimates, these can be used to 239 | // greatly improve body orientation; IMUs in Gazebo provide even non-noisy 240 | // orientation measurements! 241 | tf2::Matrix3x3 imu_rotation; 242 | if (orientation_from_imu_ && last_imu_ != nullptr) 243 | { 244 | tf2::Quaternion imu_orientation( 245 | last_imu_->orientation.x, 246 | last_imu_->orientation.y, 247 | last_imu_->orientation.z, 248 | last_imu_->orientation.w); 249 | imu_rotation.setRotation(imu_orientation); 250 | } 251 | else 252 | { 253 | imu_rotation.setIdentity(); 254 | } 255 | 256 | // handle the orientation estimation based on the number of touching legs 257 | if (foot_in_contact >= 3 && !no_contact) 258 | { 259 | // 3 or 4 legs touching. 3 points are enough to form a plane, so we choose 260 | // any 3 touching legs and create a plane from them 261 | 262 | // create two vectors in base_footprint plane 263 | x_axis = tf2::Vector3(touching_feet[0].X() - touching_feet[2].X(), 264 | touching_feet[0].Y() - touching_feet[2].Y(), 265 | touching_feet[0].Z() - touching_feet[2].Z()); 266 | x_axis.normalize(); 267 | 268 | y_axis = tf2::Vector3(touching_feet[1].X() - touching_feet[2].X(), 269 | touching_feet[1].Y() - touching_feet[2].Y(), 270 | touching_feet[1].Z() - touching_feet[2].Z()); 271 | y_axis.normalize(); 272 | 273 | // compute normal vector of the plane 274 | z_axis = x_axis.cross(y_axis); 275 | z_axis.normalize(); 276 | 277 | // we don't know which 3 feet were chosen, so it might happen the normal points downwards 278 | if (z_axis.dot(tf2::Vector3(0, 0, 1)) < 0) 279 | z_axis = -z_axis; 280 | 281 | // project 0,1,0 base_link axis to the plane defined by the normal 282 | y_axis = (tf2::Vector3(0, 1, 0) - (tf2::Vector3(0, 1, 0).dot(z_axis) * z_axis)).normalized(); 283 | // and find the last vector which just has to be perpendicular to y and z 284 | x_axis = y_axis.cross(z_axis); 285 | } 286 | else if (foot_in_contact == 2) 287 | { 288 | if ((base_.legs[0]->in_contact() && base_.legs[2]->in_contact()) || 289 | (base_.legs[1]->in_contact() && base_.legs[3]->in_contact())) 290 | { 291 | // both left or both right legs are touching... let them define the x axis 292 | x_axis = tf2::Vector3(touching_feet[0].X() - touching_feet[1].X(), 293 | touching_feet[0].Y() - touching_feet[1].Y(), 294 | touching_feet[0].Z() - touching_feet[1].Z()); 295 | x_axis.normalize(); 296 | 297 | // get Z from IMU as we do not have enough contact points to define a plane 298 | z_axis = imu_rotation.inverse() * z_axis; 299 | y_axis = z_axis.cross(x_axis); 300 | // and find the last vector which just has to be perpendicular to y and z 301 | x_axis = y_axis.cross(z_axis); 302 | } 303 | else if ((base_.legs[0]->in_contact() && base_.legs[1]->in_contact()) || 304 | (base_.legs[2]->in_contact() && base_.legs[3]->in_contact())) 305 | { 306 | // both front or both hind legs are touching... let them define the y axis 307 | y_axis = tf2::Vector3(touching_feet[0].X() - touching_feet[1].X(), 308 | touching_feet[0].Y() - touching_feet[1].Y(), 309 | touching_feet[0].Z() - touching_feet[1].Z()); 310 | y_axis.normalize(); 311 | 312 | // get Z from IMU as we do not have enough contact points to define a plane 313 | z_axis = imu_rotation.inverse() * z_axis; 314 | x_axis = y_axis.cross(z_axis); 315 | // and find the last vector which just has to be perpendicular to x and z 316 | y_axis = z_axis.cross(x_axis); 317 | } 318 | else 319 | { 320 | // diagonal legs touching... axis1 is the line going through both touching 321 | // legs. axis2 is perpendicular to axis1 and z axis (from IMU)... then we 322 | // just rotate axis1 and axis2 to form a coordinate system 323 | tf2::Vector3 axis1(touching_feet[0].X() - touching_feet[1].X(), 324 | touching_feet[0].Y() - touching_feet[1].Y(), 325 | touching_feet[0].Z() - touching_feet[1].Z()); 326 | axis1.normalize(); 327 | 328 | // get Z from IMU as we do not have enough contact points to define a plane 329 | z_axis = imu_rotation.inverse() * z_axis; 330 | auto axis2 = z_axis.cross(axis1); 331 | z_axis = axis1.cross(axis2); 332 | 333 | // project base_link 1,0,0 axis along the computed plane normal 334 | x_axis = (x_axis - (x_axis.dot(z_axis) * z_axis)).normalized(); 335 | // and find the last vector which just has to be perpendicular to x and z 336 | y_axis = z_axis.cross(x_axis); 337 | } 338 | } 339 | else if (foot_in_contact == 1 || no_contact) 340 | { 341 | // Zero or one feet in contact... There isn't much to do, so just take Z from IMU 342 | z_axis = imu_rotation.inverse() * z_axis; 343 | 344 | // project base_link 1,0,0 axis along the computed plane normal 345 | x_axis = (x_axis - (x_axis.dot(z_axis) * z_axis)).normalized(); 346 | // and find the last vector which just has to be perpendicular to x and z 347 | y_axis = z_axis.cross(x_axis); 348 | } 349 | 350 | tf2::Matrix3x3 rotationMatrix( 351 | x_axis.x(), y_axis.x(), z_axis.x(), 352 | x_axis.y(), y_axis.y(), z_axis.y(), 353 | x_axis.z(), y_axis.z(), z_axis.z()); 354 | 355 | tf2::Quaternion quaternion; 356 | rotationMatrix.getRotation(quaternion); 357 | quaternion.normalize(); 358 | 359 | geometry_msgs::PoseWithCovarianceStamped pose_msg; 360 | pose_msg.header.frame_id = base_footprint_frame_; 361 | pose_msg.header.stamp = ros::Time::now(); 362 | 363 | pose_msg.pose.covariance[0] = 0.001; 364 | pose_msg.pose.covariance[7] = 0.001; 365 | pose_msg.pose.covariance[14] = 0.001; 366 | pose_msg.pose.covariance[21] = 0.0001; 367 | pose_msg.pose.covariance[28] = 0.0001; 368 | pose_msg.pose.covariance[35] = 0.017; 369 | 370 | pose_msg.pose.pose.position.x = 0.0; 371 | pose_msg.pose.pose.position.y = 0.0; 372 | pose_msg.pose.pose.position.z = -(robot_height / (float)foot_in_contact); 373 | 374 | pose_msg.pose.pose.orientation.x = quaternion.x(); 375 | pose_msg.pose.pose.orientation.y = quaternion.y(); 376 | pose_msg.pose.pose.orientation.z = quaternion.z(); 377 | pose_msg.pose.pose.orientation.w = -quaternion.w(); 378 | 379 | base_to_footprint_publisher_.publish(pose_msg); 380 | } -------------------------------------------------------------------------------- /champ_base/src/state_estimation_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2019-2020, Juan Miguel Jimeno 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the copyright holder nor the names of its 13 | contributors may be used to endorse or promote products derived 14 | from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include "ros/ros.h" 29 | #include "state_estimation.h" 30 | 31 | int main(int argc, char** argv ) 32 | { 33 | ros::init(argc, argv, "state_estimation_node"); 34 | 35 | ros::NodeHandle nh(""); 36 | ros::NodeHandle nh_private("~"); 37 | 38 | StateEstimation state_estimator(&nh, &nh_private); 39 | 40 | ros::spin(); 41 | return 0; 42 | } -------------------------------------------------------------------------------- /champ_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(champ_bringup) 3 | 4 | find_package(catkin 5 | REQUIRED 6 | COMPONENTS 7 | ) 8 | 9 | catkin_package() 10 | 11 | install(DIRECTORY launch 12 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 13 | ) -------------------------------------------------------------------------------- /champ_bringup/launch/bringup.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 82 | 83 | -------------------------------------------------------------------------------- /champ_bringup/launch/include/laser/d435.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 10 | 11 | 12 | 13 | 14 | target_frame: laser 15 | transform_tolerance: 0.01 16 | min_height: 0.0 17 | max_height: 0.5 18 | 19 | angle_min: -1.5708 # -M_PI/2 20 | angle_max: 1.5708 # M_PI/2 21 | angle_increment: 0.0087 # M_PI/360.0 22 | scan_time: 0.3333 23 | range_min: 0.15 24 | range_max: 15 25 | use_inf: true 26 | inf_epsilon: 1.0 27 | 28 | # Concurrency level, affects number of pointclouds queued for processing, thread number governed by nodelet manager 29 | # 0 : Detect number of cores 30 | # 1 : Single threaded 31 | # 2 : Parallelism level 32 | concurrency_level: 0 33 | 34 | 35 | -------------------------------------------------------------------------------- /champ_bringup/launch/include/laser/hokuyo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /champ_bringup/launch/include/laser/kinect.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /champ_bringup/launch/include/laser/lms1xx.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /champ_bringup/launch/include/laser/realsense.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /champ_bringup/launch/include/laser/rplidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /champ_bringup/launch/include/laser/sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /champ_bringup/launch/include/laser/sweep.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /champ_bringup/launch/include/laser/xv11.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /champ_bringup/launch/include/laser/ydlidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /champ_bringup/launch/include/velocity_smoother.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /champ_bringup/launch/joints_gui.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /champ_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | champ_bringup 4 | 0.1.0 5 | CHAMP base launch files 6 | 7 | Juan Miguel Jimeno 8 | Juan Miguel Jimeno 9 | 10 | BSD 11 | 12 | catkin 13 | champ_description 14 | champ_base 15 | roslaunch 16 | rosserial_python 17 | robot_localization 18 | rosserial 19 | yocs_velocity_smoother 20 | joint_state_publisher_gui 21 | 22 | roscpp 23 | pluginlib 24 | nodelet 25 | geometry_msgs 26 | nav_msgs 27 | ecl_threads 28 | dynamic_reconfigure 29 | 30 | 31 | -------------------------------------------------------------------------------- /champ_bringup/scripts/joint_calibrator_relay.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ''' 3 | Copyright (c) 2019-2020, Juan Miguel Jimeno 4 | All rights reserved. 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the copyright holder nor the names of its 13 | contributors may be used to endorse or promote products derived 14 | from this software without specific prior written permission. 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 19 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | ''' 26 | 27 | import rospy 28 | from champ_msgs.msg import Joints 29 | from sensor_msgs.msg import JointState 30 | from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint 31 | 32 | import rosparam 33 | import os, sys 34 | 35 | class JointsCalibratorRelay: 36 | def __init__(self): 37 | rospy.Subscriber("joints_calibrator", JointState, self.joints_cmd_callback) 38 | 39 | joint_controller_topic = rospy.get_param('champ_controller/joint_controller_topic') 40 | self.joint_minimal_pub = rospy.Publisher('cmd_joints', Joints, queue_size = 100) 41 | self.joint_trajectory_pub = rospy.Publisher(joint_controller_topic, JointTrajectory, queue_size = 100) 42 | 43 | joints_map = [None,None,None,None] 44 | joints_map[3] = rospy.get_param('/joints_map/left_front') 45 | joints_map[2] = rospy.get_param('/joints_map/right_front') 46 | joints_map[1] = rospy.get_param('/joints_map/left_hind') 47 | joints_map[0] = rospy.get_param('/joints_map/right_hind') 48 | 49 | self.joint_names = [] 50 | for leg in reversed(joints_map): 51 | for joint in leg: 52 | self.joint_names.append(joint) 53 | 54 | def joints_cmd_callback(self, joints): 55 | joint_minimal_msg = Joints() 56 | for i in range(12): 57 | joint_minimal_msg.position.append(joints.position[i]) 58 | 59 | self.joint_minimal_pub.publish(joint_minimal_msg) 60 | 61 | joint_trajectory_msg = JointTrajectory() 62 | joint_trajectory_msg.joint_names = self.joint_names 63 | 64 | point = JointTrajectoryPoint() 65 | point.time_from_start = rospy.Duration(1.0 / 60.0) 66 | point.positions = joint_minimal_msg.position 67 | joint_trajectory_msg.points.append(point) 68 | 69 | self.joint_trajectory_pub.publish(joint_trajectory_msg) 70 | 71 | 72 | if __name__ == "__main__": 73 | rospy.init_node('joints_calibrator_relay', anonymous=True) 74 | j = JointsCalibratorRelay() 75 | rospy.spin() -------------------------------------------------------------------------------- /champ_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(champ_config) 3 | 4 | find_package(catkin REQUIRED COMPONENTS) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY config launch maps worlds 9 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 10 | ) -------------------------------------------------------------------------------- /champ_config/config/gait/gait.yaml: -------------------------------------------------------------------------------- 1 | knee_orientation : ">>" 2 | pantograph_leg : false 3 | odom_scaler: 0.9 4 | max_linear_velocity_x : 0.5 5 | max_linear_velocity_y : 0.25 6 | max_angular_velocity_z : 1.0 7 | com_x_translation: 0.0 8 | swing_height : 0.04 9 | stance_depth : 0.00 10 | stance_duration : 0.25 11 | nominal_height : 0.20 12 | -------------------------------------------------------------------------------- /champ_config/config/joints/joints.yaml: -------------------------------------------------------------------------------- 1 | left_front: 2 | - lf_hip_joint 3 | - lf_upper_leg_joint 4 | - lf_lower_leg_joint 5 | - lf_foot_joint 6 | 7 | right_front: 8 | - rf_hip_joint 9 | - rf_upper_leg_joint 10 | - rf_lower_leg_joint 11 | - rf_foot_joint 12 | 13 | left_hind: 14 | - lh_hip_joint 15 | - lh_upper_leg_joint 16 | - lh_lower_leg_joint 17 | - lh_foot_joint 18 | 19 | right_hind: 20 | - rh_hip_joint 21 | - rh_upper_leg_joint 22 | - rh_lower_leg_joint 23 | - rh_foot_joint 24 | -------------------------------------------------------------------------------- /champ_config/config/links/links.yaml: -------------------------------------------------------------------------------- 1 | base: base_link 2 | left_front: 3 | - lf_hip_link 4 | - lf_upper_leg_link 5 | - lf_lower_leg_link 6 | - lf_foot_link 7 | 8 | right_front: 9 | - rf_hip_link 10 | - rf_upper_leg_link 11 | - rf_lower_leg_link 12 | - rf_foot_link 13 | 14 | left_hind: 15 | - lh_hip_link 16 | - lh_upper_leg_link 17 | - lh_lower_leg_link 18 | - lh_foot_link 19 | 20 | right_hind: 21 | - rh_hip_link 22 | - rh_upper_leg_link 23 | - rh_lower_leg_link 24 | - rh_foot_link -------------------------------------------------------------------------------- /champ_config/config/move_base/base_local_planner_holonomic_params.yaml: -------------------------------------------------------------------------------- 1 | DWAPlannerROS: 2 | #http://wiki.ros.org/dwa_local_planner 3 | 4 | min_vel_trans: 0.01 5 | max_vel_trans: 0.5 6 | 7 | min_vel_x: -0.025 8 | max_vel_x: 0.5 9 | 10 | min_vel_y: 0.0 11 | max_vel_y: 0.0 12 | 13 | max_vel_rot: 1.0 14 | min_vel_rot: -1.0 15 | 16 | acc_lim_trans: 1.7 17 | acc_lim_x: 1.7 18 | acc_lim_y: 0.0 19 | acc_lim_theta: 3 20 | 21 | trans_stopped_vel: 0.1 22 | theta_stopped_vel: 0.1 23 | 24 | xy_goal_tolerance: 0.25 25 | yaw_goal_tolerance: 0.34 26 | 27 | sim_time: 3.5 28 | sim_granularity: 0.1 29 | vx_samples: 20 30 | vy_samples: 0 31 | vth_samples: 40 32 | 33 | path_distance_bias: 34.0 34 | goal_distance_bias: 24.0 35 | occdist_scale: 0.05 36 | 37 | forward_point_distance: 0.2 38 | stop_time_buffer: 0.5 39 | scaling_speed: 0.25 40 | max_scaling_factor: 0.2 41 | 42 | oscillation_reset_dist: 0.05 43 | 44 | use_dwa: true 45 | 46 | prune_plan: false 47 | -------------------------------------------------------------------------------- /champ_config/config/move_base/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | obstacle_range: 2.5 2 | raytrace_range: 3.0 3 | footprint: [[-0.25, -0.145], [-0.25, 0.145], [0.25, 0.145], [0.25, -0.145]] 4 | 5 | transform_tolerance: 0.5 6 | resolution: 0.05 7 | 8 | static_map_layer: 9 | map_topic: map 10 | subscribe_to_updates: true 11 | 12 | 2d_obstacles_layer: 13 | observation_sources: scan 14 | scan: {data_type: LaserScan, 15 | topic: scan, 16 | marking: true, 17 | clearing: true} 18 | 19 | 3d_obstacles_layer: 20 | observation_sources: depth 21 | depth: {data_type: PointCloud2, 22 | topic: camera/depth/points, 23 | min_obstacle_height: 0.1, 24 | marking: true, 25 | clearing: true} 26 | 27 | inflation_layer: 28 | inflation_radius: 2.0 -------------------------------------------------------------------------------- /champ_config/config/move_base/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: map 3 | robot_base_frame: base_footprint 4 | update_frequency: 1.0 5 | publish_frequency: 0.5 6 | static_map: true 7 | cost_scaling_factor: 10.0 8 | 9 | plugins: 10 | - {name: static_map_layer, type: "costmap_2d::StaticLayer"} 11 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 12 | - {name: 2d_obstacles_layer, type: "costmap_2d::ObstacleLayer"} 13 | - {name: 3d_obstacles_layer, type: "costmap_2d::VoxelLayer"} 14 | -------------------------------------------------------------------------------- /champ_config/config/move_base/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 3 | robot_base_frame: base_footprint 4 | update_frequency: 1.0 5 | publish_frequency: 2.0 6 | 7 | static_map: false 8 | rolling_window: true 9 | width: 2.5 10 | height: 2.5 11 | cost_scaling_factor: 5 12 | 13 | plugins: 14 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 15 | - {name: 2d_obstacles_layer, type: "costmap_2d::ObstacleLayer"} 16 | - {name: 3d_obstacles_layer, type: "costmap_2d::VoxelLayer"} 17 | -------------------------------------------------------------------------------- /champ_config/config/move_base/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | base_global_planner: global_planner/GlobalPlanner 2 | base_local_planner: dwa_local_planner/DWAPlannerROS 3 | 4 | shutdown_costmaps: false 5 | 6 | controller_frequency: 5.0 7 | controller_patience: 3.0 8 | 9 | planner_frequency: 0.5 10 | planner_patience: 5.0 11 | 12 | oscillation_timeout: 10.0 13 | oscillation_distance: 0.2 14 | 15 | conservative_reset_dist: 0.1 -------------------------------------------------------------------------------- /champ_config/config/ros_control/ros_control.yaml: -------------------------------------------------------------------------------- 1 | "": 2 | joint_states_controller: 3 | type: joint_state_controller/JointStateController 4 | publish_rate: 200 5 | 6 | joint_group_position_controller: 7 | type: effort_controllers/JointTrajectoryController 8 | joints: 9 | - lf_hip_joint 10 | - lf_upper_leg_joint 11 | - lf_lower_leg_joint 12 | - rf_hip_joint 13 | - rf_upper_leg_joint 14 | - rf_lower_leg_joint 15 | - lh_hip_joint 16 | - lh_upper_leg_joint 17 | - lh_lower_leg_joint 18 | - rh_hip_joint 19 | - rh_upper_leg_joint 20 | - rh_lower_leg_joint 21 | 22 | gains: 23 | lf_hip_joint : {p: 180, d: 0.9, i: 20} 24 | lf_upper_leg_joint : {p: 180, d: 0.9, i: 20} 25 | lf_lower_leg_joint : {p: 180, d: 0.9, i: 20} 26 | rf_hip_joint : {p: 180, d: 0.9, i: 20} 27 | rf_upper_leg_joint : {p: 180, d: 0.9, i: 20} 28 | rf_lower_leg_joint : {p: 180, d: 0.9, i: 20} 29 | lh_hip_joint : {p: 180, d: 0.9, i: 20} 30 | lh_upper_leg_joint : {p: 180, d: 0.9, i: 20} 31 | lh_lower_leg_joint : {p: 180, d: 0.9, i: 20} 32 | rh_hip_joint : {p: 180, d: 0.9, i: 20} 33 | rh_upper_leg_joint : {p: 180, d: 0.9, i: 20} 34 | rh_lower_leg_joint : {p: 180, d: 0.9, i: 20} -------------------------------------------------------------------------------- /champ_config/include/gait_config.h: -------------------------------------------------------------------------------- 1 | #ifndef GAIT_CONFIG_H 2 | #define GAIT_CONFIG_H 3 | 4 | #define KNEE_ORIENTATION ">>" 5 | #define ODOM_SCALER 1.25 6 | #define PANTOGRAPH_LEG false 7 | #define MAX_LINEAR_VELOCITY_X 0.5 8 | #define MAX_LINEAR_VELOCITY_Y 0.25 9 | #define MAX_ANGULAR_VELOCITY_Z 1.0 10 | #define COM_X_TRANSLATION 0.0 11 | #define SWING_HEIGHT 0.04 12 | #define STANCE_DEPTH 0.00 13 | #define STANCE_DURATION 0.25 14 | #define NOMINAL_HEIGHT 0.20 15 | 16 | #endif -------------------------------------------------------------------------------- /champ_config/include/hardware_config.h: -------------------------------------------------------------------------------- 1 | #ifndef HARDWARE_CONFIG_H 2 | #define HARDWARE_CONFIG_H 3 | 4 | #define USE_SIMULATION_ACTUATOR 5 | // #define USE_DYNAMIXEL_ACTUATOR 6 | // #define USE_SERVO_ACTUATOR 7 | // #define USE_BRUSHLESS_ACTUATOR 8 | 9 | #define USE_SIMULATION_IMU 10 | // #define USE_BNO0809DOF_IMU 11 | 12 | #define USE_ROS 13 | // #define USE_ROS_RF 14 | 15 | #ifdef USE_ROS_RF 16 | #define ELE_PIN 16 17 | #define AIL_PIN 21 18 | #define RUD_PIN 17 19 | #define THR_PIN 20 20 | #define AUX1_PIN 22 21 | #define AUX2_PIN 23 22 | #define RF_INV_LX false 23 | #define RF_INV_LY false 24 | #define RF_INV_AZ false 25 | #define RF_INV_ROLL true 26 | #define RF_INV_PITCH false 27 | #define RF_INV_YAW false 28 | #endif 29 | 30 | #ifdef USE_DYNAMIXEL_ACTUATOR 31 | #define LFH_SERVO_ID 16 32 | #define LFU_SERVO_ID 17 33 | #define LFL_SERVO_ID 18 34 | 35 | #define RFH_SERVO_ID 14 36 | #define RFU_SERVO_ID 7 37 | #define RFL_SERVO_ID 4 38 | 39 | #define LHH_SERVO_ID 2 40 | #define LHU_SERVO_ID 11 41 | #define LHL_SERVO_ID 12 42 | 43 | #define RHH_SERVO_ID 6 44 | #define RHU_SERVO_ID 5 45 | #define RHL_SERVO_ID 8 46 | 47 | #define LFH_INV false 48 | #define LFU_INV false 49 | #define LFL_INV true 50 | 51 | #define RFH_INV false 52 | #define RFU_INV true 53 | #define RFL_INV false 54 | 55 | #define LHH_INV false 56 | #define LHU_INV false 57 | #define LHL_INV true 58 | 59 | #define RHH_INV false 60 | #define RHU_INV true 61 | #define RHL_INV false 62 | #endif 63 | 64 | #ifdef USE_SERVO_ACTUATOR 65 | #define LFH_PIN 2 66 | #define LFU_PIN 3 67 | #define LFL_PIN 4 68 | 69 | #define RFH_PIN 23 70 | #define RFU_PIN 22 71 | #define RFL_PIN 21 72 | 73 | #define LHH_PIN 6 74 | #define LHU_PIN 7 75 | #define LHL_PIN 8 76 | 77 | #define RHH_PIN 17 78 | #define RHU_PIN 16 79 | #define RHL_PIN 14 80 | 81 | #define LFH_INV false 82 | #define LFU_INV false 83 | #define LFL_INV true 84 | 85 | #define RFH_INV false 86 | #define RFU_INV true 87 | #define RFL_INV false 88 | 89 | #define LHH_INV false 90 | #define LHU_INV false 91 | #define LHL_INV true 92 | 93 | #define RHH_INV false 94 | #define RHU_INV true 95 | #define RHL_INV false 96 | #endif 97 | 98 | #endif -------------------------------------------------------------------------------- /champ_config/include/quadruped_description.h: -------------------------------------------------------------------------------- 1 | #ifndef QUADRUPED_DESCRIPTION_H 2 | #define QUADRUPED_DESCRIPTION_H 3 | 4 | #include 5 | 6 | namespace champ 7 | { 8 | namespace URDF 9 | { 10 | void loadFromHeader(champ::QuadrupedBase &base) 11 | { 12 | base.lf.hip.setOrigin(0.175, 0.105, 0, 0, 0, 0); 13 | base.lf.upper_leg.setOrigin(0, 0.06, 0, 0, 0, 0); 14 | base.lf.lower_leg.setOrigin(0, 0, -0.141, 0, 0, 0); 15 | base.lf.foot.setOrigin(0, 0, -0.141, 0, 0, 0); 16 | 17 | base.rf.hip.setOrigin(0.175, -0.105, 0, 0, 0, 0); 18 | base.rf.upper_leg.setOrigin(0, -0.06, 0, 0, 0, 0); 19 | base.rf.lower_leg.setOrigin(0, 0, -0.141, 0, 0, 0); 20 | base.rf.foot.setOrigin(0, 0, -0.141, 0, 0, 0); 21 | 22 | base.lh.hip.setOrigin(-0.175, 0.105, 0, 0, 0, 0); 23 | base.lh.upper_leg.setOrigin(0, 0.06, 0, 0, 0, 0); 24 | base.lh.lower_leg.setOrigin(0, 0, -0.141, 0, 0, 0); 25 | base.lh.foot.setOrigin(0, 0, -0.141, 0, 0, 0); 26 | 27 | base.rh.hip.setOrigin(-0.175, -0.105, 0, 0, 0, 0); 28 | base.rh.upper_leg.setOrigin(0, -0.06, 0, 0, 0, 0); 29 | base.rh.lower_leg.setOrigin(0, 0, -0.141, 0, 0, 0); 30 | base.rh.foot.setOrigin(0, 0, -0.141, 0, 0, 0); 31 | } 32 | } 33 | } 34 | #endif -------------------------------------------------------------------------------- /champ_config/launch/bringup.launch: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /champ_config/launch/gazebo.launch: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /champ_config/launch/include/amcl.launch: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /champ_config/launch/include/gmapping.launch: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /champ_config/launch/include/move_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /champ_config/launch/navigate.launch: -------------------------------------------------------------------------------- 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 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /champ_config/launch/slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /champ_config/launch/spawn_robot.launch: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /champ_config/maps/map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chvmp/champ/7f7d91724dc9cae43d5c3868821f4f8b79877d1a/champ_config/maps/map.pgm -------------------------------------------------------------------------------- /champ_config/maps/map.yaml: -------------------------------------------------------------------------------- 1 | image: map.pgm 2 | resolution: 0.050000 3 | origin: [-50.000000, -50.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /champ_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | champ_config 4 | 0.1.0 5 | CHAMP Config Package 6 | 7 | Juan Miguel Jimeno 8 | Juan Miguel Jimeno 9 | 10 | BSD 11 | 12 | catkin 13 | roslaunch 14 | rviz 15 | champ_base 16 | 17 | 18 | -------------------------------------------------------------------------------- /champ_config/setup.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | ROBOT_CONFIG_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd) 4 | export CHAMP_ROBOT_CONFIG_DIR=$ROBOT_CONFIG_DIR/include 5 | 6 | bash -i -------------------------------------------------------------------------------- /champ_config/worlds/default.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.1 0.1 0.1 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.5 -1 15 | 16 | 17 | 1 18 | 19 | 20 | 21 | 22 | 0 0 1 23 | 100 100 24 | 25 | 26 | 27 | 28 | 29 | 100 30 | 50 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 10 42 | 43 | 44 | 0 45 | 46 | 47 | 0 0 1 48 | 100 100 49 | 50 | 51 | 52 | 56 | 57 | 58 | 0 59 | 0 60 | 1 61 | 62 | 63 | 0 0 -9.8 64 | 6e-06 2.3e-05 -4.2e-05 65 | 66 | 67 | 0.001 68 | 1 69 | 1000 70 | 71 | 72 | 0.4 0.4 0.4 1 73 | 0.7 0.7 0.7 1 74 | 1 75 | 76 | 77 | EARTH_WGS84 78 | 0 79 | 0 80 | 0 81 | 0 82 | 83 | 84 | 0 85 | 0 86 | 0 87 | 22823 88 | 89 | 0 0 0 0 -0 0 90 | 1 1 1 91 | 92 | 0 0 0 0 -0 0 93 | 0 0 0 0 -0 0 94 | 0 0 0 0 -0 0 95 | 0 0 0 0 -0 0 96 | 97 | 98 | 99 | 0 0 10 0 -0 0 100 | 101 | 102 | 103 | 104 | 4.60123 -4.94126 1.71211 0 0.275643 2.35619 105 | orbit 106 | perspective 107 | 108 | 109 | 110 | 111 | -------------------------------------------------------------------------------- /champ_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | cmake_minimum_required(VERSION 2.8.3) 3 | project(champ_description) 4 | 5 | find_package(catkin REQUIRED COMPONENTS) 6 | 7 | catkin_package() 8 | 9 | install(DIRECTORY meshes launch urdf rviz 10 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 11 | ) 12 | -------------------------------------------------------------------------------- /champ_description/launch/description.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /champ_description/launch/view_urdf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | [champ/joint_states] 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /champ_description/meshes/arm/base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chvmp/champ/7f7d91724dc9cae43d5c3868821f4f8b79877d1a/champ_description/meshes/arm/base.stl -------------------------------------------------------------------------------- /champ_description/meshes/arm/lower_arm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chvmp/champ/7f7d91724dc9cae43d5c3868821f4f8b79877d1a/champ_description/meshes/arm/lower_arm.stl -------------------------------------------------------------------------------- /champ_description/meshes/arm/upper_arm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chvmp/champ/7f7d91724dc9cae43d5c3868821f4f8b79877d1a/champ_description/meshes/arm/upper_arm.stl -------------------------------------------------------------------------------- /champ_description/meshes/arm/wrist1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chvmp/champ/7f7d91724dc9cae43d5c3868821f4f8b79877d1a/champ_description/meshes/arm/wrist1.stl -------------------------------------------------------------------------------- /champ_description/meshes/arm/wrist2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chvmp/champ/7f7d91724dc9cae43d5c3868821f4f8b79877d1a/champ_description/meshes/arm/wrist2.stl -------------------------------------------------------------------------------- /champ_description/meshes/base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chvmp/champ/7f7d91724dc9cae43d5c3868821f4f8b79877d1a/champ_description/meshes/base.stl -------------------------------------------------------------------------------- /champ_description/meshes/hip.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chvmp/champ/7f7d91724dc9cae43d5c3868821f4f8b79877d1a/champ_description/meshes/hip.stl -------------------------------------------------------------------------------- /champ_description/meshes/lower_leg.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chvmp/champ/7f7d91724dc9cae43d5c3868821f4f8b79877d1a/champ_description/meshes/lower_leg.stl -------------------------------------------------------------------------------- /champ_description/meshes/upper_leg.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chvmp/champ/7f7d91724dc9cae43d5c3868821f4f8b79877d1a/champ_description/meshes/upper_leg.stl -------------------------------------------------------------------------------- /champ_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | champ_description 4 | 0.1.0 5 | CHAMP Description Package 6 | 7 | Juan Miguel Jimeno 8 | Juan Miguel Jimeno 9 | 10 | BSD 11 | 12 | catkin 13 | roslaunch 14 | robot_state_publisher 15 | joint_state_publisher 16 | urdf 17 | xacro 18 | rviz 19 | gazebo_plugins 20 | hector_gazebo_plugins 21 | hector_sensors_description 22 | velodyne_gazebo_plugins 23 | 24 | -------------------------------------------------------------------------------- /champ_description/rviz/urdf_viewer.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Grid1 8 | - /TF1/Frames1 9 | Splitter Ratio: 0.38333332538604736 10 | Tree Height: 845 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.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 1000 53 | Reference Frame: 54 | Value: true 55 | - Alpha: 1 56 | Class: rviz/RobotModel 57 | Collision Enabled: false 58 | Enabled: true 59 | Links: 60 | All Links Enabled: true 61 | Expand Joint Details: false 62 | Expand Link Details: false 63 | Expand Tree: false 64 | Link Tree Style: Links in Alphabetic Order 65 | base_inertia: 66 | Alpha: 1 67 | Show Axes: false 68 | Show Trail: false 69 | base_link: 70 | Alpha: 1 71 | Show Axes: false 72 | Show Trail: false 73 | Value: true 74 | camera_depth_frame: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | camera_depth_optical_frame: 79 | Alpha: 1 80 | Show Axes: false 81 | Show Trail: false 82 | camera_link: 83 | Alpha: 1 84 | Show Axes: false 85 | Show Trail: false 86 | Value: true 87 | camera_rgb_frame: 88 | Alpha: 1 89 | Show Axes: false 90 | Show Trail: false 91 | camera_rgb_optical_frame: 92 | Alpha: 1 93 | Show Axes: false 94 | Show Trail: false 95 | hokuyo_frame: 96 | Alpha: 1 97 | Show Axes: false 98 | Show Trail: false 99 | Value: true 100 | imu_link: 101 | Alpha: 1 102 | Show Axes: false 103 | Show Trail: false 104 | lf_foot_link: 105 | Alpha: 1 106 | Show Axes: false 107 | Show Trail: false 108 | lf_hip_debug_link: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | lf_hip_link: 113 | Alpha: 1 114 | Show Axes: false 115 | Show Trail: false 116 | Value: true 117 | lf_lower_leg_link: 118 | Alpha: 1 119 | Show Axes: false 120 | Show Trail: false 121 | Value: true 122 | lf_upper_leg_link: 123 | Alpha: 1 124 | Show Axes: false 125 | Show Trail: false 126 | Value: true 127 | lh_foot_link: 128 | Alpha: 1 129 | Show Axes: false 130 | Show Trail: false 131 | lh_hip_debug_link: 132 | Alpha: 1 133 | Show Axes: false 134 | Show Trail: false 135 | lh_hip_link: 136 | Alpha: 1 137 | Show Axes: false 138 | Show Trail: false 139 | Value: true 140 | lh_lower_leg_link: 141 | Alpha: 1 142 | Show Axes: false 143 | Show Trail: false 144 | Value: true 145 | lh_upper_leg_link: 146 | Alpha: 1 147 | Show Axes: false 148 | Show Trail: false 149 | Value: true 150 | rf_foot_link: 151 | Alpha: 1 152 | Show Axes: false 153 | Show Trail: false 154 | rf_hip_debug_link: 155 | Alpha: 1 156 | Show Axes: false 157 | Show Trail: false 158 | rf_hip_link: 159 | Alpha: 1 160 | Show Axes: false 161 | Show Trail: false 162 | Value: true 163 | rf_lower_leg_link: 164 | Alpha: 1 165 | Show Axes: false 166 | Show Trail: false 167 | Value: true 168 | rf_upper_leg_link: 169 | Alpha: 1 170 | Show Axes: false 171 | Show Trail: false 172 | Value: true 173 | rh_foot_link: 174 | Alpha: 1 175 | Show Axes: false 176 | Show Trail: false 177 | rh_hip_debug_link: 178 | Alpha: 1 179 | Show Axes: false 180 | Show Trail: false 181 | rh_hip_link: 182 | Alpha: 1 183 | Show Axes: false 184 | Show Trail: false 185 | Value: true 186 | rh_lower_leg_link: 187 | Alpha: 1 188 | Show Axes: false 189 | Show Trail: false 190 | Value: true 191 | rh_upper_leg_link: 192 | Alpha: 1 193 | Show Axes: false 194 | Show Trail: false 195 | Value: true 196 | Name: RobotModel 197 | Robot Description: robot_description 198 | TF Prefix: "" 199 | Update Interval: 0 200 | Value: true 201 | Visual Enabled: true 202 | - Class: rviz/Axes 203 | Enabled: true 204 | Length: 1 205 | Name: Axes 206 | Radius: 0.005 207 | Reference Frame: 208 | Value: true 209 | - Class: rviz/TF 210 | Enabled: true 211 | Frame Timeout: 15 212 | Frames: 213 | All Enabled: false 214 | base_footprint: 215 | Value: true 216 | base_inertia: 217 | Value: true 218 | base_link: 219 | Value: true 220 | camera_depth_frame: 221 | Value: true 222 | camera_depth_optical_frame: 223 | Value: true 224 | camera_link: 225 | Value: true 226 | camera_rgb_frame: 227 | Value: true 228 | camera_rgb_optical_frame: 229 | Value: true 230 | hokuyo_frame: 231 | Value: true 232 | imu_link: 233 | Value: true 234 | lf_foot_link: 235 | Value: true 236 | lf_hip_debug_link: 237 | Value: true 238 | lf_hip_link: 239 | Value: true 240 | lf_lower_leg_link: 241 | Value: true 242 | lf_upper_leg_link: 243 | Value: true 244 | lh_foot_link: 245 | Value: true 246 | lh_hip_debug_link: 247 | Value: true 248 | lh_hip_link: 249 | Value: true 250 | lh_lower_leg_link: 251 | Value: true 252 | lh_upper_leg_link: 253 | Value: true 254 | odom: 255 | Value: true 256 | rf_foot_link: 257 | Value: true 258 | rf_hip_debug_link: 259 | Value: true 260 | rf_hip_link: 261 | Value: true 262 | rf_lower_leg_link: 263 | Value: true 264 | rf_upper_leg_link: 265 | Value: true 266 | rh_foot_link: 267 | Value: true 268 | rh_hip_debug_link: 269 | Value: true 270 | rh_hip_link: 271 | Value: true 272 | rh_lower_leg_link: 273 | Value: true 274 | rh_upper_leg_link: 275 | Value: true 276 | Marker Scale: 0.25 277 | Name: TF 278 | Show Arrows: true 279 | Show Axes: false 280 | Show Names: false 281 | Tree: 282 | odom: 283 | base_footprint: 284 | base_link: 285 | base_inertia: 286 | {} 287 | camera_link: 288 | camera_depth_frame: 289 | camera_depth_optical_frame: 290 | {} 291 | camera_rgb_frame: 292 | camera_rgb_optical_frame: 293 | {} 294 | hokuyo_frame: 295 | {} 296 | imu_link: 297 | {} 298 | lf_hip_debug_link: 299 | {} 300 | lf_hip_link: 301 | lf_upper_leg_link: 302 | lf_lower_leg_link: 303 | lf_foot_link: 304 | {} 305 | lh_hip_debug_link: 306 | {} 307 | lh_hip_link: 308 | lh_upper_leg_link: 309 | lh_lower_leg_link: 310 | lh_foot_link: 311 | {} 312 | rf_hip_debug_link: 313 | {} 314 | rf_hip_link: 315 | rf_upper_leg_link: 316 | rf_lower_leg_link: 317 | rf_foot_link: 318 | {} 319 | rh_hip_debug_link: 320 | {} 321 | rh_hip_link: 322 | rh_upper_leg_link: 323 | rh_lower_leg_link: 324 | rh_foot_link: 325 | {} 326 | Update Interval: 0 327 | Value: true 328 | - Class: rviz/MarkerArray 329 | Enabled: false 330 | Marker Topic: foot 331 | Name: MarkerArray 332 | Namespaces: 333 | {} 334 | Queue Size: 100 335 | Value: false 336 | - Alpha: 1 337 | Axes Length: 1 338 | Axes Radius: 0.10000000149011612 339 | Class: rviz/Pose 340 | Color: 255; 25; 0 341 | Enabled: true 342 | Head Length: 0.30000001192092896 343 | Head Radius: 0.10000000149011612 344 | Name: Pose 345 | Shaft Length: 1 346 | Shaft Radius: 0.05000000074505806 347 | Shape: Arrow 348 | Topic: /pose_debug 349 | Unreliable: false 350 | Value: true 351 | Enabled: true 352 | Global Options: 353 | Background Color: 238; 238; 236 354 | Default Light: true 355 | Fixed Frame: /odom 356 | Frame Rate: 30 357 | Name: root 358 | Tools: 359 | - Class: rviz/Interact 360 | Hide Inactive Objects: true 361 | - Class: rviz/MoveCamera 362 | - Class: rviz/Select 363 | - Class: rviz/FocusCamera 364 | - Class: rviz/Measure 365 | - Class: rviz/SetInitialPose 366 | Theta std deviation: 0.2617993950843811 367 | Topic: /initialpose 368 | X std deviation: 0.5 369 | Y std deviation: 0.5 370 | - Class: rviz/SetGoal 371 | Topic: /move_base_simple/goal 372 | - Class: rviz/PublishPoint 373 | Single click: true 374 | Topic: /clicked_point 375 | Value: true 376 | Views: 377 | Current: 378 | Class: rviz/Orbit 379 | Distance: 4.856288433074951 380 | Enable Stereo Rendering: 381 | Stereo Eye Separation: 0.05999999865889549 382 | Stereo Focal Distance: 1 383 | Swap Stereo Eyes: false 384 | Value: false 385 | Focal Point: 386 | X: 0.27266740798950195 387 | Y: -0.10151305049657822 388 | Z: 0.08205118030309677 389 | Focal Shape Fixed Size: false 390 | Focal Shape Size: 0.05000000074505806 391 | Invert Z Axis: false 392 | Name: Current View 393 | Near Clip Distance: 0.009999999776482582 394 | Pitch: 0.29539817571640015 395 | Target Frame: base_footprint 396 | Value: Orbit (rviz) 397 | Yaw: 2.140366792678833 398 | Saved: ~ 399 | Window Geometry: 400 | Displays: 401 | collapsed: true 402 | Height: 1143 403 | Hide Left Dock: true 404 | Hide Right Dock: true 405 | QMainWindow State: 000000ff00000000fd00000004000000000000021e000003d8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000003d8000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003d8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003d8000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003ab0000003ffc0100000002fb0000000800540069006d00650100000000000003ab000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003ab000003d800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 406 | Selection: 407 | collapsed: false 408 | Time: 409 | collapsed: false 410 | Tool Properties: 411 | collapsed: false 412 | Views: 413 | collapsed: true 414 | Width: 939 415 | X: 981 416 | Y: 27 417 | -------------------------------------------------------------------------------- /champ_description/urdf/accessories.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 16 | 17 | 18 | 19 | 22 | 23 | 24 | 25 | 26 | 27 | 50.0 28 | imu_link 29 | imu/data 30 | 0.005 0.005 0.005 31 | 0.005 0.005 0.005 32 | 0.005 0.005 0.005 33 | 0.005 0.005 0.005 34 | 0.005 35 | 0.005 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /champ_description/urdf/champ.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 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | Gazebo/FlatBlack 39 | 40 | 41 | 42 | 43 | 44 | 45 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | true 59 | 10.0 60 | base_link 61 | odom/ground_truth 62 | 0.01 63 | world 64 | 0 0 0 65 | 0 0 0 66 | 67 | 68 | 69 | 70 | 71 | true 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /champ_description/urdf/gen_urdf: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | set -e 4 | rosrun xacro xacro champ.urdf.xacro > champ.urdf 5 | 6 | -------------------------------------------------------------------------------- /champ_description/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 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | Gazebo/FlatBlack 50 | 51 | 52 | 53 | 54 | 55 | 56 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | Gazebo/FlatBlack 76 | 77 | 78 | 79 | 80 | 81 | 82 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 1000000.0 102 | 1.0 103 | 0.8 104 | 0.8 105 | 0.0 106 | 0.001 107 | Gazebo/FlatBlack 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | transmission_interface/SimpleTransmission 151 | 152 | hardware_interface/EffortJointInterface 153 | 154 | 155 | hardware_interface/EffortJointInterface 156 | 1 157 | 158 | 159 | 160 | 161 | transmission_interface/SimpleTransmission 162 | 163 | hardware_interface/EffortJointInterface 164 | 165 | 166 | hardware_interface/EffortJointInterface 167 | 1 168 | 169 | 170 | 171 | 172 | transmission_interface/SimpleTransmission 173 | 174 | hardware_interface/EffortJointInterface 175 | 176 | 177 | hardware_interface/EffortJointInterface 178 | 1 179 | 180 | 181 | 182 | -------------------------------------------------------------------------------- /champ_description/urdf/properties.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 | -------------------------------------------------------------------------------- /champ_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | cmake_minimum_required(VERSION 2.8.3) 3 | project(champ_gazebo) 4 | 5 | add_compile_options(-std=c++17) 6 | 7 | find_package(gazebo REQUIRED) 8 | 9 | find_package(catkin 10 | REQUIRED 11 | COMPONENTS 12 | roscpp 13 | urdf 14 | champ 15 | champ_msgs 16 | ) 17 | 18 | catkin_package( 19 | CATKIN_DEPENDS 20 | roscpp 21 | ) 22 | 23 | install(PROGRAMS 24 | scripts/imu_sensor.py 25 | scripts/odometry_tf.py 26 | scripts/odometry.py 27 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 28 | ) 29 | 30 | include_directories( 31 | include 32 | ${champ_INCLUDE_DIRS} 33 | ${champ_INCLUDE_DIRS}/champ/ 34 | ${champ_INCLUDE_DIRS}/champ/champ/ 35 | ${catkin_INCLUDE_DIRS} 36 | ${GAZEBO_INCLUDE_DIRS} 37 | ) 38 | 39 | link_directories(${GAZEBO_LIBRARY_DIRS} ${catkin_LIBRARIES}) 40 | add_compile_options("${GAZEBO_CXX_FLAGS}") 41 | 42 | add_executable(contact_sensor src/contact_sensor.cpp) 43 | target_link_libraries(contact_sensor ${GAZEBO_LIBRARIES} pthread ${catkin_LIBRARIES} ) 44 | add_dependencies(contact_sensor ${catkin_EXPORTED_TARGETS}) 45 | 46 | install(TARGETS 47 | contact_sensor 48 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 49 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 50 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 51 | ) 52 | 53 | install(DIRECTORY launch config 54 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 55 | ) 56 | 57 | install(DIRECTORY worlds/ 58 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/worlds 59 | ) 60 | -------------------------------------------------------------------------------- /champ_gazebo/config/ros_control.yaml: -------------------------------------------------------------------------------- 1 | "": 2 | joint_states_controller: 3 | type: joint_state_controller/JointStateController 4 | publish_rate: 25 5 | 6 | joint_group_position_controller: 7 | type: effort_controllers/JointTrajectoryController 8 | joints: 9 | - lf_hip_joint 10 | - lf_upper_leg_joint 11 | - lf_lower_leg_joint 12 | - rf_hip_joint 13 | - rf_upper_leg_joint 14 | - rf_lower_leg_joint 15 | - lh_hip_joint 16 | - lh_upper_leg_joint 17 | - lh_lower_leg_joint 18 | - rh_hip_joint 19 | - rh_upper_leg_joint 20 | - rh_lower_leg_joint 21 | 22 | gains: 23 | lf_hip_joint : {p: 180, d: 0.9, i: 20} 24 | lf_upper_leg_joint : {p: 180, d: 0.9, i: 20} 25 | lf_lower_leg_joint : {p: 180, d: 0.9, i: 20} 26 | rf_hip_joint : {p: 180, d: 0.9, i: 20} 27 | rf_upper_leg_joint : {p: 180, d: 0.9, i: 20} 28 | rf_lower_leg_joint : {p: 180, d: 0.9, i: 20} 29 | lh_hip_joint : {p: 180, d: 0.9, i: 20} 30 | lh_upper_leg_joint : {p: 180, d: 0.9, i: 20} 31 | lh_lower_leg_joint : {p: 180, d: 0.9, i: 20} 32 | rh_hip_joint : {p: 180, d: 0.9, i: 20} 33 | rh_upper_leg_joint : {p: 180, d: 0.9, i: 20} 34 | rh_lower_leg_joint : {p: 180, d: 0.9, i: 20} -------------------------------------------------------------------------------- /champ_gazebo/launch/gazebo.launch: -------------------------------------------------------------------------------- 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 | 30 | 31 | 32 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /champ_gazebo/launch/spawn_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 14 | 15 | 16 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /champ_gazebo/launch/spawn_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /champ_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | champ_gazebo 4 | 0.1.0 5 | CHAMP Gazebo Package 6 | 7 | Juan Miguel Jimeno 8 | Juan Miguel Jimeno 9 | 10 | BSD 11 | 12 | catkin 13 | roscpp 14 | champ 15 | champ_msgs 16 | roslaunch 17 | gazebo_ros 18 | gazebo_ros_pkgs 19 | gazebo_ros_control 20 | gazebo_plugins 21 | ros_controllers 22 | 23 | -------------------------------------------------------------------------------- /champ_gazebo/scripts/imu_sensor.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | ''' 3 | Copyright (c) 2019-2020, Juan Miguel Jimeno 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the copyright holder nor the names of its 14 | contributors may be used to endorse or promote products derived 15 | from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | ''' 28 | 29 | import rospy 30 | from nav_msgs.msg import Odometry 31 | from geometry_msgs.msg import Point, Quaternion 32 | from champ_msgs.msg import Pose 33 | import tf 34 | 35 | class SimPose: 36 | def __init__(self): 37 | rospy.Subscriber("odom/ground_truth", Odometry, self.odometry_callback) 38 | self.sim_pose_publisher = rospy.Publisher("/champ/gazebo/pose", Pose, queue_size=50) 39 | 40 | def odometry_callback(self, data): 41 | sim_pose_msg = Pose() 42 | sim_pose_msg.roll = data.pose.pose.orientation.x 43 | sim_pose_msg.pitch = data.pose.pose.orientation.y 44 | sim_pose_msg.yaw = data.pose.pose.orientation.z 45 | 46 | self.sim_pose_publisher.publish(sim_pose_msg) 47 | 48 | if __name__ == "__main__": 49 | rospy.init_node("champ_gazebo_sim_pose", anonymous = True) 50 | odom = SimPose() 51 | rospy.spin() -------------------------------------------------------------------------------- /champ_gazebo/scripts/odometry.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | ''' 3 | Copyright (c) 2019-2020, Juan Miguel Jimeno 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the copyright holder nor the names of its 14 | contributors may be used to endorse or promote products derived 15 | from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | ''' 28 | 29 | import rospy 30 | from champ_msgs.msg import Contacts 31 | from nav_msgs.msg import Odometry 32 | from geometry_msgs.msg import Point, Quaternion, Pose, Twist, Vector3 33 | import tf 34 | from tf import TransformListener 35 | from tf.transformations import euler_from_quaternion, quaternion_from_euler 36 | from nav_msgs.msg import Odometry 37 | 38 | import math 39 | 40 | class ChampOdometry: 41 | def __init__(self): 42 | rospy.Subscriber("/champ/gazebo/contacts", Contacts, self.contacts_callback) 43 | self.odom_publisher = rospy.Publisher("odom/raw", Odometry, queue_size=50) 44 | 45 | self.odom_broadcaster = tf.TransformBroadcaster() 46 | self.tf = TransformListener() 47 | 48 | self.foot_links = [ 49 | "lf_foot_link", 50 | "rf_foot_link", 51 | "lh_foot_link", 52 | "rh_foot_link" 53 | ] 54 | 55 | self.nominal_foot_positions = [[0,0],[0,0],[0,0],[0,0]] 56 | self.prev_foot_positions = [[0,0],[0,0],[0,0],[0,0]] 57 | self.prev_theta = [0,0,0,0] 58 | self.prev_stance_angles = [0,0,0,0] 59 | self.prev_time = 0 60 | 61 | self.pos_x = 0 62 | self.pos_y = 0 63 | self.theta = 0 64 | 65 | self.publish_odom_tf(0,0,0,0) 66 | rospy.sleep(1) 67 | 68 | for i in range(4): 69 | self.nominal_foot_positions[i] = self.get_foot_position(i) 70 | self.prev_foot_positions[i] = self.nominal_foot_positions[i] 71 | self.prev_theta[i] = math.atan2(self.prev_foot_positions[i][1], self.prev_foot_positions[i][0]) 72 | 73 | def publish_odom(self, x, y, z, theta, vx, vy, vth): 74 | current_time = rospy.Time.now() 75 | odom = Odometry() 76 | odom.header.stamp = current_time 77 | odom.header.frame_id = "odom" 78 | 79 | odom_quat = tf.transformations.quaternion_from_euler(0, 0, theta) 80 | odom.pose.pose = Pose(Point(x, y, z), Quaternion(*odom_quat)) 81 | 82 | odom.child_frame_id = "base_footprint" 83 | odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth)) 84 | 85 | # publish the message 86 | self.odom_publisher.publish(odom) 87 | 88 | def publish_odom_tf(self, x, y, z, theta): 89 | current_time = rospy.Time.now() 90 | 91 | odom_quat = quaternion_from_euler (0, 0, theta) 92 | 93 | self.odom_broadcaster.sendTransform( 94 | (x, y, z), 95 | odom_quat, 96 | current_time, 97 | "base_footprint", 98 | "odom" 99 | ) 100 | 101 | def get_foot_position(self, leg_id): 102 | if self.tf.frameExists("base_link" and self.foot_links[leg_id]) : 103 | t = self.tf.getLatestCommonTime("base_link", self.foot_links[leg_id]) 104 | position, quaternion = self.tf.lookupTransform("base_link", self.foot_links[leg_id], t) 105 | return position 106 | else: 107 | return 0, 0, 0 108 | 109 | def contacts_callback(self, data): 110 | self.leg_contact_states = data.contacts 111 | 112 | def is_almost_equal(self, a, b, max_rel_diff): 113 | diff = abs(a - b) 114 | a = abs(a) 115 | b = abs(b) 116 | largest = 0 117 | if b > a: 118 | largest = b 119 | else: 120 | larget = a 121 | 122 | if diff <= (largest * max_rel_diff): 123 | return True 124 | 125 | return False 126 | 127 | def run(self): 128 | while not rospy.is_shutdown(): 129 | leg_contact_states = self.leg_contact_states 130 | current_foot_position = [[0,0],[0,0],[0,0],[0,0]] 131 | stance_angles = [0, 0, 0, 0] 132 | current_theta = [0, 0, 0, 0] 133 | delta_theta = 0 134 | in_xy = False 135 | total_contact = sum(leg_contact_states) 136 | x_sum = 0 137 | y_sum = 0 138 | theta_sum = 0 139 | 140 | for i in range(4): 141 | current_foot_position[i] = self.get_foot_position(i) 142 | 143 | for i in range(4): 144 | current_theta[i] = math.atan2(current_foot_position[i][1], current_foot_position[i][0]) 145 | from_nominal_x = self.nominal_foot_positions[i][0] - current_foot_position[i][0] 146 | from_nominal_y = self.nominal_foot_positions[i][1] - current_foot_position[i][1] 147 | stance_angles[i] = math.atan2(from_nominal_y, from_nominal_x) 148 | # print stance_angles 149 | #check if it's moving in the x or y axis 150 | if self.is_almost_equal(stance_angles[i], abs(1.5708), 0.001) or self.is_almost_equal(stance_angles[i], abs(3.1416), 0.001): 151 | in_xy = True 152 | 153 | if current_foot_position[i] != None and leg_contact_states[i] == True and total_contact == 2: 154 | delta_x = (self.prev_foot_positions[i][0] - current_foot_position[i][0]) / 2 155 | delta_y = (self.prev_foot_positions[i][1] - current_foot_position[i][1]) / 2 156 | 157 | x = delta_x * math.cos(self.theta) - delta_y * math.sin(self.theta) 158 | y = delta_x * math.sin(self.theta) + delta_y * math.cos(self.theta) 159 | 160 | x_sum += delta_x 161 | y_sum += delta_y 162 | 163 | self.pos_x += x 164 | self.pos_y += y 165 | 166 | if not in_xy: 167 | delta_theta = self.prev_theta[i] - current_theta[i] 168 | theta_sum += delta_theta 169 | self.theta += delta_theta / 2 170 | 171 | now = rospy.Time.now().to_sec() 172 | dt = now - self.prev_time 173 | 174 | vx = x_sum / dt 175 | vy = y_sum / dt 176 | vth = theta_sum / dt 177 | 178 | self.publish_odom(self.pos_x, self.pos_y, 0, self.theta, vx, vy, vth) 179 | 180 | # self.publish_odom_tf(self.pos_x, self.pos_y, 0, self.theta) 181 | 182 | self.prev_foot_positions = current_foot_position 183 | self.prev_theta = current_theta 184 | self.prev_stance_angles = stance_angles 185 | 186 | self.prev_time = now 187 | rospy.sleep(0.01) 188 | 189 | if __name__ == "__main__": 190 | rospy.init_node("champ_gazebo_odometry", anonymous = True) 191 | odom = ChampOdometry() 192 | odom.run() 193 | -------------------------------------------------------------------------------- /champ_gazebo/scripts/odometry_tf.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | ''' 3 | Copyright (c) 2019-2020, Juan Miguel Jimeno 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 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the copyright holder nor the names of its 14 | contributors may be used to endorse or promote products derived 15 | from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | ''' 28 | 29 | import rospy 30 | from nav_msgs.msg import Odometry 31 | from geometry_msgs.msg import Point, Quaternion 32 | import tf 33 | 34 | class Odom: 35 | def __init__(self): 36 | rospy.Subscriber("odom/raw", Odometry, self.odometry_callback) 37 | self.odom_broadcaster = tf.TransformBroadcaster() 38 | 39 | def odometry_callback(self, data): 40 | self.current_linear_speed_x = data.twist.twist.linear.x 41 | self.current_angular_speed_z = data.twist.twist.angular.z 42 | 43 | current_time = rospy.Time.now() 44 | 45 | odom_quat = ( 46 | data.pose.pose.orientation.x, 47 | data.pose.pose.orientation.y, 48 | data.pose.pose.orientation.z, 49 | data.pose.pose.orientation.w 50 | ) 51 | 52 | self.odom_broadcaster.sendTransform( 53 | (data.pose.pose.position.x, data.pose.pose.position.y, 0), 54 | odom_quat, 55 | current_time, 56 | "base_footprint", 57 | "odom" 58 | ) 59 | 60 | if __name__ == "__main__": 61 | rospy.init_node("champ_gazebo_odometry_transform", anonymous = True) 62 | odom = Odom() 63 | rospy.spin() -------------------------------------------------------------------------------- /champ_gazebo/src/contact_sensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2012 Open Source Robotics Foundation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | #include "ros/ros.h" 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | class ContactSensor 27 | { 28 | bool foot_contacts_[4]; 29 | std::vector foot_links_; 30 | ros::Publisher contacts_publisher_; 31 | gazebo::transport::SubscriberPtr gazebo_sub; 32 | 33 | public: 34 | ContactSensor(ros::NodeHandle *nh): 35 | foot_contacts_ {false,false,false,false} 36 | { 37 | std::vector joint_names; 38 | 39 | joint_names = champ::URDF::getLinkNames(nh); 40 | 41 | foot_links_.push_back(joint_names[2]); 42 | foot_links_.push_back(joint_names[6]); 43 | foot_links_.push_back(joint_names[10]); 44 | foot_links_.push_back(joint_names[14]); 45 | 46 | contacts_publisher_ = nh->advertise("foot_contacts", 10); 47 | 48 | gazebo::client::setup(); 49 | gazebo::transport::NodePtr node(new gazebo::transport::Node()); 50 | node->Init(); 51 | gazebo_sub = node->Subscribe("~/physics/contacts", &ContactSensor::gazeboCallback_, this); 52 | } 53 | 54 | void gazeboCallback_(ConstContactsPtr &_msg) 55 | { 56 | for(size_t i = 0; i < 4; i++) 57 | { 58 | foot_contacts_[i] = false; 59 | } 60 | 61 | for (int i = 0; i < _msg->contact_size(); ++i) 62 | { 63 | std::vector results; 64 | std::string collision = _msg->contact(i).collision1(); 65 | boost::split(results, collision, [](char c){return c == ':';}); 66 | 67 | for(size_t j = 0; j < 4; j++) 68 | { 69 | if(foot_links_[j] == results[2]) 70 | { 71 | foot_contacts_[j] = true; 72 | break; 73 | } 74 | } 75 | } 76 | } 77 | 78 | void publishContacts() 79 | { 80 | champ_msgs::ContactsStamped contacts_msg; 81 | contacts_msg.header.stamp = ros::Time::now(); 82 | contacts_msg.contacts.resize(4); 83 | 84 | for(size_t i = 0; i < 4; i++) 85 | { 86 | contacts_msg.contacts[i] = foot_contacts_[i]; 87 | } 88 | 89 | contacts_publisher_.publish(contacts_msg); 90 | } 91 | }; 92 | 93 | void exitHandler(int sig) 94 | { 95 | gazebo::client::shutdown(); 96 | ros::shutdown(); 97 | } 98 | 99 | int main(int _argc, char **_argv) 100 | { 101 | ros::init(_argc, _argv, "contact_sensor"); 102 | ros::NodeHandle nh(""); 103 | 104 | signal(SIGINT, exitHandler); 105 | 106 | ContactSensor sensor(&nh); 107 | 108 | ros::Rate contact_publish_rate(50); 109 | while (ros::ok()) 110 | { 111 | sensor.publishContacts(); 112 | ros::spinOnce(); 113 | 114 | contact_publish_rate.sleep(); 115 | } 116 | return 0; 117 | } -------------------------------------------------------------------------------- /champ_gazebo/worlds/default.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.1 0.1 0.1 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.5 -1 15 | 16 | 17 | 1 18 | 19 | 20 | 21 | 22 | 0 0 1 23 | 100 100 24 | 25 | 26 | 27 | 28 | 29 | 100 30 | 50 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 10 42 | 43 | 44 | 0 45 | 46 | 47 | 0 0 1 48 | 100 100 49 | 50 | 51 | 52 | 56 | 57 | 58 | 0 59 | 0 60 | 1 61 | 62 | 63 | 0 0 -9.8 64 | 6e-06 2.3e-05 -4.2e-05 65 | 66 | 67 | 0.001 68 | 1 69 | 1000 70 | 71 | 72 | 0.4 0.4 0.4 1 73 | 0.7 0.7 0.7 1 74 | 1 75 | 76 | 77 | EARTH_WGS84 78 | 0 79 | 0 80 | 0 81 | 0 82 | 83 | 84 | 0 85 | 0 86 | 0 87 | 22823 88 | 89 | 0 0 0 0 -0 0 90 | 1 1 1 91 | 92 | 0 0 0 0 -0 0 93 | 0 0 0 0 -0 0 94 | 0 0 0 0 -0 0 95 | 0 0 0 0 -0 0 96 | 97 | 98 | 99 | 0 0 10 0 -0 0 100 | 101 | 102 | 103 | 104 | 4.60123 -4.94126 1.71211 0 0.275643 2.35619 105 | orbit 106 | perspective 107 | 108 | 109 | 110 | 111 | -------------------------------------------------------------------------------- /champ_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(champ_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS message_generation message_runtime std_msgs geometry_msgs) 5 | 6 | add_message_files( 7 | DIRECTORY msg 8 | FILES 9 | Velocities.msg 10 | PID.msg 11 | Imu.msg 12 | Point.msg 13 | PointArray.msg 14 | Joints.msg 15 | Contacts.msg 16 | ContactsStamped.msg 17 | Pose.msg 18 | ) 19 | 20 | generate_messages(DEPENDENCIES std_msgs geometry_msgs) 21 | 22 | catkin_package(CATKIN_DEPENDS message_runtime std_msgs geometry_msgs) 23 | -------------------------------------------------------------------------------- /champ_msgs/msg/Contacts.msg: -------------------------------------------------------------------------------- 1 | bool[] contacts -------------------------------------------------------------------------------- /champ_msgs/msg/ContactsStamped.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | bool[] contacts -------------------------------------------------------------------------------- /champ_msgs/msg/Imu.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Quaternion orientation 2 | geometry_msgs/Vector3 linear_acceleration 3 | geometry_msgs/Vector3 angular_velocity 4 | geometry_msgs/Vector3 magnetic_field 5 | -------------------------------------------------------------------------------- /champ_msgs/msg/Joints.msg: -------------------------------------------------------------------------------- 1 | float32[] position -------------------------------------------------------------------------------- /champ_msgs/msg/PID.msg: -------------------------------------------------------------------------------- 1 | float32 p 2 | float32 d 3 | float32 i 4 | -------------------------------------------------------------------------------- /champ_msgs/msg/Point.msg: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y 3 | float32 z -------------------------------------------------------------------------------- /champ_msgs/msg/PointArray.msg: -------------------------------------------------------------------------------- 1 | champ_msgs/Point lf 2 | champ_msgs/Point rf 3 | champ_msgs/Point lh 4 | champ_msgs/Point rh -------------------------------------------------------------------------------- /champ_msgs/msg/Pose.msg: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y 3 | float32 z 4 | float32 roll 5 | float32 pitch 6 | float32 yaw 7 | -------------------------------------------------------------------------------- /champ_msgs/msg/Velocities.msg: -------------------------------------------------------------------------------- 1 | 2 | float32 linear_x 3 | float32 linear_y 4 | float32 angular_z 5 | -------------------------------------------------------------------------------- /champ_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | champ_msgs 4 | 0.1.0 5 | champ_msgs package 6 | 7 | Juan Miguel Jimeno 8 | Juan Miguel Jimeno 9 | 10 | BSD 11 | 12 | catkin 13 | message_generation 14 | message_runtime 15 | std_msgs 16 | geometry_msgs 17 | 18 | 19 | -------------------------------------------------------------------------------- /champ_navigation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(champ_navigation) 3 | 4 | find_package(catkin REQUIRED COMPONENTS) 5 | 6 | catkin_package() 7 | 8 | include_directories( 9 | ${catkin_INCLUDE_DIRS} 10 | ) 11 | 12 | install(DIRECTORY launch maps param rviz 13 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 14 | ) 15 | -------------------------------------------------------------------------------- /champ_navigation/launch/navigate.launch: -------------------------------------------------------------------------------- 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 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /champ_navigation/launch/navigation/amcl.launch: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /champ_navigation/launch/navigation/move_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /champ_navigation/launch/octo_slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /champ_navigation/launch/slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /champ_navigation/launch/slam/gmapping.launch: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /champ_navigation/launch/slam/hector_mapping.launch: -------------------------------------------------------------------------------- 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 | 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /champ_navigation/launch/slam/octomap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /champ_navigation/launch/slam/velodyne_pointcloud_to_laser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | transform_tolerance: 0.01 7 | min_height: 0 8 | max_height: 0.1 9 | 11 | 13 | 14 | angle_min: -3.1415926535897931 # -M_PI/2 15 | angle_max: 3.1415926535897931 # M_PI/2 16 | angle_increment: 0.0174533 # 1 deg in rad 17 | scan_time: 0.1 18 | range_min: 0.1 19 | range_max: 100.0 20 | use_inf: true 21 | inf_epsilon: 0.1 22 | 23 | # Concurrency level, affects number of pointclouds queued for processing, thread number governed by nodelet manager 24 | # 0 : Detect number of cores 25 | # 1 : Single threaded 26 | # 2->inf : Parallelism level 27 | concurrency_level: 1 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /champ_navigation/maps/map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chvmp/champ/7f7d91724dc9cae43d5c3868821f4f8b79877d1a/champ_navigation/maps/map.pgm -------------------------------------------------------------------------------- /champ_navigation/maps/map.yaml: -------------------------------------------------------------------------------- 1 | image: map.pgm 2 | resolution: 0.050000 3 | origin: [-50.000000, -50.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /champ_navigation/maps/octomap.ot: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chvmp/champ/7f7d91724dc9cae43d5c3868821f4f8b79877d1a/champ_navigation/maps/octomap.ot -------------------------------------------------------------------------------- /champ_navigation/maps/octomap.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chvmp/champ/7f7d91724dc9cae43d5c3868821f4f8b79877d1a/champ_navigation/maps/octomap.pgm -------------------------------------------------------------------------------- /champ_navigation/maps/octomap.yaml: -------------------------------------------------------------------------------- 1 | image: octomap.pgm 2 | resolution: 0.050000 3 | origin: [-7.500000, -14.400000, 0.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /champ_navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | champ_navigation 4 | 0.1.0 5 | Champ Navigation Files 6 | 7 | Juan Miguel Jimeno 8 | Juan Miguel Jimeno 9 | 10 | BSD 11 | 12 | catkin 13 | roslaunch 14 | gmapping 15 | hector_mapping 16 | octomap 17 | octomap_server 18 | map_server 19 | move_base 20 | global_planner 21 | navfn 22 | dwa_local_planner 23 | base_local_planner 24 | amcl 25 | pointcloud_to_laserscan 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /champ_navigation/param/base_local_planner_holonomic_params.yaml: -------------------------------------------------------------------------------- 1 | DWAPlannerROS: 2 | max_trans_vel: 0.5 3 | min_trans_vel: 0.01 4 | max_vel_x: 0.5 5 | min_vel_x: -0.025 6 | max_vel_y: 0.0 7 | min_vel_y: 0.0 8 | max_rot_vel: 0.4 9 | min_rot_vel: -0.4 10 | acc_lim_x: 2.0 11 | acc_lim_y: 0.0 12 | acc_lim_theta: 4 13 | acc_lim_trans: 3.0 14 | 15 | prune_plan: false 16 | 17 | xy_goal_tolerance: 0.25 18 | yaw_goal_tolerance: 0.1 19 | trans_stopped_vel: 0.1 20 | rot_stopped_vel: 0.1 21 | sim_time: 3.0 22 | sim_granularity: 0.1 23 | angular_sim_granularity: 0.1 24 | path_distance_bias: 34.0 25 | goal_distance_bias: 24.0 26 | occdist_scale: 0.05 27 | twirling_scale: 0.0 28 | stop_time_buffer: 0.5 29 | oscillation_reset_dist: 0.05 30 | oscillation_reset_angle: 0.2 31 | forward_point_distance: 0.2 32 | scaling_speed: 0.25 33 | max_scaling_factor: 0.2 34 | vx_samples: 20 35 | vy_samples: 0 36 | vth_samples: 40 37 | 38 | use_dwa: true 39 | restore_defaults: true 40 | -------------------------------------------------------------------------------- /champ_navigation/param/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | obstacle_range: 2.5 2 | raytrace_range: 3.0 3 | footprint: [[-0.25, -0.145], [-0.25, 0.145], [0.25, 0.145], [0.25, -0.145]] 4 | inflation_radius: 0.55 5 | transform_tolerance: 0.5 6 | 7 | observation_sources: scan 8 | scan: 9 | data_type: LaserScan 10 | topic: scan 11 | marking: true 12 | clearing: true 13 | 14 | map_type: costmap 15 | -------------------------------------------------------------------------------- /champ_navigation/param/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: map 3 | robot_base_frame: base_footprint 4 | update_frequency: 1.0 #before: 5.0 5 | publish_frequency: 0.5 #before 0.5 6 | static_map: true 7 | transform_tolerance: 0.5 8 | cost_scaling_factor: 10.0 9 | inflation_radius: 0.55 -------------------------------------------------------------------------------- /champ_navigation/param/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 3 | robot_base_frame: base_footprint 4 | update_frequency: 1.0 #before 5.0 5 | publish_frequency: 2.0 #before 2.0 6 | static_map: false 7 | rolling_window: true 8 | width: 2.5 9 | height: 2.5 10 | resolution: 0.05 #increase to for higher res 0.025 11 | transform_tolerance: 0.5 12 | cost_scaling_factor: 5 13 | inflation_radius: 0.55 -------------------------------------------------------------------------------- /champ_navigation/param/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | base_global_planner: global_planner/GlobalPlanner 2 | base_local_planner: dwa_local_planner/DWAPlannerROS 3 | 4 | shutdown_costmaps: false 5 | 6 | controller_frequency: 5.0 #before 5.0 7 | controller_patience: 3.0 8 | 9 | planner_frequency: 0.5 10 | planner_patience: 5.0 11 | 12 | oscillation_timeout: 10.0 13 | oscillation_distance: 0.2 14 | 15 | conservative_reset_dist: 0.1 #distance from an obstacle at which it will unstuck itself 16 | 17 | cost_factor: 1.0 18 | neutral_cost: 55 19 | lethal_cost: 253 -------------------------------------------------------------------------------- /docs/images/leg_stretched.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chvmp/champ/7f7d91724dc9cae43d5c3868821f4f8b79877d1a/docs/images/leg_stretched.png -------------------------------------------------------------------------------- /docs/images/navigation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chvmp/champ/7f7d91724dc9cae43d5c3868821f4f8b79877d1a/docs/images/navigation.gif -------------------------------------------------------------------------------- /docs/images/robots.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chvmp/champ/7f7d91724dc9cae43d5c3868821f4f8b79877d1a/docs/images/robots.gif -------------------------------------------------------------------------------- /docs/images/slam.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chvmp/champ/7f7d91724dc9cae43d5c3868821f4f8b79877d1a/docs/images/slam.gif --------------------------------------------------------------------------------