fixed issue with the way indexes are passed to forwardKin

systems/plants/@RigidBodyManipulator/collisionDetect.m

@@ -135,20 +135,23 @@
   end
 
   if ~isempty(terrain_contact_point_struct)
+
     xA_new = [terrain_contact_point_struct.pts];
     numStructs = size(terrain_contact_point_struct,2);
+
+    %total_pts = size(horzcat(terrain_contact_point_struct.pts),2); %too slow
+    xA_new_in_world =  [];
     k = 1;
-    terrain_pts = horzcat(terrain_contact_point_struct.pts);
-    terrain_idxs = zeros(1,size(terrain_pts,2));
     for i = 1:numStructs
         numPts = size(terrain_contact_point_struct(i).pts,2);
+        xA_new_in_world = [xA_new_in_world, forwardKin(obj, kinsol, ...
+        terrain_contact_point_struct(i).idx, terrain_contact_point_struct(i).pts)];
         for j = 1:numPts
             terrain_idxs(k) = terrain_contact_point_struct(i).idx;
             k = k+1;
         end
     end
-    xA_new_in_world = forwardKin(obj, kinsol, terrain_idxs, terrain_pts);
-
+
     % Note: only implements collisions with the obj.terrain so far
  [phi_new,normal_new,xB_new,idxB_new] = ...
       collisionDetectTerrain(obj,xA_new_in_world);