Collision Detection, Isolation and Identification for Humanoids

Authors: Jonathan Vorndamme, Moritz Schappler, and Sami Haddadin

This repository provides supplemental material to the following publication at the IEEE ICRA 2017 conference:

@InProceedings{VorndammeSchHad2017,
  author       = {Vorndamme, Jonathan and Schappler, Moritz and Haddadin, Sami},
  booktitle    = {2017 IEEE International Conference on Robotics and Automation (ICRA)},
  title        = {Collision detection, isolation and identification for humanoids},
  doi          = {10.1109/ICRA.2017.7989552},
  organization = {IEEE},
  pages        = {4754--4761},
  year         = {2017},
}

Abstract

High-performance collision handling, which is divided into the five phases detection, isolation, estimation, classification and reaction, is a fundamental robot capability for safe and sensitive operation/interaction in unknown environments. For complex humanoid robots collision handling is obviously significantly more complex than for classical static manipulators. In particular, the robot stability during the collision reaction phase has to be carefully designed and relies on high fidelity contact information that is generated during the first three phases. In this paper, a unified real-time algorithm is presented for determining unknown contact forces and contact locations for humanoid robots based on proprioceptive sensing only, i.e. joint position, velocity and torque, as well as force/torque sensing along the structure. The proposed scheme is based on nonlinear model-based momentum observers that are able to recover the unknown contact forces and the respective locations. The dynamic loads acting on internal force/torque sensors are also corrected based on a novel nonlinear compensator. The theoretical capabilities of the presented methods are evaluated in simulation with the Atlas robot. In summary, we propose a full solution to the problem of collision detection, collision isolation and collision identification for the general class of humanoid robots.

Patent Notice

The algorithm implemented in this repository and presented in this paper may be protected by the German patent DE 10 2017 005 080.5 and the US patent application number 20200061835.

Contents

This repository contains Matlab scripts and Latex code to reproduce all figures of the paper.

Dependencies

The path initialization script humanoid_collisionhandling_path.m has to be run in Matlab.

The following repositories have to be downloaded and their respective ...path_init has to be run:

• https://github.com/SchapplM/robotics-toolbox
• https://github.com/SchapplM/matlab_toolbox
• https://github.com/SchapplM/robotics-dep-ext

Some Matlab functions have to be compiled using the Mex compiler. This is done with atlas_collhdl_mex_all.m. A Mex compiler has to be installed and configured within Matlab.

Scripts for Contact Simulation

The following scripts simulate different contact scenarios on the Atlas robot. Random contact points and forces are generated and the collision pipeline from the paper is applied for a detection of the forces.

• atlas5wbody_collisions_statistics_sc: Single contact on each link of the robot.
• atlas5wbody_collisions_statistics_sc_filter: Single contact. Assumes a first order low pass filter for the output of the disturbance observer. The observer is not implemented itself.
• atlas5wbody_collisions_statistics_mc: Two simultaneous contacts on arbitrary links of the robot
• atlas5wbody_collisions_statistics_mc_manip: Assume contacts in a manipulation scenario with four contacts at the hands and feet and one additional contact.

Scripts for Image Generation

• Fig. 6 is generated by collest_single_allresults_nosim/collest_sc_allres_nosim_picturegen.m (requires atlas5wbody_collisions_statistics_sc.m).
• Fig. 7 is generated by collest_multi_stat_2coll_rankmat/rankmat_fig_gen.m (requires atlas5wbody_collisions_statistics_mc.m)
• Fig. 8 is generated by collest_multi_stat_2coll_error_rank/histogram_error_rank.m (requires atlas5wbody_collisions_statistics_mc.m)
• Fig. 9 is generated by collest_multi_stat_manipcoll_error/error_rank_manipcoll_hist_fig_gen.m (requires atlas5wbody_collisions_statistics_mc_manip.m)
• Fig. 10 is generated by colltest_single_filter/colltest_single_filter_eval.m (requires atlas5wbody_collisions_statistics_sc_filter.m)

Algorithm and Equations

• 6x36 floating base Jacobian matrix from Equ. 3: atlas5_wbody_body_jacobig_mdh_eulangrpy_num.m
• 6x30 geometric Jacobian from Equ. 3: atlas5_wbody_body_jacobig_mdh_num.m
• Collision detection from Equ. 21: atlas5_wbody_collision_detection
• Intersection of the line and the link geometry (text after Equ. 26): atlas5_wbody_intersect_collbodies.m
• Handling multiple contacts, Equ. 28: atlas5_wbody_collision_isolation_fullchain_multi.m and atlas5_wbody_collision_isolation_fullchain_multi_ic_in.m
• Identification of the contact force, Equ. 30: atlas5_wbody_collision_identification for a single contact, atlas5_wbody_collision_identification_multi.m for multiple contacts
• Full collision pipeline based on internal torque measurements: atlas5_wbody_collision_isolation_fullchain.m
• Full pipeline based solely on force/torque sensors: atlas5_wbody_collision_isolation_forcesensor.m

Further Scripts

• All scripts aforementioned are run by testfunctions/collhdl_test_everything.m.
• The collision bodies (boxes and cylinders) from the DRCSim URDF file are compared to the mesh objects with contact/atlas_contactbody_test.m
• atlas5arm_collisions_statistics_figure.m checks collisions only for one arm (but presents an early stage of development)
• The scripts isolation_test.m and isolation_test_with_singularity.m test the plausibility of the contact isolation in one configuration of the arm

Kinematics and Dynamics Model

The files with kinematics and dynamics functions in robot_model are generated by a dynamics model generation toolbox based on Maple. The input files for creating the required models atlas5arm, atlas4leg and atlas5wbody are stored in the directory robot_codegen_definitions/examples of the toolbox.