diff --git a/@SerialLink/animate.m b/@SerialLink/animate.m
index e4689ef..c3f5cb0 100644
--- a/@SerialLink/animate.m
+++ b/@SerialLink/animate.m
@@ -69,7 +69,11 @@ function animate(robot, qq)
                             end
                         end
                         T = T * links(L).A(q(L));
-                        set(h.link(L), 'Matrix', T.T); 
+                        if isempty(robot.faces)
+                            set(h.link(L), 'Matrix', T.T); 
+                        else
+                            set(h.link(L+1), 'Matrix', T.T); 
+                        end
                         vert = [vert; T.t'];
                     end
                     % update the transform for link N+1 (the tool)
diff --git a/@SerialLink/plot3d.m b/@SerialLink/plot3d.m
index 67af416..7914cf2 100644
--- a/@SerialLink/plot3d.m
+++ b/@SerialLink/plot3d.m
@@ -102,9 +102,7 @@
 
 function plot3d(robot, q, varargin)
     
-    
-    assert( ~robot.mdh, 'RTB:plot3d:badmodel', '3D models are defined for standard, not modified, DH parameters');
-    
+
     opt = plot_options(robot, varargin);
     
 
diff --git a/AstarPO.m b/AstarPO.m
index d85e4fb..a4df373 100755
--- a/AstarPO.m
+++ b/AstarPO.m
@@ -5,6 +5,7 @@
 % navigation algorithm for multiobjective optimization (MOO) - i.e.
 % optimizes over several objectives/criteria.
 % 
+
 % Methods::
 % plan              Compute the cost map given a goal and map
 % path              Compute a path to the goal