From 1d65493940b5e3b822b9960a8e57552de673b321 Mon Sep 17 00:00:00 2001
From: Amin Yahyaabadi <aminyahyaabadi74@gmail.com>
Date: Mon, 8 Jul 2019 18:15:35 -0500
Subject: [PATCH] SerialLink.double and Link.double

Link.double converts link parameters to numeric (double) type

dl = l.double is a Link object in which all the parameters are
numeric ('double') type.

Useful when you are using Pi=sym('pi') to avoid round off errors
(e.g in sin(Pi/2)), but later you want to pass the link object to
a method (e.g. ikine) which only supports real numbers.

Will give an error if a symbolic variable is not convertible to
double number.

See also SerialLink.double

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

SerialLink.double converts SerialLink object parameters to numeric (double) type

dr = r.double is a SerialLink object in which all the parameters are
numeric ('double') type.

Useful when you are using Pi=sym('pi') to avoid round off errors
e.g in sin(Pi/2)), but later you want to pass the SerialLink object to
a method (e.g. ikine) which only supports real numbers.

Will give an error if a symbolic variable is not convertible to
double number.

See also Link.double

Author: Amin Yahyaabadi (aminyahyaabadi74@gmail.com)
---
 @SerialLink/SerialLink.m | 24 ++++++++++++++++++++++-
 Link.m                   | 42 ++++++++++++++++++++++++++++++++++++++++
 2 files changed, 65 insertions(+), 1 deletion(-)

diff --git a/@SerialLink/SerialLink.m b/@SerialLink/SerialLink.m
index 43f2acc6..7de3e1e7 100644
--- a/@SerialLink/SerialLink.m
+++ b/@SerialLink/SerialLink.m
@@ -901,7 +901,29 @@ function dyn(r, j)
                 sr.links(i) = r.links(i).sym;
             end
         end
-
+		
+		function dr = double(r)
+            % SerialLink.double converts SerialLink object parameters to numeric (double) type
+            %
+            % dr = r.double is a SerialLink object in which all the parameters are
+            % numeric ('double') type.
+            % 
+            % Useful when you are using Pi=sym('pi') to avoid round off errors
+            % e.g in sin(Pi/2)), but later you want to pass the SerialLink object to
+            % a method (e.g. ikine) which only supports real numbers. 
+            % 
+            % Will give an error if a symbolic variable is not convertable to
+            % double number.
+            %
+            % See also Link.double
+            %
+            % Author: Amin Yahyaabadi (aminyahyaabadi74@gmail.com)
+            
+            dr = SerialLink(r);
+            for i=1:r.n
+                dr.links(i) = r.links(i).double;
+            end
+        end
                  
         function p = isprismatic(robot)
         %SerialLink.isprismatic identify prismatic joints
diff --git a/Link.m b/Link.m
index f67b3951..3bfdb7be 100644
--- a/Link.m
+++ b/Link.m
@@ -991,6 +991,48 @@ function dyn(links)
             l.B = sym(l.B);
             l.Tc = sym(l.Tc);
         end
+		
+		function l = double(l)
+            %Link.double converts link parameters to numeric (double) type
+            %
+            % dl = l.double is a Link object in which all the parameters are
+            % numeric ('double') type.
+            % 
+            % Useful when you are using Pi=sym('pi') to avoid round off errors
+            % (e.g in sin(Pi/2)), but later you want to pass the link object to
+            % a method (e.g. ikine) which only supports real numbers. 
+            % 
+            % Will give an error if a symbolic variable is not convertable to
+            % double number.
+            %
+            % See also SerialLink.double
+            %
+            % Author: Amin Yahyaabadi (aminyahyaabadi74@gmail.com)
+           
+            if l.issym % we can disable the check, we should compare the speed.
+                
+                if ~isempty(l.theta) % && isa(l.d,'sym') % we can check for sym class individually, speed should be compared.
+                    l.d=double(l.d);
+                end
+                
+                if ~isempty(l.theta)
+                    l.theta=double(l.theta);
+                end
+                
+                    l.alpha=double(l.alpha);                              
+                    l.a=double(l.a);              
+                    l.offset=double(l.offset);
+                
+                    l.offset=double(l.I);                              
+                    l.offset=double(l.r);               
+                    l.offset=double(l.m);
+                
+                    l.Jm = double(l.Jm);
+                    l.G = double(l.G);
+                    l.B = double(l.B);
+                    l.Tc = double(l.Tc);
+            end        
+        end
         
 
     end % methods