Home

Overview

TOUGH library provides C++ API to humanoid robots supported by IHMC controllers. The code is tested on Valkyrie R5 (branch: ihmc-0.8.2), srcsim (tag: 0.1a), and atlas in SCS simulator(tag: 0.1a). The default branch is in active development and may not work as expected.

Installation

Install the dependencies

sudo apt-get install ros-indigo-moveit-full ros-indigo-sbpl ros-indigo-humanoid-nav-msgs ros-indigo-map-server ros-indigo-trac-ik* ros-indigo-multisense-ros ros-indigo-robot-self-filter ros-indigo-octomap ros-indigo-octomap-msgs ros-indigo-octomap-ros ros-indigo-gridmap-2d

Download humanoid-navigation package in the catkin workspace from this location.

Clone this repo in your workspace. Assuming ~/indigo_ws is the workspace, replace it with appropriate directory.

cd ~/indigo_ws/src

# clone humanoid-navigation repo
git clone https://github.com/ninja777/humanoid_navigation.git

# clone tough repo
git clone https://github.com/WPI-Humanoid-Robotics-Lab/tough.git

# build the code
cd ..
catkin_make

Directory Structure

tough_bringup

Provides launch files to start up various components required for nodes that use TOUGH

tough_common

This package hosts the global constants available for use. The constants include frame names, joint names, joint limits, etc.

tough_control

This package contains interface classes for all the controllers. Each controller has a test_ file that provides an example usage of the controller.

tough_motion_planners

This package hosts a cartesian planner using trac-ik and move-it. To use this planner, move-it configs located here are required.

tough_navigation

If a 2D map is hosted on /map topic, this package can be used to plan footsteps from current location to any goal pose on the map. Multiple examples of how to use it programmatically are present in the src directory

tough_perception

Code developed during DARPA Robotics Challenge by the WPI-CMU team for perception was used as baseline for development of this package. It provides images and pointcloud from multisense sensor. Utilities included in this package are laser assembler, walkway filter, robot filter, and a periodic snapshotter of pointcloud.

Controller Example

In this example we will write a node to move chest based such that it's roll=0, pitch=10, and yaw=30 degrees in 5 seconds. To start, let's include the header file for ChestControllerInterface.

#include <tough_controller_interface/chest_control_interface.h>

Now that we have access to the functions to control chest, we should initialize a ChestControllerInterface object in a ros node to call those functions. This needs ros::NodeHandle as a constructor argument.

#include <tough_controller_interface/chest_control_interface.h>

int main(int argc, char **argv)
{
    // Initialize a ros node
    ros::init(argc, argv, "test_node");
    ros::NodeHandle nh;

    // Create an object of ChestControlInterface
    ChestControlInterface chestTraj(nh);

Now we can call the controlChest function to move the chest in required orientation

float roll = 0.0f;
float pitch = 10.0f;
float yaw = 30.0f;
float duration = 5.0f; 

// change the chest orientation. This is a non-blocking call.
chestTraj.controlChest(roll, pitch, yaw, duration);

Putting it all together, we have the following code that moves the robot chest to the specified orientation.

#include <tough_controller_interface/chest_control_interface.h>

int main(int argc, char **argv)
{
    // Initialize a ros node
    ros::init(argc, argv, "test_node");
    ros::NodeHandle nh;

    // Create an object of ChestControlInterface
    ChestControlInterface chestTraj(nh);
    float roll = 0;
    float pitch = 10;
    float yaw = 30;
    float duration = 5.0f; 

    // change the chest orientation. This is a non-blocking call.
    chestTraj.controlChest(roll, pitch, yaw, duration);

    // wait for the robot to move
    ros::Duration(2).sleep();
    ROS_INFO("Motion finished");
    return 0;
}