Update to support new kinematic plugin format and contact managers

Author: Levi-Armstrong

trajopt/test/data/boxbot.srdf

@@ -3,5 +3,9 @@
     <group name="manipulator">
         <chain base_link="base_link" tip_link="boxbot_link" />
     </group>
+
+    <!--Contact Managers plugins -->
+    <contact_managers_plugin_config filename="package://trajopt/test/data/contact_manager_plugins.yaml"/>
+
     <virtual_joint name="world_joint" type="fixed" parent_frame="world" child_link="base_link" />
 </robot>

trajopt/test/data/config/boxbot_joint_limits.yaml

@@ -0,0 +1,22 @@
+contact_manager_plugins:
+  search_paths:
+    - /usr/local/lib
+  search_libraries:
+    - tesseract_collision_bullet_factories
+    - tesseract_collision_fcl_factories
+  discrete_plugins:
+    default: BulletDiscreteBVHManager
+    plugins:
+      BulletDiscreteBVHManager:
+        class: BulletDiscreteBVHManagerFactory
+      BulletDiscreteSimpleManager:
+        class: BulletDiscreteSimpleManagerFactory
+      FCLDiscreteBVHManager:
+        class: FCLDiscreteBVHManagerFactory
+  continuous_plugins:
+    default: BulletCastBVHManager
+    plugins:
+      BulletCastBVHManager:
+        class: BulletCastBVHManagerFactory
+      BulletCastSimpleManager:
+        class: BulletCastSimpleManagerFactory

trajopt/test/data/config/boxbot_kinematics.yaml

@@ -19,6 +19,9 @@
     <!--Groups kinematic plugins -->
     <kinematics_plugin_config filename="package://trajopt/test/data/pr2_plugins.yaml"/>
 
+    <!--Contact Managers plugins -->
+    <contact_managers_plugin_config filename="package://trajopt/test/data/contact_manager_plugins.yaml"/>
+
     <group name="right_gripper">
         <link name="r_gripper_palm_link" />
         <link name="r_gripper_led_frame" />

trajopt/test/data/config/pr2_joint_limits.yaml

@@ -3,16 +3,18 @@ kinematic_plugins:
     - tesseract_kinematics_kdl_factories
   inv_kin_plugins:
     left_arm:
-      KDLInvKinChainLMA:
-        class: KDLInvKinChainLMAFactory
-        default: true
-        config:
-          base_link: torso_lift_link
-          tip_link: l_gripper_tool_frame
+      default: KDLInvKinChainLMA
+      plugins:
+        KDLInvKinChainLMA:
+          class: KDLInvKinChainLMAFactory
+          config:
+            base_link: torso_lift_link
+            tip_link: l_gripper_tool_frame
     right_arm:
-      KDLInvKinChainLMA:
-        class: KDLInvKinChainLMAFactory
-        default: true
-        config:
-          base_link: torso_lift_link
-          tip_link: r_gripper_tool_frame
+      default: KDLInvKinChainLMA
+      plugins:
+        KDLInvKinChainLMA:
+          class: KDLInvKinChainLMAFactory
+          config:
+            base_link: torso_lift_link
+            tip_link: r_gripper_tool_frame

trajopt/test/data/config/pr2_kinematics.yaml

@@ -3,5 +3,9 @@
     <group name="manipulator">
         <chain base_link="base_link" tip_link="spherebot_link" />
     </group>
+
+    <!--Contact Managers plugins -->
+    <contact_managers_plugin_config filename="package://trajopt/test/data/contact_manager_plugins.yaml"/>
+
     <virtual_joint name="world_joint" type="fixed" parent_frame="world" child_link="base_link" />
 </robot>