Generate all disable collision lines for moveit srdf model in between a list of links

generate_collisions.py

#! /usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on 11/16/15

@author: sampfeiffer

generate_collisions.py link_1 link_2 link_3

Generates the lines to add to the moveit config <robot>.srdf in
 <robot>_moveit_config to make all the joints disable
collisions in between them.

"""
__author__ = 'sampfeiffer'

import sys

START = '    <disable_collisions link1="'
MIDDLE = '" link2="'
END = '" reason="Never" />\n'

if __name__ == '__main__':
    if len(sys.argv) < 2:
        print "Usage: " + sys.argv[0] + "link1 link2 link3 ... linkN"
        print
        print "This will generate the disable lines to be copy pasted into <robot>.srdf in <robot>_moveit_config"
        print 'Like: <disable_collisions link1="arm_left_1_link" link2="arm_left_3_link" reason="Never" />'
        exit(0)
    link_names = sys.argv[1:]
    print "We got links: " + str(link_names)
    already_disabled = []
    disable_lines = ""
    for idx, link1 in enumerate(link_names):
        print "  Link: " + str(link1) + " won't collide with: " + str(link_names[idx+1:])
        if len(already_disabled) > 0:
            print "  Neither with already disabled: " + str(already_disabled)
        print
        for link2 in link_names[idx:]:
            if link1 != link2:
                disable_lines += START + link1 + MIDDLE + link2 + END
        already_disabled.append(link1)

    print "Disabling with each other all links: " + str(link_names)
    print "Result:\n"
    print disable_lines


# Example:
#
# python generate_collisions.py hand_left_index_1_link hand_left_index_2_link hand_left_index_3_link hand_left_middle_1_link hand_left_middle_2_link hand_left_middle_3_link hand_left_thumb_link hand_left_palm_link
#
# Output:
#
#     <disable_collisions link1="hand_left_index_1_link" link2="hand_left_index_2_link" reason="Never" />
#     <disable_collisions link1="hand_left_index_1_link" link2="hand_left_index_3_link" reason="Never" />
#     <disable_collisions link1="hand_left_index_1_link" link2="hand_left_middle_1_link" reason="Never" />
#     <disable_collisions link1="hand_left_index_1_link" link2="hand_left_middle_2_link" reason="Never" />
#     <disable_collisions link1="hand_left_index_1_link" link2="hand_left_middle_3_link" reason="Never" />
#     <disable_collisions link1="hand_left_index_1_link" link2="hand_left_thumb_link" reason="Never" />
#     <disable_collisions link1="hand_left_index_1_link" link2="hand_left_palm_link" reason="Never" />
#     <disable_collisions link1="hand_left_index_2_link" link2="hand_left_index_3_link" reason="Never" />
#     <disable_collisions link1="hand_left_index_2_link" link2="hand_left_middle_1_link" reason="Never" />
#     <disable_collisions link1="hand_left_index_2_link" link2="hand_left_middle_2_link" reason="Never" />
#     <disable_collisions link1="hand_left_index_2_link" link2="hand_left_middle_3_link" reason="Never" />
#     <disable_collisions link1="hand_left_index_2_link" link2="hand_left_thumb_link" reason="Never" />
#     <disable_collisions link1="hand_left_index_2_link" link2="hand_left_palm_link" reason="Never" />
#     <disable_collisions link1="hand_left_index_3_link" link2="hand_left_middle_1_link" reason="Never" />
#     <disable_collisions link1="hand_left_index_3_link" link2="hand_left_middle_2_link" reason="Never" />
#     <disable_collisions link1="hand_left_index_3_link" link2="hand_left_middle_3_link" reason="Never" />
#     <disable_collisions link1="hand_left_index_3_link" link2="hand_left_thumb_link" reason="Never" />
#     <disable_collisions link1="hand_left_index_3_link" link2="hand_left_palm_link" reason="Never" />
#     <disable_collisions link1="hand_left_middle_1_link" link2="hand_left_middle_2_link" reason="Never" />
#     <disable_collisions link1="hand_left_middle_1_link" link2="hand_left_middle_3_link" reason="Never" />
#     <disable_collisions link1="hand_left_middle_1_link" link2="hand_left_thumb_link" reason="Never" />
#     <disable_collisions link1="hand_left_middle_1_link" link2="hand_left_palm_link" reason="Never" />
#     <disable_collisions link1="hand_left_middle_2_link" link2="hand_left_middle_3_link" reason="Never" />
#     <disable_collisions link1="hand_left_middle_2_link" link2="hand_left_thumb_link" reason="Never" />
#     <disable_collisions link1="hand_left_middle_2_link" link2="hand_left_palm_link" reason="Never" />
#     <disable_collisions link1="hand_left_middle_3_link" link2="hand_left_thumb_link" reason="Never" />
#     <disable_collisions link1="hand_left_middle_3_link" link2="hand_left_palm_link" reason="Never" />
#     <disable_collisions link1="hand_left_thumb_link" link2="hand_left_palm_link" reason="Never" />